From 52357b142de7969e5b9dc783118ee41211c4a9eb Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 6 Sep 2018 16:19:58 -0400 Subject: [PATCH] added mavros conversion header --- CMakeLists.txt | 5 +++ include/buzzuav_closures.h | 14 +------ include/rosbuzz/mavrosCC.h | 24 +++++++++++ include/roscontroller.h | 3 +- src/buzzuav_closures.cpp | 34 ++++++--------- src/roscontroller.cpp | 84 ++++++++++---------------------------- 6 files changed, 65 insertions(+), 99 deletions(-) create mode 100644 include/rosbuzz/mavrosCC.h diff --git a/CMakeLists.txt b/CMakeLists.txt index e7f0050..abe0535 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index f1d1852..71b99f1 100644 --- a/include/buzzuav_closures.h +++ b/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 diff --git a/include/rosbuzz/mavrosCC.h b/include/rosbuzz/mavrosCC.h new file mode 100644 index 0000000..1f84c29 --- /dev/null +++ b/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; diff --git a/include/roscontroller.h b/include/roscontroller.h index 781f298..c157c38 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -37,6 +37,7 @@ #include #include #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; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 1e368c5..36d47a7 100644 --- a/src/buzzuav_closures.cpp +++ b/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 = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM; -#else - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; -#endif + cur_cmd = COMPONENT_ARM_DISARM; 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 = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1; -#else - cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; -#endif + cur_cmd = COMPONENT_ARM_DISARM + 1; 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); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ddd71ea..3b3fe58 100644 --- a/src/roscontroller.cpp +++ b/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 !!! - 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: + case COMPONENT_ARM_DISARM: 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 = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; -#else - cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; -#endif + cmd_srv.request.command = DO_MOUNT_CONTROL; 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 mavros_msgs::CommandCode::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 + case COMPONENT_ARM_DISARM: + rc_cmd = COMPONENT_ARM_DISARM; 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 mavros_msgs::CommandCode::DO_MOUNT_CONTROL: -#else - case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: -#endif + case DO_MOUNT_CONTROL: ROS_INFO("RC_Call: Gimbal!!!! "); buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); -#ifdef MAVROSKINETIC - rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL; -#else - rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; -#endif + rc_cmd = DO_MOUNT_CONTROL; buzzuav_closures::rc_call(rc_cmd); res.success = true; break;