merge rosbuzz
This commit is contained in:
commit
eb130be1bb
include
launch
misc
script
src
@ -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);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -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>
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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()
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -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*/
|
||||||
|
@ -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{
|
|||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user