Untabification and Added get functions for Xbee_status
This commit is contained in:
pyhs
parent
1d24de28a5
commit
8e5acd264d
|
|
@ -42,9 +42,11 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
namespace rosbzz_node{
|
||||
namespace rosbzz_node
|
||||
{
|
||||
|
||||
class roscontroller{
|
||||
class roscontroller
|
||||
{
|
||||
|
||||
public:
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
|
|
@ -59,15 +61,14 @@ private:
|
|||
uint8_t index = 0;
|
||||
uint8_t current = 0;
|
||||
num_robot_count(){}
|
||||
|
||||
}; typedef struct num_robot_count Num_robot_count ;
|
||||
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
|
||||
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ;
|
||||
}; typedef struct gps GPS ; // not useful in cpp
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
|
@ -232,7 +233,15 @@ private:
|
|||
void SetStreamRate(int id, int rate, int on_off);
|
||||
|
||||
void get_number_of_robots();
|
||||
|
||||
void GetRobotId();
|
||||
bool GetDequeFull(bool &result);
|
||||
bool GetRssi(float &result);
|
||||
bool TriggerAPIRssi(const uint8_t short_id);
|
||||
bool GetAPIRssi(const uint8_t short_id, float &result);
|
||||
bool GetRawPacketLoss(const uint8_t short_id, float &result);
|
||||
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -414,7 +414,7 @@ int create_stig_tables() {
|
|||
//buzzvm_gstore(VM);
|
||||
//buzzvm_dump(VM);
|
||||
|
||||
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
||||
buzzvm_tget(VM);
|
||||
|
|
@ -785,5 +785,3 @@ int create_stig_tables() {
|
|||
buzzvm_gstore(VM);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -32,15 +32,18 @@ namespace buzzuav_closures{
|
|||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
int buzzros_print(buzzvm_t vm) {
|
||||
int buzzros_print(buzzvm_t vm)
|
||||
{
|
||||
int i;
|
||||
char buffer [50] = "";
|
||||
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
|
||||
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
|
||||
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i)
|
||||
{
|
||||
buzzvm_lload(vm, i);
|
||||
buzzobj_t o = buzzvm_stack_at(vm, 1);
|
||||
buzzvm_pop(vm);
|
||||
switch(o->o.type) {
|
||||
switch(o->o.type)
|
||||
{
|
||||
case BUZZTYPE_NIL:
|
||||
sprintf(buffer,"%s BUZZ - [nil]", buffer);
|
||||
break;
|
||||
|
|
@ -70,7 +73,7 @@ namespace buzzuav_closures{
|
|||
break;
|
||||
}
|
||||
}
|
||||
ROS_INFO(buffer);
|
||||
ROS_INFO("%s",buffer);
|
||||
//fprintf(stdout, "\n");
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
|
@ -107,7 +110,8 @@ namespace buzzuav_closures{
|
|||
/*----------------------------------------/
|
||||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
int buzzuav_moveto(buzzvm_t vm) {
|
||||
int buzzuav_moveto(buzzvm_t vm)
|
||||
{
|
||||
buzzvm_lnum_assert(vm, 2);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
|
|
@ -117,7 +121,8 @@ namespace buzzuav_closures{
|
|||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
float dy = buzzvm_stack_at(vm, 1)->f.value;
|
||||
float dx = buzzvm_stack_at(vm, 2)->f.value;
|
||||
double d = sqrt(dx*dx+dy*dy); //range
|
||||
// The compiler warnings are useful!
|
||||
// double d = sqrt(dx*dx+dy*dy); //range
|
||||
goto_pos[0]=dx;
|
||||
goto_pos[1]=dy;
|
||||
goto_pos[2]=height;
|
||||
|
|
|
|||
|
|
@ -39,13 +39,13 @@ namespace rosbzz_node{
|
|||
ros::spinOnce();
|
||||
}
|
||||
|
||||
|
||||
if (xbeeplugged) {
|
||||
GetRobotId();
|
||||
} else {
|
||||
robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
|
||||
}
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
std::string path =
|
||||
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
path += "Update.log";
|
||||
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
|
||||
}
|
||||
|
|
@ -65,27 +65,122 @@ namespace rosbzz_node{
|
|||
|
||||
void roscontroller::GetRobotId()
|
||||
{
|
||||
|
||||
|
||||
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
|
||||
mavros_msgs::ParamGet::Request robot_id_srv_request;
|
||||
robot_id_srv_request.param_id = "id";
|
||||
mavros_msgs::ParamGet::Response robot_id_srv_response;
|
||||
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
|
||||
while (!xbeestatus_srv.call(robot_id_srv_request, robot_id_srv_response))
|
||||
{
|
||||
ros::Duration(0.1).sleep();
|
||||
ROS_ERROR("Waiting for Xbee to respond to get device ID");
|
||||
}
|
||||
|
||||
robot_id = robot_id_srv_response.value.integer;
|
||||
}
|
||||
|
||||
//robot_id = 8;
|
||||
bool roscontroller::GetDequeFull(bool &result)
|
||||
{
|
||||
mavros_msgs::ParamGet::Request srv_request;
|
||||
srv_request.param_id = "deque_full";
|
||||
mavros_msgs::ParamGet::Response srv_response;
|
||||
|
||||
if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
|
||||
|
||||
result = (srv_response.value.integer == 1) ? true : false;
|
||||
return srv_response.success;
|
||||
}
|
||||
|
||||
bool roscontroller::GetRssi(float &result)
|
||||
{
|
||||
mavros_msgs::ParamGet::Request srv_request;
|
||||
srv_request.param_id = "rssi";
|
||||
mavros_msgs::ParamGet::Response srv_response;
|
||||
|
||||
if(!xbeestatus_srv.call(srv_request, srv_response)){return false;}
|
||||
|
||||
result = srv_response.value.real;
|
||||
return srv_response.success;
|
||||
}
|
||||
|
||||
bool roscontroller::TriggerAPIRssi(const uint8_t short_id)
|
||||
{
|
||||
mavros_msgs::ParamGet::Request srv_request;
|
||||
if(short_id == 0xFF)
|
||||
{
|
||||
srv_request.param_id = "trig_rssi_api_avg";
|
||||
}
|
||||
else
|
||||
{
|
||||
srv_request.param_id = "trig_rssi_api_" + std::to_string(short_id);
|
||||
}
|
||||
mavros_msgs::ParamGet::Response srv_response;
|
||||
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
|
||||
|
||||
return srv_response.success;
|
||||
}
|
||||
|
||||
bool roscontroller::GetAPIRssi(const uint8_t short_id, float &result)
|
||||
{
|
||||
mavros_msgs::ParamGet::Request srv_request;
|
||||
if(short_id == 0xFF)
|
||||
{
|
||||
srv_request.param_id = "get_rssi_api_avg";
|
||||
}
|
||||
else
|
||||
{
|
||||
srv_request.param_id = "get_rssi_api_" + std::to_string(short_id);
|
||||
}
|
||||
mavros_msgs::ParamGet::Response srv_response;
|
||||
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
|
||||
|
||||
result = srv_response.value.real;
|
||||
return srv_response.success;
|
||||
}
|
||||
|
||||
bool roscontroller::GetRawPacketLoss(const uint8_t short_id, float &result)
|
||||
{
|
||||
mavros_msgs::ParamGet::Request srv_request;
|
||||
if(short_id == 0xFF)
|
||||
{
|
||||
srv_request.param_id = "pl_raw_avg";
|
||||
}
|
||||
else
|
||||
{
|
||||
srv_request.param_id = "pl_raw_" + std::to_string(short_id);
|
||||
}
|
||||
mavros_msgs::ParamGet::Response srv_response;
|
||||
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
|
||||
|
||||
result = srv_response.value.real;
|
||||
return srv_response.success;
|
||||
}
|
||||
|
||||
bool roscontroller::GetFilteredPacketLoss(const uint8_t short_id, float &result)
|
||||
{
|
||||
mavros_msgs::ParamGet::Request srv_request;
|
||||
if(short_id == 0xFF)
|
||||
{
|
||||
srv_request.param_id = "pl_filtered_avg";
|
||||
}
|
||||
else
|
||||
{
|
||||
srv_request.param_id = "pl_filtered_" + std::to_string(short_id);
|
||||
}
|
||||
mavros_msgs::ParamGet::Response srv_response;
|
||||
if(xbeestatus_srv.call(srv_request, srv_response)){return false;}
|
||||
|
||||
result = srv_response.value.real;
|
||||
return srv_response.success;
|
||||
}
|
||||
|
||||
/*-------------------------------------------------
|
||||
/rosbuzz_node loop method executed once every step
|
||||
/--------------------------------------------------*/
|
||||
void roscontroller::RosControllerRun(){
|
||||
void roscontroller::RosControllerRun()
|
||||
{
|
||||
|
||||
/* Set the Buzz bytecode */
|
||||
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
|
||||
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),
|
||||
robot_id)) {
|
||||
ROS_INFO("[%i] Bytecode file found and set", robot_id);
|
||||
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
|
||||
init_update_monitor(bcfname.c_str(), standby_bo.c_str());
|
||||
|
|
@ -93,8 +188,7 @@ namespace rosbzz_node{
|
|||
// MAIN LOOP
|
||||
//////////////////////////////////////////////////////
|
||||
ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id);
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||
{
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done()) {
|
||||
/*Update neighbors position inside Buzz*/
|
||||
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
|
||||
/*Neighbours of the robot published with id in respective topic*/
|
||||
|
|
@ -108,15 +202,20 @@ namespace rosbzz_node{
|
|||
/*call flight controler service to set command long*/
|
||||
flight_controller_service_call();
|
||||
/*Set multi message available after update*/
|
||||
if(get_update_status()){
|
||||
if (get_update_status())
|
||||
{
|
||||
set_read_update_status();
|
||||
multi_msg = true;
|
||||
log<<cur_pos.latitude<<","<<cur_pos.longitude<<","<<cur_pos.altitude<<",";
|
||||
log << cur_pos.latitude << "," << cur_pos.longitude << ","
|
||||
<< cur_pos.altitude << ",";
|
||||
collect_data(log);
|
||||
map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.begin();
|
||||
map<int, buzz_utility::Pos_struct>::iterator it =
|
||||
neighbours_pos_map.begin();
|
||||
log << "," << neighbours_pos_map.size();
|
||||
for(;it != neighbours_pos_map.end();++it){
|
||||
log<<","<<(double)it->second.x<<","<<(double)it->second.y<<","<<(double)it->second.z;
|
||||
for (; it != neighbours_pos_map.end(); ++it)
|
||||
{
|
||||
log << "," << (double)it->second.x << "," << (double)it->second.y
|
||||
<< "," << (double)it->second.z;
|
||||
}
|
||||
log << std::endl;
|
||||
}
|
||||
|
|
@ -124,12 +223,14 @@ namespace rosbzz_node{
|
|||
// no_of_robots=get_number_of_robots();
|
||||
get_number_of_robots();
|
||||
buzz_utility::set_robot_var(no_of_robots);
|
||||
//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
|
||||
// if(neighbours_pos_map.size() >0) no_of_robots
|
||||
// =neighbours_pos_map.size()+1;
|
||||
// buzz_utility::set_robot_var(no_of_robots);
|
||||
/*Set no of robots for updates TODO only when not updating*/
|
||||
// if(multi_msg)
|
||||
updates_set_robots(no_of_robots);
|
||||
//ROS_INFO("ROBOTS: %i , acutal : %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
|
||||
// ROS_INFO("ROBOTS: %i , acutal :
|
||||
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
|
||||
/*run once*/
|
||||
ros::spinOnce();
|
||||
/*loop rate of ros*/
|
||||
|
|
@ -153,21 +254,34 @@ namespace rosbzz_node{
|
|||
/*--------------------------------------------------------
|
||||
/ Get all required parameters from the ROS launch file
|
||||
/-------------------------------------------------------*/
|
||||
void roscontroller::Rosparameters_get(ros::NodeHandle& n_c){
|
||||
void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
||||
{
|
||||
|
||||
/*Obtain .bzz file name from parameter server*/
|
||||
if(n_c.getParam("bzzfile_name", bzzfile_name));
|
||||
else {ROS_ERROR("Provide a .bzz file to run in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if (n_c.getParam("bzzfile_name", bzzfile_name))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a .bzz file to run in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
/*Obtain rc service option from parameter server*/
|
||||
if(n_c.getParam("rcclient", rcclient)){
|
||||
if (n_c.getParam("rcclient", rcclient))
|
||||
{
|
||||
if (rcclient == true) {
|
||||
/*Service*/
|
||||
if (!n_c.getParam("rcservice_name", rcservice_name))
|
||||
{ROS_ERROR("Provide a name topic name for rc service in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
{
|
||||
ROS_ERROR("Provide a name topic name for rc service in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
else if(rcclient==false){ROS_INFO("RC service is disabled");}
|
||||
} else if (rcclient == false)
|
||||
{
|
||||
ROS_INFO("RC service is disabled");
|
||||
}
|
||||
} else {
|
||||
ROS_ERROR("Provide a rc client option: yes or no in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
/*Obtain robot_id from parameter server*/
|
||||
// n_c.getParam("robot_id", robot_id);
|
||||
// robot_id=(int)buzz_utility::get_robotid();
|
||||
|
|
@ -178,11 +292,19 @@ namespace rosbzz_node{
|
|||
/*Obtain standby script to run during update*/
|
||||
n_c.getParam("stand_by", stand_by);
|
||||
|
||||
if(n_c.getParam("xbee_plugged", xbeeplugged));
|
||||
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if (n_c.getParam("xbee_plugged", xbeeplugged))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (!xbeeplugged) {
|
||||
if(n_c.getParam("name", robot_name));
|
||||
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if (n_c.getParam("name", robot_name))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
} else
|
||||
n_c.getParam("xbee_status_srv", xbeesrv_name);
|
||||
|
||||
|
|
@ -190,107 +312,149 @@ namespace rosbzz_node{
|
|||
|
||||
GetSubscriptionParameters(n_c);
|
||||
// initialize topics to null?
|
||||
|
||||
}
|
||||
/*-----------------------------------------------------------------------------------
|
||||
/Obtains publisher, subscriber and services from yml file based on the robot used
|
||||
/Obtains publisher, subscriber and services from yml file based on the robot
|
||||
used
|
||||
/-----------------------------------------------------------------------------------*/
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle){
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
|
||||
{
|
||||
// todo: make it as an array in yaml?
|
||||
m_sMySubscriptions.clear();
|
||||
std::string gps_topic, gps_type;
|
||||
if(node_handle.getParam("topics/gps", gps_topic));
|
||||
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if (node_handle.getParam("topics/gps", gps_topic))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a gps topic in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
node_handle.getParam("type/gps", gps_type);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type));
|
||||
|
||||
std::string battery_topic, battery_type;
|
||||
node_handle.getParam("topics/battery", battery_topic);
|
||||
node_handle.getParam("type/battery", battery_type);
|
||||
m_smTopic_infos.insert(pair <std::string, std::string>(battery_topic, battery_type));
|
||||
m_smTopic_infos.insert(
|
||||
pair<std::string, std::string>(battery_topic, battery_type));
|
||||
|
||||
std::string status_topic, status_type;
|
||||
node_handle.getParam("topics/status", status_topic);
|
||||
node_handle.getParam("type/status", status_type);
|
||||
m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
|
||||
m_smTopic_infos.insert(
|
||||
pair<std::string, std::string>(status_topic, status_type));
|
||||
|
||||
std::string altitude_topic, altitude_type;
|
||||
node_handle.getParam("topics/altitude", altitude_topic);
|
||||
node_handle.getParam("type/altitude", altitude_type);
|
||||
m_smTopic_infos.insert(pair <std::string, std::string>(altitude_topic, altitude_type));
|
||||
m_smTopic_infos.insert(
|
||||
pair<std::string, std::string>(altitude_topic, altitude_type));
|
||||
|
||||
/*Obtain fc client name from parameter server*/
|
||||
if(node_handle.getParam("topics/fcclient", fcclient_name));
|
||||
else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(node_handle.getParam("topics/setpoint", setpoint_name));
|
||||
else {ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(node_handle.getParam("topics/armclient", armclient));
|
||||
else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(node_handle.getParam("topics/modeclient", modeclient));
|
||||
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(node_handle.getParam("topics/stream", stream_client_name));
|
||||
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}
|
||||
if(node_handle.getParam("topics/localpos", local_pos_sub_name));
|
||||
else {ROS_ERROR("Provide a localpos name in YAML file"); system("rosnode kill rosbuzz_node");}
|
||||
|
||||
|
||||
|
||||
if (node_handle.getParam("topics/fcclient", fcclient_name))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a fc client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/setpoint", setpoint_name))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a Set Point name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/armclient", armclient))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide an arm client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/modeclient", modeclient))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a mode client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/stream", stream_client_name))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a mode client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/localpos", local_pos_sub_name))
|
||||
;
|
||||
else {
|
||||
ROS_ERROR("Provide a localpos name in YAML file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
}
|
||||
|
||||
/*--------------------------------------------------------
|
||||
/ Create all required subscribers, publishers and ROS service clients
|
||||
/-------------------------------------------------------*/
|
||||
void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c){
|
||||
void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
|
||||
{
|
||||
/*subscribers*/
|
||||
|
||||
Subscribe(n_c);
|
||||
|
||||
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
|
||||
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
|
||||
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this);
|
||||
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
|
||||
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
|
||||
obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
|
||||
// current_position_sub = n_c.subscribe("/global_position", 1000,
|
||||
// &roscontroller::current_pos,this);
|
||||
// battery_sub = n_c.subscribe("/power_status", 1000,
|
||||
// &roscontroller::battery,this);
|
||||
payload_sub =
|
||||
n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt, this);
|
||||
// flight_status_sub =n_c.subscribe("/flight_status",100,
|
||||
// &roscontroller::flight_extended_status_update,this);
|
||||
// Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000,
|
||||
// &roscontroller::set_robot_id,this);
|
||||
obstacle_sub = n_c.subscribe("guidance/obstacle_distance", 100,
|
||||
&roscontroller::obstacle_dist, this);
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 1000);
|
||||
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name,1000);
|
||||
localsetpoint_nonraw_pub =
|
||||
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 1000);
|
||||
/* Service Clients*/
|
||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
||||
if (rcclient == true)
|
||||
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this);
|
||||
service =
|
||||
n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
|
||||
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
|
||||
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
stream_client =
|
||||
n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
|
||||
users_sub = n_c.subscribe("users_pos", 1000, &roscontroller::users_pos, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000, &roscontroller::local_pos_callback, this);
|
||||
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 1000,
|
||||
&roscontroller::local_pos_callback, this);
|
||||
|
||||
multi_msg = true;
|
||||
}
|
||||
/*---------------------------------------
|
||||
/Robot independent subscribers
|
||||
/--------------------------------------*/
|
||||
void roscontroller::Subscribe(ros::NodeHandle& n_c){
|
||||
|
||||
for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
|
||||
void roscontroller::Subscribe(ros::NodeHandle &n_c)
|
||||
{
|
||||
for (std::map<std::string, std::string>::iterator it =
|
||||
m_smTopic_infos.begin();
|
||||
it != m_smTopic_infos.end(); ++it) {
|
||||
if (it->second == "mavros_msgs/ExtendedState") {
|
||||
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this);
|
||||
}
|
||||
else if(it->second == "mavros_msgs/State"){
|
||||
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this);
|
||||
}
|
||||
else if(it->second == "mavros_msgs/BatteryStatus"){
|
||||
battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
|
||||
}
|
||||
else if(it->second == "sensor_msgs/NavSatFix"){
|
||||
current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
|
||||
}
|
||||
else if(it->second == "std_msgs/Float64"){
|
||||
relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
|
||||
flight_status_sub = n_c.subscribe(
|
||||
it->first, 100, &roscontroller::flight_extended_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/State") {
|
||||
flight_status_sub = n_c.subscribe(
|
||||
it->first, 100, &roscontroller::flight_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/BatteryStatus") {
|
||||
battery_sub =
|
||||
n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
|
||||
} else if (it->second == "sensor_msgs/NavSatFix") {
|
||||
current_position_sub =
|
||||
n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
|
||||
} else if (it->second == "std_msgs/Float64") {
|
||||
relative_altitude_sub =
|
||||
n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
|
||||
}
|
||||
|
||||
std::cout << "Subscribed to: " << it->first << endl;
|
||||
|
|
@ -300,19 +464,23 @@ namespace rosbzz_node{
|
|||
/*--------------------------------------------------------
|
||||
/ Create Buzz bytecode from the bzz script inputed
|
||||
/-------------------------------------------------------*/
|
||||
std::string roscontroller::Compile_bzz(std::string bzzfile_name){
|
||||
std::string roscontroller::Compile_bzz(std::string bzzfile_name)
|
||||
{
|
||||
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
|
||||
/*Compile the buzz code .bzz to .bo*/
|
||||
stringstream bzzfile_in_compile;
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
std::string path =
|
||||
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
// bzzfile_in_compile << path << "/";
|
||||
// path = bzzfile_in_compile.str();
|
||||
// bzzfile_in_compile.str("");
|
||||
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name = name.substr(0, name.find_last_of("."));
|
||||
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
|
||||
bzzfile_in_compile << "bzzc -I " << path
|
||||
<< "include/"; //<<" "<<path<< name<<".basm";
|
||||
// bzzfile_in_compile.str("");
|
||||
//bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
|
||||
// bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo
|
||||
// "<<path<<name<<".bdbg";
|
||||
// system(bzzfile_in_compile.str().c_str());
|
||||
// bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile << " -b " << path << name << ".bo";
|
||||
|
|
@ -333,15 +501,18 @@ namespace rosbzz_node{
|
|||
/*----------------------------------------------------
|
||||
/ Publish neighbours pos and id in neighbours pos topic
|
||||
/----------------------------------------------------*/
|
||||
void roscontroller::neighbours_pos_publisher(){
|
||||
void roscontroller::neighbours_pos_publisher()
|
||||
{
|
||||
auto current_time = ros::Time::now();
|
||||
map<int, buzz_utility::Pos_struct>::iterator it;
|
||||
rosbuzz::neigh_pos neigh_pos_array; // neigh_pos_array.clear();
|
||||
neigh_pos_array.header.frame_id = "/world";
|
||||
neigh_pos_array.header.stamp = current_time;
|
||||
for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
|
||||
for (it = raw_neighbours_pos_map.begin(); it != raw_neighbours_pos_map.end();
|
||||
++it) {
|
||||
sensor_msgs::NavSatFix neigh_tmp;
|
||||
//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
|
||||
// cout<<"iterator it val: "<< it-> first << " After convertion: "
|
||||
// <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;
|
||||
neigh_tmp.header.stamp = current_time;
|
||||
neigh_tmp.header.frame_id = "/world";
|
||||
neigh_tmp.position_covariance_type = it->first; // custom robot id storage
|
||||
|
|
@ -357,7 +528,8 @@ namespace rosbzz_node{
|
|||
/*--------------------------------------------------------
|
||||
/ Functions handling the local MAV ROS flight controller
|
||||
/-------------------------------------------------------*/
|
||||
void roscontroller::Arm(){
|
||||
void roscontroller::Arm()
|
||||
{
|
||||
mavros_msgs::CommandBool arming_message;
|
||||
arming_message.request.value = armstate;
|
||||
if (arm_client.call(arming_message)) {
|
||||
|
|
@ -374,18 +546,28 @@ namespace rosbzz_node{
|
|||
/Prepare Buzz messages payload for each step and publish
|
||||
/-----------------------------------------------------------------------------------------------------*/
|
||||
/*----------------------------------------------------------------------------------------------------*/
|
||||
/* Message format of payload (Each slot is uint64_t) /
|
||||
/ _________________________________________________________________________________________________ /
|
||||
/| | | | | | /
|
||||
/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | /
|
||||
/|_____|_____|_____|________________________________________________|______________________________| */
|
||||
/* Message format of payload (Each slot is uint64_t)
|
||||
/
|
||||
/
|
||||
_________________________________________________________________________________________________
|
||||
/
|
||||
/| | | |
|
||||
| | /
|
||||
/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs
|
||||
with size......... | /
|
||||
/|_____|_____|_____|________________________________________________|______________________________|
|
||||
*/
|
||||
/*----------------------------------------------------------------------------------------------------*/
|
||||
void roscontroller::prepare_msg_and_publish(){
|
||||
void roscontroller::prepare_msg_and_publish()
|
||||
{
|
||||
/*obtain Pay load to be sent*/
|
||||
uint64_t *payload_out_ptr = buzz_utility::obt_out_msg();
|
||||
uint64_t position[3];
|
||||
/*Appened current position to message*/
|
||||
double tmp[3];tmp[0]=cur_pos.latitude;tmp[1]=cur_pos.longitude;tmp[2]=cur_pos.altitude;
|
||||
double tmp[3];
|
||||
tmp[0] = cur_pos.latitude;
|
||||
tmp[1] = cur_pos.longitude;
|
||||
tmp[2] = cur_pos.altitude;
|
||||
memcpy(position, tmp, 3 * sizeof(uint64_t));
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
|
|
@ -397,8 +579,10 @@ namespace rosbzz_node{
|
|||
payload_out.payload64.push_back(payload_out_ptr[i]);
|
||||
}
|
||||
/*Add Robot id and message number to the published message*/
|
||||
if (message_number < 0) message_number=0;
|
||||
else message_number++;
|
||||
if (message_number < 0)
|
||||
message_number = 0;
|
||||
else
|
||||
message_number++;
|
||||
payload_out.sysid = (uint8_t)robot_id;
|
||||
payload_out.msgid = (uint32_t)message_number;
|
||||
|
||||
|
|
@ -407,9 +591,12 @@ namespace rosbzz_node{
|
|||
delete[] out;
|
||||
delete[] payload_out_ptr;
|
||||
/*Check for updater message if present send*/
|
||||
if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
|
||||
if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 &&
|
||||
multi_msg)
|
||||
{
|
||||
uint8_t *buff_send = 0;
|
||||
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
|
||||
uint16_t updater_msgSize = *(uint16_t *)(getupdate_out_msg_size());
|
||||
;
|
||||
int tot = 0;
|
||||
mavros_msgs::Mavlink update_packets;
|
||||
fprintf(stdout, "Transfering code \n");
|
||||
|
|
@ -428,7 +615,8 @@ namespace rosbzz_node{
|
|||
destroy_out_msg_queue();
|
||||
uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t)));
|
||||
uint64_t *payload_64 = new uint64_t[total_size];
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||
memcpy((void *)payload_64, (void *)buff_send,
|
||||
total_size * sizeof(uint64_t));
|
||||
free(buff_send);
|
||||
// Send a constant number to differenciate updater msgs
|
||||
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
|
||||
|
|
@ -436,29 +624,36 @@ namespace rosbzz_node{
|
|||
update_packets.payload64.push_back(payload_64[i]);
|
||||
}
|
||||
// Add Robot id and message number to the published message
|
||||
if (message_number < 0) message_number=0;
|
||||
else message_number++;
|
||||
if (message_number < 0)
|
||||
message_number = 0;
|
||||
else
|
||||
message_number++;
|
||||
update_packets.sysid = (uint8_t)robot_id;
|
||||
update_packets.msgid = (uint32_t)message_number;
|
||||
payload_pub.publish(update_packets);
|
||||
multi_msg = false;
|
||||
delete[] payload_64;
|
||||
}
|
||||
|
||||
}
|
||||
/*---------------------------------------------------------------------------------
|
||||
/Flight controller service call every step if there is a command set from bzz script
|
||||
/-------------------------------------------------------------------------------- */
|
||||
void roscontroller::flight_controller_service_call() {
|
||||
/Flight controller service call every step if there is a command set from bzz
|
||||
script
|
||||
/--------------------------------------------------------------------------------
|
||||
*/
|
||||
void roscontroller::flight_controller_service_call()
|
||||
{
|
||||
/* flight controller client call if requested from Buzz*/
|
||||
/*FC call for takeoff,land, gohome and arm/disarm*/
|
||||
int tmp = buzzuav_closures::bzz_cmd();
|
||||
double *goto_pos = buzzuav_closures::getgoto();
|
||||
if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
|
||||
if (tmp == buzzuav_closures::COMMAND_TAKEOFF ||
|
||||
tmp == buzzuav_closures::COMMAND_LAND ||
|
||||
tmp == buzzuav_closures::COMMAND_GOHOME) {
|
||||
cmd_srv.request.param7 = goto_pos[2];
|
||||
cmd_srv.request.command = buzzuav_closures::getcmd();
|
||||
// std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
|
||||
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
|
||||
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND
|
||||
// mode
|
||||
switch (buzzuav_closures::getcmd()) {
|
||||
case mavros_msgs::CommandCode::NAV_LAND:
|
||||
if (current_mode != "LAND") {
|
||||
|
|
@ -468,8 +663,9 @@ namespace rosbzz_node{
|
|||
}
|
||||
if (mav_client.call(cmd_srv)) {
|
||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||
} else
|
||||
{ROS_ERROR("Failed to call service from flight controller");}
|
||||
} else {
|
||||
ROS_ERROR("Failed to call service from flight controller");
|
||||
}
|
||||
armstate = 0;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_TAKEOFF:
|
||||
|
|
@ -483,10 +679,11 @@ namespace rosbzz_node{
|
|||
home = cur_pos;
|
||||
}
|
||||
if (current_mode != "GUIDED")
|
||||
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
|
||||
if (mav_client.call(cmd_srv))
|
||||
{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);}
|
||||
else
|
||||
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
|
||||
// should always be in loiter after arm/disarm)
|
||||
if (mav_client.call(cmd_srv)) {
|
||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||
} else
|
||||
ROS_ERROR("Failed to call service from flight controller");
|
||||
break;
|
||||
}
|
||||
|
|
@ -521,7 +718,8 @@ namespace rosbzz_node{
|
|||
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
|
||||
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
|
||||
|
||||
//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
|
||||
// roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0],
|
||||
// goto_pos[2], 0);
|
||||
}
|
||||
}
|
||||
/*----------------------------------------------
|
||||
|
|
@ -530,15 +728,18 @@ namespace rosbzz_node{
|
|||
void roscontroller::maintain_pos(int tim_step) {
|
||||
if (timer_step >= BUZZRATE) {
|
||||
neighbours_pos_map.clear();
|
||||
//raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear !
|
||||
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
|
||||
// have to clear !
|
||||
timer_step = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------------
|
||||
/Puts neighbours position into local struct storage that is cleared every 10 step
|
||||
/Puts neighbours position into local struct storage that is cleared every 10
|
||||
step
|
||||
/---------------------------------------------------------------------------------*/
|
||||
void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
|
||||
void roscontroller::neighbours_pos_put(int id,
|
||||
buzz_utility::Pos_struct pos_arr) {
|
||||
map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.find(id);
|
||||
if (it != neighbours_pos_map.end())
|
||||
neighbours_pos_map.erase(it);
|
||||
|
|
@ -547,8 +748,10 @@ namespace rosbzz_node{
|
|||
/*-----------------------------------------------------------------------------------
|
||||
/Puts raw neighbours position into local storage for neighbours pos publisher
|
||||
/-----------------------------------------------------------------------------------*/
|
||||
void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
|
||||
map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id);
|
||||
void roscontroller::raw_neighbours_pos_put(int id,
|
||||
buzz_utility::Pos_struct pos_arr) {
|
||||
map<int, buzz_utility::Pos_struct>::iterator it =
|
||||
raw_neighbours_pos_map.find(id);
|
||||
if (it != raw_neighbours_pos_map.end())
|
||||
raw_neighbours_pos_map.erase(it);
|
||||
raw_neighbours_pos_map.insert(make_pair(id, pos_arr));
|
||||
|
|
@ -557,8 +760,7 @@ namespace rosbzz_node{
|
|||
/*--------------------------------------------------------
|
||||
/Set the current position of the robot callback
|
||||
/--------------------------------------------------------*/
|
||||
void roscontroller::set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
void roscontroller::set_cur_pos(double latitude, double longitude,
|
||||
double altitude) {
|
||||
cur_pos.latitude = latitude;
|
||||
cur_pos.longitude = longitude;
|
||||
|
|
@ -592,8 +794,7 @@ namespace rosbzz_node{
|
|||
M[2][2] = -sin_el;
|
||||
}
|
||||
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a,
|
||||
const double *b, double *c)
|
||||
{
|
||||
const double *b, double *c) {
|
||||
uint32_t i, j, k;
|
||||
for (i = 0; i < n; i++)
|
||||
for (j = 0; j < p; j++) {
|
||||
|
|
@ -603,8 +804,7 @@ namespace rosbzz_node{
|
|||
}
|
||||
}
|
||||
|
||||
void roscontroller::gps_rb(GPS nei_pos, double out[])
|
||||
{
|
||||
void roscontroller::gps_rb(GPS nei_pos, double out[]) {
|
||||
float ned_x = 0.0, ned_y = 0.0;
|
||||
gps_ned_cur(ned_x, ned_y, nei_pos);
|
||||
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
|
||||
|
|
@ -614,24 +814,19 @@ namespace rosbzz_node{
|
|||
out[2] = 0.0;
|
||||
}
|
||||
|
||||
void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t)
|
||||
{
|
||||
gps_convert_ned(ned_x, ned_y,
|
||||
t.longitude, t.latitude,
|
||||
cur_pos.longitude, cur_pos.latitude);
|
||||
void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) {
|
||||
gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude,
|
||||
cur_pos.latitude);
|
||||
}
|
||||
|
||||
void roscontroller::gps_ned_home(float &ned_x, float &ned_y)
|
||||
{
|
||||
gps_convert_ned(ned_x, ned_y,
|
||||
cur_pos.longitude, cur_pos.latitude,
|
||||
void roscontroller::gps_ned_home(float &ned_x, float &ned_y) {
|
||||
gps_convert_ned(ned_x, ned_y, cur_pos.longitude, cur_pos.latitude,
|
||||
home.longitude, home.latitude);
|
||||
}
|
||||
|
||||
void roscontroller::gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat)
|
||||
{
|
||||
double gps_r_lon, double gps_r_lat) {
|
||||
double d_lon = gps_t_lon - gps_r_lon;
|
||||
double d_lat = gps_t_lat - gps_r_lat;
|
||||
ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||
|
|
@ -643,13 +838,15 @@ namespace rosbzz_node{
|
|||
/------------------------------------------------------*/
|
||||
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr &msg) {
|
||||
buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining);
|
||||
//ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining);
|
||||
// ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage,
|
||||
// msg->current, msg ->remaining);
|
||||
}
|
||||
|
||||
/*----------------------------------------------------------------------
|
||||
/Update flight extended status into BVM from subscriber for solos
|
||||
/---------------------------------------------------------------------*/
|
||||
void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
|
||||
void roscontroller::flight_status_update(
|
||||
const mavros_msgs::State::ConstPtr &msg) {
|
||||
// http://wiki.ros.org/mavros/CustomModes
|
||||
// TODO: Handle landing
|
||||
std::cout << "Message: " << msg->mode << std::endl;
|
||||
|
|
@ -664,7 +861,8 @@ namespace rosbzz_node{
|
|||
/*------------------------------------------------------------
|
||||
/Update flight extended status into BVM from subscriber
|
||||
------------------------------------------------------------*/
|
||||
void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
|
||||
void roscontroller::flight_extended_status_update(
|
||||
const mavros_msgs::ExtendedState::ConstPtr &msg) {
|
||||
buzzuav_closures::flight_status_update(msg->landed_state);
|
||||
}
|
||||
/*-------------------------------------------------------------
|
||||
|
|
@ -680,11 +878,14 @@ namespace rosbzz_node{
|
|||
home[1]=lon;
|
||||
home[2]=cur_rel_altitude;
|
||||
}*/
|
||||
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
|
||||
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude);
|
||||
set_cur_pos(msg->latitude, msg->longitude,
|
||||
cur_rel_altitude); // msg->altitude);
|
||||
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude,
|
||||
cur_rel_altitude); // msg->altitude);
|
||||
}
|
||||
|
||||
void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose){
|
||||
void roscontroller::local_pos_callback(
|
||||
const geometry_msgs::PoseStamped::ConstPtr &pose) {
|
||||
local_pos_new[0] = pose->pose.position.x;
|
||||
local_pos_new[1] = pose->pose.position.y;
|
||||
local_pos_new[2] = pose->pose.position.z;
|
||||
|
|
@ -694,15 +895,14 @@ namespace rosbzz_node{
|
|||
|
||||
int n = data.pos_neigh.size();
|
||||
// ROS_INFO("Users array size: %i\n", n);
|
||||
if(n>0)
|
||||
{
|
||||
for(int it=0; it<n; ++it)
|
||||
{
|
||||
buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
|
||||
if (n > 0) {
|
||||
for (int it = 0; it < n; ++it) {
|
||||
buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,
|
||||
data.pos_neigh[it].latitude,
|
||||
data.pos_neigh[it].longitude,
|
||||
data.pos_neigh[it].altitude);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
/*-------------------------------------------------------------
|
||||
/ Update altitude into BVM from subscriber
|
||||
|
|
@ -710,7 +910,6 @@ namespace rosbzz_node{
|
|||
void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr &msg) {
|
||||
// ROS_INFO("Altitude in: %f", msg->data);
|
||||
cur_rel_altitude = (double)msg->data;
|
||||
|
||||
}
|
||||
/*-------------------------------------------------------------
|
||||
/Set obstacle Obstacle distance table into BVM from subscriber
|
||||
|
|
@ -732,10 +931,13 @@ namespace rosbzz_node{
|
|||
moveMsg.header.frame_id = 1;
|
||||
// float ned_x, ned_y;
|
||||
// gps_ned_home(ned_x, ned_y);
|
||||
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
|
||||
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
|
||||
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0],
|
||||
// home[1]);
|
||||
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id,
|
||||
// local_pos[0], local_pos[1]);
|
||||
|
||||
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
|
||||
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD
|
||||
* (then converted to NED)*/
|
||||
// target[0]+=y; target[1]+=x;
|
||||
moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y;
|
||||
moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+x;
|
||||
|
|
@ -770,7 +972,6 @@ namespace rosbzz_node{
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
void roscontroller::SetStreamRate(int id, int rate, int on_off) {
|
||||
mavros_msgs::StreamRate message;
|
||||
message.request.stream_id = id;
|
||||
|
|
@ -788,15 +989,21 @@ namespace rosbzz_node{
|
|||
/Push payload into BVM FIFO
|
||||
/-------------------------------------------------------------*/
|
||||
/*******************************************************************************************************/
|
||||
/* Message format of payload (Each slot is uint64_t) */
|
||||
/* _________________________________________________________________________________________________ */
|
||||
/*| | | | | | */
|
||||
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size......... | */
|
||||
/*|_____|_____|_____|________________________________________________|______________________________| */
|
||||
/* Message format of payload (Each slot is uint64_t)
|
||||
*/
|
||||
/* _________________________________________________________________________________________________
|
||||
*/
|
||||
/*| | | | |
|
||||
* | */
|
||||
/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs
|
||||
* with size......... | */
|
||||
/*|_____|_____|_____|________________________________________________|______________________________|
|
||||
*/
|
||||
/*******************************************************************************************************/
|
||||
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
|
||||
/*Check for Updater message, if updater message push it into updater FIFO*/
|
||||
if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 5){
|
||||
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT &&
|
||||
msg->payload64.size() > 5) {
|
||||
uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
|
||||
uint64_t message_obt[obt_msg_size];
|
||||
/* Go throught the obtained payload*/
|
||||
|
|
@ -813,7 +1020,8 @@ namespace rosbzz_node{
|
|||
// tot+=sizeof(uint16_t);
|
||||
fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize);
|
||||
if (unMsgSize > 0) {
|
||||
code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize);
|
||||
code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)),
|
||||
unMsgSize);
|
||||
// fprintf(stdout,"before in queue process : utils\n");
|
||||
code_message_inqueue_process();
|
||||
// fprintf(stdout,"after in queue process : utils\n");
|
||||
|
|
@ -832,28 +1040,34 @@ namespace rosbzz_node{
|
|||
/* Extract neighbours position from payload*/
|
||||
double neighbours_pos_payload[3];
|
||||
memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t));
|
||||
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],neighbours_pos_payload[1],neighbours_pos_payload[2]);
|
||||
buzz_utility::Pos_struct raw_neigh_pos(neighbours_pos_payload[0],
|
||||
neighbours_pos_payload[1],
|
||||
neighbours_pos_payload[2]);
|
||||
GPS nei_pos;
|
||||
nei_pos.latitude = neighbours_pos_payload[0];
|
||||
nei_pos.longitude = neighbours_pos_payload[1];
|
||||
nei_pos.altitude = neighbours_pos_payload[2];
|
||||
double cvt_neighbours_pos_payload[3];
|
||||
// cout<<"Got" << neighbours_pos_payload[0] <<", " << neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
|
||||
// cout<<"Got" << neighbours_pos_payload[0] <<", " <<
|
||||
//neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
|
||||
gps_rb(nei_pos, cvt_neighbours_pos_payload);
|
||||
/*Extract robot id of the neighbour*/
|
||||
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
|
||||
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
|
||||
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0],
|
||||
cvt_neighbours_pos_payload[1]);
|
||||
/*pass neighbour position to local maintaner*/
|
||||
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
|
||||
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
|
||||
cvt_neighbours_pos_payload[1],
|
||||
cvt_neighbours_pos_payload[2]);
|
||||
/*Put RID and pos*/
|
||||
raw_neighbours_pos_put((int)out[1], raw_neigh_pos);
|
||||
/* TODO: remove roscontroller local map array for neighbors */
|
||||
neighbours_pos_put((int)out[1], n_pos);
|
||||
buzzuav_closures::neighbour_pos_callback((int)out[1],n_pos.x,n_pos.y,n_pos.z);
|
||||
buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y,
|
||||
n_pos.z);
|
||||
delete[] out;
|
||||
buzz_utility::in_msg_append((message_obt + 3));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -883,8 +1097,7 @@ namespace rosbzz_node{
|
|||
ROS_INFO("RC_Call: ARM!!!!");
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
}
|
||||
else{
|
||||
} else {
|
||||
ROS_INFO("RC_Call: DISARM!!!!");
|
||||
buzzuav_closures::rc_call(rc_cmd + 1);
|
||||
res.success = true;
|
||||
|
|
@ -930,21 +1143,18 @@ namespace rosbzz_node{
|
|||
if (no_of_robots == 0) {
|
||||
no_of_robots = cur_robots;
|
||||
|
||||
}
|
||||
else{
|
||||
} else {
|
||||
if (no_of_robots != cur_robots && no_cnt == 0) {
|
||||
no_cnt++;
|
||||
old_val = cur_robots;
|
||||
|
||||
}
|
||||
else if(no_cnt!=0 && old_val==cur_robots){
|
||||
} else if (no_cnt != 0 && old_val == cur_robots) {
|
||||
no_cnt++;
|
||||
if (no_cnt >= 150 || cur_robots > no_of_robots) {
|
||||
no_of_robots = cur_robots;
|
||||
no_cnt = 0;
|
||||
}
|
||||
}
|
||||
else{
|
||||
} else {
|
||||
no_cnt = 0;
|
||||
}
|
||||
}
|
||||
|
|
@ -984,5 +1194,4 @@ namespace rosbzz_node{
|
|||
}
|
||||
*/
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue