RRT* bzz and rosbuzz floor fct

buzz_scripts/graphformGPS.bzz

@@ -8,6 +8,7 @@ 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_L.bzz"
 
@@ -178,14 +179,14 @@ function r2i(value){
 #return the index of value
 #
 function find(table,value){
-	var index=nil
+	var ind=nil
 	var i=0
 	while(i<size(table)){
 		if(table[i]==value)
-			index=i
+			ind=i
 		i=i+1
 	}
-	return index
+	return ind
 }
 
 #
@@ -578,7 +579,8 @@ function set_rc_goto() {
     var S2Pred=math.vec2.newp(m_cMeToPred.Range,m_cMeToPred.Bearing)
     var S2Target=math.vec2.add(S2Pred,P2Target)
 
-    gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
+    goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
+	print("Saving GPS goal: ",goal.latitude, goal.longitude)
     uav_storegoal(goal.latitude, goal.longitude, position.altitude)
     m_gotjoinedparent = 1
   }

buzz_scripts/include/mapmatrix.bzz

@@ -0,0 +1,93 @@
+
+# Write to matrix
+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) {
+        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]
+        icr = icr + 1
+    }
+}
+
+function getvec(t,row){
+    return math.vec2.new(rmat(t,row,1),rmat(t,row,2))
+}
+
+function init_test_map(){
+    # creates a 10x10 map
+    map = {.nb_col=10, .nb_row=10, .mat={}}
+    cur_cell = math.vec2.new(1,1)
+    index = 0
+    while(index<10*10){
+        map.mat[index]=1
+        index = index + 1
+    }
+    # puts an obstacle right in the middle
+    wmat(map,5,5,0)
+    wmat(map,6,5,0)
+    wmat(map,4,5,0)
+
+    log(rmat(map,1,1),rmat(map,3,3))
+}
+
+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
+        index = index + 1
+    }
+    log("Occupancy grid initialized (",len,"x",len,").")
+}
+
+function add_obstacle(pos) {
+    xi = floor(pos.x)+cur_cell.x
+    yi = floor(pos.y)+cur_cell.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){
+        wmat(map,xi,yi,0)
+        wmat(map,xi+1,yi,0)
+        wmat(map,xi-1,yi,0)
+        wmat(map,xi,yi+1,0)
+        wmat(map,xi,yi-1,0)
+    }
+}
+
+function table_print(t) {
+  foreach(t, function(key, value) {
+      log(key, " -> ", value)
+    })
+}
+function print_pos(t) {
+  ir=1
+  while(ir<=t.nb_row){
+      log(ir, ": ", rmat(t,ir,1), rmat(t,ir,2))
+      ir = ir + 1
+  }
+}

buzz_scripts/include/rrtstar.bzz

@@ -0,0 +1,248 @@
+#####
+# RRT* Path Planing
+#
+# map table-based matrix
+#####
+include "mapmatrix.bzz"
+
+map = {}
+cur_cell = {}
+
+function pathPlanner(m_navigation) {
+    # create a map big enough for the goal
+    init_map(2*floor(math.vec2.length(m_navigation))+2)
+    # 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))
+            })
+    # 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))
+}
+
+function RRTSTAR(GOAL,TOL) {
+    HEIGHT = map.nb_col
+    WIDTH = map.nb_row
+    RADIUS = 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)
+    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}}
+    
+    goalReached = 0;
+    timeout = 0
+    ##
+    # main search loop
+    ##
+    while(goalReached == 0 and timeout < 200) {
+        # Point generation
+        pt = math.vec2.new(HEIGHT*rng.uniform(1.0)+1,WIDTH*rng.uniform(1.0)+1)
+
+        pointList = findPointsInRadius(pt,Q,RADIUS);
+
+        # Find connection that provides the least cost to come
+        nbCount = 0;
+        minCounted = 999;
+        minCounter = 0;
+        
+        if(pointList.nb_row!=0) {
+            #log("Found ", pointList.nb_row, " close point:", pointList.mat)    
+            ipt=1
+            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));
+                #log("intersects1: ", intersects)
+
+                # If there is no intersection we need consider its connection
+                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)
+
+                    if(distance < minCounted) {
+                        minCounted = distance;
+                        minCounter = nbCount;
+                    }
+                }
+                ipt = ipt + 1
+            }
+            if(minCounter > 0) {
+                numberOfPoints = numberOfPoints + 1;
+                wmat(arrayOfPoints,numberOfPoints,1,pt.x)
+                wmat(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)
+
+                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;
+                ipt = 1
+                while(ipt<pointList.nb_row) {
+                    pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
+                    mat_copyrow(pointNumber,1,pointList,ipt)
+
+                    # Follow the line to see if it intersects anything
+                    intersects = doesItIntersect(pt,getvec(pointNumber,1));
+                    #log("intersects2: ", intersects)
+
+                    # If there is no intersection we need consider its connection
+                    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)
+                        }
+                    }
+                    ipt = ipt + 1
+                }
+
+                # 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)
+                    goalReached = 1;
+            }
+        } else {
+            # Associate with the closest point
+            pointNum = findClosestPoint(pt,arrayOfPoints);
+
+            # Follow the line to see if it intersects anything
+            intersects = doesItIntersect(pt,getvec(arrayOfPoints,pointNum));
+            #log("intersects3 (", pointNum, "): ", intersects)
+
+            # 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)
+
+                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)))
+
+                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)
+                    goalReached = 1;
+            }
+        }
+        if(numberOfPoints % 100 == 0) {
+            log(numberOfPoints, " points processed. Still looking for goal.");
+        }
+        timeout = timeout + 1
+    }
+    if(goalReached){
+        log("Goal found(",numberOfPoints,")!")
+        Path = getPath(Q,numberOfPoints)
+        print_pos(Path)
+    }
+    return Path
+}
+
+function findClosestPoint(point,aPt) {
+    # Go through each points and find the distances between them and the
+    # target point
+    distance = 999
+    pointNumber = -1
+    ifcp=1
+    while(ifcp<=aPt.nb_row) {
+        range = math.vec2.length(math.vec2.sub(point,getvec(aPt,ifcp)))
+        
+        if(range < distance) {
+            distance = range;
+            pointNumber = ifcp;
+        }
+        ifcp = ifcp + 1
+    }
+    return pointNumber
+}
+
+function findPointsInRadius(point,q,r) {
+    counted = 0;
+    pointList = {.nb_col=q.nb_col, .nb_row=counted, .mat={}}
+    iir=1
+    while(iir <= q.nb_row) {
+        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)
+        }
+
+        iir = iir + 1
+    }
+    return pointList
+}
+
+function doesItIntersect(point,vector) {
+    dif = math.vec2.sub(point,vector)
+    distance = math.vec2.length(dif)
+    vec = math.vec2.scale(dif,1/distance)
+    
+    idii = 1
+    while(idii<=100) {
+        range = distance/100.0*idii
+        pos_chk = math.vec2.sub(pt,math.vec2.scale(vec,range));
+        
+        # Find what block we're in right now
+        xi = floor(pos_chk.x)+1
+        yi = floor(pos_chk.y)+1
+        #log("Grid :", pos_chk.x, pos_chk.y, xi, yi)
+        
+        if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
+            if(rmat(map,xi,yi) == 0) {
+                #log("Obstacle (", rmat(map,xi,yi), ")!")
+                return 1;
+            }
+        } else {
+            #log("Outside the map(", x, y, ")!")
+            return 1;
+        }
+        idii = idii + 1
+    }
+    #log("No intersect!")
+    return 0
+}
+
+function getPath(Q,nb){
+    path={.nb_col=2, .nb_row=0, .mat={}}
+    npt=0
+    # get the path from the point list
+    while(nb!=1){
+        npt=npt+1
+        path.nb_row=npt
+        path.mat[npt*2]=rmat(Q,nb,1)
+        path.mat[npt*2+1]=rmat(Q,nb,2)
+        nb = rmat(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){
+        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
+        npt = npt - 1
+    }
+    return pathR
+}

buzz_scripts/include/rttstar.bzz

@@ -1,233 +0,0 @@
-#####
-# RRT* Path Planing
-# START, GOAL, TOL vec2
-# map table-based matrix
-#####
-
-map = {.nb_col=100, .nb_row=100, .mat={}}
-
-function RRTSTAR(START,GOAL,TOL) {
-    # RRT_STAR
-    HEIGHT = map.nb_col
-    WIDTH = map.nb_row
-    RADIUS = 2
-    
-    rng.setseed(11)
-
-    goalBoundary = {.xmin=GOAL.x-TOL.x, .xmax=GOAL.x+TOL.x, .ymin=GOAL.y-TOL.y, .ymax=GOAL.y+TOL.y}
-    arrayOfPoints = {.nb_col=2, .nb_row=1, .mat={.0=START.x,.1=START.y}}
-    numberOfPoints = 1
-    Q = {.nb_col=5,.nb_row=1,.mat={.0=START.x,.1=START.y,.2=0,.3=1,.4=0}}
-    log("Created ",Q)
-
-    # Open space or obstacle
-    # DISPLAY_patchwork(map);
-    
-    goalReached = 0;
-    while(goalReached == 0) {
-        # Point generation
-        x = -HEIGHT*rng.uniform(1.0)-1;
-        y = WIDTH*rng.uniform(1.0)+1;
-        log("First trial point (", rng.uniform(1.0), "): ", x, " ", y)
-
-        pointList = findPointsInRadius(x,y,Q,RADIUS);
-        
-        log(pointList.nb_col,pointList.nb_row,pointList.mat)
-
-        # Find connection that provides the least cost to come
-        nbCount = 0;
-        minCounted = inf;
-        minCounter = 0;
-        
-        if(pointList.nb_col!=0) {
-            i=0
-            while(i<pointList.nb_row){
-                pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
-                mat_copyrow(pointNumber,0,pointList,i)
-
-                # Follow the line to see if it intersects anything
-                intersects = doesItIntersect(x,y,math.vec2.new(pointNumber.mat[0],pointNumber.mat[1]),map);
-
-                # If there is no intersection we need consider its connection
-                nbCount = nbCount + 1;
-                if(intersects != 1) {
-                    log(pointNumber, "do not intersect (",pointNumber.mat[3],")")
-                    distance = math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y)+rmap(Q,pointNumber.mat[3],5)
-
-                    if(distance < minCounted) {
-                        minCounted = distance;
-                        minCounter = nbCount;
-                    }
-                }
-            }
-            if(minCounter > 0) {
-                arrayOfPoints.nb_row=arrayOfPoints.nb_row+1
-                arrayOfPoints.mat[arrayOfPoints.nb_row]=x
-                arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y
-                numberOfPoints = numberOfPoints + 1;
-
-                wmat(Q,numberOfPoints,0, x)
-                wmat(Q,numberOfPoints,1, y)
-                wmat(Q,numberOfPoints,2, rmat(pointList,minCounter,4));
-                wmat(Q,numberOfPoints,3, numberOfPoints)
-                wmat(Q,numberOfPoints,4, minCounted)
-
-                # Now check to see if any of the other points can be redirected
-                nbCount = 0;
-                i = 0
-                while(i<pointList.nb_col) {
-                    pointNumber = {.nb_col=pointList.nb_col, .nb_row=1, .mat={}}
-                    mat_copyrow(pointNumber,0,pointList,i)
-
-                    # Follow the line to see if it intersects anything
-                    intersects = doesItIntersect(x,y,math.vec2.new(pointNumber.mat[0],pointNumber.mat[1]),map);
-
-                    # If there is no intersection we need consider its connection
-                    nbCount = nbCount + 1;
-                    if(intersects != 1) {
-                        # If the alternative path is shorter than change it
-                        tmpdistance = math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y)+rmap(Q,numberOfPoints,5)
-                        if(tmpdistance < rmap(Q,pointNumber.mat[3],5)) {
-                            wmat(Q,pointNumber.mat[3],3, numberOfPoints)
-                            wmat(Q,pointNumber.mat[3],5, rmap(Q,numberOfPoints,5)+math.vec2.length(pointNumber.mat[0]-x,pointNumber.mat[1]-y))
-                        }
-                    }
-                }
-
-                # Check to see if this new point is within the goal
-                if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax)
-                    goalReached = 1;
-            }
-        } else {
-            # Associate with the closest point
-            pointNum = findClosestPoint(x,y,arrayOfPoints);
-
-            # Follow the line to see if it intersects anything
-            intersects = doesItIntersect(x,y,math.vec2.new(rmat(arrayOfPoints,1,pointNum),rmat(arrayOfPoints,2,pointNum)),map);
-
-            # If there is no intersection we need to add to the tree
-            if(intersects != 1) {
-                arrayOfPoints.nb_row=arrayOfPoints.nb_row+1
-                arrayOfPoints.mat[arrayOfPoints.nb_row]=x
-                arrayOfPoints.mat[arrayOfPoints.nb_row+1]=y
-                numberOfPoints = numberOfPoints + 1;
-
-                wmat(Q,numberOfPoints,0, x)
-                wmat(Q,numberOfPoints,1, y)
-                wmat(Q,numberOfPoints,2, pointNum);
-                wmat(Q,numberOfPoints,3, numberOfPoints)
-                wmat(Q,numberOfPoints,4, rmat(Q,pointNum,5)+math.vec2.length(rmat(Q,pointNum,1)-x,rmat(Q,pointNum,2)-y))
-
-                # Check to see if this new point is within the goal
-                if(x < goalBoundary.xmax and x > goalBoundary.xmin and y > goalBoundary.ymin and y < goalBoundary.ymax)
-                    goalReached = 1;
-            }
-        }
-        if(numberOfPoints % 100 == 0) {
-            log(numberOfPoints, " points processed. Still looking for goal.");
-        }
-    }
-    log("Goal found!");
-}
-
-function findClosestPoint(x,y,arrayOfPoints) {
-    # Go through each points and find the distances between them and the
-    # target point
-    distance = inf;
-    i=1
-    while(i<arrayOfPoints.nb_row) {
-        range = (x-rmat(arrayOfPoints,1,i))^2+(y-rmat(arrayOfPoints,2,i))^2;
-        
-        if(range < distance) {
-            distance = range;
-            pointNumber = i;
-        }
-        i = i + 1
-    }
-    return pointNumber
-}
-
-function findPointsInRadius(x,y,arrayOfPoints,RADIUS) {
-    counted = 0;
-    pointList = {.nb_col=arrayOfPoints.nb_col, .nb_row=counted, .mat={}}
-    i=1
-    while(i < arrayOfPoints.nb_row){
-        distance = math.vec2.length(rmat(arrayOfPoints,i,1)-x,rmat(arrayOfPoints,i,2)-y);
-        
-        if(distance < RADIUS){
-            counted = counted+1;
-            pointList.nb_row=counted
-            mat_copyrow(pointList,counted,arrayOfPoints,i)
-        }
-        i = i + 1
-    }
-    return pointList
-}
-
-function doesItIntersect(x,y,vector) {
-    x = -x;
-    vector.x = -vector.x;
-    distance = math.vec2.length(vector);
-    vec = math.vec2.scale(math.vec2.sub(math.vec2.new(x,y),vector),1/distance);
-    log("doesItIntersect: ",vec)
-    #range = linspace(0,distance,distance*100);
-    i = 1;
-    #plot(y,-x,'b*');
-    while(i<100) {
-        if(block == 0) {
-            range = distance/100.0*i
-            position = math.vec2.sub(math.vec2.new(x,y),math.vec2.scale(vec,range));
-            
-            # Find what block we're in right now
-            xi = math.floor(position.x)
-            yi = math.floor(position.y)
-            
-            if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
-                if(rmap(xi,yi) == 0)
-                    return 1;
-            } else
-                return 1;
-        }
-        i = i + 1
-    }
-    return 0
-}
-
-# Write to matrix
-function wmat(mat, row, col, val) {
-    var index = (row-1)*mat.nb_col + col
-    mat.mat[index] = val
-}
-
-# Read from matrix
-function rmat(mat, row, col) {
-    var index = (row-1)*mat.nb_col + col
-    if (mat.mat[index] == nil) {
-        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
-    i=0
-    while(i<in.nb_col){
-        out.mat[indexO+i]=in.mat[indexI+i]
-        i = i + 1
-    }
-}
-
-function make_test_map(){
-    # creates a 5x5 map
-    map = {.nb_col=5, .nb_row=5, .mat={}}
-    index = 0
-    while(index<25){
-        map.mat[index]=0
-        index = index + 1
-    }
-    # puts an obstacle right in the middle
-    map.mat[5*2+3]=1
-}

buzz_scripts/include/uavstates.bzz

@@ -10,7 +10,7 @@ GOTO_MAXVEL = 2 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.5  # m.
 GOTOANG_TOL = 0.1 # rad.
-goal = {.range=0, .bearing=0, .latitude=0, .longitude=0}
+cur_goal_l = 0
 
 function uav_initswarm() {
 	s = swarm.create(1)
@@ -86,19 +86,40 @@ function picture() {
 }
 
 function gotoWP(transf) {
-	rb_from_gps(rc_goto.latitude, rc_goto.longitude)
+	rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
-	print(" has to move ", goal.range, goal.bearing)
+	print(" has to move ", math.vec2.length(rc_goal))
-	m_navigation = math.vec2.newp(goal.range, goal.bearing)
 
-	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
+	if(math.vec2.length(rc_goal)>GOTO_MAXDIST*5)
 		log("Sorry this is too far.")
-  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))
+		if(Path==nil){
-		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
+			Path = pathPlanner(rc_goal)
-	} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
+			cur_goal_l = 1
-		transf()
+		} else if(cur_goal_l<=Path.nb_row) {
+			cur_gps=getvec(Path,cur_goal_l)	#x=latitude, y=longitude
+			cur_pt=vec_from_gps(cur_gps.x,cur_gps.y)
+			print(" first to ", cur_pt.x,cur_pt.y)
+			if(math.vec2.length(cur_pt)>GOTODIST_TOL) {
+				cur_pt = math.vec2.scale(cur_pt, GOTO_MAXVEL/math.vec2.length(cur_pt))
+				uav_moveto(cur_pt.x, cur_pt.y, rc_goto.altitude-position.altitude)
+			}
 			else
-		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
+				cur_goal_l = cur_goal_l + 1
+		} else {
+			Path = nil
+			transf()
+		}
+	}
+
+#	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
+#		log("Sorry this is too far.")
+#  	else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
+#		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)
+#	} else if(goal.range < GOTODIST_TOL and goal.bearing < GOTOANG_TOL) # reached destination
+#		transf()
+#	else
+#		uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
 }
 
 function follow() {
@@ -181,18 +202,18 @@ function LimitAngle(angle){
 		return angle
 }
 
-function rb_from_gps(lat, lon) {
+function vec_from_gps(lat, lon) {
-#  print("gps2rb d: ",lat,lon)
-#  print("gps2rb h: ",position.latitude,position.longitude)
 	d_lon = lon - position.longitude
   d_lat = lat - position.latitude
   ned_x = d_lat/180*math.pi * 6371000.0;
   ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
-	goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
+	#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
-	goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
+	#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x));
+	return math.vec2.new(ned_x,ned_y)
 }
 
 function gps_from_vec(vec) {
+  Lgoal = {.latitude=0, .longitude=0}
   Vrange = math.vec2.length(vec)
   Vbearing = LimitAngle(math.atan(vec.y, vec.x))
 #  print("rb2gps: ",Vrange,Vbearing,position.latitude,position.longitude)
@@ -200,10 +221,12 @@ function gps_from_vec(vec) {
   lonR = position.longitude*math.pi/180.0;
   target_lat = math.asin(math.sin(latR) * math.cos(Vrange/6371000.0) + math.cos(latR) * math.sin(Vrange/6371000.0) * math.cos(Vbearing));
   target_lon = lonR + math.atan(math.sin(Vbearing) * math.sin(Vrange/6371000.0) * math.cos(latR), math.cos(Vrange/6371000.0) - math.sin(latR) * math.sin(target_lat));
-  goal.latitude = target_lat*180.0/math.pi;
+  Lgoal.latitude = target_lat*180.0/math.pi;
-  goal.longitude = target_lon*180.0/math.pi;
+  Lgoal.longitude = target_lon*180.0/math.pi;
   #d_lat = (vec.x / 6371000.0)*180.0/math.pi;
   #goal.latitude = d_lat + position.latitude;
   #d_lon = (vec.y / (6371000.0 * math.cos(goal.latitude*math.pi/180.0)))*180.0/math.pi;
   #goal.longitude = d_lon + position.longitude;
+
+  return Lgoal
 }

buzz_scripts/testalone.bzz

@@ -3,12 +3,12 @@ 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 "rttstar.bzz"
+include "rrtstar.bzz"
 
 function action() {
   statef=action
-# test moveto cmd dx, dy
+  uav_storegoal(45.5085,-73.1531935979886,5.0)
-#	uav_moveto(0.5, 0.5, 0.0)
+  set_goto(picture)
 }
 
 # Executed once at init time.
@@ -16,8 +16,6 @@ function init() {
 	statef=turnedoff
 	UAVSTATE = "TURNEDOFF"
   	uav_initstig()
-	make_test_map()
-	RRTSTAR(math.vec2.new(0,0),math.vec2.new(5,5),math.vec2.new(1,1))
 }
 
 # Executed at each time step.

include/buzzuav_closures.h

@@ -38,6 +38,7 @@ void setWPlist(std::string path);
  * buzzuav_goto(latitude,longitude,altitude) function in Buzz
  * commands the UAV to go to a position supplied
  */
+int buzz_floor(buzzvm_t vm);
 int buzzuav_moveto(buzzvm_t vm);
 int buzzuav_storegoal(buzzvm_t vm);
 int buzzuav_setgimbal(buzzvm_t vm);

src/buzz_utility.cpp

@@ -297,6 +297,9 @@ 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));

src/buzzuav_closures.cpp

@@ -170,6 +170,15 @@ namespace buzzuav_closures{
 		fin.close();
 	}
 
+
+	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
 	/----------------------------------------*/