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

fix barrier.bzz merge error

This commit is contained in:
dave 2017-09-06 19:18:20 -04:00
parent b1514752b9 796bb78dd8
commit 7284565504
5 changed files with 40 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/graphform.bzz
View File

@ -797,7 +797,7 @@ function init() {
v_tag = stigmergy.create(m_lockstig)
uav_initstig()
# go to diff. height since no collision avoidance implemented yet
TARGET_ALTITUDE = 2.5 + id * 1.5
TARGET_ALTITUDE = 7.5 #2.5 + id * 1.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}

2
buzz_scripts/include/barrier.bzz
View File

@ -30,7 +30,7 @@ function barrier_create() {
function barrier_set(threshold, transf, resumef, bdt) {
statef = function() {
barrier_wait(threshold, transf, resumef);
barrier_wait(threshold, transf, resumef, bdt);
}
UAVSTATE = "BARRIERWAIT"
barrier_create()

27
buzz_scripts/include/uavstates.bzz
View File

@ -27,27 +27,20 @@ function idle() {
UAVSTATE = "IDLE"
}
firstpassT = 1
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
if(firstpassT) {
barrier_create(TAKEOFF_VSTIG)
firstpassT = 0
}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1)
barrier_ready()
} else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
firstpassL = 1
function land() {
UAVSTATE = "LAND"
statef=land
@ -62,7 +55,7 @@ function land() {
}
function set_goto(transf) {
UAVSTATE = "GOTO"
UAVSTATE = "GOTOGPS"
statef=function() {
gotoWP(transf);
}
@ -93,7 +86,7 @@ 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)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
@ -141,7 +134,7 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
UAVSTATE = "GOTO"
UAVSTATE = "GOTOGPS"
statef = goto
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
@ -190,7 +183,9 @@ function LimitAngle(angle){
}
function rb_from_gps(lat, lon) {
# print("gps2rb: ",lat,lon,position.latitude,position.longitude)
# print("gps2rb d: ",lat,lon)
# print("gps2rb h: ",position.latitude,position.longitude)
d_lon = lon - position.longitude
d_lat = lat - position.latitude
ned_x = d_lat/180*math.pi * 6371000.0;
@ -213,4 +208,4 @@ function gps_from_vec(vec) {
#goal.latitude = d_lat + position.latitude;
#d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
#goal.longitude = d_lon + position.longitude;
}
}

35
src/buzzuav_closures.cpp
View File

@ -35,6 +35,7 @@ namespace buzzuav_closures{
string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::RB_struct> wplist_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
@ -158,13 +159,13 @@ namespace buzzuav_closures{
RB_arr.longitude=lon;
RB_arr.altitude=alt;
// Insert elements.
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid);
if(it!=targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(tid, RB_arr));
map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid);
if(it!=wplist_map.end())
wplist_map.erase(it);
wplist_map.insert(make_pair(tid, RB_arr));
}
ROS_INFO("----->Saved %i waypoints.", targets_map.size());
ROS_INFO("----->Saved %i waypoints.", wplist_map.size());
// Close the file:
fin.close();
@ -213,23 +214,23 @@ namespace buzzuav_closures{
rb_from_gps(tmp, rb, cur_pos);
//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
/* When we get here, the "targets" table is on top of the stack */
// When we get here, the "targets" table is on top of the stack
//ROS_INFO("Buzz_utility will save user %i.", it->first);
/* Push user id */
// Push user id
buzzvm_pushi(vm, it->first);
/* Create entry table */
// Create entry table
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */
// Insert range
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
/* Insert longitude */
// Insert longitude
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
/* Save entry into data table */
// Save entry into data table
buzzvm_push(vm, entry);
buzzvm_tput(vm);
}
@ -341,12 +342,12 @@ namespace buzzuav_closures{
goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
if(targets_map.size()<=0)
if(wplist_map.size()<=0)
parse_gpslist();
goal[0] = targets_map.begin()->second.latitude;
goal[1] = targets_map.begin()->second.longitude;
goal[2] = targets_map.begin()->second.altitude;
targets_map.erase(targets_map.begin()->first);
goal[0] = wplist_map.begin()->second.latitude;
goal[1] = wplist_map.begin()->second.longitude;
goal[2] = wplist_map.begin()->second.altitude;
wplist_map.erase(wplist_map.begin()->first);
}
double rb[3];
@ -355,7 +356,7 @@ namespace buzzuav_closures{
if(fabs(rb[0])<150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
return buzzvm_ret0(vm);
}

9
src/roscontroller.cpp
View File

@ -761,6 +761,15 @@ 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.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);
}