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

reinstate gohome in sim and webcontrol

This commit is contained in:
dave 2018-06-01 21:20:57 -04:00
parent 42d990676c
commit 845742767b
7 changed files with 48 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -46,12 +46,18 @@ function launch() {
}
gohomeT=100
function stop() {
BVMSTATE = "STOP"
function goinghome() {
BVMSTATE = "GOHOME"
if(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
}
neighbors.broadcast("cmd", 20)
} else
BVMSTATE = AUTO_LAUNCH_STATE
}
function stop() {
BVMSTATE = "STOP"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21)
uav_land()
@ -62,12 +68,12 @@ function stop() {
} else {
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
}
}
function take_picture() {
BVMSTATE="PICTURE"
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
setgimbal(0.0, 0.0, -90.0, 20.0)
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
takepicture()
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
@ -180,18 +186,25 @@ function rc_cmd_listen() {
} else if(flight.rc_cmd==21) {
log("cmd 21")
flight.rc_cmd=0
BVMSTATE = "STOP"
AUTO_LAUNCH_STATE = "STOP"
BVMSTATE = "GOHOME"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) {
log("cmd 20")
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME"
neighbors.broadcast("cmd", 20)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
BVMSTATE = "PATHPLAN"
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
disarm()
neighbors.broadcast("cmd", 401)
} else if (flight.rc_cmd==666){
flight.rc_cmd=0
@ -230,12 +243,16 @@ function nei_cmd_listen() {
if(BVMSTATE!="BARRIERWAIT") {
if(value==22) {
BVMSTATE = "LAUNCH"
}else if(value==20) {
AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME"
} else if(value==21) {
BVMSTATE = "STOP"
AUTO_LAUNCH_STATE = "STOP"
BVMSTATE = "GOHOME"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
uav_arm()
arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
uav_disarm()
disarm()
} else if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)

2
buzz_scripts/include/plan/rrtstar.bzz
View File

@ -20,7 +20,7 @@ mapgoal = {}
function navigate() {
BVMSTATE="NAVIGATE"
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
}

2
buzz_scripts/main.bzz
View File

@ -53,6 +53,8 @@ function step() {
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="AGGREGATE")

13
include/buzzuav_closures.h
View File

@ -13,19 +13,6 @@
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,
COMMAND_PICTURE,
COMMAND_GIMBAL,
} Custom_CommandCode;
/*
* prextern int() function in Buzz
* This function is used to print data from buzz

2
launch/rosbuzz.launch
View File

@ -10,7 +10,7 @@
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="debug" value="true" />
<param name="debug" value="false" />
<param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" />
<param name="in_payload" value="inMavlink"/>

16
src/buzzuav_closures.cpp
View File

@ -230,7 +230,7 @@ int buzzuav_moveto(buzzvm_t vm)
// DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
buzz_cmd = mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT;
return buzzvm_ret0(vm);
}
@ -331,7 +331,7 @@ int buzzuav_takepicture(buzzvm_t vm)
/ Buzz closure to take a picture here.
/----------------------------------------*/
{
buzz_cmd = COMMAND_PICTURE;
buzz_cmd = mavros_msgs::CommandCode::IMAGE_START_CAPTURE;
return buzzvm_ret0(vm);
}
@ -355,7 +355,7 @@ int buzzuav_setgimbal(buzzvm_t vm)
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL;
buzz_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
return buzzvm_ret0(vm);
}
@ -402,7 +402,7 @@ int buzzuav_arm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -413,7 +413,7 @@ int buzzuav_disarm(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
buzz_cmd = COMMAND_DISARM;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -429,7 +429,7 @@ 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 = COMMAND_TAKEOFF;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -440,7 +440,7 @@ int buzzuav_land(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n");
buzz_cmd = COMMAND_LAND;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}
@ -451,7 +451,7 @@ int buzzuav_gohome(buzzvm_t vm)
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
buzz_cmd = COMMAND_GOHOME;
buzz_cmd = cur_cmd;
return buzzvm_ret0(vm);
}

40
src/roscontroller.cpp
View File

@ -622,7 +622,7 @@ script
float* gimbal;
switch (buzzuav_closures::bzz_cmd())
{
case buzzuav_closures::COMMAND_TAKEOFF:
case mavros_msgs::CommandCode::NAV_TAKEOFF:
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param7 = goto_pos[2];
cmd_srv.request.command = buzzuav_closures::getcmd();
@ -645,7 +645,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_LAND:
case mavros_msgs::CommandCode::NAV_LAND:
cmd_srv.request.command = buzzuav_closures::getcmd();
if (current_mode != "LAND")
{
@ -664,10 +664,7 @@ script
armstate = 0;
break;
case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
cmd_srv.request.param5 = home.latitude;
cmd_srv.request.param6 = home.longitude;
cmd_srv.request.param7 = home.altitude;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: // TODO: NOT FULLY TESTED HITL/FIELD !!!
cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv))
{
@ -677,28 +674,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_GOTO: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param5 = goto_pos[0];
cmd_srv.request.param6 = goto_pos[1];
cmd_srv.request.param7 = 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");
cmd_srv.request.command = mavros_msgs::CommandCode::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");
break;
case buzzuav_closures::COMMAND_ARM:
case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
if (!armstate)
{
SetMode("LOITER", 0);
@ -707,7 +683,7 @@ script
}
break;
case buzzuav_closures::COMMAND_DISARM:
case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM+1:
if (armstate)
{
armstate = 0;
@ -716,12 +692,12 @@ script
}
break;
case buzzuav_closures::COMMAND_MOVETO:
case mavros_msgs::CommandCode::NAV_SPLINE_WAYPOINT:
goto_pos = buzzuav_closures::getgoto();
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
break;
case buzzuav_closures::COMMAND_GIMBAL:
case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
gimbal = buzzuav_closures::getgimbal();
cmd_srv.request.param1 = gimbal[0];
cmd_srv.request.param2 = gimbal[1];
@ -736,7 +712,7 @@ script
ROS_ERROR("Failed to call service from flight controller");
break;
case buzzuav_closures::COMMAND_PICTURE:
case mavros_msgs::CommandCode::IMAGE_START_CAPTURE:
ROS_INFO("TAKING A PICTURE HERE!! --------------");
mavros_msgs::CommandBool capture_command;
if (capture_srv.call(capture_command))