From 4eb044a4a91058c61738ac675be14ddcd913d6ab Mon Sep 17 00:00:00 2001 From: dave Date: Sun, 23 Sep 2018 22:02:05 -0400 Subject: [PATCH] fix modes usage for spiris mavros compatibility --- buzz_scripts/include/act/states.bzz | 4 ++-- include/roscontroller.h | 1 + src/roscontroller.cpp | 34 ++++++++++++++++++++++------- 3 files changed, 29 insertions(+), 10 deletions(-) diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 0869852..0cca81d 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -48,7 +48,7 @@ function launch() { BVMSTATE = "LAUNCH" if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF 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_ready(22) } else { @@ -77,7 +77,7 @@ function stop() { if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND neighbors.broadcast("cmd", 21) uav_land() - if(flight.status != 2 and flight.status != 3) { + if(pose.position.altitude <= 0.1) { BVMSTATE = "TURNEDOFF" #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21) #barrier_ready(21) diff --git a/include/roscontroller.h b/include/roscontroller.h index d7bbbc5..00a61a3 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -139,6 +139,7 @@ private: uint8_t no_cnt = 0; uint8_t old_val = 0; bool debug = false; + bool setmode = false; std::string bzzfile_name; std::string fcclient_name; std::string armclient; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index a16b042..bc4080a 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -30,7 +30,8 @@ logical_clock(ros::Time()), previous_step_time(ros::Time()) buzz_update::set_bzz_file(bzzfile_name.c_str(),debug); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); // Initialize variables - SetMode("LOITER", 0); + if(setmode) + SetMode("LOITER", 0); armstate = 0; multi_msg = true; setpoint_counter = 0; @@ -251,6 +252,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c) ROS_ERROR("Provide a debug mode in Launch file"); 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 if (n_c.getParam("rcclient", rcclient)) { @@ -403,9 +412,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c) // Service Clients arm_client = n_c.serviceClient(armclient); - mode_client = n_c.serviceClient(modeclient); + if(setmode) + mode_client = n_c.serviceClient(modeclient); mav_client = n_c.serviceClient(fcclient_name); - if (rcclient == true) + if (rcclient) service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this); ROS_INFO("Ready to receive Mav Commands from RC client"); xbeestatus_srv = n_c.serviceClient(xbeesrv_name); @@ -719,14 +729,15 @@ script cmd_srv.request.command = buzzuav_closures::getcmd(); if (!armstate) { - SetMode("LOITER", 0); + if(setmode) + SetMode("LOITER", 0); armstate = 1; Arm(); ros::Duration(0.5).sleep(); // Registering HOME POINT. home = cur_pos; } - if (current_mode != "GUIDED") + if (current_mode != "GUIDED" && setmode) SetMode("GUIDED", 2000); // added for compatibility with 3DR Solo if (mav_client.call(cmd_srv)) { @@ -738,12 +749,17 @@ script case NAV_LAND: cmd_srv.request.command = buzzuav_closures::getcmd(); - if (current_mode != "LAND") + if (current_mode != "LAND" && setmode) { SetMode("LAND", 0); armstate = 0; Arm(); } + else if(cur_pos.altitude < 0.1) //disarm only when close to ground + { + armstate = 0; + Arm(); + } if (mav_client.call(cmd_srv)) { ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); @@ -771,7 +787,8 @@ script case COMPONENT_ARM_DISARM: if (!armstate) { - SetMode("LOITER", 0); + if(setmode) + SetMode("LOITER", 0); armstate = 1; Arm(); } @@ -780,8 +797,9 @@ script case COMPONENT_ARM_DISARM+1: if (armstate) { + if(setmode) + SetMode("LOITER", 0); armstate = 0; - SetMode("LOITER", 0); Arm(); } break;