From 27bdabeda4cd9f2d53beabd01c1f68bc59e5c77a Mon Sep 17 00:00:00 2001 From: David St-Onge Date: Fri, 18 Nov 2016 14:05:31 -0500 Subject: [PATCH] addition of some additional buzz hooks --- include/buzzuav_closures.h | 13 +++++ include/roscontroller.h | 26 +++++---- include/roscontroller.h.save | 100 +++++++++++++++++++++++++++++++++++ src/buzz_utility.cpp | 4 +- src/buzzuav_closures.cpp | 47 ++++++++++++++++ src/out.basm | 82 ++++++++++++++++++++-------- src/out.bdbg | Bin 2310 -> 4158 bytes src/out.bo | Bin 244 -> 441 bytes src/roscontroller.cpp | 11 ++-- src/test.bzz | 4 +- 10 files changed, 250 insertions(+), 37 deletions(-) create mode 100644 include/roscontroller.h.save diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 4e24c6b..dfddc43 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -28,8 +28,13 @@ void set_goto(double pos[]); void rc_call(int rc_cmd); /* sets the battery state */ void set_battery(float voltage,float current,float remaining); +/* sets current position */ +void set_currentpos(double latitude, double longitude, double altitude); /*retuns the current go to position */ double* getgoto(); +/* updates flight status*/ +void flight_status_update(uint8_t state); + /* * Commands the UAV to takeoff */ @@ -47,6 +52,14 @@ int buzzuav_gohome(buzzvm_t vm); * Updates battery information in Buzz */ int buzzuav_update_battery(buzzvm_t vm); +/* + * Updates current position in Buzz + */ +int buzzuav_update_currentpos(buzzvm_t vm); +/* + * Updates flight status in Buzz + */ +int buzzuav_update_flight_status(buzzvm_t vm); /* * Updates IR information in Buzz diff --git a/include/roscontroller.h b/include/roscontroller.h index 9047349..9ccda74 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -4,6 +4,7 @@ #include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandInt.h" +#include "mavros_msgs/ExtendedState.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/Mavlink.h" @@ -50,6 +51,7 @@ private: ros::Subscriber current_position_sub; ros::Subscriber battery_sub; ros::Subscriber payload_sub; + ros::Subscriber flight_status_sub; /*Create node Handler*/ ros::NodeHandle n_c; /*Commands for flight controller*/ @@ -61,36 +63,40 @@ private: /*Obtain data from ros parameter server*/ void Rosparameters_get(); + /*compiles buzz script from the specified .bzz file*/ void Compile_bzz(); /*Prepare messages and publish*/ - inline void prepare_msg_and_publish(); + void prepare_msg_and_publish(); /*Refresh neighbours Position for every ten step*/ - inline void maintain_pos(int tim_step); + void maintain_pos(int tim_step); /*Maintain neighbours position*/ - inline void neighbours_pos_maintain(int id, buzz_utility::Pos_struct pos_arr ); + 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, + 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 []); + double* cvt_spherical_coordinates(double neighbours_pos_payload []); /*battery status callback*/ - inline void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); - + void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); + + /*flight status callback*/ + void flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); + /*current position callback*/ - inline void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); + void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg); /*payload callback callback*/ - inline void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); + void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg); /* RC commands service */ - inline bool rc_callback(mavros_msgs::CommandInt::Request &req, + bool rc_callback(mavros_msgs::CommandInt::Request &req, mavros_msgs::CommandInt::Response &res); diff --git a/include/roscontroller.h.save b/include/roscontroller.h.save new file mode 100644 index 0000000..02dd0dc --- /dev/null +++ b/include/roscontroller.h.save @@ -0,0 +1,100 @@ + #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/src/buzz_utility.cpp b/src/buzz_utility.cpp index be42fa6..7fc2c24 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -301,7 +301,9 @@ void buzz_script_step() { */ buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_prox(VM); - /* + buzzuav_closures::buzzuav_update_currentpos(VM); + buzzuav_closures::buzzuav_update_flight_status(VM); + /* * Call Buzz step() function */ if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 033edc7..4bf41c3 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -10,6 +10,8 @@ namespace buzzuav_closures{ static double goto_pos[3]; static float batt[3]; +static double cur_pos[3]; +static uint8_t status; static int cur_cmd; static int rc_cmd=0; /****************************************/ @@ -148,6 +150,51 @@ int buzzuav_update_battery(buzzvm_t vm) { buzzvm_gstore(vm); return vm->state; } +/****************************************/ +/*current pos update*/ +/***************************************/ +void set_currentpos(double latitude, double longitude, double altitude){ + cur_pos[0]=latitude; + cur_pos[1]=longitude; + cur_pos[2]=altitude; +} +/****************************************/ +int buzzuav_update_currentpos(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1)); + buzzvm_pushf(vm, cur_pos[0]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1)); + buzzvm_pushf(vm, cur_pos[1]); + buzzvm_tput(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1)); + buzzvm_pushf(vm, cur_pos[2]); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} +/****************************************/ +/*flight status update*/ +/***************************************/ +void flight_status_update(uint8_t state){ + status=state; +} +/****************************************/ +int buzzuav_update_flight_status(buzzvm_t vm) { + buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); + buzzvm_pusht(vm); + buzzvm_dup(vm); + buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1)); + buzzvm_pushf(vm, status); + buzzvm_tput(vm); + buzzvm_gstore(vm); + return vm->state; +} + /****************************************/ /*To do !!!!!*/ diff --git a/src/out.basm b/src/out.basm index 7ff9a4b..3435c2d 100644 --- a/src/out.basm +++ b/src/out.basm @@ -1,4 +1,4 @@ -!11 +!21 'init 'i 'uav_takeoff @@ -7,6 +7,16 @@ 'bzz print: remaining: 'battery 'capacity +'latitude : +'position +'latitude +' longitude: +'longitude +' altitude : +'altitude +'flight status: +'flight +'status 'uav_land 'reset 'destroy @@ -17,10 +27,10 @@ pushs 3 pushcn @__label_2 gstore - pushs 9 + pushs 19 pushcn @__label_5 gstore - pushs 10 + pushs 20 pushcn @__label_6 gstore nop @@ -50,26 +60,54 @@ 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 1 |11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |11,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 20 |11,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gt |11,7,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - jumpz @__label_3 |11,8,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 8 |11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |11,17,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 0 |11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - callc |11,19,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz -@__label_3 |12,0,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 1 |12,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushs 1 |12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gload |12,3,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - pushi 1 |12,4,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - add |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - gstore |12,5,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz - ret0 |14,1,/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 |18,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + ret0 |20,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz @__label_6 - ret0 |22,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz + ret0 |24,1,/home/ubuntu/ROS_WS/src/ROSBuzz/src/test.bzz diff --git a/src/out.bdbg b/src/out.bdbg index 11df892642d4ad3f6a5b1525abf11d57ead1bfe7..1a8acb01a5a8f59b815f35f47816adc153fbe983 100644 GIT binary patch literal 4158 zcmbuBD^CMK6ot1u%ByV%R6qi)p+JDJt-Q(b~Z|G z^Q7`O&3nEd%v;rFYqsqBw?OEj2w5TYg)2KLLJkN~if{)h1}MTDIE+w)d$q#@5R#O` zygJ1K5JoA7M|FxPAtb_g%QP5vyLuT?WGF%&2;&st8B(-UgaSA$QiK<^!z&OvD2G*b ziZviCQ4VjAg6mQ5YC;iG6sQy*Lg)`y$}|{u=b{7-D-_{V2&Ol24qvitD10|Z*py}F zkF!gq_?A`XOmYreYKLtg>`^IxAVnKRsDML_A~-@Yl{8PgJ-H7Kb;=~L=9ge|)yA8&LU@9onVA$0mAW-XYBwWUZg_%Z z0y9}AyrkHKS4bwXkmZK=NG6b%sy|R{LNCw+SO}6A7axHp@BuLhkY~atpiUvMesWFt z4Ad<~kqMK4x?$nK1T~4Y5@tG*2{4mLGhqhM1XxOfl~tsfFqdKz<{_ED3uJ=;X>M2y z)B*Dac_u6YngGiW-n4K%JDCuo}q(HjqPrfHXI30P29bK^?50 xR1>yRY{E7q6Ew(j!)_!K49GHJ50VLnWSMY~ViOJlO@QS|Sn4J%Hjh(m0swimRIC61 diff --git a/src/out.bo b/src/out.bo index 563229b469ca1af5248de95a4090e223e556bb00..f47b011db6445a8e1ae9fc05c2e95002a45f8260 100644 GIT binary patch literal 441 zcmYL@>rTTk423!#lu}@f+J?Yo(riKF7BF z2tIlO554_-CHwADT|*|0Kt-{zH-Q6 zo3baL;=u$ADK?sOd|*Hu#;H9+9endfn8{L~xqnylQBv!j!Nv+YmmNXnGIePH1pc0X zSr&LKIGYIG3w|p;2>uAd1o;8OKersQmdeWlPo%(t6;BnXtWQRImjB_Igy$MwNQjrC waizxfXw1YgvT8qmBjGJeiE|BiyXBzL$in%{7Z-S^1`AJ0s8Sli>7po#KksK!WdHyG delta 74 zcmdnV{DqN`dm^Lz#Er7by$lQt5(>;fQgJDm$q8b90yDWl%+CxHKUxbh0-56SB0y4D O{XURX;Fvs%Q62ydf(-cp diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 4103277..8d81551 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -79,9 +79,10 @@ namespace rosbzz_node{ void roscontroller::Initialize_pub_sub(){ /*subscribers*/ - current_position_sub = n_c.subscribe("/dji_sdk/global_position", 1000, &roscontroller::current_pos,this); + 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); + flight_status_sub =n_c.subscribe("/mav/flight_status",100, &roscontroller::flight_status_update,this); /*publishers*/ payload_pub = n_c.advertise("outMavlink", 1000); /* Clients*/ @@ -140,7 +141,7 @@ namespace rosbzz_node{ cout<<" [Debug:] sent message "<<*it<voltage,msg->current,msg->remaining); //ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, msg->current, msg ->remaining); } - + /*flight status update*/ + void roscontroller::flight_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){ + buzzuav_closures::flight_status_update(msg->landed_state); + } /*current position callback*/ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){ set_cur_pos(msg->latitude,msg->longitude,msg->altitude); + buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude); } /*payload callback callback*/ diff --git a/src/test.bzz b/src/test.bzz index 96e5d5a..a1af528 100644 --- a/src/test.bzz +++ b/src/test.bzz @@ -8,7 +8,9 @@ uav_takeoff() function step() { print("bzz print: remaining: ", battery.capacity) -if(i>20){uav_land()} +print("latitude : ",position.latitude," longitude: ",position.longitude," altitude : ", position.altitude) +print("flight status: ",flight.status) +if(i==10){uav_land()} i=i+1 }