From da86b4be817452a324a2cc0484774ca0267ab4fa Mon Sep 17 00:00:00 2001 From: dave Date: Mon, 26 Feb 2018 13:26:33 -0500 Subject: [PATCH] added minimal ROSBuzz example script --- buzz_scripts/empty.bzz | 17 ----- buzz_scripts/include/act/states.bzz | 89 +++++---------------------- buzz_scripts/include/plan/rrtstar.bzz | 58 +++++++++++++++++ buzz_scripts/main.bzz | 17 +++-- buzz_scripts/minimal.bzz | 64 +++++++++++++++++++ 5 files changed, 145 insertions(+), 100 deletions(-) delete mode 100644 buzz_scripts/empty.bzz create mode 100644 buzz_scripts/minimal.bzz diff --git a/buzz_scripts/empty.bzz b/buzz_scripts/empty.bzz deleted file mode 100644 index 3bb5b12..0000000 --- a/buzz_scripts/empty.bzz +++ /dev/null @@ -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() { -} diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index d8e3790..e987b60 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -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() diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 6d40626..bfdd065 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -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) { diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index 6bf2856..2563c3e 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -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 diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz new file mode 100644 index 0000000..8f0494b --- /dev/null +++ b/buzz_scripts/minimal.bzz @@ -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() { +}