added minimal ROSBuzz example script
This commit is contained in:
parent
6cd162bff4
commit
da86b4be81
|
@ -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() {
|
||||
}
|
|
@ -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()
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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() {
|
||||
}
|
Loading…
Reference in New Issue