ported AGF to new rosbuzz file structure
This commit is contained in:
dave
parent
48a2dc53b3
commit
fd7414f382
|
|
@ -73,17 +73,17 @@ function take_picture() {
|
|||
}
|
||||
|
||||
function goto_gps(transf) {
|
||||
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
|
||||
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.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.")
|
||||
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
|
||||
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
|
||||
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
||||
transf()
|
||||
else
|
||||
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
|
||||
}
|
||||
|
||||
function follow() {
|
||||
|
|
@ -94,7 +94,7 @@ function follow() {
|
|||
force=(0.05)*(tab.range)^4
|
||||
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
|
||||
})
|
||||
uav_moveto(attractor.x, attractor.y, 0.0)
|
||||
goto_abs(attractor.x, attractor.y, 0.0)
|
||||
} else {
|
||||
log("No target in local table!")
|
||||
BVMSTATE = "IDLE"
|
||||
|
|
|
|||
|
|
@ -1,18 +1,11 @@
|
|||
#
|
||||
# Include files
|
||||
#
|
||||
include "string.bzz"
|
||||
include "vec2.bzz"
|
||||
include "update.bzz"
|
||||
|
||||
#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header.
|
||||
include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
|
||||
include "graphs/shapes_P.bzz"
|
||||
include "graphs/shapes_O.bzz"
|
||||
include "graphs/shapes_L.bzz"
|
||||
include "graphs/shapes_Y.bzz"
|
||||
include "taskallocate/graphs/shapes_P.bzz"
|
||||
include "taskallocate/graphs/shapes_O.bzz"
|
||||
include "taskallocate/graphs/shapes_L.bzz"
|
||||
include "taskallocate/graphs/shapes_Y.bzz"
|
||||
|
||||
ROBOT_RADIUS = 50
|
||||
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
||||
|
|
@ -20,9 +13,6 @@ ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
|
|||
ROOT_ID = 2
|
||||
old_state = -1
|
||||
|
||||
# max velocity in cm/step
|
||||
ROBOT_MAXVEL = 150.0
|
||||
|
||||
#
|
||||
# Global variables
|
||||
#
|
||||
|
|
@ -40,12 +30,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("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
|
||||
m_receivedMessage={.State=s2i("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("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
|
||||
#navigation vector
|
||||
m_navigation={.x=0,.y=0}
|
||||
|
|
@ -117,38 +107,38 @@ function count(table,value){
|
|||
#
|
||||
function i2s(value){
|
||||
if(value==1){
|
||||
return "STATE_FREE"
|
||||
return "GRAPH_FREE"
|
||||
}
|
||||
else if(value==2){
|
||||
return "STATE_ASKING"
|
||||
return "GRAPH_ASKING"
|
||||
}
|
||||
else if(value==3){
|
||||
return "STATE_JOINING"
|
||||
return "GRAPH_JOINING"
|
||||
}
|
||||
else if(value==4){
|
||||
return "STATE_JOINED"
|
||||
return "GRAPH_JOINED"
|
||||
}
|
||||
else if(value==5){
|
||||
return "STATE_LOCK"
|
||||
return "GRAPH_LOCK"
|
||||
}
|
||||
}
|
||||
#
|
||||
#map from state to int
|
||||
#
|
||||
function s2i(value){
|
||||
if(value=="STATE_FREE"){
|
||||
if(value=="GRAPH_FREE"){
|
||||
return 1
|
||||
}
|
||||
else if(value=="STATE_ASKING"){
|
||||
else if(value=="GRAPH_ASKING"){
|
||||
return 2
|
||||
}
|
||||
else if(value=="STATE_JOINING"){
|
||||
else if(value=="GRAPH_JOINING"){
|
||||
return 3
|
||||
}
|
||||
else if(value=="STATE_JOINED"){
|
||||
else if(value=="GRAPH_JOINED"){
|
||||
return 4
|
||||
}
|
||||
else if(value=="STATE_LOCK"){
|
||||
else if(value=="GRAPH_LOCK"){
|
||||
return 5
|
||||
}
|
||||
}
|
||||
|
|
@ -344,11 +334,11 @@ function UpdateNodeInfo(){
|
|||
var i=0
|
||||
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
if(m_MessageState[i]=="GRAPH_JOINED"){
|
||||
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
|
||||
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
else if(m_MessageState[i]=="STATE_JOINING"){
|
||||
else if(m_MessageState[i]=="GRAPH_JOINING"){
|
||||
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
|
||||
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
|
|
@ -369,18 +359,18 @@ function UpdateNodeInfo(){
|
|||
#Transistion to state free
|
||||
#
|
||||
function TransitionToFree(){
|
||||
UAVSTATE="STATE_FREE"
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
m_unWaitCount=m_unLabelSearchWaitTime
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
}
|
||||
#
|
||||
#Transistion to state asking
|
||||
#
|
||||
function TransitionToAsking(un_label){
|
||||
UAVSTATE="STATE_ASKING"
|
||||
BVMSTATE="GRAPH_ASKING"
|
||||
m_nLabel=un_label
|
||||
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
|
||||
|
|
@ -390,8 +380,8 @@ function TransitionToAsking(un_label){
|
|||
#Transistion to state joining
|
||||
#
|
||||
function TransitionToJoining(){
|
||||
UAVSTATE="STATE_JOINING"
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
BVMSTATE="GRAPH_JOINING"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
|
|
@ -399,26 +389,26 @@ function TransitionToJoining(){
|
|||
#Transistion to state joined
|
||||
#
|
||||
function TransitionToJoined(){
|
||||
UAVSTATE="STATE_JOINED"
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
BVMSTATE="GRAPH_JOINED"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
|
||||
#write statues
|
||||
#v_tag.put(m_nLabel, m_lockstig)
|
||||
barrier_create()
|
||||
barrier_create()
|
||||
barrier_ready()
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
#
|
||||
#Transistion to state Lock, lock the current formation
|
||||
#
|
||||
function TransitionToLock(){
|
||||
UAVSTATE="STATE_LOCK"
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
BVMSTATE="GRAPH_LOCK"
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
#record neighbor distance
|
||||
|
|
@ -432,7 +422,7 @@ function TransitionToLock(){
|
|||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0)
|
||||
|
||||
# prepare to restart a new shape
|
||||
old_state = rc_State
|
||||
|
|
@ -443,7 +433,8 @@ function TransitionToLock(){
|
|||
# Do free
|
||||
#
|
||||
function DoFree() {
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
|
||||
#wait for a while before looking for a Label
|
||||
if(m_unWaitCount>0)
|
||||
|
|
@ -455,7 +446,7 @@ function DoFree() {
|
|||
var i=0
|
||||
var j=0
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
if(m_MessageState[i]=="GRAPH_JOINED"){
|
||||
setJoinedLabels[j]=m_MessageLabel[i]
|
||||
setJoinedIndexes[j]=i
|
||||
j=j+1
|
||||
|
|
@ -482,13 +473,15 @@ function DoFree() {
|
|||
}
|
||||
|
||||
#set message
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
#Do asking
|
||||
#
|
||||
function DoAsking(){
|
||||
UpdateGraph()
|
||||
#look for response from predecessor
|
||||
var i=0
|
||||
var psResponse=-1
|
||||
|
|
@ -496,13 +489,13 @@ function DoAsking(){
|
|||
#the respond robot in joined state
|
||||
#the request Label be the same as requesed
|
||||
#get a respond
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
if(m_MessageState[i]=="GRAPH_JOINED"){
|
||||
#log("received label = ",m_MessageReqLabel[i])
|
||||
if(m_MessageReqLabel[i]==m_nLabel)
|
||||
if(m_MessageResponse[i]!="REQ_NONE"){
|
||||
psResponse=i
|
||||
}}
|
||||
if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){
|
||||
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
|
|
@ -513,7 +506,7 @@ function DoAsking(){
|
|||
#no response, wait
|
||||
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
#if(m_unWaitCount==0){
|
||||
|
|
@ -543,20 +536,23 @@ function DoAsking(){
|
|||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
#Do joining
|
||||
#
|
||||
function DoJoining(){
|
||||
UpdateGraph()
|
||||
|
||||
if(m_gotjoinedparent!=1)
|
||||
set_rc_goto()
|
||||
else
|
||||
gotoWP(TransitionToJoined)
|
||||
goto_gps(TransitionToJoined)
|
||||
#pack the communication package
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
BroadcastGraph()
|
||||
}
|
||||
|
||||
function set_rc_goto() {
|
||||
|
|
@ -564,7 +560,7 @@ function set_rc_goto() {
|
|||
var 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]=="STATE_JOINED")
|
||||
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="GRAPH_JOINED")
|
||||
IDofPred=i
|
||||
i=i+1
|
||||
}
|
||||
|
|
@ -582,7 +578,7 @@ function set_rc_goto() {
|
|||
|
||||
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
|
||||
print("Saving GPS goal: ",goal.latitude, goal.longitude)
|
||||
uav_storegoal(goal.latitude, goal.longitude, position.altitude)
|
||||
uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude)
|
||||
m_gotjoinedparent = 1
|
||||
}
|
||||
}
|
||||
|
|
@ -590,7 +586,8 @@ function set_rc_goto() {
|
|||
#Do joined
|
||||
#
|
||||
function DoJoined(){
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
||||
#collect all requests
|
||||
|
|
@ -601,7 +598,7 @@ function DoJoined(){
|
|||
var JoiningLabel
|
||||
var seenPred=0
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="STATE_ASKING"){
|
||||
if(m_MessageState[i]=="GRAPH_ASKING"){
|
||||
ReqLabel=m_MessageReqLabel[i]
|
||||
#log("ReqLabel var:",ReqLabel)
|
||||
#log("M_vec var",m_vecNodes[ReqLabel].State)
|
||||
|
|
@ -612,7 +609,7 @@ function DoJoined(){
|
|||
j=j+1
|
||||
}
|
||||
}
|
||||
if(m_MessageState[i]=="STATE_JOINING"){
|
||||
if(m_MessageState[i]=="GRAPH_JOINING"){
|
||||
JoiningLabel=m_MessageLabel[i]
|
||||
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
|
||||
##joining wrt this dot,send the global bearing
|
||||
|
|
@ -620,13 +617,13 @@ function DoJoined(){
|
|||
}
|
||||
}
|
||||
|
||||
if(m_MessageState[i]=="STATE_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
|
||||
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
|
||||
repeat_assign=0
|
||||
}
|
||||
|
||||
|
||||
#if it is the pred
|
||||
if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
|
||||
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
|
||||
seenPred=1
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
|
|
@ -676,13 +673,15 @@ function DoJoined(){
|
|||
#if(v_tag.size()==ROBOTS){
|
||||
# TransitionToLock()
|
||||
#}
|
||||
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
|
||||
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
#Do Lock
|
||||
#
|
||||
function DoLock(){
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
UpdateGraph()
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
|
|
@ -695,24 +694,22 @@ function DoLock(){
|
|||
m_navigation=motion_vector()
|
||||
}
|
||||
#move
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0)
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
# Executed after takeoff
|
||||
#
|
||||
function action(){
|
||||
|
||||
statef=action
|
||||
UAVSTATE="STATE_FREE"
|
||||
function startGraph(){
|
||||
BVMSTATE="GRAPH_FREE"
|
||||
|
||||
# reset the graph
|
||||
Reset()
|
||||
statef=resetGraph
|
||||
}
|
||||
#
|
||||
# Executed at init
|
||||
#
|
||||
function init() {
|
||||
function initGraph() {
|
||||
#
|
||||
# Global parameters for graph formation
|
||||
#
|
||||
|
|
@ -721,55 +718,18 @@ function init() {
|
|||
m_fTargetDistanceTolerance=100
|
||||
m_fTargetAngleTolerance=0.1
|
||||
m_unJoiningLostPeriod=100
|
||||
|
||||
#
|
||||
# Join Swarm
|
||||
#
|
||||
uav_initswarm()
|
||||
#v_tag = stigmergy.create(m_lockstig)
|
||||
#uav_initstig()
|
||||
# go to diff. height since no collision avoidance implemented yet
|
||||
#TARGET_ALTITUDE = 20.0 + id * 2.5
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
}
|
||||
#
|
||||
# Executed every step (main loop)
|
||||
#
|
||||
function step() {
|
||||
# listen to potential RC
|
||||
uav_rccmd()
|
||||
# get the swarm commands
|
||||
uav_neicmd()
|
||||
# update the vstig (status/net/batt)
|
||||
#uav_updatestig()
|
||||
|
||||
function UpdateGraph() {
|
||||
#update the graph
|
||||
UpdateNodeInfo()
|
||||
#reset message package to be sent
|
||||
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
#
|
||||
# graph state machine
|
||||
#
|
||||
if(UAVSTATE=="STATE_FREE")
|
||||
statef=DoFree
|
||||
else if(UAVSTATE=="STATE_ASKING")
|
||||
statef=DoAsking
|
||||
else if(UAVSTATE=="STATE_JOINING")
|
||||
statef=DoJoining
|
||||
else if(UAVSTATE=="STATE_JOINED")
|
||||
statef=DoJoined
|
||||
else if(UAVSTATE=="STATE_LOCK" and old_state!=rc_State)
|
||||
statef=action
|
||||
else if(UAVSTATE=="STATE_LOCK" and old_state==rc_State)
|
||||
statef=DoLock
|
||||
|
||||
# high level UAV state machine
|
||||
statef()
|
||||
|
||||
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
|
||||
log("Swarm size: ", ROBOTS)
|
||||
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
}
|
||||
|
||||
function BroadcastGraph() {
|
||||
#navigation
|
||||
#broadcast message
|
||||
neighbors.broadcast("m",packmessage(m_selfMessage))
|
||||
|
|
@ -789,9 +749,9 @@ function step() {
|
|||
#
|
||||
# Executed when reset
|
||||
#
|
||||
function Reset(){
|
||||
m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
|
||||
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
|
||||
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_navigation={.x=0,.y=0}
|
||||
m_nLabel=-1
|
||||
m_messageID={}
|
||||
|
|
@ -838,11 +798,11 @@ function Reset(){
|
|||
#
|
||||
# Executed upon destroy
|
||||
#
|
||||
function destroy() {
|
||||
function destroyGraph() {
|
||||
#clear neighbour message
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
m_vecNodes={}
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
|
|
|
|||
|
|
@ -3,10 +3,11 @@ include "update.bzz"
|
|||
# it requires an 'action' function to be defined here.
|
||||
include "act/states.bzz"
|
||||
include "plan/rrtstar.bzz"
|
||||
include "taskallocate/graphformGPS.bzz"
|
||||
include "vstigenv.bzz"
|
||||
|
||||
#State launched after takeoff
|
||||
AUTO_LAUNCH_STATE = "IDLE"
|
||||
AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
|
||||
|
||||
#####
|
||||
# Vehicule type:
|
||||
|
|
@ -28,11 +29,10 @@ function init() {
|
|||
TARGET_ALTITUDE = 25.0 # m
|
||||
|
||||
# start the swarm command listener
|
||||
# nei_cmd_listen()
|
||||
nei_cmd_listen()
|
||||
|
||||
# Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup.
|
||||
# BVMSTATE = "TURNEDOFF"
|
||||
BVMSTATE = "LAUNCHED"
|
||||
# Starting state: TURNEDOFF to wait for user input.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
|
|
@ -44,18 +44,32 @@ function step() {
|
|||
# uav_updatestig()
|
||||
|
||||
#
|
||||
# graph state machine
|
||||
# State machine
|
||||
#
|
||||
if(BVMSTATE=="TURNEDOFF")
|
||||
statef=turnedoff
|
||||
else if(BVMSTATE=="STOP") # ends on turnedoff
|
||||
statef=stop
|
||||
else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE
|
||||
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
|
||||
statef=launch
|
||||
else if(BVMSTATE=="IDLE")
|
||||
statef=idle
|
||||
else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure
|
||||
statef=makegraph # or bidding
|
||||
else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ?
|
||||
startGraph()
|
||||
} 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_TRANSTOLOCK")
|
||||
statef=TransitionToLock
|
||||
else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State) #switch to a new graph
|
||||
startGraph()
|
||||
else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State) # move all together (TODO: not tested)
|
||||
statef=DoLock
|
||||
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
|
||||
statef=rrtstar
|
||||
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
|
||||
|
|
@ -65,6 +79,7 @@ function step() {
|
|||
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
|
||||
statef=take_picture
|
||||
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,80 @@
|
|||
include "update.bzz"
|
||||
# don't use a stigmergy id=11 with this header, for barrier
|
||||
# it requires an 'action' function to be defined here.
|
||||
include "act/states.bzz"
|
||||
include "plan/rrtstar.bzz"
|
||||
include "vstigenv.bzz"
|
||||
|
||||
#State launched after takeoff
|
||||
AUTO_LAUNCH_STATE = "IDLE"
|
||||
|
||||
#####
|
||||
# Vehicule type:
|
||||
# 0 -> outdoor flying vehicle
|
||||
# 1 -> indoor flying vehicle
|
||||
# 2 -> outdoor wheeled vehicle
|
||||
# 3 -> indoor wheeled vehicle
|
||||
V_TYPE = 0
|
||||
|
||||
goal_list = {
|
||||
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
init_stig()
|
||||
init_swarm()
|
||||
|
||||
TARGET_ALTITUDE = 25.0 # m
|
||||
|
||||
# start the swarm command listener
|
||||
# nei_cmd_listen()
|
||||
|
||||
# Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup.
|
||||
# BVMSTATE = "TURNEDOFF"
|
||||
BVMSTATE = "LAUNCHED"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
# listen to Remote Controller
|
||||
rc_cmd_listen()
|
||||
|
||||
# update the vstig (status/net/batt/...)
|
||||
# uav_updatestig()
|
||||
|
||||
#
|
||||
# graph state machine
|
||||
#
|
||||
if(BVMSTATE=="TURNEDOFF")
|
||||
statef=turnedoff
|
||||
else if(BVMSTATE=="STOP") # ends on turnedoff
|
||||
statef=stop
|
||||
else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE
|
||||
statef=launch
|
||||
else if(BVMSTATE=="IDLE")
|
||||
statef=idle
|
||||
else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure
|
||||
statef=makegraph # or bidding
|
||||
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
|
||||
statef=rrtstar
|
||||
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
|
||||
statef=navigate
|
||||
else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure
|
||||
statef=follow
|
||||
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
|
||||
statef=take_picture
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
Loading…
Reference in New Issue