diff --git a/buzz_scripts/include/barrier.bzz b/buzz_scripts/include/barrier.bzz index e280a66..31a452e 100644 --- a/buzz_scripts/include/barrier.bzz +++ b/buzz_scripts/include/barrier.bzz @@ -32,7 +32,7 @@ function barrier_set(threshold, transf, resumef, bdt) { statef = function() { barrier_wait(threshold, transf, resumef, bdt); } - UAVSTATE = "BARRIERWAIT" + BVMSTATE = "BARRIERWAIT" barrier_create() } diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 2c2a6d5..86d1ce0 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -6,10 +6,13 @@ include "mapmatrix.bzz" RRT_TIMEOUT = 500 +RRT_RUNSTEP = 3 +PROX_SENSOR_THRESHOLD = 0.11 GSCALE = {.x=1, .y=1} map = nil cur_cell = {} nei_cell = {} +mapgoal = {} # create a map with its length to fit the goal (rel. pos.) and the current position as the center function dyn_init_map(m_navigation) { @@ -29,8 +32,25 @@ function pathPlanner(m_goal, m_pos, kh4) { #print_map(map) export_map(map) + # Initialize RRTStar var + log("--------Initialize RRTStar--------------") + HEIGHT = size(map) + WIDTH = size(map[1]) + RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive + goalBoundary = {.xmin=mapgoal.x-HEIGHT/20.0, .xmax=mapgoal.x+HEIGHT/20.0, .ymin=mapgoal.y-WIDTH/20.0, .ymax=mapgoal.y+WIDTH/20.0} + #table_print(goalBoundary) + numberOfPoints = 1 + arrayOfPoints = {} + arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} + # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance + Q = {} + Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} + goalReached = 0 + timeout = 0 + # search for a path - return RRTSTAR(mapgoal,math.vec2.new(5 * GSCALE.x, 5 * GSCALE.y)) #size(map[1])/20.0,size(map[1])/20.0)) + old_statef = statef + rrtstar() } function getcell(m_curpos) { @@ -53,12 +73,13 @@ function getcell(m_curpos) { return cell } -function updateMap(m_pos) { +function populateGrid(m_pos) { #getproxobs(m_pos) - UAVgetneiobs (m_pos) + getneiobs (m_pos) } -function UAVgetneiobs (m_curpos) { +# TODO: populate the map with a blob instead? +function getneiobs (m_curpos) { cur_cell = getcell(m_curpos) # add all neighbors as obstacles in the grid neighbors.foreach(function(rid, data) { @@ -66,54 +87,47 @@ function UAVgetneiobs (m_curpos) { }) } -function getneiobs (m_curpos) { - var foundobstacle = 0 - cur_cell = getcell(m_curpos) - var old_nei = table_copy(nei_cell) - #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) +# old function tested with the khepera for reference +# DS 20/11 +# function getneiobs (m_curpos) { +# var foundobstacle = 0 +# cur_cell = getcell(m_curpos) +# var old_nei = table_copy(nei_cell) - neighbors.foreach(function(rid, data) { - #log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg") - Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) - #log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm") - Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) - Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) - nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} - #log("Neighbor in : ", Ncell.x, " ", Ncell.y) - if(old_nei[rid]!=nil) { - if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { - #log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) - remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) - add_obstacle(Ncell, 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) - foundobstacle = 1 - } - } else { - add_obstacle(Ncell, 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) - add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) - foundobstacle = 1 - } - }) - - #if(foundobstacle) { - #print_map(map) - #} - - return foundobstacle -} +# neighbors.foreach(function(rid, data) { +# Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos) +# Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) +# Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) +# nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} +# if(old_nei[rid]!=nil) { +# if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) { +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) +# remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# } else { +# add_obstacle(Ncell, 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0) +# add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) +# foundobstacle = 1 +# } +# }) +# return foundobstacle +# } +# populate a line in front of the sensor using plateform independant proximity sensing function getproxobs (m_curpos) { - foundobstacle = 0 + var foundobstacle = 0 cur_cell = getcell(m_curpos) reduce(proximity, @@ -125,7 +139,7 @@ function getproxobs (m_curpos) { obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2) obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs) obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2) - if(value.value > IR_SENSOR_THRESHOLD) { + if(value.value > PROX_SENSOR_THRESHOLD) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) { add_obstacle(obs, 0, 0.25) add_obstacle(obs2, 0, 0.25) @@ -142,15 +156,6 @@ function getproxobs (m_curpos) { return acc }, math.vec2.new(0, 0)) - #if(foundobstacle) { - # reduce(proximity, - # function(key, value, acc){ - # log(value.value, ", ", value.angle) - # return acc - # }, math.vec2.new(0, 0)) - # print_map(map) - #} - return foundobstacle } @@ -176,29 +181,16 @@ function check_path(m_path, goal_l, m_curpos, kh4) { return pathisblocked } -function RRTSTAR(GOAL,TOL) { - HEIGHT = size(map) - WIDTH = size(map[1]) - RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive +function rrtstar() { + # update state machine variables + statef = rrtstar + BVMSTATE = "RRTSTAR" - - var goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} - #table_print(goalBoundary) - var numberOfPoints = 1 - var arrayOfPoints = {} - arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y} - Path = {} - mat_copyrow(Path,1,arrayOfPoints,1) - # RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance - Q = {} - Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0} - - goalReached = 0; - timeout = 0 ## # main search loop ## - while(goalReached == 0 and timeout < RRT_TIMEOUT) { + step_timeout = 0 + while(goalReached == 0 and timeout < RRT_TIMEOUT and step_timeout < RRT_RUNSTEP) { # Point generation only as grid cell centers pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1)) @@ -313,15 +305,19 @@ function RRTSTAR(GOAL,TOL) { log(numberOfPoints, " points processed. Still looking for goal."); } timeout = timeout + 1 + step_timeout = step_timeout + 1 } + if(goalReached){ log("Goal found(",numberOfPoints,")!") Path = getPath(Q,numberOfPoints, 1) + # done. resume the state machine + BVMSTATE = "GOTOGPS" + statef = old_statef } else { - log("FAILED TO FIND A PATH!!!") + #log("FAILED TO FIND A PATH!!!") Path = nil } - return Path } function findClosestPoint(point,aPt) { diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index d3b015d..10f1cfd 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -7,7 +7,7 @@ include "vec2.bzz" include "rrtstar.bzz" TARGET_ALTITUDE = 15.0 # m. -UAVSTATE = "TURNEDOFF" +BVMSTATE = "TURNEDOFF" PICTURE_WAIT = 20 # steps GOTO_MAXVEL = 2 # m/steps GOTO_MAXDIST = 150 # m. @@ -23,16 +23,16 @@ function uav_initswarm() { function turnedoff() { statef=turnedoff - UAVSTATE = "TURNEDOFF" + BVMSTATE = "TURNEDOFF" } function idle() { statef=idle - UAVSTATE = "IDLE" + BVMSTATE = "IDLE" } function takeoff() { - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" statef=takeoff homegps = {.lat=position.latitude, .long=position.longitude} @@ -47,7 +47,7 @@ function takeoff() { } function land() { - UAVSTATE = "LAND" + BVMSTATE = "LAND" statef=land neighbors.broadcast("cmd", 21) @@ -60,7 +60,7 @@ function land() { } function set_goto(transf) { - UAVSTATE = "GOTOGPS" + BVMSTATE = "GOTOGPS" statef=function() { gotoWPRRT(transf) } @@ -79,7 +79,7 @@ function set_goto(transf) { ptime=0 function picture() { statef=picture - UAVSTATE="PICTURE" + BVMSTATE="PICTURE" uav_setgimbal(0.0, 0.0, -90.0, 20.0) if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize uav_takepicture() @@ -98,50 +98,45 @@ function gotoWPRRT(transf) { m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) print(" has to move ", math.vec2.length(rc_goal), "from ", m_pos.x, " ", m_pos.y) - if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) + if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5) { log("Sorry this is too far.") - else { - if(Path==nil){ - # create the map - if(map==nil) { - dyn_init_map(rc_goal) - homegps.lat = position.latitude - homegps.long = position.longitude + return + } + # create the map + if(map==nil) { + dyn_init_map(rc_goal) + homegps.lat = position.latitude + homegps.long = position.longitude + } + + if(Path==nil) { + # add proximity sensor and neighbor obstacles to the map + populateGrid(m_pos) + # start the planner + pathPlanner(rc_goal, m_pos) + cur_goal_l = 1 + } else if(cur_goal_l <= size(Path)) { + var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude + var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) + print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) + if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { + populateGrid(m_pos) + if(check_path(Path, cur_goal_l, m_pos, 0)) { + uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) + Path = nil + rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) + # re-start the planner + pathPlanner(rc_goal, m_pos) + cur_goal_l = 1 + } else { + cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) + uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) } - # add proximity sensor and neighbor obstacles to the map - while(Path==nil) { - updateMap(m_pos) - Path = pathPlanner(rc_goal, m_pos) - } - print_pos(Path) - cur_goal_l = 1 - } else if(cur_goal_l <= size(Path)) { - var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude - var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0) - print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y) - if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) { - updateMap(m_pos) - if(check_path(Path, cur_goal_l, m_pos, 0)) { - uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) - Path = nil - rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0) - while(Path == nil) { - updateMap(m_pos) - Path = pathPlanner(rc_goal, m_pos) - } - print_pos(Path) - cur_goal_l = 1 - } else { - cur_goal_pt = math.vec2.scale(cur_goal_pt, GOTO_MAXVEL/math.vec2.length(cur_goal_pt)) - uav_moveto(cur_goal_pt.x, cur_goal_pt.y, rc_goto.altitude-position.altitude) - } - } - else - cur_goal_l = cur_goal_l + 1 - } else { - Path = nil - transf() - } + } else + cur_goal_l = cur_goal_l + 1 + } else { + Path = nil + transf() } } @@ -162,7 +157,7 @@ function gotoWP(transf) { function follow() { if(size(targets)>0) { - UAVSTATE = "FOLLOW" + BVMSTATE = "FOLLOW" statef=follow attractor=math.vec2.newp(0,0) foreach(targets, function(id, tab) { @@ -181,18 +176,18 @@ function uav_rccmd() { log("cmd 22") flight.rc_cmd=0 statef = takeoff - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" neighbors.broadcast("cmd", 22) } else if(flight.rc_cmd==21) { log("cmd 21") log("To land") flight.rc_cmd=0 statef = land - UAVSTATE = "LAND" + BVMSTATE = "LAND" neighbors.broadcast("cmd", 21) } else if(flight.rc_cmd==16) { flight.rc_cmd=0 - UAVSTATE = "GOTOGPS" + BVMSTATE = "GOTOGPS" statef = goto } else if(flight.rc_cmd==400) { flight.rc_cmd=0 @@ -227,16 +222,16 @@ function uav_rccmd() { function uav_neicmd() { neighbors.listen("cmd", function(vid, value, rid) { - print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") - if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { + print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") + if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") { statef=takeoff - UAVSTATE = "TAKEOFF" - } else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { + BVMSTATE = "TAKEOFF" + } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") { statef=land - UAVSTATE = "LAND" - } else if(value==400 and UAVSTATE=="TURNEDOFF") { + BVMSTATE = "LAND" + } else if(value==400 and BVMSTATE=="TURNEDOFF") { uav_arm() - } else if(value==401 and UAVSTATE=="TURNEDOFF"){ + } else if(value==401 and BVMSTATE=="TURNEDOFF"){ uav_disarm() } else if(value==900){ rc_State = 0 @@ -246,7 +241,7 @@ function uav_neicmd() { rc_State = 2 } else if(value==903){ rc_State = 3 - } else if(value==16 and UAVSTATE=="IDLE"){ + } else if(value==16 and BVMSTATE=="IDLE"){ # neighbors.listen("gt",function(vid, value, rid) { # print("Got (", vid, ",", value, ") from robot #", rid) # # if(gt.id == id) statef=goto diff --git a/buzz_scripts/testRRT.bzz b/buzz_scripts/testRRT.bzz index 7590470..7478529 100644 --- a/buzz_scripts/testRRT.bzz +++ b/buzz_scripts/testRRT.bzz @@ -16,7 +16,7 @@ function init() { #statef=turnedoff #UAVSTATE = "TURNEDOFF" statef = takeoff - UAVSTATE = "TAKEOFF" + BVMSTATE = "TAKEOFF" } # Executed at each time step. @@ -25,7 +25,7 @@ function step() { statef() - log("Current state: ", UAVSTATE) + log("Current state: ", BVMSTATE) } # Executed once when the robot (or the simulator) is reset. diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index a58c8f2..c61d9a3 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -480,7 +480,7 @@ string getuavstate() ---------------------------------------*/ { static buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); buzzvm_pop(VM);