made rrtstar a state

This commit is contained in:
dave 2017-12-20 14:33:23 -05:00 committed by vivek-shankar
parent 50368d84d3
commit 63f9111d2f
5 changed files with 136 additions and 145 deletions

View File

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

View File

@ -6,10 +6,13 @@
include "mapmatrix.bzz" include "mapmatrix.bzz"
RRT_TIMEOUT = 500 RRT_TIMEOUT = 500
RRT_RUNSTEP = 3
PROX_SENSOR_THRESHOLD = 0.11
GSCALE = {.x=1, .y=1} GSCALE = {.x=1, .y=1}
map = nil map = nil
cur_cell = {} cur_cell = {}
nei_cell = {} nei_cell = {}
mapgoal = {}
# create a map with its length to fit the goal (rel. pos.) and the current position as the center # 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) { function dyn_init_map(m_navigation) {
@ -29,8 +32,25 @@ function pathPlanner(m_goal, m_pos, kh4) {
#print_map(map) #print_map(map)
export_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 # 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) { function getcell(m_curpos) {
@ -53,12 +73,13 @@ function getcell(m_curpos) {
return cell return cell
} }
function updateMap(m_pos) { function populateGrid(m_pos) {
#getproxobs(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) cur_cell = getcell(m_curpos)
# add all neighbors as obstacles in the grid # add all neighbors as obstacles in the grid
neighbors.foreach(function(rid, data) { neighbors.foreach(function(rid, data) {
@ -66,54 +87,47 @@ function UAVgetneiobs (m_curpos) {
}) })
} }
function getneiobs (m_curpos) { # old function tested with the khepera for reference
var foundobstacle = 0 # DS 20/11
cur_cell = getcell(m_curpos) # function getneiobs (m_curpos) {
var old_nei = table_copy(nei_cell) # var foundobstacle = 0
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) # cur_cell = getcell(m_curpos)
# var old_nei = table_copy(nei_cell)
neighbors.foreach(function(rid, data) { # 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)
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))
#log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm") # Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) # nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y)) # if(old_nei[rid]!=nil) {
nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y} # if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
#log("Neighbor in : ", Ncell.x, " ", Ncell.y) # remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
if(old_nei[rid]!=nil) { # remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
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-1, old_nei[rid].y), 0, 1)
#log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y) # 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), 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+1, old_nei[rid].y), 0, 1) # add_obstacle(Ncell, 0, 1.0)
remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1) # add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1) # add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1) # add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
add_obstacle(Ncell, 0, 1.0) # add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) # foundobstacle = 1
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) # } else {
add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0) # add_obstacle(Ncell, 0, 1.0)
foundobstacle = 1 # 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)
} else { # add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
add_obstacle(Ncell, 0, 1.0) # add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0) # foundobstacle = 1
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) # return foundobstacle
foundobstacle = 1 # }
}
})
#if(foundobstacle) {
#print_map(map)
#}
return foundobstacle
}
# populate a line in front of the sensor using plateform independant proximity sensing
function getproxobs (m_curpos) { function getproxobs (m_curpos) {
foundobstacle = 0 var foundobstacle = 0
cur_cell = getcell(m_curpos) cur_cell = getcell(m_curpos)
reduce(proximity, 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) 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) 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) 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) { if(map[math.round(obs.x)][math.round(obs.y)]!=0) {
add_obstacle(obs, 0, 0.25) add_obstacle(obs, 0, 0.25)
add_obstacle(obs2, 0, 0.25) add_obstacle(obs2, 0, 0.25)
@ -142,15 +156,6 @@ function getproxobs (m_curpos) {
return acc return acc
}, math.vec2.new(0, 0)) }, 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 return foundobstacle
} }
@ -176,29 +181,16 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
return pathisblocked return pathisblocked
} }
function RRTSTAR(GOAL,TOL) { function rrtstar() {
HEIGHT = size(map) # update state machine variables
WIDTH = size(map[1]) statef = rrtstar
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive 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 # 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 # 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)) 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."); log(numberOfPoints, " points processed. Still looking for goal.");
} }
timeout = timeout + 1 timeout = timeout + 1
step_timeout = step_timeout + 1
} }
if(goalReached){ if(goalReached){
log("Goal found(",numberOfPoints,")!") log("Goal found(",numberOfPoints,")!")
Path = getPath(Q,numberOfPoints, 1) Path = getPath(Q,numberOfPoints, 1)
# done. resume the state machine
BVMSTATE = "GOTOGPS"
statef = old_statef
} else { } else {
log("FAILED TO FIND A PATH!!!") #log("FAILED TO FIND A PATH!!!")
Path = nil Path = nil
} }
return Path
} }
function findClosestPoint(point,aPt) { function findClosestPoint(point,aPt) {

View File

@ -7,7 +7,7 @@ include "vec2.bzz"
include "rrtstar.bzz" include "rrtstar.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
UAVSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2 # m/steps GOTO_MAXVEL = 2 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
@ -23,16 +23,16 @@ function uav_initswarm() {
function turnedoff() { function turnedoff() {
statef=turnedoff statef=turnedoff
UAVSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
} }
function idle() { function idle() {
statef=idle statef=idle
UAVSTATE = "IDLE" BVMSTATE = "IDLE"
} }
function takeoff() { function takeoff() {
UAVSTATE = "TAKEOFF" BVMSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
homegps = {.lat=position.latitude, .long=position.longitude} homegps = {.lat=position.latitude, .long=position.longitude}
@ -47,7 +47,7 @@ function takeoff() {
} }
function land() { function land() {
UAVSTATE = "LAND" BVMSTATE = "LAND"
statef=land statef=land
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
@ -60,7 +60,7 @@ function land() {
} }
function set_goto(transf) { function set_goto(transf) {
UAVSTATE = "GOTOGPS" BVMSTATE = "GOTOGPS"
statef=function() { statef=function() {
gotoWPRRT(transf) gotoWPRRT(transf)
} }
@ -79,7 +79,7 @@ function set_goto(transf) {
ptime=0 ptime=0
function picture() { function picture() {
statef=picture statef=picture
UAVSTATE="PICTURE" BVMSTATE="PICTURE"
uav_setgimbal(0.0, 0.0, -90.0, 20.0) uav_setgimbal(0.0, 0.0, -90.0, 20.0)
if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize if(ptime==PICTURE_WAIT/2) { # wait for the drone to stabilize
uav_takepicture() uav_takepicture()
@ -98,50 +98,45 @@ function gotoWPRRT(transf) {
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1) 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) 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.") log("Sorry this is too far.")
else { return
if(Path==nil){ }
# create the map # create the map
if(map==nil) { if(map==nil) {
dyn_init_map(rc_goal) dyn_init_map(rc_goal)
homegps.lat = position.latitude homegps.lat = position.latitude
homegps.long = position.longitude 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 } else
while(Path==nil) { cur_goal_l = cur_goal_l + 1
updateMap(m_pos) } else {
Path = pathPlanner(rc_goal, m_pos) Path = nil
} transf()
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()
}
} }
} }
@ -162,7 +157,7 @@ function gotoWP(transf) {
function follow() { function follow() {
if(size(targets)>0) { if(size(targets)>0) {
UAVSTATE = "FOLLOW" BVMSTATE = "FOLLOW"
statef=follow statef=follow
attractor=math.vec2.newp(0,0) attractor=math.vec2.newp(0,0)
foreach(targets, function(id, tab) { foreach(targets, function(id, tab) {
@ -181,18 +176,18 @@ function uav_rccmd() {
log("cmd 22") log("cmd 22")
flight.rc_cmd=0 flight.rc_cmd=0
statef = takeoff statef = takeoff
UAVSTATE = "TAKEOFF" BVMSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) { } else if(flight.rc_cmd==21) {
log("cmd 21") log("cmd 21")
log("To land") log("To land")
flight.rc_cmd=0 flight.rc_cmd=0
statef = land statef = land
UAVSTATE = "LAND" BVMSTATE = "LAND"
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
UAVSTATE = "GOTOGPS" BVMSTATE = "GOTOGPS"
statef = goto statef = goto
} else if(flight.rc_cmd==400) { } else if(flight.rc_cmd==400) {
flight.rc_cmd=0 flight.rc_cmd=0
@ -227,16 +222,16 @@ function uav_rccmd() {
function uav_neicmd() { function uav_neicmd() {
neighbors.listen("cmd", neighbors.listen("cmd",
function(vid, value, rid) { function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")") print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") { if(value==22 and BVMSTATE!="TAKEOFF" and BVMSTATE!="BARRIERWAIT") {
statef=takeoff statef=takeoff
UAVSTATE = "TAKEOFF" BVMSTATE = "TAKEOFF"
} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") { } else if(value==21 and BVMSTATE!="LAND" and BVMSTATE!="BARRIERWAIT") {
statef=land statef=land
UAVSTATE = "LAND" BVMSTATE = "LAND"
} else if(value==400 and UAVSTATE=="TURNEDOFF") { } else if(value==400 and BVMSTATE=="TURNEDOFF") {
uav_arm() uav_arm()
} else if(value==401 and UAVSTATE=="TURNEDOFF"){ } else if(value==401 and BVMSTATE=="TURNEDOFF"){
uav_disarm() uav_disarm()
} else if(value==900){ } else if(value==900){
rc_State = 0 rc_State = 0
@ -246,7 +241,7 @@ function uav_neicmd() {
rc_State = 2 rc_State = 2
} else if(value==903){ } else if(value==903){
rc_State = 3 rc_State = 3
} else if(value==16 and UAVSTATE=="IDLE"){ } else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) { # neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid) # print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto # # if(gt.id == id) statef=goto

View File

@ -16,7 +16,7 @@ function init() {
#statef=turnedoff #statef=turnedoff
#UAVSTATE = "TURNEDOFF" #UAVSTATE = "TURNEDOFF"
statef = takeoff statef = takeoff
UAVSTATE = "TAKEOFF" BVMSTATE = "TAKEOFF"
} }
# Executed at each time step. # Executed at each time step.
@ -25,7 +25,7 @@ function step() {
statef() statef()
log("Current state: ", UAVSTATE) log("Current state: ", BVMSTATE)
} }
# Executed once when the robot (or the simulator) is reset. # Executed once when the robot (or the simulator) is reset.

View File

@ -480,7 +480,7 @@ string getuavstate()
---------------------------------------*/ ---------------------------------------*/
{ {
static buzzvm_t VM = buzz_utility::get_vm(); 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); buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1); buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM); buzzvm_pop(VM);