From b1197377400415205d20f210e943b9216b41fd10 Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 2 Oct 2017 12:47:02 -0400 Subject: [PATCH] RRT* bzz and rosbuzz floor fct --- buzz_scripts/graphformGPS.bzz | 10 +- buzz_scripts/include/mapmatrix.bzz | 93 +++++++++++ buzz_scripts/include/rrtstar.bzz | 248 +++++++++++++++++++++++++++++ buzz_scripts/include/rttstar.bzz | 233 --------------------------- buzz_scripts/include/uavstates.bzz | 61 ++++--- buzz_scripts/testalone.bzz | 8 +- include/buzzuav_closures.h | 1 + src/buzz_utility.cpp | 3 + src/buzzuav_closures.cpp | 9 ++ 9 files changed, 405 insertions(+), 261 deletions(-) create mode 100644 buzz_scripts/include/mapmatrix.bzz create mode 100644 buzz_scripts/include/rrtstar.bzz delete mode 100644 buzz_scripts/include/rttstar.bzz diff --git a/buzz_scripts/graphformGPS.bzz b/buzz_scripts/graphformGPS.bzz index f3b0bd5..01fc3f9 100644 --- a/buzz_scripts/graphformGPS.bzz +++ b/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 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 + } +} \ No newline at end of file diff --git a/buzz_scripts/include/rrtstar.bzz b/buzz_scripts/include/rrtstar.bzz new file mode 100644 index 0000000..f37fe10 --- /dev/null +++ b/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 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 +} \ No newline at end of file diff --git a/buzz_scripts/include/rttstar.bzz b/buzz_scripts/include/rttstar.bzz deleted file mode 100644 index cfd4924..0000000 --- a/buzz_scripts/include/rttstar.bzz +++ /dev/null @@ -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 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 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 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(iGOTO_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 - 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) + else { + if(Path==nil){ + Path = pathPlanner(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 + 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 + 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) { -# print("gps2rb d: ",lat,lon) -# print("gps2rb h: ",position.latitude,position.longitude) +function vec_from_gps(lat, lon) { 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); - goal.bearing = LimitAngle(math.atan(ned_y,ned_x)); + #Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); + #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; - goal.longitude = target_lon*180.0/math.pi; + Lgoal.latitude = target_lat*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 } diff --git a/buzz_scripts/testalone.bzz b/buzz_scripts/testalone.bzz index 215497e..bba5d7a 100644 --- a/buzz_scripts/testalone.bzz +++ b/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_moveto(0.5, 0.5, 0.0) + uav_storegoal(45.5085,-73.1531935979886,5.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. diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index eeb9184..6a5d18a 100644 --- a/include/buzzuav_closures.h +++ b/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); diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 1a02c36..947136e 100644 --- a/src/buzz_utility.cpp +++ b/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)); diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 5083dd1..68b0065 100644 --- a/src/buzzuav_closures.cpp +++ b/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 /----------------------------------------*/