Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

RRT* bzz and rosbuzz floor fct

This commit is contained in:
dave 2017-10-02 12:47:02 -04:00
parent 37c3cf151b
commit b119737740
9 changed files with 405 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
buzz_scripts/graphformGPS.bzz
View File

@ -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
}

93
buzz_scripts/include/mapmatrix.bzz Normal file
View File

@ -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
}
}

248
buzz_scripts/include/rrtstar.bzz Normal file
View File

@ -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
}

233
buzz_scripts/include/rttstar.bzz
View File

@ -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
}

61
buzz_scripts/include/uavstates.bzz
View File

@ -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)
print(" has to move ", goal.range, goal.bearing)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
rc_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
print(" has to move ", math.vec2.length(rc_goal))
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
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
}

8
buzz_scripts/testalone.bzz
View File

@ -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.

1
include/buzzuav_closures.h
View File

@ -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);

3
src/buzz_utility.cpp
View File

@ -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));

9
src/buzzuav_closures.cpp
View File

@ -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
/----------------------------------------*/