merge rosbuzz

This commit is contained in:
dave 2017-06-26 14:35:15 -04:00
commit eb130be1bb
9 changed files with 1492 additions and 1254 deletions

View File

@ -42,9 +42,11 @@
using namespace std; using namespace std;
namespace rosbzz_node{ namespace rosbzz_node
{
class roscontroller{ class roscontroller
{
public: public:
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv); roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
@ -59,15 +61,14 @@ private:
uint8_t index = 0; uint8_t index = 0;
uint8_t current = 0; uint8_t current = 0;
num_robot_count(){} num_robot_count(){}
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
}; typedef struct num_robot_count Num_robot_count ;
struct gps struct gps
{ {
double longitude=0.0; double longitude=0.0;
double latitude=0.0; double latitude=0.0;
float altitude=0.0; float altitude=0.0;
}; typedef struct gps GPS ; }; typedef struct gps GPS ; // not useful in cpp
GPS target, home, cur_pos; GPS target, home, cur_pos;
double cur_rel_altitude; double cur_rel_altitude;
@ -80,6 +81,7 @@ private:
int robot_id=0; int robot_id=0;
std::string robot_name = ""; std::string robot_name = "";
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout; float fcu_timeout;
int armstate; int armstate;
@ -233,7 +235,15 @@ private:
void SetStreamRate(int id, int rate, int on_off); void SetStreamRate(int id, int rate, int on_off);
void get_number_of_robots(); void get_number_of_robots();
void GetRobotId(); 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);
}; };
} }

View File

@ -1,14 +1,14 @@
<launch> <launch>
<!-- RUN mavros --> <!-- RUN mavros -->
<arg name="fcu_url" default="/dev/ttyAMA0:115200" /> <arg name="fcu_url" default="/dev/ttyS0:115200" />
<arg name="gcs_url" default="" /> <arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" /> <arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" /> <arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" /> <arg name="log_output" default="screen" />
<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch"> <include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" /> <arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_config.yaml" /> <arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" /> <arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" /> <arg name="gcs_url" value="$(arg gcs_url)" />
@ -36,13 +36,14 @@
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" /> <param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" /> <param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" /> <param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
<param name="Baud_rate" type="double" value="230400" />
<!-- run rosbuzz --> <!-- run rosbuzz -->
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="/home/pi/ros_catkinKin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/> <rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" /> <param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" /> <param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/> <param name="in_payload" value="inMavlink"/>
@ -50,7 +51,7 @@
<param name="xbee_status_srv" value="xbee_status"/> <param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/> <param name="xbee_plugged" value="true"/>
<param name="name" value="solos1"/> <param name="name" value="solos1"/>
<param name="stand_by" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/stand_by.bzz"/> <param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
</node> </node>
</launch> </launch>

View File

@ -16,6 +16,7 @@ function testWP {
} }
function record { function record {
rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
} }
function clean { function clean {
sudo rm /var/log/upstart/robot* sudo rm /var/log/upstart/robot*
@ -23,10 +24,18 @@ function clean {
sudo rm /var/log/upstart/x3s* sudo rm /var/log/upstart/x3s*
} }
function startrobot { function startrobot {
sudo service robot start sudo service dji start
} }
function stoprobot { function stoprobot {
sudo service robot stop sudo service dji stop
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
}
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
} }
function updaterobot { function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch

View File

@ -1,5 +1,9 @@
0 -1 -1 -1 0 -1 -1 -1
1 0 1000.0 0.0 1 0 1000.0 0.0
2 0 1000.0 1.57 2 0 1000.0 1.57
<<<<<<< HEAD
3 0 1000.0 3.14 3 0 1000.0 3.14
4 0 1000.0 4.71 4 0 1000.0 4.71
=======
3 0 1000.0 3.14
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7

View File

@ -50,7 +50,11 @@ function land() {
uav_land() uav_land()
} }
else { else {
<<<<<<< HEAD
barrier_set(ROBOTS,turnedoff,land) barrier_set(ROBOTS,turnedoff,land)
=======
barrier_set(ROBOTS,idle,land)
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
barrier_ready() barrier_ready()
timeW=0 timeW=0
#barrier = nil #barrier = nil
@ -58,6 +62,7 @@ function land() {
} }
} }
<<<<<<< HEAD
function follow() { function follow() {
if(size(targets)>0) { if(size(targets)>0) {
UAVSTATE = "FOLLOW" UAVSTATE = "FOLLOW"
@ -74,6 +79,8 @@ function follow() {
} }
} }
=======
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
function uav_rccmd() { function uav_rccmd() {
if(flight.rc_cmd==22) { if(flight.rc_cmd==22) {
log("cmd 22") log("cmd 22")
@ -90,11 +97,17 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
<<<<<<< HEAD
UAVSTATE = "FOLLOW" UAVSTATE = "FOLLOW"
log(rc_goto.latitude, " ", rc_goto.longitude) log(rc_goto.latitude, " ", rc_goto.longitude)
add_targetrb(0,rc_goto.latitude,rc_goto.longitude) add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
statef = follow statef = follow
#uav_goto() #uav_goto()
=======
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
>>>>>>> 8e5acd264dbf2b5fc3cafd00fb9a08a82a8095f7
} else if(flight.rc_cmd==400) { } else if(flight.rc_cmd==400) {
flight.rc_cmd=0 flight.rc_cmd=0
uav_arm() uav_arm()

View File

@ -405,7 +405,7 @@ int create_stig_tables() {
//buzzvm_gstore(VM); //buzzvm_gstore(VM);
//buzzvm_dump(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_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
buzzvm_tget(VM); buzzvm_tget(VM);
@ -489,6 +489,7 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id); ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0; return 0;
} }
/* Create vstig tables /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
@ -777,5 +778,3 @@ int create_stig_tables() {
buzzvm_gstore(VM); buzzvm_gstore(VM);
} }
} }

View File

@ -10,7 +10,6 @@
#include "math.h" #include "math.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
//static const rosbzz_node::roscontroller* roscontroller_ptr; //static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/ /*forward declaration of ros controller ptr storing function*/

View File

@ -39,13 +39,13 @@ namespace rosbzz_node{
ros::spinOnce(); ros::spinOnce();
} }
if (xbeeplugged) { if (xbeeplugged) {
GetRobotId(); GetRobotId();
} else { } else {
robot_id = strtol(robot_name.c_str() + 5, NULL, 10); 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"; path += "Update.log";
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out); log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
} }
@ -65,27 +65,122 @@ namespace rosbzz_node{
void roscontroller::GetRobotId() 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; 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::Duration(0.1).sleep();
ROS_ERROR("Waiting for Xbee to respond to get device ID"); ROS_ERROR("Waiting for Xbee to respond to get device ID");
} }
robot_id = robot_id_srv_response.value.integer; 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 /rosbuzz_node loop method executed once every step
/--------------------------------------------------*/ /--------------------------------------------------*/
void roscontroller::RosControllerRun(){ void roscontroller::RosControllerRun()
{
/* Set the Buzz bytecode */ /* 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); ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo"; std::string standby_bo = Compile_bzz(stand_by) + ".bo";
init_update_monitor(bcfname.c_str(), standby_bo.c_str()); init_update_monitor(bcfname.c_str(), standby_bo.c_str());
@ -93,50 +188,49 @@ namespace rosbzz_node{
// MAIN LOOP // MAIN LOOP
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
//ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); //ROS_WARN("[%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*/ /*Update neighbors position inside Buzz*/
// buzz_closure::neighbour_pos_callback(neighbours_pos_map); // buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
//ROS_WARN("[%i]-----------NEIG", robot_id);
neighbours_pos_publisher(); neighbours_pos_publisher();
/*Check updater state and step code*/ /*Check updater state and step code*/
//ROS_WARN("[%i]-----------UPD", robot_id);
update_routine(bcfname.c_str(), dbgfname.c_str()); update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */ /*Step buzz script */
//ROS_WARN("[%i]-----------STEP", robot_id);
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
//ROS_WARN("[%i]-----------MSG", robot_id);
prepare_msg_and_publish(); prepare_msg_and_publish();
/*call flight controler service to set command long*/ /*call flight controler service to set command long*/
//ROS_WARN("[%i]-----------FC", robot_id);
flight_controller_service_call(); flight_controller_service_call();
/*Set multi message available after update*/ /*Set multi message available after update*/
//ROS_WARN("[%i]-----------UPD", robot_id); if (get_update_status())
if(get_update_status()){ {
set_read_update_status(); set_read_update_status();
multi_msg = true; 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); 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(); log << "," << neighbours_pos_map.size();
for(;it != neighbours_pos_map.end();++it){ for (; it != neighbours_pos_map.end(); ++it)
log<<","<<(double)it->second.x<<","<<(double)it->second.y<<","<<(double)it->second.z; {
log << "," << (double)it->second.x << "," << (double)it->second.y
<< "," << (double)it->second.z;
} }
log << std::endl; log << std::endl;
} }
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/ /*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots(); // no_of_robots=get_number_of_robots();
//ROS_WARN("[%i]-----------ROBOTS", robot_id);
get_number_of_robots(); get_number_of_robots();
buzz_utility::set_robot_var(no_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); // buzz_utility::set_robot_var(no_of_robots);
/*Set no of robots for updates TODO only when not updating*/ /*Set no of robots for updates TODO only when not updating*/
// if(multi_msg) // if(multi_msg)
updates_set_robots(no_of_robots); 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*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
/*loop rate of ros*/ /*loop rate of ros*/
@ -160,21 +254,34 @@ namespace rosbzz_node{
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Get all required parameters from the ROS launch file / 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*/ /*Obtain .bzz file name from parameter server*/
if(n_c.getParam("bzzfile_name", bzzfile_name)); 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");} ;
else {
ROS_ERROR("Provide a .bzz file to run in Launch file");
system("rosnode kill rosbuzz_node");
}
/*Obtain rc service option from parameter server*/ /*Obtain rc service option from parameter server*/
if(n_c.getParam("rcclient", rcclient)){ if (n_c.getParam("rcclient", rcclient))
{
if (rcclient == true) { if (rcclient == true) {
/*Service*/ /*Service*/
if (!n_c.getParam("rcservice_name", rcservice_name)) 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*/ /*Obtain robot_id from parameter server*/
// n_c.getParam("robot_id", robot_id); // n_c.getParam("robot_id", robot_id);
// robot_id=(int)buzz_utility::get_robotid(); // robot_id=(int)buzz_utility::get_robotid();
@ -185,11 +292,19 @@ namespace rosbzz_node{
/*Obtain standby script to run during update*/ /*Obtain standby script to run during update*/
n_c.getParam("stand_by", stand_by); n_c.getParam("stand_by", stand_by);
if(n_c.getParam("xbee_plugged", xbeeplugged)); if (n_c.getParam("xbee_plugged", xbeeplugged))
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} ;
else {
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node");
}
if (!xbeeplugged) { if (!xbeeplugged) {
if(n_c.getParam("name", robot_name)); if (n_c.getParam("name", robot_name))
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} ;
else {
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
system("rosnode kill rosbuzz_node");
}
} else } else
n_c.getParam("xbee_status_srv", xbeesrv_name); n_c.getParam("xbee_status_srv", xbeesrv_name);
@ -197,107 +312,149 @@ namespace rosbzz_node{
GetSubscriptionParameters(n_c); GetSubscriptionParameters(n_c);
// initialize topics to null? // 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? // todo: make it as an array in yaml?
m_sMySubscriptions.clear(); m_sMySubscriptions.clear();
std::string gps_topic, gps_type; std::string gps_topic, gps_type;
if(node_handle.getParam("topics/gps", gps_topic)); if (node_handle.getParam("topics/gps", gps_topic))
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");} ;
else {
ROS_ERROR("Provide a gps topic in Launch file");
system("rosnode kill rosbuzz_node");
}
node_handle.getParam("type/gps", gps_type); node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type)); m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type));
std::string battery_topic, battery_type; std::string battery_topic, battery_type;
node_handle.getParam("topics/battery", battery_topic); node_handle.getParam("topics/battery", battery_topic);
node_handle.getParam("type/battery", battery_type); 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; std::string status_topic, status_type;
node_handle.getParam("topics/status", status_topic); node_handle.getParam("topics/status", status_topic);
node_handle.getParam("type/status", status_type); 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; std::string altitude_topic, altitude_type;
node_handle.getParam("topics/altitude", altitude_topic); node_handle.getParam("topics/altitude", altitude_topic);
node_handle.getParam("type/altitude", altitude_type); 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*/ /*Obtain fc client name from parameter server*/
if(node_handle.getParam("topics/fcclient", fcclient_name)); 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 {
else {ROS_ERROR("Provide a Set Point name in Launch file"); system("rosnode kill rosbuzz_node");} ROS_ERROR("Provide a fc client name in Launch file");
if(node_handle.getParam("topics/armclient", armclient)); system("rosnode kill rosbuzz_node");
else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");} }
if(node_handle.getParam("topics/modeclient", modeclient)); if (node_handle.getParam("topics/setpoint", setpoint_name))
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 {
else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} ROS_ERROR("Provide a Set Point name in Launch file");
if(node_handle.getParam("topics/localpos", local_pos_sub_name)); system("rosnode kill rosbuzz_node");
else {ROS_ERROR("Provide a localpos name in YAML 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 / 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*/ /*subscribers*/
Subscribe(n_c); Subscribe(n_c);
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this); // current_position_sub = n_c.subscribe("/global_position", 1000,
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); // &roscontroller::current_pos,this);
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); // battery_sub = n_c.subscribe("/power_status", 1000,
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this); // &roscontroller::battery,this);
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this); payload_sub =
obstacle_sub= n_c.subscribe("guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); 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*/ /*publishers*/
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 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*/ /* Service Clients*/
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if (rcclient == true) 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"); ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name); 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); 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; multi_msg = true;
} }
/*--------------------------------------- /*---------------------------------------
/Robot independent subscribers /Robot independent subscribers
/--------------------------------------*/ /--------------------------------------*/
void roscontroller::Subscribe(ros::NodeHandle& n_c){ 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){ 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") { if (it->second == "mavros_msgs/ExtendedState") {
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_extended_status_update, this); flight_status_sub = n_c.subscribe(
} it->first, 100, &roscontroller::flight_extended_status_update, this);
else if(it->second == "mavros_msgs/State"){ } else if (it->second == "mavros_msgs/State") {
flight_status_sub = n_c.subscribe(it->first, 100, &roscontroller::flight_status_update, this); flight_status_sub = n_c.subscribe(
} it->first, 100, &roscontroller::flight_status_update, this);
else if(it->second == "mavros_msgs/BatteryStatus"){ } else if (it->second == "mavros_msgs/BatteryStatus") {
battery_sub = n_c.subscribe(it->first, 1000, &roscontroller::battery, this); battery_sub =
} n_c.subscribe(it->first, 1000, &roscontroller::battery, this);
else if(it->second == "sensor_msgs/NavSatFix"){ } else if (it->second == "sensor_msgs/NavSatFix") {
current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this); current_position_sub =
} n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
else if(it->second == "std_msgs/Float64"){ } else if (it->second == "std_msgs/Float64") {
relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this); relative_altitude_sub =
n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
} }
std::cout << "Subscribed to: " << it->first << endl; std::cout << "Subscribed to: " << it->first << endl;
@ -307,19 +464,23 @@ namespace rosbzz_node{
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Create Buzz bytecode from the bzz script inputed / 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*/ /*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
/*Compile the buzz code .bzz to .bo*/ /*Compile the buzz code .bzz to .bo*/
stringstream bzzfile_in_compile; 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 << "/"; // bzzfile_in_compile << path << "/";
// path = bzzfile_in_compile.str(); // path = bzzfile_in_compile.str();
// bzzfile_in_compile.str(""); // bzzfile_in_compile.str("");
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
name = name.substr(0, name.find_last_of(".")); 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.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()); // system(bzzfile_in_compile.str().c_str());
// bzzfile_in_compile.str(""); // bzzfile_in_compile.str("");
bzzfile_in_compile << " -b " << path << name << ".bo"; bzzfile_in_compile << " -b " << path << name << ".bo";
@ -340,15 +501,18 @@ namespace rosbzz_node{
/*---------------------------------------------------- /*----------------------------------------------------
/ Publish neighbours pos and id in neighbours pos topic / 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(); auto current_time = ros::Time::now();
map<int, buzz_utility::Pos_struct>::iterator it; map<int, buzz_utility::Pos_struct>::iterator it;
rosbuzz::neigh_pos neigh_pos_array; // neigh_pos_array.clear(); rosbuzz::neigh_pos neigh_pos_array; // neigh_pos_array.clear();
neigh_pos_array.header.frame_id = "/world"; neigh_pos_array.header.frame_id = "/world";
neigh_pos_array.header.stamp = current_time; 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; 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.stamp = current_time;
neigh_tmp.header.frame_id = "/world"; neigh_tmp.header.frame_id = "/world";
neigh_tmp.position_covariance_type = it->first; // custom robot id storage neigh_tmp.position_covariance_type = it->first; // custom robot id storage
@ -364,7 +528,8 @@ namespace rosbzz_node{
/*-------------------------------------------------------- /*--------------------------------------------------------
/ Functions handling the local MAV ROS flight controller / Functions handling the local MAV ROS flight controller
/-------------------------------------------------------*/ /-------------------------------------------------------*/
void roscontroller::Arm(){ void roscontroller::Arm()
{
mavros_msgs::CommandBool arming_message; mavros_msgs::CommandBool arming_message;
arming_message.request.value = armstate; arming_message.request.value = armstate;
if (arm_client.call(arming_message)) { if (arm_client.call(arming_message)) {
@ -381,18 +546,28 @@ namespace rosbzz_node{
/Prepare Buzz messages payload for each step and publish /Prepare Buzz messages payload for each step and publish
/-----------------------------------------------------------------------------------------------------*/ /-----------------------------------------------------------------------------------------------------*/
/*----------------------------------------------------------------------------------------------------*/ /*----------------------------------------------------------------------------------------------------*/
/* Message format of payload (Each slot is uint64_t) / /* 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......... | / _________________________________________________________________________________________________
/|_____|_____|_____|________________________________________________|______________________________| */ /
/| | | |
| | /
/|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*/ /*obtain Pay load to be sent*/
uint64_t *payload_out_ptr = buzz_utility::obt_out_msg(); uint64_t *payload_out_ptr = buzz_utility::obt_out_msg();
uint64_t position[3]; uint64_t position[3];
/*Appened current position to message*/ /*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)); memcpy(position, tmp, 3 * sizeof(uint64_t));
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(position[0]); payload_out.payload64.push_back(position[0]);
@ -404,8 +579,10 @@ namespace rosbzz_node{
payload_out.payload64.push_back(payload_out_ptr[i]); payload_out.payload64.push_back(payload_out_ptr[i]);
} }
/*Add Robot id and message number to the published message*/ /*Add Robot id and message number to the published message*/
if (message_number < 0) message_number=0; if (message_number < 0)
else message_number++; message_number = 0;
else
message_number++;
payload_out.sysid = (uint8_t)robot_id; payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number; payload_out.msgid = (uint32_t)message_number;
@ -414,9 +591,12 @@ namespace rosbzz_node{
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
/*Check for updater message if present send*/ /*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; 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; int tot = 0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
fprintf(stdout, "Transfering code \n"); fprintf(stdout, "Transfering code \n");
@ -435,7 +615,8 @@ namespace rosbzz_node{
destroy_out_msg_queue(); destroy_out_msg_queue();
uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t))); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t)));
uint64_t *payload_64 = new uint64_t[total_size]; 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); free(buff_send);
// Send a constant number to differenciate updater msgs // Send a constant number to differenciate updater msgs
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT); update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
@ -443,29 +624,36 @@ namespace rosbzz_node{
update_packets.payload64.push_back(payload_64[i]); update_packets.payload64.push_back(payload_64[i]);
} }
// Add Robot id and message number to the published message // Add Robot id and message number to the published message
if (message_number < 0) message_number=0; if (message_number < 0)
else message_number++; message_number = 0;
else
message_number++;
update_packets.sysid = (uint8_t)robot_id; update_packets.sysid = (uint8_t)robot_id;
update_packets.msgid = (uint32_t)message_number; update_packets.msgid = (uint32_t)message_number;
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg = false; multi_msg = false;
delete[] payload_64; delete[] payload_64;
} }
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------
/Flight controller service call every step if there is a command set from bzz script /Flight controller service call every step if there is a command set from bzz
/-------------------------------------------------------------------------------- */ script
void roscontroller::flight_controller_service_call() { /--------------------------------------------------------------------------------
*/
void roscontroller::flight_controller_service_call()
{
/* flight controller client call if requested from Buzz*/ /* flight controller client call if requested from Buzz*/
/*FC call for takeoff,land, gohome and arm/disarm*/ /*FC call for takeoff,land, gohome and arm/disarm*/
int tmp = buzzuav_closures::bzz_cmd(); int tmp = buzzuav_closures::bzz_cmd();
double *goto_pos = buzzuav_closures::getgoto(); 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.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
// std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl; // 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()) { switch (buzzuav_closures::getcmd()) {
case mavros_msgs::CommandCode::NAV_LAND: case mavros_msgs::CommandCode::NAV_LAND:
if (current_mode != "LAND") { if (current_mode != "LAND") {
@ -475,8 +663,9 @@ namespace rosbzz_node{
} }
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else {
{ROS_ERROR("Failed to call service from flight controller");} ROS_ERROR("Failed to call service from flight controller");
}
armstate = 0; armstate = 0;
break; break;
case mavros_msgs::CommandCode::NAV_TAKEOFF: case mavros_msgs::CommandCode::NAV_TAKEOFF:
@ -490,10 +679,11 @@ namespace rosbzz_node{
home = cur_pos; home = cur_pos;
} }
if (current_mode != "GUIDED") if (current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
if (mav_client.call(cmd_srv)) // should always be in loiter after arm/disarm)
{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);} if (mav_client.call(cmd_srv)) {
else ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
} }
@ -528,7 +718,8 @@ namespace rosbzz_node{
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); 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);
} }
} }
/*---------------------------------------------- /*----------------------------------------------
@ -537,15 +728,18 @@ namespace rosbzz_node{
void roscontroller::maintain_pos(int tim_step) { void roscontroller::maintain_pos(int tim_step) {
if (timer_step >= BUZZRATE) { if (timer_step >= BUZZRATE) {
neighbours_pos_map.clear(); 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; 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); map<int, buzz_utility::Pos_struct>::iterator it = neighbours_pos_map.find(id);
if (it != neighbours_pos_map.end()) if (it != neighbours_pos_map.end())
neighbours_pos_map.erase(it); neighbours_pos_map.erase(it);
@ -554,8 +748,10 @@ namespace rosbzz_node{
/*----------------------------------------------------------------------------------- /*-----------------------------------------------------------------------------------
/Puts raw neighbours position into local storage for neighbours pos publisher /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 ){ void roscontroller::raw_neighbours_pos_put(int id,
map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(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()) if (it != raw_neighbours_pos_map.end())
raw_neighbours_pos_map.erase(it); raw_neighbours_pos_map.erase(it);
raw_neighbours_pos_map.insert(make_pair(id, pos_arr)); raw_neighbours_pos_map.insert(make_pair(id, pos_arr));
@ -564,8 +760,7 @@ namespace rosbzz_node{
/*-------------------------------------------------------- /*--------------------------------------------------------
/Set the current position of the robot callback /Set the current position of the robot callback
/--------------------------------------------------------*/ /--------------------------------------------------------*/
void roscontroller::set_cur_pos(double latitude, void roscontroller::set_cur_pos(double latitude, double longitude,
double longitude,
double altitude) { double altitude) {
cur_pos.latitude = latitude; cur_pos.latitude = latitude;
cur_pos.longitude = longitude; cur_pos.longitude = longitude;
@ -599,8 +794,7 @@ namespace rosbzz_node{
M[2][2] = -sin_el; M[2][2] = -sin_el;
} }
void matrix_multiply(uint32_t n, uint32_t m, uint32_t p, const double *a, 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; uint32_t i, j, k;
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
for (j = 0; j < p; j++) { for (j = 0; j < p; j++) {
@ -617,35 +811,30 @@ namespace rosbzz_node{
return x; return x;
} }
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; float ned_x = 0.0, ned_y = 0.0;
gps_ned_cur(ned_x, ned_y, nei_pos); gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y); out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
// out[0] = std::floor(out[0] * 1000000) / 1000000; // out[0] = std::floor(out[0] * 1000000) / 1000000;
out[1] = atan2(ned_y, ned_x);
out[1] = constrainAngle(atan2(ned_y,ned_x)); out[1] = constrainAngle(atan2(ned_y,ned_x));
// out[1] = std::floor(out[1] * 1000000) / 1000000; // out[1] = std::floor(out[1] * 1000000) / 1000000;
out[2] = 0.0; out[2] = 0.0;
} }
void roscontroller::gps_ned_cur(float &ned_x, float &ned_y, GPS t) 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,
gps_convert_ned(ned_x, ned_y, cur_pos.latitude);
t.longitude, t.latitude,
cur_pos.longitude, cur_pos.latitude);
} }
void roscontroller::gps_ned_home(float &ned_x, float &ned_y) void roscontroller::gps_ned_home(float &ned_x, float &ned_y) {
{ gps_convert_ned(ned_x, ned_y, cur_pos.longitude, cur_pos.latitude,
gps_convert_ned(ned_x, ned_y,
cur_pos.longitude, cur_pos.latitude,
home.longitude, home.latitude); home.longitude, home.latitude);
} }
void roscontroller::gps_convert_ned(float &ned_x, float &ned_y, void roscontroller::gps_convert_ned(float &ned_x, float &ned_y,
double gps_t_lon, double gps_t_lat, 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_lon = gps_t_lon - gps_r_lon;
double d_lat = gps_t_lat - gps_r_lat; double d_lat = gps_t_lat - gps_r_lat;
ned_x = DEG2RAD(d_lat) * EARTH_RADIUS; ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
@ -657,13 +846,15 @@ namespace rosbzz_node{
/------------------------------------------------------*/ /------------------------------------------------------*/
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr &msg) { void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr &msg) {
buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining); 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 /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 // http://wiki.ros.org/mavros/CustomModes
// TODO: Handle landing // TODO: Handle landing
std::cout << "Message: " << msg->mode << std::endl; std::cout << "Message: " << msg->mode << std::endl;
@ -678,7 +869,8 @@ namespace rosbzz_node{
/*------------------------------------------------------------ /*------------------------------------------------------------
/Update flight extended status into BVM from subscriber /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); buzzuav_closures::flight_status_update(msg->landed_state);
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------
@ -694,11 +886,14 @@ namespace rosbzz_node{
home[1]=lon; home[1]=lon;
home[2]=cur_rel_altitude; home[2]=cur_rel_altitude;
}*/ }*/
set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude); set_cur_pos(msg->latitude, msg->longitude,
buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude);//msg->altitude); 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[0] = pose->pose.position.x;
local_pos_new[1] = pose->pose.position.y; local_pos_new[1] = pose->pose.position.y;
local_pos_new[2] = pose->pose.position.z; local_pos_new[2] = pose->pose.position.z;
@ -708,15 +903,14 @@ namespace rosbzz_node{
int n = data.pos_neigh.size(); int n = data.pos_neigh.size();
// ROS_INFO("Users array size: %i\n", n); // ROS_INFO("Users array size: %i\n", n);
if(n>0) if (n > 0) {
{ for (int it = 0; it < n; ++it) {
for(int it=0; it<n; ++it) buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,
{ data.pos_neigh[it].latitude,
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); data.pos_neigh[it].longitude,
data.pos_neigh[it].altitude);
} }
} }
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------
/ Update altitude into BVM from subscriber / Update altitude into BVM from subscriber
@ -724,7 +918,6 @@ namespace rosbzz_node{
void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr &msg) { void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr &msg) {
// ROS_INFO("Altitude in: %f", msg->data); // ROS_INFO("Altitude in: %f", msg->data);
cur_rel_altitude = (double)msg->data; cur_rel_altitude = (double)msg->data;
} }
/*------------------------------------------------------------- /*-------------------------------------------------------------
/Set obstacle Obstacle distance table into BVM from subscriber /Set obstacle Obstacle distance table into BVM from subscriber
@ -746,10 +939,13 @@ namespace rosbzz_node{
moveMsg.header.frame_id = 1; moveMsg.header.frame_id = 1;
// float ned_x, ned_y; // float ned_x, ned_y;
// gps_ned_home(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 Home: %.7f, %.7f", robot_id, home[0],
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]); // 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; // target[0]+=y; target[1]+=x;
moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y; moveMsg.pose.position.x = local_pos_new[0] + y; // ned_y+y;
moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+x; moveMsg.pose.position.y = local_pos_new[1] + x; // ned_x+x;
@ -784,7 +980,6 @@ namespace rosbzz_node{
} }
} }
void roscontroller::SetStreamRate(int id, int rate, int on_off) { void roscontroller::SetStreamRate(int id, int rate, int on_off) {
mavros_msgs::StreamRate message; mavros_msgs::StreamRate message;
message.request.stream_id = id; message.request.stream_id = id;
@ -802,15 +997,21 @@ namespace rosbzz_node{
/Push payload into BVM FIFO /Push payload into BVM FIFO
/-------------------------------------------------------------*/ /-------------------------------------------------------------*/
/*******************************************************************************************************/ /*******************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */ /* 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......... | */ */
/*|_____|_____|_____|________________________________________________|______________________________| */ /*| | | | |
* | */
/*|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) { void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
/*Check for Updater message, if updater message push it into updater FIFO*/ /*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()); uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
uint64_t message_obt[obt_msg_size]; uint64_t message_obt[obt_msg_size];
/* Go throught the obtained payload*/ /* Go throught the obtained payload*/
@ -827,7 +1028,8 @@ namespace rosbzz_node{
// tot+=sizeof(uint16_t); // tot+=sizeof(uint16_t);
fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize); fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize);
if (unMsgSize > 0) { 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"); // fprintf(stdout,"before in queue process : utils\n");
code_message_inqueue_process(); code_message_inqueue_process();
// fprintf(stdout,"after in queue process : utils\n"); // fprintf(stdout,"after in queue process : utils\n");
@ -846,28 +1048,34 @@ namespace rosbzz_node{
/* Extract neighbours position from payload*/ /* Extract neighbours position from payload*/
double neighbours_pos_payload[3]; double neighbours_pos_payload[3];
memcpy(neighbours_pos_payload, message_obt, 3 * sizeof(uint64_t)); 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; GPS nei_pos;
nei_pos.latitude = neighbours_pos_payload[0]; nei_pos.latitude = neighbours_pos_payload[0];
nei_pos.longitude = neighbours_pos_payload[1]; nei_pos.longitude = neighbours_pos_payload[1];
nei_pos.altitude = neighbours_pos_payload[2]; nei_pos.altitude = neighbours_pos_payload[2];
double cvt_neighbours_pos_payload[3]; 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); gps_rb(nei_pos, cvt_neighbours_pos_payload);
/*Extract robot id of the neighbour*/ /*Extract robot id of the neighbour*/
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3)); 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*/ /*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*/ /*Put RID and pos*/
raw_neighbours_pos_put((int)out[1], raw_neigh_pos); raw_neighbours_pos_put((int)out[1], raw_neigh_pos);
/* TODO: remove roscontroller local map array for neighbors */ /* TODO: remove roscontroller local map array for neighbors */
neighbours_pos_put((int)out[1], n_pos); 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; delete[] out;
buzz_utility::in_msg_append((message_obt + 3)); buzz_utility::in_msg_append((message_obt + 3));
} }
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------
@ -897,8 +1105,7 @@ namespace rosbzz_node{
ROS_INFO("RC_Call: ARM!!!!"); ROS_INFO("RC_Call: ARM!!!!");
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
} } else {
else{
ROS_INFO("RC_Call: DISARM!!!!"); ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd + 1); buzzuav_closures::rc_call(rc_cmd + 1);
res.success = true; res.success = true;
@ -944,21 +1151,18 @@ namespace rosbzz_node{
if (no_of_robots == 0) { if (no_of_robots == 0) {
no_of_robots = cur_robots; no_of_robots = cur_robots;
} } else {
else{
if (no_of_robots != cur_robots && no_cnt == 0) { if (no_of_robots != cur_robots && no_cnt == 0) {
no_cnt++; no_cnt++;
old_val = cur_robots; old_val = cur_robots;
} } else if (no_cnt != 0 && old_val == cur_robots) {
else if(no_cnt!=0 && old_val==cur_robots){
no_cnt++; no_cnt++;
if (no_cnt >= 150 || cur_robots > no_of_robots) { if (no_cnt >= 150 || cur_robots > no_of_robots) {
no_of_robots = cur_robots; no_of_robots = cur_robots;
no_cnt = 0; no_cnt = 0;
} }
} } else {
else{
no_cnt = 0; no_cnt = 0;
} }
} }
@ -998,5 +1202,4 @@ namespace rosbzz_node{
} }
*/ */
} }
} }