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

debug new matrix form for RRT and add blob check for intersection

This commit is contained in:
dave 2017-12-19 18:36:10 -05:00
parent c03d6e269b
commit 94ad933452
5 changed files with 167 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
buzz_scripts/include/mapmatrix.bzz
View File

@ -5,8 +5,8 @@ robot_marker = "X"
# copy a full matrix row # copy a full matrix row
function mat_copyrow(out,ro,in,ri){ function mat_copyrow(out,ro,in,ri){
out[ro] = {} out[ro] = {}
var icr = 0 var icr = 1
while(icr < size(in[ri])) { while(icr <= size(in[ri])) {
out[ro][icr]=in[ri][icr] out[ro][icr]=in[ri][icr]
icr = icr + 1 icr = icr + 1
} }
@ -18,11 +18,11 @@ function getvec(t,row){
function init_test_map(len){ function init_test_map(len){
map = {} map = {}
var indexR = 0 var indexR = 1
while(indexR < len) { while(indexR <= len) {
map[indexR] = {} map[indexR] = {}
var indexC = 0 var indexC = 1
while(indexC < len) { while(indexC <= len) {
map[indexR][indexC] = 1.0 map[indexR][indexC] = 1.0
indexC = indexC + 1 indexC = indexC + 1
} }
@ -33,22 +33,22 @@ function init_test_map(len){
map[6][5] = 0.0 map[6][5] = 0.0
map[4][5] = 0.0 map[4][5] = 0.0
log("Occupancy grid initialized (",len,"x",len,") with obstacles.") log("Occupancy grid initialized (",size(map),"x",size(map[1]),") with obstacles.")
} }
function init_map(len){ function init_map(len){
map = {} map = {}
var indexR = 0 var indexR = 1
while(indexR < len) { while(indexR <= len) {
map[indexR] = {} map[indexR] = {}
var indexC = 0 var indexC = 1
while(indexC < len) { while(indexC <= len) {
map[indexR][indexC] = 1.0 map[indexR][indexC] = 1.0
indexC = indexC + 1 indexC = indexC + 1
} }
indexR = indexR + 1 indexR = indexR + 1
} }
log("Occupancy grid initialized (",len,"x",len,").") log("Occupancy grid initialized (",size(map),"x",size(map[1]),").")
} }
function add_obstacle(pos, off, inc_trust) { function add_obstacle(pos, off, inc_trust) {
@ -57,7 +57,7 @@ function add_obstacle(pos, off, inc_trust) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) { if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
#log("Add obstacle in cell: ", xi, " ", yi) #log("Add obstacle in cell: ", xi, " ", yi)
old=map[xi][yi] var old=map[xi][yi]
if(old-inc_trust > 0.0) if(old-inc_trust > 0.0)
map[xi][yi] = old-inc_trust map[xi][yi] = old-inc_trust
else else
@ -71,7 +71,7 @@ function remove_obstacle(pos, off, dec_trust) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){ if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0){
#log("Remove obstacle in cell: ", xi, " ", yi) #log("Remove obstacle in cell: ", xi, " ", yi)
old=map[xi][yi] var old=map[xi][yi]
if(old + dec_trust < 1.0) #x,y if(old + dec_trust < 1.0) #x,y
map[xi][yi] = old+dec_trust map[xi][yi] = old+dec_trust
else else
@ -79,6 +79,30 @@ function remove_obstacle(pos, off, dec_trust) {
} }
} }
function get_occupied_cells(cur_cell){
var i = 1
var occupied_cells = {}
occupied_cells[0] = cur_cell
occupied_cells[1] = math.vec2.new(cur_cell.x + 1, cur_cell.y)
occupied_cells[2] = math.vec2.new(cur_cell.x - 1, cur_cell.y)
occupied_cells[3] = math.vec2.new(cur_cell.x, cur_cell.y + 1)
occupied_cells[4] = math.vec2.new(cur_cell.x, cur_cell.y - 1)
return occupied_cells
}
function is_in(dictionary, x, y){
var i = 0
while(i < size(dictionary)){
if(dictionary[i].x == x and dictionary[i].y == y){
return 1
}
i = i + 1
}
return 0
}
function table_print(t) { function table_print(t) {
foreach(t, function(key, value) { foreach(t, function(key, value) {
log(key, " -> ", value) log(key, " -> ", value)

158
buzz_scripts/include/rrtstar.bzz
View File

@ -14,12 +14,12 @@ function UAVinit_map(m_navigation) {
# create a map big enough for the goal # create a map big enough for the goal
init_map(2*math.round(math.vec2.length(m_navigation))+10) init_map(2*math.round(math.vec2.length(m_navigation))+10)
# center the robot on the grid # center the robot on the grid
cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0)) cur_cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
} }
function UAVpathPlanner(m_navigation, m_pos) { function UAVpathPlanner(m_navigation, m_pos) {
# place the robot on the grid # place the robot on the grid
update_curcell(m_pos,0) cur_cell = getcell(m_pos,0)
# create the goal in the map grid # create the goal in the map grid
mapgoal = math.vec2.add(m_navigation,cur_cell) mapgoal = math.vec2.add(m_navigation,cur_cell)
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
@ -34,7 +34,7 @@ function Kh4pathPlanner(m_goal, m_pos) {
m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y)) m_goal = math.vec2.new(math.round(m_goal.x*CM2KH4.x), math.round(m_goal.y*CM2KH4.y))
# place the robot on the grid # place the robot on the grid
update_curcell(m_pos,1) cur_cell = getcell(m_pos,1)
log("Starting from cell: ", cur_cell.x, " ", cur_cell.y) log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
log("Going to cell: ", m_goal.x, " ", m_goal.y) log("Going to cell: ", m_goal.x, " ", m_goal.y)
@ -44,26 +44,35 @@ function Kh4pathPlanner(m_goal, m_pos) {
return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y)) return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
} }
function update_curcell(m_curpos, kh4) { function getcell(m_curpos, kh4) {
var cell = {}
if(kh4) { # for khepera playground if(kh4) { # for khepera playground
cur_cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin)) cell = math.vec2.sub(m_curpos, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
cur_cell = math.vec2.new(math.round(cur_cell.x*CM2KH4.x), math.round(cur_cell.y*CM2KH4.y)) cell = math.vec2.new(math.round(cell.x*CM2KH4.x), math.round(cell.y*CM2KH4.y))
} else { # for uav map relative to home } else { # for uav map relative to home
cur_cell = math.vec2.add(cur_cell, m_curpos) cell = math.vec2.new(math.round(size(map)/2.0),math.round(size(map[1])/2.0))
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y)) cell = math.vec2.add(cell, m_curpos)
cell = math.vec2.new(math.round(cell.x), math.round(cell.y))
} }
if(cur_cell.x > size(map)) if(cell.x > size(map))
cur_cell.x = size(map) cell.x = size(map)
if(cur_cell.y > size(map[1])) if(cell.y > size(map[1]))
cur_cell.y = size(map[1]) cell.y = size(map[1])
if(cur_cell.x < 1) if(cell.x < 1)
cur_cell.x = 1 cell.x = 1
if(cur_cell.y < 1) if(cell.y < 1)
cur_cell.y = 1 cell.y = 1
return cell
}
function updateMap(m_pos) {
#getproxobs(m_pos)
UAVgetneiobs (m_pos)
} }
function UAVgetneiobs (m_curpos) { function UAVgetneiobs (m_curpos) {
update_curcell(m_curpos,0) cur_cell = getcell(m_curpos,0)
# 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) {
add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0) add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
@ -72,7 +81,7 @@ function UAVgetneiobs (m_curpos) {
function getneiobs (m_curpos) { function getneiobs (m_curpos) {
var foundobstacle = 0 var foundobstacle = 0
update_curcell(m_curpos,1) cur_cell = getcell(m_curpos,1)
var old_nei = table_copy(nei_cell) var old_nei = table_copy(nei_cell)
#log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3]) #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
@ -118,7 +127,7 @@ function getneiobs (m_curpos) {
function getproxobs (m_curpos) { function getproxobs (m_curpos) {
foundobstacle = 0 foundobstacle = 0
update_curcell(m_curpos,1) cur_cell = getcell(m_curpos,1)
reduce(proximity, reduce(proximity,
function(key, value, acc) { function(key, value, acc) {
@ -159,19 +168,21 @@ function getproxobs (m_curpos) {
} }
function check_path(m_path, goal_l, m_curpos, kh4) { function check_path(m_path, goal_l, m_curpos, kh4) {
pathisblocked = 0 var pathisblocked = 0
nb=goal_l var nb = goal_l
update_curcell(m_curpos,kh4) cur_cell = getcell(m_curpos,kh4)
Cvec = cur_cell var Cvec = cur_cell
while(nb < size(m_path) and nb <= goal_l+1) { while(nb <= size(m_path) and nb <= goal_l+1) {
Nvec = getvec(m_path, nb) var Nvec = getvec(m_path, nb)
if(kh4==0) if(kh4 == 0) {
Nvec=vec_from_gps(Nvec.x,Nvec.y) Nvec = vec_from_gps(Nvec.x,Nvec.y,1)
if(doesItIntersect(Cvec, Nvec)){ Nvec = getcell(Nvec,kh4)
log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")") }
if(doesItIntersect(Cvec, Nvec)) {
log("Path blocked ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
pathisblocked = 1 pathisblocked = 1
} }
Cvec=Nvec Cvec = Nvec
nb = nb + 1 nb = nb + 1
} }
@ -184,12 +195,16 @@ function RRTSTAR(GOAL,TOL) {
RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive RADIUS = 1.25 #TOL.x #size(map[1])/10.0 # to consider 2 points consecutive
goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y} 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) #table_print(goalBoundary)
numberOfPoints = 1 var numberOfPoints = 1
arrayOfPoints = {} var arrayOfPoints = {}
arrayOfPoints[1] = {.1=cur_cell.x, .2=cur_cell.y}
Path = {} Path = {}
mat_copyrow(Path,1,arrayOfPoints,1)
# RRT* tree matrix: x; y; predecessor line in Q; nb of pts; min. distance
Q = {} Q = {}
Q[1] = {.1=cur_cell.x,.2=cur_cell.y,.3=1,.4=1,.5=0}
goalReached = 0; goalReached = 0;
timeout = 0 timeout = 0
@ -202,7 +217,7 @@ function RRTSTAR(GOAL,TOL) {
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))
#log("Point generated: ", pt.x, " ", pt.y) #log("Point generated: ", pt.x, " ", pt.y)
pointList = findPointsInRadius(pt,Q,RADIUS); var pointList = findPointsInRadius(pt,Q,RADIUS);
# Find connection that provides the least cost to come # Find connection that provides the least cost to come
nbCount = 0; nbCount = 0;
@ -210,7 +225,7 @@ function RRTSTAR(GOAL,TOL) {
minCounter = 0; minCounter = 0;
if(size(pointList) != 0) { if(size(pointList) != 0) {
#log("Found ", size(pointList), " close point:", pointList.mat) log("Found ", size(pointList), " close to point: ", pointList[size(pointList)][3])
ipt=1 ipt=1
while(ipt <= size(pointList)) { while(ipt <= size(pointList)) {
pointNumber = {} pointNumber = {}
@ -224,7 +239,7 @@ function RRTSTAR(GOAL,TOL) {
nbCount = nbCount + 1; nbCount = nbCount + 1;
if(intersects != 1) { if(intersects != 1) {
#log(pointNumber, "do not intersect (",pointNumber.mat[3],")") #log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5] var distance = math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))+Q[pointNumber[1][3]][5]
if(distance < minCounted) { if(distance < minCounted) {
minCounted = distance; minCounted = distance;
@ -235,9 +250,11 @@ function RRTSTAR(GOAL,TOL) {
} }
if(minCounter > 0) { if(minCounter > 0) {
numberOfPoints = numberOfPoints + 1; numberOfPoints = numberOfPoints + 1;
arrayOfPoints[numberOfPoints] = {}
arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y arrayOfPoints[numberOfPoints][2]=pt.y
Q[numberOfPoints] = {}
Q[numberOfPoints][1] = pt.x Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointList[minCounter][4] Q[numberOfPoints][3] = pointList[minCounter][4]
@ -249,9 +266,10 @@ function RRTSTAR(GOAL,TOL) {
# Now check to see if any of the other points can be redirected # Now check to see if any of the other points can be redirected
nbCount = 0; nbCount = 0;
ipt = 1 ipt = 1
while(ipt < size(pointList)) { while(ipt <= size(pointList)) {
pointNumber = {} pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt) mat_copyrow(pointNumber,1,pointList,ipt)
#log("pN ", size(pointNumber), ", ipt: ", ipt, "prec: ", pointNumber[1][4])
# Follow the line to see if it intersects anything # Follow the line to see if it intersects anything
intersects = doesItIntersect(pt,getvec(pointNumber,1)); intersects = doesItIntersect(pt,getvec(pointNumber,1));
@ -262,6 +280,7 @@ function RRTSTAR(GOAL,TOL) {
if(intersects != 1) { if(intersects != 1) {
# If the alternative path is shorter than change it # If the alternative path is shorter than change it
tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt)) tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
#log("Q: ", size(Q), "x", size(Q[1]))
if(tmpdistance < Q[pointNumber[1][4]][5]) { if(tmpdistance < Q[pointNumber[1][4]][5]) {
Q[pointNumber[1][4]][3] = numberOfPoints Q[pointNumber[1][4]][3] = numberOfPoints
Q[pointNumber[1][4]][5] = tmpdistance Q[pointNumber[1][4]][5] = tmpdistance
@ -285,9 +304,11 @@ function RRTSTAR(GOAL,TOL) {
# If there is no intersection we need to add to the tree # If there is no intersection we need to add to the tree
if(intersects != 1) { if(intersects != 1) {
numberOfPoints = numberOfPoints + 1; numberOfPoints = numberOfPoints + 1;
arrayOfPoints[numberOfPoints] = {}
arrayOfPoints[numberOfPoints][1]=pt.x arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y arrayOfPoints[numberOfPoints][2]=pt.y
Q[numberOfPoints] = {}
Q[numberOfPoints][1] = pt.x Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointNum Q[numberOfPoints][3] = pointNum
@ -320,51 +341,51 @@ function RRTSTAR(GOAL,TOL) {
function findClosestPoint(point,aPt) { function findClosestPoint(point,aPt) {
# Go through each points and find the distances between them and the # Go through each points and find the distances between them and the
# target point # target point
distance = 999 var distance = 999
pointNumber = -1 var pointNb = -1
ifcp=1 var ifcp=1
while(ifcp <= size(aPt)) { while(ifcp <= size(aPt)) {
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) var range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
if(range < distance) { if(range < distance) {
distance = range; distance = range;
pointNumber = ifcp; pointNb = ifcp;
} }
ifcp = ifcp + 1 ifcp = ifcp + 1
} }
return pointNumber return pointNb
} }
# Find the closest point in the tree # Find the closest point in the tree
function findPointsInRadius(point,q,r) { function findPointsInRadius(point,q,r) {
pointList = {} var ptList = {}
var counted = 0; var counted = 0;
var iir = 1 var iir = 1
while(iir <= size(q)) { while(iir <= size(q)) {
tmpvv = getvec(q,iir) var tmpvv = getvec(q,iir)
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y) #log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point)) var distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
if(distance < r) { if(distance < r) {
counted = counted+1; counted = counted+1;
mat_copyrow(pointList,counted,q,iir) mat_copyrow(ptList,counted,q,iir)
} }
iir = iir + 1 iir = iir + 1
} }
return pointList return ptList
} }
# check if the line between 2 point intersect an obstacle # check if the line between 2 point intersect an obstacle
function doesItIntersect(point,vector) { function doesItIntersect(point,vector) {
#log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y) #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
dif = math.vec2.sub(point,vector) var dif = math.vec2.sub(point,vector)
distance = math.vec2.length(dif) var distance = math.vec2.length(dif)
if(distance==0.0){ if(distance == 0.0){
# Find what block we're in right now # Find what block we're in right now
var xi = math.round(point.x) #+1 var xi = math.round(point.x) #+1
var yi = math.round(point.y) #+1 var yi = math.round(point.y) #+1
if(xi!=cur_cell.x and yi!=cur_cell.y){ if(xi >= size(map) and yi >= size(map[1]) and xi != cur_cell.x and yi != cur_cell.y) {
if(map[xi][yi] > 0.5) if(map[xi][yi] > 0.5)
return 1 return 1
else else
@ -372,21 +393,22 @@ function doesItIntersect(point,vector) {
} else } else
return 0 return 0
} }
vec = math.vec2.scale(dif,1.0/distance) var vec = math.vec2.scale(dif,1.0/distance)
pathstep = size(map[1]) - 2 var pathstep = size(map) - 2
idii = 1.0 idii = 1.0
while(idii <= pathstep) { while(idii <= pathstep) {
range = distance*idii/pathstep var range = distance*idii/pathstep
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range)); var pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
# Find what block we're in right now # Find what block we're in right now
var xi = math.round(pos_chk.x) #+1 var xi = math.round(pos_chk.x) #+1
var yi = math.round(pos_chk.y) #+1 var yi = math.round(pos_chk.y) #+1
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range, "(Map size: ", size(map), ")")
if(xi!=cur_cell.x and yi!=cur_cell.y){ if(is_in(get_occupied_cells(cur_cell), xi, yi) == 0){
if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) { #if(xi!=cur_cell.x and yi!=cur_cell.y) {
if(xi <= size(map) and yi <= size(map[1]) and xi > 0 and yi > 0) {
if(map[xi][yi] < 0.5) { # because obstacle use trust values if(map[xi][yi] < 0.5) { # because obstacle use trust values
#log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!") #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
return 1; return 1;
@ -403,20 +425,28 @@ function doesItIntersect(point,vector) {
} }
function getPathGPS(Q,nb){ function getPathGPS(Q,nb){
#log("-----------CONVERTING PATH!!!")
path={} path={}
var npt=0 var npt=0
var isrecursive = 0
# get the path from the point list # get the path from the point list
while(nb!=1){ while(nb > 1 and isrecursive!=1){
npt=npt+1 npt=npt+1
path[npt] = {} path[npt] = {}
path[npt][1]=Q[nb][1] path[npt][1]=Q[nb][1]
path[npt][2]=Q[nb][2] path[npt][2]=Q[nb][2]
nb = Q[nb][3]; if(nb != Q[Q[nb][3]][3])
nb = Q[nb][3];
else {
isrecursive = 1
path={}
log("ERROR - Recursive path !!!")
}
} }
# re-order the list and make the path points absolute # re-order the list and make the path points absolute
pathR={} pathR={}
var totpt = npt var totpt = npt + 1
while(npt > 0){ while(npt > 0){
tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell)) tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell))
pathR[totpt-npt] = {} pathR[totpt-npt] = {}
@ -433,7 +463,7 @@ function getPath(Q,nb){
var npt=0 var npt=0
# log("get the path from the point list") # log("get the path from the point list")
while(nb!=1){ while(nb > 1){
npt=npt+1 npt=npt+1
path[npt] = {} path[npt] = {}
path[npt][1]=Q[nb][1] path[npt][1]=Q[nb][1]
@ -444,7 +474,7 @@ function getPath(Q,nb){
# log("re-order the list") # log("re-order the list")
# table_print(path.mat) # table_print(path.mat)
pathR={} pathR={}
var totpt = npt var totpt = npt + 1
while(npt > 0){ while(npt > 0){
tmpgoal = getvec(path,npt) tmpgoal = getvec(path,npt)
pathR[totpt-npt] = {} pathR[totpt-npt] = {}

52
buzz_scripts/include/uavstates.bzz
View File

@ -3,6 +3,7 @@
# FLIGHT-RELATED FUNCTIONS # FLIGHT-RELATED FUNCTIONS
# #
######################################## ########################################
include "vec2.bzz"
include "rrtstar.bzz" include "rrtstar.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
@ -14,7 +15,6 @@ GOTODIST_TOL = 0.5 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.1 # rad.
cur_goal_l = 0 cur_goal_l = 0
rc_State = 0 rc_State = 0
homegps = {.lat=0, .long=0}
function uav_initswarm() { function uav_initswarm() {
s = swarm.create(1) s = swarm.create(1)
@ -34,6 +34,7 @@ function idle() {
function takeoff() { function takeoff() {
UAVSTATE = "TAKEOFF" UAVSTATE = "TAKEOFF"
statef=takeoff statef=takeoff
homegps = {.lat=position.latitude, .long=position.longitude}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1) barrier_set(ROBOTS, action, land, -1)
@ -61,7 +62,7 @@ function land() {
function set_goto(transf) { function set_goto(transf) {
UAVSTATE = "GOTOGPS" UAVSTATE = "GOTOGPS"
statef=function() { statef=function() {
gotoWP(transf) gotoWPRRT(transf)
} }
if(rc_goto.id==id){ if(rc_goto.id==id){
@ -93,10 +94,8 @@ function picture() {
# still requires to be tuned, replaning takes too much time.. # still requires to be tuned, replaning takes too much time..
# DS 23/11/2017 # DS 23/11/2017
function gotoWPRRT(transf) { function gotoWPRRT(transf) {
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
homegps.lat = position.latitude m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
homegps.long = position.longitude
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-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)
@ -104,35 +103,36 @@ function gotoWPRRT(transf) {
else { else {
if(Path==nil){ if(Path==nil){
# create the map # create the map
if(map==nil) if(map==nil) {
UAVinit_map(rc_goal) UAVinit_map(rc_goal)
homegps.lat = position.latitude
homegps.long = position.longitude
}
# add proximity sensor and neighbor obstacles to the map # add proximity sensor and neighbor obstacles to the map
while(Path==nil) { while(Path==nil) {
#getproxobs(m_pos) updateMap(m_pos)
UAVgetneiobs (m_pos)
Path = UAVpathPlanner(rc_goal, m_pos) Path = UAVpathPlanner(rc_goal, m_pos)
} }
cur_goal_l = 1 cur_goal_l = 1
} else if(cur_goal_l <= size(Path)) { } else if(cur_goal_l <= size(Path)) {
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude var cur_goal_gps = getvec(Path, cur_goal_l) #x=latitude, y=longitude
cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) var cur_goal_pt = vec_from_gps(cur_goal_gps.x, cur_goal_gps.y, 0)
print(" heading to ", cur_pt.x,cur_pt.y) print(" heading to [", cur_goal_l, "/", size(Path), "]", cur_goal_pt.x, cur_goal_pt.y)
if(math.vec2.length(cur_pt)>GOTODIST_TOL) { if(math.vec2.length(cur_goal_pt) > GOTODIST_TOL) {
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long),-1) updateMap(m_pos)
UAVgetneiobs(m_pos) if(check_path(Path, cur_goal_l, m_pos, 0)) {
if(check_path(Path,cur_goal_l,m_pos,0)) {
uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude) uav_moveto(0.0, 0.0, rc_goto.altitude-position.altitude)
Path = nil Path = nil
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude) rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
while(Path == nil) { while(Path == nil) {
#getproxobs(m_pos) updateMap(m_pos)
UAVgetneiobs (m_pos)
Path = UAVpathPlanner(rc_goal, m_pos) Path = UAVpathPlanner(rc_goal, m_pos)
} }
cur_goal_l = 1 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)
} }
cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
} }
else else
cur_goal_l = cur_goal_l + 1 cur_goal_l = cur_goal_l + 1
@ -262,14 +262,18 @@ function LimitAngle(angle){
return angle return angle
} }
function vec_from_gps(lat, lon) { function vec_from_gps(lat, lon, home_ref) {
d_lon = lon - position.longitude d_lon = lon - position.longitude
d_lat = lat - position.latitude d_lat = lat - position.latitude
if(home_ref == 1) {
d_lon = lon - homegps.long
d_lat = lat - homegps.lat
}
ned_x = d_lat/180*math.pi * 6371000.0; ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); #Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.new(ned_x,ned_y) return math.vec2.new(ned_x,ned_y)
} }
function gps_from_vec(vec) { function gps_from_vec(vec) {

12
buzz_scripts/testalone.bzz → buzz_scripts/testRRT.bzz
View File

@ -1,4 +1,3 @@
include "vec2.bzz"
include "update.bzz" include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=11 with this header. include "barrier.bzz" # don't use a stigmergy id=11 with this header.
include "uavstates.bzz" # require an 'action' function to be defined here. include "uavstates.bzz" # require an 'action' function to be defined here.
@ -6,15 +5,18 @@ include "vstigenv.bzz"
function action() { function action() {
statef=action statef=action
uav_storegoal(45.5085,-73.1531935979886,5.0) uav_storegoal(45.5088103899,-73.1540826153,25.0)
set_goto(picture) set_goto(idle)
} }
# Executed once at init time. # Executed once at init time.
function init() { function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig() uav_initstig()
uav_initswarm()
#statef=turnedoff
#UAVSTATE = "TURNEDOFF"
statef = takeoff
UAVSTATE = "TAKEOFF"
} }
# Executed at each time step. # Executed at each time step.

2
buzz_scripts/testaloneWP.bzz
View File

@ -2,7 +2,6 @@ include "vec2.bzz"
include "update.bzz" include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header. include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header.
include "uavstates.bzz" # require an 'action' function to be defined here. include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
function action() { function action() {
uav_storegoal(-1.0,-1.0,-1.0) uav_storegoal(-1.0,-1.0,-1.0)
@ -13,7 +12,6 @@ function action() {
function init() { function init() {
statef=turnedoff statef=turnedoff
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"
uav_initstig()
TARGET_ALTITUDE = 15.0 TARGET_ALTITUDE = 15.0
} }