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_ready()
} else {
log("Altitude: ", TARGET_ALTITUDE)
log("Altitude: ", position.altitude)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
@ -60,15 +60,15 @@ function set_goto(transf) {
gotoWP(transf);
}
#if(rc_goto.id==id){
# if(s!=nil){
# if(s.in())
# s.leave()
#}
#} else {
# neighbors.broadcast("cmd", 16)
# neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
#}
if(rc_goto.id==id){
if(s!=nil){
if(s.in())
s.leave()
}
} else {
neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
}
}
ptime=0
@ -86,14 +86,14 @@ function picture() {
}
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)
print(" has to move ", goal.range)
print(" has to move ", goal.range, goal.bearing)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
log("Sorry this is too far.")
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))
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

2
buzz_scripts/testaloneWP.bzz
View File

@ -23,7 +23,7 @@ function step() {
statef()
# log("Current state: ", UAVSTATE)
log("Current state: ", UAVSTATE)
}
# 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.
/----------------------------------------*/
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;
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);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------");
cmd_srv.request.param1 = 90;
cmd_srv.request.param2 = 0;
cmd_srv.request.param3 = 0;
cmd_srv.request.param4 = 0;
cmd_srv.request.param1 = 0.0;
cmd_srv.request.param2 = 0.0;
cmd_srv.request.param3 = -90.0;
cmd_srv.request.param4 = 0.0;
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
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");
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);
// ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0],
// home[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id,
// local_pos[0], local_pos[1]);
// ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos_new[0], local_pos_new[1]);
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD
* (then converted to NED)*/