From 60aef71f38382d6bf759925f0b03e2b61e5f0a69 Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 8 Dec 2017 19:44:24 -0500 Subject: [PATCH] removed unused functions and variables --- CMakeLists.txt | 1 - include/buzzuav_closures.h | 3 --- include/roscontroller.h | 16 +++++++++++----- include/uav_utility.h | 8 -------- log.txt | 3 --- readme.md | 30 +++++++++++++++++------------- src/buzz_utility.cpp | 38 -------------------------------------- src/buzzuav_closures.cpp | 32 +++----------------------------- src/roscontroller.cpp | 26 ++++++++++++-------------- src/uav_utility.cpp | 25 ------------------------- 10 files changed, 43 insertions(+), 139 deletions(-) delete mode 100644 include/uav_utility.h delete mode 100644 log.txt delete mode 100644 src/uav_utility.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f9d152b..6b7e4b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,6 @@ add_executable(rosbuzz_node src/rosbuzz.cpp src/roscontroller.cpp src/buzz_utility.cpp src/buzzuav_closures.cpp - src/uav_utility.cpp src/buzz_update.cpp) target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread) add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 6beb81c..1ff186c 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -2,7 +2,6 @@ #include #include -#include "uav_utility.h" #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/Mavlink.h" #include "ros/ros.h" @@ -46,8 +45,6 @@ void parse_gpslist(); int buzzuav_takepicture(buzzvm_t vm); /* Returns the current command from local variable*/ int getcmd(); -/*Sets goto position */ -void set_goto(double pos[]); /*Sets goto position from rc client*/ void rc_set_goto(int id, double latitude, double longitude, double altitude); /*Sets gimbal orientation from rc client*/ diff --git a/include/roscontroller.h b/include/roscontroller.h index 61f635c..17533a4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -20,12 +20,12 @@ #include "mavros_msgs/ParamGet.h" #include "geometry_msgs/PoseStamped.h" #include "std_msgs/Float64.h" +#include "std_msgs/String.h" #include #include #include #include #include "buzz_utility.h" -#include "uav_utility.h" #include #include #include @@ -65,7 +65,9 @@ private: { } }; - typedef struct num_robot_count Num_robot_count; // not useful in cpp + typedef struct num_robot_count Num_robot_count; + + Num_robot_count count_robots; struct gps { @@ -115,13 +117,13 @@ private: bool rcclient; bool xbeeplugged = false; bool multi_msg; - Num_robot_count count_robots; ros::ServiceClient mav_client; ros::ServiceClient xbeestatus_srv; ros::ServiceClient capture_srv; ros::Publisher payload_pub; ros::Publisher MPpayload_pub; ros::Publisher neigh_pos_pub; + ros::Publisher uavstate_pub; ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; @@ -160,7 +162,7 @@ private: /*Initialize publisher and subscriber, done in the constructor*/ void Initialize_pub_sub(ros::NodeHandle& n_c); - std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode + std::string current_mode; /*Obtain data from ros parameter server*/ void Rosparameters_get(ros::NodeHandle& n_c_priv); @@ -174,6 +176,10 @@ private: /*Neighbours pos publisher*/ void neighbours_pos_publisher(); + /*UAVState publisher*/ + void uavstate_publisher(); + + /*BVM message payload publisher*/ void send_MPpayload(); /*Prepare messages and publish*/ @@ -190,11 +196,11 @@ private: /*Set the current position of the robot callback*/ void set_cur_pos(double latitude, double longitude, double altitude); + /*convert from spherical to cartesian coordinate system callback */ float constrainAngle(float x); void gps_rb(GPS nei_pos, double out[]); void gps_ned_cur(float& ned_x, float& ned_y, GPS t); - void gps_ned_home(float& ned_x, float& ned_y); void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, double gps_r_lat); diff --git a/include/uav_utility.h b/include/uav_utility.h deleted file mode 100644 index 69c447d..0000000 --- a/include/uav_utility.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef UAV_UTILITY_H -#define UAV_UTILITY_H - -extern void uav_setup(); - -extern void uav_done(); - -#endif diff --git a/log.txt b/log.txt deleted file mode 100644 index e038009..0000000 --- a/log.txt +++ /dev/null @@ -1,3 +0,0 @@ - -2017-01-08-14-24-24 enter TranslateFunc -2017-01-08-14-24-24 start to read frames! \ No newline at end of file diff --git a/readme.md b/readme.md index 7ab83c4..e686ac0 100644 --- a/readme.md +++ b/readme.md @@ -75,37 +75,41 @@ Run === To run the ROSBuzz package using the launch file, execute the following: - $ roslaunch rosbuzz rosbuzzm100.launch + $ roslaunch rosbuzz rosbuzz.launch -Note : Before launching the ROSBuzz node, verify all the parameter in the launch file. +Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch). + +* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script. Publisher ========= * Messages from Buzz (BVM): -The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink". +The package publishes mavros_msgs/Mavlink message "outMavlink". + +* Command to the flight controller: +The package publishes geometry_msgs/PoseStamped message "setpoint_position/local". Subscribers ----------- * Current position of the Robot: -The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos". +The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose". * Messages to Buzz (BVM): -The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink". +The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink". -* Battery status: -The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state". +* Status: +The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state". Service ------- -* Remote Controller command: -The package offers service using mavros_msgs/CommandInt service with name "rc_cmd". +* Remote Controller: +The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. -Client +References ------ +* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 -* Flight controller client: -This package is a client of mavros_msgs/CommandInt service with name "fc_cmd". - +* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index a8b19df..db19fa4 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -450,44 +450,6 @@ struct buzzswarm_elem_s }; typedef struct buzzswarm_elem_s* buzzswarm_elem_t; -void check_swarm_members(const void* key, void* data, void* params) -{ - buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; - int* status = (int*)params; - if (*status == 3) - return; - fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n", buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, - e->age); - if (buzzdarray_size(e->swarms) != 1) - { - fprintf(stderr, "Swarm list size is not 1\n"); - *status = 3; - } - else - { - int sid = 1; - if (!buzzdict_isempty(VM->swarms)) - { - if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid) - { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - if (buzzdict_size(VM->swarms) > 1) - { - sid = 2; - if (*buzzdict_get(VM->swarms, &sid, uint8_t) && buzzdarray_get(e->swarms, 0, uint16_t) != sid) - { - fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid, buzzdarray_get(e->swarms, 0, uint16_t)); - *status = 3; - return; - } - } - } -} - /*Step through the buzz script*/ void update_sensors() { diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 988b2d8..f84cecc 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -94,22 +94,6 @@ void setWPlist(string path) WPlistname = path + "include/graphs/waypointlist.csv"; } -/*----------------------------------------/ -/ Compute GPS destination from current position and desired Range and Bearing -/----------------------------------------*/ - -void gps_from_rb(double range, double bearing, double out[3]) -{ - double lat = RAD2DEG(cur_pos[0]); - double lon = RAD2DEG(cur_pos[1]); - out[0] = asin(sin(lat) * cos(range / EARTH_RADIUS) + cos(lat) * sin(range / EARTH_RADIUS) * cos(bearing)); - out[1] = lon + atan2(sin(bearing) * sin(range / EARTH_RADIUS) * cos(lat), - cos(range / EARTH_RADIUS) - sin(lat) * sin(out[0])); - out[0] = RAD2DEG(out[0]); - out[1] = RAD2DEG(out[1]); - out[2] = height; // constant height. -} - float constrainAngle(float x) { x = fmod(x, 2 * M_PI); @@ -118,6 +102,9 @@ float constrainAngle(float x) return x; } +/*----------------------------------------/ +/ Compute Range and Bearing from 2 GPS set of coordinates +/----------------------------------------*/ void rb_from_gps(double nei[], double out[], double cur[]) { double d_lon = nei[1] - cur[1]; @@ -129,11 +116,6 @@ void rb_from_gps(double nei[], double out[], double cur[]) out[2] = 0.0; } -// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests -double hcpos1[4] = { 45.564489, -73.562537, 45.564140, -73.562336 }; -double hcpos2[4] = { 45.564729, -73.562060, 45.564362, -73.562958 }; -double hcpos3[4] = { 45.564969, -73.562838, 45.564636, -73.563677 }; - void parse_gpslist() { // Open file: @@ -468,7 +450,6 @@ float* getgimbal() string getuavstate() { static buzzvm_t VM = buzz_utility::get_vm(); - std::stringstream state_buff; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); @@ -481,13 +462,6 @@ int getcmd() return cur_cmd; } -void set_goto(double pos[]) -{ - goto_pos[0] = pos[0]; - goto_pos[1] = pos[1]; - goto_pos[2] = pos[2]; -} - int bzz_cmd() { int cmd = buzz_cmd; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 25d2bf9..468c908 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -71,7 +71,6 @@ roscontroller::~roscontroller() /* Cleanup */ buzz_utility::buzz_script_destroy(); /* Stop the robot */ - uav_done(); log.close(); } @@ -364,7 +363,7 @@ used void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle) { m_sMySubscriptions.clear(); - std::string gps_topic, gps_type; + std::string gps_topic; if (node_handle.getParam("topics/gps", gps_topic)) ; else @@ -452,6 +451,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) payload_pub = n_c.advertise(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); neigh_pos_pub = n_c.advertise("neighbours_pos", 5); + uavstate_pub = n_c.advertise("uavstate", 5); localsetpoint_nonraw_pub = n_c.advertise(setpoint_name, 5); /* Service Clients*/ @@ -549,6 +549,16 @@ void roscontroller::neighbours_pos_publisher() neigh_pos_pub.publish(neigh_pos_array); } +/*---------------------------------------------------- +/ Publish current UAVState from Buzz script +/----------------------------------------------------*/ +void roscontroller::uavstate_publisher() +{ + std_msgs::String uavstate_msg; + uavstate_msg.data = buzzuav_closures::getuavstate(); + uavstate_pub.publish(uavstate_msg); +} + /*-------------------------------------------------------- / Functions handling the local MAV ROS flight controller /-------------------------------------------------------*/ @@ -877,11 +887,6 @@ void roscontroller::gps_ned_cur(float& ned_x, float& ned_y, GPS t) gps_convert_ned(ned_x, ned_y, t.longitude, t.latitude, cur_pos.longitude, cur_pos.latitude); } -void roscontroller::gps_ned_home(float& ned_x, float& ned_y) -{ - gps_convert_ned(ned_x, ned_y, cur_pos.longitude, cur_pos.latitude, home.longitude, home.latitude); -} - void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon, double gps_r_lat) { @@ -931,13 +936,6 @@ void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) { // ROS_INFO("Altitude out: %f", cur_rel_altitude); fcu_timeout = TIMEOUT; - // double lat = std::floor(msg->latitude * 1000000) / 1000000; - // double lon = std::floor(msg->longitude * 1000000) / 1000000; - /*if(cur_rel_altitude<1.2){ - home[0]=lat; - home[1]=lon; - home[2]=cur_rel_altitude; - }*/ set_cur_pos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); buzzuav_closures::set_currentpos(msg->latitude, msg->longitude, cur_rel_altitude); // msg->altitude); } diff --git a/src/uav_utility.cpp b/src/uav_utility.cpp deleted file mode 100644 index 98c6eeb..0000000 --- a/src/uav_utility.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "uav_utility.h" -#include "stdio.h" - -/****************************************/ -/****************************************/ - -/****************************************/ -/*To do !*/ -/****************************************/ - -void uav_setup() -{ -} - -/****************************************/ -/*To do !*/ -/****************************************/ - -void uav_done() -{ - fprintf(stdout, "Robot stopped.\n"); -} - -/****************************************/ -/****************************************/