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

made rrtstar a state

This commit is contained in:
dave 2017-12-20 14:33:23 -05:00
parent 661f7897c3
commit 2751230d6f
5 changed files with 136 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/include/barrier.bzz
View File

@ -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()
}

156
buzz_scripts/include/rrtstar.bzz
View File

@ -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) {

117
buzz_scripts/include/uavstates.bzz
View File

@ -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

4
buzz_scripts/testRRT.bzz
View File

@ -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.

2
src/buzzuav_closures.cpp
View File

@ -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);