move rosbuzz headers and reneabled pursuit and graph formation from webcontrol
This commit is contained in:
dave
parent
499484778b
commit
073e107da7
|
|
@ -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){
|
||||
|
|
@ -48,4 +48,187 @@ 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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
}
|
||||
|
|
@ -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}
|
||||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) \
|
||||
|
|
@ -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();
|
||||
|
|
@ -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)))
|
||||
|
|
@ -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;
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
* OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
|
||||
*/
|
||||
|
||||
#include "VoronoiDiagramGenerator.h"
|
||||
#include <rosbuzz/VoronoiDiagramGenerator.h>
|
||||
|
||||
VoronoiDiagramGenerator::VoronoiDiagramGenerator()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
|
||||
#include "buzz_update.h"
|
||||
#include <rosbuzz/buzz_update.h>
|
||||
|
||||
namespace buzz_update
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
|
||||
#include "roscontroller.h"
|
||||
#include <rosbuzz/roscontroller.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue