removed floor, add PY rosprint, hard-coded swarm size, add states bzzcmd

buzz_scripts/graphformGPS.bzz

@@ -8,9 +8,11 @@ include "update.bzz"
 #include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
 include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
-include "rrtstar.bzz"
 
+include "graphs/shapes_P.bzz"
+include "graphs/shapes_O.bzz"
 include "graphs/shapes_L.bzz"
+include "graphs/shapes_Y.bzz"
 
 ROBOT_RADIUS = 50
 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
@@ -680,7 +682,8 @@ function DoJoined(){
 	#if(v_tag.size()==ROBOTS){
 	#	TransitionToLock()
 	#}
-  barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
+  #barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
+  barrier_wait(6, TransitionToLock, DoJoined, -1)
 }
 
 #
@@ -701,6 +704,9 @@ function DoLock(){
   }
   #move
   uav_moveto(m_navigation.x, m_navigation.y, 0.0)
+
+  # prepare to restart a new shape
+  statef=action
 }
 
 function action(){
@@ -745,7 +751,7 @@ function step() {
 	# get the swarm commands
 	uav_neicmd()
 	# update the vstig (status/net/batt)
-  #uav_updatestig()
+  	#uav_updatestig()
 
 	#update the graph
 	UpdateNodeInfo()
@@ -797,8 +803,16 @@ function step() {
 # Executed when reset
 #
 function Reset(){
-	Read_Graph()
+	if(rc_State==0){
-  m_nLabel=-1
+		Read_GraphP()
+	} else if(rc_State==1) {
+		Read_GraphO()
+	} else if(rc_State==2) {
+		Read_GraphL()
+	} else if(rc_State==3) {
+		Read_GraphY()
+	}
+  	m_nLabel=-1
 
 	#start listening
 	start_listen()

buzz_scripts/include/graphs/shapes_L.bzz

@@ -1,7 +1,7 @@
 #Table of the nodes in the graph
 m_vecNodes={}
 
-function Read_Graph(){
+function Read_GraphL(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
     .Label = 0,   # Label of the point
     .Pred = -1,   # Label of its predecessor

buzz_scripts/include/graphs/shapes_O.bzz

@@ -1,7 +1,7 @@
 #Table of the nodes in the graph
 m_vecNodes={}
 m_vecNodes_fixed={}
-function Read_Graph(){
+function Read_GraphO(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
     .Label = 0,   # Label of the point
     .Pred = -1,   # Label of its predecessor

buzz_scripts/include/graphs/shapes_P.bzz

@@ -1,7 +1,7 @@
 #Table of the nodes in the graph
 m_vecNodes={}
 m_vecNodes_fixed={}
-function Read_Graph(){
+function Read_GraphP(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
     .Label = 0,   # Label of the point
     .Pred = -1,   # Label of its predecessor

buzz_scripts/include/graphs/shapes_Y.bzz

@@ -1,7 +1,7 @@
 #Table of the nodes in the graph
 m_vecNodes={}
 m_vecNodes_fixed={}
-function Read_Graph(){
+function Read_GraphY(){
 m_vecNodes[0] = {      # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
     .Label = 0,   # Label of the point
     .Pred = -1,   # Label of its predecessor

buzz_scripts/include/mapmatrix.bzz

@@ -1,5 +1,7 @@
 
 # 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
@@ -13,8 +15,9 @@ function wmat(mat, row, col, val) {
 # Read from matrix
 function rmat(mat, row, col) {
     #log("rmat ", mat, row, col)
-    var index = (row-1)*mat.nb_col + (col - 1)
+    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]
@@ -36,46 +39,56 @@ function getvec(t,row){
     return math.vec2.new(rmat(t,row,1),rmat(t,row,2))
 }
 
-function init_test_map(){
+function init_test_map(len){
-    # creates a 10x10 map
+    map = {.nb_col=len, .nb_row=len, .mat={}}
-    map = {.nb_col=10, .nb_row=10, .mat={}}
-    cur_cell = math.vec2.new(1,1)
     index = 0
-    while(index<10*10){
+    while(index<len*len){
-        map.mat[index]=1
+        map.mat[index]=1.0
         index = index + 1
     }
     # puts an obstacle right in the middle
-    wmat(map,5,5,0)
+    wmat(map,5,5,0.0)
-    wmat(map,6,5,0)
+    wmat(map,6,5,0.0)
-    wmat(map,4,5,0)
+    wmat(map,4,5,0.0)
 
-    log(rmat(map,1,1),rmat(map,3,3))
+    log("Occupancy grid initialized (",len,"x",len,") with obstacles.")
 }
 
 function init_map(len){
     map = {.nb_col=len, .nb_row=len, .mat={}}
-    # center the robot on the grid
-    cur_cell = math.vec2.new(floor(len/2.0),floor(len/2.0))
     index = 0
     while(index<len*len){
-        map.mat[index]=1
+        map.mat[index]=1.0
         index = index + 1
     }
     log("Occupancy grid initialized (",len,"x",len,").")
 }
 
-function add_obstacle(pos) {
+function add_obstacle(pos, off, inc_trust) {
-    xi = floor(pos.x)+cur_cell.x
+    xi = math.round(pos.x)
-    yi = floor(pos.y)+cur_cell.y
+    yi = math.round(pos.y)
-    log("Obstacle in cell: ", xi, yi)
+
+    if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
+        #log("Add obstacle in cell: ", xi, " ", yi)
+        old=rmat(map,xi,yi)
+        if(old-inc_trust > 0.0)
+          wmat(map,xi,yi,old-inc_trust)
+        else
+          wmat(map,xi,yi,0.0)
+    }
+}
+
+function remove_obstacle(pos, off, dec_trust) {
+    xi = math.round(pos.x)
+    yi = math.round(pos.y)
 
     if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){
-        wmat(map,xi,yi,0)
+        #log("Remove obstacle in cell: ", xi, " ", yi)
-        wmat(map,xi+1,yi,0)
+        old=rmat(map, xi, yi)
-        wmat(map,xi-1,yi,0)
+        if(old + dec_trust < 1.0)  #x,y
-        wmat(map,xi,yi+1,0)
+          wmat(map, xi, yi, old+dec_trust)
-        wmat(map,xi,yi-1,0)
+        else
+          wmat(map, xi, yi, 1.0)
     }
 }
 
@@ -84,10 +97,55 @@ function table_print(t) {
       log(key, " -> ", value)
     })
 }
+
+function table_copy(t) {
+  var t2 = {}
+  foreach(t, function(key, value) {
+      t2[key] = value
+    })
+  return t2
+}
+
 function print_pos(t) {
   ir=1
   while(ir<=t.nb_row){
-      log(ir, ": ", rmat(t,ir,1), rmat(t,ir,2))
+      log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2))
       ir = ir + 1
   }
 }
+function print_map(t) {
+  ir=t.nb_row
+  log("Printing a ", t.nb_row, " by ", t.nb_col, " map")
+  while(ir>0){
+      logst=string.concat("\t", string.tostring(ir), "\t:")
+      ic=t.nb_col
+      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)))
+        ic = ic - 1
+      }
+      log(logst)
+      ir = ir - 1
+  }
+}
+
+function print_map_argos(t){
+  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)
+}

buzz_scripts/include/rrtstar.bzz

@@ -5,32 +5,172 @@
 #####
 include "mapmatrix.bzz"
 
-map = {}
+RRT_TIMEOUT = 200
+map = nil
 cur_cell = {}
+nei_cell = {}
 
-function pathPlanner(m_navigation) {
+function UAVpathPlanner(m_navigation) {
     # create a map big enough for the goal
-    init_map(2*floor(math.vec2.length(m_navigation))+2)
+    init_map(2*math.round(math.vec2.length(m_navigation))+2)
+    # center the robot on the grid
+    cur_cell = math.vec2.new(math.round(len/2.0),math.round(len/2.0))
     # create the goal in the map grid
     mapgoal = math.vec2.add(m_navigation,cur_cell)
     # add all neighbors as obstacles in the grid
     neighbors.foreach(function(rid, data) {
-            add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell))
+            add_obstacle(math.vec2.add(math.vec2.newp(data.distance,data.azimuth),cur_cell), 0, 1.0)
             })
     # TODO: add proximity sensor obstacles to the grid
     # search for a path
-    return RRTSTAR(mapgoal,math.vec2.new(map.nb_col/20.0,map.nb_row/20.0))
+    return RRTSTAR(mapgoal,math.vec2.new(5,5))  #map.nb_col/20.0,map.nb_row/20.0))
+}
+
+function Kh4pathPlanner(m_goal, m_pos) {
+    # move 0,0 to a corner instead of the center
+    m_goal = math.vec2.sub(m_goal,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
+    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
+    update_curcell(m_pos)
+    log("Starting from cell: ", cur_cell.x, " ", cur_cell.y)
+    log("Going to cell: ", m_goal.x, " ", m_goal.y)
+
+    # search for a path
+    print_map(map)
+    # print_map_argos(map)
+    return RRTSTAR(m_goal, math.vec2.new(10.0 * CM2KH4.x, 10.0 * CM2KH4.y))
+}
+
+function update_curcell(m_curpos) {
+  cur_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))
+  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
+}
+
+function getneiobs (m_curpos) {
+  foundobstacle = 0
+  update_curcell(m_curpos)
+  old_nei = table_copy(nei_cell)
+  #log("NeiObs debug: ", nei_cell[9], " ", nei_cell[3])
+
+  neighbors.foreach(function(rid, data) {
+  #log("nei rel pos: ", data.distance, "m ", data.azimuth*180.0/math.pi, "deg")
+          Ncell = math.vec2.add(math.vec2.newp(data.distance*100,data.azimuth + absolute_position.theta), m_curpos)
+  #log("nei abs pos: ", Ncell.x, "cm ", Ncell.y, "cm")
+          Ncell = math.vec2.sub(Ncell, math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
+          Ncell = math.vec2.new(math.round(Ncell.x*CM2KH4.x), math.round(Ncell.y*CM2KH4.y))
+          nei_cell[rid] = {.x=Ncell.x, .y=Ncell.y}
+  #log("Neighbor in : ", Ncell.x, " ", Ncell.y)
+          if(old_nei[rid]!=nil) {
+            if(old_nei[rid].x!=nei_cell[rid].x or old_nei[rid].y!=nei_cell[rid].y) {
+  #log("Neighbor moved ! ", nei_cell[rid].x, " ", nei_cell[rid].y)
+              remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y), 0, 1)
+              remove_obstacle(math.vec2.new(old_nei[rid].x+1, old_nei[rid].y), 0, 1)
+              remove_obstacle(math.vec2.new(old_nei[rid].x-1, old_nei[rid].y), 0, 1)
+              remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y+1), 0, 1)
+              remove_obstacle(math.vec2.new(old_nei[rid].x, old_nei[rid].y-1), 0, 1)
+              add_obstacle(Ncell, 0, 1.0)
+              add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
+              add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
+              add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
+              add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
+              foundobstacle = 1
+            }
+          } else {
+            add_obstacle(Ncell, 0, 1.0)
+            add_obstacle(math.vec2.new(Ncell.x+1, Ncell.y), 0, 1.0)
+            add_obstacle(math.vec2.new(Ncell.x-1, Ncell.y), 0, 1.0)
+            add_obstacle(math.vec2.new(Ncell.x, Ncell.y+1), 0, 1.0)
+            add_obstacle(math.vec2.new(Ncell.x, Ncell.y-1), 0, 1.0)
+            foundobstacle = 1
+          }
+        })
+
+  #if(foundobstacle) {
+    #print_map(map)
+  #}
+
+  return foundobstacle
+}
+
+function getproxobs (m_curpos) {
+  foundobstacle = 0
+  update_curcell(m_curpos)
+
+  reduce(proximity,
+    function(key, value, acc) {
+      obs = math.vec2.add(math.vec2.newp(1.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
+      obs2 = math.vec2.add(math.vec2.newp(2.15, value.angle * math.pi / 180.0 + absolute_position.theta), cur_cell)
+      per = math.vec2.sub(obs,cur_cell)
+      obsr = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs)
+      obsr2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) + math.pi/2.0), obs2)
+      obsl = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs)
+      obsl2 = math.vec2.add(math.vec2.newp(1.25, math.vec2.angle(per) - math.pi/2.0), obs2)
+      if(value.value > IR_SENSOR_THRESHOLD) {
+        if(rmat(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)
+          add_obstacle(obsr2, 0, 0.25)
+          add_obstacle(obsl, 0, 0.25)
+          add_obstacle(obsl2, 0, 0.25)
+          foundobstacle = 1
+        }
+      } else if(rmat(map,math.round(obs.x),math.round(obs.y))!=1) {
+        remove_obstacle(obs, 0, 0.05)
+        foundobstacle = 1
+      }
+      return acc
+  }, math.vec2.new(0, 0))
+
+  #if(foundobstacle) {
+  #  reduce(proximity,
+  #        function(key, value, acc){
+  #            log(value.value, ", ",  value.angle)
+  #            return acc
+  #        }, math.vec2.new(0, 0))
+  #  print_map(map)
+  #}
+
+  return foundobstacle
+}
+
+function check_path(m_path, goal_l, m_curpos) {
+  pathisblocked = 0
+  nb=goal_l
+  update_curcell(m_curpos)
+  #m_curpos = math.vec2.sub(m_curpos,math.vec2.new(GRIDCM.xmin, GRIDCM.ymin))
+  #Cvec = math.vec2.new(math.round(m_curpos.x*CM2KH4.x), math.round(m_curpos.y*CM2KH4.y))
+  Cvec = cur_cell
+  while(nb < m_path.nb_row and nb <= goal_l+1){
+    Nvec = getvec(m_path, nb)
+    if(doesItIntersect(Cvec, Nvec)){
+      log("Obstacle in the way ! (", Cvec.x, "/", Cvec.y, "->", Nvec.x, "/", Nvec.y, ")")
+      pathisblocked = 1
+    }
+    Cvec=Nvec
+    nb = nb + 1
+  }
+
+  return pathisblocked
 }
 
 function RRTSTAR(GOAL,TOL) {
-    HEIGHT = map.nb_col
+    HEIGHT = map.nb_col-1
-    WIDTH = map.nb_row
+    WIDTH = map.nb_row-1
-    RADIUS = map.nb_col/10.0 # to consider 2 points consecutive
+    RADIUS = 1.25 #TOL.x #map.nb_col/10.0 # to consider 2 points consecutive
 
-    rng.setseed(11)
 
     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
@@ -41,9 +181,11 @@ function RRTSTAR(GOAL,TOL) {
     ##
     # main search loop
     ##
-    while(goalReached == 0 and timeout < 200) {
+    while(goalReached == 0 and timeout < RRT_TIMEOUT) {
-        # Point generation
+
-        pt = math.vec2.new(HEIGHT*rng.uniform(1.0)+1,WIDTH*rng.uniform(1.0)+1)
+        # Point generation only as grid cell centers
+        pt = math.vec2.new(math.round(HEIGHT * math.rng.uniform(1.0) + 1), math.round(WIDTH * math.rng.uniform(1.0) + 1))
+        #log("Point generated: ", pt.x, " ", pt.y)
 
         pointList = findPointsInRadius(pt,Q,RADIUS);
 
@@ -58,7 +200,6 @@ function RRTSTAR(GOAL,TOL) {
             while(ipt<=pointList.nb_row){
                 pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
                 mat_copyrow(pointNumber,1,pointList,ipt)
-                #log("pointnumber: ", pointNumber.mat)
 
                 # Follow the line to see if it intersects anything
                 intersects = doesItIntersect(pt,getvec(pointNumber,1));
@@ -88,7 +229,7 @@ function RRTSTAR(GOAL,TOL) {
                 wmat(Q,numberOfPoints,4, numberOfPoints)
                 wmat(Q,numberOfPoints,5, minCounted)
 
-                log("added point to Q(", Q.nb_row, "): ", pt.x, pt.y)
+                #log("added point to Q(", Q.nb_row, "): ", pt.x, " ", pt.y)
 
                 # Now check to see if any of the other points can be redirected
                 nbCount = 0;
@@ -138,7 +279,7 @@ function RRTSTAR(GOAL,TOL) {
                 wmat(Q,numberOfPoints,4, numberOfPoints)
                 wmat(Q,numberOfPoints,5, rmat(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(", Q.nb_row, "): ", 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)
@@ -154,6 +295,9 @@ function RRTSTAR(GOAL,TOL) {
         log("Goal found(",numberOfPoints,")!")
         Path = getPath(Q,numberOfPoints)
         print_pos(Path)
+    } else {
+      log("FAILED TO FIND A PATH!!!")
+      Path = nil
     }
     return Path
 }
@@ -176,11 +320,14 @@ function findClosestPoint(point,aPt) {
     return pointNumber
 }
 
+# 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) {
+        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) {
@@ -194,37 +341,54 @@ function findPointsInRadius(point,q,r) {
     return pointList
 }
 
+# check if the line between 2 point intersect an obstacle
 function doesItIntersect(point,vector) {
+    #log("DiI :", point.x, " ", point.y," ", vector.x," ", vector.y)
     dif = math.vec2.sub(point,vector)
     distance = math.vec2.length(dif)
-    vec = math.vec2.scale(dif,1/distance)
+    if(distance==0.0){
+        # Find what block we're in right now
+        xi = math.round(point.x) #+1
+        yi = math.round(point.y) #+1
+        if(xi!=cur_cell.x and yi!=cur_cell.y){
+            if(rmat(map,xi,yi) > 0.5)
+                return 1
+            else
+                return 0
+        } else
+            return 0
+    }
+    vec = math.vec2.scale(dif,1.0/distance)
+    pathstep = map.nb_col - 2
 
-    idii = 1
+    idii = 1.0
-    while(idii<=100) {
+    while(idii <= pathstep) {
-        range = distance/100.0*idii
+        range = distance*idii/pathstep
-        pos_chk = math.vec2.sub(pt,math.vec2.scale(vec,range));
+        pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
 
         # Find what block we're in right now
-        xi = floor(pos_chk.x)+1
+        xi = math.round(pos_chk.x) #+1
-        yi = floor(pos_chk.y)+1
+        yi = math.round(pos_chk.y) #+1
-        #log("Grid :", pos_chk.x, pos_chk.y, xi, yi)
+        #log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
 
-        if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
+        if(xi!=cur_cell.x and yi!=cur_cell.y){
-            if(rmat(map,xi,yi) == 0) {
+          if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
-                #log("Obstacle (", rmat(map,xi,yi), ")!")
+              if(rmat(map,xi,yi) < 0.5) { # because obstacle use trust values
-                return 1;
+                  #log("Obstacle in the way(", xi, " ", yi, ": ", rmat(map,xi,yi), ")!")
-            }
+                  return 1;
-        } else {
+              }
-            #log("Outside the map(", x, y, ")!")
+          } else {
-            return 1;
+              #log("Outside the map(", xi, " ", yi, ")! ", range, "/", distance, " : ", pos_chk.x, " ", pos_chk.y, " : ", vec.x, " ", vec.y)
+              return 1;
+          }
         }
-        idii = idii + 1
+        idii = idii + 1.0
     }
     #log("No intersect!")
     return 0
 }
 
-function getPath(Q,nb){
+function getPathGPS(Q,nb){
     path={.nb_col=2, .nb_row=0, .mat={}}
     npt=0
     # get the path from the point list
@@ -246,3 +410,30 @@ function getPath(Q,nb){
     }
     return pathR
 }
+
+# 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
+
+    # 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);
+    }
+
+    # log("re-order the list")
+    # table_print(path.mat)
+    pathR={.nb_col=2, .nb_row=path.nb_row, .mat={}}
+    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
+        npt = npt - 1
+    }
+    log("Double-check path: ", check_path(pathR, 1, cur_cell))
+    return pathR
+}

buzz_scripts/include/uavstates.bzz

@@ -3,6 +3,8 @@
 # FLIGHT-RELATED FUNCTIONS
 #
 ########################################
+include "rrtstar.bzz"
+
 TARGET_ALTITUDE = 5.0 # m.
 UAVSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
@@ -11,6 +13,7 @@ GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.5  # m.
 GOTOANG_TOL = 0.1 # rad.
 cur_goal_l = 0
+rc_State = 0
 
 function uav_initswarm() {
 	s = swarm.create(1)
@@ -32,7 +35,8 @@ function takeoff() {
 	statef=takeoff
 
 	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)
+		barrier_set(6, action, land, -1)
 		barrier_ready()
 	} else {
     log("Altitude: ", position.altitude)
@@ -49,7 +53,8 @@ function land() {
 	uav_land()
 	
 	if(flight.status != 2 and flight.status != 3) {
-    barrier_set(ROBOTS,turnedoff,land, 21)
+    	#barrier_set(ROBOTS,turnedoff,land, 21)
+		barrier_set(6,turnedoff,land, 21)
 		barrier_ready()
 	}
 }
@@ -85,7 +90,7 @@ function picture() {
   ptime=ptime+1
 }
 
-function gotoWP(transf) {
+function gotoWPRRT(transf) {
 	rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
 	print(" has to move ", math.vec2.length(rc_goal))
 
@@ -93,7 +98,7 @@ function gotoWP(transf) {
 		log("Sorry this is too far.")
 	else {
 		if(Path==nil){
-			Path = pathPlanner(rc_goal)
+			Path = UAVpathPlanner(rc_goal)
 			cur_goal_l = 1
 		} else if(cur_goal_l<=Path.nb_row) {
 			cur_gps=getvec(Path,cur_goal_l)	#x=latitude, y=longitude
@@ -110,16 +115,21 @@ function gotoWP(transf) {
 			transf()
 		}
 	}
+}
 
-#	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
+function gotoWP(transf) {
-#		log("Sorry this is too far.")
+	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
-#  	else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
+	print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
-#		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
+
-#		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
+	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
-#	} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
+		log("Sorry this is too far.")
-#		transf()
+  else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
-#	else
+		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
-#		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
+		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
+	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
+		transf()
+	else
+		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
 }
 
 function follow() {
@@ -167,6 +177,18 @@ function uav_rccmd() {
 	} else if (flight.rc_cmd==666){
 		flight.rc_cmd=0
 		stattab_send()
+	} else if (flight.rc_cmd==900){
+		flight.rc_cmd=0
+		rc_State = 0
+	} else if (flight.rc_cmd==901){
+		flight.rc_cmd=0
+		rc_State = 1
+	} else if (flight.rc_cmd==902){
+		flight.rc_cmd=0
+		rc_State = 2
+	} else if (flight.rc_cmd==903){
+		flight.rc_cmd=0
+		rc_State = 3
 	}
 }
 
@@ -176,14 +198,22 @@ function uav_neicmd() {
 				print("Got (", vid, ",", value, ") #", rid, "(", UAVSTATE, ")")
 				if(value==22 and UAVSTATE!="TAKEOFF" and UAVSTATE!="BARRIERWAIT") {
 					statef=takeoff
-		      UAVSTATE = "TAKEOFF"
+		      		UAVSTATE = "TAKEOFF"
 				} else if(value==21 and UAVSTATE!="LAND" and UAVSTATE!="BARRIERWAIT") {
 					statef=land
-		      UAVSTATE = "LAND"
+		      		UAVSTATE = "LAND"
 				} else if(value==400 and UAVSTATE=="TURNEDOFF") {
 					uav_arm()
 				} else if(value==401 and UAVSTATE=="TURNEDOFF"){
 					uav_disarm()
+				} else if(value==900){
+					rc_State = 0
+				} else if(value==901){
+					rc_State = 1
+				} else if(value==902){
+					rc_State = 2
+				} else if(value==903){
+					rc_State = 3
 				} else if(value==16 and UAVSTATE=="IDLE"){
 				#  neighbors.listen("gt",function(vid, value, rid) {
 				#    print("Got (", vid, ",", value, ") from robot #", rid)

src/buzz_utility.cpp

@@ -297,9 +297,6 @@ void in_message_process(){
    		buzzvm_gstore(VM);
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "log", 1));
         buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
-        buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "floor", 1));
-        buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzz_floor));
         buzzvm_gstore(VM);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
 		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
@@ -470,9 +467,11 @@ int create_stig_tables() {
 	   	if(VM) buzzvm_destroy(&VM);
 		Robot_id = robot_id;
 	   	VM = buzzvm_new((int)robot_id);
+	   	ROS_INFO(" Robot ID -1: %i" , robot_id);
 	   	/* Get rid of debug info */
 	   	if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
 	   	DBG_INFO = buzzdebug_new();
+	   	ROS_INFO(" Robot ID -2: %i" , robot_id);
 	   	/* Read bytecode and fill in data structure */
 	   	FILE* fd = fopen(bo_filename, "rb");
 	   	if(!fd) {
@@ -491,6 +490,7 @@ int create_stig_tables() {
       		return 0;
 	   	}
 	   	fclose(fd);
+	   	ROS_INFO(" Robot ID -3: %i" , robot_id);
 	   	/* Read debug information */
 	   	if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
 	      		buzzvm_destroy(&VM);
@@ -498,6 +498,7 @@ int create_stig_tables() {
 	      		perror(bdbg_filename);
       		return 0;
    	}
+	   	ROS_INFO(" Robot ID -4: %i" , robot_id);
    	/* Set byte code */
    	if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
@@ -505,6 +506,7 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
       		return 0;
    	}
+	   	ROS_INFO(" Robot ID -5: %i" , robot_id);
    	/* Register hook functions */
    	if(buzz_register_hooks() != BUZZVM_STATE_READY) {
       		buzzvm_destroy(&VM);
@@ -512,6 +514,7 @@ int create_stig_tables() {
       		ROS_ERROR("[%i] Error registering hooks", Robot_id);
       		return 0;
    	}
+	   	ROS_INFO(" Robot ID -6: %i" , robot_id);
 
    	/* Create vstig tables
 	if(create_stig_tables() != BUZZVM_STATE_READY) {

src/buzzuav_closures.cpp

@@ -41,48 +41,52 @@ namespace buzzuav_closures{
 	/****************************************/
 	/****************************************/
 
-	int buzzros_print(buzzvm_t vm) {
+int buzzros_print(buzzvm_t vm)
-	int i;
+{
-	char buffer [100] = "";
+  std::ostringstream buffer (std::ostringstream::ate);
-        sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
+  buffer << buzz_utility::get_robotid();
-	   for(uint32_t i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
+  for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
-	      buzzvm_lload(vm, i);
+  {
-	      buzzobj_t o = buzzvm_stack_at(vm, 1);
+    buzzvm_lload(vm, index);
-	      buzzvm_pop(vm);
+    buzzobj_t o = buzzvm_stack_at(vm, 1);
-	      switch(o->o.type) {
+    buzzvm_pop(vm);
-		 case BUZZTYPE_NIL:
+    switch (o->o.type)
-		    sprintf(buffer,"%s BUZZ - [nil]", buffer);
+    {
-		    break;
+      case BUZZTYPE_NIL:
-		 case BUZZTYPE_INT:
+        buffer << " BUZZ - [nil]";
-		    sprintf(buffer,"%s %d", buffer, o->i.value);
+        break;
-		    //fprintf(stdout, "%d", o->i.value);
+      case BUZZTYPE_INT:
-		    break;
+        buffer << " " << o->i.value;
-		 case BUZZTYPE_FLOAT:
+        break;
-		    sprintf(buffer,"%s %f", buffer, o->f.value);
+      case BUZZTYPE_FLOAT:
-		    break;
+        buffer << " " << o->f.value;
-		 case BUZZTYPE_TABLE:
+        break;
-		    sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
+      case BUZZTYPE_TABLE:
-		    break;
+        buffer << " [table with " << buzzdict_size(o->t.value) << " elems]";
-		 case BUZZTYPE_CLOSURE:
+        break;
-		    if(o->c.value.isnative)
+      case BUZZTYPE_CLOSURE:
-		    	sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
+        if (o->c.value.isnative)
-		    else
+        {
-		    	sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
+          buffer << " [n-closure @" << o->c.value.ref << "]";
-		    break;
+        }
-		 case BUZZTYPE_STRING:
+        else
-		    sprintf(buffer,"%s %s", buffer, o->s.value.str);
+        {
-		    break;
+          buffer << " [c-closure @" << o->c.value.ref << "]";
-		 case BUZZTYPE_USERDATA:
+        }
-		    sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
+        break;
-		    break;
+      case BUZZTYPE_STRING:
-		 default:
+        buffer << "  " << o->s.value.str;
-		    break;
+        break;
-	      }
+      case BUZZTYPE_USERDATA:
-	   }
+        buffer << " [userdata @" << o->u.value << "]";
-	   ROS_INFO(buffer);
+        break;
-	   //fprintf(stdout, "\n");
+      default:
-	   return buzzvm_ret0(vm);
+        break;
-	}
+    }
+  }
+  ROS_INFO("%s",buffer.str().c_str());
+  return buzzvm_ret0(vm);
+}
 
   void setWPlist(string path){
     WPlistname = path + "include/graphs/waypointlist.csv";
@@ -171,14 +175,6 @@ namespace buzzuav_closures{
 	}
 
 
-	int buzz_floor(buzzvm_t vm) {
-		buzzvm_lnum_assert(vm, 1);
-    	buzzvm_lload(vm, 1);
-    	buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-    	float fval = buzzvm_stack_at(vm, 1)->f.value;
-		buzzvm_pushi(vm, floor(fval));
-    	return buzzvm_ret1(vm);
-	}
 	/*----------------------------------------/
 	/ Buzz closure to move following a 2D vector
 	/----------------------------------------*/