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

minor fixes for 24/11 demo

This commit is contained in:
dave 2017-11-22 21:59:57 -05:00
parent 98c0dbc62f
commit 89b263e4df
9 changed files with 114 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
buzz_scripts/graphformGPS.bzz
View File

@ -18,6 +18,7 @@ ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2 ROOT_ID = 2
old_state = -1
# max velocity in cm/step # max velocity in cm/step
ROBOT_MAXVEL = 150.0 ROBOT_MAXVEL = 150.0
@ -46,21 +47,16 @@ m_receivedMessage={.State=s2i("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Respo
#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}) #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("STATE_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
#Current robot state
# m_eState="STATE_FREE" # replace with default UAVSTATE
#navigation vector #navigation vector
m_navigation={.x=0,.y=0} m_navigation={.x=0,.y=0}
#Debug message to be displayed in qt-opengl
#m_ossDebugMsg
#Debug vector to draw
#CVector2 m_cDebugVector
#Current label being requested or chosen (-1 when none) #Current label being requested or chosen (-1 when none)
m_nLabel=-1 m_nLabel=-1
m_messageID={} m_messageID={}
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
#neighbor distance to lock the current pattern #neighbor distance to lock the current pattern
lock_neighbor_id={} lock_neighbor_id={}
lock_neighbor_dis={} lock_neighbor_dis={}
@ -89,9 +85,6 @@ m_unJoiningLostPeriod=0
#Tolerance distance to a target location #Tolerance distance to a target location
m_fTargetDistanceTolerance=0 m_fTargetDistanceTolerance=0
#step cunt
step_cunt=0
# virtual stigmergy for the LOCK barrier. # virtual stigmergy for the LOCK barrier.
m_lockstig = 1 m_lockstig = 1
@ -119,8 +112,9 @@ function count(table,value){
} }
return number return number
} }
#
#map from int to state #map from int to state
#
function i2s(value){ function i2s(value){
if(value==1){ if(value==1){
return "STATE_FREE" return "STATE_FREE"
@ -138,8 +132,9 @@ function i2s(value){
return "STATE_LOCK" return "STATE_LOCK"
} }
} }
#
#map from state to int #map from state to int
#
function s2i(value){ function s2i(value){
if(value=="STATE_FREE"){ if(value=="STATE_FREE"){
return 1 return 1
@ -157,7 +152,9 @@ function s2i(value){
return 5 return 5
} }
} }
#
#map form int to response #map form int to response
#
function i2r(value){ function i2r(value){
if(value==1){ if(value==1){
return "REQ_NONE" return "REQ_NONE"
@ -166,7 +163,9 @@ function i2r(value){
return "REQ_GRANTED" return "REQ_GRANTED"
} }
} }
#
#map from response to int #map from response to int
#
function r2i(value){ function r2i(value){
if(value=="REQ_NONE"){ if(value=="REQ_NONE"){
return 1 return 1
@ -175,8 +174,6 @@ function r2i(value){
return 2 return 2
} }
} }
# #
#return the index of value #return the index of value
# #
@ -190,7 +187,6 @@ function find(table,value){
} }
return ind return ind
} }
# #
#pack message into 1 number #pack message into 1 number
# #
@ -255,8 +251,9 @@ log(id,"I pass value=",recv_value)
return_table.Bearing=b return_table.Bearing=b
return return_table return return_table
} }
#
#get the target distance to neighbr nei_id #get the target distance to neighbr nei_id
#
function target4label(nei_id){ function target4label(nei_id){
var return_val="miss" var return_val="miss"
var i=0 var i=0
@ -268,7 +265,9 @@ function target4label(nei_id){
} }
return return_val return return_val
} }
#
#calculate LJ vector for neibhor stored in i #calculate LJ vector for neibhor stored in i
#
function LJ_vec(i){ function LJ_vec(i){
var dis=m_MessageRange[i] var dis=m_MessageRange[i]
var ljbearing=m_MessageBearing[i] var ljbearing=m_MessageBearing[i]
@ -282,7 +281,9 @@ function LJ_vec(i){
#log("x=",cDir.x,"y=",cDir.y) #log("x=",cDir.x,"y=",cDir.y)
return cDir return cDir
} }
#
#calculate the motion vector #calculate the motion vector
#
function motion_vector(){ function motion_vector(){
var i=0 var i=0
var m_vector={.x=0.0,.y=0.0} var m_vector={.x=0.0,.y=0.0}
@ -296,7 +297,9 @@ function motion_vector(){
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y) #log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
return m_vector return m_vector
} }
#
# starts the neighbors listener
#
function start_listen(){ function start_listen(){
neighbors.listen("m", neighbors.listen("m",
function(vid,value,rid){ function(vid,value,rid){
@ -314,13 +317,15 @@ neighbors.listen("m",
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
m_messageID[m_neighbourCount]=rid m_messageID[m_neighbourCount]=rid
log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
#log(rid, " is in ", m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
m_neighbourCount=m_neighbourCount+1 m_neighbourCount=m_neighbourCount+1
}) })
} }
# #
#Function used to get the station info of the sender of the message #Function used to get the station info of the sender of the message
#
function Get_DisAndAzi(id){ function Get_DisAndAzi(id){
neighbors.foreach( neighbors.foreach(
function(rid, data) { function(rid, data) {
@ -330,7 +335,6 @@ function Get_DisAndAzi(id){
} }
}) })
} }
# #
#Update node info according to neighbour robots #Update node info according to neighbour robots
# #
@ -361,7 +365,6 @@ function UpdateNodeInfo(){
i=i+1 i=i+1
} }
} }
# #
#Transistion to state free #Transistion to state free
# #
@ -370,7 +373,6 @@ function TransitionToFree(){
m_unWaitCount=m_unLabelSearchWaitTime m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(UAVSTATE)
} }
# #
#Transistion to state asking #Transistion to state asking
# #
@ -384,7 +386,6 @@ function TransitionToAsking(un_label){
m_unWaitCount=m_unResponseTimeThreshold m_unWaitCount=m_unResponseTimeThreshold
} }
# #
#Transistion to state joining #Transistion to state joining
# #
@ -394,7 +395,6 @@ function TransitionToJoining(){
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod m_unWaitCount=m_unJoiningLostPeriod
} }
# #
#Transistion to state joined #Transistion to state joined
# #
@ -413,7 +413,6 @@ function TransitionToJoined(){
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
# #
#Transistion to state Lock, lock the current formation #Transistion to state Lock, lock the current formation
# #
@ -434,8 +433,12 @@ function TransitionToLock(){
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x, m_navigation.y, 0.0) uav_moveto(m_navigation.x, m_navigation.y, 0.0)
}
# prepare to restart a new shape
old_state = rc_State
#stop listening
neighbors.ignore("m")
}
# #
# Do free # Do free
# #
@ -482,7 +485,6 @@ function DoFree() {
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(UAVSTATE)
} }
# #
#Do asking #Do asking
# #
@ -544,11 +546,9 @@ function DoAsking(){
m_navigation.y=0.0 m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0) uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
} }
# #
#Do joining #Do joining
# #
m_gotjoinedparent = 0
function DoJoining(){ function DoJoining(){
if(m_gotjoinedparent!=1) if(m_gotjoinedparent!=1)
@ -587,13 +587,9 @@ function set_rc_goto() {
m_gotjoinedparent = 1 m_gotjoinedparent = 1
} }
} }
# #
#Do joined #Do joined
# #
repeat_assign=0
assign_label=-1
assign_id=-1
function DoJoined(){ function DoJoined(){
m_selfMessage.State=s2i(UAVSTATE) m_selfMessage.State=s2i(UAVSTATE)
m_selfMessage.Label=m_nLabel m_selfMessage.Label=m_nLabel
@ -685,7 +681,6 @@ function DoJoined(){
#barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1) #barrier_wait(ROBOTS, TransitionToLock, DoJoined, -1)
barrier_wait(6, TransitionToLock, DoJoined, -1) barrier_wait(6, TransitionToLock, DoJoined, -1)
} }
# #
#Do Lock #Do Lock
# #
@ -705,18 +700,18 @@ function DoLock(){
#move #move
uav_moveto(m_navigation.x, m_navigation.y, 0.0) uav_moveto(m_navigation.x, m_navigation.y, 0.0)
# prepare to restart a new shape
statef=action
} }
#
# Executed after takeoff
#
function action(){ function action(){
statef=action statef=action
UAVSTATE="STATE_FREE" UAVSTATE="STATE_FREE"
# reset the graph # reset the graph
Reset() Reset()
} }
# #
# Executed at init # Executed at init
# #
@ -737,13 +732,12 @@ function init() {
#v_tag = stigmergy.create(m_lockstig) #v_tag = stigmergy.create(m_lockstig)
#uav_initstig() #uav_initstig()
# go to diff. height since no collision avoidance implemented yet # go to diff. height since no collision avoidance implemented yet
#TARGET_ALTITUDE = 3.0 + id * 1.25 TARGET_ALTITUDE = 20.0 + id * 2.5
statef=turnedoff statef=turnedoff
UAVSTATE = "TURNEDOFF" UAVSTATE = "TURNEDOFF"
} }
# #
# Executed every step # Executed every step (main loop)
# #
function step() { function step() {
# listen to potential RC # listen to potential RC
@ -762,15 +756,15 @@ function step() {
# #
if(UAVSTATE=="STATE_FREE") if(UAVSTATE=="STATE_FREE")
statef=DoFree statef=DoFree
else if(UAVSTATE=="STATE_ESCAPE")
statef=DoEscape
else if(UAVSTATE=="STATE_ASKING") else if(UAVSTATE=="STATE_ASKING")
statef=DoAsking statef=DoAsking
else if(UAVSTATE=="STATE_JOINING") else if(UAVSTATE=="STATE_JOINING")
statef=DoJoining statef=DoJoining
else if(UAVSTATE=="STATE_JOINED") else if(UAVSTATE=="STATE_JOINED")
statef=DoJoined statef=DoJoined
else if(UAVSTATE=="STATE_LOCK") else if(UAVSTATE=="STATE_LOCK" and old_state!=rc_State)
statef=action
else if(UAVSTATE=="STATE_LOCK" and old_state==rc_State)
statef=DoLock statef=DoLock
# high level UAV state machine # high level UAV state machine
@ -794,25 +788,40 @@ function step() {
m_MessageBearing={}#store received neighbour message m_MessageBearing={}#store received neighbour message
m_neighbourCount=0 m_neighbourCount=0
#step cunt+1
step_cunt=step_cunt+1
} }
# #
# Executed when reset # Executed when reset
# #
function 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")}
m_navigation={.x=0,.y=0}
m_nLabel=-1
m_messageID={}
lock_neighbor_id={}
lock_neighbor_dis={}
m_unRequestId=0
m_bias=0
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
m_unWaitCount=0
repeat_assign=0
assign_label=-1
assign_id=-1
m_gotjoinedparent = 0
if(rc_State==0){ if(rc_State==0){
log("Loading P graph")
Read_GraphP() Read_GraphP()
} else if(rc_State==1) { } else if(rc_State==1) {
log("Loading O graph")
Read_GraphO() Read_GraphO()
} else if(rc_State==2) { } else if(rc_State==2) {
log("Loading L graph")
Read_GraphL() Read_GraphL()
} else if(rc_State==3) { } else if(rc_State==3) {
log("Loading Y graph")
Read_GraphY() Read_GraphY()
} }
m_nLabel=-1
#start listening #start listening
start_listen() start_listen()
@ -829,7 +838,6 @@ function Reset(){
TransitionToFree() TransitionToFree()
} }
} }
# #
# Executed upon destroy # Executed upon destroy
# #

2
buzz_scripts/include/graphs/shapes_O.bzz
View File

@ -1,6 +1,6 @@
#Table of the nodes in the graph #Table of the nodes in the graph
m_vecNodes={} m_vecNodes={}
m_vecNodes_fixed={}
function Read_GraphO(){ function Read_GraphO(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point .Label = 0, # Label of the point

12
buzz_scripts/include/graphs/shapes_P.bzz
View File

@ -1,6 +1,6 @@
#Table of the nodes in the graph #Table of the nodes in the graph
m_vecNodes={} m_vecNodes={}
m_vecNodes_fixed={}
function Read_GraphP(){ function Read_GraphP(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point .Label = 0, # Label of the point
@ -14,7 +14,7 @@ m_vecNodes[0] = { # The .graph file is stored according the sequence of Lab
m_vecNodes[1] = { m_vecNodes[1] = {
.Label = 1, .Label = 1,
.Pred = 0, .Pred = 0,
.distance = 2000, .distance = 1000,
.bearing = 0.0, .bearing = 0.0,
.height = 5000, .height = 5000,
.State="UNASSIGNED", .State="UNASSIGNED",
@ -23,7 +23,7 @@ m_vecNodes[1] = {
m_vecNodes[2] = { m_vecNodes[2] = {
.Label = 2, .Label = 2,
.Pred = 0, .Pred = 0,
.distance = 2000, .distance = 1000,
.bearing = 1.57, .bearing = 1.57,
.height = 7000, .height = 7000,
.State="UNASSIGNED", .State="UNASSIGNED",
@ -32,7 +32,7 @@ m_vecNodes[2] = {
m_vecNodes[3] = { m_vecNodes[3] = {
.Label = 3, .Label = 3,
.Pred = 0, .Pred = 0,
.distance = 2000, .distance = 1000,
.bearing = 4.71, .bearing = 4.71,
.height = 9000, .height = 9000,
.State="UNASSIGNED", .State="UNASSIGNED",
@ -41,7 +41,7 @@ m_vecNodes[3] = {
m_vecNodes[4] = { m_vecNodes[4] = {
.Label = 4, .Label = 4,
.Pred = 1, .Pred = 1,
.distance = 1414, .distance = 707,
.bearing = 0.79, .bearing = 0.79,
.height = 11000, .height = 11000,
.State="UNASSIGNED", .State="UNASSIGNED",
@ -50,7 +50,7 @@ m_vecNodes[4] = {
m_vecNodes[5] = { m_vecNodes[5] = {
.Label = 5, .Label = 5,
.Pred = 2, .Pred = 2,
.distance = 2000, .distance = 1000,
.bearing = 0.0, .bearing = 0.0,
.height = 14000, .height = 14000,
.State="UNASSIGNED", .State="UNASSIGNED",

2
buzz_scripts/include/graphs/shapes_Y.bzz
View File

@ -1,6 +1,6 @@
#Table of the nodes in the graph #Table of the nodes in the graph
m_vecNodes={} m_vecNodes={}
m_vecNodes_fixed={}
function Read_GraphY(){ function Read_GraphY(){
m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing m_vecNodes[0] = { # The .graph file is stored according the sequence of Label, predecessor, distance, bearing
.Label = 0, # Label of the point .Label = 0, # Label of the point

4
buzz_scripts/include/uavstates.bzz
View File

@ -180,15 +180,19 @@ function uav_rccmd() {
} else if (flight.rc_cmd==900){ } else if (flight.rc_cmd==900){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 0 rc_State = 0
neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){ } else if (flight.rc_cmd==901){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 1 rc_State = 1
neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){ } else if (flight.rc_cmd==902){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 2 rc_State = 2
neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){ } else if (flight.rc_cmd==903){
flight.rc_cmd=0 flight.rc_cmd=0
rc_State = 3 rc_State = 3
neighbors.broadcast("cmd", 903)
} }
} }

2
buzz_scripts/include/vec2.bzz
View File

@ -41,7 +41,7 @@ math.vec2.length = function(v) {
# RETURN: The angle of the vector. # RETURN: The angle of the vector.
# #
math.vec2.angle = function(v) { math.vec2.angle = function(v) {
return math.atan2(v.y, v.x) return math.atan(v.y, v.x)
} }
# #

35
buzz_scripts/startswarmsimER.launch Normal file
View File

@ -0,0 +1,35 @@
include "vec2.bzz"
include "update.bzz"
include "barrier.bzz" # don't use a stigmergy id=80 (auto-increment up) with this header.
include "uavstates.bzz" # require an 'action' function to be defined here.
include "vstigenv.bzz"
function action() {
uav_storegoal(-1.0,-1.0,-1.0)
set_goto(picture)
}
# Executed once at init time.
function init() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
uav_initstig()
TARGET_ALTITUDE = 15.0
}
# Executed at each time step.
function step() {
uav_rccmd()
statef()
log("Current state: ", UAVSTATE)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

4
misc/cmdlinectr.sh
View File

@ -34,3 +34,7 @@ function updaterobot {
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch
} }
function uavstate {
let "a = $1 + 900"
rosservice call robot0/buzzcmd 0 $a 0 0 0 0 0 0 0 0
}

4
src/roscontroller.cpp
View File

@ -1166,7 +1166,9 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
res.success = true; res.success = true;
break; break;
default: default:
res.success = false; buzzuav_closures::rc_call(req.command);
ROS_INFO("----> Received unregistered command: ", req.command);
res.success = true;
break; break;
} }
return true; return true;