Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

merge rosbuzz

This commit is contained in:
dave 2017-06-26 14:35:15 -04:00
parent d7ca910e21 8e5acd264d
commit eb130be1bb
9 changed files with 1492 additions and 1254 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
include/roscontroller.h
View File

@ -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;
@ -80,6 +81,7 @@ private:
int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0;
int rc_cmd;
float fcu_timeout;
int armstate;
@ -233,7 +235,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);
};
}

17
launch/rosbuzz-solo.launch
View File

@ -1,14 +1,14 @@
<launch>
<!-- 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="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/node.launch">
<arg name="pluginlists_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="/home/pi/ros_catkinKin_ws/src/mavros/mavros/launch/apm_config.yaml" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_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_In_From_Controller" type="str" value="xbee_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 -->
<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"/>
<param name="bzzfile_name" value="/home/pi/ros_catkinKin_ws/src/ROSBuzz/script/testflockfev.bzz" />
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>
@ -50,7 +51,7 @@
<param name="xbee_status_srv" value="xbee_status"/>
<param name="xbee_plugged" value="true"/>
<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>
</launch>

13
misc/cmdlinectr.sh
View File

@ -16,6 +16,7 @@ function testWP {
}
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
}
function clean {
sudo rm /var/log/upstart/robot*
@ -23,10 +24,18 @@ function clean {
sudo rm /var/log/upstart/x3s*
}
function startrobot {
sudo service robot start
sudo service dji start
}
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 {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch

4
script/Graph_drone.graph
View File

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

13
script/include/uavstates.bzz
View File

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

5
src/buzz_utility.cpp
View File

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

1
src/buzzuav_closures.cpp
View File

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

571
src/roscontroller.cpp
View File

@ -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,50 +188,49 @@ namespace rosbzz_node{
// MAIN LOOP
//////////////////////////////////////////////////////
//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*/
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
/*Neighbours of the robot published with id in respective topic*/
//ROS_WARN("[%i]-----------NEIG", robot_id);
neighbours_pos_publisher();
/*Check updater state and step code*/
//ROS_WARN("[%i]-----------UPD", robot_id);
update_routine(bcfname.c_str(), dbgfname.c_str());
/*Step buzz script */
//ROS_WARN("[%i]-----------STEP", robot_id);
buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/
//ROS_WARN("[%i]-----------MSG", robot_id);
prepare_msg_and_publish();
/*call flight controler service to set command long*/
//ROS_WARN("[%i]-----------FC", robot_id);
flight_controller_service_call();
/*Set multi message available after update*/
//ROS_WARN("[%i]-----------UPD", robot_id);
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;
}
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
// no_of_robots=get_number_of_robots();
//ROS_WARN("[%i]-----------ROBOTS", robot_id);
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*/
@ -160,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();
@ -185,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);
@ -197,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;
@ -307,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";
@ -340,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
@ -364,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)) {
@ -381,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]);
@ -404,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;
@ -414,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");
@ -435,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);
@ -443,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") {
@ -475,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:
@ -490,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;
}
@ -528,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);
}
}
/*----------------------------------------------
@ -537,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);
@ -554,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));
@ -564,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;
@ -599,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++) {
@ -617,35 +811,30 @@ namespace rosbzz_node{
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;
gps_ned_cur(ned_x, ned_y, nei_pos);
out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
// 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] = std::floor(out[1] * 1000000) / 1000000;
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;
@ -657,13 +846,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;
@ -678,7 +869,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);
}
/*-------------------------------------------------------------
@ -694,11 +886,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;
@ -708,15 +903,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
@ -724,7 +918,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
@ -746,10 +939,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;
@ -784,7 +980,6 @@ namespace rosbzz_node{
}
}
void roscontroller::SetStreamRate(int id, int rate, int on_off) {
mavros_msgs::StreamRate message;
message.request.stream_id = id;
@ -802,15 +997,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*/
@ -827,7 +1028,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");
@ -846,28 +1048,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));
}
}
/*-----------------------------------------------------------
@ -897,8 +1105,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;
@ -944,21 +1151,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;
}
}
@ -998,5 +1202,4 @@ namespace rosbzz_node{
}
*/
}
}