From 39cfdee6de7107ce8771e94f345092723d432c4a Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Fri, 31 Mar 2017 23:46:44 -0400 Subject: [PATCH] local enum for internal command passing and proxy table to fit argos --- include/buzzuav_closures.h | 11 ++++++ src/buzzuav_closures.cpp | 72 +++++++++++++++++++++++++++++++++----- src/roscontroller.cpp | 10 +++--- 3 files changed, 80 insertions(+), 13 deletions(-) diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 38229c4..dcfa26a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -10,6 +10,17 @@ //#include "roscontroller.h" 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, + } Custom_CommandCode; + /* * prextern int() function in Buzz * This function is used to print data from buzz diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index f8fb491..55343cc 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -108,7 +108,7 @@ namespace buzzuav_closures{ gps_from_rb(d, b, goto_pos); cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/ printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f \n",goto_pos[0], goto_pos[1], goto_pos[2]); - buzz_cmd=5; + buzz_cmd= COMMAND_MOVETO; // TO DO what should we use return buzzvm_ret0(vm); } @@ -120,7 +120,7 @@ namespace buzzuav_closures{ set_goto(rc_goto_pos); 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]); - buzz_cmd=2; + buzz_cmd=COMMAND_GOTO; return buzzvm_ret0(vm); } @@ -130,13 +130,13 @@ namespace buzzuav_closures{ int buzzuav_arm(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; printf(" Buzz requested Arm \n"); - buzz_cmd=3; + buzz_cmd=COMMAND_ARM; return buzzvm_ret0(vm); } int buzzuav_disarm(buzzvm_t vm) { cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; printf(" Buzz requested Disarm \n"); - buzz_cmd=4; + buzz_cmd=COMMAND_DISARM; return buzzvm_ret0(vm); } @@ -151,21 +151,21 @@ namespace buzzuav_closures{ height = goto_pos[2]; cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; printf(" Buzz requested Take off !!! \n"); - buzz_cmd = 1; + buzz_cmd = COMMAND_TAKEOFF; 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 = COMMAND_LAND; 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 = COMMAND_GOHOME; return buzzvm_ret0(vm); } @@ -306,9 +306,65 @@ namespace buzzuav_closures{ /******************************************************/ /*Create an obstacle Buzz table from proximity sensors*/ + /* Acessing proximity in buzz script + proximity[0].angle and proximity[0].value - front + "" "" "" - right and back + proximity[3].angle and proximity[3].value - left + proximity[4].angle = -1 and proximity[4].value -bottom */ /******************************************************/ int buzzuav_update_prox(buzzvm_t vm) { + + buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); + buzzvm_pusht(vm); + buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); + buzzvm_gstore(vm); + + /* Fill into the proximity table */ + buzzobj_t tProxRead; + float angle =0; + for(size_t i = 0; i < 4; ++i) { + /* Create table for i-th read */ + buzzvm_pusht(vm); + tProxRead = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + /* Fill in the read */ + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[i+1]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + /* Store read table in the proximity table */ + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, i); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + angle+=1.5708; + } + /* Create table for bottom read */ + angle =-1; + buzzvm_pusht(vm); + tProxRead = buzzvm_stack_at(vm, 1); + buzzvm_pop(vm); + /* Fill in the read */ + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); + buzzvm_pushf(vm, obst[0]); + buzzvm_tput(vm); + buzzvm_push(vm, tProxRead); + buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); + buzzvm_pushf(vm, angle); + buzzvm_tput(vm); + /*Store read table in the proximity table*/ + buzzvm_push(vm, tProxTable); + buzzvm_pushi(vm, 4); + buzzvm_push(vm, tProxRead); + buzzvm_tput(vm); + + /* buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pusht(vm); buzzvm_dup(vm); @@ -331,7 +387,7 @@ namespace buzzuav_closures{ buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1)); buzzvm_pushf(vm, obst[4]); buzzvm_tput(vm); - buzzvm_gstore(vm); + buzzvm_gstore(vm);*/ return vm->state; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 4af5487..6ab0be0 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -353,13 +353,13 @@ namespace rosbzz_node{ /*FC call for takeoff,land, gohome and arm/disarm*/ int tmp = buzzuav_closures::bzz_cmd(); double* goto_pos = buzzuav_closures::getgoto(); - if (tmp == 1){ + if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME){ cmd_srv.request.param7 = goto_pos[2]; //cmd_srv.request.z = 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"); - } else if (tmp == 2) { /*FC call for goto*/ + } else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ /*prepare the goto publish message */ cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param6 = goto_pos[1]; @@ -370,13 +370,13 @@ namespace rosbzz_node{ cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; 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"); - } else if (tmp == 3) { /*FC call for arm*/ + } else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/ armstate=1; Arm(); - } else if (tmp == 4){ + } else if (tmp == buzzuav_closures::COMMAND_DISARM){ armstate=0; Arm(); - } else if (tmp == 5) { /*Buzz call for moveto*/ + } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ /*prepare the goto publish message */ mavros_msgs::PositionTarget pt; pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | mavros_msgs::PositionTarget::IGNORE_VY | mavros_msgs::PositionTarget::IGNORE_VZ | mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | mavros_msgs::PositionTarget::IGNORE_AFZ | mavros_msgs::PositionTarget::IGNORE_YAW_RATE;