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

added minimal ROSBuzz example script

This commit is contained in:
dave 2018-02-26 13:26:33 -05:00
parent 80799aeaa6
commit 48a2dc53b3
5 changed files with 145 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
buzz_scripts/empty.bzz
View File

@ -1,17 +0,0 @@
include "update.bzz"
# Executed once at init time.
function init() {
}
# Executed at each time step.
function step() {
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

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

@ -4,7 +4,6 @@
#
########################################
include "utils/vec2.bzz"
include "plan/rrtstar.bzz"
include "act/barrier.bzz"
include "utils/conversions.bzz"
@ -33,7 +32,7 @@ function launch() {
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
barrier_ready()
} else {
log("Altitude: ", pose.position.altitude)
@ -41,7 +40,7 @@ function launch() {
uav_takeoff(TARGET_ALTITUDE)
}
} else {
barrier_set(ROBOTS, "NAVIGATE", "LAUNCH")
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
barrier_ready()
}
}
@ -73,78 +72,20 @@ function take_picture() {
pic_time=pic_time+1
}
#
# Work in progress....
function navigate() {
BVMSTATE="NAVIGATE"
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
}
print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
function goto_gps(transf) {
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
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.")
return
}
# create the map
if(map==nil) {
dyn_init_map(cur_goal)
if(V_TYPE == 0) {
homegps.lat = pose.position.latitude
homegps.long = pose.position.longitude
}
}
if(Path==nil) {
# add proximity sensor and neighbor obstacles to the map
populateGrid(m_pos)
# start the planner
path_it = 1
pathPlanner(cur_goal, m_pos)
} else if(path_it <= size(Path)) {
var cur_path_pt = convert_pt(getvec(Path, path_it))
print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
populateGrid(m_pos)
if(check_path(Path, path_it, m_pos, 0)) {
# stop
goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
Path = nil
if(V_TYPE == 0)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
# re-start the planner
path_it = 1
pathPlanner(cur_goal, m_pos)
} else {
cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude)
}
} else
path_it = path_it + 1
} else {
Path = nil
BVMSTATE="IDLE"
}
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)
} 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)
}
# function goto_direct(transf) {
# m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude)
# 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)
# } 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)
# }
function follow() {
if(size(targets)>0) {
BVMSTATE = "FOLLOW"
@ -208,9 +149,9 @@ function nei_cmd_listen() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(value==22 and BVMSTATE!="LAUNCH" and BVMSTATE!="BARRIERWAIT") {
if(value==22 and BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "LAUNCH"
} else if(value==21 and BVMSTATE!="STOP" and BVMSTATE!="BARRIERWAIT") {
} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "STOP"
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
uav_arm()

58
buzz_scripts/include/plan/rrtstar.bzz
View File

@ -14,6 +14,64 @@ cur_cell = {}
nei_cell = {}
mapgoal = {}
#
# Work in progress....
function navigate() {
BVMSTATE="NAVIGATE"
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
}
print(" has to move ", math.vec2.length(cur_goal), "from ", m_pos.x, " ", m_pos.y)
if(math.vec2.length(cur_goal)>GOTO_MAXDIST*5) {
log("Sorry this is too far.")
return
}
# create the map
if(map==nil) {
dyn_init_map(cur_goal)
if(V_TYPE == 0) {
homegps.lat = pose.position.latitude
homegps.long = pose.position.longitude
}
}
if(Path==nil) {
# add proximity sensor and neighbor obstacles to the map
populateGrid(m_pos)
# start the planner
path_it = 1
pathPlanner(cur_goal, m_pos)
} else if(path_it <= size(Path)) {
var cur_path_pt = convert_pt(getvec(Path, path_it))
print(" heading to [", path_it, "/", size(Path), "]", cur_path_pt.x, cur_path_pt.y)
if(math.vec2.length(cur_path_pt) > GOTODIST_TOL) {
populateGrid(m_pos)
if(check_path(Path, path_it, m_pos, 0)) {
# stop
goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
Path = nil
if(V_TYPE == 0)
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
# re-start the planner
path_it = 1
pathPlanner(cur_goal, m_pos)
} else {
cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude)
}
} else
path_it = path_it + 1
} else {
Path = nil
BVMSTATE="IDLE"
}
}
# create a map with its length to fit the goal (rel. pos.) and the current position as the center
# TODO: test with khepera/argos
function dyn_init_map(m_navigation) {

17
buzz_scripts/main.bzz
View File

@ -2,8 +2,12 @@ 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
@ -16,11 +20,6 @@ goal_list = {
.0={.x = 45.5088103899, .y = -73.1540826153, .z = 2.5}
}
function action() {
statef=action
set_goto(idle)
}
# Executed once at init time.
function init() {
init_stig()
@ -31,7 +30,7 @@ function init() {
# start the swarm command listener
# nei_cmd_listen()
# Starting state
# Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup.
# BVMSTATE = "TURNEDOFF"
BVMSTATE = "LAUNCHED"
}
@ -51,15 +50,15 @@ function step() {
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCHED") # ends on navigate
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
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar
else if(BVMSTATE=="NAVIGATE") # ends on idle
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
statef=navigate
else if(BVMSTATE == "FOLLOW") #TODO: not tested in new structure
statef=follow

64
buzz_scripts/minimal.bzz Normal file
View File

@ -0,0 +1,64 @@
include "update.bzz"
include "act/states.bzz"
include "vstigenv.bzz"
# State launched after takeoff
AUTO_LAUNCH_STATE = "ACTION"
#####
# 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_swarm()
TARGET_ALTITUDE = 25.0 # m
# start the swarm command listener
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
}
function action() {
BVMSTATE = "ACTION"
# do some actions....
}
# Executed at each time step.
function step() {
# listen to Remote Controller
rc_cmd_listen()
#
# State machine
#
if(BVMSTATE=="TURNEDOFF")
statef=turnedoff
else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch
else if(BVMSTATE=="IDLE")
statef=idle
else if(BVMSTATE=="ACTION")
statef=action
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() {
}