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
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
function mat_copyrow(out,ro,in,ri){
var indexI = (ri-1)*in.nb_col
var indexO = (ro-1)*out.nb_col
icr=0
while(icr<in.nb_col){
out.mat[indexO+icr]=in.mat[indexI+icr]
out[ro] = {}
var icr = 0
while(icr < size(in[ri])) {
out[ro][icr]=in[ri][icr]
icr = icr + 1
}
}
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){
map = {.nb_col=len, .nb_row=len, .mat={}}
var index = 0
while(index<len*len){
map.mat[index]=1.0
index = index + 1
map = {}
var indexR = 0
while(indexR < len) {
map[indexR] = {}
var indexC = 0
while(indexC < len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
indexR = indexR + 1
}
# puts an obstacle right in the middle
wmat(map,5,5,0.0)
wmat(map,6,5,0.0)
wmat(map,4,5,0.0)
# DEBUG\TEST: puts an obstacle right in the middle
map[5][5] = 0.0
map[6][5] = 0.0
map[4][5] = 0.0
log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
}
function init_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}}
var index = 0
while(index<len*len){
map.mat[index]=1.0
index = index + 1
map = {}
var indexR = 0
while(indexR < len) {
map[indexR] = {}
var indexC = 0
while(indexC < len) {
map[indexR][indexC] = 1.0
indexC = indexC + 1
}
indexR = indexR + 1
}
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 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)
old=rmat(map,xi,yi)
old=map[xi][yi]
if(old-inc_trust > 0.0)
wmat(map,xi,yi,old-inc_trust)
map[xi][yi] = old-inc_trust
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 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)
old=rmat(map, xi, yi)
old=map[xi][yi]
if(old + dec_trust < 1.0) #x,y
wmat(map, xi, yi, old+dec_trust)
map[xi][yi] = old+dec_trust
else
wmat(map, xi, yi, 1.0)
map[xi][yi] = 1.0
}
}
@ -108,44 +95,27 @@ function table_copy(t) {
function print_pos(t) {
var ir=1
while(ir<=t.nb_row){
log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2))
while(ir <= size(t)) {
log(ir, ": ", t[ir][1], " ", t[ir][2])
ir = ir + 1
}
}
function print_map(t) {
var ir=t.nb_row
log("Printing a ", t.nb_row, " by ", t.nb_col, " map")
while(ir>0){
var ir=size(t)
log("Printing a ", size(t), " by ", size(t[1]), " map")
while(ir > 0) {
logst=string.concat("\t", string.tostring(ir), "\t:")
ic=t.nb_col
while(ic>0){
ic = size(t[ir])
while(ic > 0) {
if(ir==cur_cell.x and ic==cur_cell.y)
logst = string.concat(logst, " XXXXXXXX")
else
logst = string.concat(logst, " ", string.tostring(rmat(t,ir,ic)))
logst = string.concat(logst, " ", string.tostring(t[ir][ic]))
ic = ic - 1
}
log(logst)
ir = ir - 1
}
}
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)
export_map(map)
}

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
init_map(2*math.round(math.vec2.length(m_navigation))+10)
# 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) {
@ -25,7 +25,7 @@ function UAVpathPlanner(m_navigation, m_pos) {
mapgoal = math.vec2.new(math.round(mapgoal.x),math.round(mapgoal.y))
# 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) {
@ -52,14 +52,14 @@ function update_curcell(m_curpos, kh4) {
cur_cell = math.vec2.add(cur_cell, m_curpos)
cur_cell = math.vec2.new(math.round(m_curpos.x), math.round(m_curpos.y))
}
if(cur_cell.x>map.nb_row)
cur_cell.x=map.nb_row
if(cur_cell.y>map.nb_col)
cur_cell.y=map.nb_col
if(cur_cell.x<1)
cur_cell.x=1
if(cur_cell.y<1)
cur_cell.y=1
if(cur_cell.x > size(map))
cur_cell.x = size(map)
if(cur_cell.y > size(map[1]))
cur_cell.y = size(map[1])
if(cur_cell.x < 1)
cur_cell.x = 1
if(cur_cell.y < 1)
cur_cell.y = 1
}
function UAVgetneiobs (m_curpos) {
@ -71,9 +71,9 @@ function UAVgetneiobs (m_curpos) {
}
function getneiobs (m_curpos) {
foundobstacle = 0
var foundobstacle = 0
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])
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)
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(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(obs2, 0, 0.25)
add_obstacle(obsr, 0, 0.25)
@ -139,7 +139,7 @@ function getproxobs (m_curpos) {
add_obstacle(obsl2, 0, 0.25)
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)
foundobstacle = 1
}
@ -163,7 +163,7 @@ function check_path(m_path, goal_l, m_curpos, kh4) {
nb=goal_l
update_curcell(m_curpos,kh4)
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)
if(kh4==0)
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) {
HEIGHT = map.nb_col-1
WIDTH = map.nb_row-1
RADIUS = 1.25 #TOL.x #map.nb_col/10.0 # to consider 2 points consecutive
HEIGHT = size(map)
WIDTH = size(map[1])
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}
#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
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;
timeout = 0
@ -209,11 +209,11 @@ function RRTSTAR(GOAL,TOL) {
minCounted = 999;
minCounter = 0;
if(pointList.nb_row!=0) {
#log("Found ", pointList.nb_row, " close point:", pointList.mat)
if(size(pointList) != 0) {
#log("Found ", size(pointList), " close point:", pointList.mat)
ipt=1
while(ipt<=pointList.nb_row){
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
while(ipt <= size(pointList)) {
pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything
@ -224,7 +224,7 @@ function RRTSTAR(GOAL,TOL) {
nbCount = nbCount + 1;
if(intersects != 1) {
#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) {
minCounted = distance;
@ -235,22 +235,22 @@ function RRTSTAR(GOAL,TOL) {
}
if(minCounter > 0) {
numberOfPoints = numberOfPoints + 1;
wmat(arrayOfPoints,numberOfPoints,1,pt.x)
wmat(arrayOfPoints,numberOfPoints,2,pt.y)
arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y
wmat(Q,numberOfPoints,1, pt.x)
wmat(Q,numberOfPoints,2, pt.y)
wmat(Q,numberOfPoints,3, rmat(pointList,minCounter,4));
wmat(Q,numberOfPoints,4, numberOfPoints)
wmat(Q,numberOfPoints,5, minCounted)
Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointList[minCounter][4]
Q[numberOfPoints][4] = numberOfPoints
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
nbCount = 0;
ipt = 1
while(ipt<pointList.nb_row) {
pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
while(ipt < size(pointList)) {
pointNumber = {}
mat_copyrow(pointNumber,1,pointList,ipt)
# Follow the line to see if it intersects anything
@ -261,10 +261,10 @@ function RRTSTAR(GOAL,TOL) {
nbCount = nbCount + 1;
if(intersects != 1) {
# If the alternative path is shorter than change it
tmpdistance = rmat(Q,numberOfPoints,5)+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
if(tmpdistance < rmat(Q,rmat(pointNumber,1,4),5)) {
wmat(Q,rmat(pointNumber,1,4),3, numberOfPoints)
wmat(Q,rmat(pointNumber,1,4),5, tmpdistance)
tmpdistance = Q[numberOfPoints][5]+math.vec2.length(math.vec2.sub(getvec(pointNumber,1),pt))
if(tmpdistance < Q[pointNumber[1][4]][5]) {
Q[pointNumber[1][4]][3] = numberOfPoints
Q[pointNumber[1][4]][5] = tmpdistance
}
}
ipt = ipt + 1
@ -285,16 +285,16 @@ function RRTSTAR(GOAL,TOL) {
# If there is no intersection we need to add to the tree
if(intersects != 1) {
numberOfPoints = numberOfPoints + 1;
wmat(arrayOfPoints,numberOfPoints,1,pt.x)
wmat(arrayOfPoints,numberOfPoints,2,pt.y)
arrayOfPoints[numberOfPoints][1]=pt.x
arrayOfPoints[numberOfPoints][2]=pt.y
wmat(Q,numberOfPoints,1, pt.x)
wmat(Q,numberOfPoints,2, pt.y)
wmat(Q,numberOfPoints,3, pointNum);
wmat(Q,numberOfPoints,4, numberOfPoints)
wmat(Q,numberOfPoints,5, rmat(Q,pointNum,5)+math.vec2.length(math.vec2.sub(getvec(Q,pointNum),pt)))
Q[numberOfPoints][1] = pt.x
Q[numberOfPoints][2] = pt.y
Q[numberOfPoints][3] = pointNum
Q[numberOfPoints][4] = numberOfPoints
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
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
pointNumber = -1
ifcp=1
while(ifcp<=aPt.nb_row) {
while(ifcp <= size(aPt)) {
range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
if(range < distance) {
@ -337,17 +337,16 @@ function findClosestPoint(point,aPt) {
# Find the closest point in the tree
function findPointsInRadius(point,q,r) {
counted = 0;
pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}}
iir=1
while(iir <= q.nb_row) {
pointList = {}
var counted = 0;
var iir = 1
while(iir <= size(q)) {
tmpvv = getvec(q,iir)
#log("FPiR :", point.x, " ", point.y," ", tmpvv.x," ", tmpvv.y)
distance = math.vec2.length(math.vec2.sub(getvec(q,iir),point))
if(distance < r) {
counted = counted+1;
pointList.nb_row=counted
mat_copyrow(pointList,counted,q,iir)
}
@ -366,7 +365,7 @@ function doesItIntersect(point,vector) {
var xi = math.round(point.x) #+1
var yi = math.round(point.y) #+1
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
else
return 0
@ -374,7 +373,7 @@ function doesItIntersect(point,vector) {
return 0
}
vec = math.vec2.scale(dif,1.0/distance)
pathstep = map.nb_col - 2
pathstep = size(map[1]) - 2
idii = 1.0
while(idii <= pathstep) {
@ -387,9 +386,9 @@ function doesItIntersect(point,vector) {
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
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(rmat(map,xi,yi) < 0.5) { # because obstacle use trust values
#log("Obstacle in the way(", xi, " ", yi, ": ", rmat(map,xi,yi), ")!")
if(xi <= size(map[1]) and yi <= size(map) and xi > 0 and yi > 0) {
if(map[xi][yi] < 0.5) { # because obstacle use trust values
#log("Obstacle in the way(", xi, " ", yi, ": ", map[xi][yi], ")!")
return 1;
}
} else {
@ -404,23 +403,25 @@ function doesItIntersect(point,vector) {
}
function getPathGPS(Q,nb){
path={.nb_col=2, .nb_row=0, .mat={}}
npt=0
path={}
var npt=0
# get the path from the point list
while(nb!=1){
npt=npt+1
path.nb_row=npt
path.mat[(npt-1)*2]=rmat(Q,nb,1)
path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
nb = rmat(Q,nb,3);
path[npt] = {}
path[npt][1]=Q[nb][1]
path[npt][2]=Q[nb][2]
nb = Q[nb][3];
}
# re-order the list and make the path points absolute
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}}
while(npt>0){
pathR={}
var totpt = npt
while(npt > 0){
tmpgoal = gps_from_vec(math.vec2.sub(getvec(path,npt),cur_cell))
pathR.mat[(path.nb_row-npt)*2]=tmpgoal.latitude
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.longitude
pathR[totpt-npt] = {}
pathR[totpt-npt][1]=tmpgoal.latitude
pathR[totpt-npt][2]=tmpgoal.longitude
npt = npt - 1
}
return pathR
@ -428,25 +429,27 @@ function getPathGPS(Q,nb){
# create a table with only the path's points in order
function getPath(Q,nb){
path={.nb_col=2, .nb_row=0, .mat={}}
npt=0
path={}
var npt=0
# log("get the path from the point list")
while(nb!=1){
npt=npt+1
path.nb_row=npt
path.mat[(npt-1)*2]=rmat(Q,nb,1)
path.mat[(npt-1)*2+1]=rmat(Q,nb,2)
nb = rmat(Q,nb,3);
path[npt] = {}
path[npt][1]=Q[nb][1]
path[npt][2]=Q[nb][2]
nb = Q[nb][3];
}
# log("re-order the list")
# table_print(path.mat)
pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}}
while(npt>0){
pathR={}
var totpt = npt
while(npt > 0){
tmpgoal = getvec(path,npt)
pathR.mat[(path.nb_row-npt)*2]=tmpgoal.x
pathR.mat[(path.nb_row-npt)*2+1]=tmpgoal.y
pathR[totpt-npt] = {}
pathR[totpt-npt][1]=tmpgoal.x
pathR[totpt-npt][2]=tmpgoal.y
npt = npt - 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)
}
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_pt=vec_from_gps(cur_gps.x,cur_gps.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_ground = {}
b_updating = 0
counter=WAIT4STEP
vstig_counter = WAIT4STEP
function uav_initstig() {
v_status = stigmergy.create(STATUS_VSTIG)
@ -26,16 +26,16 @@ function uav_initstig() {
function uav_updatestig() {
# TODO: Push values on update only.
if(counter<=0) {
if(vstig_counter<=0) {
b_updating=1
#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
log("Pushing ", ls, "on vstig with id:", id)
v_status.put(id, ls)
counter=WAIT4STEP
vstig_counter=WAIT4STEP
} else {
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 "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
include "rrtstar.bzz"
function action() {
statef=action

8
include/buzz_update.h
View File

@ -129,9 +129,9 @@ void destroy_out_msg_queue();
/***************************************************/
/*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*/
/***************************************************/
@ -155,7 +155,7 @@ int compile_bzz(std::string bzz_file);
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

6
include/buzzuav_closures.h
View File

@ -50,6 +50,10 @@ int buzzuav_setgimbal(buzzvm_t vm);
* parse a csv list of waypoints
*/
void parse_gpslist();
/*
* closure to export a 2D map
*/
int buzz_exportmap(buzzvm_t vm);
/*
* closure to take a picture
*/
@ -81,7 +85,7 @@ void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_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
*/

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
* 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);
size += sizeof(uint16_t);
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;
}
@ -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;
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,
// 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 << ","
<< old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
<< (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_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus));
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;
}

34
src/buzzuav_closures.cpp
View File

@ -46,7 +46,7 @@ int buzzros_print(buzzvm_t vm)
----------------------------------------------------------- */
{
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)
{
buzzvm_lload(vm, index);
@ -154,7 +154,7 @@ void parse_gpslist()
double lon = atof(strtok(NULL, DELIMS));
double lat = atof(strtok(NULL, DELIMS));
int alt = atoi(strtok(NULL, DELIMS));
//int tilt = atoi(strtok(NULL, DELIMS));
// int tilt = atoi(strtok(NULL, DELIMS));
// DEBUG
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude = lat;
@ -173,6 +173,36 @@ void parse_gpslist()
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)
/*
/ Buzz closure to move following a 2D vector