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

add camera service check and fix ROSBuzz gimbal control

This commit is contained in:
David St-Onge 2017-09-06 21:53:00 -04:00
parent 7b041dcf1b
commit dc01484105
4 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
buzz_scripts/include/uavstates.bzz
View File

@ -35,7 +35,7 @@ function takeoff() {
barrier_set(ROBOTS, action, land, -1) barrier_set(ROBOTS, action, land, -1)
barrier_ready() barrier_ready()
} else { } else {
log("Altitude: ", TARGET_ALTITUDE) log("Altitude: ", position.altitude)
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
} }
@ -60,15 +60,15 @@ function set_goto(transf) {
gotoWP(transf); gotoWP(transf);
} }
#if(rc_goto.id==id){ if(rc_goto.id==id){
# if(s!=nil){ if(s!=nil){
# if(s.in()) if(s.in())
# s.leave() s.leave()
#} }
#} else { } else {
# neighbors.broadcast("cmd", 16) neighbors.broadcast("cmd", 16)
# neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude) neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
#} }
} }
ptime=0 ptime=0
@ -86,14 +86,14 @@ function picture() {
} }
function gotoWP(transf) { function gotoWP(transf) {
print(rc_goto.id, " is going alone to lat=", rc_goto.latitude) # print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
rb_from_gps(rc_goto.latitude, rc_goto.longitude) rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(" has to move ", goal.range) print(" has to move ", goal.range, goal.bearing)
m_navigation = math.vec2.newp(goal.range, goal.bearing) m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.") log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation)) m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude) uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination } else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination

2
buzz_scripts/testaloneWP.bzz
View File

@ -23,7 +23,7 @@ function step() {
statef() statef()
# log("Current state: ", UAVSTATE) log("Current state: ", UAVSTATE)
} }
# Executed once when the robot (or the simulator) is reset. # Executed once when the robot (or the simulator) is reset.

2
src/buzzuav_closures.cpp
View File

@ -321,7 +321,7 @@ namespace buzzuav_closures{
/ Buzz closure to take a picture here. / Buzz closure to take a picture here.
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm) { int buzzuav_takepicture(buzzvm_t vm) {
cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST; //cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE; buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }

16
src/roscontroller.cpp
View File

@ -761,17 +761,20 @@ void roscontroller::flight_controller_service_call()
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0); roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/ } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------"); ROS_INFO("TAKING A PICTURE HERE!! --------------");
cmd_srv.request.param1 = 90; cmd_srv.request.param1 = 0.0;
cmd_srv.request.param2 = 0; cmd_srv.request.param2 = 0.0;
cmd_srv.request.param3 = 0; cmd_srv.request.param3 = -90.0;
cmd_srv.request.param4 = 0; cmd_srv.request.param4 = 0.0;
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
mavros_msgs::CommandBool capture_command; mavros_msgs::CommandBool capture_command;
capture_srv.call(capture_command); if (capture_srv.call(capture_command)) {
ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
} else
ROS_ERROR("Failed to call service from camera streamer");
} }
} }
/*---------------------------------------------- /*----------------------------------------------
@ -993,8 +996,7 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw) {
// gps_ned_home(ned_x, ned_y); // gps_ned_home(ned_x, ned_y);
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], // ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0],
// home[1]); // home[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, // ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos_new[0], local_pos_new[1]);
// local_pos[0], local_pos[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD
* (then converted to NED)*/ * (then converted to NED)*/