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

Merge commit 'b3f9b7cf4e39329abcb820ae0892725b25b2f61a' into ros_drones_ws

This commit is contained in:
dave 2018-11-12 16:00:21 -05:00
parent 5800cb8b8c b3f9b7cf4e
commit 777c1b43d2
18 changed files with 820 additions and 334 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)) {

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

@ -2,44 +2,30 @@ GOTO_MAXVEL = 1.5 # m/steps
GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.4 # m.
GOTOANG_TOL = 0.1 # rad.
GPSlimit = {.1={.lat=45.510386, .lng=-73.610400},
.2={.lat=45.509839, .lng=-73.610047},
.3={.lat=45.510859, .lng=-73.608714},
.4={.lat=45.510327, .lng=-73.608393}}
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}}
# Core naviguation function to travel to a GPS target location.
function goto_gps(transf) {
if(Geofence()) {
m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
transf()
} else {
m_navigation = LimitSpeed(m_navigation, 1.0)
#m_navigation = LCA(m_navigation)
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
}
} else
log("Geofencing prevents from going to that location!")
m_navigation = vec_from_gps(cur_goal.latitude, cur_goal.longitude, 0)
#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL){ # reached destination
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)
#m_navigation = LCA(m_navigation)
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
}
}
function LimitSpeed(vel_vec, factor){
if(math.vec2.length(vel_vec)>GOTO_MAXVEL*factor)
vel_vec = math.vec2.scale(vel_vec, GOTO_MAXVEL*factor/math.vec2.length(vel_vec))
return vel_vec
}
function Geofence(){ #TODO: rotate the fence box to really fit the coordinates
if(cur_goal.latitude > GPSlimit[1].lat and cur_goal.latitude > GPSlimit[2].lat and cur_goal.latitude > GPSlimit[3].lat and cur_goal.latitude > GPSlimit[4].lat)
return 0;
if(cur_goal.latitude < GPSlimit[1].lat and cur_goal.latitude < GPSlimit[2].lat and cur_goal.latitude < GPSlimit[3].lat and cur_goal.latitude < GPSlimit[4].lat)
return 0;
if(cur_goal.longitude > GPSlimit[1].lng and cur_goal.longitude > GPSlimit[2].lng and cur_goal.longitude > GPSlimit[3].lng and cur_goal.longitude > GPSlimit[4].lng)
return 0;
if(cur_goal.longitude < GPSlimit[1].lng and cur_goal.longitude < GPSlimit[2].lng and cur_goal.longitude < GPSlimit[3].lng and cur_goal.longitude < GPSlimit[4].lng)
return 0;
return 1
}

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

@ -1,71 +1,74 @@
# listens to commands from the remote control (web, commandline, rcclient node, etc)
function rc_cmd_listen() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22)
} 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"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "IDLE"
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){
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){
flight.rc_cmd=0
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
} else if (flight.rc_cmd==904){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
barrier_ready(904)
neighbors.broadcast("cmd", 904)
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==777){ #SYNC
flight.rc_cmd=0
reinit_time_sync()
neighbors.broadcast("cmd", 777)
}
} else {
if(flight.rc_cmd==21) {
flight.rc_cmd=0
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
#barrier_ready(21)
BVMSTATE = "STOP"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==20) {
flight.rc_cmd=0
AUTO_LAUNCH_STATE = "IDLE"
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
barrier_ready(20)
neighbors.broadcast("cmd", 20)
} else if(BVMSTATE!="BARRIERWAIT" and BVMSTATE!="LAUNCH") {
if (flight.rc_cmd==666){
flight.rc_cmd=0
stattab_send()
} else if (flight.rc_cmd==900){
flight.rc_cmd=0
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
barrier_ready(900)
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){
flight.rc_cmd=0
destroyGraph()
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)
} else if (flight.rc_cmd==903){
flight.rc_cmd=0
destroyGraph()
resetWP()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
} else if (flight.rc_cmd==904){
flight.rc_cmd=0
destroyGraph()
barrier_set(ROBOTS, "IDLE", BVMSTATE, 904)
barrier_ready(904)
neighbors.broadcast("cmd", 904)
}
}
}
}
@ -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
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
#neighbors.broadcast("cmd", 900)
} else if(value==901 and BVMSTATE!="BARRIERWAIT"){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
#barrier_ready(901)
#neighbors.broadcast("cmd", 901)
} else if(value==902 and BVMSTATE!="BARRIERWAIT" and BVMSTATE!="WAYPOINT"){ # 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
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
destroyGraph()
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
# })
}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!="DEPLOY" ){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
#barrier_ready(901)
#neighbors.broadcast("cmd", 901)
} 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!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){ # Formation
destroyGraph()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
#barrier_ready(903)
#neighbors.broadcast("cmd", 903)
} 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)
}
}
#}
}
})
}
@ -141,7 +143,8 @@ function check_rc_wp() {
v_wp.put(rc_goto.id,ls)
reset_rc()
}
}
} else
v_wp.get(0)
}
function resetWP() {

115
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(gohomeT > 0) { # TODO: Make a real check if home is reached
gohome()
gohomeT = gohomeT - 1
} else
BVMSTATE = AUTO_LAUNCH_STATE
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,18 +249,54 @@ function lennardjones() {
counter = 0
function voronoicentroid() {
BVMSTATE="DEPLOY"
if(counter==0 and id!=0) {
pts = getbounds()
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
#table_print(pts)
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) {
pts[it_pts]=math.vec2.newp(data.distance,data.azimuth+pts.oa)
it_pts = it_pts + 1})
if(rid!=0){ #remove GS (?)
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
@ -265,45 +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
minYid = 1
maxY = RBlimit[1].y
maxYid = 1
foreach(RBlimit, function(key, value) {
if(value.y<minY){
minY = value.y
minYid = key
}
if(value.y>maxY){
maxY = value.y
maxYid = key
}
})
minY2 = maxY
minY2id = maxYid
foreach(RBlimit, function(key, value) {
if(value.y<minY2 and key!=minYid){
minY2 = value.y
minY2id = key
}
})
angle_offset = math.atan(RBlimit[minY2id].y-RBlimit[minYid].y,RBlimit[minY2id].x-RBlimit[minYid].x)
if(angle_offset > math.pi/2.0)
angle_offset = math.pi - angle_offset
#log(angle_offset, minYid, minY2id, maxYid)
dvec = math.vec2.sub(RBlimit[maxYid],RBlimit[minYid])
new_maxY = math.vec2.add(RBlimit[minYid], math.vec2.newp(math.vec2.length(dvec),math.vec2.angle(dvec)+angle_offset))
if(new_maxY.x > RBlimit[minYid].x)
return {.miny=minY, .minx=RBlimit[minYid].x, .maxy=new_maxY.y, .maxx=new_maxY.x}
else
return {.miny=minY, .minx=new_maxY.x, .maxy=new_maxY.y, .maxx=RBlimit[minYid].x, .oa=angle_offset}
}
# Custom state function for the developer to play with
function cusfun(){
BVMSTATE="CUSFUN"

9
buzz_scripts/include/taskallocate/bidding.bzz
View File

@ -457,6 +457,9 @@ function init_bidding() {
# executed at each time step
function bidding() {
# enable debug will increase the message size
debug = 0
# read the csv file with the waypoints information
read_from_csv(CSV_FILENAME_AND_PATH)
@ -565,11 +568,11 @@ function bidding() {
bid_made = 1
bid_time = experiment_iteration
bidded_area = highest_area
log("Drone", id, "SET its bid", highest_bid, "on", highest_area, "at iter", experiment_iteration, "status was", stig_get_status(highest_area))
log("Drone", id, "SET its bid", highest_bid, "on", highest_area, "at iter", experiment_iteration)
} else {
bid_made = 0
bidded_area = -1
log("Drone", id, "GAVE UP bid", highest_bid, "on", highest_area, "at iter", experiment_iteration, "status was", stig_get_status(highest_area))
log("Drone", id, "GAVE UP bid", highest_bid, "on", highest_area, "at iter", experiment_iteration)
}
}
}
@ -616,7 +619,7 @@ function bidding() {
########################################
# TEMP DEBUG BLOCK START ###############
########################################
if (id == 2) {
if (id == 2 and debug == 1) {
if (experiment_iteration%20==0){
print_out_bidding_stigmergy()
}

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]

2
buzz_scripts/include/utils/conversions.bzz
View File

@ -74,4 +74,4 @@ function unpackWP2i(wp_int){
var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0
var pro = wp_int.dlo-dlon*10
return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lon=dlon/1000000.0+GPSoffset.lon, .pro=pro}
}
}

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

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

@ -1,7 +1,11 @@
function table_print(t) {
foreach(t, function(key, value) {
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)
}

30
buzz_scripts/log/anim_voronoi_cells.py Normal file
View File

@ -0,0 +1,30 @@
#!/usr/bin/python
import matplotlib.pyplot as plt
plt.style.use('seaborn-whitegrid')
import matplotlib.animation as animation
import numpy as np
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_3.csv', 'r')
Vorreader = csv.reader(datafile, delimiter=',')
def animate(i):
for row in Vorreader:
ax.clear()
# ax.plot([-50, -50, 50, 50, -50],[-50, 50, 50, -50, -50],'b--')
j = 1
while j < len(row)-2:
ax.plot([float(row[j]), float(row[j+2])], [float(row[j+1]), float(row[j+3])])
j += 6
ax.plot(float(row[len(row)-2]),float(row[len(row)-1]),'x')
return
ani = animation.FuncAnimation(fig, animate, interval=250)
ani.save('Voronoi.gif', dpi=80, writer='imagemagick')
plt.show()

11
buzz_scripts/main.bzz
View File

@ -3,9 +3,9 @@ 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"
@ -13,11 +13,6 @@ include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "IDLE"
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 3
graph_id = 3
graph_loop = 0
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]

14
include/VoronoiDiagramGenerator.h
View File

@ -33,6 +33,8 @@
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <algorithm>
#ifndef NULL
@ -66,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
@ -84,12 +88,14 @@ struct Edge
struct Site *ep[2];
struct Site *reg[2];
int edgenbr;
int sites[2];
};
struct GraphEdge
{
float x1,y1,x2,y2;
int sites[2];
struct GraphEdge* next;
};
@ -123,7 +129,7 @@ public:
iteratorEdges = allEdges;
}
bool getNext(float& x1, float& y1, float& x2, float& y2)
bool getNext(float& x1, float& y1, float& x2, float& y2, int *s)
{
if(iteratorEdges == 0)
return false;
@ -132,6 +138,7 @@ public:
x2 = iteratorEdges->x2;
y1 = iteratorEdges->y1;
y2 = iteratorEdges->y2;
std::copy(iteratorEdges->sites, iteratorEdges->sites+2, s);
iteratorEdges = iteratorEdges->next;
@ -194,14 +201,13 @@ private:
void out_vertex(struct Site *v);
struct Site *nextone();
void pushGraphEdge(float x1, float y1, float x2, float y2);
void pushGraphEdge(float x1, float y1, float x2, float y2, int s[2]);
void openpl();
void line(float x1, float y1, float x2, float y2);
void line(float x1, float y1, float x2, float y2, int s[2]);
void circle(float x, float y, float radius);
void range(float minX, float minY, float maxX, float maxY);
struct Freelist hfl;
struct Halfedge *ELleftend, *ELrightend;
int ELhashsize;

2
include/buzzuav_closures.h
View File

@ -21,6 +21,7 @@ namespace buzzuav_closures
*/
int buzzros_print(buzzvm_t vm);
void setWPlist(std::string file);
void setVorlog(std::string path);
void check_targets_sim(double lat, double lon, double *res);
/*
@ -154,6 +155,7 @@ int buzzuav_land(buzzvm_t vm);
* Command the UAV to go to home location
*/
int buzzuav_gohome(buzzvm_t vm);
int buzzuav_geofence(buzzvm_t vm);
/*
* Updates battery information in Buzz

21
src/VoronoiDiagramGenerator.cpp
View File

@ -295,7 +295,7 @@ void VoronoiDiagramGenerator::geominit()
}
struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
{
float dx,dy,adx,ady;
struct Edge *newedge;
@ -325,8 +325,10 @@ struct Edge * VoronoiDiagramGenerator::bisect(struct Site *s1,struct Site *s2)
};
newedge -> edgenbr = nedges;
newedge -> sites[0] = s1->sitenbr;
newedge -> sites[1] = s2->sitenbr;
//printf("\nbisect(%d) ((%f,%f) and (%f,%f)",nedges,s1->coord.x,s1->coord.y,s2->coord.x,s2->coord.y);
//printf("\nbisect(%d) (%d(%f,%f) and %d(%f,%f)",nedges,s1->sitenbr,s1->coord.x,s1->coord.y,s2->sitenbr,s2->coord.x,s2->coord.y);
nedges += 1;
return(newedge);
@ -655,7 +657,7 @@ void VoronoiDiagramGenerator::cleanupEdges()
}
void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2)
void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float y2, int s[2])
{
GraphEdge* newEdge = new GraphEdge;
newEdge->next = allEdges;
@ -664,6 +666,7 @@ void VoronoiDiagramGenerator::pushGraphEdge(float x1, float y1, float x2, float
newEdge->y1 = y1;
newEdge->x2 = x2;
newEdge->y2 = y2;
std::copy(s, s+2, newEdge->sites);
}
@ -679,9 +682,9 @@ char * VoronoiDiagramGenerator::myalloc(unsigned n)
/* for those who don't have Cherry's plot */
/* #include <plot.h> */
void VoronoiDiagramGenerator::openpl(){}
void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2)
void VoronoiDiagramGenerator::line(float x1, float y1, float x2, float y2, int s[2])
{
pushGraphEdge(x1,y1,x2,y2);
pushGraphEdge(x1,y1,x2,y2,s);
}
void VoronoiDiagramGenerator::circle(float x, float y, float radius){}
@ -739,7 +742,6 @@ void VoronoiDiagramGenerator::plotinit()
range(pxmin, pymin, pxmax, pymax);
}
void VoronoiDiagramGenerator::clip_line(struct Edge *e)
{
struct Site *s1, *s2;
@ -771,7 +773,7 @@ void VoronoiDiagramGenerator::clip_line(struct Edge *e)
s1 = e -> ep[0];
s2 = e -> ep[1];
};
if(e -> a == 1.0)
{
y1 = pymin;
@ -848,8 +850,8 @@ 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);
line(x1,y1,x2,y2);
//printf("Pushing line (%f,%f,%f,%f)\n",x1,y1,x2,y2);
line(x1,y1,x2,y2,e->sites);
}
@ -895,6 +897,7 @@ bool VoronoiDiagramGenerator::voronoi(int triangulate)
e = bisect(bot, newsite); //create a new edge that bisects
bisector = HEcreate(e, le); //create a new HalfEdge, setting its ELpm field to 0
ELinsert(lbnd, bisector); //insert this new bisector edge between the left and right vectors in a linked list
//printf("Newsite %d: %f,%f\n",newsite->sitenbr,newsite->coord.x,newsite->coord.y);
if ((p = intersect(lbnd, bisector)) != (struct Site *) NULL) //if the new bisector intersects with the left edge, remove the left edge's vertex, and put in the new one
{

3
src/buzz_utility.cpp
View File

@ -243,6 +243,9 @@ static int buzz_register_hooks()
buzzvm_pushs(VM, buzzvm_string_register(VM, "voronoi", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::voronoi_center));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "geofence", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_geofence));
buzzvm_gstore(VM);
return VM->state;
}

498
src/buzzuav_closures.cpp
View File

@ -37,7 +37,17 @@ static int rssi = 0;
static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0;
static float api_rssi = 0.0;
static bool logVoronoi = false;
std::ofstream voronoicsv;
struct Point
{
float x;
float y;
Point(): x( 0.0 ), y( 0.0 ) { }
Point( float x, float y ): x( x ), y( y ) { }
};
string WPlistname = "";
std::map<int, buzz_utility::RB_struct> targets_map;
@ -55,7 +65,7 @@ int buzzros_print(buzzvm_t vm)
----------------------------------------------------------- */
{
std::ostringstream buffer(std::ostringstream::ate);
buffer << std::fixed << std::setprecision(6) << "[" << buzz_utility::get_robotid() << "] ";
buffer << "[" << buzz_utility::get_robotid() << "] ";
for (uint32_t index = 1; index < buzzdarray_size(vm->lsyms->syms); ++index)
{
buzzvm_lload(vm, index);
@ -104,10 +114,19 @@ void setWPlist(string file)
/ set the absolute path for a csv list of waypoints
----------------------------------------------------------- */
{
WPlistname = file;//path + "include/taskallocate/waypointlist.csv";
WPlistname = file;
parse_gpslist();
}
void setVorlog(string path)
/*
/ set the absolute path for a csv list of waypoints
----------------------------------------------------------- */
{
voronoicsv.open(path + "/log/voronoi_"+std::to_string(buzz_utility::get_robotid())+".csv", std::ios_base::trunc | std::ios_base::out);
logVoronoi = true;
}
float constrainAngle(float x)
/*
/ Wrap the angle between -pi, pi
@ -200,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)
{
@ -207,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;
@ -248,50 +268,330 @@ int buzz_exportmap(buzzvm_t vm)
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/
*/
// Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
bool onSegment(Point p, Point q, Point r)
{
if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) &&
q.y <= max(p.y, r.y) && q.y >= 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 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 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
}
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;
Point P;
Point V[4];
int tmp;
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);
if(buzzdict_size(t->t.value) != 5) {
ROS_ERROR("Wrong Geofence input size (%i).", buzzdict_size(t->t.value));
return buzzvm_ret0(vm);
}
for(int32_t i = 0; i < buzzdict_size(t->t.value); ++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);
tmp = buzzvm_stack_at(vm, 1)->f.value;
//ROS_INFO("[%i]---x-->%i",buzz_utility::get_robotid(), tmp);
if(i==0)
P.x = tmp;
else
V[i-1].x = tmp;
buzzvm_pop(vm);
buzzvm_dup(vm);
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 1));
buzzvm_tget(vm);
tmp = buzzvm_stack_at(vm, 1)->f.value;
//ROS_INFO("[%i]---y-->%i",buzz_utility::get_robotid(), tmp);
if(i==0)
P.y = tmp;
else
V[i-1].y = tmp;
buzzvm_pop(vm);
buzzvm_pop(vm);
}
// TODO: use vector
//sortclose_polygon(&V);
// simple polygon: rectangle, 4 points
int n = 4;
// Create a point for line segment from p to infinite
Point extreme = {10000, P.y};
// Count intersections of the above line with sides of polygon
int count = 0, i = 0;
do
{
int next = (i+1)%n;
// Check if the line segment from 'p' to 'extreme' intersects
// with the line segment from 'polygon[i]' to 'polygon[next]'
if (doIntersect(V[i], V[next], P, 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(V[i], P, V[next]) == 0) {
onedge = onSegment(V[i], P, V[next]);
if(onedge)
break;
}
count++;
}
i = next;
} while (i != 0);
//ROS_INFO("[%i] Geofence: %i, %i",buzz_utility::get_robotid(),count, onedge);
if((count%2 == 0) || onedge) {
goto_gpsgoal[0] = cur_pos[0];
goto_gpsgoal[1] = cur_pos[1];
ROS_WARN("Geofencing trigered, not going any further!");
}
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 *xValues;//[4] = {-22, -17, 4,22};
float *yValues;//[4] = {-9, 31,13,-5};
float minx, miny, maxx, maxy;
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, "maxx", 1));
buzzvm_pushs(vm, buzzvm_string_register(vm, "np", 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;
int Poly_vert = buzzvm_stack_at(vm, 1)->i.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) {
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);
@ -309,68 +609,90 @@ int voronoi_center(buzzvm_t vm) {
}
VoronoiDiagramGenerator vdg;
vdg.generateVoronoi(xValues,yValues,count, minx, maxx, miny, maxy,3);
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;
map<float, std::array<float, 4>> edges;
while(vdg.getNext(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)\n",x1,y1,x2, y2);
edges.insert(make_pair(sqrt((x2+x1)*(x2+x1)/4+(y2+y1)*(y2+y1)/4), std::array<float, 4>{x1,y1,x2,y2}));
}
double center_dist[2] = {0,0};
float closest_points[8];
int nit = 1;
float prev;
map<float, std::array<float, 4>>::iterator it;
int got3 = 0;
for (it = edges.begin(); it != edges.end(); ++it)
{
if(nit == 1) {
//center_dist[0] += it->second[0] + it->second[2];
//center_dist[1] += it->second[1] + it->second[3];
closest_points[0] = it->second[0]; closest_points[1] = it->second[1]; closest_points[2] = it->second[2]; closest_points[3] = it->second[3];
ROS_INFO("USE Line (%f,%f)->(%f,%f): %f\n",it->second[0],it->second[1],it->second[2], it->second[3], it->first);
prev = it->first;
} else if(nit == 2) {
map<float, std::array<float, 4>>::iterator it_prev = edges.find(prev);
if(it->second[0]!=it_prev->second[0] && it->second[1]!=it_prev->second[1] && it->second[0]!=it_prev->second[2] && it->second[1]!=it_prev->second[3]){
//center_dist[0] += it->second[0];
//center_dist[1] += it->second[1];
closest_points[4] = it->second[0]; closest_points[5] = it->second[1];
ROS_INFO("USE Point (%f,%f): %f\n",it->second[0],it->second[1], it->first);
got3 = 1;
}
if(it->second[2]!=it_prev->second[0] && it->second[3]!=it_prev->second[1] && it->second[2]!=it_prev->second[2] && it->second[3]!=it_prev->second[3]){
//center_dist[0] += it->second[2];
//center_dist[1] += it->second[3];
if(!got3) {
closest_points[4] = it->second[2]; closest_points[5] = it->second[3];
} else {
closest_points[6] = it->second[2]; closest_points[7] = it->second[3];
got3=2;
//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;
}
}
ROS_INFO("USE Point (%f,%f): %f (%i)\n",it->second[2],it->second[3], it->first, got3);
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));
}
} else
break;
nit++;
}
}
if(cell_vert.size()<3){
ROS_WARN("[%i] Voronoi Bounded tessellation failed (%d)!", buzz_utility::get_robotid(),cell_vert.size());
return buzzvm_ret0(vm);
}
if(got3==2)
center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4]+closest_points[6])/4;
else
center_dist[0]=(closest_points[0]+closest_points[2]+closest_points[4])/3;
if(got3==2)
center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5]+closest_points[7])/4;
else
center_dist[1]=(closest_points[1]+closest_points[3]+closest_points[5])/3;
center_dist[0]=center_dist[0]*cos(offset_angle)-center_dist[1]*sin(offset_angle);
center_dist[1]=center_dist[0]*sin(offset_angle)+center_dist[1]*cos(offset_angle);
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]);
//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);
@ -457,8 +779,8 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
buzzobj_t t = buzzvm_stack_at(vm, 1);
if(buzzdict_size(t->t.value) != 5) {
ROS_WARN("Wrong neighbor status size.");
return vm->state;
ROS_ERROR("Wrong neighbor status size.");
return buzzvm_ret0(vm);
}
buzz_utility::neighbors_status newRS;

3
src/roscontroller.cpp
View File

@ -36,7 +36,6 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
bcfname = fname + ".bo";
dbgfname = fname + ".bdb";
buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
buzzuav_closures::setWPlist(WPfile);
// Initialize variables
if(setmode)
SetMode("LOITER", 0);
@ -71,6 +70,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
out_msg_time=0;
// Create log dir and open log file
initcsvlog();
buzzuav_closures::setWPlist(WPfile);
}
roscontroller::~roscontroller()
@ -114,6 +114,7 @@ void roscontroller::RosControllerRun()
// Set the Buzz bytecode
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(), robot_id))
{
buzzuav_closures::setVorlog(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")));
ROS_INFO("[%i] INIT DONE!!!", robot_id);
int packet_loss_tmp, time_step = 0;
double cur_packet_loss = 0;