ROSBuzz_MISTLab/buzz_scripts/mainRRT.bzz

81 lines
1.8 KiB
Plaintext
Raw Normal View History

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