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

Merge branch 'master' into dev

This commit is contained in:
dave 2017-12-19 13:20:33 -05:00
parent 2cb4c2c29e 177baf5d15
commit c03d6e269b
11 changed files with 190 additions and 171 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
buzz_scripts/include/mapmatrix.bzz
View File

@ -2,64 +2,51 @@
# Write to matrix # Write to matrix
robot_marker = "X" robot_marker = "X"
function wmat(mat, row, col, val) {
var index = (row-1)*mat.nb_col + (col - 1)
if( row <= mat.nb_row) { # update val
mat.mat[index] = val
} else if(row == mat.nb_row + 1){ # add entry
mat.nb_row = mat.nb_row + 1
mat.mat[index] = val
}
}
# Read from matrix
function rmat(mat, row, col) {
#log("rmat ", mat, row, col)
var index = (row-1)*mat.nb_col + (col - 1)
if (mat.mat[index] == nil) {
log("Wrong matrix read index: ", row, " ", col)
return -1
} else {
return mat.mat[index]
}
}
# copy a full matrix row # copy a full matrix row
function mat_copyrow(out,ro,in,ri){ function mat_copyrow(out,ro,in,ri){
var indexI = (ri-1)*in.nb_col out[ro] = {}
var indexO = (ro-1)*out.nb_col var icr = 0
icr=0 while(icr < size(in[ri])) {
while(icr<in.nb_col){ out[ro][icr]=in[ri][icr]
out.mat[indexO+icr]=in.mat[indexI+icr]
icr = icr + 1 icr = icr + 1
} }
} }
function getvec(t,row){ function getvec(t,row){
return math.vec2.new(rmat(t,row,1),rmat(t,row,2)) return math.vec2.new(t[row][1],t[row][2])
} }
function init_test_map(len){ function init_test_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}} map = {}
var index = 0 var indexR = 0
while(index<len*len){ while(indexR < len) {
map.mat[index]=1.0 map[indexR] = {}
index = index + 1 var indexC = 0
while(indexC < len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
indexR = indexR + 1
} }
# puts an obstacle right in the middle # DEBUG\TEST: puts an obstacle right in the middle
wmat(map,5,5,0.0) map[5][5] = 0.0
wmat(map,6,5,0.0) map[6][5] = 0.0
wmat(map,4,5,0.0) map[4][5] = 0.0
log("Occupancy grid initialized (",len,"x",len,") with obstacles.") log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
} }
function init_map(len){ function init_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}} map = {}
var index = 0 var indexR = 0
while(index<len*len){ while(indexR < len) {
map.mat[index]=1.0 map[indexR] = {}
index = index + 1 var indexC = 0
while(indexC < len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
indexR = indexR + 1
} }
log("Occupancy grid initialized (",len,"x",len,").") log("Occupancy grid initialized (",len,"x",len,").")
} }
@ -68,13 +55,13 @@ function add_obstacle(pos, off, inc_trust) {
var xi = math.round(pos.x) var xi = math.round(pos.x)
var yi = math.round(pos.y) var yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+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=rmat(map,xi,yi) old=map[xi][yi]
if(old-inc_trust > 0.0) if(old-inc_trust > 0.0)
wmat(map,xi,yi,old-inc_trust) map[xi][yi] = old-inc_trust
else else
wmat(map,xi,yi,0.0) map[xi][yi] = 0.0
} }
} }
@ -82,13 +69,13 @@ function remove_obstacle(pos, off, dec_trust) {
var xi = math.round(pos.x) var xi = math.round(pos.x)
var yi = math.round(pos.y) var yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+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=rmat(map, xi, yi) old=map[xi][yi]
if(old + dec_trust < 1.0) #x,y if(old + dec_trust < 1.0) #x,y
wmat(map, xi, yi, old+dec_trust) map[xi][yi] = old+dec_trust
else else
wmat(map, xi, yi, 1.0) map[xi][yi] = 1.0
} }
} }
@ -108,44 +95,27 @@ function table_copy(t) {
function print_pos(t) { function print_pos(t) {
var ir=1 var ir=1
while(ir<=t.nb_row){ while(ir <= size(t)) {
log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2)) log(ir, ": ", t[ir][1], " ", t[ir][2])
ir = ir + 1 ir = ir + 1
} }
} }
function print_map(t) { function print_map(t) {
var ir=t.nb_row var ir=size(t)
log("Printing a ", t.nb_row, " by ", t.nb_col, " map") log("Printing a ", size(t), " by ", size(t[1]), " map")
while(ir>0){ while(ir > 0) {
logst=string.concat("\t", string.tostring(ir), "\t:") logst=string.concat("\t", string.tostring(ir), "\t:")
ic=t.nb_col ic = size(t[ir])
while(ic>0){ while(ic > 0) {
if(ir==cur_cell.x and ic==cur_cell.y) if(ir==cur_cell.x and ic==cur_cell.y)
logst = string.concat(logst, " XXXXXXXX") logst = string.concat(logst, " XXXXXXXX")
else else
logst = string.concat(logst, " ", string.tostring(rmat(t,ir,ic))) logst = string.concat(logst, " ", string.tostring(t[ir][ic]))
ic = ic - 1 ic = ic - 1
} }
log(logst) log(logst)
ir = ir - 1 ir = ir - 1
} }
} export_map(map)
function print_map_argos(t){
var ir=t.nb_row
msg = string.tostring(ir)
while(ir>0){
ic=t.nb_col
while(ic>0){
if(ir==cur_cell.x and ic==cur_cell.y){
msg = string.concat(msg, ":", robot_marker)
}
else {
msg = string.concat(msg, ":", string.tostring(rmat(t,ir,ic)))
}
ic = ic - 1
}
ir = ir - 1
}
#set_argos_map(msg)
} }

161
buzz_scripts/include/rrtstar.bzz
View File

@ -14,7 +14,7 @@ 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(map.nb_col/2.0),math.round(map.nb_row/2.0)) cur_cell = math.vec2.new(math.round(size(map[1])/2.0),math.round(size(map)/2.0))
} }
function UAVpathPlanner(m_navigation, m_pos) { function UAVpathPlanner(m_navigation, m_pos) {
@ -25,7 +25,7 @@ function UAVpathPlanner(m_navigation, m_pos) {
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y)) mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
# search for a path # search for a path
return RRTSTAR(mapgoal,math.vec2.new(5,5)) #map.nb_col/20.0,map.nb_row/20.0)) return RRTSTAR(mapgoal,math.vec2.new(5,5)) #size(map[1])/20.0,size(map[1])/20.0))
} }
function Kh4pathPlanner(m_goal, m_pos) { function Kh4pathPlanner(m_goal, m_pos) {
@ -52,14 +52,14 @@ function update_curcell(m_curpos, kh4) {
cur_cell = math.vec2.add(cur_cell, m_curpos) cur_cell = math.vec2.add(cur_cell, m_curpos)
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y)) cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
} }
if(cur_cell.x>map.nb_row) if(cur_cell.x > size(map))
cur_cell.x=map.nb_row cur_cell.x = size(map)
if(cur_cell.y>map.nb_col) if(cur_cell.y > size(map[1]))
cur_cell.y=map.nb_col cur_cell.y = size(map[1])
if(cur_cell.x<1) if(cur_cell.x < 1)
cur_cell.x=1 cur_cell.x = 1
if(cur_cell.y<1) if(cur_cell.y < 1)
cur_cell.y=1 cur_cell.y = 1
} }
function UAVgetneiobs (m_curpos) { function UAVgetneiobs (m_curpos) {
@ -71,9 +71,9 @@ function UAVgetneiobs (m_curpos) {
} }
function getneiobs (m_curpos) { function getneiobs (m_curpos) {
foundobstacle = 0 var foundobstacle = 0
update_curcell(m_curpos,1) update_curcell(m_curpos,1)
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])
neighbors.foreach(function(rid, data) { neighbors.foreach(function(rid, data) {
@ -130,7 +130,7 @@ function getproxobs (m_curpos) {
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 > IR_SENSOR_THRESHOLD) {
if(rmat(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)
add_obstacle(obsr, 0, 0.25) add_obstacle(obsr, 0, 0.25)
@ -139,7 +139,7 @@ function getproxobs (m_curpos) {
add_obstacle(obsl2, 0, 0.25) add_obstacle(obsl2, 0, 0.25)
foundobstacle = 1 foundobstacle = 1
} }
} else if(rmat(map,math.round(obs.x),math.round(obs.y))!=1) { } else if(map[math.round(obs.x)][math.round(obs.y)]!=1) {
remove_obstacle(obs, 0, 0.05) remove_obstacle(obs, 0, 0.05)
foundobstacle = 1 foundobstacle = 1
} }
@ -163,7 +163,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
nb=goal_l nb=goal_l
update_curcell(m_curpos,kh4) update_curcell(m_curpos,kh4)
Cvec = cur_cell Cvec = cur_cell
while(nb < m_path.nb_row and nb <= goal_l+1){ while(nb < size(m_path) and nb <= goal_l+1) {
Nvec = getvec(m_path, nb) 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)
@ -179,17 +179,17 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
} }
function RRTSTAR(GOAL,TOL) { function RRTSTAR(GOAL,TOL) {
HEIGHT = map.nb_col-1 HEIGHT = size(map)
WIDTH = map.nb_row-1 WIDTH = size(map[1])
RADIUS = 1.25 #TOL.x #map.nb_col/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} 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)
arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}}
Path = {.nb_col=2, .nb_row=1, .mat={.0=cur_cell.x,.1=cur_cell.y}}
numberOfPoints = 1 numberOfPoints = 1
Q = {.nb_col=5,.nb_row=1,.mat={.0=cur_cell.x,.1=cur_cell.y,.2=0,.3=1,.4=0}} arrayOfPoints = {}
Path = {}
Q = {}
goalReached = 0; goalReached = 0;
timeout = 0 timeout = 0
@ -209,11 +209,11 @@ function RRTSTAR(GOAL,TOL) {
minCounted = 999; minCounted = 999;
minCounter = 0; minCounter = 0;
if(pointList.nb_row!=0) { if(size(pointList) != 0) {
#log("Found ", pointList.nb_row, " close point:", pointList.mat) #log("Found ", size(pointList), " close point:", pointList.mat)
ipt=1 ipt=1
while(ipt<=pointList.nb_row){ while(ipt <= size(pointList)) {
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}} pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt) mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything # Follow the line to see if it intersects anything
@ -224,7 +224,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))+rmat(Q,pointNumber.mat[3],5) 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,22 +235,22 @@ function RRTSTAR(GOAL,TOL) {
} }
if(minCounter > 0) { if(minCounter > 0) {
numberOfPoints = numberOfPoints + 1; numberOfPoints = numberOfPoints + 1;
wmat(arrayOfPoints,numberOfPoints,1,pt.x) arrayOfPoints[numberOfPoints][1]=pt.x
wmat(arrayOfPoints,numberOfPoints,2,pt.y) arrayOfPoints[numberOfPoints][2]=pt.y
wmat(Q,numberOfPoints,1, pt.x) Q[numberOfPoints][1] = pt.x
wmat(Q,numberOfPoints,2, pt.y) Q[numberOfPoints][2] = pt.y
wmat(Q,numberOfPoints,3, rmat(pointList,minCounter,4)); Q[numberOfPoints][3] = pointList[minCounter][4]
wmat(Q,numberOfPoints,4, numberOfPoints) Q[numberOfPoints][4] = numberOfPoints
wmat(Q,numberOfPoints,5, minCounted) Q[numberOfPoints][5] = minCounted
#log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y) #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)
# 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<pointList.nb_row) { while(ipt < size(pointList)) {
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}} pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt) mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything # Follow the line to see if it intersects anything
@ -261,10 +261,10 @@ function RRTSTAR(GOAL,TOL) {
nbCount = nbCount + 1; nbCount = nbCount + 1;
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 = rmat(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))
if(tmpdistance < rmat(Q,rmat(pointNumber,1,4),5)) { if(tmpdistance < Q[pointNumber[1][4]][5]) {
wmat(Q,rmat(pointNumber,1,4),3, numberOfPoints) Q[pointNumber[1][4]][3] = numberOfPoints
wmat(Q,rmat(pointNumber,1,4),5, tmpdistance) Q[pointNumber[1][4]][5] = tmpdistance
} }
} }
ipt = ipt + 1 ipt = ipt + 1
@ -285,16 +285,16 @@ 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;
wmat(arrayOfPoints,numberOfPoints,1,pt.x) arrayOfPoints[numberOfPoints][1]=pt.x
wmat(arrayOfPoints,numberOfPoints,2,pt.y) arrayOfPoints[numberOfPoints][2]=pt.y
wmat(Q,numberOfPoints,1, pt.x) Q[numberOfPoints][1] = pt.x
wmat(Q,numberOfPoints,2, pt.y) Q[numberOfPoints][2] = pt.y
wmat(Q,numberOfPoints,3, pointNum); Q[numberOfPoints][3] = pointNum
wmat(Q,numberOfPoints,4, numberOfPoints) Q[numberOfPoints][4] = numberOfPoints
wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))) Q[numberOfPoints][5] = Q[pointNum][5]+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt))
#log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y) #log("added point to Q(", size(Q), "): ", pt.x, " ", pt.y)
# Check to see if this new point is within the goal # Check to see if this new point is within the goal
if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax) if(pt.x < goalBoundary.xmax and pt.x > goalBoundary.xmin and pt.y > goalBoundary.ymin and pt.y < goalBoundary.ymax)
@ -323,7 +323,7 @@ function findClosestPoint(point,aPt) {
distance = 999 distance = 999
pointNumber = -1 pointNumber = -1
ifcp=1 ifcp=1
while(ifcp<=aPt.nb_row) { while(ifcp <= size(aPt)) {
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp))) range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
if(range < distance) { if(range < distance) {
@ -337,17 +337,16 @@ function findClosestPoint(point,aPt) {
# Find the closest point in the tree # Find the closest point in the tree
function findPointsInRadius(point,q,r) { function findPointsInRadius(point,q,r) {
counted = 0; pointList = {}
pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}} var counted = 0;
iir=1 var iir = 1
while(iir <= q.nb_row) { while(iir <= size(q)) {
tmpvv = getvec(q,iir) 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)) distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
if(distance < r) { if(distance < r) {
counted = counted+1; counted = counted+1;
pointList.nb_row=counted
mat_copyrow(pointList,counted,q,iir) mat_copyrow(pointList,counted,q,iir)
} }
@ -366,7 +365,7 @@ function doesItIntersect(point,vector) {
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!=cur_cell.x and yi!=cur_cell.y){
if(rmat(map,xi,yi) > 0.5) if(map[xi][yi] > 0.5)
return 1 return 1
else else
return 0 return 0
@ -374,7 +373,7 @@ function doesItIntersect(point,vector) {
return 0 return 0
} }
vec = math.vec2.scale(dif,1.0/distance) vec = math.vec2.scale(dif,1.0/distance)
pathstep = map.nb_col - 2 pathstep = size(map[1]) - 2
idii = 1.0 idii = 1.0
while(idii <= pathstep) { while(idii <= pathstep) {
@ -387,9 +386,9 @@ function doesItIntersect(point,vector) {
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range) #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
if(xi!=cur_cell.x and yi!=cur_cell.y){ if(xi!=cur_cell.x and yi!=cur_cell.y){
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) { if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) {
if(rmat(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, ": ", rmat(map,xi,yi), ")!") #log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
return 1; return 1;
} }
} else { } else {
@ -404,23 +403,25 @@ function doesItIntersect(point,vector) {
} }
function getPathGPS(Q,nb){ function getPathGPS(Q,nb){
path={.nb_col=2, .nb_row=0, .mat={}} path={}
npt=0 var npt=0
# get the path from the point list # get the path from the point list
while(nb!=1){ while(nb!=1){
npt=npt+1 npt=npt+1
path.nb_row=npt path[npt] = {}
path.mat[(npt-1)*2]=rmat(Q,nb,1) path[npt][1]=Q[nb][1]
path.mat[(npt-1)*2+1]=rmat(Q,nb,2) path[npt][2]=Q[nb][2]
nb = rmat(Q,nb,3); nb = Q[nb][3];
} }
# re-order the list and make the path points absolute # re-order the list and make the path points absolute
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}} pathR={}
while(npt>0){ var totpt = npt
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.mat[(path.nb_row-npt)*2]=tmpgoal.latitude pathR[totpt-npt] = {}
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.longitude pathR[totpt-npt][1]=tmpgoal.latitude
pathR[totpt-npt][2]=tmpgoal.longitude
npt = npt - 1 npt = npt - 1
} }
return pathR return pathR
@ -428,25 +429,27 @@ function getPathGPS(Q,nb){
# create a table with only the path's points in order # create a table with only the path's points in order
function getPath(Q,nb){ function getPath(Q,nb){
path={.nb_col=2, .nb_row=0, .mat={}} path={}
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.nb_row=npt path[npt] = {}
path.mat[(npt-1)*2]=rmat(Q,nb,1) path[npt][1]=Q[nb][1]
path.mat[(npt-1)*2+1]=rmat(Q,nb,2) path[npt][2]=Q[nb][2]
nb = rmat(Q,nb,3); nb = Q[nb][3];
} }
# log("re-order the list") # log("re-order the list")
# table_print(path.mat) # table_print(path.mat)
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}} pathR={}
while(npt>0){ var totpt = npt
while(npt > 0){
tmpgoal = getvec(path,npt) tmpgoal = getvec(path,npt)
pathR.mat[(path.nb_row-npt)*2]=tmpgoal.x pathR[totpt-npt] = {}
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y pathR[totpt-npt][1]=tmpgoal.x
pathR[totpt-npt][2]=tmpgoal.y
npt = npt - 1 npt = npt - 1
} }
#log("Double-check path: ", check_path(pathR, 1, cur_cell, 1)) #log("Double-check path: ", check_path(pathR, 1, cur_cell, 1))

2
buzz_scripts/include/uavstates.bzz
View File

@ -113,7 +113,7 @@ function gotoWPRRT(transf) {
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<=Path.nb_row) { } else if(cur_goal_l <= size(Path)) {
cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude cur_gps=getvec(Path,cur_goal_l) #x=latitude, y=longitude
cur_pt=vec_from_gps(cur_gps.x,cur_gps.y) cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
print(" heading to ", cur_pt.x,cur_pt.y) print(" heading to ", cur_pt.x,cur_pt.y)

8
buzz_scripts/include/vstigenv.bzz
View File

@ -17,7 +17,7 @@ WAIT4STEP = 10
var v_status = {} var v_status = {}
var v_ground = {} var v_ground = {}
b_updating = 0 b_updating = 0
counter=WAIT4STEP vstig_counter = WAIT4STEP
function uav_initstig() { function uav_initstig() {
v_status = stigmergy.create(STATUS_VSTIG) v_status = stigmergy.create(STATUS_VSTIG)
@ -26,16 +26,16 @@ function uav_initstig() {
function uav_updatestig() { function uav_updatestig() {
# TODO: Push values on update only. # TODO: Push values on update only.
if(counter<=0) { if(vstig_counter<=0) {
b_updating=1 b_updating=1
#var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status} #var ls={.1=0,.2=battery.capacity,.3=xbee_status.rssi,.4=flight.status}
ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status ls = 50*1000000 + battery.capacity*1000 + xbee_status.rssi*10 + flight.status
log("Pushing ", ls, "on vstig with id:", id) log("Pushing ", ls, "on vstig with id:", id)
v_status.put(id, ls) v_status.put(id, ls)
counter=WAIT4STEP vstig_counter=WAIT4STEP
} else { } else {
b_updating=0 b_updating=0
counter=counter-1 vstig_counter=vstig_counter-1
} }
} }

1
buzz_scripts/testalone.bzz
View File

@ -3,7 +3,6 @@ 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.
include "vstigenv.bzz" include "vstigenv.bzz"
include "rrtstar.bzz"
function action() { function action() {
statef=action statef=action

8
include/buzz_update.h
View File

@ -129,9 +129,9 @@ void destroy_out_msg_queue();
/***************************************************/ /***************************************************/
/*obatins updater state*/ /*obatins updater state*/
/***************************************************/ /***************************************************/
//int get_update_mode(); // int get_update_mode();
//buzz_updater_elem_t get_updater(); // buzz_updater_elem_t get_updater();
/***************************************************/ /***************************************************/
/*sets bzz file name*/ /*sets bzz file name*/
/***************************************************/ /***************************************************/
@ -155,7 +155,7 @@ int compile_bzz(std::string bzz_file);
void updates_set_robots(int robots); void updates_set_robots(int robots);
//void set_packet_id(int packet_id); // void set_packet_id(int packet_id);
//void collect_data(std::ofstream& logger); // void collect_data(std::ofstream& logger);
#endif #endif

6
include/buzzuav_closures.h
View File

@ -50,6 +50,10 @@ int buzzuav_setgimbal(buzzvm_t vm);
* parse a csv list of waypoints * parse a csv list of waypoints
*/ */
void parse_gpslist(); void parse_gpslist();
/*
* closure to export a 2D map
*/
int buzz_exportmap(buzzvm_t vm);
/* /*
* closure to take a picture * closure to take a picture
*/ */
@ -81,7 +85,7 @@ void set_deque_full(bool state);
void set_rssi(float value); void set_rssi(float value);
void set_raw_packet_loss(float value); void set_raw_packet_loss(float value);
void set_filtered_packet_loss(float value); void set_filtered_packet_loss(float value);
//void set_api_rssi(float value); // void set_api_rssi(float value);
/* /*
* sets current position * sets current position
*/ */

9
readme.md
View File

@ -100,3 +100,12 @@ References
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 * ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. * Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
Visual Studio Code
--------------------
To activate highlights of the code in Visual Studio Code or Roboware add the following to settings.json:
```
"files.associations": {
"*.bzz":"python"
}
```

5
src/buzz_update.cpp
View File

@ -267,7 +267,7 @@ void code_message_outqueue_append()
*(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size);
size += sizeof(uint16_t); size += sizeof(uint16_t);
memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size));
//size += (uint16_t) * (size_t*)(updater->patch_size); // size += (uint16_t) * (size_t*)(updater->patch_size);
updater_msg_ready = 1; updater_msg_ready = 1;
} }
@ -713,7 +713,8 @@ int compile_bzz(std::string bzz_file)
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
// RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
// Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter //
Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << ","
<< old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
<< (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;

3
src/buzz_utility.cpp
View File

@ -234,6 +234,9 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "export_map", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_exportmap));
buzzvm_gstore(VM);
return VM->state; return VM->state;
} }

34
src/buzzuav_closures.cpp
View File

@ -46,7 +46,7 @@ int buzzros_print(buzzvm_t vm)
----------------------------------------------------------- */ ----------------------------------------------------------- */
{ {
std::ostringstream buffer(std::ostringstream::ate); std::ostringstream buffer(std::ostringstream::ate);
buffer << buzz_utility::get_robotid(); buffer << "[" << buzz_utility::get_robotid() << "] ";
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index) for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{ {
buzzvm_lload(vm, index); buzzvm_lload(vm, index);
@ -154,7 +154,7 @@ void parse_gpslist()
double lon = atof(strtok(NULL, DELIMS)); double lon = atof(strtok(NULL, DELIMS));
double lat = atof(strtok(NULL, DELIMS)); double lat = atof(strtok(NULL, DELIMS));
int alt = atoi(strtok(NULL, DELIMS)); int alt = atoi(strtok(NULL, DELIMS));
//int tilt = atoi(strtok(NULL, DELIMS)); // int tilt = atoi(strtok(NULL, DELIMS));
// DEBUG // DEBUG
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude = lat; RB_arr.latitude = lat;
@ -173,6 +173,36 @@ void parse_gpslist()
fin.close(); fin.close();
} }
int buzz_exportmap(buzzvm_t vm)
/*
/ Buzz closure to export a 2D map
/----------------------------------------*/
{
/* Make sure one parameter has been passed */
buzzvm_lnum_assert(vm, 1);
/* Get the parameter */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // matrix
/* Get the table */
buzzobj_t t = buzzvm_stack_at(vm, 1);
/* Copy the values into a vector */
std::vector<float> mat;
for (int32_t i = 0; i < buzzdict_size(t->t.value); ++i)
{
/* Duplicate the table */
buzzvm_dup(vm);
/* Push the index */
buzzvm_pushi(vm, i);
/* Get the value */
buzzvm_tget(vm);
/* Store it in the vector (assume all values are float, no mistake...) */
mat.push_back((float)buzzvm_stack_at(vm, 1)->f.value);
/* Get rid of the value, now useless */
buzzvm_pop(vm);
}
return buzzvm_ret0(vm);
}
int buzzuav_moveto(buzzvm_t vm) int buzzuav_moveto(buzzvm_t vm)
/* /*
/ Buzz closure to move following a 2D vector / Buzz closure to move following a 2D vector