From 9088a5d1f5503a5227ff6d3e5398a0babb27adf8 Mon Sep 17 00:00:00 2001 From: isvogor Date: Fri, 17 Feb 2017 16:35:08 -0500 Subject: [PATCH 1/2] added arming and status to GUIDED mode --- include/roscontroller.h | 11 +++++++++++ src/buzzuav_closures.cpp | 6 +++--- src/roscontroller.cpp | 30 ++++++++++++++++++++++++++++- src/test1.bzz | 41 ++++++++++++++++++++-------------------- 4 files changed, 64 insertions(+), 24 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index 9ccbaf4..df706d4 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -5,7 +5,9 @@ #include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandInt.h" #include "mavros_msgs/CommandLong.h" +#include "mavros_msgs/CommandBool.h" #include "mavros_msgs/ExtendedState.h" +#include "mavros_msgs/SetMode.h" #include "mavros_msgs/State.h" #include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/Mavlink.h" @@ -71,6 +73,11 @@ private: std::vector m_sMySubscriptions; std::map m_smTopic_infos; + mavros_msgs::CommandBool m_cmdBool; + ros::ServiceClient arm_client; + + mavros_msgs::SetMode m_cmdSetMode; + ros::ServiceClient mode_client; void Initialize_pub_sub(ros::NodeHandle n_c); @@ -134,6 +141,10 @@ private: void GetSubscriptionParameters(ros::NodeHandle node_handle); + void Arm(); + + void SetMode(); + void Subscribe(ros::NodeHandle n_c); }; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index d48f689..a24488e 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -219,21 +219,21 @@ int buzzuav_takeoff(buzzvm_t vm) { height = goto_pos[2]; cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); - buzz_cmd=1; + buzz_cmd = 1; return buzzvm_ret0(vm); } int buzzuav_land(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::NAV_LAND; printf(" Buzz requested Land !!! \n"); - buzz_cmd=1; + buzz_cmd = 1; return buzzvm_ret0(vm); } int buzzuav_gohome(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; printf(" Buzz requested gohome !!! \n"); - buzz_cmd=1; + buzz_cmd = 1; return buzzvm_ret0(vm); } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index bd6b885..a21018b 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -185,6 +185,8 @@ namespace rosbzz_node{ //} //else { //TODO: Here + arm_client = n_c.serviceClient("/mavros/cmd/arming"); + mode_client = n_c.serviceClient("/mavros/set_mode"); mav_client = n_c.serviceClient(fcclient_name); //} @@ -234,6 +236,29 @@ namespace rosbzz_node{ dbgfname = bzzfile_in_compile.str(); } + + void roscontroller::Arm(){ + mavros_msgs::CommandBool arming_message; + arming_message.request.value = true; + if(arm_client.call(arming_message)) { + ROS_INFO("Service called!"); + } else { + ROS_INFO("Service call failed!"); + } + } + + void roscontroller::SetMode(){ + mavros_msgs::SetMode set_mode_message; + set_mode_message.request.base_mode = 0; + set_mode_message.request.custom_mode = "GUIDED"; // shit! + if(mode_client.call(set_mode_message)) { + ROS_INFO("Service called!"); + } else { + ROS_INFO("Service call failed!"); + } + } + + /*Prepare messages for each step and publish*/ /*******************************************************************************************************/ /* Message format of payload (Each slot is uint64_t) */ @@ -551,7 +576,7 @@ namespace rosbzz_node{ else if (msg->mode == "LAND") buzzuav_closures::flight_status_update(4); else // ground standby = LOITER? - buzzuav_closures::flight_status_update(5);//? + buzzuav_closures::flight_status_update(1);//? } /*flight extended status update*/ @@ -667,6 +692,9 @@ namespace rosbzz_node{ case mavros_msgs::CommandCode::NAV_TAKEOFF: ROS_INFO("RC_call: TAKE OFF!!!!"); rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; + /* arming */ + SetMode(); + Arm(); buzzuav_closures::rc_call(rc_cmd); res.success = true; break; diff --git a/src/test1.bzz b/src/test1.bzz index 5298fe8..76d6609 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -106,25 +106,23 @@ if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/ } function takeoff() { - if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { - barrier_set(ROBOTS,hexagon) - barrier_ready() - } - else if( flight.status !=3){ + #if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { + # barrier_set(ROBOTS,hexagon) + # barrier_ready() + #} + if( flight.status !=3){ + log("Altitude: ", TARGET_ALTITUDE) neighbors.broadcast("cmd", 22) uav_takeoff(TARGET_ALTITUDE) } } function land() { - log("Land: ", flight.status) - if( flight.status == 1) { - barrier_set(ROBOTS,idle) - barrier_ready() - } - else if(flight.status!=0 and flight.status!=4){ - neighbors.broadcast("cmd", 21) - uav_land() - } + #log("Land: ", flight.status) + #if(flight.status != 0 and flight.status != 4){ + # neighbors.broadcast("cmd", 21) + # uav_land() + #} + uav_land() } # Executed once at init time. @@ -135,19 +133,22 @@ function init() { # Executed at each time step. function step() { - if(flight.rc_cmd==22 and flight.status==1) { + if(flight.rc_cmd==22) { + log("cmd 22") flight.rc_cmd=0 - statef=takeoff + statef = takeoff neighbors.broadcast("cmd", 22) - } else if(flight.rc_cmd==21 and flight.status==2) { + } else if(flight.rc_cmd==21) { + log("cmd 21") + log("To land") flight.rc_cmd=0 - statef=land + statef = land neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16 and flight.status==2) { flight.rc_cmd=0 uav_goto() - } -statef() + } + statef() } # Executed once when the robot (or the simulator) is reset. From a6de3cecd1e6f3a642fa99f83bfd158192abda1f Mon Sep 17 00:00:00 2001 From: isvogor Date: Fri, 17 Feb 2017 17:29:42 -0500 Subject: [PATCH 2/2] take off and land works --- src/buzzuav_closures.cpp | 4 ++-- src/test1.bzz | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a24488e..334359c 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -157,7 +157,7 @@ int buzzuav_moveto(buzzvm_t vm) { } }*/ cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; - printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]); + printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); buzz_cmd=2; return buzzvm_ret0(vm); } @@ -215,7 +215,7 @@ int buzzuav_takeoff(buzzvm_t vm) { buzzvm_lnum_assert(vm, 1); buzzvm_lload(vm, 1); /* Altitude */ buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); - goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value; + goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value; height = goto_pos[2]; cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); diff --git a/src/test1.bzz b/src/test1.bzz index 76d6609..b494a15 100644 --- a/src/test1.bzz +++ b/src/test1.bzz @@ -144,7 +144,7 @@ function step() { flight.rc_cmd=0 statef = land neighbors.broadcast("cmd", 21) - } else if(flight.rc_cmd==16 and flight.status==2) { + } else if(flight.rc_cmd==16) { flight.rc_cmd=0 uav_goto() }