ROSBuzz_MISTLab/buzz_scripts/main.bzz

120 lines
2.9 KiB
Plaintext

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 "utils/table.bzz"
include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "taskallocate/bidding.bzz"
include "vstigenv.bzz"
#include "timesync.bzz"
include "utils/takeoff_heights.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "INDIWP"
TARGET = 9.0
EPSILON = 30.0
ROOT_ID = 3
graph_id = 3
graph_loop = 0
LAND_AFTER_BARRIER_EXPIRE = 1 # if set to be 1 , the robots will land after barrier expire; if set to be 0, the robots will carry on to AUTO_LAUNCH_STATE.
#####
# Vehicule type:
# 0 -> outdoor flying vehicle
# 1 -> indoor flying vehicle
# 2 -> outdoor wheeled vehicle
# 3 -> indoor wheeled vehicle
V_TYPE = 0
# Executed once at init time.
function init() {
init_stig()
init_swarm()
init_bidding()
TARGET_ALTITUDE = takeoff_heights[id]
# start the swarm command listener
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
}
# Executed at each time step.
function step() {
# listen to Remote Controller
rc_cmd_listen()
# update the vstig (status/net/batt/...)
#uav_updatestig()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="CUSFUN")
statef=cusfun
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
if(LAND_AFTER_BARRIER_EXPIRE == 1)
statef=launch
else
statef=launch_switch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome
else if(BVMSTATE=="INDIWP")
statef=indiWP
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="AGGREGATE")
statef=aggregate
else if(BVMSTATE=="FORMATION")
statef=formation
else if(BVMSTATE=="PURSUIT")
statef=pursuit
else if(BVMSTATE=="TASK_ALLOCATE") # or bidding ?
statef=resetGraph
else if(BVMSTATE=="BIDDING") # check the absolute path of the waypointlist csv file in bidding.bzz
statef=bidding
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
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() {
close_bidding()
}