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

Merged RosBuzz Modifications

This commit is contained in:
py-humanitas 2017-06-19 16:36:18 -04:00
parent 7d3e844f6c 78edc3c9f0
commit 1d24de28a5
24 changed files with 1240 additions and 2702 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
misc/cmdlinectr.sh
View File

@ -1,9 +1,15 @@
#! /bin/bash #! /bin/bash
function takeoff { function takeoff {
rosservice call /buzzcmd 0 22 0 0 0 0 0 0 0 0 rosservice call $1/buzzcmd 0 22 0 0 0 0 0 0 0 0
} }
function land { function land {
rosservice call /buzzcmd 0 21 0 0 0 0 0 0 0 0 rosservice call $1/buzzcmd 0 21 0 0 0 0 0 0 0 0
}
function arm {
rosservice call $1/buzzcmd 0 400 0 1 0 0 0 0 0 0
}
function disarm {
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
} }
function arm { function arm {
rosservice call /buzzcmd 0 400 0 1 0 0 0 0 0 0 rosservice call /buzzcmd 0 400 0 1 0 0 0 0 0 0
@ -12,7 +18,11 @@ function disarm {
rosservice call /buzzcmd 0 400 0 0 0 0 0 0 0 0 rosservice call /buzzcmd 0 400 0 0 0 0 0 0 0 0
} }
function record { function record {
<<<<<<< HEAD
rosbag record /flight_status /global_position /users_pos /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info rosbag record /flight_status /global_position /users_pos /dji_sdk/local_position /neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
=======
rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
>>>>>>> 78edc3c9f069e1c186f3b9a5cb3853a233f3dde3
} }
function clean { function clean {
sudo rm /var/log/upstart/robot* sudo rm /var/log/upstart/robot*
@ -29,3 +39,7 @@ function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
} }
function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch/m100buzzy.launch
}

4
script/Graph_drone.graph Normal file
View File

@ -0,0 +1,4 @@
0 -1 -1 -1
1 0 1000.0 0.0
2 0 1000.0 1.57
3 0 1000.0 3.14

5
script/Graph_fixed.graph Normal file
View File

@ -0,0 +1,5 @@
0 -1 -1 -1 -1
1 0 1000.0 -1 -1
2 0 1000.0 1 1414.2
3 0 1000.0 2 1414.2
4 0 1000.0 1 1414.2

102
script/flock.bzz Normal file
View File

@ -0,0 +1,102 @@
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 = 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()
}
# Executed at each time step.
function step() {
uav_rccmd()
uav_neicmd()
statef()
log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

848
script/graphform.bzz Normal file
View File

@ -0,0 +1,848 @@
#
# Include files
#
include "string.bzz"
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.
#
#Constant parameters, need to be adjust
#parameters for set_wheels
m_sWheelTurningParams={.MaxSpeed=1.0}
ROBOT_RADIUS=0.5
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
#
# 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_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageSide={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCunt=0#used to cunt neighbours
#Save message from one neighbour
#the indexes are as State(received state),Lable(received lable),ReqLable,ReqID,Side,Response,Range,Bearing
m_receivedMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE",.Range=0,.Bearing=0}
#
#Save the message to send
#The keys of the talbe is State(current state),Lable(current lable),ReqLable(requested lable),ReqID(request id),Side(side),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
#Current robot state
m_eState="STATE_FREE"
#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
#Table of the nodes in the graph
m_vecNodes={}
m_vecNodes_fixed={}
#Current label being requested or chosen (-1 when none)
m_nLabel=-1
#Label request id
m_unRequestId=0
#Side
m_unbSide=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
v_tag = stigmergy.create(1)
# Lennard-Jones parameters
EPSILON = 3.5 #3.5
# Lennard-Jones interaction magnitude
function FlockInteraction(dist,target,epsilon){
var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
return mag
}
function Norm(vector) {
var l = math.vec2.length(vector)
return {
.x = vector.x / l,
.y = vector.y / l
}
}
function LimitAngle(angle){
if(angle>2*math.pi)
return angle-2*math.pi
else if (angle<0)
return angle+2*math.pi
else
return angle
}
#
# Calculates the length of the given vector2.
# PARAM v: The vector2.
# RETURN: The length of the vector.
#
Length = function(v) {
return math.sqrt(v.x * v.x + v.y * v.y)
}
#
# Calculates the angle of the given vector2.
# PARAM v: The vector2.
# RETURN: The angle of the vector.
#
Angle = function(v) {
return math.atan(v.y, v.x)
}
#
#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
}
#
#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
}
function pow(base,exponent){
var i=0
var renturn_val=1
if(exponent==0)
return 1
else{
while(i<exponent){
renturn_val=renturn_val*base
i=i+1
}
return renturn_val
}
}
#
# Graph parsing
#
function parse_graph(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, predecessor, distance, bearing
.Lable = string.toint(rrec[0]), # Lable of the point
.Pred = string.toint(rrec[1]), # Lable of its predecessor
.distance = string.tofloat(rrec[2]), # distance to the predecessor
.bearing = string.tofloat(rrec[3]), # bearing form the predecessor to this dot
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function parse_graph_fixed(fname) {
# Graph data
var gd = {}
# Open the file
var fd = io.fopen(fname, "r")
if(not fd) {
log("Can't open '", fname, "'")
return nil
}
# Parse the file line by line
var rrec # Record read from line
var grec # Record parsed into graph
io.fforeach(fd, function(line) {
# Parse file line
rrec = string.split(line, "\t ")
# Make record
gd[string.toint(rrec[0])] = { # The .graph file is stored according the sequence of lable, pre1, dis2pr1, pre2, ids2pre2, side
.Pred1 = string.toint(rrec[1]), # Pred 1 lable
.Pred2 = string.toint(rrec[3]), # Pred 2 lable
.d1 = string.tofloat(rrec[2]), # Pred 1 distance
.d2 = string.tofloat(rrec[4]), # Pred 2 distance
#.S = string.toint(rrec[5]), # Side
.Lable=string.toint(rrec[0]),
.State="UNASSIGNED",
.StateAge=0
}})
# All done
io.fclose(fd)
return gd
}
function start_listen(){
neighbors.listen("msg",
function(vid,value,rid){
#store the received message
var temp_id=rid
m_receivedMessage.State=value.State
m_receivedMessage.Lable=value.Lable
m_receivedMessage.ReqLable=value.ReqLable
m_receivedMessage.ReqID=value.ReqID
m_receivedMessage.Side=value.Side
m_receivedMessage.Response=value.Response
Get_DisAndAzi(temp_id)
#add the received message
#
m_MessageState[m_neighbourCunt]=value.State
m_MessageLable[m_neighbourCunt]=value.Lable
m_MessageReqLable[m_neighbourCunt]=value.ReqLable
m_MessageReqID[m_neighbourCunt]=value.ReqID
m_MessageSide[m_neighbourCunt]=value.Side
m_MessageResponse[m_neighbourCunt]=value.Response
m_MessageRange[m_neighbourCunt]=m_receivedMessage.Range
m_MessageBearing[m_neighbourCunt]=m_receivedMessage.Bearing
m_neighbourCunt=m_neighbourCunt+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_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
}
else if(m_MessageState[i]=="STATE_JOINING"){
m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
m_vecNodes[m_MessageLable[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(){
m_eState="STATE_FREE"
m_unWaitCount=m_unLabelSearchWaitTime
m_selfMessage.State=m_eState
}
#
#Transistion to state asking
#
function TransitionToAsking(un_label){
m_eState="STATE_ASKING"
m_nLabel=un_label
m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
m_unWaitCount=m_unResponseTimeThreshold
}
#
#Transistion to state joining
#
function TransitionToJoining(){
m_eState="STATE_JOINING"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_unWaitCount=m_unJoiningLostPeriod
neighbors.listen("reply",
function(vid,value,rid){
#store the received message
if(value.Lable==m_nLabel){
m_cMeToPred.GlobalBearing=value.GlobalBearing
}
})
}
#
#Transistion to state joined
#
function TransitionToJoined(){
m_eState="STATE_JOINED"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
neighbors.ignore("reply")
#write statues
v_tag.put(m_nLabel, 1)
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
#Transistion to state Lock, lock the current formation
#
function TransitionToLock(){
m_eState="STATE_LOCK"
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_vecNodes[m_nLabel].State="ASSIGNED"
m_navigation.x=0.0
m_navigation.y=0.0
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
#
# Do free
#
function DoFree() {
m_selfMessage.State=m_eState
#wait for a while before looking for a lable
if(m_unWaitCount>0)
m_unWaitCount=m_unWaitCount-1
#find a set of joined robots
var setJoinedLables={}
var setJoinedIndexes={}
var i=0
var j=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_JOINED"){
setJoinedLables[j]=m_MessageLable[i]
setJoinedIndexes[j]=i
j=j+1
}
i=i+1
}
#go through the graph to look for a proper lable
var unFoundLable=0
var IDofPred=0
i=1
while(i<size(m_vecNodes) and (unFoundLable==0)){
#if the node is unassigned and the predecessor is insight
if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLables,m_vecNodes[i].Pred)==1){
unFoundLable=m_vecNodes[i].Lable
IDofPred=find(m_MessageLable,m_vecNodes[unFoundLable].Pred)
}
i=i+1
}
if(unFoundLable>0){
TransitionToAsking(unFoundLable)
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)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}else{ #no joined robots in sight
i=0
var tempvec={.x=0.0,.y=0.0}
while(i<m_neighbourCunt){
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)
}
#jump the first step
if(step_cunt<=1){
uav_moveto(0.0,0.0)
}
#set message
m_selfMessage.State=m_eState
}
#
#Do asking
#
function DoAsking(){
#look for response from predecessor
var i=0
var psResponse=-1
while(i<m_neighbourCunt and psResponse==-1){
#the respond robot in joined state
#the request lable be the same as requesed
#get a respond
if(m_MessageState[i]=="STATE_JOINED"){
if(m_MessageReqLable[i]==m_nLabel)
if(m_MessageResponse[i]!="REQ_NONE"){
psResponse=i
}}
i=i+1
}
#analyse response
if(psResponse==-1){
#no response, wait
m_unWaitCount=m_unWaitCount-1
m_selfMessage.State=m_eState
m_selfMessage.ReqLable=m_nLabel
m_selfMessage.ReqID=m_unRequestId
if(m_unWaitCount==0){
TransitionToFree()
return
}
}
else{
if(m_MessageReqID[psResponse]!=m_unRequestId)
TransitionToFree()
if(m_MessageReqID[psResponse]==m_unRequestId){
if(m_MessageResponse[psResponse]=="REQ_GRANTED"){
TransitionToJoining()
#TransitionToJoined()
return
}
else{
TransitionToAsking(m_nLabel)
return
}
}
}
uav_moveto(0.0,0.0)
}
#
#Do joining
#
function DoJoining(){
#get information of pred
var i=0
var IDofPred=-1
while(i<m_neighbourCunt and IDofPred==-1){
if(m_MessageLable[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_dis=Length(S2Target)
var S2Target_bearing=Angle(S2Target)
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
S2Target_bearing=S2Target_bearing+m_bias
m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
#test if is already in desired position
if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
log(S2Target_dis,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=m_eState
m_selfMessage.Lable=m_nLabel
}
#
#Do joined
#
function DoJoined(){
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
#collect all requests
var mapRequests={}
var i=0
var j=0
var ReqLable
var JoiningLable
var seenPred=0
while(i<m_neighbourCunt){
if(m_MessageState[i]=="STATE_ASKING"){
ReqLable=m_MessageReqLable[i]
if(m_vecNodes[ReqLable].State=="UNASSIGNED")
if(m_nLabel==m_vecNodes[ReqLable].Pred){
#is a request, store the index
mapRequests[j]=i
j=j+1
}
}
if(m_MessageState[i]=="STATE_JOINING"){
JoiningLable=m_MessageLable[i]
if(m_nLabel==m_vecNodes[JoiningLable].Pred){
##joining wrt this dot,send the global bearing
var m_messageForJoining={.Lable=JoiningLable,.GlobalBearing=m_MessageBearing[i]-m_bias}
neighbors.broadcast("reply",m_messageForJoining)
}
}
#if it is the pred
if(m_MessageState[i]=="STATE_JOINED" and m_MessageLable[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 Reqlable and Reqid are
ReqLable=m_MessageReqLable[mapRequests[ReqIndex]]
var ReqID=m_MessageReqID[mapRequests[ReqIndex]]
m_selfMessage.ReqLable=ReqLable
m_selfMessage.ReqID=ReqID
m_selfMessage.Response="REQ_GRANTED"
}
#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)
#check if should to transists to lock
if(v_tag.size()==ROBOTS){
#TransitionToLock()
}
}
#
#Do Lock
#
function DoLock(){
m_selfMessage.State=m_eState
m_selfMessage.Lable=m_nLabel
m_navigation.x=0.0
m_navigation.y=0.0
#collect preds information
var i=0
var mypred1={.range=0,.bearing=0}
var mypred2={.range=0,.bearing=0}
while(i<m_neighbourCunt){
#is the first predecessor
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred1){
mypred1.range=m_MessageRange[i]
mypred1.bearing=m_MessageBearing[i]
}
#is the second predecessor
if(m_MessageLable[i]==m_vecNodes_fixed[m_nLabel].Pred2){
mypred2.range=m_MessageRange[i]
mypred2.bearing=m_MessageBearing[i]
}
i=i+1
}
#calculate motion vection
if(m_nLabel==0){
m_navigation.x=0.0
m_navigation.y=0.0
log(";",m_nLabel,";",0)
}
if(m_nLabel==1){
var tempvec={.Range=0.0,.Bearing=0.0}
tempvec.Range=FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,10)
#tempvec.Range=mypred1.range-m_vecNodes_fixed[m_nLabel].d1
tempvec.Bearing=mypred1.bearing
m_navigation=math.vec2.newp(tempvec.Range,tempvec.Bearing)
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
if(m_nLabel>1){
var cDir={.x=0.0,.y=0.0}
var cDir1={.x=0.0,.y=0.0}
var cDir2={.x=0.0,.y=0.0}
cDir1=math.vec2.newp(FlockInteraction(mypred1.range,m_vecNodes_fixed[m_nLabel].d1,EPSILON),mypred1.bearing)
cDir2=math.vec2.newp(FlockInteraction(mypred2.range,m_vecNodes_fixed[m_nLabel].d2,EPSILON),mypred2.bearing)
#cDir1=math.vec2.newp((mypred1.range-m_vecNodes_fixed[m_nLabel].d1),mypred1.bearing)
#cDir2=math.vec2.newp((mypred2.range-m_vecNodes_fixed[m_nLabel].d2),mypred2.bearing)
cDir=math.vec2.add(cDir1,cDir2)
cDir=math.vec2.scale(cDir,100)
m_navigation.x=cDir.x
m_navigation.y=cDir.y
#log(m_nLabel,"error:",mypred1.range-m_vecNodes_fixed[m_nLabel].d1,"and",mypred2.range-m_vecNodes_fixed[m_nLabel].d2)
log(";",m_nLabel,";",mypred1.range-m_vecNodes_fixed[m_nLabel].d1)
}
#move
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
}
function action(){
statef=action
}
#
# Executed at init
#
function init() {
#
#Adjust parameters here
#
m_unResponseTimeThreshold=10
m_unLabelSearchWaitTime=10
m_fTargetDistanceTolerance=150
m_unJoiningLostPeriod=100
#
# Join Swarm
#
uav_initswarm()
Reset();
}
#
# Executed every step
#
function step(){
uav_rccmd()
uav_neicmd()
#update the graph
UpdateNodeInfo()
#reset message package to be sent
m_selfMessage={.State="STATE_FREE",.Lable=0,.ReqLable=0,.ReqID=0,.Side=0,.Response="REQ_NONE"}
#
#act according to current state
#
if(UAVSTATE=="IDLE"){
if(m_eState=="STATE_FREE")
DoFree()
else if(m_eState=="STATE_ESCAPE")
DoEscape()
else if(m_eState=="STATE_ASKING")
DoAsking()
else if(m_eState=="STATE_JOINING")
DoJoining()
else if(m_eState=="STATE_JOINED")
DoJoined()
else if(m_eState=="STATE_LOCK")
DoLock()
}
statef()
debug(m_eState,m_nLabel)
log("Current state: ", UAVSTATE)
log("Swarm size: ", ROBOTS)
#navigation
#broadcast messag
neighbors.broadcast("msg",m_selfMessage)
#
#clean message storage
m_MessageState={}#store received neighbour message
m_MessageLable={}#store received neighbour message
m_MessageReqLable={}#store received neighbour message
m_MessageReqID={}#store received neighbour message
m_MessageSide={}#store received neighbour message
m_MessageResponse={}#store received neighbour message
m_MessageRange={}#store received neighbour message
m_MessageBearing={}#store received neighbour message
m_neighbourCunt=0
#step cunt+1
step_cunt=step_cunt+1
}
#
# Executed when reset
#
function Reset(){
m_vecNodes={}
m_vecNodes = parse_graph("/home/dave/ROS_WS/src/rosbuzz/script/Graph_drone.graph")#change the .graph file when necessary
m_vecNodes_fixed={}
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/script/Graph_fixed.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==0){
m_nLabel=0
TransitionToJoined()
}
#[B]Other robots are defined as free.
else{
TransitionToFree()
}
}
#
# Executed upon destroy
#
function destroy() {
#clear neighbour message
uav_moveto(0.0,0.0)
m_vecNodes={}
#stop listening
neighbors.ignore("msg")
}

47
script/include/barrier.bzz Normal file
View File

@ -0,0 +1,47 @@
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 11
#
# Sets a barrier
#
function barrier_set(threshold, transf, resumef) {
statef = function() {
barrier_wait(threshold, transf, resumef);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
BARRIER_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf, resumef) {
barrier.get(id)
barrier.put(id, 1)
UAVSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
#barrier = nil
transf()
} else if(timeW>=BARRIER_TIMEOUT) {
barrier = nil
resumef()
timeW=0
}
timeW = timeW+1
}

105
script/include/uavstates.bzz Normal file
View File

@ -0,0 +1,105 @@
########################################
#
# FLIGHT-RELATED FUNCTIONS
#
########################################
TARGET_ALTITUDE = 5.0
UAVSTATE = "TURNEDOFF"
function uav_initswarm(){
s = swarm.create(1)
s.join()
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
function turnedoff() {
statef=turnedoff
UAVSTATE = "TURNEDOFF"
}
function idle() {
statef=idle
UAVSTATE = "IDLE"
}
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,action,land)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
UAVSTATE = "LAND"
statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
barrier_set(ROBOTS,idle,land)
barrier_ready()
timeW=0
#barrier = nil
#statef=idle
}
}
function uav_rccmd() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
UAVSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
UAVSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
}
function uav_neicmd() {
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and UAVSTATE!="TAKEOFF") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and UAVSTATE=="IDLE") {
uav_arm()
} else if(value==401 and UAVSTATE=="IDLE"){
uav_disarm()
}
})
}

9
script/include/update.bzz Normal file
View File

@ -0,0 +1,9 @@
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}

33
script/include/vstigenv.bzz Normal file
View File

@ -0,0 +1,33 @@
function checkusers() {
# Read a value from the structure
if(size(users)>0)
log("Got a user!")
# 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)
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}

193
script/tentacle.bzz
View File

@ -1,11 +1,7 @@
include "vec2.bzz" include "vec2.bzz"
updated="update_ack" include "update.bzz"
update_no=0 include "barrier.bzz" # don't use a stigmergy id=11 with this header.
function updated_neigh(){ include "uavstates.bzz" # require an 'action' function to be defined here.
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters # Lennard-Jones parameters
TARGET = 12.0 TARGET = 12.0
@ -226,125 +222,23 @@ function onetwo() {
} }
} }
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
barrier.put(id, 1)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
#barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
if(id==0)
barrier_set(ROBOTS, zero)
else
barrier_set(ROBOTS, onetwo)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
barrier_set(ROBOTS,idle)
barrier_ready()
timeW=0
#barrier = nil
#statef=idle
}
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
################################################# #################################################
### BUZZ FUNCTIONS ############################## ### BUZZ FUNCTIONS ##############################
################################################# #################################################
function action(){
if (id == 0)
statef=zero
else
statef=onetwo
UAVSTATE="TENTACLES"
}
# Executed at init time # Executed at init time
function init() { function init() {
s = swarm.create(0) uav_initswarm()
s.join()
# Local knowledge table # Local knowledge table
knowledge = {} knowledge = {}
# Update local knowledge with information from the neighbors # Update local knowledge with information from the neighbors
@ -352,72 +246,17 @@ function init() {
# Variables initialization # Variables initialization
iteration = 0 iteration = 0
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
} }
# Executed every time step # Executed every time step
function step() { function step() {
if(flight.rc_cmd==22) { uav_rccmd()
log("cmd 22") uav_neicmd()
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef() statef()
log("Current state: ", CURSTATE) log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS) 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)
# Count the number of steps # Count the number of steps
iteration = iteration + 1 iteration = iteration + 1
} }

45
script/test.bzz
View File

@ -1,45 +0,0 @@
# Executed once at init time.
function init() {
i = 1
a = 0
val = 0
}
# Executed at each time step.
function step() {
if (i == 0) {
neighbors.listen("Take",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
}
)
neighbors.listen("key",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
val = value
}
)
print(val)
if ((val == 23) and (a == 0)) {
uav_takeoff()
a=1
}
if (a == 10) uav_land()
if (a != 0) a = a+1
}
else{
neighbors.broadcast("key", 23)
neighbors.broadcast("Take", "no")
}
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

159
script/test1.bzz
View File

@ -1,159 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 2.0
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 10.0 #0.001
# 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() {
statef=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
print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x, accum.y)
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 2 # number of robots in the swarm
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf) {
barrier.get(id)
if(barrier.size() >= threshold) {
barrier = nil
transf()
}
}
# flight status
function idle() {
statef=idle
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22) {
statef=takeoff
} else if(value==21) {
statef=land
}
}
)
}
function takeoff() {
log("TakeOff: ", flight.status)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
else if( flight.status !=3){
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else
statef=idle
}
# Executed once at init time.
function init() {
statef=idle
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
statef()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

37
script/testalone.bzz
View File

@ -1,20 +1,15 @@
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"
# We need this for 2D vectors function action() {
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..." statef=action
#include "/home/ubuntu/buzz/src/include/vec2.bzz" # test moveto cmd dx, dy
#include "vec2.bzz" # uav_moveto(0.5, 0.5)
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
} }
TARGET_ALTITUDE = 5.0
# Executed once at init time. # Executed once at init time.
function init() { function init() {
} }
@ -22,19 +17,7 @@ function init() {
# Executed at each time step. # Executed at each time step.
function step() { function step() {
log("Altitude: ", position.altitude) log("Altitude: ", position.altitude)
if(flight.rc_cmd==22) { uav_rccmd()
flight.rc_cmd=0
uav_takeoff(TARGET_ALTITUDE)
} else if(flight.rc_cmd==21) {
flight.rc_cmd=0
uav_land()
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
# test moveto cmd
#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0)
# uav_moveto(0.5, 0.5)
} }
# Executed once when the robot (or the simulator) is reset. # Executed once when the robot (or the simulator) is reset.

275
script/testflockfev.bzz
View File

@ -1,275 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
CURSTATE = "TURNEDOFF"
# 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 hexagon() {
statef=hexagon
# 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)
CURSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# CURSTATE ="GOEAST"
# timeW = timeW+1
# uav_moveto(0.0,5.0)
# } else {
# CURSTATE ="GONORTH"
# timeW = timeW+1
# uav_moveto(5.0,0.0)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
barrier.put(id, 1)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
#barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
barrier_set(ROBOTS,idle)
barrier_ready()
timeW=0
#barrier = nil
#statef=idle
}
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
########################################
#
# MAIN FUNCTIONS
#
########################################
# Executed once at init time.
function init() {
s = swarm.create(1)
s.join()
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
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() {
}

242
script/testflockfev.bzz.old
View File

@ -1,242 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
#include "vec2.bzz"
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 12.0 #0.000001001
EPSILON = 3.0 #0.001
# 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() {
statef=hexagon
CURSTATE = "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
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x,accum.y)
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0
# statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) {
# timeW = timeW+1
# uav_moveto(0.06,0.0)
# } else {
# timeW = timeW+1
# uav_moveto(0.0,0.06)
# }
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
# ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.put(id, 1)
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
# barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
s.join()
vt = stigmergy.create(5)
t = {}
vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
# Check local users and push to v.stig
if(size(users.dataG)>0)
vt.put("p", users.dataG)
# Save locally the users and print RG
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() {
}

171
script/testflockfev_barrier.bzz
View File

@ -1,171 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 5.0
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 10.0 #0.001
# 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() {
statef=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
print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
uav_moveto(accum.x, accum.y)
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
ROBOTS = 3 # number of robots in the swarm
barrier_number=0
barrier_break=0
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
function barrier_wait(threshold, transf) {
barrier.get(id)
if ( (barrier.size() >= threshold) or (barrier_break==1) ) {
barrier = nil
transf()
barrier_number=barrier_number+1
barrier_break=0
}
}
# flight status
function idle() {
statef=idle
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22) {
statef=takeoff
} else if(value==21) {
statef=land
}
}
)
}
function takeoff() {
log("TakeOff: ", flight.status)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
}
else if( flight.status !=3){
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else
statef=idle
}
# Executed once at init time.
function init() {
statef=idle
}
# Executed at each time step.
function step() {
neighbors.broadcast("barrier_num", barrier_number)
neighbors.listen("barrier_num",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value > barrier_number) {
barrier_break=1
}
}
)
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
}
statef()
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

184
script/testflocksim.bzz
View File

@ -1,18 +1,10 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "vec2.bzz" include "vec2.bzz"
#################################################################################################### include "update.bzz"
# Updater related include "barrier.bzz" # don't use a stigmergy id=11 with this header.
# This should be here for the updater to work, changing position of code will crash the updater include "uavstates.bzz" # require an 'action' function to be defined here.
#################################################################################################### include "vstigenv.bzz"
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 10.0 TARGET_ALTITUDE = 10.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters # Lennard-Jones parameters
TARGET = 12.0 TARGET = 12.0
@ -48,8 +40,8 @@ function user_attract(t) {
} }
# Calculates and actuates the flocking interaction # Calculates and actuates the flocking interaction
function hexagon() { function action() {
statef=hexagon statef=action
# Calculate accumulator # Calculate accumulator
var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0) if(neighbors.count() > 0)
@ -65,129 +57,22 @@ function hexagon() {
# Move according to vector # Move according to vector
print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum)) print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
uav_moveto(accum.x, accum.y) uav_moveto(accum.x, accum.y)
CURSTATE = "LENNARDJONES" UAVSTATE = "LENNARDJONES"
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS # if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
# timeW =0 # timeW =0
# statef=land # statef=land
# } else if(timeW>=WAIT_TIMEOUT/2) { # } else if(timeW>=WAIT_TIMEOUT/2) {
# CURSTATE ="GOEAST" # UAVSTATE ="GOEAST"
# timeW = timeW+1 # timeW = timeW+1
# uav_moveto(0.0,5.0) # uav_moveto(0.0,5.0)
# } else { # } else {
# CURSTATE ="GONORTH" # UAVSTATE ="GONORTH"
# timeW = timeW+1 # timeW = timeW+1
# uav_moveto(5.0,0.0) # uav_moveto(5.0,0.0)
# } # }
} }
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 200
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
barrier.put(id, 1)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
#barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
#log("TakeOff: ", flight.status)
#log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS,hexagon)
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
#log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
barrier_set(ROBOTS,idle)
barrier_ready()
timeW=0
#barrier = nil
#statef=idle
}
}
function users_save(t) {
if(size(t)>0) {
foreach(t, function(id, tab) {
#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
add_user_rb(id,tab.la,tab.lo)
})
}
}
# printing the contents of a table: a custom function
function table_print(t) {
if(size(t)>0) {
foreach(t, function(u, tab) {
log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
})
}
}
######################################## ########################################
# #
@ -197,61 +82,20 @@ function table_print(t) {
# Executed once at init time. # Executed once at init time.
function init() { function init() {
s = swarm.create(1) uav_initswarm()
s.join()
vt = stigmergy.create(5) vt = stigmergy.create(5)
t = {} t = {}
vt.put("p",t) vt.put("p",t)
statef=idle
CURSTATE = "IDLE"
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
if(flight.rc_cmd==22) { uav_rccmd()
log("cmd 22") uav_neicmd()
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
#uav_goto()
add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef() statef()
log("Current state: ", CURSTATE) log("Current state: ", UAVSTATE)
log("Swarm size: ",ROBOTS) log("Swarm size: ",ROBOTS)
# Read a value from the structure # Read a value from the structure

1156
script/testsolo.basm
View File

File diff suppressed because it is too large Load Diff

BIN
script/testsolo.bdbg
View File

Binary file not shown.

214
script/testsolo.bzz
View File

@ -1,214 +0,0 @@
# We need this for 2D vectors
# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
include "/home/ubuntu/buzz/src/include/vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
TARGET_ALTITUDE = 3.0
CURSTATE = "TURNEDOFF"
# Lennard-Jones parameters
TARGET = 10.0 #0.000001001
EPSILON = 18.0 #0.001
# 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() {
statef=hexagon
CURSTATE = "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
#print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
# uav_moveto(accum.x,accum.y)
log("Time: ", timeW)
if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
timeW =0
statef=land
} else {
if(timeW >= (WAIT_TIMEOUT / 2)){
uav_moveto(0.03,0.0)
} else {
uav_moveto(0.0,0.03)
}
timeW = timeW+1
}
}
########################################
#
# BARRIER-RELATED FUNCTIONS
#
########################################
#
# Constants
#
BARRIER_VSTIG = 1
# ROBOTS = 3 # number of robots in the swarm
#
# Sets a barrier
#
function barrier_set(threshold, transf) {
statef = function() {
barrier_wait(threshold, transf);
}
barrier = stigmergy.create(BARRIER_VSTIG)
}
#
# Make yourself ready
#
function barrier_ready() {
barrier.put(id, 1)
}
#
# Executes the barrier
#
WAIT_TIMEOUT = 300
timeW=0
function barrier_wait(threshold, transf) {
barrier.get(id)
CURSTATE = "BARRIERWAIT"
if(barrier.size() >= threshold) {
barrier = nil
transf()
} else if(timeW>=WAIT_TIMEOUT) {
barrier = nil
statef=land
timeW=0
}
timeW = timeW+1
}
# flight status
function idle() {
statef=idle
CURSTATE = "IDLE"
}
function takeoff() {
CURSTATE = "TAKEOFF"
statef=takeoff
log("TakeOff: ", flight.status)
log("Relative position: ", position.altitude)
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, hexagon)
#barrier_set(ROBOTS, land);
barrier_ready()
#statef=hexagon
}
else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
function land() {
CURSTATE = "LAND"
statef=land
log("Land: ", flight.status)
if(flight.status == 2 or flight.status == 3){
neighbors.broadcast("cmd", 21)
uav_land()
}
else {
timeW=0
barrier = nil
statef=idle
}
}
# Executed once at init time.
function init() {
s = swarm.create(1)
# s.select(1)
s.join()
statef=idle
CURSTATE = "IDLE"
}
# Executed at each time step.
function step() {
if(flight.rc_cmd==22) {
log("cmd 22")
flight.rc_cmd=0
statef = takeoff
CURSTATE = "TAKEOFF"
neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) {
log("cmd 21")
log("To land")
flight.rc_cmd=0
statef = land
CURSTATE = "LAND"
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
statef = idle
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){
flight.rc_cmd=0
uav_disarm()
neighbors.broadcast("cmd", 401)
}
neighbors.listen("cmd",
function(vid, value, rid) {
print("Got (", vid, ",", value, ") from robot #", rid)
if(value==22 and CURSTATE=="IDLE") {
statef=takeoff
} else if(value==21) {
statef=land
} else if(value==400 and CURSTATE=="IDLE") {
uav_arm()
} else if(value==401 and CURSTATE=="IDLE"){
uav_disarm()
}
}
)
statef()
log("Current state: ", CURSTATE)
log("Swarm size: ",ROBOTS)
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

39
script/teststig.bzz
View File

@ -1,39 +0,0 @@
include "vec2.bzz"
####################################################################################################
# Updater related
# This should be here for the updater to work, changing position of code will crash the updater
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
neighbors.broadcast(updated, update_no)
}
function init(){
s = swarm.create(1)
s.join()
v = stigmergy.create(5)
t= {}
v.put("p",t)
v.put("u",1)
}
function step() {
log("Swarm size: ",ROBOTS)
log("The vstig has ", v.size(), " elements")
log(v.get("u"))
if (id==1) {
tmp = { .x=3}
v.put("p",tmp)
v.put("u",2)
}
log(v.get("p"))
}
# Executed once when the robot (or the simulator) is reset.
function reset() {
}
# Executed once at the end of experiment.
function destroy() {
}

2
src/buzz_update.cpp
View File

@ -29,7 +29,7 @@ static int updated=0;
/*Initialize updater*/ /*Initialize 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){
ROS_INFO("intiialized file monitor.\n"); ROS_INFO("Initializing file monitor...");
fd=inotify_init1(IN_NONBLOCK); fd=inotify_init1(IN_NONBLOCK);
if ( fd < 0 ) { if ( fd < 0 ) {
perror( "inotify_init error" ); perror( "inotify_init error" );

45
src/buzz_utility.cpp
View File

@ -17,9 +17,9 @@ namespace buzz_utility{
static char* BO_FNAME = 0; static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0; static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0; static buzzdebug_t DBG_INFO = 0;
static uint8_t MSG_SIZE = 250; // Only 100 bytes of Buzz messages every step static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step
static int MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static int Robot_id = 0; static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG; static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map; std::map< int, Pos_struct> users_map;
@ -214,10 +214,10 @@ namespace buzz_utility{
/* Process messages VM call*/ /* Process messages VM call*/
buzzvm_process_inmsgs(VM); buzzvm_process_inmsgs(VM);
} }
/***************************************************/ /***************************************************/
/*Obtains messages from buzz out message Queue*/ /*Obtains messages from buzz out message Queue*/
/***************************************************/ /***************************************************/
uint64_t* obt_out_msg(){ uint64_t* obt_out_msg(){
/* Process out messages */ /* Process out messages */
buzzvm_process_outmsgs(VM); buzzvm_process_outmsgs(VM);
@ -234,10 +234,9 @@ namespace buzz_utility{
if(buzzoutmsg_queue_isempty(VM)) break; if(buzzoutmsg_queue_isempty(VM)) break;
/* Get first message */ /* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than 100 Bytes */ /* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t) //ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
> if((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MSG_SIZE) {
MSG_SIZE) {
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
break; break;
} }
@ -305,6 +304,9 @@ namespace buzz_utility{
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
@ -344,6 +346,9 @@ namespace buzz_utility{
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
buzzvm_gstore(VM); buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
@ -446,7 +451,7 @@ int create_stig_tables() {
/****************************************/ /****************************************/
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) { const char* bdbg_filename, int robot_id) {
ROS_INFO(" Robot ID: " , robot_id); ROS_INFO(" Robot ID: %i" , robot_id);
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
Robot_id = robot_id; Robot_id = robot_id;
@ -493,7 +498,7 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id); ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0; return 0;
} }
/* Create vstig tables */ /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -501,7 +506,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl; //cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0; return 0;
} }*/
/* Save bytecode file name */ /* Save bytecode file name */
BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
@ -555,7 +560,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0; return 0;
} }
/* Create vstig tables */ /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -563,7 +568,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl; //cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0; return 0;
} }*/
// Execute the global part of the script // Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
@ -611,7 +616,7 @@ int create_stig_tables() {
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME); fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
return 0; return 0;
} }
/* Create vstig tables */ /* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) { if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -619,7 +624,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl; //cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl; //cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0; return 0;
} }*/
// Execute the global part of the script // Execute the global part of the script
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){ if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
ROS_ERROR("Error executing global part, VM state : %i",VM->state); ROS_ERROR("Error executing global part, VM state : %i",VM->state);
@ -687,7 +692,7 @@ int create_stig_tables() {
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM); buzzuav_closures::update_neighbors(VM);
update_users(); //update_users();
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
} }
@ -698,7 +703,7 @@ int create_stig_tables() {
update_sensors(); update_sensors();
/* Call Buzz step() function */ /* Call Buzz step() function */
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) { if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", ROS_ERROR("%s: execution terminated abnormally: %s",
BO_FNAME, BO_FNAME,
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
@ -722,7 +727,7 @@ int create_stig_tables() {
void buzz_script_destroy() { void buzz_script_destroy() {
if(VM) { if(VM) {
if(VM->state != BUZZVM_STATE_READY) { if(VM->state != BUZZVM_STATE_READY) {
fprintf(stderr, "%s: execution terminated abnormally: %s\n\n", ROS_ERROR("%s: execution terminated abnormally: %s",
BO_FNAME, BO_FNAME,
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
@ -732,7 +737,7 @@ int create_stig_tables() {
free(BO_FNAME); free(BO_FNAME);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
} }
fprintf(stdout, "Script execution stopped.\n"); ROS_INFO("Script execution stopped.");
} }
@ -754,7 +759,7 @@ int create_stig_tables() {
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM); buzzuav_closures::update_neighbors(VM);
update_users(); //update_users();
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
//set_robot_var(buzzdict_size(VM->swarmmembers)+1); //set_robot_var(buzzdict_size(VM->swarmmembers)+1);

3
src/roscontroller.cpp
View File

@ -86,12 +86,13 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo"; std::string standby_bo = Compile_bzz(stand_by) + ".bo";
init_update_monitor(bcfname.c_str(),standby_bo.c_str()); init_update_monitor(bcfname.c_str(),standby_bo.c_str());
/////////////////////////////////////////////////////// ///////////////////////////////////////////////////////
// MAIN LOOP // MAIN LOOP
////////////////////////////////////////////////////// //////////////////////////////////////////////////////
ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/