merged with master
This commit is contained in:
commit
269f22a23b
|
@ -5,6 +5,11 @@ if(UNIX)
|
|||
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=gnu++11")
|
||||
endif()
|
||||
|
||||
if(SIM)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1")
|
||||
else()
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0")
|
||||
endif()
|
||||
## Find catkin macros and libraries
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
|
@ -54,7 +59,6 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
|
|||
src/roscontroller.cpp
|
||||
src/buzz_utility.cpp
|
||||
src/buzzuav_closures.cpp
|
||||
src/uav_utility.cpp
|
||||
src/buzz_update.cpp)
|
||||
target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} buzz buzzdbg pthread)
|
||||
add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
|
||||
|
|
|
@ -1,93 +0,0 @@
|
|||
include "vec2.bzz"
|
||||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 12.0
|
||||
EPSILON = 18.0
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
|
||||
}
|
||||
|
||||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
function action() {
|
||||
statef=action
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
|
||||
if(math.vec2.length(accum) > 1.0) {
|
||||
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
|
||||
}
|
||||
|
||||
# Move according to vector
|
||||
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
|
||||
uav_moveto(accum.x, accum.y, 0.0)
|
||||
UAVSTATE = "LENNARDJONES"
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# UAVSTATE ="GOEAST"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0, 5.0, 0.0)
|
||||
# } else {
|
||||
# UAVSTATE ="GONORTH"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(5.0, 0.0, 0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
########################################
|
||||
#
|
||||
# MAIN FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
uav_initswarm()
|
||||
uav_initstig()
|
||||
# TARGET_ALTITUDE = 2.5 + id * 5
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
uav_rccmd()
|
||||
uav_neicmd()
|
||||
uav_updatestig()
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
if(id==0)
|
||||
stattab_print()
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
|
@ -1,896 +0,0 @@
|
|||
#
|
||||
# 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=11 for this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
|
||||
include "graphs/shapes_Y.bzz"
|
||||
|
||||
ROBOT_RADIUS = 50
|
||||
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
|
||||
ROOT_ID = 2
|
||||
|
||||
# max velocity in cm/step
|
||||
ROBOT_MAXVEL = 150.0
|
||||
|
||||
#
|
||||
# Global variables
|
||||
#
|
||||
|
||||
#
|
||||
# Save message from all neighours
|
||||
#the indexes are as 1,2,3..., while each value is a table that store the information of a neighbour robot
|
||||
m_MessageState={}#store received neighbour message
|
||||
m_MessageLabel={}#store received neighbour message
|
||||
m_MessageReqLabel={}#store received neighbour message
|
||||
m_MessageReqID={}#store received neighbour message
|
||||
m_MessageResponse={}#store received neighbour message
|
||||
m_MessageRange={}#store received neighbour message
|
||||
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}
|
||||
|
||||
#
|
||||
#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")}
|
||||
|
||||
#Current robot state
|
||||
# m_eState="STATE_FREE" # replace with default UAVSTATE
|
||||
|
||||
#navigation vector
|
||||
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)
|
||||
m_nLabel=-1
|
||||
m_messageID={}
|
||||
#neighbor distance to lock the current pattern
|
||||
lock_neighbor_id={}
|
||||
lock_neighbor_dis={}
|
||||
|
||||
#Label request id
|
||||
m_unRequestId=0
|
||||
|
||||
#Global bias, used to map local coordinate to global coordinate
|
||||
m_bias=0
|
||||
|
||||
#Vector to predecessor,range is the distance between robots, bearing is the angle of pred wrt self in local coordinate of self, globalbearing is the angle of self wrt pred in global coordinate
|
||||
m_cMeToPred={.Range=0.0,.Bearing=0.0,.GlobalBearing=0.0}
|
||||
|
||||
#Counter to wait for something to happen
|
||||
m_unWaitCount=0
|
||||
|
||||
#Number of steps to wait before looking for a free label
|
||||
m_unLabelSearchWaitTime=0
|
||||
|
||||
#Number of steps to wait for an answer to be received
|
||||
m_unResponseTimeThreshold=0
|
||||
|
||||
#Number of steps to wait until giving up joining
|
||||
m_unJoiningLostPeriod=0
|
||||
|
||||
#Tolerance distance to a target location
|
||||
m_fTargetDistanceTolerance=0
|
||||
|
||||
#step cunt
|
||||
step_cunt=0
|
||||
|
||||
# virtual stigmergy for the LOCK barrier.
|
||||
m_lockstig = 1
|
||||
|
||||
# Lennard-Jones parameters, may need change
|
||||
EPSILON = 1800 #13.5 the LJ parameter for other robots
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
|
||||
function FlockInteraction(dist,target,epsilon){
|
||||
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
return mag
|
||||
}
|
||||
|
||||
#
|
||||
#return the number of value in table
|
||||
#
|
||||
function count(table,value){
|
||||
var number=0
|
||||
var i=0
|
||||
while(i<size(table)){
|
||||
if(table[i]==value){
|
||||
number=number+1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
return number
|
||||
}
|
||||
|
||||
#map from int to state
|
||||
function i2s(value){
|
||||
if(value==1){
|
||||
return "STATE_FREE"
|
||||
}
|
||||
else if(value==2){
|
||||
return "STATE_ASKING"
|
||||
}
|
||||
else if(value==3){
|
||||
return "STATE_JOINING"
|
||||
}
|
||||
else if(value==4){
|
||||
return "STATE_JOINED"
|
||||
}
|
||||
else if(value==5){
|
||||
return "STATE_LOCK"
|
||||
}
|
||||
}
|
||||
|
||||
#map from state to int
|
||||
function s2i(value){
|
||||
if(value=="STATE_FREE"){
|
||||
return 1
|
||||
}
|
||||
else if(value=="STATE_ASKING"){
|
||||
return 2
|
||||
}
|
||||
else if(value=="STATE_JOINING"){
|
||||
return 3
|
||||
}
|
||||
else if(value=="STATE_JOINED"){
|
||||
return 4
|
||||
}
|
||||
else if(value=="STATE_LOCK"){
|
||||
return 5
|
||||
}
|
||||
}
|
||||
#map form int to response
|
||||
function i2r(value){
|
||||
if(value==1){
|
||||
return "REQ_NONE"
|
||||
}
|
||||
else if(value==2){
|
||||
return "REQ_GRANTED"
|
||||
}
|
||||
}
|
||||
#map from response to int
|
||||
function r2i(value){
|
||||
if(value=="REQ_NONE"){
|
||||
return 1
|
||||
}
|
||||
else if(value=="REQ_GRANTED"){
|
||||
return 2
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#
|
||||
#return the index of value
|
||||
#
|
||||
function find(table,value){
|
||||
var index=nil
|
||||
var i=0
|
||||
while(i<size(table)){
|
||||
if(table[i]==value)
|
||||
index=i
|
||||
i=i+1
|
||||
}
|
||||
return index
|
||||
}
|
||||
|
||||
#
|
||||
#pack message into 1 number
|
||||
#
|
||||
function packmessage(send_table){
|
||||
var send_value
|
||||
send_value=100000*send_table.State+10000*send_table.Label+1000*send_table.ReqLabel+10*send_table.ReqID+send_table.Response
|
||||
return send_value
|
||||
}
|
||||
#
|
||||
#pack guide message into 1 number
|
||||
#
|
||||
function pack_guide_msg(send_table){
|
||||
var send_value
|
||||
var r_id=send_table.Label#id of target robot
|
||||
var pon#positive or negative ,0 postive, 1 negative
|
||||
if(send_table.Bearing>=0){
|
||||
pon=0
|
||||
}
|
||||
else{
|
||||
pon=1
|
||||
}
|
||||
var b=math.abs(send_table.Bearing)
|
||||
send_value=r_id*1000+pon*100+b
|
||||
return send_value
|
||||
}
|
||||
#
|
||||
#unpack message
|
||||
#
|
||||
function unpackmessage(recv_value){
|
||||
var wan=(recv_value-recv_value%100000)/100000
|
||||
recv_value=recv_value-wan*100000
|
||||
var qian=(recv_value-recv_value%10000)/10000
|
||||
recv_value=recv_value-qian*10000
|
||||
var bai=(recv_value-recv_value%1000)/1000
|
||||
recv_value=recv_value-bai*1000
|
||||
var shi=(recv_value-recv_value%10)/10
|
||||
recv_value=recv_value-shi*10
|
||||
var ge=recv_value
|
||||
var return_table={.State=0.0,.Label=0.0,.ReqLabel=0.0,.ReqID=0.0,.Response=0.0}
|
||||
return_table.State=wan
|
||||
return_table.Label=qian
|
||||
return_table.ReqLabel=bai
|
||||
return_table.ReqID=shi
|
||||
return_table.Response=ge
|
||||
return return_table
|
||||
}
|
||||
#
|
||||
#unpack guide message
|
||||
#
|
||||
function unpack_guide_msg(recv_value){
|
||||
log(id,"I pass value=",recv_value)
|
||||
var qian=(recv_value-recv_value%1000)/1000
|
||||
recv_value=recv_value-qian*1000
|
||||
var bai=(recv_value-recv_value%100)/100
|
||||
recv_value=recv_value-bai*100
|
||||
var b=recv_value
|
||||
var return_table={.Label=0.0,.Bearing=0.0}
|
||||
return_table.Label=qian
|
||||
if(bai==1){
|
||||
b=b*-1.0
|
||||
}
|
||||
return_table.Bearing=b
|
||||
return return_table
|
||||
}
|
||||
|
||||
#get the target distance to neighbr nei_id
|
||||
function target4label(nei_id){
|
||||
var return_val="miss"
|
||||
var i=0
|
||||
while(i<size(lock_neighbor_id)){
|
||||
if(lock_neighbor_id[i]==nei_id){
|
||||
return_val=lock_neighbor_dis[i]
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
return return_val
|
||||
}
|
||||
#calculate LJ vector for neibhor stored in i
|
||||
function LJ_vec(i){
|
||||
var dis=m_MessageRange[i]
|
||||
var bearing=m_MessageBearing[i]
|
||||
var nei_id=m_messageID[i]
|
||||
var target=target4label(nei_id)
|
||||
var cDir={.x=0.0,.y=0.0}
|
||||
if(target!="miss"){
|
||||
cDir=math.vec2.newp(FlockInteraction(dis,target,EPSILON),bearing)
|
||||
}
|
||||
#log(id,"dis=",dis,"target=",target,"label=",nei_id)
|
||||
#log("x=",cDir.x,"y=",cDir.y)
|
||||
return cDir
|
||||
}
|
||||
#calculate the motion vector
|
||||
function motion_vector(){
|
||||
var i=0
|
||||
var m_vector={.x=0.0,.y=0.0}
|
||||
while(i<m_neighbourCount){
|
||||
#calculate and add the motion vector
|
||||
m_vector=math.vec2.add(m_vector,LJ_vec(i))
|
||||
#log(id,"x=",m_vector.x,"y=",m_vector.y)
|
||||
i=i+1
|
||||
}
|
||||
m_vector=math.vec2.scale(m_vector,1.0/m_neighbourCount)
|
||||
#log(id,"fnal=","x=",m_vector.x,"y=",m_vector.y)
|
||||
return m_vector
|
||||
}
|
||||
|
||||
function start_listen(){
|
||||
neighbors.listen("m",
|
||||
function(vid,value,rid){
|
||||
#store the received message
|
||||
var temp_id=rid
|
||||
var recv_val=unpackmessage(value)
|
||||
Get_DisAndAzi(temp_id)
|
||||
#add the received message
|
||||
#
|
||||
m_MessageState[m_neighbourCount]=i2s(recv_val.State)
|
||||
m_MessageLabel[m_neighbourCount]=recv_val.Label
|
||||
m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
|
||||
m_MessageReqID[m_neighbourCount]=recv_val.ReqID
|
||||
m_MessageResponse[m_neighbourCount]=i2r(recv_val.Response)
|
||||
m_MessageRange[m_neighbourCount]=m_receivedMessage.Range
|
||||
m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
|
||||
m_messageID[m_neighbourCount]=rid
|
||||
m_neighbourCount=m_neighbourCount+1
|
||||
})
|
||||
}
|
||||
#
|
||||
#Function used to get the station info of the sender of the message
|
||||
function Get_DisAndAzi(id){
|
||||
neighbors.foreach(
|
||||
function(rid, data) {
|
||||
if(rid==id){
|
||||
m_receivedMessage.Range=data.distance*100.0
|
||||
m_receivedMessage.Bearing=data.azimuth
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
#
|
||||
#Update node info according to neighbour robots
|
||||
#
|
||||
function UpdateNodeInfo(){
|
||||
#Collect informaiton
|
||||
#Update information
|
||||
var i=0
|
||||
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
m_vecNodes[m_MessageLabel[i]].State="ASSIGNED"
|
||||
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
else if(m_MessageState[i]=="STATE_JOINING"){
|
||||
m_vecNodes[m_MessageLabel[i]].State="ASSIGNING"
|
||||
m_vecNodes[m_MessageLabel[i]].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
#Forget old information
|
||||
i=0
|
||||
while(i<size(m_vecNodes)){
|
||||
if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
|
||||
m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
|
||||
if(m_vecNodes[i].StateAge==0)
|
||||
m_vecNodes[i].State="UNASSIGNED"
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state free
|
||||
#
|
||||
function TransitionToFree(){
|
||||
UAVSTATE="STATE_FREE"
|
||||
m_unWaitCount=m_unLabelSearchWaitTime
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state asking
|
||||
#
|
||||
function TransitionToAsking(un_label){
|
||||
UAVSTATE="STATE_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.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
|
||||
m_unWaitCount=m_unResponseTimeThreshold
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state joining
|
||||
#
|
||||
function TransitionToJoining(){
|
||||
UAVSTATE="STATE_JOINING"
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
|
||||
neighbors.listen("r",
|
||||
function(vid,value,rid){
|
||||
var recv_table={.Label=0,.Bearing=0.0}
|
||||
recv_table=unpack_guide_msg(value)
|
||||
#store the received message
|
||||
if(recv_table.Label==m_nLabel){
|
||||
m_cMeToPred.GlobalBearing=recv_table.Bearing
|
||||
}
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
#
|
||||
#Transistion to state joined
|
||||
#
|
||||
function TransitionToJoined(){
|
||||
UAVSTATE="STATE_JOINED"
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
neighbors.ignore("r")
|
||||
|
||||
#write statues
|
||||
v_tag.put(m_nLabel, m_lockstig)
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=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
|
||||
#
|
||||
function TransitionToLock(){
|
||||
UAVSTATE="STATE_LOCK"
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_vecNodes[m_nLabel].State="ASSIGNED"
|
||||
#record neighbor distance
|
||||
lock_neighbor_id={}
|
||||
lock_neighbor_dis={}
|
||||
var i=0
|
||||
while(i<m_neighbourCount){
|
||||
lock_neighbor_id[i]=m_messageID[i]
|
||||
lock_neighbor_dis[i]=m_MessageRange[i]
|
||||
i=i+1
|
||||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
}
|
||||
|
||||
#
|
||||
# Do free
|
||||
#
|
||||
function DoFree() {
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
|
||||
#wait for a while before looking for a Label
|
||||
if(m_unWaitCount>0)
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
|
||||
#find a set of joined robots
|
||||
var setJoinedLabels={}
|
||||
var setJoinedIndexes={}
|
||||
var i=0
|
||||
var j=0
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
setJoinedLabels[j]=m_MessageLabel[i]
|
||||
setJoinedIndexes[j]=i
|
||||
j=j+1
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#go through the graph to look for a proper Label
|
||||
var unFoundLabel=0
|
||||
# var IDofPred=0
|
||||
i=1
|
||||
while(i<size(m_vecNodes) and (unFoundLabel==0)){
|
||||
#if the node is unassigned and the predecessor is insight
|
||||
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
|
||||
unFoundLabel=m_vecNodes[i].Label
|
||||
# IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
if(unFoundLabel>0){
|
||||
TransitionToAsking(unFoundLabel)
|
||||
return
|
||||
}
|
||||
|
||||
#navigation
|
||||
#if there is a joined robot within sight, move around joined robots
|
||||
#else, gather with other free robots
|
||||
if(size(setJoinedIndexes)>0){
|
||||
var tempvec_P={.x=0.0,.y=0.0}
|
||||
var tempvec_N={.x=0.0,.y=0.0}
|
||||
i=0
|
||||
while(i<size(setJoinedIndexes)){
|
||||
var index=setJoinedIndexes[i]
|
||||
tempvec_P=math.vec2.add(tempvec_P,math.vec2.newp(m_MessageRange[index],m_MessageBearing[index]+0.5*math.pi))
|
||||
tempvec_N=math.vec2.add(tempvec_N,math.vec2.newp(m_MessageRange[index]-5.0*ROBOT_SAFETYDIST,m_MessageBearing[index]))
|
||||
i=i+1
|
||||
}
|
||||
tempvec_P=math.vec2.scale(tempvec_P,size(setJoinedIndexes))
|
||||
tempvec_N=math.vec2.scale(tempvec_N,size(setJoinedIndexes))
|
||||
m_navigation=math.vec2.add(tempvec_P,tempvec_N)
|
||||
# Limit the mvt
|
||||
if(math.vec2.length(m_navigation)>ROBOT_MAXVEL*2)
|
||||
m_navigation = math.vec2.scale(m_navigation, ROBOT_MAXVEL*2/math.vec2.length(m_navigation))
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}else{ #no joined robots in sight
|
||||
i=0
|
||||
var tempvec={.x=0.0,.y=0.0}
|
||||
|
||||
while(i<m_neighbourCount){
|
||||
tempvec=math.vec2.add(tempvec,math.vec2.newp(m_MessageRange[i]-2.0*ROBOT_SAFETYDIST,m_MessageBearing[i]))
|
||||
i=i+1
|
||||
}
|
||||
m_navigation=math.vec2.scale(tempvec,1.0/i)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
|
||||
|
||||
|
||||
#jump the first step
|
||||
if(step_cunt<=1){
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
#set message
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#
|
||||
#Do asking
|
||||
#
|
||||
function DoAsking(){
|
||||
#look for response from predecessor
|
||||
var i=0
|
||||
var psResponse=-1
|
||||
while(i<m_neighbourCount and psResponse==-1){
|
||||
#the respond robot in joined state
|
||||
#the request Label be the same as requesed
|
||||
#get a respond
|
||||
if(m_MessageState[i]=="STATE_JOINED"){
|
||||
log("received label = ",m_MessageReqLabel[i])
|
||||
if(m_MessageReqLabel[i]==m_nLabel)
|
||||
if(m_MessageResponse[i]!="REQ_NONE"){
|
||||
log("get response")
|
||||
psResponse=i
|
||||
}}
|
||||
i=i+1
|
||||
}
|
||||
#analyse response
|
||||
if(psResponse==-1){
|
||||
#no response, wait
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.ReqLabel=m_nLabel
|
||||
m_selfMessage.ReqID=m_unRequestId
|
||||
if(m_unWaitCount==0){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
}
|
||||
else{
|
||||
log("respond id=",m_MessageReqID[psResponse])
|
||||
if(m_MessageReqID[psResponse]!=m_unRequestId){
|
||||
m_vecNodes[m_nLabel].State="ASSIGNING"
|
||||
m_vecNodes[m_nLabel].StateAge=m_unJoiningLostPeriod
|
||||
TransitionToFree()
|
||||
}
|
||||
if(m_MessageReqID[psResponse]==m_unRequestId){
|
||||
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
|
||||
TransitionToJoining()
|
||||
return
|
||||
}
|
||||
else{
|
||||
TransitionToAsking(m_nLabel)
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
|
||||
#
|
||||
#Do joining
|
||||
#
|
||||
function DoJoining(){
|
||||
#get information of pred
|
||||
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")
|
||||
IDofPred=i
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#found pred
|
||||
if(IDofPred!=-1){
|
||||
m_unWaitCount=m_unJoiningLostPeriod#if see pred, reset the timer
|
||||
|
||||
var P2Target=math.vec2.newp(m_vecNodes[m_nLabel].distance,m_vecNodes[m_nLabel].bearing)
|
||||
m_cMeToPred.Range=m_MessageRange[IDofPred]#the poition of self to pred in local coordinate
|
||||
m_cMeToPred.Bearing=m_MessageBearing[IDofPred]
|
||||
|
||||
#attention, m_cMeToPred.GlobalBearing is the bearing of self wrt pred in global coordinate
|
||||
var S2PGlobalBearing=0
|
||||
|
||||
m_cMeToPred.GlobalBearing=LimitAngle(m_cMeToPred.GlobalBearing)
|
||||
|
||||
if(m_cMeToPred.GlobalBearing>math.pi)
|
||||
S2PGlobalBearing=m_cMeToPred.GlobalBearing-math.pi
|
||||
else
|
||||
S2PGlobalBearing=m_cMeToPred.GlobalBearing+math.pi
|
||||
|
||||
var S2Pred=math.vec2.newp(m_cMeToPred.Range,S2PGlobalBearing)
|
||||
|
||||
#the vector from self to target in global coordinate
|
||||
var S2Target=math.vec2.add(S2Pred,P2Target)
|
||||
#change the vector to local coordinate of self
|
||||
var S2Target_bearing=math.atan(S2Target.y, S2Target.x)
|
||||
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
||||
S2Target_bearing=S2Target_bearing+m_bias
|
||||
|
||||
# Limit the mvt
|
||||
if(math.vec2.length(S2Target)>ROBOT_MAXVEL)
|
||||
S2Target = math.vec2.scale(S2Target, ROBOT_MAXVEL/math.vec2.length(S2Target))
|
||||
|
||||
m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
|
||||
|
||||
|
||||
#test if is already in desired position
|
||||
if(math.vec2.length(S2Target)<m_fTargetDistanceTolerance and S2Target_bearing<m_fTargetAngleTolerance){
|
||||
log("JOINED! ", math.vec2.length(S2Target), ", ", S2Target_bearing)
|
||||
TransitionToJoined()
|
||||
return
|
||||
}
|
||||
} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
}
|
||||
|
||||
|
||||
if(m_unWaitCount==0){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
|
||||
#pack the communication package
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
||||
}
|
||||
|
||||
#
|
||||
#Do joined
|
||||
#
|
||||
function DoJoined(){
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
|
||||
#collect all requests
|
||||
var mapRequests={}
|
||||
var i=0
|
||||
var j=0
|
||||
var ReqLabel
|
||||
var JoiningLabel
|
||||
var seenPred=0
|
||||
while(i<m_neighbourCount){
|
||||
if(m_MessageState[i]=="STATE_ASKING"){
|
||||
ReqLabel=m_MessageReqLabel[i]
|
||||
log("ReqLabel var:",ReqLabel)
|
||||
log("M_vec var",m_vecNodes[ReqLabel].State)
|
||||
if(m_vecNodes[ReqLabel].State=="UNASSIGNED")
|
||||
if(m_nLabel==m_vecNodes[ReqLabel].Pred){
|
||||
#is a request, store the index
|
||||
mapRequests[j]=i
|
||||
log("Into if m_nLabel")
|
||||
j=j+1
|
||||
}
|
||||
}
|
||||
if(m_MessageState[i]=="STATE_JOINING"){
|
||||
JoiningLabel=m_MessageLabel[i]
|
||||
if(m_nLabel==m_vecNodes[JoiningLabel].Pred){
|
||||
##joining wrt this dot,send the global bearing
|
||||
var m_messageForJoining={.Label=JoiningLabel,.Bearing=m_MessageBearing[i]-m_bias}
|
||||
neighbors.broadcast("r",pack_guide_msg(m_messageForJoining))
|
||||
}
|
||||
}
|
||||
#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){
|
||||
seenPred=1
|
||||
m_unWaitCount=m_unJoiningLostPeriod
|
||||
}
|
||||
i=i+1
|
||||
}
|
||||
|
||||
#get request
|
||||
if(size(mapRequests)!=0){
|
||||
i=1
|
||||
var ReqIndex=0
|
||||
while(i<size(mapRequests)){
|
||||
#compare the distance
|
||||
if(m_MessageRange[mapRequests[ReqIndex]]>m_MessageRange[mapRequests[i]])
|
||||
ReqIndex=i
|
||||
i=i+1
|
||||
}
|
||||
#get the best index, whose ReqLabel and Reqid are
|
||||
ReqLabel=m_MessageReqLabel[mapRequests[ReqIndex]]
|
||||
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
|
||||
m_selfMessage.ReqLabel=ReqLabel
|
||||
m_selfMessage.ReqID=ReqID
|
||||
m_selfMessage.Response=r2i("REQ_GRANTED")
|
||||
m_vecNodes[ReqLabel].State="ASSIGNING"
|
||||
log("Label=",ReqLabel)
|
||||
log("ID=",ReqID)
|
||||
m_vecNodes[ReqLabel].StateAge=m_unJoiningLostPeriod
|
||||
}
|
||||
|
||||
#lost pred, wait for some time and transit to free
|
||||
if(seenPred==0){
|
||||
m_unWaitCount=m_unWaitCount-1
|
||||
if(m_unWaitCount==0){
|
||||
TransitionToFree()
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
|
||||
|
||||
#check if should to transists to lock
|
||||
#write statues
|
||||
v_tag.get(m_nLabel)
|
||||
log(v_tag.size(), " of ", ROBOTS, " ready to lock")
|
||||
if(v_tag.size()==ROBOTS){
|
||||
TransitionToLock()
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
#Do Lock
|
||||
#
|
||||
function DoLock(){
|
||||
m_selfMessage.State=s2i(UAVSTATE)
|
||||
m_selfMessage.Label=m_nLabel
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
#calculate motion vection
|
||||
if(m_nLabel==0){
|
||||
m_navigation.x=0.0 #change value so that robot 0 will move
|
||||
m_navigation.y=0.0
|
||||
}
|
||||
if(m_nLabel!=0){
|
||||
m_navigation=motion_vector()
|
||||
}
|
||||
#move
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
}
|
||||
|
||||
function action(){
|
||||
statef=action
|
||||
UAVSTATE="STATE_FREE"
|
||||
|
||||
# reset the graph
|
||||
Reset()
|
||||
}
|
||||
|
||||
#
|
||||
# Executed at init
|
||||
#
|
||||
function init() {
|
||||
#
|
||||
# Global parameters for graph formation
|
||||
#
|
||||
m_unResponseTimeThreshold=10
|
||||
m_unLabelSearchWaitTime=10
|
||||
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 = 7.5 #2.5 + id * 1.5
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
#
|
||||
# Executed every step
|
||||
#
|
||||
function step() {
|
||||
# listen to potential RC
|
||||
uav_rccmd()
|
||||
# get the swarm commands
|
||||
uav_neicmd()
|
||||
# update the vstig (status/net/batt)
|
||||
uav_updatestig()
|
||||
|
||||
#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_ESCAPE")
|
||||
statef=DoEscape
|
||||
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")
|
||||
statef=DoLock
|
||||
|
||||
# high level UAV state machine
|
||||
statef()
|
||||
|
||||
log("Current state: ", UAVSTATE, " and label: ", m_nLabel)
|
||||
log("Swarm size: ", ROBOTS)
|
||||
|
||||
#navigation
|
||||
#broadcast message
|
||||
neighbors.broadcast("m",packmessage(m_selfMessage))
|
||||
|
||||
#
|
||||
#clean message storage
|
||||
m_MessageState={}#store received neighbour message
|
||||
m_MessageLabel={}#store received neighbour message
|
||||
m_MessageReqLabel={}#store received neighbour message
|
||||
m_MessageReqID={}#store received neighbour message
|
||||
m_MessageResponse={}#store received neighbour message
|
||||
m_MessageRange={}#store received neighbour message
|
||||
m_MessageBearing={}#store received neighbour message
|
||||
m_neighbourCount=0
|
||||
|
||||
|
||||
#step cunt+1
|
||||
step_cunt=step_cunt+1
|
||||
}
|
||||
|
||||
#
|
||||
# Executed when reset
|
||||
#
|
||||
function Reset(){
|
||||
Read_Graph()
|
||||
m_nLabel=-1
|
||||
|
||||
#start listening
|
||||
start_listen()
|
||||
#
|
||||
#set initial state, only one robot choose [A], while the rest choose [B]
|
||||
#
|
||||
#[A]The robot used to triger the formation process is defined as joined,
|
||||
if(id==ROOT_ID){
|
||||
m_nLabel=0
|
||||
TransitionToJoined()
|
||||
}
|
||||
#[B]Other robots are defined as free.
|
||||
else{
|
||||
TransitionToFree()
|
||||
}
|
||||
}
|
||||
|
||||
#
|
||||
# Executed upon destroy
|
||||
#
|
||||
function destroy() {
|
||||
#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)
|
||||
m_vecNodes={}
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
}
|
|
@ -1,35 +0,0 @@
|
|||
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() {
|
||||
}
|
|
@ -1,269 +0,0 @@
|
|||
include "vec2.bzz"
|
||||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 12.0
|
||||
EPSILON = 10.0
|
||||
|
||||
#################################################
|
||||
### UTILITY FUNCTIONS ###########################
|
||||
#################################################
|
||||
|
||||
# Write a table as if it was a matrix
|
||||
function write_knowledge(k, row, col, val) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
k[key] = val
|
||||
log("Writing knowledge:", val, " to ", row, " ", col)
|
||||
}
|
||||
|
||||
# Read a table as if it was a matrix
|
||||
function read_knowledge(k, row, col) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
if (k[key] == nil) {
|
||||
log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
|
||||
return -1
|
||||
} else {
|
||||
return k[key]
|
||||
}
|
||||
}
|
||||
|
||||
# Int to String
|
||||
function itos(i) {
|
||||
|
||||
log("Use 'string.tostring(OJB)' instead")
|
||||
|
||||
if (i==0) { return "0" }
|
||||
if (i==1) { return "1" }
|
||||
if (i==2) { return "2" }
|
||||
if (i==3) { return "3" }
|
||||
if (i==4) { return "4" }
|
||||
if (i==5) { return "5" }
|
||||
if (i==6) { return "6" }
|
||||
if (i==7) { return "7" }
|
||||
if (i==8) { return "8" }
|
||||
if (i==9) { return "9" }
|
||||
|
||||
log("Function 'itos' out of bounds, returning the answer (42)")
|
||||
return "42"
|
||||
}
|
||||
|
||||
# String to Int
|
||||
function stoi(s) {
|
||||
if (s=='0') { return 0 }
|
||||
if (s=='1') { return 1 }
|
||||
if (s=='2') { return 2 }
|
||||
if (s=='3') { return 3 }
|
||||
if (s=='4') { return 4 }
|
||||
if (s=='5') { return 5 }
|
||||
if (s=='6') { return 6 }
|
||||
if (s=='7') { return 7 }
|
||||
if (s=='8') { return 8 }
|
||||
if (s=='9') { return 9 }
|
||||
|
||||
log("Function 'stoi' out of bounds, returning the answer (42)")
|
||||
return 42
|
||||
|
||||
}
|
||||
|
||||
# Rads to degrees
|
||||
function rtod(r) {
|
||||
return (r*(180.0/math.pi))
|
||||
}
|
||||
|
||||
# Degrees to rads
|
||||
function dtor(d) {
|
||||
return (math.pi*(d/180.0))
|
||||
}
|
||||
|
||||
# Force angles in the (-180,180) interval
|
||||
function degrees_interval(a) {
|
||||
var temp = a
|
||||
while ((temp>360.0) or (temp<0.0)) {
|
||||
if (temp > 360.0) {
|
||||
temp = temp - 360.0
|
||||
} else if (temp < 0.0){
|
||||
temp = temp + 360.0
|
||||
}
|
||||
}
|
||||
if (temp > 180.0) {
|
||||
temp = temp - 360.0
|
||||
}
|
||||
return temp
|
||||
}
|
||||
|
||||
# Force angles in the (-pi,pi) interval
|
||||
function radians_interval(a) {
|
||||
var temp = a
|
||||
while ((temp>2.0*math.pi) or (temp<0.0)) {
|
||||
if (temp > 2.0*math.pi) {
|
||||
temp = temp - 2.0*math.pi
|
||||
} else if (temp < 0.0){
|
||||
temp = temp + 2.0*math.pi
|
||||
}
|
||||
}
|
||||
if (temp > math.pi) {
|
||||
temp = temp - 2.0*math.pi
|
||||
}
|
||||
return temp
|
||||
}
|
||||
|
||||
#################################################
|
||||
### MOVEMENT/COMMUNICATION PRIMITIVES ###########
|
||||
#################################################
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
}
|
||||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
function hexagon() {
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
# Move according to vector
|
||||
moveto(accum.x, accum.y)
|
||||
}
|
||||
|
||||
function inform_your_neighborhood() {
|
||||
# Reset to 0 the visibility of all neighbors
|
||||
foreach(knowledge, function(key, value) {
|
||||
column = string.sub(key, string.length(key)-1,string.length(key))
|
||||
if (column=='3') {
|
||||
knowledge[key] = 0
|
||||
}
|
||||
})
|
||||
neighbors.foreach( function(rid, data) {
|
||||
# For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
|
||||
message_id = string.tostring(rid)
|
||||
neighbors.broadcast(message_id, rtod(data.azimuth))
|
||||
# Record the neighbor azimuth in my own knowledge table
|
||||
write_knowledge(knowledge, rid, 0, rtod(data.azimuth))
|
||||
# Record the neighbor distance in my own knowledge table
|
||||
write_knowledge(knowledge, rid, 2, data.distance)
|
||||
# Set neighbor as visible
|
||||
write_knowledge(knowledge, rid, 3, 1)
|
||||
})
|
||||
# Send a message with the desired direction, as seen by the broadcasting robot
|
||||
neighbors.broadcast("direction", local_dir)
|
||||
|
||||
}
|
||||
|
||||
function listen_to_your_neighborhood() {
|
||||
# For all "senders" in my neighborhood, record my azimuth, as seen by them
|
||||
message_id = string.tostring(id)
|
||||
neighbors.listen(message_id, function(vid, value, rid) {
|
||||
write_knowledge(knowledge, rid, 1, value)
|
||||
})
|
||||
}
|
||||
|
||||
#################################################
|
||||
### ACTUAL CONTROLLERS ##########################
|
||||
#################################################
|
||||
|
||||
function zero() {
|
||||
# Do not move
|
||||
moveto(0.0,0.0)
|
||||
# Tell the neighbors of the center where to go
|
||||
inform_your_neighborhood()
|
||||
}
|
||||
|
||||
function onetwo() {
|
||||
if (id == 1) {
|
||||
angle = 45
|
||||
} else {
|
||||
angle = -135
|
||||
}
|
||||
# Broadcast information
|
||||
inform_your_neighborhood()
|
||||
# If the center 0 is in sight
|
||||
if (read_knowledge(knowledge, 0, 3) == 1) {
|
||||
arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
|
||||
if (arm_offset<3 and arm_offset>(-3)) {
|
||||
hexagon() # Underlying Lennard-Jones potential behavior
|
||||
} else {
|
||||
|
||||
local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
|
||||
local_arm = degrees_interval(angle - local_rotation)
|
||||
|
||||
if (read_knowledge(knowledge, 0, 2) > 250.0) {
|
||||
x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
} else if (read_knowledge(knowledge, 0, 2) < 30.0) {
|
||||
x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
} else {
|
||||
spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
|
||||
if (arm_offset > 0) { # Clockwise
|
||||
|
||||
x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
|
||||
} else { # Counterclockwise
|
||||
x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
|
||||
y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
|
||||
}
|
||||
}
|
||||
speed = 100
|
||||
moveto(speed * x_mov,speed * y_mov)
|
||||
}
|
||||
} else {
|
||||
hexagon()
|
||||
}
|
||||
}
|
||||
|
||||
#################################################
|
||||
### BUZZ FUNCTIONS ##############################
|
||||
#################################################
|
||||
|
||||
function action(){
|
||||
if (id == 0)
|
||||
statef=zero
|
||||
else
|
||||
statef=onetwo
|
||||
|
||||
UAVSTATE="TENTACLES"
|
||||
}
|
||||
|
||||
# Executed at init time
|
||||
function init() {
|
||||
uav_initswarm()
|
||||
|
||||
# Local knowledge table
|
||||
knowledge = {}
|
||||
# Update local knowledge with information from the neighbors
|
||||
listen_to_your_neighborhood()
|
||||
# Variables initialization
|
||||
iteration = 0
|
||||
|
||||
}
|
||||
|
||||
# Executed every time step
|
||||
function step() {
|
||||
uav_rccmd()
|
||||
uav_neicmd()
|
||||
|
||||
statef()
|
||||
log("Current state: ", CURSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Count the number of steps
|
||||
iteration = iteration + 1
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
# Execute at exit
|
||||
function destroy() {
|
||||
}
|
|
@ -1,119 +0,0 @@
|
|||
include "vec2.bzz"
|
||||
include "update.bzz"
|
||||
include "barrier.bzz" # don't use a stigmergy id=11 with this header.
|
||||
include "uavstates.bzz" # require an 'action' function to be defined here.
|
||||
include "vstigenv.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 10.0
|
||||
|
||||
# Lennard-Jones parameters
|
||||
TARGET = 12.0
|
||||
EPSILON = 14.0
|
||||
|
||||
# Lennard-Jones interaction magnitude
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
#return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
|
||||
}
|
||||
|
||||
# Neighbor data to LJ interaction vector
|
||||
function lj_vector(rid, data) {
|
||||
return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
|
||||
}
|
||||
|
||||
# Accumulator of neighbor LJ interactions
|
||||
function lj_sum(rid, data, accum) {
|
||||
return math.vec2.add(data, accum)
|
||||
}
|
||||
|
||||
function user_attract(t) {
|
||||
fus = math.vec2.new(0.0, 0.0)
|
||||
if(size(t)>0) {
|
||||
foreach(t, function(u, tab) {
|
||||
#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
|
||||
fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, 3 * TARGET / 4.0, EPSILON * 2.0), tab.b))
|
||||
})
|
||||
math.vec2.scale(fus, 1.0 / size(t))
|
||||
}
|
||||
#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
|
||||
return fus
|
||||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
function action() {
|
||||
statef=action
|
||||
# Calculate accumulator
|
||||
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
accum = math.vec2.scale(accum, 1.0 / neighbors.count())
|
||||
|
||||
accum = math.vec2.add(accum, user_attract(users.dataL))
|
||||
accum = math.vec2.scale(accum, 1.0 / 2.0)
|
||||
|
||||
if(math.vec2.length(accum) > 1.0) {
|
||||
accum = math.vec2.scale(accum, 1.0 / math.vec2.length(accum))
|
||||
}
|
||||
|
||||
# Move according to vector
|
||||
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
|
||||
uav_moveto(accum.x, accum.y)
|
||||
UAVSTATE = "LENNARDJONES"
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
# timeW =0
|
||||
# statef=land
|
||||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# UAVSTATE ="GOEAST"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,5.0)
|
||||
# } else {
|
||||
# UAVSTATE ="GONORTH"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(5.0,0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
|
||||
########################################
|
||||
#
|
||||
# MAIN FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
uav_initswarm()
|
||||
|
||||
vt = stigmergy.create(5)
|
||||
t = {}
|
||||
vt.put("p",t)
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
uav_rccmd()
|
||||
uav_neicmd()
|
||||
|
||||
statef()
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
|
||||
# Read a value from the structure
|
||||
# log(users)
|
||||
#users_print(users.dataG)
|
||||
if(size(users.dataG)>0)
|
||||
vt.put("p", users.dataG)
|
||||
|
||||
# Get the number of keys in the structure
|
||||
#log("The vstig has ", vt.size(), " elements")
|
||||
users_save(vt.get("p"))
|
||||
table_print(users.dataL)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
|
@ -1,537 +0,0 @@
|
|||
# Include files
|
||||
include "string.bzz"
|
||||
include "vec2.bzz"
|
||||
include "utilities.bzz"
|
||||
include "barrier.bzz"
|
||||
|
||||
############################################
|
||||
|
||||
# Global variables
|
||||
|
||||
RANGE = 200 # rab range in cm, get from argos file
|
||||
|
||||
N_SONS = 10 # Maximum number of sons
|
||||
|
||||
TRIGGER_VSTIG = 101 # ID of trigger virtual stigmergy
|
||||
BARRIER_VSTIG = 102 # ID of barrier virtual stigmergy
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
# Tree utility functions
|
||||
|
||||
function parent_select() {
|
||||
|
||||
|
||||
# Selects potential parents
|
||||
var candidates = {}
|
||||
candidates = neighbors.filter(function(rid, data) {
|
||||
return ((knowledge.level[rid] > 0) and (data.distance < (RANGE - 10.0)) and (knowledge.free[rid] == 1))})
|
||||
# and (data.distance > 10.0)
|
||||
|
||||
# Selects closest parent candidate as parent
|
||||
var temp = {}
|
||||
temp = candidates.reduce(function(rid, data, accum) {
|
||||
accum.min = math.min(accum.min, data.distance)
|
||||
return accum
|
||||
}, {.min = RANGE})
|
||||
var min = temp.min
|
||||
|
||||
|
||||
var flag = 0
|
||||
foreach(knowledge.distance, function(key, value) {
|
||||
if ((flag == 0) and (candidates.data[key] != nil)) {
|
||||
if (value == min) {
|
||||
tree.parent.id = key
|
||||
tree.parent.distance = value
|
||||
tree.parent.azimuth = knowledge.azimuth[key]
|
||||
flag = 1
|
||||
}
|
||||
}
|
||||
#break (when implemented)
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function count() {
|
||||
|
||||
if (nb_sons == 0) {
|
||||
eq_level = level
|
||||
}
|
||||
|
||||
else if (nb_sons >= 1) {
|
||||
var temp = {}
|
||||
temp = sons.reduce(function(rid, data, accum) {
|
||||
accum.sum = accum.sum + tree.sons[rid].eq_level
|
||||
return accum
|
||||
}, {.sum = 0})
|
||||
eq_level = temp.sum - (nb_sons - 1) * level
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function change_frame(p01, p1, theta) {
|
||||
|
||||
var p0 = {
|
||||
.x = math.cos(theta) * p1.x + math.sin(theta) * p1.y + p01.x,
|
||||
.y = -math.sin(theta) * p1.x + math.cos(theta) * p1.y + p01.y
|
||||
}
|
||||
return p0
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
transform_accum = function(rid, data) {
|
||||
|
||||
# Son contribution in frame F1
|
||||
var p1 = {
|
||||
.x = tree.sons[rid].accum_x,
|
||||
.y = tree.sons[rid].accum_y
|
||||
}
|
||||
|
||||
# Rotation angle between F0 and F1
|
||||
var theta = tree.sons[rid].angle_parent - data.azimuth - math.pi
|
||||
|
||||
# Translation vector from F0 to F1
|
||||
var p01 = {
|
||||
.x = 0,
|
||||
.y = 0
|
||||
}
|
||||
|
||||
var p0 = {}
|
||||
|
||||
if (tree.sons[rid].angle_parent != nil) {
|
||||
var rot_angle = radians_interval(theta)
|
||||
p0 = change_frame(p01, p1, rot_angle)
|
||||
}
|
||||
else {
|
||||
p0.x = p1.x
|
||||
p0.y = p1.y
|
||||
}
|
||||
|
||||
return p0
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function centroid() {
|
||||
|
||||
# If the robot has a parent
|
||||
if ((tree.parent != nil) or (root == 1)) {
|
||||
var sum_F1 = { .x = 0, .y = 0}
|
||||
|
||||
# If the robot has at least one son
|
||||
if (nb_sons > 0) {
|
||||
var temp = {}
|
||||
# Expresses son contrib (in F1) in its own reference frame (F0)
|
||||
temp = sons.map(transform_accum)
|
||||
# Sums son contributions expressed in F0 frame
|
||||
sum_F1 = temp.reduce(function(rid, data, accum) {
|
||||
accum.x = accum.x + data.x
|
||||
accum.y = accum.y + data.y
|
||||
#log("id ", rid, " sum_x ", accum.x, " sum_y ", accum.y)
|
||||
return accum
|
||||
}, {.x = 0, .y = 0})
|
||||
}
|
||||
|
||||
# If the robot is not the root
|
||||
if ((root == 0) and (tree.parent.id != nil)) {
|
||||
#var nb_descendants = eq_level - level
|
||||
var p0 = knowledge.distance[tree.parent.id]#tree.parent.distance
|
||||
var theta = knowledge.azimuth[tree.parent.id]#tree.parent.azimuth
|
||||
# Adds current robot contribution to centroid sum
|
||||
accum_x = sum_F1.x - (nb_descendants + 1) * p0 * math.cos(theta)
|
||||
accum_y = sum_F1.y - (nb_descendants + 1) * p0 * math.sin(theta)
|
||||
}
|
||||
# If the robot is the root
|
||||
else if ((root == 1) and (robot_count != 0)) {
|
||||
# Centroid coordinates in root ref frame
|
||||
accum_x = sum_F1.x / robot_count
|
||||
accum_y = sum_F1.y / robot_count
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
# Tree reconfiguration functions
|
||||
|
||||
function tree_config() {
|
||||
statef()
|
||||
}
|
||||
|
||||
function end_fun() {
|
||||
if (root == 1) {
|
||||
log("centroid X: ", accum_x, " Y ", accum_y)
|
||||
}
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function root_select() {
|
||||
|
||||
log(id," root_select")
|
||||
if (tree.parent.id != nil) {
|
||||
if(neighbors.data[tree.parent.id] != nil) {
|
||||
angle_parent = neighbors.data[tree.parent.id].azimuth
|
||||
}
|
||||
}
|
||||
|
||||
if (root == 1) {
|
||||
|
||||
# Listens for new root acknowledgment
|
||||
|
||||
foreach(knowledge.ackn, function(key, value) {
|
||||
if (value == better_root) {
|
||||
#log(id, " got it")
|
||||
root = 0
|
||||
level = 0
|
||||
setleds(0,0,0)
|
||||
}
|
||||
})
|
||||
|
||||
if (ack == 1) {
|
||||
# Triggers transition to new state
|
||||
trigger.put("a", 1)
|
||||
}
|
||||
else if ((root == 1) and (better_root != id) and (trigger.get("a") != 1)) {
|
||||
setleds(255,0,0)
|
||||
better_root = id
|
||||
|
||||
# Centroid position in root reference frame (F0)
|
||||
var c0 = math.vec2.new(accum_x, accum_y)
|
||||
|
||||
# Distance from current root to centroid
|
||||
var dist_rc = math.vec2.length(c0)
|
||||
#log("root ", id, " dist_centr ", dist_rc)
|
||||
|
||||
# Distances from neighbors to centroid
|
||||
var dist_centroid = {}
|
||||
dist_centroid = neighbors.map(function(rid, data) {
|
||||
# Neighbour position in frame F0
|
||||
var p0 = math.vec2.newp(data.distance, data.azimuth)
|
||||
# Difference vector between p0 and c0
|
||||
var v = math.vec2.sub(p0, c0)
|
||||
# Distance between robot and centroid
|
||||
return math.vec2.length(v)
|
||||
})
|
||||
|
||||
# Minimal distance to centroid
|
||||
var temp = {}
|
||||
temp = dist_centroid.reduce(function(rid, data, accum) {
|
||||
accum.min = math.min(accum.min, data)
|
||||
return accum
|
||||
}, {.min = dist_rc})
|
||||
var min = temp.min
|
||||
#log("min ", min)
|
||||
|
||||
# If the current root is the closest to the centroid
|
||||
if(dist_rc == min) {
|
||||
ack = 1
|
||||
}
|
||||
# Otherwise
|
||||
else {
|
||||
var flag = 0
|
||||
# Selects closest robot to centroid as better root
|
||||
foreach(dist_centroid.data, function(key, value) {
|
||||
if(flag == 0) {
|
||||
if(value == min) {
|
||||
better_root = key
|
||||
flag = 1
|
||||
}
|
||||
# break (when implemented)
|
||||
}
|
||||
})
|
||||
|
||||
|
||||
#log(id, " better root : ", better_root)
|
||||
#log("X : ", accum_x, "Y : ", accum_y)
|
||||
var angle = neighbors.data[better_root].azimuth
|
||||
# Broadcasts
|
||||
var message = {
|
||||
.better_root = better_root,
|
||||
.centroid_x = accum_x,
|
||||
.centroid_y = accum_y,
|
||||
.angle_old_root = angle
|
||||
}
|
||||
neighbors.broadcast("msg1", message)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
else if (better_root == nil) {
|
||||
# Listen for old root message
|
||||
foreach(knowledge.better_root, function(rid, value) {
|
||||
if(value == id) {
|
||||
|
||||
var theta = neighbors.data[rid].azimuth
|
||||
|
||||
var p1 = {
|
||||
.x = knowledge.centroid_x[rid],
|
||||
.y = knowledge.centroid_y[rid]
|
||||
}
|
||||
|
||||
var p01 = {
|
||||
.x = neighbors.data[rid].distance * math.cos(theta),
|
||||
.y = neighbors.data[rid].distance * math.sin(theta)
|
||||
}
|
||||
|
||||
var p0 = {}
|
||||
|
||||
if (knowledge.angle_old_root[rid] != nil) {
|
||||
var rot_angle = radians_interval(knowledge.angle_old_root[rid] - theta - math.pi)
|
||||
p0 = change_frame(p01, p1, rot_angle)
|
||||
}
|
||||
else {
|
||||
p0.x = p1.x
|
||||
p0.y = p1.y
|
||||
}
|
||||
|
||||
accum_x = p0.x
|
||||
accum_y = p0.y
|
||||
|
||||
centroid_x = accum_x
|
||||
centroid_y = accum_y
|
||||
|
||||
root = 1
|
||||
neighbors.broadcast("got_it", id)
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
# Transitions to new state when all robots are ready
|
||||
if (trigger.get("a") == 1) {
|
||||
barrier_set(ROBOTS, end_fun)
|
||||
barrier_ready()
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
####################################
|
||||
|
||||
function tree_select() {
|
||||
|
||||
log(id, " tree_select")
|
||||
|
||||
neighbors.map(function(rid, data) {
|
||||
knowledge.distance[rid] = data.distance
|
||||
knowledge.azimuth[rid] = data.azimuth
|
||||
return 1
|
||||
})
|
||||
|
||||
if (level == 0) {
|
||||
# Finds a parent
|
||||
parent_select()
|
||||
# Updates robot level
|
||||
if (tree.parent.id != nil) {
|
||||
#log(" ")
|
||||
#log("selected")
|
||||
#log("son ", id)
|
||||
#log("parent ", tree.parent.id)
|
||||
#log(" ")
|
||||
|
||||
level = knowledge.level[tree.parent.id] + 1
|
||||
angle_parent = neighbors.data[tree.parent.id].azimuth
|
||||
}
|
||||
}
|
||||
else {
|
||||
# Updates list of sons
|
||||
foreach(knowledge.parent, function(key, value) {
|
||||
if(value == id) {
|
||||
#log(value)
|
||||
if(tree.sons[key] == nil) {
|
||||
# Updates robot nb_sons
|
||||
nb_sons = nb_sons + 1
|
||||
# Updates robot free status
|
||||
if (nb_sons >= N_SONS) {
|
||||
free = 0
|
||||
}
|
||||
}
|
||||
tree.sons[key] = {
|
||||
.distance = knowledge.distance[key],
|
||||
.azimuth = knowledge.azimuth[key],
|
||||
.eq_level = knowledge.eq_level[key],
|
||||
.accum_x = knowledge.accum_x[key],
|
||||
.accum_y = knowledge.accum_y[key],
|
||||
.angle_parent = knowledge.angle_parent[key]
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
# Creates a subset of neighbors to get the sons
|
||||
# and parent
|
||||
sons = {}
|
||||
sons = neighbors.filter(function(rid, data) {
|
||||
return (tree.sons[rid] != nil)
|
||||
})
|
||||
parent = {}
|
||||
parent = neighbors.filter(function(rid, data) {
|
||||
return (tree.parent.id == rid)
|
||||
})
|
||||
|
||||
# Counts robots (updates eq_level)
|
||||
count()
|
||||
|
||||
# Updates count of robots in the tree
|
||||
if (root == 1) {
|
||||
robot_count = eq_level
|
||||
}
|
||||
|
||||
nb_descendants = eq_level - level
|
||||
|
||||
|
||||
# Computes centroid (updates accum_x, accum_y)
|
||||
centroid()
|
||||
|
||||
# Broadcast information to other robots
|
||||
var message = {
|
||||
.level = level,
|
||||
.parent = tree.parent.id,
|
||||
.eq_level = eq_level,
|
||||
.free = free,
|
||||
.accum_x = accum_x,
|
||||
.accum_y = accum_y,
|
||||
.angle_parent = angle_parent
|
||||
}
|
||||
neighbors.broadcast("msg2", message)
|
||||
|
||||
# Triggers transition to new state if root count = ROBOTS
|
||||
if (root == 1) {
|
||||
if(robot_count == ROBOTS) {
|
||||
log("centroid X: ", accum_x, " Y ", accum_y)
|
||||
trigger.put("b", 1)
|
||||
}
|
||||
}
|
||||
|
||||
# Transitions to new state when all robots are ready
|
||||
if (trigger.get("b") == 1) {
|
||||
barrier_set(ROBOTS, root_select)
|
||||
barrier_ready()
|
||||
}
|
||||
}
|
||||
|
||||
################################################################
|
||||
################################################################
|
||||
|
||||
|
||||
# This function is executed every time you press the 'execute' button
|
||||
function init() {
|
||||
|
||||
trigger = stigmergy.create(TRIGGER_VSTIG)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
|
||||
# Trees
|
||||
old_tree = {}
|
||||
tree = old_tree
|
||||
old_tree.parent = {}
|
||||
old_tree.sons = {}
|
||||
|
||||
# Boolean variables
|
||||
root = 0 # Root status
|
||||
free = 1 # Node status (true if accepts sons)
|
||||
ack = 0
|
||||
|
||||
# Tree variables
|
||||
level = 0
|
||||
eq_level = 0
|
||||
nb_sons = 0
|
||||
nb_descendants = 0
|
||||
|
||||
# Update root status
|
||||
if (id == 0) {
|
||||
root = 1 # True if robot is the ro ot
|
||||
level = 1
|
||||
robot_count = 0
|
||||
}
|
||||
|
||||
statef = tree_select
|
||||
|
||||
knowledge = {
|
||||
.level = {},
|
||||
.parent = {},
|
||||
.eq_level = {},
|
||||
.free = {},
|
||||
.accum_x = {},
|
||||
.accum_y = {},
|
||||
.angle_parent = {},
|
||||
.distance = {},
|
||||
.azimuth = {},
|
||||
.better_root = {},
|
||||
.centroid_x = {},
|
||||
.centroid_y = {},
|
||||
.angle_old_root = {},
|
||||
.ackn = {}
|
||||
}
|
||||
|
||||
# Broadcast information to other robots
|
||||
var message = {
|
||||
.level = level,
|
||||
.parent = old_tree.parent.id,
|
||||
.eq_level = eq_level,
|
||||
.free = free,
|
||||
.accum_x = accum_x,
|
||||
.accum_y = accum_y,
|
||||
.angle_parent = 0.0
|
||||
}
|
||||
neighbors.broadcast("msg2", message)
|
||||
|
||||
# Listen to information from other robots for root_select
|
||||
neighbors.listen("msg1", function(vid, value, rid) {
|
||||
knowledge.better_root[rid] = value.better_root
|
||||
knowledge.centroid_x[rid] = value.centroid_x
|
||||
knowledge.centroid_y[rid] = value.centroid_y
|
||||
knowledge.angle_old_root[rid] = value.angle_old_root
|
||||
knowledge.angle_parent[rid] = value.angle_parent
|
||||
})
|
||||
|
||||
# Listen to information from other robots for tree_select
|
||||
neighbors.listen("msg2", function(vid, value, rid) {
|
||||
knowledge.level[rid] = value.level
|
||||
knowledge.parent[rid] = value.parent
|
||||
knowledge.eq_level[rid] = value.eq_level
|
||||
knowledge.free[rid] = value.free
|
||||
knowledge.accum_x[rid] = value.accum_x
|
||||
knowledge.accum_y[rid] = value.accum_y
|
||||
knowledge.angle_parent[rid] = value.angle_parent
|
||||
})
|
||||
|
||||
neighbors.listen("got_it", function(vid, value, rid) {
|
||||
knowledge.ackn[rid] = value
|
||||
})
|
||||
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
# This function is executed at each time step
|
||||
function step() {
|
||||
|
||||
tree_config()
|
||||
goal = math.vec2.new(0.0, 0.0)
|
||||
|
||||
}
|
||||
############################################
|
||||
|
||||
# This function is executed every time you press the 'reset'
|
||||
# button in the GUI.
|
||||
function reset() {
|
||||
# put your code here
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
# This function is executed only once, when the robot is removed
|
||||
# from the simulation
|
||||
function destroy() {
|
||||
# put your code here
|
||||
}
|
||||
|
||||
################
|
|
@ -1,5 +1,8 @@
|
|||
#ifndef BUZZ_UPDATE_H
|
||||
#define BUZZ_UPDATE_H
|
||||
/*Simulation or robot check*/
|
||||
//#define SIMULATION 1 // set in CMAKELIST
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <buzz/buzztype.h>
|
||||
|
@ -7,80 +10,100 @@
|
|||
#include <buzz/buzzdarray.h>
|
||||
#include <buzz/buzzvstig.h>
|
||||
#include <fstream>
|
||||
#define delete_p(p) do { free(p); p = NULL; } while(0)
|
||||
|
||||
|
||||
#define delete_p(p) \
|
||||
do \
|
||||
{ \
|
||||
free(p); \
|
||||
p = NULL; \
|
||||
} while (0)
|
||||
|
||||
static const uint16_t CODE_REQUEST_PADDING = 250;
|
||||
static const uint16_t MIN_UPDATE_PACKET = 251;
|
||||
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
|
||||
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
|
||||
/*********************/
|
||||
/* Updater states */
|
||||
/********************/
|
||||
|
||||
typedef enum {
|
||||
CODE_RUNNING = 0, // Code executing
|
||||
CODE_STANDBY, // Standing by for others to update
|
||||
} code_states_e;
|
||||
CODE_RUNNING = 0, // Code executing
|
||||
CODE_STANDBY, // Standing by for others to update
|
||||
} code_states_e;
|
||||
|
||||
/*********************/
|
||||
/*Message types */
|
||||
/********************/
|
||||
|
||||
typedef enum {
|
||||
SEND_CODE = 0, // Broadcast code with state
|
||||
STATE_MSG, // Broadcast state
|
||||
} code_message_e;
|
||||
SENT_CODE = 0, // Broadcast code
|
||||
RESEND_CODE, // ReBroadcast request
|
||||
} code_message_e;
|
||||
|
||||
/*************************/
|
||||
/*Updater message queue */
|
||||
/*************************/
|
||||
|
||||
struct updater_msgqueue_s {
|
||||
uint8_t* queue;
|
||||
uint8_t* size;
|
||||
} ;
|
||||
typedef struct updater_msgqueue_s* updater_msgqueue_t;
|
||||
struct updater_msgqueue_s
|
||||
{
|
||||
uint8_t* queue;
|
||||
uint8_t* size;
|
||||
};
|
||||
typedef struct updater_msgqueue_s* updater_msgqueue_t;
|
||||
|
||||
struct updater_code_s
|
||||
{
|
||||
uint8_t* bcode;
|
||||
uint8_t* bcode_size;
|
||||
};
|
||||
typedef struct updater_code_s* updater_code_t;
|
||||
|
||||
/**************************/
|
||||
/*Updater data*/
|
||||
/**************************/
|
||||
|
||||
struct buzz_updater_elem_s {
|
||||
/* robot id */
|
||||
//uint16_t robotid;
|
||||
/*current Bytecode content */
|
||||
uint8_t* bcode;
|
||||
/*current bcode size*/
|
||||
size_t* bcode_size;
|
||||
/*current Bytecode content */
|
||||
uint8_t* standby_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* standby_bcode_size;
|
||||
/*updater out msg queue */
|
||||
updater_msgqueue_t outmsg_queue;
|
||||
/*updater in msg queue*/
|
||||
updater_msgqueue_t inmsg_queue;
|
||||
/*Current state of the updater one in code_states_e ENUM*/
|
||||
int* mode;
|
||||
uint8_t* update_no;
|
||||
} ;
|
||||
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
|
||||
struct buzz_updater_elem_s
|
||||
{
|
||||
/* robot id */
|
||||
// uint16_t robotid;
|
||||
/*current Bytecode content */
|
||||
uint8_t* bcode;
|
||||
/*old Bytecode name */
|
||||
const char* old_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* bcode_size;
|
||||
/*Update patch*/
|
||||
uint8_t* patch;
|
||||
/* Update patch size*/
|
||||
size_t* patch_size;
|
||||
/*current Bytecode content */
|
||||
uint8_t* standby_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* standby_bcode_size;
|
||||
/*updater out msg queue */
|
||||
updater_msgqueue_t outmsg_queue;
|
||||
/*updater in msg queue*/
|
||||
updater_msgqueue_t inmsg_queue;
|
||||
/*Current state of the updater one in code_states_e ENUM*/
|
||||
int* mode;
|
||||
uint8_t* update_no;
|
||||
};
|
||||
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
|
||||
|
||||
/**************************************************************************/
|
||||
/*Updater routine from msg processing to file checks to be called from main*/
|
||||
/**************************************************************************/
|
||||
void update_routine(const char* bcfname,
|
||||
const char* dbgfname);
|
||||
void update_routine();
|
||||
|
||||
/************************************************/
|
||||
/*Initalizes the updater */
|
||||
/************************************************/
|
||||
void init_update_monitor(const char* bo_filename,const char* stand_by_script);
|
||||
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
|
||||
|
||||
/*********************************************************/
|
||||
/*Appends buffer of given size to in msg queue of updater*/
|
||||
/*********************************************************/
|
||||
|
||||
void code_message_inqueue_append(uint8_t* msg,uint16_t size);
|
||||
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
|
||||
|
||||
/*********************************************************/
|
||||
/*Processes messages inside the queue of the updater*/
|
||||
|
@ -108,14 +131,13 @@ void destroy_out_msg_queue();
|
|||
/***************************************************/
|
||||
int get_update_mode();
|
||||
|
||||
|
||||
buzz_updater_elem_t get_updater();
|
||||
/***************************************************/
|
||||
/*sets bzz file name*/
|
||||
/***************************************************/
|
||||
void set_bzz_file(const char* in_bzz_file);
|
||||
|
||||
int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size);
|
||||
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
|
||||
|
||||
/****************************************************/
|
||||
/*Destroys the updater*/
|
||||
|
@ -129,9 +151,11 @@ int get_update_status();
|
|||
|
||||
void set_read_update_status();
|
||||
|
||||
int compile_bzz();
|
||||
int compile_bzz(std::string bzz_file);
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
|
||||
void collect_data(std::ofstream &logger);
|
||||
void set_packet_id(int packet_id);
|
||||
|
||||
void collect_data(std::ofstream& logger);
|
||||
#endif
|
||||
|
|
|
@ -12,20 +12,25 @@
|
|||
#include <map>
|
||||
|
||||
using namespace std;
|
||||
namespace buzz_utility{
|
||||
|
||||
namespace buzz_utility
|
||||
{
|
||||
struct pos_struct
|
||||
{
|
||||
double x,y,z;
|
||||
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
|
||||
pos_struct(){}
|
||||
double x, y, z;
|
||||
pos_struct(double x, double y, double z) : x(x), y(y), z(z){};
|
||||
pos_struct()
|
||||
{
|
||||
}
|
||||
};
|
||||
typedef struct pos_struct Pos_struct;
|
||||
struct rb_struct
|
||||
{
|
||||
double r,b,latitude,longitude,altitude;
|
||||
rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
|
||||
rb_struct(){}
|
||||
double r, b, latitude, longitude, altitude;
|
||||
rb_struct(double la, double lo, double al, double r, double b)
|
||||
: latitude(la), longitude(lo), altitude(al), r(r), b(b){};
|
||||
rb_struct()
|
||||
{
|
||||
}
|
||||
};
|
||||
typedef struct rb_struct RB_struct;
|
||||
|
||||
|
@ -35,21 +40,15 @@ struct neiStatus
|
|||
uint batt_lvl = 0;
|
||||
uint xbee = 0;
|
||||
uint flight_status = 0;
|
||||
}; typedef struct neiStatus neighbors_status ;
|
||||
|
||||
};
|
||||
typedef struct neiStatus neighbors_status;
|
||||
|
||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||
|
||||
int buzz_listen(const char* type,
|
||||
int msg_size);
|
||||
|
||||
void add_user(int id, double latitude, double longitude, float altitude);
|
||||
void update_users();
|
||||
int buzz_listen(const char* type, int msg_size);
|
||||
int make_table(buzzobj_t* t);
|
||||
int buzzusers_add(int id, double latitude, double longitude, double altitude);
|
||||
int buzzusers_reset();
|
||||
int compute_users_rb();
|
||||
int create_stig_tables();
|
||||
int create_stig_tables();
|
||||
|
||||
void in_msg_append(uint64_t* payload);
|
||||
|
||||
|
@ -57,12 +56,11 @@ uint64_t* obt_out_msg();
|
|||
|
||||
void update_sensors();
|
||||
|
||||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename, int robot_id);
|
||||
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id);
|
||||
|
||||
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
|
||||
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
|
||||
|
||||
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
|
||||
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
|
||||
|
||||
void buzz_script_step();
|
||||
|
||||
|
|
|
@ -1,31 +1,30 @@
|
|||
#pragma once
|
||||
//#ifndef BUZZUAV_CLOSURES_H
|
||||
//#define BUZZUAV_CLOSURES_H
|
||||
|
||||
#include <buzz/buzzvm.h>
|
||||
#include <stdio.h>
|
||||
#include "uav_utility.h"
|
||||
#include "mavros_msgs/CommandCode.h"
|
||||
#include "mavros_msgs/Mavlink.h"
|
||||
#include "ros/ros.h"
|
||||
#include "buzz_utility.h"
|
||||
|
||||
#define EARTH_RADIUS (double) 6371000.0
|
||||
#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0)))
|
||||
#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI)))
|
||||
#define EARTH_RADIUS (double)6371000.0
|
||||
#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
|
||||
#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI)))
|
||||
|
||||
namespace buzzuav_closures{
|
||||
typedef enum {
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_LAND,
|
||||
COMMAND_GOHOME,
|
||||
COMMAND_ARM,
|
||||
COMMAND_DISARM,
|
||||
COMMAND_GOTO,
|
||||
COMMAND_MOVETO,
|
||||
COMMAND_PICTURE,
|
||||
COMMAND_GIMBAL,
|
||||
} Custom_CommandCode;
|
||||
namespace buzzuav_closures
|
||||
{
|
||||
typedef enum {
|
||||
COMMAND_NIL = 0, // Dummy command
|
||||
COMMAND_TAKEOFF, // Take off
|
||||
COMMAND_LAND,
|
||||
COMMAND_GOHOME,
|
||||
COMMAND_ARM,
|
||||
COMMAND_DISARM,
|
||||
COMMAND_GOTO,
|
||||
COMMAND_MOVETO,
|
||||
COMMAND_PICTURE,
|
||||
COMMAND_GIMBAL,
|
||||
} Custom_CommandCode;
|
||||
|
||||
/*
|
||||
* prextern int() function in Buzz
|
||||
|
@ -46,8 +45,6 @@ void parse_gpslist();
|
|||
int buzzuav_takepicture(buzzvm_t vm);
|
||||
/* Returns the current command from local variable*/
|
||||
int getcmd();
|
||||
/*Sets goto position */
|
||||
void set_goto(double pos[]);
|
||||
/*Sets goto position from rc client*/
|
||||
void rc_set_goto(int id, double latitude, double longitude, double altitude);
|
||||
/*Sets gimbal orientation from rc client*/
|
||||
|
@ -55,7 +52,7 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
|
|||
/*sets rc requested command */
|
||||
void rc_call(int rc_cmd);
|
||||
/* sets the battery state */
|
||||
void set_battery(float voltage,float current,float remaining);
|
||||
void set_battery(float voltage, float current, float remaining);
|
||||
void set_deque_full(bool state);
|
||||
void set_rssi(float value);
|
||||
void set_raw_packet_loss(float value);
|
||||
|
@ -89,7 +86,7 @@ int buzzuav_arm(buzzvm_t vm);
|
|||
/*
|
||||
* Disarm from buzz
|
||||
*/
|
||||
int buzzuav_disarm(buzzvm_t vm) ;
|
||||
int buzzuav_disarm(buzzvm_t vm);
|
||||
/* Commands the UAV to land
|
||||
*/
|
||||
int buzzuav_land(buzzvm_t vm);
|
||||
|
@ -127,7 +124,6 @@ int buzzuav_update_prox(buzzvm_t vm);
|
|||
|
||||
int bzz_cmd();
|
||||
|
||||
|
||||
int dummy_closure(buzzvm_t vm);
|
||||
|
||||
//#endif
|
||||
|
|
|
@ -20,12 +20,12 @@
|
|||
#include "mavros_msgs/ParamGet.h"
|
||||
#include "geometry_msgs/PoseStamped.h"
|
||||
#include "std_msgs/Float64.h"
|
||||
#include "std_msgs/String.h"
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <rosbuzz/neigh_pos.h>
|
||||
#include <sstream>
|
||||
#include <buzz/buzzasm.h>
|
||||
#include "buzz_utility.h"
|
||||
#include "uav_utility.h"
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
@ -37,7 +37,7 @@
|
|||
#define UPDATER_MESSAGE_CONSTANT 987654321
|
||||
#define XBEE_MESSAGE_CONSTANT 586782343
|
||||
#define XBEE_STOP_TRANSMISSION 4355356352
|
||||
#define TIMEOUT 60
|
||||
#define TIMEOUT 60
|
||||
#define BUZZRATE 10
|
||||
#define CMD_REQUEST_UPDATE 666
|
||||
|
||||
|
@ -45,14 +45,12 @@ using namespace std;
|
|||
|
||||
namespace rosbzz_node
|
||||
{
|
||||
|
||||
class roscontroller
|
||||
{
|
||||
|
||||
public:
|
||||
roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
|
||||
~roscontroller();
|
||||
//void RosControllerInit();
|
||||
// void RosControllerInit();
|
||||
void RosControllerRun();
|
||||
|
||||
static const string CAPTURE_SRV_DEFAULT_NAME;
|
||||
|
@ -63,43 +61,49 @@ private:
|
|||
uint8_t history[10];
|
||||
uint8_t index = 0;
|
||||
uint8_t current = 0;
|
||||
num_robot_count(){}
|
||||
}; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
|
||||
num_robot_count()
|
||||
{
|
||||
}
|
||||
};
|
||||
typedef struct num_robot_count Num_robot_count;
|
||||
|
||||
Num_robot_count count_robots;
|
||||
|
||||
struct gps
|
||||
{
|
||||
double longitude=0.0;
|
||||
double latitude=0.0;
|
||||
float altitude=0.0;
|
||||
}; typedef struct gps GPS ;
|
||||
double longitude = 0.0;
|
||||
double latitude = 0.0;
|
||||
float altitude = 0.0;
|
||||
};
|
||||
typedef struct gps GPS;
|
||||
|
||||
GPS target, home, cur_pos;
|
||||
double cur_rel_altitude;
|
||||
|
||||
uint64_t payload;
|
||||
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map< int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step=0;
|
||||
int robot_id=0;
|
||||
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
// std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
|
||||
int timer_step = 0;
|
||||
int robot_id = 0;
|
||||
std::string robot_name = "";
|
||||
//int oldcmdID=0;
|
||||
// int oldcmdID=0;
|
||||
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int message_number=0;
|
||||
uint8_t no_of_robots=0;
|
||||
int message_number = 0;
|
||||
uint8_t no_of_robots = 0;
|
||||
/*tmp to be corrected*/
|
||||
uint8_t no_cnt=0;
|
||||
uint8_t old_val=0 ;
|
||||
uint8_t no_cnt = 0;
|
||||
uint8_t old_val = 0;
|
||||
std::string bzzfile_name;
|
||||
std::string fcclient_name;
|
||||
std::string armclient;
|
||||
std::string modeclient;
|
||||
std::string rcservice_name;
|
||||
std::string bcfname,dbgfname;
|
||||
std::string bcfname, dbgfname;
|
||||
std::string out_payload;
|
||||
std::string in_payload;
|
||||
std::string obstacles_topic;
|
||||
|
@ -113,13 +117,13 @@ private:
|
|||
bool rcclient;
|
||||
bool xbeeplugged = false;
|
||||
bool multi_msg;
|
||||
Num_robot_count count_robots;
|
||||
ros::ServiceClient mav_client;
|
||||
ros::ServiceClient xbeestatus_srv;
|
||||
ros::ServiceClient capture_srv;
|
||||
ros::Publisher payload_pub;
|
||||
ros::Publisher MPpayload_pub;
|
||||
ros::Publisher neigh_pos_pub;
|
||||
ros::Publisher uavstate_pub;
|
||||
ros::Publisher localsetpoint_nonraw_pub;
|
||||
ros::ServiceServer service;
|
||||
ros::Subscriber current_position_sub;
|
||||
|
@ -136,7 +140,6 @@ private:
|
|||
ros::Subscriber local_pos_sub;
|
||||
double local_pos_new[3];
|
||||
|
||||
|
||||
ros::ServiceClient stream_client;
|
||||
|
||||
int setpoint_counter;
|
||||
|
@ -145,7 +148,7 @@ private:
|
|||
std::ofstream log;
|
||||
|
||||
/*Commands for flight controller*/
|
||||
//mavros_msgs::CommandInt cmd_srv;
|
||||
// mavros_msgs::CommandInt cmd_srv;
|
||||
mavros_msgs::CommandLong cmd_srv;
|
||||
std::vector<std::string> m_sMySubscriptions;
|
||||
std::map<std::string, std::string> m_smTopic_infos;
|
||||
|
@ -159,7 +162,7 @@ private:
|
|||
/*Initialize publisher and subscriber, done in the constructor*/
|
||||
void Initialize_pub_sub(ros::NodeHandle& n_c);
|
||||
|
||||
std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
|
||||
std::string current_mode;
|
||||
|
||||
/*Obtain data from ros parameter server*/
|
||||
void Rosparameters_get(ros::NodeHandle& n_c_priv);
|
||||
|
@ -173,33 +176,33 @@ private:
|
|||
/*Neighbours pos publisher*/
|
||||
void neighbours_pos_publisher();
|
||||
|
||||
/*UAVState publisher*/
|
||||
void uavstate_publisher();
|
||||
|
||||
/*BVM message payload publisher*/
|
||||
void send_MPpayload();
|
||||
|
||||
/*Prepare messages and publish*/
|
||||
void prepare_msg_and_publish();
|
||||
|
||||
|
||||
/*Refresh neighbours Position for every ten step*/
|
||||
void maintain_pos(int tim_step);
|
||||
|
||||
/*Puts neighbours position inside neigbours_pos_map*/
|
||||
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
|
||||
|
||||
/*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
|
||||
void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
|
||||
|
||||
/*Set the current position of the robot callback*/
|
||||
void set_cur_pos(double latitude,
|
||||
double longitude,
|
||||
double altitude);
|
||||
void set_cur_pos(double latitude, double longitude, double altitude);
|
||||
|
||||
/*convert from spherical to cartesian coordinate system callback */
|
||||
float constrainAngle(float x);
|
||||
float constrainAngle(float x);
|
||||
void gps_rb(GPS nei_pos, double out[]);
|
||||
void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
|
||||
void gps_ned_home(float &ned_x, float &ned_y);
|
||||
void gps_convert_ned(float &ned_x, float &ned_y,
|
||||
double gps_t_lon, double gps_t_lat,
|
||||
double gps_r_lon, double gps_r_lat);
|
||||
void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
|
||||
void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
|
||||
double gps_r_lat);
|
||||
|
||||
/*battery status callback */
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
|
@ -212,8 +215,6 @@ private:
|
|||
|
||||
/*current position callback*/
|
||||
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
|
||||
void users_pos(const rosbuzz::neigh_pos msg);
|
||||
|
||||
|
||||
/*current relative altitude callback*/
|
||||
void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
|
||||
|
@ -222,7 +223,7 @@ private:
|
|||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
|
||||
|
||||
/* RC commands service */
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request &req, mavros_msgs::CommandLong::Response &res);
|
||||
bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
|
||||
|
||||
/*robot id sub callback*/
|
||||
void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
|
||||
|
@ -244,7 +245,7 @@ private:
|
|||
|
||||
void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
|
||||
|
||||
//void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
// void WaypointMissionSetup(float lat, float lng, float alt);
|
||||
|
||||
void fc_command_setup();
|
||||
|
||||
|
@ -257,15 +258,13 @@ private:
|
|||
void get_number_of_robots();
|
||||
|
||||
void GetRobotId();
|
||||
bool GetDequeFull(bool &result);
|
||||
bool GetRssi(float &result);
|
||||
bool GetDequeFull(bool& result);
|
||||
bool GetRssi(float& result);
|
||||
bool TriggerAPIRssi(const uint8_t short_id);
|
||||
bool GetAPIRssi(const uint8_t short_id, float &result);
|
||||
bool GetRawPacketLoss(const uint8_t short_id, float &result);
|
||||
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
|
||||
bool GetAPIRssi(const uint8_t short_id, float& result);
|
||||
bool GetRawPacketLoss(const uint8_t short_id, float& result);
|
||||
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
|
||||
|
||||
void get_xbee_status();
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
@ -1,9 +0,0 @@
|
|||
#ifndef UAV_UTILITY_H
|
||||
#define UAV_UTILITY_H
|
||||
|
||||
extern void uav_setup();
|
||||
|
||||
extern void uav_done();
|
||||
|
||||
|
||||
#endif
|
3
log.txt
3
log.txt
|
@ -1,3 +0,0 @@
|
|||
|
||||
2017-01-08-14-24-24 enter TranslateFunc
|
||||
2017-01-08-14-24-24 start to read frames!
|
45
readme.md
45
readme.md
|
@ -21,20 +21,7 @@ More information is available at http://the.swarming.buzz/wiki/doku.php?id=start
|
|||
Description:
|
||||
============
|
||||
|
||||
Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz as a node in ROS. The rosbuzz_node requires certain parameters to run. These parameters are supplied to the node from the ros parameter server. The package also contains a launch file called “rosbuzzm100.launch”, which initializes the required parameters. The required parameters are the .bzz file location, presence of Remote Controller client, Remote Controller service name, Flight Controller client name, Robot ID and Number of Robots.
|
||||
|
||||
* The .bzz file location parameter is a string with name “bzzfile_name”, the node compiles and parses the specified .bzz file into .basm, .bo and .bdbg files. The node runs the parsed .bo byte file.
|
||||
|
||||
* The presence of remote controller client is a bool parameter with name “rcclient”, this specifies whether there is a remote controller present to issue bypassing commands like takeoff, land, goto location and go home. If there is no remote controller present then this parameter could be set to “false”. If a remote controller exists, this parameter could be set “True” and the service topic offered by the remote controller should be specified to the parameter named “rcservice_name”.
|
||||
|
||||
* The flight controller client present in the node, that is used to send commands to the flight controller. The commands that could be sent are takeoff, land, goto location and go home. The topic name used to communicate with the flight controller should be set to the parameter named “fcclient_name”.
|
||||
|
||||
* The next parameter that the rosbuzz_node depends on is the robot ID of type “int”, this parameter should be set to the parameter named “robot_id” in the parameter server. This parameter is by default set to ‘0’, if not specified.
|
||||
|
||||
* Hot swaps of code can be integrated into the ROSBuzz node directly from a text editor. The ROSBuzz node monitors the file supplied i.e. the name specified in "bzzfile_name". When this file is modified and saved, the new script is tested and once test passes. The new script is set to the current robot and sent to all the robots in the network. To ensure convergence, i.e. all the robots recived the code, number of robots present in the network is needed. The number of robots has to be specified under the parameter name "No_of_Robots".
|
||||
|
||||
An example template of parameter setting could be found in the launch file “launch/rosbuzzm100.launch”.
|
||||
The launch file named "launch/rosbuzz_2_parallel.launch" can be used to launch two nodes in parellel for testing ROSBuzz node on a single Machine.
|
||||
Rosbuzz package is the ROS version of Buzz. The package contains a node called “rosbuzz_node”, which implements buzz virtual machine (BVM) as a node in ROS.
|
||||
|
||||
|
||||
Downloading ROS Package
|
||||
|
@ -75,37 +62,41 @@ Run
|
|||
===
|
||||
To run the ROSBuzz package using the launch file, execute the following:
|
||||
|
||||
$ roslaunch rosbuzz rosbuzzm100.launch
|
||||
$ roslaunch rosbuzz rosbuzz.launch
|
||||
|
||||
Note : Before launching the ROSBuzz node, verify all the parameter in the launch file.
|
||||
Note : Before launching the ROSBuzz node, verify all the parameters in the launch file. A launch file using gdb is available also (rosbuzzd.launch).
|
||||
|
||||
* Buzz scripts: Several behavioral scripts are included in the "buzz_Scripts" folder, such as "graphformGPS.bzz" uses in the ICRA publication below and the "testaloneWP.bzz" to control a single drone with a ".csv" list of waypoints. The script "empty.bzz" is a template script.
|
||||
|
||||
Publisher
|
||||
=========
|
||||
|
||||
* Messages from Buzz (BVM):
|
||||
The package publishes mavros_msgs/Mavlink message with a topic name of "outMavlink".
|
||||
The package publishes mavros_msgs/Mavlink message "outMavlink".
|
||||
|
||||
* Command to the flight controller:
|
||||
The package publishes geometry_msgs/PoseStamped message "setpoint_position/local".
|
||||
|
||||
Subscribers
|
||||
-----------
|
||||
|
||||
* Current position of the Robot:
|
||||
The package subscribes' to sensor_msgs/NavSatFix message with a topic name of "current_pos".
|
||||
The package subscribes to sensor_msgs/NavSatFix message "global_position/global", to a std_msgs/Float64 message "global_position/rel_alt" and to a geometry_msgs/PoseStamped message "local_position/pose".
|
||||
|
||||
* Messages to Buzz (BVM):
|
||||
The package subscribes' to mavros_msgs/Mavlink message with a topic name of "inMavlink".
|
||||
The package subscribes to mavros_msgs/Mavlink message with a topic name of "inMavlink".
|
||||
|
||||
* Battery status:
|
||||
The package subscribes' to mavros_msgs/BatteryStatus message with a topic name of "battery_state".
|
||||
* Status:
|
||||
The package subscribes to mavros_msgs/BatteryStatus message "battery" and to either a mavros_msgs/ExtendedState message "extended_state" or a mavros_msgs/State message "state".
|
||||
|
||||
Service
|
||||
-------
|
||||
|
||||
* Remote Controller command:
|
||||
The package offers service using mavros_msgs/CommandInt service with name "rc_cmd".
|
||||
* Remote Controller:
|
||||
The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its state. In the "misc" folder a bash script shows how to control the Buzz states from the command line.
|
||||
|
||||
Client
|
||||
References
|
||||
------
|
||||
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
|
||||
|
||||
* Flight controller client:
|
||||
This package is a client of mavros_msgs/CommandInt service with name "fc_cmd".
|
||||
|
||||
* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
|
||||
|
|
1062
src/buzz_update.cpp
1062
src/buzz_update.cpp
File diff suppressed because it is too large
Load Diff
1344
src/buzz_utility.cpp
1344
src/buzz_utility.cpp
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
@ -1,8 +1,8 @@
|
|||
/** @file rosbuzz.cpp
|
||||
* @version 1.0
|
||||
* @version 1.0
|
||||
* @date 27.09.2016
|
||||
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
||||
* @author Vivek Shankar Varadharajan
|
||||
* @brief Buzz Implementation as a node in ROS.
|
||||
* @author Vivek Shankar Varadharajan and David St-Onge
|
||||
* @copyright 2016 MistLab. All rights reserved.
|
||||
*/
|
||||
|
||||
|
@ -11,18 +11,16 @@
|
|||
/**
|
||||
* This program implements Buzz node in ros using mavros_msgs.
|
||||
*/
|
||||
|
||||
int main(int argc, char **argv)
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
/*Initialize rosBuzz node*/
|
||||
ros::init(argc, argv, "rosBuzz");
|
||||
ros::init(argc, argv, "rosBuzz");
|
||||
ros::NodeHandle nh_priv("~");
|
||||
ros::NodeHandle nh;
|
||||
rosbzz_node::roscontroller RosController(nh, nh_priv);
|
||||
/*Register ros controller object inside buzz*/
|
||||
//buzzuav_closures::set_ros_controller_ptr(&RosController);
|
||||
RosController.RosControllerRun();
|
||||
// buzzuav_closures::set_ros_controller_ptr(&RosController);
|
||||
RosController.RosControllerRun();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -1,25 +0,0 @@
|
|||
#include "uav_utility.h"
|
||||
#include "stdio.h"
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
||||
/****************************************/
|
||||
/*To do !*/
|
||||
/****************************************/
|
||||
|
||||
void uav_setup() {
|
||||
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/*To do !*/
|
||||
/****************************************/
|
||||
|
||||
void uav_done() {
|
||||
|
||||
fprintf(stdout, "Robot stopped.\n");
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
/****************************************/
|
Loading…
Reference in New Issue