ported AGF to new rosbuzz file structure

This commit is contained in:
dave 2018-03-02 15:35:06 -05:00
parent 48a2dc53b3
commit fd7414f382
4 changed files with 182 additions and 127 deletions

View File

@ -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"

View File

@ -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(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageState[i]=="GRAPH_JOINED"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
else if(m_MessageState[i]=="GRAPH_JOINING"){
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
}
@ -369,18 +359,18 @@ function UpdateNodeInfo(){
#Transistion to state free
#
function TransitionToFree(){
UAVSTATE="STATE_FREE"
BVMSTATE="GRAPH_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.State=s2i(BVMSTATE)
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
UAVSTATE="STATE_ASKING"
BVMSTATE="GRAPH_ASKING"
m_nLabel=un_label
m_unRequestId=id #don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
@ -390,8 +380,8 @@ function TransitionToAsking(un_label){
#Transistion to state joining
#
function TransitionToJoining(){
UAVSTATE="STATE_JOINING"
m_selfMessage.State=s2i(UAVSTATE)
BVMSTATE="GRAPH_JOINING"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
}
@ -399,26 +389,26 @@ function TransitionToJoining(){
#Transistion to state joined
#
function TransitionToJoined(){
UAVSTATE="STATE_JOINED"
m_selfMessage.State=s2i(UAVSTATE)
BVMSTATE="GRAPH_JOINED"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#write statues
#v_tag.put(m_nLabel, m_lockstig)
barrier_create()
barrier_create()
barrier_ready()
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
UAVSTATE="STATE_LOCK"
m_selfMessage.State=s2i(UAVSTATE)
BVMSTATE="GRAPH_LOCK"
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
#record neighbor distance
@ -432,7 +422,7 @@ function TransitionToLock(){
}
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
goto_abs(m_navigation.x, m_navigation.y, 0.0)
# prepare to restart a new shape
old_state = rc_State
@ -443,7 +433,8 @@ function TransitionToLock(){
# Do free
#
function DoFree() {
m_selfMessage.State=s2i(UAVSTATE)
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
#wait for a while before looking for a Label
if(m_unWaitCount>0)
@ -455,7 +446,7 @@ function DoFree() {
var i=0
var j=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageState[i]=="GRAPH_JOINED"){
setJoinedLabels[j]=m_MessageLabel[i]
setJoinedIndexes[j]=i
j=j+1
@ -482,13 +473,15 @@ function DoFree() {
}
#set message
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.State=s2i(BVMSTATE)
BroadcastGraph()
}
#
#Do asking
#
function DoAsking(){
UpdateGraph()
#look for response from predecessor
var i=0
var psResponse=-1
@ -496,13 +489,13 @@ function DoAsking(){
#the respond robot in joined state
#the request Label be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageState[i]=="GRAPH_JOINED"){
#log("received label = ",m_MessageReqLabel[i])
if(m_MessageReqLabel[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
if(m_MessageState[i]=="STATE_JOINING" and m_MessageLabel[i]==m_nLabel){
if(m_MessageState[i]=="GRAPH_JOINING" and m_MessageLabel[i]==m_nLabel){
TransitionToFree()
return
}
@ -513,7 +506,7 @@ function DoAsking(){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.ReqLabel=m_nLabel
m_selfMessage.ReqID=m_unRequestId
#if(m_unWaitCount==0){
@ -543,20 +536,23 @@ function DoAsking(){
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
BroadcastGraph()
}
#
#Do joining
#
function DoJoining(){
UpdateGraph()
if(m_gotjoinedparent!=1)
set_rc_goto()
else
gotoWP(TransitionToJoined)
goto_gps(TransitionToJoined)
#pack the communication package
m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
BroadcastGraph()
}
function set_rc_goto() {
@ -564,7 +560,7 @@ function set_rc_goto() {
var i=0
var IDofPred=-1
while(i<m_neighbourCount and IDofPred==-1){
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="STATE_JOINED")
if(m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred and m_MessageState[i]=="GRAPH_JOINED")
IDofPred=i
i=i+1
}
@ -582,7 +578,7 @@ function set_rc_goto() {
goal = gps_from_vec(math.vec2.newp(math.vec2.length(S2Target)/100.0, math.atan(S2Target.y,S2Target.x)))
print("Saving GPS goal: ",goal.latitude, goal.longitude)
uav_storegoal(goal.latitude, goal.longitude, position.altitude)
uav_storegoal(goal.latitude, goal.longitude, pose.position.altitude)
m_gotjoinedparent = 1
}
}
@ -590,7 +586,8 @@ function set_rc_goto() {
#Do joined
#
function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE)
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
#collect all requests
@ -601,7 +598,7 @@ function DoJoined(){
var JoiningLabel
var seenPred=0
while(i<m_neighbourCount){
if(m_MessageState[i]=="STATE_ASKING"){
if(m_MessageState[i]=="GRAPH_ASKING"){
ReqLabel=m_MessageReqLabel[i]
#log("ReqLabel var:",ReqLabel)
#log("M_vec var",m_vecNodes[ReqLabel].State)
@ -612,7 +609,7 @@ function DoJoined(){
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
if(m_MessageState[i]=="GRAPH_JOINING"){
JoiningLabel=m_MessageLabel[i]
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
##joining wrt this dot,send the global bearing
@ -620,13 +617,13 @@ function DoJoined(){
}
}
if(m_MessageState[i]=="STATE_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
if(m_MessageState[i]=="GRAPH_JOINING" and repeat_assign==1 and m_MessageLabel[i]==assign_label){
repeat_assign=0
}
#if it is the pred
if((m_MessageState[i]=="STATE_JOINED" or m_MessageState[i]=="STATE_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
if((m_MessageState[i]=="GRAPH_JOINED" or m_MessageState[i]=="GRAPH_LOCK") and m_MessageLabel[i]==m_vecNodes[m_nLabel].Pred){
seenPred=1
m_unWaitCount=m_unJoiningLostPeriod
}
@ -676,13 +673,15 @@ function DoJoined(){
#if(v_tag.size()==ROBOTS){
# TransitionToLock()
#}
barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
BroadcastGraph()
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=s2i(UAVSTATE)
UpdateGraph()
m_selfMessage.State=s2i(BVMSTATE)
m_selfMessage.Label=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
@ -695,24 +694,22 @@ function DoLock(){
m_navigation=motion_vector()
}
#move
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
goto_abs(m_navigation.x, m_navigation.y, 0.0)
BroadcastGraph()
}
#
# Executed after takeoff
#
function action(){
statef=action
UAVSTATE="STATE_FREE"
function startGraph(){
BVMSTATE="GRAPH_FREE"
# reset the graph
Reset()
statef=resetGraph
}
#
# Executed at init
#
function init() {
function initGraph() {
#
# Global parameters for graph formation
#
@ -721,55 +718,18 @@ function init() {
m_fTargetDistanceTolerance=100
m_fTargetAngleTolerance=0.1
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
#v_tag = stigmergy.create(m_lockstig)
#uav_initstig()
# go to diff. height since no collision avoidance implemented yet
#TARGET_ALTITUDE = 20.0 + id * 2.5
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
#
# Executed every step (main loop)
#
function step() {
# listen to potential RC
uav_rccmd()
# get the swarm commands
uav_neicmd()
# update the vstig (status/net/batt)
#uav_updatestig()
function UpdateGraph() {
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#
# graph state machine
#
if(UAVSTATE=="STATE_FREE")
statef=DoFree
else if(UAVSTATE=="STATE_ASKING")
statef=DoAsking
else if(UAVSTATE=="STATE_JOINING")
statef=DoJoining
else if(UAVSTATE=="STATE_JOINED")
statef=DoJoined
else if(UAVSTATE=="STATE_LOCK" and old_state!=rc_State)
statef=action
else if(UAVSTATE=="STATE_LOCK" and old_state==rc_State)
statef=DoLock
# high level UAV state machine
statef()
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
log("Swarm size: ", ROBOTS)
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
}
function BroadcastGraph() {
#navigation
#broadcast message
neighbors.broadcast("m",packmessage(m_selfMessage))
@ -789,9 +749,9 @@ function step() {
#
# Executed when reset
#
function Reset(){
m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_selfMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
function resetGraph(){
m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
m_navigation={.x=0,.y=0}
m_nLabel=-1
m_messageID={}
@ -838,11 +798,11 @@ function Reset(){
#
# Executed upon destroy
#
function destroy() {
function destroyGraph() {
#clear neighbour message
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("m")

View File

@ -3,10 +3,11 @@ include "update.bzz"
# it requires an 'action' function to be defined here.
include "act/states.bzz"
include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz"
#State launched after takeoff
AUTO_LAUNCH_STATE = "IDLE"
AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
#####
# Vehicule type:
@ -28,11 +29,10 @@ function init() {
TARGET_ALTITUDE = 25.0 # m
# start the swarm command listener
# nei_cmd_listen()
nei_cmd_listen()
# Starting state: TURNEDOFF to wait for user input, LAUNCHED to auto-takeoff at startup.
# BVMSTATE = "TURNEDOFF"
BVMSTATE = "LAUNCHED"
# Starting state: TURNEDOFF to wait for user input.
BVMSTATE = "TURNEDOFF"
}
# Executed at each time step.
@ -44,18 +44,32 @@ function step() {
# uav_updatestig()
#
# graph state machine
# 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
else if(BVMSTATE=="LAUNCH") # 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=="TASK_ALLOCATE") { # or bidding ?
startGraph()
} else if(BVMSTATE=="GRAPH_FREE") {
statef=DoFree
} else if(BVMSTATE=="GRAPH_ASKING") {
statef=DoAsking
} else if(BVMSTATE=="GRAPH_JOINING") {
statef=DoJoining
} else if(BVMSTATE=="GRAPH_JOINED") {
statef=DoJoined
} else if(BVMSTATE=="GRAPH_TRANSTOLOCK")
statef=TransitionToLock
else if(BVMSTATE=="GRAPH_LOCK" and old_state!=rc_State) #switch to a new graph
startGraph()
else if(BVMSTATE=="GRAPH_LOCK" and old_state==rc_State) # move all together (TODO: not tested)
statef=DoLock
else if(BVMSTATE=="PATHPLAN") # ends on navigate, defined in rrtstar
statef=rrtstar
else if(BVMSTATE=="NAVIGATE") # ends on idle, defined in rrtstar
@ -65,6 +79,7 @@ function step() {
else if(BVMSTATE == "PICTURE") #TODO: not tested in new structure
statef=take_picture
statef()
log("Current state: ", BVMSTATE)

80
buzz_scripts/mainRRT.bzz Normal file
View File

@ -0,0 +1,80 @@
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() {
}