From ed59c6de7c0653e736be4f31bf67dedf3a652a1d Mon Sep 17 00:00:00 2001 From: dave Date: Fri, 2 Mar 2018 15:35:06 -0500 Subject: [PATCH] ported AGF to new rosbuzz file structure --- buzz_scripts/include/act/states.bzz | 8 +- .../include/taskallocate/graphformGPS.bzz | 188 +++++++----------- buzz_scripts/main.bzz | 33 ++- buzz_scripts/mainRRT.bzz | 80 ++++++++ 4 files changed, 182 insertions(+), 127 deletions(-) create mode 100644 buzz_scripts/mainRRT.bzz diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index e987b60..9dfc8fc 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -73,17 +73,17 @@ function take_picture() { } function goto_gps(transf) { - m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude) + m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) 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) + goto_abs(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) + goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude) } function follow() { @@ -94,7 +94,7 @@ function follow() { force=(0.05)*(tab.range)^4 attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing)) }) - uav_moveto(attractor.x, attractor.y, 0.0) + goto_abs(attractor.x, attractor.y, 0.0) } else { log("No target in local table!") BVMSTATE = "IDLE" diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index d4e5aae..e35fe37 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -1,18 +1,11 @@ # # Include files # -include "string.bzz" -include "vec2.bzz" -include "update.bzz" -#include "vstigenv.bzz" # reserve stigmergy id=20 and 21 for this header. -include "barrier.bzz" # reserve stigmergy id=80 (auto-increment up) for this header. -include "uavstates.bzz" # require an 'action' function to be defined here. - -include "graphs/shapes_P.bzz" -include "graphs/shapes_O.bzz" -include "graphs/shapes_L.bzz" -include "graphs/shapes_Y.bzz" +include "taskallocate/graphs/shapes_P.bzz" +include "taskallocate/graphs/shapes_O.bzz" +include "taskallocate/graphs/shapes_L.bzz" +include "taskallocate/graphs/shapes_Y.bzz" ROBOT_RADIUS = 50 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS @@ -20,9 +13,6 @@ ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROOT_ID = 2 old_state = -1 -# max velocity in cm/step -ROBOT_MAXVEL = 150.0 - # # Global variables # @@ -40,12 +30,12 @@ m_MessageBearing={}#store received neighbour message m_neighbourCount=0#used to cunt neighbours #Save message from one neighbour #the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing -m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} +m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0} # #Save the message to send #The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND}) -m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} +m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")} #navigation vector m_navigation={.x=0,.y=0} @@ -117,38 +107,38 @@ function count(table,value){ # function i2s(value){ if(value==1){ - return "STATE_FREE" + return "GRAPH_FREE" } else if(value==2){ - return "STATE_ASKING" + return "GRAPH_ASKING" } else if(value==3){ - return "STATE_JOINING" + return "GRAPH_JOINING" } else if(value==4){ - return "STATE_JOINED" + return "GRAPH_JOINED" } else if(value==5){ - return "STATE_LOCK" + return "GRAPH_LOCK" } } # #map from state to int # function s2i(value){ - if(value=="STATE_FREE"){ + if(value=="GRAPH_FREE"){ return 1 } - else if(value=="STATE_ASKING"){ + else if(value=="GRAPH_ASKING"){ return 2 } - else if(value=="STATE_JOINING"){ + else if(value=="GRAPH_JOINING"){ return 3 } - else if(value=="STATE_JOINED"){ + else if(value=="GRAPH_JOINED"){ return 4 } - else if(value=="STATE_LOCK"){ + else if(value=="GRAPH_LOCK"){ return 5 } } @@ -344,11 +334,11 @@ function UpdateNodeInfo(){ var i=0 while(i0) @@ -455,7 +446,7 @@ function DoFree() { var i=0 var j=0 while(i 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() { +}