Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added mavros conversion header

This commit is contained in:
dave 2018-09-06 16:19:58 -04:00
parent 7cb51a4cbc
commit 52357b142d
6 changed files with 65 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -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)

14
include/buzzuav_closures.h
View File

@ -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

24
include/rosbuzz/mavrosCC.h Normal file
View File

@ -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;

3
include/roscontroller.h
View File

@ -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;

34
src/buzzuav_closures.cpp
View File

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

84
src/roscontroller.cpp
View File

@ -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;