move rosbuzz headers and reneabled pursuit and graph formation from webcontrol

This commit is contained in:
dave 2019-04-10 14:14:19 -04:00
parent 499484778b
commit 073e107da7
22 changed files with 419 additions and 133 deletions

View File

@ -1,10 +1,10 @@
# Lightweight collision avoidance
function LCA( vel_vec ) {
var safety_radius = 3.0
var safety_radius = 2.0
collide = 0
var k_v = 5 # x axis gain
var k_w = 5 # y axis gain
var k_v = 4 # x axis gain
var k_w = 4 # y axis gain
cart = neighbors.map(
function(rid, data) {
@ -17,7 +17,7 @@ function LCA( vel_vec ) {
})
if (collide) {
log("")
log("------> AVOIDING NEIGHBOR! <------")
log("------> AVOIDING NEIGHBOR! (LCA) <------")
log("")
result = cart.reduce(function(rid, data, accum) {
if(data.distance < accum.distance and data.distance > 0.0){
@ -49,3 +49,186 @@ function LCA( vel_vec ) {
} else
return vel_vec
}
robot_radius = 1.0
safety_radius = 2.0
combined_radius = 2 * robot_radius + safety_radius
vel_sample_count = 50
velocities = {}
function in_between(theta_right, theta_dif, theta_left) {
if(math.abs(theta_right - theta_left) < (math.pi - 0.000001)) {
if((theta_right <= theta_dif) and (theta_dif <= theta_left)) {
return 1
} else {
return 0
}
} else if(math.abs(theta_right - theta_left) > (math.pi + 0.000001)) {
if((theta_right <= theta_dif) or (theta_dif <= theta_left)) {
return 1
} else {
return 0
}
} else {
# Exactly pi rad between right and left
if(theta_right <= 0) {
if((theta_right <= theta_dif) and (theta_dif <= theta_left)) {
return 1
} else {
return 0
}
} else if(theta_left <= 0) {
if((theta_right <= theta_dif) or (theta_dif <= theta_left)) {
return 1
} else {
return 0
}
}
}
}
# VO magic happens here
function RVO(preferedVelocity) {
#data_string = string.concat(data_string, ",", string.tostring(preferedVelocity.x), ",", string.tostring(preferedVelocity.y), ",", string.tostring(preferedVelocity.z))
final_V = math.vec2.new(0.0, 0.0)
collision = 0
suitable_V = {}
var VO_all = {}
neighbors.foreach(
function(rid, data) {
var angle = data.azimuth#-(data.azimuth * 0.017454)
#data_string = string.concat(data_string, ",", string.tostring(data.distance), ",", string.tostring(angle))
var distance = data.distance
if(distance <= combined_radius) distance = combined_radius
theta_BA_ort = math.asin(combined_radius / distance)
theta_BA_left = angle + theta_BA_ort
if(theta_BA_left > math.pi) {
theta_BA_left = theta_BA_left - 2 * math.pi
} else if(theta_BA_left < -math.pi) {
theta_BA_left = theta_BA_left + 2 * math.pi
}
theta_BA_right = angle - theta_BA_ort
if(theta_BA_right > math.pi) {
theta_BA_right = theta_BA_right - 2 * math.pi
} else if(theta_BA_right < -math.pi) {
theta_BA_right = theta_BA_right + 2 * math.pi
}
neighbor_velocity = velocities[rid]
if(neighbor_velocity == nil) {
neighbor_velocity = math.vec2.new(0.0, 0.0)
}
#data_string = string.concat(data_string, ",", string.tostring(neighbor_velocity.x), ",", string.tostring(neighbor_velocity.y), ",", string.tostring(neighbor_velocity.z))
VO_all[rid] = {
.velocity = math.vec2.new(neighbor_velocity.x, neighbor_velocity.y),
.theta_left = theta_BA_left,
.theta_right = theta_BA_right
}
}
)
# Detect collision
foreach(VO_all, function(rid, vo) {
vAB = math.vec2.sub(preferedVelocity, vo.velocity)
var vel_angle = math.acos(vAB.x / math.vec2.length(vAB))
if(vAB.y < 0) vel_angle = vel_angle * -1
if(in_between(vo.theta_right, vel_angle, vo.theta_left)) {
collision = 1
}
})
# Calculate suitable velocities
if(collision) {
log("")
log("------> AVOIDING NEIGHBOR! (RVO) <------")
log("")
var idx = 0
var n = 0
while (n < vel_sample_count) {
v_cand = math.vec2.new( 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL, 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL)
while(math.vec2.length(v_cand) > GOTO_MAXVEL) {
v_cand = math.vec2.new( 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL, 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL)
}
suit = 1
foreach(VO_all, function(rid, vo) {
#vAB = new_V
vAB = math.vec2.sub(v_cand, vo.velocity)
#vAB = math.vec3.sub(new_V, math.vec3.scale(math.vec3.add(preferedVelocity, vo.velocity), 0.5))
var vel_angle = math.acos(vAB.x / math.vec2.length(vAB))
if(vAB.y < 0) vel_angle = vel_angle * -1
if(in_between(vo.theta_right, vel_angle, vo.theta_left)) {
suit = 0
}
})
if(suit) {
suitable_V[idx] = v_cand
idx = idx + 1
}
n = n + 1
}
# Chose a velocity closer to the desired one
if(size(suitable_V) > 0) {
min_dist = 99999
sv = 0
while(sv < size(suitable_V)) {
var RVOdist = math.vec2.length(math.vec2.sub(suitable_V[sv], preferedVelocity))
if(RVOdist < min_dist) {
min_dist = RVOdist
final_V = suitable_V[sv]
}
sv = sv + 1
}
}
#data_string = string.concat(data_string, ",", string.tostring(final_V.x), ",", string.tostring(final_V.y), ",", string.tostring(final_V.z))
broadcast_velocity(final_V)
return final_V
} else {
#data_string = string.concat(data_string, ",", string.tostring(preferedVelocity.x), ",", string.tostring(preferedVelocity.y), ",", string.tostring(preferedVelocity.z))
broadcast_velocity(preferedVelocity)
return preferedVelocity
}
}
function broadcast_velocity(velocity) {
neighbors.broadcast(string.concat("v", string.tostring(id)), velocity)
}
function setup_velocity_callbacks() {
r = 1
while(r <= ROBOTS) {
if(r != id) {
neighbors.listen(string.concat("v", string.tostring(r)),
function(vid, value, rid) {
velocities[rid] = value
}
)
}
r = r + 1
}
}

View File

@ -18,21 +18,7 @@ hvs = 0;
# Sets a barrier
#
function barrier_create() {
# reset
timeW = 1
# create barrier vstig
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
#if(barrier!=nil) {
# barrier=nil
#if(hvs) {
# BARRIER_VSTIG = BARRIER_VSTIG -1
# hvs = 0
#}else{
# BARRIER_VSTIG = BARRIER_VSTIG +1
# hvs = 1
#}
#}
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier==nil)
barrier = stigmergy.create(BARRIER_VSTIG)
}
@ -95,6 +81,43 @@ function barrier_wait(threshold, transf, resumef, bc) {
timeW = timeW+1
}
function barrier_wait_graph(threshold, transf, resumef, bc) {
if(barrier==nil) #failsafe
barrier_create()
barrier.put(id, bc)
if(barrier.get("n")<threshold)
barrier.put("n", threshold)
if(threshold>1) {
neighbors.foreach(function (nei,data){
barrier.get(nei)
})
}
# Not to miss any RC inputs, for waypoints, potential and deploy states
check_rc_wp()
#log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
if(barrier.size()-2 >= barrier.get("n")) {
if(barrier_allgood(barrier,bc)) {
barrier.put("d", 1)
timeW = 0
GRAPHSTATE = transf
} else
barrier.put("d", 0)
}
if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!")
#barrier = nil
timeW = 0
GRAPHSTATE = resumef
} else if(timeW % 10 == 0 and bc > 0)
neighbors.broadcast("cmd", bc)
timeW = timeW+1
}
# Check all members state
function barrier_allgood(barrier, bc) {
barriergood = 1

View File

@ -1,23 +1,9 @@
GOTO_MAXVEL = 12.5 # m/steps
GOTO_MAXVEL = 2.5 # m/steps
GOTO_MAXDIST = 250 # m.
GOTODIST_TOL = 0.4 # m.
GOTOANG_TOL = 0.1 # rad.
# Geofence polygon
# CEPSUM field
#GPSlimit = {.1={.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}}
# Pangeae final site
#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315},
# .2={.lat=29.068724, .lng=-13.662634},
# .3={.lat=29.068113, .lng=-13.661427},
# .4={.lat=29.067014, .lng=-13.661564}}
# Pangeae first site
GPSlimit = {.1={.lat=29.021055, .lng=-13.715155},
.2={.lat=29.021055, .lng=-13.714132},
.3={.lat=29.019470, .lng=-13.714132},
.4={.lat=29.019470, .lng=-13.715240}}
include "utils/field_config.bzz"
# Core naviguation function to travel to a GPS target location.
function goto_gps(transf) {
@ -30,8 +16,12 @@ 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)}
#TODO: ENSURE THAT IF WE GO OUT WE CAN GET BACK IN!!!
geofence(gf)
#m_navigation = LCA(m_navigation)
if(CA_ON==1)
m_navigation = LCA(m_navigation)
if(CA_ON==2)
m_navigation = RVO(m_navigation)
goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
}
}

View File

@ -59,7 +59,7 @@ function rc_cmd_listen() {
destroyGraph()
resetWP()
check_rc_wp()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903)
} else if (flight.rc_cmd==904){
@ -100,11 +100,11 @@ function nei_cmd_listen() {
if(BVMSTATE!=AUTO_LAUNCH_STATE)
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
#barrier_ready(20)
} else if(value==900){ # Shapes
} else if(value==900 and BVMSTATE!="TASK_ALLOCATE"){ # Shapes
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
#barrier_ready(900)
#neighbors.broadcast("cmd", 900)
} else if(value==901 and BVMSTATE!="DEPLOY" ){ # Pursuit
} else if(value==901 and BVMSTATE!="DEPLOY" ){ # Voronoi
destroyGraph()
barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
#barrier_ready(901)
@ -114,9 +114,9 @@ function nei_cmd_listen() {
barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
#barrier_ready(902)
#neighbors.broadcast("cmd", 902)
} else if(value==903 and BVMSTATE!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){ # Formation
} else if(value==903 and BVMSTATE!="PURSUIT" and BVMSTATE!="TURNEDOFF"){ # Pursuit
destroyGraph()
barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 903)
#barrier_ready(903)
#neighbors.broadcast("cmd", 903)
} else if(value==904 and BVMSTATE!="IDLE" and BVMSTATE!="TURNEDOFF"){ # idle

View File

@ -276,7 +276,7 @@ function aggregate() {
# State fucntion to circle all together around centroid
function pursuit() {
BVMSTATE="PURSUIT"
rd = 20.0
rd = 30.0
pc = 3.2
vd = 15.0
r_vec = neighbors.reduce(function(rid, data, r_vec) {

View File

@ -11,8 +11,9 @@ include "taskallocate/graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2
graph_id = 3
GRAPHSTATE="NONE"
ROOT_ID = 0
graph_id = 0
graph_loop = 1
#
@ -32,12 +33,12 @@ m_MessageBearing={}#store received neighbour message
m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_receivedMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
#
#Save the message to send
#The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#navigation vector
m_navigation={.x=0,.y=0}
@ -240,7 +241,7 @@ neighbors.listen("m",
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
m_MessageState[m_neighbourCount]=i2s_graph(recv_val.State)
m_MessageLabel[m_neighbourCount]=recv_val.Label
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
@ -249,7 +250,7 @@ neighbors.listen("m",
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid
#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
m_neighbourCount=m_neighbourCount+1
})
@ -270,7 +271,7 @@ function Get_DisAndAzi(t_id){
#Update node info according to neighbour robots
#
function UpdateNodeInfo(){
#Collect informaiton
#Collect information
#Update information
i=0
@ -301,13 +302,13 @@ function UpdateNodeInfo(){
# Do free
#
function DoFree() {
if(BVMSTATE!="GRAPH_FREE"){
BVMSTATE="GRAPH_FREE"
if(GRAPHSTATE!="GRAPH_FREE"){
GRAPHSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
}else{
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
@ -316,28 +317,28 @@ function DoFree() {
#find a set of joined robots
var setJoinedLabels={}
var setJoinedIndexes={}
i=0
j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="GRAPH_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
neighbor_it=0
joined_it=0
while(neighbor_it<m_neighbourCount){
if(m_MessageState[neighbor_it]=="GRAPH_JOINED"){
setJoinedLabels[joined_it]=m_MessageLabel[neighbor_it]
setJoinedIndexes[joined_it]=neighbor_it
joined_it=joined_it+1
}
i=i+1
neighbor_it=neighbor_it+1
}
#go through the graph to look for a proper Label
var unFoundLabel=0
# var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLabel==0)){
vecNode_it=1
while(vecNode_it<size(m_vecNodes) and (unFoundLabel==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
unFoundLabel=m_vecNodes[i].Label
if(m_vecNodes[vecNode_it].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[vecNode_it].Pred)==1){
unFoundLabel=m_vecNodes[vecNode_it].Label
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
}
i=i+1
vecNode_it=vecNode_it+1
}
if(unFoundLabel>0){
@ -347,7 +348,7 @@ function DoFree() {
}
#set message
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
BroadcastGraph()
}
@ -356,11 +357,11 @@ function DoFree() {
#Do asking
#
function DoAsking(){
if(BVMSTATE!="GRAPH_ASKING"){
BVMSTATE="GRAPH_ASKING"
if(GRAPHSTATE!="GRAPH_ASKING"){
GRAPHSTATE="GRAPH_ASKING"
#math.rng.setseed(id)
m_unRequestId=id#math.rng.uniform(0,10) #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
@ -390,7 +391,7 @@ function DoAsking(){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){
@ -428,9 +429,9 @@ function DoAsking(){
#Do joining
#
function DoJoining(){
if(BVMSTATE!="GRAPH_JOINING") {
BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(BVMSTATE)
if(GRAPHSTATE!="GRAPH_JOINING") {
GRAPHSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
} else {
@ -440,7 +441,7 @@ function DoJoining(){
else
goto_gps(DoJoined)
#pack the communication package
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.Label=m_nLabel
BroadcastGraph()
}
@ -477,9 +478,9 @@ function set_rc_goto() {
#Do joined
#
function DoJoined(){
if(BVMSTATE!="GRAPH_JOINED"){
BVMSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i(BVMSTATE)
if(GRAPHSTATE!="GRAPH_JOINED"){
GRAPHSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
@ -492,7 +493,7 @@ function DoJoined(){
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
} else {
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
@ -571,7 +572,7 @@ function DoJoined(){
}
}
# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
barrier_wait_graph(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
BroadcastGraph()
}
}
@ -581,8 +582,8 @@ function DoJoined(){
timeout_graph = 40
function DoLock() {
#[transition to lock...
BVMSTATE="GRAPH_LOCK"
m_selfMessage.State=s2i(BVMSTATE)
GRAPHSTATE="GRAPH_LOCK"
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
@ -602,7 +603,7 @@ function DoLock() {
neighbors.ignore("m")
#..]
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.State=s2i_graph(GRAPHSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
@ -621,12 +622,12 @@ function DoLock() {
if(graph_loop) {
if(timeout_graph==0) {
if(graph_id < 3)
if(graph_id < 1)
graph_id = graph_id + 1
else
graph_id = 0
timeout_graph = 40
BVMSTATE="TASK_ALLOCATE"
GRAPHSTATE="NONE"
}
timeout_graph = timeout_graph - 1
}
@ -652,7 +653,7 @@ function UpdateGraph() {
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
}
function BroadcastGraph() {
@ -676,8 +677,8 @@ function BroadcastGraph() {
# Executed when reset
#
function resetGraph(){
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_receivedMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_navigation={.x=0,.y=0}
m_nLabel=-1
m_messageID={}
@ -695,15 +696,15 @@ function resetGraph(){
if(graph_id==0){
log("Loading P graph")
Read_GraphP()
# } else if(graph_id==1) {
# log("Loading O graph")
# Read_GraphO()
} else if(graph_id==1) {
log("Loading O graph")
Read_GraphO()
} else if(graph_id==2) {
log("Loading L graph")
Read_GraphL()
} else if(graph_id==3) {
log("Loading Y graph")
Read_GraphY()
# } else if(graph_id==3) {
# log("Loading Y graph")
# Read_GraphY()
}
#start listening
@ -719,6 +720,25 @@ function resetGraph(){
DoFree()
}
}
function graph_state(){
if(GRAPHSTATE=="GRAPH_FREE")
stateG=DoFree
else if(GRAPHSTATE=="GRAPH_ASKING")
stateG=DoAsking
else if(GRAPHSTATE=="GRAPH_JOINING")
stateG=DoJoining
else if(GRAPHSTATE=="GRAPH_JOINED")
stateG=DoJoined
else if(GRAPHSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
stateG=DoLock
else
stateG=resetGraph
stateG()
log("Current graph state: ", GRAPHSTATE)
}
#
# Executed upon destroy
#

View File

@ -62,17 +62,14 @@ function gps_from_vec(vec) {
return Lgoal
}
GPSoffset = {.lat=45.50, .lng=-73.61}
GPSoffsetL = {.lat=29.01, .lng=-13.70}
function packWP2i(in_lat, in_long, processed) {
var dlat = math.round((in_lat - GPSoffsetL.lat)*1000000)
var dlon = math.round((in_long - GPSoffsetL.lng)*1000000)
var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
var dlon = math.round((in_long - GPSoffset.lng)*1000000)
return {.dla=dlat, .dlo=dlon*10+processed}
}
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+GPSoffsetL.lat, .lng=dlon/1000000.0+GPSoffsetL.lng, .pro=pro}
return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lng=dlon/1000000.0+GPSoffset.lng, .pro=pro}
}

View File

@ -0,0 +1,20 @@
# Geofence polygon
# CEPSUM field
#GPSlimit = {.1={.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}}
# Pangeae final site
#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315},
# .2={.lat=29.068724, .lng=-13.662634},
# .3={.lat=29.068113, .lng=-13.661427},
# .4={.lat=29.067014, .lng=-13.661564}}
# Pangeae first site
GPSlimit = {.1={.lat=29.021055, .lng=-13.715155},
.2={.lat=29.021055, .lng=-13.714132},
.3={.lat=29.019470, .lng=-13.714132},
.4={.lat=29.019470, .lng=-13.715240}}
#GPSoffset = {.lat=45.50, .lng=-73.61}
GPSoffset = {.lat=29.01, .lng=-13.70}

View File

@ -34,7 +34,9 @@ function count(table,value){
# map from int to state - vstig serialization limits to 9....
#
function i2s(value){
if(value==1){
if(value==0){
return "PURSUIT"
}else if(value==1){
return "IDLE"
}
else if(value==2){
@ -50,7 +52,7 @@ function i2s(value){
return "BARRIERWAIT"
}
else if(value==6){
return "INDIWP"
return "WAYPOINT"
}
else if(value==7){
return "GOHOME"
@ -59,7 +61,27 @@ function i2s(value){
return "LAUNCH"
}
else if(value==9){
return "FORMATION"
return "TASK_ALLOCATE"
}
else {
return "UNDETERMINED"
}
}
function i2s_graph(value){
if(value==1){
return "GRAPH_FREE"
}
else if(value==2){
return "GRAPH_ASKING"
}
else if(value==3){
return "GRAPH_JOINING"
}
else if(value==4){
return "GRAPH_JOINED"
}
else if(value==5){
return "GRAPH_LOCK"
}
else {
return "UNDETERMINED"
@ -69,7 +91,9 @@ function i2s(value){
# map from state to int - vstig serialization limits to 9....
#
function s2i(value){
if(value=="IDLE"){
if(value=="PURSUIT"){
return 0
}else if(value=="IDLE"){
return 1
}
else if(value=="DEPLOY"){
@ -93,9 +117,29 @@ function s2i(value){
else if(value=="LAUNCH"){
return 8
}
else if(value=="POTENTIAL"){
else if(value=="TASK_ALLOCATE"){
return 9
}
else
return 0
return -1
}
function s2i_graph(value){
if(value=="GRAPH_FREE"){
return 1
}
else if(value=="GRAPH_ASKING"){
return 2
}
else if(value=="GRAPH_JOINING"){
return 3
}
else if(value=="GRAPH_JOINED"){
return 4
}
else if(value=="GRAPH_LOCK"){
return 5
}
else {
return 0
}
}

View File

@ -80,21 +80,11 @@ function step() {
else if(BVMSTATE=="PURSUIT")
statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
statef=resetGraph
statef=graph_state
else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz
statef=bidding
else if(BVMSTATE=="DEPLOY") # check the absolute path of the waypointlist csv file in bidding.bzz
statef=voronoicentroid
else if(BVMSTATE=="GRAPH_FREE")
statef=DoFree
else if(BVMSTATE=="GRAPH_ASKING")
statef=DoAsking
else if(BVMSTATE=="GRAPH_JOINING")
statef=DoJoining
else if(BVMSTATE=="GRAPH_JOINED")
statef=DoJoined
else if(BVMSTATE=="GRAPH_LOCK") # move all together (TODO: not tested), but auto-loop in graph list
statef=DoLock
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar

View File

@ -12,7 +12,7 @@
#include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h>
#include <buzz_utility.h>
#include <rosbuzz/buzz_utility.h>
#include <fstream>
#define delete_p(p) \

View File

@ -1,8 +1,8 @@
#pragma once
#include <stdio.h>
#include "buzz_utility.h"
#include "buzzuav_closures.h"
#include "buzz_update.h"
#include <rosbuzz/buzz_utility.h>
#include <rosbuzz/buzzuav_closures.h>
#include <rosbuzz/buzz_update.h>
#include <buzz/buzzdebug.h>
#include <stdlib.h>
#include <string.h>
@ -100,6 +100,8 @@ buzzvm_t get_vm();
void set_robot_var(int ROBOTS);
void set_ca_on_var(int CA_ON);
int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector();

View File

@ -5,8 +5,8 @@
#include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/Mavlink.h"
#include "ros/ros.h"
#include "buzz_utility.h"
#include "rosbuzz/mavrosCC.h"
#include <rosbuzz/buzz_utility.h>
#include <rosbuzz/mavrosCC.h>
#define EARTH_RADIUS (double)6371000.0
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))

View File

@ -35,8 +35,8 @@
#include <signal.h>
#include <ostream>
#include <map>
#include "buzzuav_closures.h"
#include "rosbuzz/mavrosCC.h"
#include <rosbuzz/buzzuav_closures.h>
#include <rosbuzz/mavrosCC.h>
/*
* ROSBuzz message types
@ -131,6 +131,7 @@ private:
int rc_cmd;
float fcu_timeout;
int armstate;
int ca_on;
int barrier;
int update;
int message_number = 0;

View File

@ -12,6 +12,7 @@
<arg name="setmode" default="false" />
<arg name="latitude" default="0" />
<arg name="longitude" default="0" />
<arg name="ca_on" default="2" />
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
@ -24,5 +25,6 @@
<param name="latitude" value="$(arg latitude)"/>
<param name="longitude" value="$(arg longitude)"/>
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
<param name="ca_on" value="$(arg ca_on)"/>
</node>
</launch>

View File

@ -27,7 +27,7 @@
* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
*/
#include "VoronoiDiagramGenerator.h"
#include <rosbuzz/VoronoiDiagramGenerator.h>
VoronoiDiagramGenerator::VoronoiDiagramGenerator()
{

View File

@ -6,7 +6,7 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzz_update.h"
#include <rosbuzz/buzz_update.h>
namespace buzz_update
{

View File

@ -6,7 +6,7 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzz_utility.h"
#include <rosbuzz/buzz_utility.h>
namespace buzz_utility
{
@ -565,6 +565,16 @@ void set_robot_var(int ROBOTS)
buzzvm_gstore(VM);
}
void set_ca_on_var(int CA_ON)
/*
/ set swarm size in the BVM
-----------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "CA_ON", 1));
buzzvm_pushi(VM, CA_ON);
buzzvm_gstore(VM);
}
int get_inmsg_size()
/*
/ get the incoming msgs size

View File

@ -6,9 +6,9 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include "buzzuav_closures.h"
#include <rosbuzz/buzzuav_closures.h>
#include "math.h"
#include "VoronoiDiagramGenerator.h"
#include <rosbuzz/VoronoiDiagramGenerator.h>
namespace buzzuav_closures
{

View File

@ -6,7 +6,7 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include "roscontroller.h"
#include <rosbuzz/roscontroller.h>
int main(int argc, char** argv)
/*

View File

@ -6,7 +6,7 @@
* @copyright 2016 MistLab. All rights reserved.
*/
#include "roscontroller.h"
#include <rosbuzz/roscontroller.h>
#include <thread>
namespace rosbuzz_node
{
@ -130,6 +130,7 @@ void roscontroller::RosControllerRun()
ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
// DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
buzz_utility::set_ca_on_var(ca_on);
while (ros::ok() && !buzz_utility::buzz_script_done())
{
// Publish topics
@ -300,6 +301,9 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
// Obtain standby script to run during update
n_c.getParam("stand_by", stand_by);
// Obtain collision avoidance mode
n_c.getParam("ca_on", ca_on);
if (n_c.getParam("xbee_plugged", xbeeplugged))
;
else