From 9f5338602330696885e83089f4af2803b6ccf215 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 19 Dec 2017 13:09:22 -0500 Subject: [PATCH 1/2] changed matrix call in bzz, fixed ros prints from bzz and add closure to export maps --- buzz_scripts/include/mapmatrix.bzz | 124 +++++++++------------- buzz_scripts/include/rrtstar.bzz | 161 +++++++++++++++-------------- buzz_scripts/include/uavstates.bzz | 4 +- buzz_scripts/include/vstigenv.bzz | 8 +- buzz_scripts/testalone.bzz | 1 - include/buzzuav_closures.h | 4 + readme.md | 9 ++ src/buzz_utility.cpp | 3 + src/buzzuav_closures.cpp | 31 +++++- 9 files changed, 181 insertions(+), 164 deletions(-) diff --git a/buzz_scripts/include/mapmatrix.bzz b/buzz_scripts/include/mapmatrix.bzz index bd7bdd2..b99432a 100644 --- a/buzz_scripts/include/mapmatrix.bzz +++ b/buzz_scripts/include/mapmatrix.bzz @@ -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 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) } diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz index 3047827..ff0630c 100644 --- a/buzz_scripts/include/rrtstar.bzz +++ b/buzz_scripts/include/rrtstar.bzz @@ -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 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)) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index a789600..8824173 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -61,7 +61,7 @@ function land() { function set_goto(transf) { UAVSTATE = "GOTOGPS" statef=function() { - gotoWP(transf) + gotoWPRRT(transf) } if(rc_goto.id==id){ @@ -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) diff --git a/buzz_scripts/include/vstigenv.bzz b/buzz_scripts/include/vstigenv.bzz index 7fd8e42..0a7b01f 100644 --- a/buzz_scripts/include/vstigenv.bzz +++ b/buzz_scripts/include/vstigenv.bzz @@ -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 } } diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index bba5d7a..7460de4 100644 --- a/buzz_scripts/testalone.bzz +++ b/buzz_scripts/testalone.bzz @@ -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 diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 6b33843..2f5d1dd 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 */ diff --git a/readme.md b/readme.md index 4295150..82d3941 100644 --- a/readme.md +++ b/readme.md @@ -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" + } +``` \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 38277d8..db43d6f 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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; } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index ad27ed5..6b453b8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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); @@ -175,6 +175,35 @@ 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 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 From 177baf5d15e903c4c0b31c83f19cc3706ad8b8b0 Mon Sep 17 00:00:00 2001 From: dave Date: Tue, 19 Dec 2017 13:14:56 -0500 Subject: [PATCH 2/2] beautified --- buzz_scripts/include/uavstates.bzz | 2 +- include/buzz_update.h | 8 ++--- include/buzzuav_closures.h | 2 +- src/buzz_update.cpp | 5 ++-- src/buzzuav_closures.cpp | 47 +++++++++++++++--------------- 5 files changed, 33 insertions(+), 31 deletions(-) diff --git a/buzz_scripts/include/uavstates.bzz b/buzz_scripts/include/uavstates.bzz index 8824173..843f34f 100644 --- a/buzz_scripts/include/uavstates.bzz +++ b/buzz_scripts/include/uavstates.bzz @@ -61,7 +61,7 @@ function land() { function set_goto(transf) { UAVSTATE = "GOTOGPS" statef=function() { - gotoWPRRT(transf) + gotoWP(transf) } if(rc_goto.id==id){ diff --git a/include/buzz_update.h b/include/buzz_update.h index d858deb..5680f85 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -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 diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 71eb847..56f0bd6 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -85,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 */ diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index d9696ca..1d169f9 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -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_; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index b5b6286..6b380d6 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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; @@ -178,28 +178,29 @@ 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 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); + /* 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 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)