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

fix modes usage for spiris mavros compatibility

This commit is contained in:
dave 2018-09-23 22:02:05 -04:00
parent 4fa1e7a4ec
commit 4eb044a4a9
3 changed files with 29 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
buzz_scripts/include/act/states.bzz
View File

@ -48,7 +48,7 @@ function launch() {
BVMSTATE = "LAUNCH" BVMSTATE = "LAUNCH"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude} homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22) barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready(22) barrier_ready(22)
} else { } else {
@ -77,7 +77,7 @@ function stop() {
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
uav_land() uav_land()
if(flight.status != 2 and flight.status != 3) { if(pose.position.altitude <= 0.1) {
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21) #barrier_ready(21)

1
include/roscontroller.h
View File

@ -139,6 +139,7 @@ private:
uint8_t no_cnt = 0; uint8_t no_cnt = 0;
uint8_t old_val = 0; uint8_t old_val = 0;
bool debug = false; bool debug = false;
bool setmode = false;
std::string bzzfile_name; std::string bzzfile_name;
std::string fcclient_name; std::string fcclient_name;
std::string armclient; std::string armclient;

34
src/roscontroller.cpp
View File

@ -30,7 +30,8 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
// Initialize variables // Initialize variables
SetMode("LOITER", 0); if(setmode)
SetMode("LOITER", 0);
armstate = 0; armstate = 0;
multi_msg = true; multi_msg = true;
setpoint_counter = 0; setpoint_counter = 0;
@ -251,6 +252,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
ROS_ERROR("Provide a debug mode in Launch file"); ROS_ERROR("Provide a debug mode in Launch file");
system("rosnode kill rosbuzz_node"); system("rosnode kill rosbuzz_node");
} }
// Obtain setmode mode from launch file parameter
if (n_c.getParam("setmode", setmode))
;
else
{
ROS_ERROR("Provide a setmode mode in Launch file");
system("rosnode kill rosbuzz_node");
}
// Obtain rc service option from parameter server // Obtain rc service option from parameter server
if (n_c.getParam("rcclient", rcclient)) if (n_c.getParam("rcclient", rcclient))
{ {
@ -403,9 +412,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
// Service Clients // Service Clients
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient); arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient); if(setmode)
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name); mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
if (rcclient == true) if (rcclient)
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
ROS_INFO("Ready to receive Mav Commands from RC client"); ROS_INFO("Ready to receive Mav Commands from RC client");
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name); xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
@ -719,14 +729,15 @@ script
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (!armstate) if (!armstate)
{ {
SetMode("LOITER", 0); if(setmode)
SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
// Registering HOME POINT. // Registering HOME POINT.
home = cur_pos; home = cur_pos;
} }
if (current_mode != "GUIDED") if (current_mode != "GUIDED" && setmode)
SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo
if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
{ {
@ -738,12 +749,17 @@ script
case NAV_LAND: case NAV_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND") if (current_mode != "LAND" && setmode)
{ {
SetMode("LAND", 0); SetMode("LAND", 0);
armstate = 0; armstate = 0;
Arm(); Arm();
} }
else if(cur_pos.altitude < 0.1) //disarm only when close to ground
{
armstate = 0;
Arm();
}
if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
{ {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@ -771,7 +787,8 @@ script
case COMPONENT_ARM_DISARM: case COMPONENT_ARM_DISARM:
if (!armstate) if (!armstate)
{ {
SetMode("LOITER", 0); if(setmode)
SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
} }
@ -780,8 +797,9 @@ script
case COMPONENT_ARM_DISARM+1: case COMPONENT_ARM_DISARM+1:
if (armstate) if (armstate)
{ {
if(setmode)
SetMode("LOITER", 0);
armstate = 0; armstate = 0;
SetMode("LOITER", 0);
Arm(); Arm();
} }
break; break;