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

ported AGF to new rosbuzz file structure

This commit is contained in:
dave 2018-03-02 15:35:06 -05:00
parent 48a2dc53b3
commit fd7414f382
4 changed files with 182 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -73,17 +73,17 @@ function take_picture() {
} }
function goto_gps(transf) { 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)) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
log("Sorry this is too far.") log("Sorry this is too far.")
else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity 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)) 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 } else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf() transf()
else 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() { function follow() {
@ -94,7 +94,7 @@ function follow() {
force=(0.05)*(tab.range)^4 force=(0.05)*(tab.range)^4
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) 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 { } else {
log("No target in local table!") log("No target in local table!")
BVMSTATE = "IDLE" BVMSTATE = "IDLE"

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

@ -1,18 +1,11 @@
# #
# Include files # Include files
# #
include "string.bzz"
include "vec2.bzz"
include "update.bzz"
#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. include "taskallocate/graphs/shapes_P.bzz"
include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. include "taskallocate/graphs/shapes_O.bzz"
include "uavstates.bzz" # require an 'action' function to be defined here. include "taskallocate/graphs/shapes_L.bzz"
include "taskallocate/graphs/shapes_Y.bzz"
include "graphs/shapes_P.bzz"
include "graphs/shapes_O.bzz"
include "graphs/shapes_L.bzz"
include "graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50 ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
@ -20,9 +13,6 @@ ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2 ROOT_ID = 2
old_state = -1 old_state = -1
# max velocity in cm/step
ROBOT_MAXVEL = 150.0
# #
# Global variables # Global variables
# #
@ -40,12 +30,12 @@ m_MessageBearing={}#store received neighbour message
m_neighbourCount=0#used to cunt neighbours m_neighbourCount=0#used to cunt neighbours
#Save message from one neighbour #Save message from one neighbour
#the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing #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 #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}) #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 #navigation vector
m_navigation={.x=0,.y=0} m_navigation={.x=0,.y=0}
@ -117,38 +107,38 @@ function count(table,value){
# #
function i2s(value){ function i2s(value){
if(value==1){ if(value==1){
return "STATE_FREE" return "GRAPH_FREE"
} }
else if(value==2){ else if(value==2){
return "STATE_ASKING" return "GRAPH_ASKING"
} }
else if(value==3){ else if(value==3){
return "STATE_JOINING" return "GRAPH_JOINING"
} }
else if(value==4){ else if(value==4){
return "STATE_JOINED" return "GRAPH_JOINED"
} }
else if(value==5){ else if(value==5){
return "STATE_LOCK" return "GRAPH_LOCK"
} }
} }
# #
#map from state to int #map from state to int
# #
function s2i(value){ function s2i(value){
if(value=="STATE_FREE"){ if(value=="GRAPH_FREE"){
return 1 return 1
} }
else if(value=="STATE_ASKING"){ else if(value=="GRAPH_ASKING"){
return 2 return 2
} }
else if(value=="STATE_JOINING"){ else if(value=="GRAPH_JOINING"){
return 3 return 3
} }
else if(value=="STATE_JOINED"){ else if(value=="GRAPH_JOINED"){
return 4 return 4
} }
else if(value=="STATE_LOCK"){ else if(value=="GRAPH_LOCK"){
return 5 return 5
} }
} }
@ -344,11 +334,11 @@ function UpdateNodeInfo(){
var i=0 var i=0
while(i<m_neighbourCount){ 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]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod 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]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
} }
@ -369,18 +359,18 @@ function UpdateNodeInfo(){
#Transistion to state free #Transistion to state free
# #
function TransitionToFree(){ function TransitionToFree(){
UAVSTATE="STATE_FREE" BVMSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
} }
# #
#Transistion to state asking #Transistion to state asking
# #
function TransitionToAsking(un_label){ function TransitionToAsking(un_label){
UAVSTATE="STATE_ASKING" BVMSTATE="GRAPH_ASKING"
m_nLabel=un_label 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_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.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId m_selfMessage.ReqID=m_unRequestId
@ -390,8 +380,8 @@ function TransitionToAsking(un_label){
#Transistion to state joining #Transistion to state joining
# #
function TransitionToJoining(){ function TransitionToJoining(){
UAVSTATE="STATE_JOINING" BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod m_unWaitCount=m_unJoiningLostPeriod
} }
@ -399,26 +389,26 @@ function TransitionToJoining(){
#Transistion to state joined #Transistion to state joined
# #
function TransitionToJoined(){ function TransitionToJoined(){
UAVSTATE="STATE_JOINED" BVMSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED" m_vecNodes[m_nLabel].State="ASSIGNED"
#write statues #write statues
#v_tag.put(m_nLabel, m_lockstig) #v_tag.put(m_nLabel, m_lockstig)
barrier_create() barrier_create()
barrier_ready() barrier_ready()
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=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 #Transistion to state Lock, lock the current formation
# #
function TransitionToLock(){ function TransitionToLock(){
UAVSTATE="STATE_LOCK" BVMSTATE="GRAPH_LOCK"
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED" m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance #record neighbor distance
@ -432,7 +422,7 @@ function TransitionToLock(){
} }
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=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 # prepare to restart a new shape
old_state = rc_State old_state = rc_State
@ -443,7 +433,8 @@ function TransitionToLock(){
# Do free # Do free
# #
function DoFree() { function DoFree() {
m_selfMessage.State=s2i(UAVSTATE) UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
#wait for a while before looking for a Label #wait for a while before looking for a Label
if(m_unWaitCount>0) if(m_unWaitCount>0)
@ -455,7 +446,7 @@ function DoFree() {
var i=0 var i=0
var j=0 var j=0
while(i<m_neighbourCount){ while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){ if(m_MessageState[i]=="GRAPH_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i] setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i setJoinedIndexes[j]=i
j=j+1 j=j+1
@ -482,13 +473,15 @@ function DoFree() {
} }
#set message #set message
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph()
} }
# #
#Do asking #Do asking
# #
function DoAsking(){ function DoAsking(){
UpdateGraph()
#look for response from predecessor #look for response from predecessor
var i=0 var i=0
var psResponse=-1 var psResponse=-1
@ -496,13 +489,13 @@ function DoAsking(){
#the respond robot in joined state #the respond robot in joined state
#the request Label be the same as requesed #the request Label be the same as requesed
#get a respond #get a respond
if(m_MessageState[i]=="STATE_JOINED"){ if(m_MessageState[i]=="GRAPH_JOINED"){
#log("received label = ",m_MessageReqLabel[i]) #log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel) if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){ if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i 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() TransitionToFree()
return return
} }
@ -513,7 +506,7 @@ function DoAsking(){
#no response, wait #no response, wait
m_unWaitCount=m_unWaitCount-1 m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){ #if(m_unWaitCount==0){
@ -543,20 +536,23 @@ function DoAsking(){
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=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 #Do joining
# #
function DoJoining(){ function DoJoining(){
UpdateGraph()
if(m_gotjoinedparent!=1) if(m_gotjoinedparent!=1)
set_rc_goto() set_rc_goto()
else else
gotoWP(TransitionToJoined) goto_gps(TransitionToJoined)
#pack the communication package #pack the communication package
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
BroadcastGraph()
} }
function set_rc_goto() { function set_rc_goto() {
@ -564,7 +560,7 @@ function set_rc_goto() {
var i=0 var i=0
var IDofPred=-1 var IDofPred=-1
while(i<m_neighbourCount and 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 IDofPred=i
i=i+1 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))) 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) 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 m_gotjoinedparent = 1
} }
} }
@ -590,7 +586,8 @@ function set_rc_goto() {
#Do joined #Do joined
# #
function DoJoined(){ function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE) UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
#collect all requests #collect all requests
@ -601,7 +598,7 @@ function DoJoined(){
var JoiningLabel var JoiningLabel
var seenPred=0 var seenPred=0
while(i<m_neighbourCount){ while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){ if(m_MessageState[i]=="GRAPH_ASKING"){
ReqLabel=m_MessageReqLabel[i] ReqLabel=m_MessageReqLabel[i]
#log("ReqLabel var:",ReqLabel) #log("ReqLabel var:",ReqLabel)
#log("M_vec var",m_vecNodes[ReqLabel].State) #log("M_vec var",m_vecNodes[ReqLabel].State)
@ -612,7 +609,7 @@ function DoJoined(){
j=j+1 j=j+1
} }
} }
if(m_MessageState[i]=="STATE_JOINING"){ if(m_MessageState[i]=="GRAPH_JOINING"){
JoiningLabel=m_MessageLabel[i] JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){ if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing ##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 repeat_assign=0
} }
#if it is the pred #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 seenPred=1
m_unWaitCount=m_unJoiningLostPeriod m_unWaitCount=m_unJoiningLostPeriod
} }
@ -676,13 +673,15 @@ function DoJoined(){
#if(v_tag.size()==ROBOTS){ #if(v_tag.size()==ROBOTS){
# TransitionToLock() # TransitionToLock()
#} #}
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1) barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
BroadcastGraph()
} }
# #
#Do Lock #Do Lock
# #
function DoLock(){ function DoLock(){
m_selfMessage.State=s2i(UAVSTATE) UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
@ -695,24 +694,22 @@ function DoLock(){
m_navigation=motion_vector() m_navigation=motion_vector()
} }
#move #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 # Executed after takeoff
# #
function action(){ function startGraph(){
BVMSTATE="GRAPH_FREE"
statef=action
UAVSTATE="STATE_FREE"
# reset the graph # reset the graph
Reset() statef=resetGraph
} }
# #
# Executed at init # Executed at init
# #
function init() { function initGraph() {
# #
# Global parameters for graph formation # Global parameters for graph formation
# #
@ -721,55 +718,18 @@ function init() {
m_fTargetDistanceTolerance=100 m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1 m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100 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) # Executed every step (main loop)
# #
function step() { function UpdateGraph() {
# listen to potential RC
uav_rccmd()
# get the swarm commands
uav_neicmd()
# update the vstig (status/net/batt)
#uav_updatestig()
#update the graph #update the graph
UpdateNodeInfo() UpdateNodeInfo()
#reset message package to be sent #reset message package to be sent
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")}
# }
# 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 function BroadcastGraph() {
statef()
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
log("Swarm size: ", ROBOTS)
#navigation #navigation
#broadcast message #broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage)) neighbors.broadcast("m",packmessage(m_selfMessage))
@ -789,9 +749,9 @@ function step() {
# #
# Executed when reset # Executed when reset
# #
function Reset(){ function resetGraph(){
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}
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")}
m_navigation={.x=0,.y=0} m_navigation={.x=0,.y=0}
m_nLabel=-1 m_nLabel=-1
m_messageID={} m_messageID={}
@ -838,11 +798,11 @@ function Reset(){
# #
# Executed upon destroy # Executed upon destroy
# #
function destroy() { function destroyGraph() {
#clear neighbour message #clear neighbour message
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=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={} m_vecNodes={}
#stop listening #stop listening
neighbors.ignore("m") neighbors.ignore("m")

33
buzz_scripts/main.bzz
View File

@ -3,10 +3,11 @@ include "update.bzz"
# it requires an 'action' function to be defined here. # it requires an 'action' function to be defined here.
include "act/states.bzz" include "act/states.bzz"
include "plan/rrtstar.bzz" include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz" include "vstigenv.bzz"
#State launched after takeoff #State launched after takeoff
AUTO_LAUNCH_STATE = "IDLE" AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
##### #####
# Vehicule type: # Vehicule type:
@ -28,11 +29,10 @@ function init() {
TARGET_ALTITUDE = 25.0 # m TARGET_ALTITUDE = 25.0 # m
# start the swarm command listener # 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. # Starting state: TURNEDOFF to wait for user input.
# BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
BVMSTATE = "LAUNCHED"
} }
# Executed at each time step. # Executed at each time step.
@ -44,18 +44,32 @@ function step() {
# uav_updatestig() # uav_updatestig()
# #
# graph state machine # State machine
# #
if(BVMSTATE=="TURNEDOFF") if(BVMSTATE=="TURNEDOFF")
statef=turnedoff statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop statef=stop
else if(BVMSTATE=="LAUNCHED") # ends on AUTO_LAUNCH_STATE else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch statef=launch
else if(BVMSTATE=="IDLE") else if(BVMSTATE=="IDLE")
statef=idle statef=idle
else if(BVMSTATE=="TASK_ALLOCATE") #TODO: not tested in new structure else if(BVMSTATE=="TASK_ALLOCATE") { # or bidding ?
statef=makegraph # 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 else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar statef=rrtstar
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
@ -64,6 +78,7 @@ function step() {
statef=follow statef=follow
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
statef=take_picture statef=take_picture
statef() statef()

80
buzz_scripts/mainRRT.bzz Normal file
View File

@ -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() {
}