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

local enum for internal command passing and proxy table to fit argos

This commit is contained in:
vivek-shankar 2017-03-31 23:46:44 -04:00
parent 583b20c113
commit 39cfdee6de
3 changed files with 80 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
include/buzzuav_closures.h
View File

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

72
src/buzzuav_closures.cpp
View File

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

10
src/roscontroller.cpp
View File

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