added mavros conversion header

CMakeLists.txt

@@ -75,6 +75,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)

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

include/rosbuzz/mavrosCC.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#ifdef 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

@@ -37,6 +37,7 @@
 #include <map>
 #include <cmath>
 #include "buzzuav_closures.h"
+#include "rosbuzz/mavrosCC.h"
 
 /*
 * ROSBuzz message types
@@ -56,8 +57,6 @@ typedef enum {
 
 #define TIMEOUT 60
 #define BUZZRATE 10
-#define CMD_REQUEST_UPDATE 666
-#define CMD_SYNC_CLOCK 777
 
 using namespace std;
 

src/buzzuav_closures.cpp

@@ -231,7 +231,7 @@ int buzzuav_moveto(buzzvm_t vm)
   //  DEBUG
    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]);
-  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);
 }
 
@@ -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/roscontroller.cpp

@@ -714,7 +714,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 +737,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 +756,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 +769,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 +778,7 @@ script
       }
       break;
 
-    case buzzuav_closures::COMMAND_DISARM:
+    case COMPONENT_ARM_DISARM+1:
       if (armstate)
       {
         armstate = 0;
@@ -812,22 +787,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 +807,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))
@@ -1218,25 +1189,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)
       {
@@ -1251,31 +1217,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;