diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e8ead7..a0adbeb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(catkin REQUIRED COMPONENTS roscpp std_msgs - mavros_msgs + # mavros_msgs sensor_msgs ) diff --git a/include/roscontroller.h b/include/roscontroller.h index 9ccda74..ce82fae 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -24,12 +24,13 @@ using namespace std; namespace rosbzz_node{ - - +/*Create node Handler*/ +//ros::NodeHandle n_c; + class roscontroller{ public: - roscontroller(); + roscontroller(ros::NodeHandle n_c); ~roscontroller(); //void RosControllerInit(); void RosControllerRun(); @@ -43,7 +44,7 @@ private: int robot_id=0; int oldcmdID=0; int rc_cmd; - std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname; //, rcclient; + std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient; bool rcclient; ros::ServiceClient mav_client; ros::Publisher payload_pub; @@ -52,16 +53,14 @@ private: ros::Subscriber battery_sub; ros::Subscriber payload_sub; ros::Subscriber flight_status_sub; - /*Create node Handler*/ - ros::NodeHandle n_c; /*Commands for flight controller*/ mavros_msgs::CommandInt cmd_srv; - void Initialize_pub_sub(); + void Initialize_pub_sub(ros::NodeHandle n_c); /*Obtain data from ros parameter server*/ - void Rosparameters_get(); + void Rosparameters_get(ros::NodeHandle n_c); /*compiles buzz script from the specified .bzz file*/ void Compile_bzz(); diff --git a/include/roscontroller.h.save b/include/roscontroller.h.save deleted file mode 100644 index 02dd0dc..0000000 --- a/include/roscontroller.h.save +++ /dev/null @@ -1,100 +0,0 @@ - #pragma once -#include "ros/ros.h" -#include "sensor_msgs/NavSatFix.h" -#include "mavros_msgs/GlobalPositionTarget.h" -#include "mavros_msgs/CommandCode.h" -#include "mavros_msgs/CommandInt.h" -#include "mavros_msgs/State.h" -#include "mavros_msgs/BatteryStatus.h" -#include "mavros_msgs/Mavlink.h" -#include "sensor_msgs/NavSatStatus.h" -#include -#include -#include "buzzuav_closures.h" -#include "buzz_utility.h" -#include "uav_utility.h" -#include -#include -#include -#include -#include -#include - -using namespace std; - -namespace rosbzz_node{ - - -class roscontroller{ - -public: - roscontroller(); - ~roscontroller(); - //void RosControllerInit(); - void RosControllerRun(); - -private: - - double cur_pos[3]; - uint64_t payload; - std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; - int timer_step=0; - int robot_id=0; - int oldcmdID=0; - int rc_cmd; - std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname; //, rcclient; - bool rcclient; - ros::ServiceClient mav_client; - ros::Publisher payload_pub; - ros::ServiceServer service; - ros::Subscriber current_position_sub; - ros::Subscriber battery_sub; - ros::Subscriber payload_sub; - /*Create node Handler*/ - ros::NodeHandle n_c; - /*Commands for flight controller*/ - mavros_msgs::CommandInt cmd_srv; - - - void Initialize_pub_sub(); - - /*Obtain data from ros parameter server*/ - void Rosparameters_get(); - - void Compile_bzz(); - - /*Prepare messages and publish*/ - inline void prepare_msg_and_publish(); - - - /*Refresh neighbours Position for every ten step*/ - inline void maintain_pos(int tim_step); - - /*Maintain neighbours position*/ - inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ); - - /*Set the current position of the robot callback*/ - inline void set_cur_pos(double latitude, - double longitude, - double altitude); - /*convert from catresian to spherical coordinate system callback */ - inline double* cvt_spherical_coordinates(double neighbours_pos_payload []); - - /*battery status callback*/ - inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); - - /*current position callback*/ - inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); - - /*payload callback callback*/ - inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); - - /* RC commands service */ - inline bool rc_callback(mavros_msgs::CommandInt::Request &req, - mavros_msgs::CommandInt::Response &res); - - - -}; - -} diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index df715b1..05a4870 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -1,11 +1,15 @@ - - + + - - - + + + + + + + diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch new file mode 100644 index 0000000..9f9aaa0 --- /dev/null +++ b/launch/rosbuzz_2_parallel.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rosbuzz.launch.save b/src/.Rhistory similarity index 100% rename from launch/rosbuzz.launch.save rename to src/.Rhistory diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 7fc2c24..a2d7ec7 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -66,7 +66,12 @@ namespace buzz_utility{ memcpy(pl, payload ,size); size_t tot = sizeof(uint32_t); - + /*for(int i=0;i 0 && unMsgSize <= size*sizeof(uint64_t) - tot) { + if(unMsgSize > 0 && unMsgSize <= size - tot ) { buzzinmsg_queue_append(VM->inmsgs, buzzmsg_payload_frombuffer(pl +tot, unMsgSize)); tot += unMsgSize; } }while(size - tot > sizeof(uint16_t) && unMsgSize > 0); - + /* Process messages */ buzzvm_process_inmsgs(VM); } diff --git a/src/out.basm b/src/out.basm deleted file mode 100644 index 3435c2d..0000000 --- a/src/out.basm +++ /dev/null @@ -1,113 +0,0 @@ -!21 -'init -'i -'uav_takeoff -'step -'print -'bzz print: remaining: -'battery -'capacity -'latitude : -'position -'latitude -' longitude: -'longitude -' altitude : -'altitude -'flight status: -'flight -'status -'uav_land -'reset -'destroy - - pushs 0 - pushcn @__label_1 - gstore - pushs 3 - pushcn @__label_2 - gstore - pushs 19 - pushcn @__label_5 - gstore - pushs 20 - pushcn @__label_6 - gstore - nop - -@__label_0 - -@__exitpoint - done - -@__label_1 - pushs 1 |3,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 0 |3,2,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gstore |3,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 2 |4,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |4,11,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 0 |4,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - callc |4,13,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - ret0 |5,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - -@__label_2 - pushs 4 |10,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |10,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 5 |10,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 6 |10,39,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |10,39,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 7 |10,40,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - tget |10,48,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 2 |10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - callc |10,49,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 4 |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 8 |11,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 9 |11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |11,28,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 10 |11,29,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - tget |11,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 11 |11,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 9 |11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |11,61,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 12 |11,62,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - tget |11,71,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 13 |11,72,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 9 |11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |11,96,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 14 |11,97,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - tget |11,105,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 6 |11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - callc |11,106,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 4 |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 15 |12,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 16 |12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |12,30,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 17 |12,31,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - tget |12,37,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 2 |12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - callc |12,38,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 1 |13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |13,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 10 |13,6,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - eq |13,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - jumpz @__label_3 |13,9,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 18 |13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |13,18,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 0 |13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - callc |13,20,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz -@__label_3 |14,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 1 |14,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 1 |14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |14,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 1 |14,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - add |14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gstore |14,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - ret0 |16,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - -@__label_5 - ret0 |20,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - -@__label_6 - ret0 |24,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz diff --git a/src/out.bdbg b/src/out.bdbg deleted file mode 100644 index 1a8acb0..0000000 Binary files a/src/out.bdbg and /dev/null differ diff --git a/src/out.bo b/src/out.bo deleted file mode 100644 index f47b011..0000000 Binary files a/src/out.bo and /dev/null differ diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp index efa02dd..cfef7b2 100644 --- a/src/rosbuzz.cpp +++ b/src/rosbuzz.cpp @@ -16,7 +16,8 @@ int main(int argc, char **argv) { /*Initialize rosBuzz node*/ ros::init(argc, argv, "rosBuzz"); - rosbzz_node::roscontroller RosController; + ros::NodeHandle n_c("~"); + rosbzz_node::roscontroller RosController(n_c); RosController.RosControllerRun(); return 0; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 8d81551..aaec61e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -3,15 +3,18 @@ namespace rosbzz_node{ +/*Create node Handler*/ /***Constructor***/ - roscontroller::roscontroller() + roscontroller::roscontroller(ros::NodeHandle n_c) { + /*Create node Handler*/ + ROS_INFO("Buzz_node"); /*Obtain parameters from ros parameter server*/ - Rosparameters_get(); + Rosparameters_get(n_c); /*Initialize publishers, subscribers and client*/ - Initialize_pub_sub(); + Initialize_pub_sub(n_c); /*Compile the .bzz file to .basm, .bo and .bdbg*/ Compile_bzz(); } @@ -52,15 +55,16 @@ namespace rosbzz_node{ } } - void roscontroller::Rosparameters_get(){ + void roscontroller::Rosparameters_get(ros::NodeHandle n_c){ + /*Obtain .bzz file name from parameter server*/ - if(ros::param::get("/rosbuzz_node/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");} /*Obtain rc service option from parameter server*/ - if(ros::param::get("/rosbuzz_node/rcclient", rcclient)){ + if(n_c.getParam("rcclient", rcclient)){ if(rcclient==true){ /*Service*/ - if(ros::param::get("/rosbuzz_node/rcservice_name", rcservice_name)){ + if(n_c.getParam("rcservice_name", rcservice_name)){ service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback,this); ROS_INFO("Ready to receive Mav Commands from RC client"); } @@ -70,21 +74,28 @@ namespace rosbzz_node{ } else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} /*Obtain fc client name from parameter server*/ - if(ros::param::get("/rosbuzz_node/fcclient_name", fcclient_name)); + if(n_c.getParam("fcclient_name", fcclient_name)); else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");} /*Obtain robot_id from parameter server*/ - ros::param::get("/rosbuzz_node/robot_id", robot_id); + n_c.getParam("robot_id", robot_id); + /*Obtain out payload name*/ + n_c.getParam("out_payload", out_payload); + /*Obtain in payload name*/ + n_c.getParam("in_payload", in_payload); + + } - void roscontroller::Initialize_pub_sub(){ + void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){ /*subscribers*/ current_position_sub = n_c.subscribe("/mav/global_position", 1000, &roscontroller::current_pos,this); battery_sub = n_c.subscribe("/mav/power_status", 1000, &roscontroller::battery,this); - payload_sub = n_c.subscribe("inMavlink", 1000, &roscontroller::payload_obt,this); + payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this); /*publishers*/ - payload_pub = n_c.advertise("outMavlink", 1000); + payload_pub = n_c.advertise(out_payload, 1000); + /* Clients*/ mav_client = n_c.serviceClient(fcclient_name); @@ -94,19 +105,24 @@ namespace rosbzz_node{ /*Compile the buzz code .bzz to .bo*/ stringstream bzzfile_in_compile; std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")); - bzzfile_in_compile << "bzzparse "<::const_iterator it = payload_out.payload64.begin(); it != payload_out.payload64.end(); ++it){ message_obt[i] =(uint64_t) *it; - cout<<" [Debug:] sent message "<<*it<