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

added Hull polygon from user inputs and minor enhancement to GIS and state machine

This commit is contained in:
dave 2018-11-11 15:11:41 -05:00
parent 4ad8ecf443
commit b3f9b7cf4e
14 changed files with 630 additions and 419 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
buzz_scripts/include/act/barrier.bzz
View File

@ -9,7 +9,7 @@
#
BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80
timeW = 0
timeW = 1
barrier = nil
hvs = 0;
@ -19,7 +19,7 @@ hvs = 0;
#
function barrier_create() {
# reset
timeW = 0
timeW = 1
# create barrier vstig
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) {
@ -61,7 +61,6 @@ function barrier_wait(threshold, transf, resumef, bc) {
barrier_create()
barrier.put(id, bc)
barrier.get(id)
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
#if(barrier.size()-1 >= threshold or barrier.get("d") == 1) {
if(barrier_allgood(barrier,bc)) {

4
buzz_scripts/include/act/naviguation.bzz
View File

@ -2,7 +2,7 @@ GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.4 # m.
GOTOANG_TOL = 0.1 # rad.
GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
GPSlimit = {.1={.lat=45.510523, .lng=-73.611118},#{.lat=45.510400, .lng=-73.610421},
.2={.lat=45.510896, .lng=-73.608731},
.3={.lat=45.510355, .lng=-73.608404},
.4={.lat=45.509840, .lng=-73.610072}}
@ -18,7 +18,7 @@ function goto_gps(transf) {
} else {
m_navigation = LimitSpeed(m_navigation, 1.0)
gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)}
geofence(gf)
#geofence(gf)
#m_navigation = LCA(m_navigation)
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
}

95
buzz_scripts/include/act/neighborcomm.bzz
View File

@ -1,13 +1,26 @@
# listens to commands from the remote control (web, commandline, rcclient node, etc)
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
log("cmd 22")
if(BVMSTATE=="TURNEDOFF") {
if(flight.rc_cmd==400) { #ARM
flight.rc_cmd=0
arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){ #DISARM
flight.rc_cmd=0
disarm()
neighbors.broadcast("cmd", 401)
} else if(flight.rc_cmd==22) { #TAKEOFF\LAUNCH
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
} else if (flight.rc_cmd==777){ #SYNC
flight.rc_cmd=0
reinit_time_sync()
neighbors.broadcast("cmd", 777)
}
} else {
if(flight.rc_cmd==21) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "TURNEDOFF"
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
#barrier_ready(21)
BVMSTATE = "STOP"
@ -18,25 +31,11 @@ function rc_cmd_listen() {
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
barrier_ready(20)
neighbors.broadcast("cmd", 20)
# } else if(flight.rc_cmd==16) {
# flight.rc_cmd=0
# BVMSTATE = "PATHPLAN"
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
disarm()
neighbors.broadcast("cmd", 401)
} else if (flight.rc_cmd==666){
} else if(BVMSTATE!="BARRIERWAIT" and BVMSTATE!="LAUNCH") {
if (flight.rc_cmd==666){
flight.rc_cmd=0
stattab_send()
} else if (flight.rc_cmd==777){
flight.rc_cmd=0
reinit_time_sync()
neighbors.broadcast("cmd", 777)
}else if (flight.rc_cmd==900){
} else if (flight.rc_cmd==900){
flight.rc_cmd=0
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
barrier_ready(900)
@ -44,12 +43,14 @@ function rc_cmd_listen() {
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
resetWP()
barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
resetWP()
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
@ -67,6 +68,8 @@ function rc_cmd_listen() {
barrier_ready(904)
neighbors.broadcast("cmd", 904)
}
}
}
}
# listens to neighbors broadcasting commands
@ -74,53 +77,52 @@ function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
#if(BVMSTATE!="BARRIERWAIT") {
if(value==22 and BVMSTATE=="TURNEDOFF") {
if(BVMSTATE=="TURNEDOFF") {
if(value==22) {
BVMSTATE = "LAUNCH"
}else if(value==20) {
} else if(value==400) {
arm()
} else if(value==401){
disarm()
} else if(value==777){
reinit_time_sync()
}
} else {
if(value==20) {
AUTO_LAUNCH_STATE = "IDLE"
BVMSTATE = "GOHOME"
} else if(value==21 and BVMSTATE!="TURNEDOFF") {
} else if(value==21 ) {
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
arm()
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
disarm()
} else if(value==777 and BVMSTATE=="TURNEDOFF"){
reinit_time_sync()
#neighbors.broadcast("cmd", 777)
}else if(value==900){ # Shapes
}else if(BVMSTATE!="BARRIERWAIT" and BVMSTATE!="LAUNCH") {
if(value==900){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
#neighbors.broadcast("cmd", 900)
} else if(value==901 and BVMSTATE!="BARRIERWAIT"){ # Pursuit
} else if(value==901 and BVMSTATE!="DEPLOY" ){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
#barrier_ready(901)
#neighbors.broadcast("cmd", 901)
} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # Waypoint
} else if(value==902 and BVMSTATE!="WAYPOINT" and BVMSTATE!="TURNEDOFF"){ # Waypoint
destroyGraph()
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
#barrier_ready(902)
#neighbors.broadcast("cmd", 902)
} else if(value==903 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="POTENTIAL"){ # Formation
} else if(value==903 and BVMSTATE!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){ # Formation
destroyGraph()
resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
#barrier_ready(903)
#neighbors.broadcast("cmd", 903)
} else if(value==904 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="IDLE"){ # idle
} else if(value==904 and BVMSTATE!="IDLE" and BVMSTATE!="TURNEDOFF"){ # idle
destroyGraph()
resetWP()
barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
#barrier_ready(904)
#neighbors.broadcast("cmd", 904)
} else if(value==16 and BVMSTATE=="IDLE"){
# neighbors.listen("gt",function(vid, value, rid) {
# print("Got (", vid, ",", value, ") from robot #", rid)
# # if(gt.id == id) statef=goto
# })
}
#}
}
}
})
}
@ -141,7 +143,8 @@ function check_rc_wp() {
v_wp.put(rc_goto.id,ls)
reset_rc()
}
}
} else
v_wp.get(0)
}
function resetWP() {

86
buzz_scripts/include/act/states.bzz
View File

@ -6,6 +6,7 @@
include "utils/vec2.bzz"
include "act/barrier.bzz"
include "utils/conversions.bzz"
include "utils/quickhull.bzz"
include "act/naviguation.bzz"
include "act/CA.bzz"
include "act/neighborcomm.bzz"
@ -32,6 +33,7 @@ function idle() {
# Core state function to launch the robot: takeoff and wait for others, or stop (land)
function launch() {
BVMSTATE = "LAUNCH"
log("AUTO_LAUNCH_STATE: ", AUTO_LAUNCH_STATE)
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
@ -71,11 +73,13 @@ gohomeT=100
# State function to go back to ROSBuzz recorded home GPS position (at takeoff)
function goinghome() {
BVMSTATE = "GOHOME"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
if(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
} else
BVMSTATE = AUTO_LAUNCH_STATE
}
}
# Core state function to stop and land.
@ -85,14 +89,14 @@ function stop() {
neighbors.broadcast("cmd", 21)
uav_land()
if(pose.position.altitude <= 0.2) {
BVMSTATE = "TURNEDOFF"
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21)
BVMSTATE = "STOP"
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
} else {
BVMSTATE = "TURNEDOFF"
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21)
BVMSTATE = "STOP"
barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
barrier_ready(21)
}
}
@ -214,6 +218,8 @@ function lj_sum(rid, data, accum) {
function lennardjones() {
BVMSTATE="POTENTIAL"
check_rc_wp()
if(V_TYPE == 2) # NOT MOVING!
return
# Calculate accumulator
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0)
@ -243,22 +249,54 @@ function lennardjones() {
counter = 0
function voronoicentroid() {
BVMSTATE="DEPLOY"
if(counter==0 and id!=0) {
pts = getbounds()
pts[0] = {.x=0, .y=0} #add itself
it_pts = 1
#table_print(pts)
check_rc_wp()
log("V_wp size:",v_wp.size())
if(V_TYPE == 2) # NOT MOVING!
return
if(not(v_wp.size() > 0))
return
it_pts = 0
att = {}
v_wp.foreach(
function(key, value, robot){
wp = unpackWP2i(value)
if(key > 99)
log("Nothing planed for the repulsors yet....")
else if(key > 49)
att[it_pts]=vec_from_gps(wp.lat, wp.lon, 0)
it_pts = it_pts + 1
})
# Boundaries from Geofence
#it_pts = 0
#foreach(GPSlimit, function(key, value) {
# bounds[it_pts]=vec_from_gps(value.lat, value.lng, 0)
# it_pts = it_pts + 1
#})
# Boundaries from user attractors
#att = {.0=vec_from_gps(45.510283, -73.609633, 0), .1=vec_from_gps(45.510398, -73.609281, 0)}
bounds = QuickHull(att)
if(size(bounds)<3 )
return
if(counter==0) {
pts = {.np=size(bounds)}
it_pts = 0
foreach(bounds, function(key, value) {
pts[it_pts]=value
it_pts = it_pts + 1
})
pts[it_pts] = {.x=0, .y=0} #add itself
it_pts = it_pts + 1
if(neighbors.count() > 0) {
neighbors.foreach(function(rid, data) {
if(rid!=0){ #remove GS (?)
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth)
it_pts = it_pts + 1
}
})
#table_print(pts)
voronoi(pts)
}
counter=10
counter=4
}
counter=counter-1
@ -269,28 +307,6 @@ function voronoicentroid_done() {
BVMSTATE="DEPLOY"
}
function getbounds(){
var RBlimit = {}
foreach(GPSlimit, function(key, value) {
RBlimit[key] = vec_from_gps(value.lat, value.lng, 0)
})
minY = RBlimit[1].y
maxY = RBlimit[1].y
minX = RBlimit[1].x
maxX = RBlimit[1].x
foreach(RBlimit, function(key, value) {
if(value.y<minY)
minY = value.y
if(value.y>maxY)
maxY = value.y
if(value.x>maxX)
maxX = value.x
if(value.x<minX)
minX = value.x
})
return {.miny=minY, .minx=minX, .maxy=maxY, .maxx=maxX, .oa=0.0}
}
# Custom state function for the developer to play with
function cusfun(){
BVMSTATE="CUSFUN"

22
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -110,7 +110,7 @@ function r2i(value){
#
function find(table,value){
var ind=nil
var i=0
i=0
while(i<size(table)){
if(table[i]==value)
ind=i
@ -187,7 +187,7 @@ function unpack_guide_msg(recv_value){
#
function target4label(nei_id){
var return_val="miss"
var i=0
i=0
while(i<size(lock_neighbor_id)){
if(lock_neighbor_id[i]==nei_id){
return_val=lock_neighbor_dis[i]
@ -216,7 +216,7 @@ function LJ_vec(i){
#calculate the motion vector
#
function motion_vector(){
var i=0
i=0
var m_vector={.x=0.0,.y=0.0}
while(i<m_neighbourCount){
#calculate and add the motion vector
@ -272,7 +272,7 @@ function Get_DisAndAzi(t_id){
function UpdateNodeInfo(){
#Collect informaiton
#Update information
var i=0
i=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="GRAPH_JOINED"){
@ -316,8 +316,8 @@ function DoFree() {
#find a set of joined robots
var setJoinedLabels={}
var setJoinedIndexes={}
var i=0
var j=0
i=0
j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="GRAPH_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
@ -367,7 +367,7 @@ function DoAsking(){
}else {
UpdateGraph()
#look for response from predecessor
var i=0
i=0
var psResponse=-1
while(i<m_neighbourCount and psResponse==-1){
#the respond robot in joined state
@ -448,7 +448,7 @@ function DoJoining(){
function set_rc_goto() {
#get information of pred
var i=0
i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="GRAPH_JOINED")
@ -497,8 +497,8 @@ function DoJoined(){
#collect all requests
var mapRequests={}
var i=0
var j=0
i=0
j=0
var ReqLabel
var JoiningLabel
var seenPred=0
@ -588,7 +588,7 @@ function DoLock() {
#record neighbor distance
lock_neighbor_id={}
lock_neighbor_dis={}
var i=0
i=0
while(i<m_neighbourCount){
lock_neighbor_id[i]=m_messageID[i]
lock_neighbor_dis[i]=m_MessageRange[i]

122
buzz_scripts/include/utils/quickhull.bzz Normal file
View File

@ -0,0 +1,122 @@
hullPts = {}
hullC = 0
# When called returns a list of points that forms a convex hull around
# the listPts Given (http://adultstudylife.blogspot.com/2016/06/quick-hull-in-python.html)
function QuickHull(focalpts) {
listPts = {}
hullPts = {}
jp = 0
foreach(focalpts, function(key, pt) {
ip = 0
while(ip<6){
listPts[jp*6+ip]=math.vec2.add(pt,math.vec2.newp(20, ip*math.pi/3))
#log("Created new pt: ", listPts[j*6+i].x, listPts[j*6+i].y, "(", pt.x, pt.y, ")")
ip=ip+1
}
jp=jp+1
})
# get the min, and max from the list of points
min_max = get_min_max_x(listPts)
hullPts = quickhull_rec(listPts, min_max.min, min_max.max)
hullPts = quickhull_rec(listPts, min_max.max, min_max.min)
return hullPts
}
# Does the sorting for the quick hull sorting algorithm
function quickhull_rec(listPts, minp, maxp) {
left_of_line_pts = get_points_left_of_line(minp, maxp, listPts)
ptC = point_max_from_line(minp, maxp, left_of_line_pts)
if(size(ptC) < 1){
hullPts[hullC] = maxp # max
hullC = hullC +1
return hullPts
#log("Add pt:", maxp.x, maxp.y)
}
hullPts = quickhull_rec(left_of_line_pts, minp, ptC)
hullPts = quickhull_rec(left_of_line_pts, ptC, maxp)
return hullPts
}
# Returns all points that are LEFT of a line start->end
function get_points_left_of_line(minp, maxp, listPts) {
pts = {}
ih = 0
foreach(listPts, function(key, pt) {
if(isCCW(minp, maxp, pt)){
pts[ih]=pt
ih=ih+1
}
})
return pts
}
# Returns the maximum point from a line start->end
function point_max_from_line(minp, maxp, points) {
max_dist = 0
max_point = {}
foreach(points, function(key, point) {
if((not(math.vec2.equal(point, minp))) and (not(math.vec2.equal(point, maxp)))) {
#log("Get distance of pt: ", point.x, point.y)
dist = distance_toline(minp, maxp, point)
if(dist > max_dist) {
max_dist = dist
max_point = point
}
}
})
return max_point
}
function get_min_max_x(list_pts) {
min_x = 100000
max_x = 0
min_y = 0
max_y = 0
foreach(list_pts, function(key, point) {
if(point.x < min_x){
min_x = point.x
min_y = point.y
}
if(point.x > max_x){
max_x = point.x
max_y = point.y
}
})
return {.min=math.vec2.new(min_x, min_y), .max=math.vec2.new(max_x, max_y)}
}
# Given a line of start->end, will return the distance that
# point, pt, is from the line.
function distance_toline(start, end, pt) { # pt is the point
x1 = start.x
y1 = start.y
x2 = end.x
y2 = end.y
x0 = pt.x
y0 = pt.y
nom = math.abs((y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1)
denom = math.vec2.length(math.vec2.sub(end,start))
result = nom / denom
return result
}
# Tests whether the turn formed by A, B, and C is ccw
function isCCW(A, B, C){
return (B.x - A.x) * (C.y - A.y) > (B.y - A.y) * (C.x - A.x)
}

24
buzz_scripts/include/utils/table.bzz
View File

@ -1,7 +1,11 @@
function table_print(t) {
if(t==nil)
log("Table do not exist!")
else {
foreach(t, function(key, value) {
log(key, " -> ", value)
})
}
}
function table_copy(t) {
@ -16,8 +20,8 @@ function table_copy(t) {
#return the number of value in table
#
function count(table,value){
var number=0
var i=0
number=0
i=0
while(i<size(table)){
if(table[i]==value){
number=number+1
@ -27,17 +31,17 @@ function count(table,value){
return number
}
#
#map from int to state
# map from int to state - vstig serialization limits to 9....
#
function i2s(value){
if(value==1){
return "GRAPH_ASKING"
return "IDLE"
}
else if(value==2){
return "GRAPH_JOINING"
return "DEPLOY"
}
else if(value==3){
return "GRAPH_JOINED"
return "STOP"
}
else if(value==4){
return "TURNEDOFF"
@ -62,16 +66,16 @@ function i2s(value){
}
}
#
#map from state to int
# map from state to int - vstig serialization limits to 9....
#
function s2i(value){
if(value=="GRAPH_ASKING"){
if(value=="IDLE"){
return 1
}
else if(value=="GRAPH_JOINING"){
else if(value=="DEPLOY"){
return 2
}
else if(value=="GRAPH_JOINED"){
else if(value=="STOP"){
return 3
}
else if(value=="TURNEDOFF"){

4
buzz_scripts/include/utils/vec2.bzz
View File

@ -105,3 +105,7 @@ math.vec2.scale = function(v, s) {
math.vec2.dot = function(v1, v2) {
return v1.x * v2.x + v1.y * v2.y
}
math.vec2.equal = function(v1, v2) {
return (v1.x == v2.x) and (v1.y == v2.y)
}

4
buzz_scripts/log/anim_voronoi_cells.py
View File

@ -7,10 +7,11 @@ import csv
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
fig.set_tight_layout(True)
axes = plt.gca()
axes.set_xlim([-70,70])
axes.set_ylim([-70,70])
datafile = open('src/rosbuzz/buzz_scripts/log/voronoi_4.csv', 'r')
datafile = open('src/rosbuzz/buzz_scripts/log/voronoi_3.csv', 'r')
Vorreader = csv.reader(datafile, delimiter=',')
def animate(i):
@ -25,4 +26,5 @@ def animate(i):
return
ani = animation.FuncAnimation(fig, animate, interval=250)
ani.save('Voronoi.gif', dpi=80, writer='imagemagick')
plt.show()

15
buzz_scripts/main.bzz
View File

@ -3,21 +3,16 @@ include "update.bzz"
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "utils/table.bzz"
include "plan/rrtstar.bzz"
#include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "taskallocate/bidding.bzz"
#include "taskallocate/bidding.bzz"
include "vstigenv.bzz"
#include "timesync.bzz"
include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "DEPLOY"
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 3
graph_id = 3
graph_loop = 0
AUTO_LAUNCH_STATE = "IDLE"
LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
#####
@ -36,7 +31,7 @@ else
function init() {
init_stig()
init_swarm()
init_bidding()
#init_bidding()
TARGET_ALTITUDE = takeoff_heights[id]
@ -44,7 +39,7 @@ function init() {
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "LAUNCH"
BVMSTATE = "TURNEDOFF"
}
# Executed at each time step.

8
include/VoronoiDiagramGenerator.h
View File

@ -68,6 +68,8 @@ struct Freelist
struct Point
{
float x,y;
Point(): x( 0.0 ), y( 0.0 ) { }
Point( float x, float y ): x( x ), y( y ) { }
};
// structure used both for sites and for vertices
@ -206,12 +208,6 @@ private:
void circle(float x, float y, float radius);
void range(float minX, float minY, float maxX, float maxY);
bool onSegment(Point p, Point q, Point r);
int orientation(Point p, Point q, Point r);
bool doIntersect(Point p1, Point q1, Point p2, Point q2);
struct Freelist hfl;
struct Halfedge *ELleftend, *ELrightend;
int ELhashsize;

57
src/VoronoiDiagramGenerator.cpp
View File

@ -742,60 +742,6 @@ void VoronoiDiagramGenerator::plotinit()
range(pxmin, pymin, pxmax, pymax);
}
// Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
bool VoronoiDiagramGenerator::onSegment(Point p, Point q, Point r)
{
if (q.x <= std::max(p.x, r.x) && q.x >= std::min(p.x, r.x) &&
q.y <= std::max(p.y, r.y) && q.y >= std::min(p.y, r.y))
return true;
return false;
}
// To find orientation of ordered triplet (p, q, r).
// The function returns following values
// 0 --> p, q and r are colinear
// 1 --> Clockwise
// 2 --> Counterclockwise
int VoronoiDiagramGenerator::orientation(Point p, Point q, Point r)
{
int val = (q.y - p.y) * (r.x - q.x) -
(q.x - p.x) * (r.y - q.y);
if (val == 0) return 0; // colinear
return (val > 0)? 1: 2; // clock or counterclock wise
}
// The function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
bool VoronoiDiagramGenerator::doIntersect(Point p1, Point q1, Point p2, Point q2)
{
// Find the four orientations needed for general and
// special cases
int o1 = orientation(p1, q1, p2);
int o2 = orientation(p1, q1, q2);
int o3 = orientation(p2, q2, p1);
int o4 = orientation(p2, q2, q1);
// General case
if (o1 != o2 && o3 != o4)
return true;
// Special Cases
// p1, q1 and p2 are colinear and p2 lies on segment p1q1
if (o1 == 0 && onSegment(p1, p2, q1)) return true;
// p1, q1 and p2 are colinear and q2 lies on segment p1q1
if (o2 == 0 && onSegment(p1, q2, q1)) return true;
// p2, q2 and p1 are colinear and p1 lies on segment p2q2
if (o3 == 0 && onSegment(p2, p1, q2)) return true;
// p2, q2 and q1 are colinear and q1 lies on segment p2q2
if (o4 == 0 && onSegment(p2, q1, q2)) return true;
return false; // Doesn't fall in any of the above cases
}
void VoronoiDiagramGenerator::clip_line(struct Edge *e)
{
struct Site *s1, *s2;
@ -805,7 +751,6 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
x2 = e->reg[1]->coord.x;
y1 = e->reg[0]->coord.y;
y2 = e->reg[1]->coord.y;
//printf("Clip line for edges: %d", e->edgenbr);
//if the distance between the two points this line was created from is less than
//the square root of 2, then ignore it
@ -905,7 +850,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
{ y2 = pymin; x2 = (e -> c - y2)/e -> a;};
};
//printf("\nPushing line (%f,%f,%f,%f)",x1,y1,x2,y2);
//printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2);
line(x1,y1,x2,y2,e->sites);
}

461
src/buzzuav_closures.cpp
View File

@ -42,16 +42,11 @@ static bool logVoronoi = false;
std::ofstream voronoicsv;
struct Point
{
int x;
int y;
};
struct Pointf
{
float x;
float y;
Pointf(): x( 0.0 ), y( 0.0 ) { }
Pointf( float x, float y ): x( x ), y( y ) { }
Point(): x( 0.0 ), y( 0.0 ) { }
Point( float x, float y ): x( x ), y( y ) { }
};
string WPlistname = "";
@ -224,6 +219,7 @@ void check_targets_sim(double lat, double lon, double *res)
/ check if a listed target is close
----------------------------------------------------------- */
{
float visibility_radius = 5.0;
map<int, buzz_utility::RB_struct>::iterator it;
for (it = wplist_map.begin(); it != wplist_map.end(); ++it)
{
@ -231,7 +227,7 @@ void check_targets_sim(double lat, double lon, double *res)
double ref[2]={lat, lon};
double tar[2]={it->second.latitude, it->second.longitude};
rb_from_gps(tar, rb, ref);
if(rb[0]<3.0){
if(rb[0] < visibility_radius){
ROS_WARN("FOUND A TARGET!!! [%i]", it->first);
res[0] = it->first;
res[1] = it->second.latitude;
@ -272,164 +268,6 @@ int buzz_exportmap(buzzvm_t vm)
return buzzvm_ret0(vm);
}
float pol_area(vector <Pointf> vert) {
float a = 0.0;
vector <Pointf>::iterator it;
vector <Pointf>::iterator next;
for (it = vert.begin(); it != vert.end()-1; ++it){
next = it+1;
a += it->x * next->y - next->x * it->y;
}
a *= 0.5;
//ROS_INFO("Polygon area: %f",a);
return a;
}
double* polygone_center(vector <Pointf> vert, double *c) {
float A = pol_area(vert);
int i1 = 1;
vector <Pointf>::iterator it;
vector <Pointf>::iterator next;
for (it = vert.begin(); it != vert.end()-1; ++it){
next = it+1;
float t = it->x*next->y - next->x*it->y;
c[0] += (it->x+next->x) * t;
c[1] += (it->y+next->y) * t;
}
c[0] = c[0] / (6.0 * A);
c[1] = c[1] / (6.0 * A);
return c;
}
float clockwise_angle_of( const Pointf& p )
{
return atan2(p.y,p.x);
}
bool clockwise_compare_points( const Pointf& a, const Pointf& b )
{
return clockwise_angle_of( a ) < clockwise_angle_of( b );
}
int voronoi_center(buzzvm_t vm) {
float *xValues;//[4] = {-22, -17, 4,22};
float *yValues;//[4] = {-9, 31,13,-5};
float minx, miny, maxx, maxy;
buzzvm_lnum_assert(vm, 1);
// Get the parameter
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "maxx", 1));
buzzvm_tget(vm);
maxx = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "maxy", 1));
buzzvm_tget(vm);
maxy = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "minx", 1));
buzzvm_tget(vm);
minx = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "miny", 1));
buzzvm_tget(vm);
miny = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "oa", 1));
buzzvm_tget(vm);
float offset_angle = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
long count = buzzdict_size(t->t.value)-5;
xValues = new float[count];
yValues = new float[count];
for(int32_t i = 0; i < count; ++i) {
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm);
//ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
xValues[i] = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm);
//ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
yValues[i] = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_pop(vm);
}
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3);
if(logVoronoi) voronoicsv << ros::Time::now().toNSec() << ",";
vdg.resetIterator();
float x1,y1,x2,y2;
int s[2];
vector <Pointf> cell_vert;
int i=0;
while(vdg.getNext(x1,y1,x2,y2,s))
{
//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d\n",x1,y1,x2,y2,s[0],s[1]);
if(logVoronoi) voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
i++;
if((s[0]==0 || s[1]==0) && sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))>0.1) {
if(cell_vert.empty()){
cell_vert.push_back(Pointf(x1,y1));
cell_vert.push_back(Pointf(x2,y2));
} else {
bool alreadyin = false;
vector <Pointf>::iterator itc;
for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
if(dist < 0.1) {
alreadyin = true;
break;
}
}
if(!alreadyin)
cell_vert.push_back(Pointf(x1, y1));
alreadyin = false;
for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
double dist = sqrt((itc->x - x2) * (itc->x - x2) + (itc->y - y2) * (itc->y - y2));
if(dist < 0.1) {
alreadyin = true;
break;
}
}
if(!alreadyin)
cell_vert.push_back(Pointf(x2, y2));
}
}
}
std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points );
cell_vert.push_back(cell_vert[0]);
double center_dist[2] = {0.0, 0.0};
polygone_center(cell_vert, center_dist);
if(logVoronoi) voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
center_dist[0]/=2;
center_dist[1]/=2;
double gps[3];
gps_from_vec(center_dist, gps);
//ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1], gps[0], gps[1]);
set_gpsgoal(gps);
return buzzvm_ret0(vm);
}
/*
* Geofence(): test for a point in a polygon
* TAKEN from https://www.geeksforgeeks.org/how-to-check-if-a-given-point-lies-inside-a-polygon/
@ -488,6 +326,21 @@ bool doIntersect(Point p1, Point q1, Point p2, Point q2)
return false; // Doesn't fall in any of the above cases
}
float clockwise_angle_of( const Point& p )
{
return atan2(p.y,p.x);
}
bool clockwise_compare_points( const Point& a, const Point& b )
{
return clockwise_angle_of( a ) < clockwise_angle_of( b );
}
void sortclose_polygon(vector <Point> *P){
std::sort( P->begin(), P->end(), clockwise_compare_points );
P->push_back((*P)[0]);
}
int buzzuav_geofence(buzzvm_t vm)
{
bool onedge = false;
@ -512,7 +365,7 @@ int buzzuav_geofence(buzzvm_t vm)
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm);
tmp = round(buzzvm_stack_at(vm, 1)->f.value*10);
tmp = buzzvm_stack_at(vm, 1)->f.value;
//ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
if(i==0)
P.x = tmp;
@ -522,7 +375,7 @@ int buzzuav_geofence(buzzvm_t vm)
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm);
tmp = round(buzzvm_stack_at(vm, 1)->f.value*10);
tmp = buzzvm_stack_at(vm, 1)->f.value;
//ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
if(i==0)
P.y = tmp;
@ -532,6 +385,8 @@ int buzzuav_geofence(buzzvm_t vm)
buzzvm_pop(vm);
}
// TODO: use vector
//sortclose_polygon(&V);
// simple polygon: rectangle, 4 points
int n = 4;
@ -573,6 +428,276 @@ int buzzuav_geofence(buzzvm_t vm)
return buzzvm_ret0(vm);
}
float pol_area(vector <Point> vert) {
float a = 0.0;
//ROS_INFO("Polygone %d edges area.",vert.size());
vector <Point>::iterator it;
vector <Point>::iterator next;
for (it = vert.begin(); it != vert.end()-1; ++it){
next = it+1;
a += it->x * next->y - next->x * it->y;
}
a *= 0.5;
//ROS_INFO("Polygon area: %f",a);
return a;
}
double* polygone_center(vector <Point> vert, double *c) {
float A = pol_area(vert);
int i1 = 1;
vector <Point>::iterator it;
vector <Point>::iterator next;
for (it = vert.begin(); it != vert.end()-1; ++it){
next = it+1;
float t = it->x*next->y - next->x*it->y;
c[0] += (it->x+next->x) * t;
c[1] += (it->y+next->y) * t;
}
c[0] = c[0] / (6.0 * A);
c[1] = c[1] / (6.0 * A);
return c;
}
double numerator ( Point A, Point C, Point E, Point F ) { return (A.y - C.y) * (F.x - E.x) - (A.x - C.x) * (F.y - E.y); }
double denominator( Point A, Point B, Point C, Point D ) { return (B.x - A.x) * (D.y - C.y) - (B.y - A.y) * (D.x - C.x); }
void getintersection(Point S, Point D, std::vector <Point> Poly, Point *I) {
//printf("Points for intersection 1(%f,%f->%f,%f) and 2(%f,%f->%f,%f)\n",q1.x,q1.y,p1.x,p1.y,q2.x,q2.y,p2.x,p2.y);
bool parallel = false;
bool collinear = false;
std::vector <Point>::iterator itc;
std::vector <Point>::iterator next;
for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
next = itc+1;
if (doIntersect((*itc), (*next), S, D))
{
// Uses the determinant of the two lines. For more information, refer to one of the following:
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
// http://www.faqs.org/faqs/graphics/algorithms-faq/ (Subject 1.03)
double d = denominator( S, D, (*itc), (*next) );
if (std::abs( d ) < 0.000000001)
{
parallel = true;
collinear = abs(numerator( S, D, (*itc), (*next) )) < 0.000000001;
return;
}
double r = numerator( S, (*itc), (*itc), (*next) ) / d;
double s = numerator( S, (*itc), S, D ) / d;
(*I)=Point(S.x + r * (D.x - S.x), S.y + r * (D.y - S.y));
}
}
}
bool isSiteout(Point S, std::vector <Point> Poly) {
bool onedge = false;
// Create a point for line segment from p to infinite
Point extreme = {10000, S.y};
// Count intersections of the above line with sides of polygon
int count = 0;
std::vector <Point>::iterator itc;
std::vector <Point>::iterator next;
for (itc = Poly.begin(); itc != Poly.end()-1; ++itc) {
next = itc+1;
// Check if the line segment from 'p' to 'extreme' intersects
// with the line segment from 'polygon[i]' to 'polygon[next]'
if (doIntersect((*itc), (*next), S, extreme))
{
// If the point 'p' is colinear with line segment 'i-next',
// then check if it lies on segment. If it lies, return true,
// otherwise false
if (orientation((*itc), S, (*next)) == 0) {
onedge = onSegment((*itc), S, (*next));
if(onedge)
break;
}
count++;
}
}
return ((count%2 == 0) && !onedge);
}
int voronoi_center(buzzvm_t vm) {
float dist_max = 300;
buzzvm_lnum_assert(vm, 1);
// Get the parameter
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE); // dictionary
buzzobj_t t = buzzvm_stack_at(vm, 1);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "np", 1));
buzzvm_tget(vm);
int Poly_vert = buzzvm_stack_at(vm, 1)->i.value;
buzzvm_pop(vm);
std::vector <Point> polygon_bound;
for(int32_t i = 0; i < Poly_vert; ++i) {
buzzvm_dup(vm);
buzzvm_pushi(vm, i);
buzzvm_tget(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm);
//ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
float tmpx = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm);
//ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
float tmpy = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
polygon_bound.push_back(Point(tmpx, tmpy));
//ROS_INFO("[%i] Polygon vertex: %f, %f", buzz_utility::get_robotid(),tmpx,tmpy);
buzzvm_pop(vm);
}
sortclose_polygon(&polygon_bound);
// Check if we are in the zone
if(isSiteout(Point(0,0), polygon_bound)){
//ROS_WARN("Not in the Zone!!!");
double goal_tmp[2];
do{
goal_tmp[0] = polygon_bound[0].x + (rand()%100)/100.0*(polygon_bound[2].x- polygon_bound[0].x);
goal_tmp[1] = polygon_bound[0].y + (rand()%100)/100.0*(polygon_bound[2].y- polygon_bound[0].y);
//ROS_WARN(" in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
} while(isSiteout(Point(goal_tmp[0],goal_tmp[1]), polygon_bound));
ROS_WARN("Sending at a random location in the Zone (%f,%f)!",goal_tmp[0],goal_tmp[1]);
double gps[3];
gps_from_vec(goal_tmp, gps);
set_gpsgoal(gps);
return buzzvm_ret0(vm);
}
int count = buzzdict_size(t->t.value)-(Poly_vert+1);
ROS_WARN("NP: %d, Sites: %d", Poly_vert, count);
float *xValues = new float[count];
float *yValues = new float[count];
for(int32_t i = 0; i < count; ++i) {
int index = i + Poly_vert;
buzzvm_dup(vm);
buzzvm_pushi(vm, index);
buzzvm_tget(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 1));
buzzvm_tget(vm);
//ROS_INFO("---x-->%f",buzzvm_stack_at(vm, 1)->f.value);
xValues[i] = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm);
//ROS_INFO("---y-->%f",buzzvm_stack_at(vm, 1)->f.value);
yValues[i] = buzzvm_stack_at(vm, 1)->f.value;
buzzvm_pop(vm);
buzzvm_pop(vm);
}
VoronoiDiagramGenerator vdg;
ROS_WARN("[%i] Voronoi Bounded tessellation starting with %i sites...", buzz_utility::get_robotid(),count);
vdg.generateVoronoi(xValues, yValues, count, -dist_max, dist_max, -dist_max, dist_max, 3.0);
if(logVoronoi) voronoicsv << ros::Time::now().toNSec() << ",";
vdg.resetIterator();
//ROS_WARN("[%i] Voronoi Bounded tessellation done!", buzz_utility::get_robotid());
std::vector <Point>::iterator itc, next;
for (itc = polygon_bound.begin(); itc != polygon_bound.end()-1; ++itc) {
next = itc+1;
if(logVoronoi) voronoicsv << itc->x << "," << itc->y << "," << next->x << "," << next->y << "," << 0 << "," << 0 << ",";
}
float x1,y1,x2,y2;
int s[2];
vector <Point> cell_vert;
Point Intersection;
int i=0;
while(vdg.getNext(x1,y1,x2,y2,s))
{
//ROS_INFO("GOT Line (%f,%f)->(%f,%f) between sites %d,%d",x1,y1,x2,y2,s[0],s[1]);
if(sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1))<0.1)
continue;
bool isout1 = isSiteout(Point(x1,y1), polygon_bound);
bool isout2 = isSiteout(Point(x2,y2), polygon_bound);
if(isout1 && isout2){
//ROS_INFO("Line out of area!");
continue;
}else if(isout1) {
getintersection(Point(x2,y2), Point(x1,y1), polygon_bound, &Intersection);
x1 = Intersection.x;
y1 = Intersection.y;
//ROS_INFO("Site out 1 -> (%f,%f)", x1, y1);
}else if(isout2) {
getintersection(Point(x1,y1), Point(x2,y2), polygon_bound, &Intersection);
x2 = Intersection.x;
y2 = Intersection.y;
//ROS_INFO("Site out 2 -> (%f,%f)", x2, y2);
}
if(logVoronoi) voronoicsv << x1 << "," << y1 << "," << x2 << "," << y2 << "," << s[0] << "," << s[1] << ",";
i++;
if((s[0]==0 || s[1]==0)) {
if(cell_vert.empty()){
cell_vert.push_back(Point(x1,y1));
cell_vert.push_back(Point(x2,y2));
} else {
bool alreadyin = false;
vector <Point>::iterator itc;
for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
double dist = sqrt((itc->x - x1) * (itc->x - x1) + (itc->y - y1) * (itc->y - y1));
if(dist < 0.1) {
alreadyin = true;
break;
}
}
if(!alreadyin)
cell_vert.push_back(Point(x1, y1));
alreadyin = false;
for (itc = cell_vert.begin(); itc != cell_vert.end(); ++itc) {
double dist = sqrt((itc->x - x2) * (itc->x - x2) + (itc->y - y2) * (itc->y - y2));
if(dist < 0.1) {
alreadyin = true;
break;
}
}
if(!alreadyin)
cell_vert.push_back(Point(x2, y2));
}
}
}
if(cell_vert.size()<3){
ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(),cell_vert.size());
return buzzvm_ret0(vm);
}
std::sort( cell_vert.begin(), cell_vert.end(), clockwise_compare_points );
cell_vert.push_back(cell_vert[0]);
double center_dist[2] = {0.0, 0.0};
polygone_center(cell_vert, center_dist);
if(logVoronoi) voronoicsv << center_dist[0] << "," << center_dist[1] << std::endl;
center_dist[0]/=2;
center_dist[1]/=2;
double gps[3];
gps_from_vec(center_dist, gps);
//ROS_INFO("[%i] Voronoi cell center: %f, %f, %f, %f", buzz_utility::get_robotid(), center_dist[0], center_dist[1], gps[0], gps[1]);
set_gpsgoal(gps);
return buzzvm_ret0(vm);
}
int buzzuav_moveto(buzzvm_t vm)
/*
/ Buzz closure to move following a 3D vector + Yaw