removed unused functions and variables

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)

include/buzzuav_closures.h

@@ -2,7 +2,6 @@
 
 #include <buzz/buzzvm.h>
 #include <stdio.h>
-#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*/

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 <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
 #include <sstream>
 #include <buzz/buzzasm.h>
 #include "buzz_utility.h"
-#include "uav_utility.h"
 #include <stdlib.h>
 #include <string.h>
 #include <math.h>
@@ -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);
 

include/uav_utility.h

@@ -1,8 +0,0 @@
-#ifndef UAV_UTILITY_H
-#define UAV_UTILITY_H
-
-extern void uav_setup();
-
-extern void uav_done();
-
-#endif

log.txt

@@ -1,3 +0,0 @@
-
-2017-01-08-14-24-24    enter TranslateFunc
-2017-01-08-14-24-24    start to read frames!

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:
+* Status:
-The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
+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:
+* Remote Controller:
-The package offers service using mavros_msgs/CommandInt service with name "rc_cmd".
+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:
+* 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.
-This package is a client of mavros_msgs/CommandInt service with name "fc_cmd".
- 

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()
 {

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;

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<mavros_msgs::Mavlink>(out_payload, 5);
   MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
   neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
+  uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
   localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(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);
 }

src/uav_utility.cpp

@@ -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");
-}
-
-/****************************************/
-/****************************************/