force push from ros_drones_ws

CMakeLists.txt

@@ -5,11 +5,8 @@ if(UNIX)
     SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
 endif()
 
-if(SIM)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=${SIM} -DMAVROSKINETIC=${KIN}")
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1")
+
-else()
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1")
-endif()
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED COMPONENTS
   roscpp
@@ -70,6 +67,11 @@ install(TARGETS rosbuzz_node
    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+## Install project namespaced headers
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE)
 
 find_package(catkin REQUIRED COMPONENTS roslaunch)
 roslaunch_add_file_check(launch)

buzz_scripts/include/act/states.bzz

@@ -4,12 +4,14 @@
 #
 ########################################
 include "utils/vec2.bzz"
-include "act/old_barrier.bzz"
+include "act/barrier.bzz"
+#include "act/old_barrier.bzz"
 include "utils/conversions.bzz"
 
 TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
+
 GOTO_MAXVEL = 0.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 1.0  # m.
@@ -27,6 +29,7 @@ function idle() {
     BVMSTATE = "IDLE"
 }
 
+
 # Custom function for the user.
 function cusfun(){
 	BVMSTATE="CUSFUN"
@@ -108,9 +111,7 @@ function goto_gps(transf) {
 	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
 		transf()
 	else {
-		log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y )
+		m_navigation = LimitSpeed(m_navigation, 1.0)
-               	m_navigation = LimitSpeed(m_navigation, 1.0)
-                log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y )
 		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
 	}
 }

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -389,7 +389,7 @@ function TransitionToJoined(){
 	#write statues
 	#v_tag.put(m_nLabel, m_lockstig)
   	barrier_create()
-	barrier_ready(940)
+	barrier_ready(900+50)
 
 	m_navigation.x=0.0
 	m_navigation.y=0.0
@@ -568,7 +568,7 @@ function set_rc_goto() {
 
     goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
 	print("Saving GPS goal: ",goal.latitude, goal.longitude)
-    uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude)
+    storegoal(goal.latitude, goal.longitude, pose.position.altitude)
     m_gotjoinedparent = 1
   }
 }
@@ -677,6 +677,7 @@ function DoLock() {
       m_navigation=motion_vector()
   }
 #  #move
+
 #  goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
   BroadcastGraph()
 

buzz_scripts/include/utils/conversions.bzz

@@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) {
   }
   ned_x = d_lat/180*math.pi * 6371000.0;
   ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
-  m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
+	#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
-  m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x));
+	#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
-  return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing)
+  return math.vec2.new(ned_x,ned_y)
 }
 
 function gps_from_vec(vec) {

buzz_scripts/main.bzz

@@ -27,14 +27,15 @@ goal_list = {
 	.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
 }
 
-
 # Executed once at init time.
 function init() {
   	init_stig()
 	init_swarm()
-	initGraph()
+
+#	initGraph()
 
 	TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
+	
 	loop = 1
 
 	# start the swarm command listener
@@ -109,10 +110,8 @@ function step() {
  
 # Executed once when the robot (or the simulator) is reset.
 function reset() {
-	resetGraph()
 }
  
 # Executed once at the end of experiment.
 function destroy() {
-	destroyGraph()
 }

include/buzz_utility.h

@@ -90,6 +90,8 @@ int update_step_test();
 
 int get_robotid();
 
+int get_swarmsize();
+
 buzzvm_t get_vm();
 
 void set_robot_var(int ROBOTS);
@@ -98,7 +100,7 @@ int get_inmsg_size();
 
 std::vector<uint8_t*> get_inmsg_vector();
 
-std::string getuavstate();
+std::string get_bvmstate();
 
 int get_timesync_state();
 

include/buzzuav_closures.h

@@ -6,6 +6,7 @@
 #include "mavros_msgs/Mavlink.h"
 #include "ros/ros.h"
 #include "buzz_utility.h"
+#include "rosbuzz/mavrosCC.h"
 
 #define EARTH_RADIUS (double)6371000.0
 #define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
@@ -13,19 +14,6 @@
 
 namespace buzzuav_closures
 {
-typedef enum {
-  COMMAND_NIL = 0,  // Dummy command
-  COMMAND_TAKEOFF,  // Take off
-  COMMAND_LAND,
-  COMMAND_GOHOME,
-  COMMAND_ARM,
-  COMMAND_DISARM,
-  COMMAND_GOTO,
-  COMMAND_MOVETO,
-  COMMAND_PICTURE,
-  COMMAND_GIMBAL,
-} Custom_CommandCode;
-
 /*
  * prextern int() function in Buzz
  * This function is used to print data from buzz
@@ -89,6 +77,7 @@ void set_filtered_packet_loss(float value);
 /*
  * sets current position
  */
+
 void set_currentNEDpos(double x, double y);
 
 void set_currentpos(double latitude, double longitude, float altitude, float yaw);
@@ -101,6 +90,7 @@ double* getgoto();
  */
 std::map<int, std::map<int,int>> getgrid();
 
+
 /*
  * returns the gimbal commands
  */

include/rosbuzz/mavrosCC.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#if MAVROSKINETIC
+
+const short MISSION_START = mavros_msgs::CommandCode::MISSION_START;
+const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
+const short COMPONENT_ARM_DISARM = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
+
+#else
+
+const short MISSION_START = mavros_msgs::CommandCode::CMD_MISSION_START;
+const short DO_MOUNT_CONTROL = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
+const short COMPONENT_ARM_DISARM = CMD_COMPONENT_ARM_DISARM;
+
+#endif
+
+const short NAV_TAKEOFF = mavros_msgs::CommandCode::NAV_TAKEOFF;
+const short NAV_LAND = mavros_msgs::CommandCode::NAV_LAND;
+const short NAV_RETURN_TO_LAUNCH = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
+const short NAV_SPLINE_WAYPOINT = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT;
+const short NAV_WAYPOINT = mavros_msgs::CommandCode::NAV_WAYPOINT;
+const short IMAGE_START_CAPTURE = mavros_msgs::CommandCode::IMAGE_START_CAPTURE;
+const short CMD_REQUEST_UPDATE = 666;
+const short CMD_SYNC_CLOCK = 777;

include/roscontroller.h

@@ -35,8 +35,8 @@
 #include <signal.h>
 #include <ostream>
 #include <map>
-#include <cmath>
 #include "buzzuav_closures.h"
+#include "rosbuzz/mavrosCC.h"
 
 /*
 * ROSBuzz message types
@@ -44,7 +44,7 @@
 typedef enum {
   ROS_BUZZ_MSG_NIL = 0,    // dummy msg
   UPDATER_MESSAGE,         // Update msg
-  BUZZ_MESSAGE_WTO_TIME,   // Broadcast message wihout time info
+  BUZZ_MESSAGE_NO_TIME,   // Broadcast message wihout time info
   BUZZ_MESSAGE_TIME,       // Broadcast message with time info
 } rosbuzz_msgtype;
 
@@ -56,12 +56,10 @@ typedef enum {
 
 #define TIMEOUT 60
 #define BUZZRATE 10
-#define CMD_REQUEST_UPDATE 666
-#define CMD_SYNC_CLOCK 777
 
 using namespace std;
 
-namespace rosbzz_node
+namespace rosbuzz_node
 {
 class roscontroller
 {
@@ -99,6 +97,8 @@ private:
   };
   typedef struct POSE ros_pose;
 
+  ros_pose target, home, cur_pos;
+
   struct MsgData
   {
     int msgid;
@@ -112,8 +112,6 @@ private:
   };
   typedef struct MsgData msg_data;
 
-  ros_pose target, home, cur_pos;
-
   uint64_t payload;
   std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
   std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
@@ -133,7 +131,6 @@ private:
   int armstate;
   int barrier;
   int update;
-  int statepub_active;
   int message_number = 0;
   uint8_t no_of_robots = 0;
   bool rcclient;
@@ -141,6 +138,7 @@ private:
   bool multi_msg;
   uint8_t no_cnt = 0;
   uint8_t old_val = 0;
+  bool debug = false;
   std::string bzzfile_name;
   std::string fcclient_name;
   std::string armclient;
@@ -164,7 +162,7 @@ private:
   ros::Publisher payload_pub;
   ros::Publisher MPpayload_pub;
   ros::Publisher neigh_pos_pub;
-  ros::Publisher uavstate_pub;
+  ros::Publisher bvmstate_pub;
   ros::Publisher grid_pub;
   ros::Publisher localsetpoint_nonraw_pub;
   ros::ServiceServer service;
@@ -212,7 +210,7 @@ private:
   void neighbours_pos_publisher();
 
   /*UAVState publisher*/
-  void uavstate_publisher();
+  void state_publisher();
 
   /*Grid publisher*/
   void grid_publisher();
@@ -302,6 +300,7 @@ private:
   bool GetRawPacketLoss(const uint8_t short_id, float& result);
   bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
   void get_xbee_status();
+
   void time_sync_step();
   void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr);
   uint64_t get_logical_time();

src/buzz_utility.cpp

@@ -204,25 +204,25 @@ static int buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
@@ -258,25 +258,25 @@ static int testing_buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
@@ -366,7 +366,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
   }
 
   // Initialize UAVSTATE variable
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "BMVSTATE", 1));
   buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
   buzzvm_gstore(VM);
 
@@ -426,7 +426,7 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t
   }
 
   // Initialize UAVSTATE variable
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
   buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
   buzzvm_gstore(VM);
 
@@ -539,14 +539,6 @@ int update_step_test()
   return a == BUZZVM_STATE_READY;
 }
 
-buzzvm_t get_vm()
-/*
-/ return the BVM
-----------------*/
-{
-  return VM;
-}
-
 void set_robot_var(int ROBOTS)
 /*
 / set swarm size in the BVM
@@ -569,7 +561,15 @@ std::vector<uint8_t*> get_inmsg_vector(){
   return IN_MSG;
 }
 
-string getuavstate()
+buzzvm_t get_vm()
+/*
+/ return the BVM
+----------------*/
+{
+  return VM;
+}
+
+string get_bvmstate()
 /*
 / return current BVM state
 ---------------------------------------*/
@@ -579,12 +579,19 @@ string getuavstate()
     buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
     buzzvm_gload(VM);
     buzzobj_t obj = buzzvm_stack_at(VM, 1);
-    uav_state = string(obj->s.value.str);
+    if(obj->o.type == BUZZTYPE_STRING)
+      uav_state = string(obj->s.value.str);
+    else
+      uav_state = "Not Available";
     buzzvm_pop(VM);
   }
   return uav_state;
 }
 
+int get_swarmsize() {
+  return (int)buzzdict_size(VM->swarmmembers) + 1;
+}
+
 int get_timesync_state()
 /*
 / return time sync state from bzz script

src/buzzuav_closures.cpp

@@ -105,10 +105,10 @@ float constrainAngle(float x)
 / Wrap the angle between -pi, pi
 ----------------------------------------------------------- */
 {
-  x = fmod(x, 2 * M_PI);
+  x = fmod(x + M_PI, 2 * M_PI);
   if (x < 0.0)
     x += 2 * M_PI;
-  return x;
+  return x - M_PI;
 }
 
 void rb_from_gps(double nei[], double out[], double cur[])
@@ -229,9 +229,9 @@ int buzzuav_moveto(buzzvm_t vm)
   goto_pos[2] = height + dh;
   goto_pos[3] = dyaw;
   //  DEBUG
-   ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
+  // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
-   goto_pos[1], goto_pos[2]);
+  // goto_pos[1], goto_pos[2]);
-  buzz_cmd = COMMAND_MOVETO;  // TODO: standard mavros?
+  buzz_cmd = NAV_SPLINE_WAYPOINT;  // TODO: standard mavros?
   return buzzvm_ret0(vm);
 }
 
@@ -332,7 +332,7 @@ int buzzuav_takepicture(buzzvm_t vm)
 / Buzz closure to take a picture here.
 /----------------------------------------*/
 {
-  buzz_cmd = COMMAND_PICTURE;
+  buzz_cmd = IMAGE_START_CAPTURE;
   return buzzvm_ret0(vm);
 }
 
@@ -356,7 +356,7 @@ int buzzuav_setgimbal(buzzvm_t vm)
   rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
 
   ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
-  buzz_cmd = COMMAND_GIMBAL;
+  buzz_cmd = DO_MOUNT_CONTROL;
   return buzzvm_ret0(vm);
 }
 
@@ -389,10 +389,10 @@ int buzzuav_storegoal(buzzvm_t vm)
   double rb[3];
 
   rb_from_gps(goal, rb, cur_pos);
-  if (fabs(rb[0]) < 150.0)
+  if (fabs(rb[0]) < 150.0) {
     rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
-
+    ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
-  ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
+  }
   return buzzvm_ret0(vm);
 }
 
@@ -401,13 +401,9 @@ int buzzuav_arm(buzzvm_t vm)
 / Buzz closure to arm
 /---------------------------------------*/
 {
-#ifdef MAVROSKINETIC
+  cur_cmd = COMPONENT_ARM_DISARM;
-  cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
-#else
-  cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
-#endif
   printf(" Buzz requested Arm \n");
-  buzz_cmd = COMMAND_ARM;
+  buzz_cmd = cur_cmd;
   return buzzvm_ret0(vm);
 }
 
@@ -416,13 +412,9 @@ int buzzuav_disarm(buzzvm_t vm)
 / Buzz closure to disarm
 /---------------------------------------*/
 {
-#ifdef MAVROSKINETIC
+  cur_cmd = COMPONENT_ARM_DISARM + 1;
-  cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1;
-#else
-  cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
-#endif
   printf(" Buzz requested Disarm  \n");
-  buzz_cmd = COMMAND_DISARM;
+  buzz_cmd = cur_cmd;
   return buzzvm_ret0(vm);
 }
 
@@ -436,9 +428,9 @@ int buzzuav_takeoff(buzzvm_t vm)
   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
   goto_pos[2] = buzzvm_stack_at(vm, 1)->f.value;
   height = goto_pos[2];
-  cur_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
+  cur_cmd = NAV_TAKEOFF;
   printf(" Buzz requested Take off !!! \n");
-  buzz_cmd = COMMAND_TAKEOFF;
+  buzz_cmd = cur_cmd;
   return buzzvm_ret0(vm);
 }
 
@@ -447,9 +439,9 @@ int buzzuav_land(buzzvm_t vm)
 / Buzz closure to land
 /-------------------------------------------------------------*/
 {
-  cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
+  cur_cmd = NAV_LAND;
   printf(" Buzz requested Land !!! \n");
-  buzz_cmd = COMMAND_LAND;
+  buzz_cmd = cur_cmd;
   return buzzvm_ret0(vm);
 }
 
@@ -458,9 +450,9 @@ int buzzuav_gohome(buzzvm_t vm)
 / Buzz closure to return Home
 /-------------------------------------------------------------*/
 {
-  cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
+  cur_cmd = NAV_RETURN_TO_LAUNCH;
   printf(" Buzz requested gohome !!! \n");
-  buzz_cmd = COMMAND_GOHOME;
+  buzz_cmd = cur_cmd;
   return buzzvm_ret0(vm);
 }
 

src/rosbuzz.cpp

@@ -17,7 +17,7 @@ int main(int argc, char** argv)
   ros::init(argc, argv, "rosBuzz");
   ros::NodeHandle nh_priv("~");
   ros::NodeHandle nh;
-  rosbzz_node::roscontroller RosController(nh, nh_priv);
+  rosbuzz_node::roscontroller RosController(nh, nh_priv);
 
   RosController.RosControllerRun();
   return 0;

src/roscontroller.cpp

@@ -8,10 +8,9 @@
 
 #include "roscontroller.h"
 #include <thread>
-namespace rosbzz_node
+namespace rosbuzz_node
 {
 const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
-const bool debug = true;
 
 roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
 logical_clock(ros::Time()), previous_step_time(ros::Time())
@@ -132,23 +131,15 @@ void roscontroller::RosControllerRun()
     //  set ROS loop rate
     ros::Rate loop_rate(BUZZRATE);
     // check for BVMSTATE variable
-    buzzvm_t VM = buzz_utility::get_vm();
+    if(buzz_utility::get_bvmstate()=="Not Available")
-    buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
+      ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
-    buzzvm_gload(VM);
-    buzzobj_t obj = buzzvm_stack_at(VM, 1);
-    if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1;
-    else 
-    {
-      statepub_active = 0;
-      ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled.");
-    }
     //  DEBUG
     // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
     while (ros::ok() && !buzz_utility::buzz_script_done())
     {
       //  Publish topics
       neighbours_pos_publisher();
-      if(statepub_active) uavstate_publisher();
+      state_publisher();
       grid_publisher();
       send_MPpayload();
       //  Check updater state and step code
@@ -184,10 +175,6 @@ void roscontroller::RosControllerRun()
             << cur_pos.altitude * 100000 << ",";
       log << (int)no_of_robots<<",";
       log  << neighbours_pos_map.size()<< ",";
-      log<<(int)inmsgdata.size()<<",";
-      log<< message_number<<",";
-      log<< out_msg_time<<",";
-      log <<buzz_utility::getuavstate();
       // if(neighbours_pos_map.size() > 0)log<<",";
       map<int, buzz_utility::Pos_struct>::iterator it =
             neighbours_pos_map.begin();
@@ -202,7 +189,9 @@ void roscontroller::RosControllerRun()
             <<","<<it->received_time;
       }
       inmsgdata.clear();
-      log<<std::endl;
+      log<<(int)inmsgdata.size()<<","<< message_number<<",";
+      log<< out_msg_time<<",";
+      log <<buzz_utility::get_bvmstate()<<std::endl;
 
       // time_sync_step();
       // if(debug)
@@ -227,7 +216,7 @@ void roscontroller::RosControllerRun()
                  loop_rate.cycleTime().toSec());
       // Safety land if the data coming from the flight controller are too old
       if (fcu_timeout <= 0)
-        buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);
+        buzzuav_closures::rc_call(NAV_LAND);
       else
         fcu_timeout -= 1 / BUZZRATE;
       timer_step += 1;
@@ -252,6 +241,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
     ROS_ERROR("Provide a .bzz file to run in Launch file");
     system("rosnode kill rosbuzz_node");
   }
+  // Obtain debug mode from launch file parameter
+  if (n_c.getParam("debug", debug))
+    ;
+  else
+  {
+    ROS_ERROR("Provide a debug mode in Launch file");
+    system("rosnode kill rosbuzz_node");
+  }
   // Obtain rc service option from parameter server
   if (n_c.getParam("rcclient", rcclient))
   {
@@ -335,7 +332,7 @@ void roscontroller::GetSubscriptionParameters(ros::NodeHandle& node_handle)
   m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/LaserScan"));
 
   node_handle.getParam("topics/battery", topic);
-  m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/BatteryStatus"));
+  m_smTopic_infos.insert(pair<std::string, std::string>(topic, "sensor_msgs/BatteryState"));
 
   node_handle.getParam("topics/status", topic);
   m_smTopic_infos.insert(pair<std::string, std::string>(topic, "mavros_msgs/State"));
@@ -398,7 +395,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", MAX_NUMBER_OF_ROBOTS);
-  uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
+  bvmstate_pub = n_c.advertise<std_msgs::String>("bvmstate", 5);
   grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
   localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
 
@@ -431,7 +428,7 @@ void roscontroller::Subscribe(ros::NodeHandle& n_c)
     {
       flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
     }
-    else if (it->second == "mavros_msgs/BatteryStatus")
+    else if (it->second == "sensor_msgs/BatteryState")
     {
       battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
     }
@@ -506,14 +503,14 @@ void roscontroller::neighbours_pos_publisher()
   neigh_pos_pub.publish(neigh_pos_array);
 }
 
-void roscontroller::uavstate_publisher()
+void roscontroller::state_publisher()
 /*
 / Publish current UAVState from Buzz script
 /----------------------------------------------------*/
 {
-  std_msgs::String uavstate_msg;
+  std_msgs::String state_msg;
-  uavstate_msg.data = buzz_utility::getuavstate();
+  state_msg.data = buzz_utility::get_bvmstate();
-  uavstate_pub.publish(uavstate_msg);
+  bvmstate_pub.publish(state_msg);
 }
 
 void roscontroller::grid_publisher()
@@ -639,7 +636,7 @@ with size.........  |   /
   }
   else{
     // prepare rosbuzz msg header
-    tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
+    tmphead[0] = (uint8_t)BUZZ_MESSAGE_NO_TIME;
     memcpy(rheader, tmphead, sizeof(uint64_t));
     // push header into the buffer
     payload_out.payload64.push_back(rheader[0]);
@@ -714,7 +711,7 @@ script
   float* gimbal;
   switch (buzzuav_closures::bzz_cmd())
   {
-    case buzzuav_closures::COMMAND_TAKEOFF:
+    case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
       cmd_srv.request.param7 = goto_pos[2];
       cmd_srv.request.command = buzzuav_closures::getcmd();
@@ -737,7 +734,7 @@ script
         ROS_ERROR("Failed to call service from flight controller");
       break;
 
-    case buzzuav_closures::COMMAND_LAND:
+    case NAV_LAND:
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (current_mode != "LAND")
       {
@@ -756,7 +753,7 @@ script
       armstate = 0;
       break;
 
-    case buzzuav_closures::COMMAND_GOHOME:  // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
+    case NAV_RETURN_TO_LAUNCH:  // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
       cmd_srv.request.param5 = home.latitude;
       cmd_srv.request.param6 = home.longitude;
       cmd_srv.request.param7 = home.altitude;
@@ -769,32 +766,7 @@ script
         ROS_ERROR("Failed to call service from flight controller");
       break;
 
-    case buzzuav_closures::COMMAND_GOTO:  // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
+    case COMPONENT_ARM_DISARM:
-      goto_pos = buzzuav_closures::getgoto();
-      cmd_srv.request.param5 = goto_pos[0];
-      cmd_srv.request.param6 = goto_pos[1];
-      cmd_srv.request.param7 = goto_pos[2];
-      cmd_srv.request.command = buzzuav_closures::getcmd();
-      if (mav_client.call(cmd_srv))
-      {
-        ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
-      }
-      else
-        ROS_ERROR("Failed to call service from flight controller");
-#ifdef MAVROSKINETIC
-      cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START;
-#else
-      cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
-#endif
-      if (mav_client.call(cmd_srv))
-      {
-        ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
-      }
-      else
-        ROS_ERROR("Failed to call service from flight controller");
-      break;
-
-    case buzzuav_closures::COMMAND_ARM:
       if (!armstate)
       {
         SetMode("LOITER", 0);
@@ -803,7 +775,7 @@ script
       }
       break;
 
-    case buzzuav_closures::COMMAND_DISARM:
+    case COMPONENT_ARM_DISARM+1:
       if (armstate)
       {
         armstate = 0;
@@ -812,22 +784,18 @@ script
       }
       break;
 
-    case buzzuav_closures::COMMAND_MOVETO:
+    case NAV_SPLINE_WAYPOINT:
       goto_pos = buzzuav_closures::getgoto();
       roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
-    case buzzuav_closures::COMMAND_GIMBAL:
+    case DO_MOUNT_CONTROL:
       gimbal = buzzuav_closures::getgimbal();
       cmd_srv.request.param1 = gimbal[0];
       cmd_srv.request.param2 = gimbal[1];
       cmd_srv.request.param3 = gimbal[2];
       cmd_srv.request.param4 = gimbal[3];
-#ifdef MAVROSKINETIC
+      cmd_srv.request.command = DO_MOUNT_CONTROL;
-      cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
-#else
-      cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
-#endif
       if (mav_client.call(cmd_srv))
       {
         ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@@ -836,7 +804,7 @@ script
         ROS_ERROR("Failed to call service from flight controller");
       break;
 
-    case buzzuav_closures::COMMAND_PICTURE:
+    case IMAGE_START_CAPTURE:
       ROS_INFO("TAKING A PICTURE HERE!! --------------");
       mavros_msgs::CommandBool capture_command;
       if (capture_srv.call(capture_command))
@@ -902,10 +870,10 @@ float roscontroller::constrainAngle(float x)
 / Wrap the angle between -pi, pi
 ----------------------------------------------------------- */
 {
-  x = fmod(x, 2 * M_PI);
+  x = fmod(x + M_PI, 2 * M_PI);
   if (x < 0.0)
     x += 2 * M_PI;
-  return x;
+  return x - M_PI;
 }
 
 void roscontroller::gps_rb(POSE nei_pos, double out[])
@@ -917,10 +885,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
   float ned_x = 0.0, ned_y = 0.0;
   gps_ned_cur(ned_x, ned_y, nei_pos);
   out[0] = sqrt(ned_x * ned_x + ned_y * ned_y);
-  // 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] = std::floor(out[1] * 1000000) / 1000000;
   out[2] = 0.0;
 }
 
@@ -996,6 +961,7 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
 {
   cur_pos.x = msg->pose.position.x;
   cur_pos.y = msg->pose.position.y;
+
   buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
   //  cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
   tf::Quaternion q(
@@ -1156,7 +1122,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
   }
   //  BVM FIFO message
   else if ((int)mtype == (int)BUZZ_MESSAGE_TIME ||
-           (int)mtype == (int)BUZZ_MESSAGE_WTO_TIME)
+           (int)mtype == (int)BUZZ_MESSAGE_NO_TIME)
   {
     uint64_t message_obt[msg->payload64.size() - 1];
     //  Go throught the obtained payload
@@ -1217,25 +1183,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
   int rc_cmd;
   switch (req.command)
   {
-    case mavros_msgs::CommandCode::NAV_TAKEOFF:
+    case NAV_TAKEOFF:
       ROS_INFO("RC_call: TAKE OFF!!!!");
-      rc_cmd = mavros_msgs::CommandCode::NAV_TAKEOFF;
+      rc_cmd = NAV_TAKEOFF;
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
-    case mavros_msgs::CommandCode::NAV_LAND:
+    case NAV_LAND:
       ROS_INFO("RC_Call: LAND!!!!");
-      rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
+      rc_cmd = NAV_LAND;
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
-#ifdef MAVROSKINETIC
+    case COMPONENT_ARM_DISARM:
-    case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
+      rc_cmd = COMPONENT_ARM_DISARM;
-      rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
-#else
-    case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
-      rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
-#endif
       armstate = req.param1;
       if (armstate)
       {
@@ -1250,31 +1211,23 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
         res.success = true;
       }
       break;
-    case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
+    case NAV_RETURN_TO_LAUNCH:
       ROS_INFO("RC_Call: GO HOME!!!!");
-      rc_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
+      rc_cmd = NAV_RETURN_TO_LAUNCH;
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
-    case mavros_msgs::CommandCode::NAV_WAYPOINT:
+    case NAV_WAYPOINT:
       ROS_INFO("RC_Call: GO TO!!!! ");
       buzzuav_closures::rc_set_goto(req.param1, req.param5, req.param6, req.param7);
-      rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
+      rc_cmd = NAV_WAYPOINT;
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
-#ifdef MAVROSKINETIC
+    case DO_MOUNT_CONTROL:
-    case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
-#else
-    case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
-#endif
       ROS_INFO("RC_Call: Gimbal!!!! ");
       buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5);
-#ifdef MAVROSKINETIC
+      rc_cmd = DO_MOUNT_CONTROL;
-      rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
-#else
-      rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
-#endif
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
@@ -1303,7 +1256,7 @@ void roscontroller::get_number_of_robots()
 / Garbage collector for the number of robots in the swarm
 --------------------------------------------------------------------------*/
 {
-  int cur_robots = (int)buzzdict_size(buzz_utility::get_vm()->swarmmembers) + 1;
+  int cur_robots = buzz_utility::get_swarmsize();
   if (no_of_robots == 0)
   {
     no_of_robots = cur_robots;