Merge commit '32c10dbc42497d12f7b3dc57a2aab0f601d280b5' into ros_drones_ws
This commit is contained in:
David St-Onge
commit
f22d54b016
|
|
@ -38,7 +38,7 @@ function action() {
|
|||
|
||||
# Move according to vector
|
||||
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, 0.0)
|
||||
UAVSTATE = "LENNARDJONES"
|
||||
|
||||
# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS
|
||||
|
|
@ -47,11 +47,11 @@ function action() {
|
|||
# } else if(timeW>=WAIT_TIMEOUT/2) {
|
||||
# UAVSTATE ="GOEAST"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(0.0,5.0)
|
||||
# uav_moveto(0.0, 5.0, 0.0)
|
||||
# } else {
|
||||
# UAVSTATE ="GONORTH"
|
||||
# timeW = timeW+1
|
||||
# uav_moveto(5.0,0.0)
|
||||
# uav_moveto(5.0, 0.0, 0.0)
|
||||
# }
|
||||
}
|
||||
|
||||
|
|
@ -65,15 +65,19 @@ function action() {
|
|||
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()
|
||||
uav_updatestig()
|
||||
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ",ROBOTS)
|
||||
if(id==0)
|
||||
|
|
|
|||
|
|
@ -10,9 +10,10 @@ include "vstigenv.bzz"
|
|||
|
||||
include "graphs/shapes_Y.bzz"
|
||||
|
||||
ROBOT_RADIUS=50
|
||||
ROBOT_DIAMETER=2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
|
||||
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 = 250.0
|
||||
|
|
@ -446,7 +447,7 @@ function TransitionToJoined(){
|
|||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
|
||||
#
|
||||
|
|
@ -471,7 +472,7 @@ while(i<m_neighbourCount){
|
|||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
}
|
||||
|
||||
#
|
||||
|
|
@ -535,7 +536,7 @@ function DoFree() {
|
|||
# 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)
|
||||
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}
|
||||
|
|
@ -545,7 +546,7 @@ function DoFree() {
|
|||
i=i+1
|
||||
}
|
||||
m_navigation=math.vec2.scale(tempvec,1.0/i)
|
||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -554,7 +555,7 @@ function DoFree() {
|
|||
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)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
#set message
|
||||
m_selfMessage.State=s2i(m_eState)
|
||||
|
|
@ -614,7 +615,7 @@ function DoAsking(){
|
|||
}
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
}
|
||||
|
||||
#
|
||||
|
|
@ -662,7 +663,7 @@ function DoJoining(){
|
|||
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)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
|
||||
|
||||
|
||||
|
|
@ -760,7 +761,7 @@ function DoJoined(){
|
|||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
|
||||
|
||||
#check if should to transists to lock
|
||||
|
|
@ -789,7 +790,7 @@ if(m_nLabel!=0){
|
|||
m_navigation=motion_vector()
|
||||
}
|
||||
#move
|
||||
uav_moveto(m_navigation.x,m_navigation.y)
|
||||
uav_moveto(m_navigation.x, m_navigation.y, 0.0)
|
||||
}
|
||||
|
||||
function action(){
|
||||
|
|
@ -802,7 +803,7 @@ function action(){
|
|||
#
|
||||
function init() {
|
||||
#
|
||||
#Adjust parameters here
|
||||
# Global parameters for graph formation
|
||||
#
|
||||
m_unResponseTimeThreshold=10
|
||||
m_unLabelSearchWaitTime=10
|
||||
|
|
@ -816,23 +817,32 @@ function init() {
|
|||
uav_initswarm()
|
||||
v_tag = stigmergy.create(m_lockstig)
|
||||
uav_initstig()
|
||||
Reset()
|
||||
# go to diff. height since no collision avoidance implemented yet
|
||||
TARGET_ALTITUDE = 2.5 + id * 1.5
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
|
||||
# reset the graph
|
||||
Reset()
|
||||
}
|
||||
|
||||
#
|
||||
# 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")}
|
||||
#
|
||||
#act according to current state
|
||||
# graph state machine
|
||||
#
|
||||
if(UAVSTATE=="GRAPH"){
|
||||
if(m_eState=="STATE_FREE")
|
||||
|
|
@ -849,17 +859,16 @@ function step(){
|
|||
DoLock()
|
||||
}
|
||||
|
||||
# high level UAV state machine
|
||||
statef()
|
||||
|
||||
|
||||
debug(m_eState,m_nLabel)
|
||||
log("Current state: ", UAVSTATE)
|
||||
log("Swarm size: ", ROBOTS)
|
||||
# if(id==0)
|
||||
# stattab_print()
|
||||
|
||||
#navigation
|
||||
#broadcast messag
|
||||
#broadcast message
|
||||
neighbors.broadcast("m",packmessage(m_selfMessage))
|
||||
|
||||
#
|
||||
|
|
@ -895,7 +904,7 @@ function Reset(){
|
|||
#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){
|
||||
if(id==ROOT_ID){
|
||||
m_nLabel=0
|
||||
TransitionToJoined()
|
||||
}
|
||||
|
|
@ -912,7 +921,7 @@ 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)
|
||||
uav_moveto(m_navigation.x/100.0, m_navigation.y/100.0, 0.0)
|
||||
m_vecNodes={}
|
||||
#stop listening
|
||||
neighbors.ignore("m")
|
||||
|
|
|
|||
|
|
@ -1,826 +0,0 @@
|
|||
#
|
||||
# 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.
|
||||
|
||||
ROBOT_RADIUS=50
|
||||
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 = 13.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 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 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_bearing=Angle(S2Target)
|
||||
m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
|
||||
#S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
|
||||
m_navigation=math.vec2.newp(math.vec2.length(S2Target),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,m_navigation.y)
|
||||
}
|
||||
|
||||
function action(){
|
||||
statef=action
|
||||
UAVSTATE="GRAPH"
|
||||
}
|
||||
|
||||
#
|
||||
# Executed at init
|
||||
#
|
||||
function init() {
|
||||
#
|
||||
#Adjust parameters here
|
||||
#
|
||||
m_unResponseTimeThreshold=10
|
||||
m_unLabelSearchWaitTime=10
|
||||
m_fTargetDistanceTolerance=10
|
||||
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=="GRAPH"){
|
||||
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/buzz_scripts/include/Graph_drone.graph")#change the .graph file when necessary
|
||||
m_vecNodes_fixed={}
|
||||
m_vecNodes_fixed=parse_graph_fixed("/home/dave/ROS_WS/src/rosbuzz/buzz_scripts/include/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")
|
||||
}
|
||||
|
|
@ -12,9 +12,9 @@ BARRIER_VSTIG = 11
|
|||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_set(threshold, transf, resumef) {
|
||||
function barrier_set(threshold, transf, resumef, bdt) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf, resumef);
|
||||
barrier_wait(threshold, transf, resumef, bdt);
|
||||
}
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
|
@ -31,14 +31,16 @@ function barrier_ready() {
|
|||
#
|
||||
BARRIER_TIMEOUT = 200
|
||||
timeW=0
|
||||
function barrier_wait(threshold, transf, resumef) {
|
||||
barrier.get(id)
|
||||
function barrier_wait(threshold, transf, resumef, bdt) {
|
||||
#barrier.get(id)
|
||||
barrier.put(id, 1)
|
||||
UAVSTATE = "BARRIERWAIT"
|
||||
if(bdt!=-1)
|
||||
neighbors.broadcast("cmd", brd)
|
||||
if(barrier.size() >= threshold) {
|
||||
# getlowest()
|
||||
transf()
|
||||
} else if(timeW>=BARRIER_TIMEOUT) {
|
||||
} else if(timeW >= BARRIER_TIMEOUT) {
|
||||
barrier = nil
|
||||
resumef()
|
||||
timeW=0
|
||||
|
|
|
|||
|
|
@ -5,12 +5,12 @@
|
|||
########################################
|
||||
TARGET_ALTITUDE = 5.0
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
GOTO_MAXVEL = 2
|
||||
goal = {.range=0, .bearing=0}
|
||||
|
||||
function uav_initswarm(){
|
||||
function uav_initswarm() {
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
}
|
||||
|
||||
function turnedoff() {
|
||||
|
|
@ -26,13 +26,10 @@ function 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_set(ROBOTS,action,land,22)
|
||||
barrier_ready()
|
||||
#statef=hexagon
|
||||
}
|
||||
else {
|
||||
log("Altitude: ", TARGET_ALTITUDE)
|
||||
|
|
@ -44,13 +41,13 @@ function takeoff() {
|
|||
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,turnedoff,land)
|
||||
barrier_set(ROBOTS,turnedoff,land,21)
|
||||
barrier_ready()
|
||||
timeW=0
|
||||
#barrier = nil
|
||||
|
|
@ -58,6 +55,28 @@ function land() {
|
|||
}
|
||||
}
|
||||
|
||||
function goto() {
|
||||
UAVSTATE = "GOTO"
|
||||
statef=goto
|
||||
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
|
||||
if(rc_goto.id==id){
|
||||
s.leave()
|
||||
rb_from_gps(rc_goto.latitude, rc_goto.longitude)
|
||||
print(id, " has to move ", goal.range)
|
||||
m_navigation = math.vec2.newp(goal.range, goal.bearing)
|
||||
if(math.vec2.length(m_navigation)>GOTO_MAXVEL) {
|
||||
m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
|
||||
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
|
||||
} else if(math.vec2.length(m_navigation)>0.25)
|
||||
uav_moveto(m_navigation.x, m_navigation.y, rc_goto.altitude-position.altitude)
|
||||
else
|
||||
statef = idle
|
||||
} else {
|
||||
neighbors.broadcast("cmd", 16)
|
||||
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
|
||||
}
|
||||
}
|
||||
|
||||
function follow() {
|
||||
if(size(targets)>0) {
|
||||
UAVSTATE = "FOLLOW"
|
||||
|
|
@ -67,7 +86,7 @@ function follow() {
|
|||
force=(0.05)*(tab.range)^4
|
||||
attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
|
||||
})
|
||||
uav_moveto(attractor.x, attractor.y)
|
||||
uav_moveto(attractor.x, attractor.y, 0.0)
|
||||
} else {
|
||||
log("No target in local table!")
|
||||
#statef=idle
|
||||
|
|
@ -90,11 +109,8 @@ function uav_rccmd() {
|
|||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
UAVSTATE = "FOLLOW"
|
||||
log(rc_goto.latitude, " ", rc_goto.longitude)
|
||||
add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
|
||||
statef = follow
|
||||
#uav_goto()
|
||||
UAVSTATE = "GOTO"
|
||||
statef = goto
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
|
|
@ -121,6 +137,30 @@ function uav_neicmd() {
|
|||
uav_arm()
|
||||
} else if(value==401 and UAVSTATE=="IDLE"){
|
||||
uav_disarm()
|
||||
} else if(value==16){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
# # if(gt.id == id) statef=goto
|
||||
# })
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
function rb_from_gps(lat, lon) {
|
||||
print("rb dbg: ",lat,lon,position.latitude,position.longitude)
|
||||
d_lon = lon - position.longitude
|
||||
d_lat = lat - position.latitude
|
||||
ned_x = d_lat/180*math.pi * 6371000.0;
|
||||
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
|
||||
goal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
|
||||
goal.bearing = LimitAngle(math.atan(ned_y,ned_x));
|
||||
}
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
# Utilities
|
||||
|
||||
# Rads to degrees
|
||||
function rtod(r) {
|
||||
return (r*(180.0/math.pi))
|
||||
}
|
||||
|
||||
# Copy a table
|
||||
function table_deep_copy(new_t, old_t, depth) {
|
||||
depth = depth - 1
|
||||
if (old_t != nil) {
|
||||
foreach(old_t, function(key, value) {
|
||||
new_t[key] = value
|
||||
if(depth != 0) {
|
||||
new_t[key] = {}
|
||||
table_deep_copy(new_t[key], value, depth)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
function table_copy(old_t, depth) {
|
||||
var t = {};
|
||||
table_deep_copy(t, old_t, depth);
|
||||
return t;
|
||||
}
|
||||
|
||||
# Print the contents of a table
|
||||
function table_print(t, depth) {
|
||||
depth = depth - 1
|
||||
if (t != nil) {
|
||||
foreach(t, function(key, value) {
|
||||
log(key, " -> ", value)
|
||||
if(depth != 0) {
|
||||
table_print(t[key], depth)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
# Write a table as if it was a vector
|
||||
function write_vector(k, index, val) {
|
||||
var key = string.tostring(index)
|
||||
k[key] = val
|
||||
}
|
||||
|
||||
# Read a table as if it was a vector
|
||||
function read_vector(k, index) {
|
||||
var key = string.tostring(index)
|
||||
if (k[key] == nil) {
|
||||
return -1
|
||||
} else {
|
||||
return k[key]
|
||||
}
|
||||
}
|
||||
|
||||
# Write a table as if it was a matrix
|
||||
function write_matrix(k, row, col, val) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
k[key] = val
|
||||
}
|
||||
|
||||
# Read a table as if it was a matrix
|
||||
function read_matrix(k, row, col) {
|
||||
var key = string.concat(string.tostring(row),"-",string.tostring(col))
|
||||
if (k[key] == nil) {
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
# 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
|
||||
}
|
||||
|
||||
############################################
|
||||
|
||||
#base = {}
|
||||
|
||||
#base.create = function() {
|
||||
# return {
|
||||
# .method = function(a,b) {
|
||||
# return a + b
|
||||
# }
|
||||
# }
|
||||
# }
|
||||
|
||||
#x = base.create()
|
||||
#x.method(3,4) # returns 7
|
||||
|
||||
#derived = {}
|
||||
|
||||
#derived.create = function() {
|
||||
# b = base.create()
|
||||
# b.method = function(a,b) {
|
||||
# return a * b
|
||||
# }
|
||||
#}
|
||||
|
||||
#x = derived.create()
|
||||
#x.method(3,4) # returns 12
|
||||
|
|
@ -7,17 +7,23 @@ include "vstigenv.bzz"
|
|||
function action() {
|
||||
statef=action
|
||||
# test moveto cmd dx, dy
|
||||
# uav_moveto(0.5, 0.5)
|
||||
# uav_moveto(0.5, 0.5, 0.0)
|
||||
}
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
statef=turnedoff
|
||||
UAVSTATE = "TURNEDOFF"
|
||||
uav_initstig()
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
log("Altitude: ", position.altitude)
|
||||
uav_rccmd()
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", UAVSTATE)
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,537 @@
|
|||
# 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
|
||||
}
|
||||
|
||||
################
|
||||
|
|
@ -36,13 +36,15 @@ int buzzros_print(buzzvm_t vm);
|
|||
* commands the UAV to go to a position supplied
|
||||
*/
|
||||
int buzzuav_moveto(buzzvm_t vm);
|
||||
int buzzuav_goto(buzzvm_t vm);
|
||||
int buzzuav_storegoal(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(double pos[]);
|
||||
void rc_set_goto(int id, double latitude, double longitude, double altitude);
|
||||
/*Sets gimbal orientation from rc client*/
|
||||
void rc_set_gimbal(int id, float yaw, float pitch);
|
||||
/*sets rc requested command */
|
||||
void rc_call(int rc_cmd);
|
||||
/* sets the battery state */
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@
|
|||
#define XBEE_STOP_TRANSMISSION 4355356352
|
||||
#define TIMEOUT 60
|
||||
#define BUZZRATE 10
|
||||
#define CMD_REQUEST_UPDATE 666
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -110,6 +111,7 @@ private:
|
|||
ros::Subscriber users_sub;
|
||||
ros::Subscriber battery_sub;
|
||||
ros::Subscriber payload_sub;
|
||||
ros::Subscriber flight_estatus_sub;
|
||||
ros::Subscriber flight_status_sub;
|
||||
ros::Subscriber obstacle_sub;
|
||||
ros::Subscriber Robot_id_sub;
|
||||
|
|
|
|||
|
|
@ -1,59 +0,0 @@
|
|||
<launch>
|
||||
<group ns="gps">
|
||||
<!-- NavSat Serial -->
|
||||
<node pkg="nmea_comms" type="serial_node" name="nmea_serial_node" output="screen">
|
||||
<param name="port" value="$(optenv HUSKY_NAVSAT_PORT /dev/clearpath/gps)" />
|
||||
<param name="baud" value="$(optenv HUSKY_NAVSAT_BAUD 19200)" />
|
||||
</node>
|
||||
|
||||
<!-- NavSat Processing -->
|
||||
<node pkg="nmea_navsat_driver" type="nmea_topic_driver" name="nmea_topic_driver">
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" >
|
||||
<rosparam>
|
||||
magnetic_declination_radians: 0
|
||||
roll_offset: 0
|
||||
pitch_offset: 0
|
||||
yaw_offset: 0
|
||||
zero_altitude: false
|
||||
broadcast_utm_transform: false
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- run xbee>
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
|
||||
|
||||
<!-- xmee_mav Drone type and Commununication Mode -->
|
||||
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
|
||||
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
|
||||
<!-- xmee_mav Topics and Services Names -->
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
|
||||
|
||||
|
||||
<node pkg="dji_sdk_mistlab" type="dji_sdk_mav" name="dji_sdk_mav" output="screen"/>
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/husky.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
<param name="out_payload" value="outMavlink"/>
|
||||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="xbee_plugged" value="true"/>
|
||||
<param name="name" value="husky1"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
topics:
|
||||
gps : global_position
|
||||
localpos : local_position
|
||||
battery : power_status
|
||||
status : flight_status
|
||||
fcclient : dji_mavcmd
|
||||
setpoint : setpoint_position/local
|
||||
armclient: dji_mavarm
|
||||
modeclient: dji_mavmode
|
||||
altitude: rel_alt
|
||||
stream: set_stream_rate
|
||||
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
battery : mavros_msgs/BatteryStatus
|
||||
status : mavros_msgs/ExtendedState
|
||||
altitude: std_msgs/Float64
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
topics:
|
||||
gps : global_position
|
||||
localpos : local_position
|
||||
battery : power_status
|
||||
status : flight_status
|
||||
fcclient : dji_mavcmd
|
||||
setpoint : setpoint_position/local
|
||||
armclient: dji_mavarm
|
||||
modeclient: dji_mavmode
|
||||
altitude: rel_alt
|
||||
stream: set_stream_rate
|
||||
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
battery : mavros_msgs/BatteryStatus
|
||||
status : mavros_msgs/ExtendedState
|
||||
altitude: std_msgs/Float64
|
||||
|
|
@ -1,21 +0,0 @@
|
|||
topics:
|
||||
gps : mavros/global_position/global
|
||||
battery : mavros/battery
|
||||
status : mavros/state
|
||||
fcclient: mavros/cmd/command
|
||||
setpoint: mavros/setpoint_position/local
|
||||
armclient: mavros/cmd/arming
|
||||
modeclient: mavros/set_mode
|
||||
localpos: /mavros/local_position/pose
|
||||
stream: mavros/set_stream_rate
|
||||
altitude: mavros/global_position/rel_alt
|
||||
type:
|
||||
gps : sensor_msgs/NavSatFix
|
||||
# for SITL Solo
|
||||
battery : mavros_msgs/BatteryState
|
||||
# for solo
|
||||
#battery : mavros_msgs/BatteryStatus
|
||||
status : mavros_msgs/State
|
||||
altitude: std_msgs/Float64
|
||||
environment :
|
||||
environment : solo-simulator
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
topics:
|
||||
gps : global_position/global
|
||||
battery : battery
|
||||
status : state
|
||||
estatus : extended_state
|
||||
fcclient: cmd/command
|
||||
setpoint: setpoint_position/local
|
||||
armclient: cmd/arming
|
||||
modeclient: set_mode
|
||||
localpos: local_position/pose
|
||||
stream: set_stream_rate
|
||||
altitude: global_position/rel_alt
|
||||
|
|
@ -1,57 +0,0 @@
|
|||
<launch>
|
||||
<!-- RUN mavros -->
|
||||
<arg name="fcu_url" default="/dev/ttyS0:115200" />
|
||||
<arg name="gcs_url" default="" />
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="1" />
|
||||
<arg name="log_output" default="screen" />
|
||||
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
|
||||
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||
<arg name="log_output" value="$(arg log_output)" />
|
||||
</include>
|
||||
|
||||
<!-- set streaming rate -->
|
||||
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
|
||||
|
||||
<!-- run xbee>
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" / -->
|
||||
|
||||
<!-- xmee_mav Drone type and Commununication Mode -->
|
||||
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="master solo" output="screen" -->
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<!-- node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave solo" output="screen" -->
|
||||
<!--node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" -->
|
||||
<!-- xmee_mav Topics and Services Names -->
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
|
||||
<param name="USB_port" type="str" value="/dev/ttyUSB0" />
|
||||
<param name="Baud_rate" type="double" value="230400" />
|
||||
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/flock.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
<param name="out_payload" value="outMavlink"/>
|
||||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="xbee_plugged" value="true"/>
|
||||
<param name="name" value="solos1"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,51 +0,0 @@
|
|||
<launch>
|
||||
<!-- RUN mavros
|
||||
<arg name="fcu_url" default="tcp://127.0.0.1:5760" />
|
||||
<arg name="gcs_url" default="" />
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="1" />
|
||||
<arg name="log_output" default="screen" />
|
||||
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||
<arg name="log_output" value="$(arg log_output)" />
|
||||
</include>
|
||||
-->
|
||||
<!-- run xbee -->
|
||||
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/script/testsolo.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
<param name="out_payload" value="outMavlink"/>
|
||||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="xbee_plugged" value="false"/>
|
||||
<param name="name" value="solos1"/>
|
||||
<param name="stand_by" value="/home/ivan/catkin_ws/src/rosbuzz/script/stand_by.bzz"/>
|
||||
</node>
|
||||
|
||||
<!--
|
||||
<node pkg="rosloader" type="rosloader" name="rosloader" output="screen"/>
|
||||
-->
|
||||
|
||||
<!-- set streaming rate
|
||||
/mavros/cmd/arming
|
||||
<node pkg="rosservice" type="rosservice" name="stream_rate" args="call /mavros/set_stream_rate 0 10 1" />
|
||||
-->
|
||||
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
<launch>
|
||||
<!-- RUN mavros -->
|
||||
<arg name="fcu_url" default="/dev/ttyAMA0:115200" />
|
||||
<arg name="gcs_url" default="" />
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="1" />
|
||||
<arg name="log_output" default="screen" />
|
||||
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />
|
||||
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
<arg name="tgt_component" value="$(arg tgt_component)" />
|
||||
<arg name="log_output" value="$(arg log_output)" />
|
||||
</include>
|
||||
|
||||
<!-- set streaming rate -->
|
||||
<!-- node pkg="rosservice" type="rosservice" name="freq" args="call /mavros/set_stream_rate 0 10 1" output="screen" / -->
|
||||
|
||||
<!-- run xbee -->
|
||||
<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
|
||||
<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
|
||||
<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
|
||||
<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
|
||||
<param name="No_of_dev" type="int" value="3" />
|
||||
|
||||
<!-- run rosbuzz -->
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="/home/pi/ros_catkin_ws/src/ROSBuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/testflockfev.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="3"/>
|
||||
<param name="No_of_Robots" value="3"/>
|
||||
<param name="stand_by" value="/home/pi/ros_catkin_ws/src/ROSBuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -1,14 +1,22 @@
|
|||
<!-- Launch file for ROSBuzz -->
|
||||
|
||||
<?xml version="1.0"?>
|
||||
<!-- Generic launch file for ROSBuzz -->
|
||||
<!-- This file is included in all ROS workspace launch files -->
|
||||
<!-- Modify with great care! -->
|
||||
<launch>
|
||||
<arg name="name" default="robot0"/>
|
||||
<arg name="xbee_plugged" default="true"/>
|
||||
<arg name="script" default="testalone"/>
|
||||
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>
|
||||
<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.bzz" />
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="/buzzcmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="xbee_status_srv" value="/xbee_status"/>
|
||||
<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
<param name="out_payload" value="outMavlink"/>
|
||||
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
|
||||
<param name="name" value="$(arg name)"/>
|
||||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,27 +0,0 @@
|
|||
<!-- Launch file for ROSBuzz -->
|
||||
|
||||
<launch>
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_2parallel.bzz" />
|
||||
<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="rc_cmd" />
|
||||
<param name="fcclient_name" value="/djicmd" />
|
||||
<param name="in_payload" value="/inMavlink"/>
|
||||
<!-- param name="in_payload" value="/rosbuzz_node1/outMavlink"-->
|
||||
<param name="out_payload" value="/outMavlink"/>
|
||||
<param name="robot_id" value="1"/>
|
||||
<param name="No_of_Robots" value="1"/>
|
||||
<param name="stand_by" value="/home/vivek/catkin_ws/src/rosbuzz/src/stand_by.bo"/>
|
||||
</node>
|
||||
<!-- node name="rosbuzz_node1" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" -->
|
||||
<!-- param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" -->
|
||||
<!-- param name="rcclient" value="true" -->
|
||||
<!-- param name="rcservice_name" value="rc_cmd" -->
|
||||
<!-- param name="fcclient_name" value="/djicmd" -->
|
||||
<!-- param name="in_payload" value="/rosbuzz_node/outMavlink"-->
|
||||
<!-- param name="out_payload" value="outMavlink"-->
|
||||
<!-- param name="robot_id" value="2"-->
|
||||
<!-- node-->
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
<?xml version="1.0"?>
|
||||
<!-- Generic launch file for ROSBuzz -->
|
||||
<!-- This file is included in all ROS workspace launch files -->
|
||||
<!-- Modify with great care! -->
|
||||
<launch>
|
||||
<arg name="name" default="robot0"/>
|
||||
<arg name="xbee_plugged" default="true"/>
|
||||
<arg name="script" default="testalone"/>
|
||||
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
<param name="out_payload" value="outMavlink"/>
|
||||
<param name="xbee_plugged" value="$(arg xbee_plugged)"/>
|
||||
<param name="name" value="$(arg name)"/>
|
||||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
<!-- Launch file for ROSBuzz -->
|
||||
|
||||
<launch>
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen">
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/m100.yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/script/testflockfev.bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
<param name="in_payload" value="inMavlink"/>
|
||||
<param name="out_payload" value="outMavlink"/>
|
||||
<param name="xbee_plugged" value="true"/>
|
||||
<param name="name" value="m1001"/>
|
||||
<param name="xbee_status_srv" value="xbee_status"/>
|
||||
<param name="stand_by" value="$(find rosbuzz)/script/stand_by.bzz"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
|
@ -17,7 +17,7 @@ namespace buzz_utility{
|
|||
static char* BO_FNAME = 0;
|
||||
static uint8_t* BO_BUF = 0;
|
||||
static buzzdebug_t DBG_INFO = 0;
|
||||
static uint32_t MSG_SIZE = 500; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
|
||||
static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
|
||||
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
||||
static uint8_t Robot_id = 0;
|
||||
static std::vector<uint8_t*> IN_MSG;
|
||||
|
|
@ -304,8 +304,8 @@ void in_message_process(){
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
|
||||
|
|
@ -349,7 +349,7 @@ void in_message_process(){
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
||||
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
|
||||
|
|
@ -487,7 +487,7 @@ int create_stig_tables() {
|
|||
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
ROS_ERROR("%s: Error loading Buzz script", bo_filename);
|
||||
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
|
||||
return 0;
|
||||
}
|
||||
/* Register hook functions */
|
||||
|
|
@ -550,14 +550,14 @@ int create_stig_tables() {
|
|||
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
|
||||
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
// Register hook functions
|
||||
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
|
||||
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
/* Create vstig tables
|
||||
|
|
@ -606,14 +606,14 @@ int create_stig_tables() {
|
|||
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
|
||||
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
// Register hook functions
|
||||
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
fprintf(stdout, "%s: Error registering hooks\n\n", BO_FNAME);
|
||||
ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
/* Create vstig tables
|
||||
|
|
|
|||
|
|
@ -16,12 +16,14 @@ namespace buzzuav_closures{
|
|||
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
|
||||
static double goto_pos[3];
|
||||
static double rc_goto_pos[3];
|
||||
static float rc_gimbal[2];
|
||||
static float batt[3];
|
||||
static float obst[5]={0,0,0,0,0};
|
||||
static double cur_pos[3];
|
||||
static uint8_t status;
|
||||
static int cur_cmd = 0;
|
||||
static int rc_cmd=0;
|
||||
static int rc_id=-1;
|
||||
static int buzz_cmd=0;
|
||||
static float height=0;
|
||||
static bool deque_full = false;
|
||||
|
|
@ -121,19 +123,19 @@ namespace buzzuav_closures{
|
|||
/ Buzz closure to move following a 2D vector
|
||||
/----------------------------------------*/
|
||||
int buzzuav_moveto(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 2);
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); /* dx */
|
||||
buzzvm_lload(vm, 2); /* dy */
|
||||
//buzzvm_lload(vm, 3); /* Latitude */
|
||||
//buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_lload(vm, 3); /* dheight */
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
float dy = buzzvm_stack_at(vm, 1)->f.value;
|
||||
float dx = buzzvm_stack_at(vm, 2)->f.value;
|
||||
double d = sqrt(dx*dx+dy*dy); //range
|
||||
float dh = buzzvm_stack_at(vm, 1)->f.value;
|
||||
float dy = buzzvm_stack_at(vm, 2)->f.value;
|
||||
float dx = buzzvm_stack_at(vm, 3)->f.value;
|
||||
goto_pos[0]=dx;
|
||||
goto_pos[1]=dy;
|
||||
goto_pos[2]=height;
|
||||
goto_pos[2]=height+dh;
|
||||
/*double b = atan2(dy,dx); //bearing
|
||||
printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
|
||||
gps_from_rb(d, b, goto_pos);
|
||||
|
|
@ -263,14 +265,17 @@ namespace buzzuav_closures{
|
|||
return payload_out;
|
||||
}
|
||||
/*----------------------------------------/
|
||||
/ Buzz closure to go directly to a GPS destination from the Mission Planner
|
||||
/ Buzz closure to store locally a GPS destination from the fleet
|
||||
/----------------------------------------*/
|
||||
int buzzuav_goto(buzzvm_t vm) {
|
||||
rc_goto_pos[2]=height;
|
||||
set_goto(rc_goto_pos);
|
||||
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
|
||||
buzz_cmd=COMMAND_GOTO;
|
||||
int buzzuav_storegoal(buzzvm_t vm) {
|
||||
buzzvm_lnum_assert(vm, 3);
|
||||
buzzvm_lload(vm, 1); // latitude
|
||||
buzzvm_lload(vm, 2); // longitude
|
||||
buzzvm_lload(vm, 3); // altitude
|
||||
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
|
||||
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
|
||||
rc_set_goto(buzzvm_stack_at(vm, 1)->f.value, buzzvm_stack_at(vm, 2)->f.value, buzzvm_stack_at(vm, 3)->f.value, (int)buzz_utility::get_robotid());
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
||||
|
|
@ -344,12 +349,22 @@ namespace buzzuav_closures{
|
|||
return cmd;
|
||||
}
|
||||
|
||||
void rc_set_goto(double pos[]) {
|
||||
rc_goto_pos[0] = pos[0];
|
||||
rc_goto_pos[1] = pos[1];
|
||||
rc_goto_pos[2] = pos[2];
|
||||
void rc_set_goto(int id, double latitude, double longitude, double altitude) {
|
||||
rc_id = id;
|
||||
rc_goto_pos[0] = latitude;
|
||||
rc_goto_pos[1] = longitude;
|
||||
rc_goto_pos[2] = altitude;
|
||||
|
||||
}
|
||||
|
||||
void rc_set_gimbal(int id, float yaw, float pitch) {
|
||||
|
||||
rc_id = id;
|
||||
rc_gimbal[0] = yaw;
|
||||
rc_gimbal[1] = pitch;
|
||||
|
||||
}
|
||||
|
||||
void rc_call(int rc_cmd_in) {
|
||||
rc_cmd = rc_cmd_in;
|
||||
}
|
||||
|
|
@ -517,6 +532,10 @@ namespace buzzuav_closures{
|
|||
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
|
||||
buzzvm_pusht(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "id", 1));
|
||||
buzzvm_pushi(vm, rc_id);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_dup(vm);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
|
||||
buzzvm_pushf(vm, rc_goto_pos[0]);
|
||||
buzzvm_tput(vm);
|
||||
|
|
|
|||
|
|
@ -266,7 +266,8 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
|||
/*Obtain .bzz file name from parameter server*/
|
||||
if (n_c.getParam("bzzfile_name", bzzfile_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a .bzz file to run in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
|
|
@ -284,13 +285,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
|||
{
|
||||
ROS_INFO("RC service is disabled");
|
||||
}
|
||||
} else {
|
||||
} else
|
||||
{
|
||||
ROS_ERROR("Provide a rc client option: yes or no in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
/*Obtain robot_id from parameter server*/
|
||||
// n_c.getParam("robot_id", robot_id);
|
||||
// robot_id=(int)buzz_utility::get_robotid();
|
||||
/*Obtain out payload name*/
|
||||
n_c.getParam("out_payload", out_payload);
|
||||
/*Obtain in payload name*/
|
||||
|
|
@ -300,22 +299,22 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
|
|||
|
||||
if (n_c.getParam("xbee_plugged", xbeeplugged))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (!xbeeplugged) {
|
||||
if (n_c.getParam("name", robot_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide the xbee plugged boolean in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
} else
|
||||
n_c.getParam("xbee_status_srv", xbeesrv_name);
|
||||
|
||||
std::cout << "////////////////// " << xbeesrv_name;
|
||||
|
||||
GetSubscriptionParameters(n_c);
|
||||
// initialize topics to null?
|
||||
}
|
||||
|
|
@ -325,70 +324,71 @@ used
|
|||
/-----------------------------------------------------------------------------------*/
|
||||
void roscontroller::GetSubscriptionParameters(ros::NodeHandle &node_handle)
|
||||
{
|
||||
// todo: make it as an array in yaml?
|
||||
m_sMySubscriptions.clear();
|
||||
std::string gps_topic, gps_type;
|
||||
if (node_handle.getParam("topics/gps", gps_topic))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a gps topic in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
node_handle.getParam("type/gps", gps_type);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, gps_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(gps_topic, "sensor_msgs/NavSatFix"));
|
||||
|
||||
std::string battery_topic, battery_type;
|
||||
std::string battery_topic;
|
||||
node_handle.getParam("topics/battery", battery_topic);
|
||||
node_handle.getParam("type/battery", battery_type);
|
||||
m_smTopic_infos.insert(
|
||||
pair<std::string, std::string>(battery_topic, battery_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(battery_topic, "mavros_msgs/BatteryStatus"));
|
||||
|
||||
std::string status_topic, status_type;
|
||||
std::string status_topic;
|
||||
node_handle.getParam("topics/status", status_topic);
|
||||
node_handle.getParam("type/status", status_type);
|
||||
m_smTopic_infos.insert(
|
||||
pair<std::string, std::string>(status_topic, status_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/State"));
|
||||
node_handle.getParam("topics/estatus", status_topic);
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(status_topic, "mavros_msgs/ExtendedState"));
|
||||
|
||||
std::string altitude_topic, altitude_type;
|
||||
std::string altitude_topic;
|
||||
node_handle.getParam("topics/altitude", altitude_topic);
|
||||
node_handle.getParam("type/altitude", altitude_type);
|
||||
m_smTopic_infos.insert(
|
||||
pair<std::string, std::string>(altitude_topic, altitude_type));
|
||||
m_smTopic_infos.insert(pair<std::string, std::string>(altitude_topic, "std_msgs/Float64"));
|
||||
|
||||
/*Obtain fc client name from parameter server*/
|
||||
// Obtain required topic and service names from the parameter server
|
||||
if (node_handle.getParam("topics/fcclient", fcclient_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a fc client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/setpoint", setpoint_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a Set Point name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/armclient", armclient))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide an arm client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/modeclient", modeclient))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a mode client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/stream", stream_client_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a mode client name in Launch file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
if (node_handle.getParam("topics/localpos", local_pos_sub_name))
|
||||
;
|
||||
else {
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Provide a localpos name in YAML file");
|
||||
system("rosnode kill rosbuzz_node");
|
||||
}
|
||||
|
|
@ -405,33 +405,28 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
|
|||
|
||||
Subscribe(n_c);
|
||||
|
||||
payload_sub =
|
||||
n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
|
||||
payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
|
||||
|
||||
obstacle_sub = n_c.subscribe(obstacles_topic, 5,
|
||||
&roscontroller::obstacle_dist, this);
|
||||
obstacle_sub = n_c.subscribe(obstacles_topic, 5, &roscontroller::obstacle_dist, this);
|
||||
|
||||
/*publishers*/
|
||||
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
|
||||
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
|
||||
localsetpoint_nonraw_pub =
|
||||
n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
|
||||
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
|
||||
|
||||
/* Service Clients*/
|
||||
arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
|
||||
mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
|
||||
mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
|
||||
if (rcclient == true)
|
||||
service =
|
||||
n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
|
||||
service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
|
||||
ROS_INFO("Ready to receive Mav Commands from RC client");
|
||||
xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
|
||||
stream_client =
|
||||
n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
|
||||
|
||||
users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5,
|
||||
&roscontroller::local_pos_callback, this);
|
||||
local_pos_sub = n_c.subscribe(local_pos_sub_name, 5, &roscontroller::local_pos_callback, this);
|
||||
|
||||
multi_msg = true;
|
||||
}
|
||||
|
|
@ -444,20 +439,15 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
|
|||
m_smTopic_infos.begin();
|
||||
it != m_smTopic_infos.end(); ++it) {
|
||||
if (it->second == "mavros_msgs/ExtendedState") {
|
||||
flight_status_sub = n_c.subscribe(
|
||||
it->first, 100, &roscontroller::flight_extended_status_update, this);
|
||||
flight_estatus_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_extended_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/State") {
|
||||
flight_status_sub = n_c.subscribe(
|
||||
it->first, 100, &roscontroller::flight_status_update, this);
|
||||
flight_status_sub = n_c.subscribe(it->first, 5, &roscontroller::flight_status_update, this);
|
||||
} else if (it->second == "mavros_msgs/BatteryStatus") {
|
||||
battery_sub =
|
||||
n_c.subscribe(it->first, 5, &roscontroller::battery, this);
|
||||
battery_sub = n_c.subscribe(it->first, 5, &roscontroller::battery, this);
|
||||
} else if (it->second == "sensor_msgs/NavSatFix") {
|
||||
current_position_sub =
|
||||
n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
|
||||
current_position_sub = n_c.subscribe(it->first, 5, &roscontroller::current_pos, this);
|
||||
} else if (it->second == "std_msgs/Float64") {
|
||||
relative_altitude_sub =
|
||||
n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
|
||||
relative_altitude_sub = n_c.subscribe(it->first, 5, &roscontroller::current_rel_alt, this);
|
||||
}
|
||||
|
||||
std::cout << "Subscribed to: " << it->first << endl;
|
||||
|
|
@ -469,31 +459,17 @@ void roscontroller::Subscribe(ros::NodeHandle &n_c)
|
|||
/-------------------------------------------------------*/
|
||||
std::string roscontroller::Compile_bzz(std::string bzzfile_name)
|
||||
{
|
||||
/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
|
||||
/*Compile the buzz code .bzz to .bo*/
|
||||
stringstream bzzfile_in_compile;
|
||||
std::string path =
|
||||
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
// bzzfile_in_compile << path << "/";
|
||||
// path = bzzfile_in_compile.str();
|
||||
// bzzfile_in_compile.str("");
|
||||
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name = name.substr(0, name.find_last_of("."));
|
||||
bzzfile_in_compile << "bzzc -I " << path
|
||||
<< "include/"; //<<" "<<path<< name<<".basm";
|
||||
// bzzfile_in_compile.str("");
|
||||
// bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo
|
||||
// "<<path<<name<<".bdbg";
|
||||
// system(bzzfile_in_compile.str().c_str());
|
||||
// bzzfile_in_compile.str("");
|
||||
<< "include/";
|
||||
bzzfile_in_compile << " -b " << path << name << ".bo";
|
||||
// bcfname = bzzfile_in_compile.str();
|
||||
// std::string tmp_bcfname = path + name + ".bo";
|
||||
// bzzfile_in_compile.str("");
|
||||
bzzfile_in_compile << " -d " << path << name << ".bdb ";
|
||||
// bzzfile_in_compile << " -a " << path << name << ".asm ";
|
||||
bzzfile_in_compile << bzzfile_name;
|
||||
// std::string tmp_dbgfname = path + name + ".bdb";
|
||||
|
||||
ROS_WARN("Launching buzz compilation: %s", bzzfile_in_compile.str().c_str());
|
||||
|
||||
|
|
@ -977,7 +953,7 @@ void roscontroller::SetMode(std::string mode, int delay_miliseconds) {
|
|||
set_mode_message.request.custom_mode = mode;
|
||||
current_mode = mode;
|
||||
if (mode_client.call(set_mode_message)) {
|
||||
ROS_INFO("Set Mode Service call successful!");
|
||||
;//ROS_INFO("Set Mode Service call successful!");
|
||||
} else {
|
||||
ROS_INFO("Set Mode Service call failed!");
|
||||
}
|
||||
|
|
@ -993,7 +969,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) {
|
|||
ROS_INFO("Set stream rate call failed!, trying again...");
|
||||
ros::Duration(0.1).sleep();
|
||||
}
|
||||
ROS_INFO("Set stream rate call successful");
|
||||
//ROS_INFO("Set stream rate call successful");
|
||||
}
|
||||
|
||||
/*-------------------------------------------------------------
|
||||
|
|
@ -1059,13 +1035,10 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
|
|||
nei_pos.longitude = neighbours_pos_payload[1];
|
||||
nei_pos.altitude = neighbours_pos_payload[2];
|
||||
double cvt_neighbours_pos_payload[3];
|
||||
// cout<<"Got" << neighbours_pos_payload[0] <<", " <<
|
||||
//neighbours_pos_payload[1] << ", " << neighbours_pos_payload[2] << endl;
|
||||
gps_rb(nei_pos, cvt_neighbours_pos_payload);
|
||||
/*Extract robot id of the neighbour*/
|
||||
uint16_t *out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
|
||||
ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0],
|
||||
cvt_neighbours_pos_payload[1]);
|
||||
//ROS_WARN("RAB of %i: %f, %f", (int)out[1], cvt_neighbours_pos_payload[0], cvt_neighbours_pos_payload[1]);
|
||||
/*pass neighbour position to local maintaner*/
|
||||
buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],
|
||||
cvt_neighbours_pos_payload[1],
|
||||
|
|
@ -1096,7 +1069,7 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
|
|||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_LAND:
|
||||
ROS_INFO("RC_Call: LAND!!!! sending land");
|
||||
ROS_INFO("RC_Call: LAND!!!!");
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_LAND;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
|
|
@ -1121,20 +1094,22 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
|
|||
res.success = true;
|
||||
break;
|
||||
case mavros_msgs::CommandCode::NAV_WAYPOINT:
|
||||
ROS_INFO("RC_Call: GO TO!!!! --- Doing this! ");
|
||||
double rc_goto[3];
|
||||
// testing PositionTarget
|
||||
rc_goto[0] = req.param5;
|
||||
rc_goto[1] = req.param6;
|
||||
rc_goto[2] = req.param7;
|
||||
buzzuav_closures::rc_set_goto(rc_goto);
|
||||
ROS_INFO("RC_Call: GO TO!!!! ");
|
||||
buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
|
||||
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case 666:
|
||||
ROS_INFO("RC_Call: Update Fleet Status!!!!");
|
||||
rc_cmd = 666;
|
||||
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
|
||||
ROS_INFO("RC_Call: Gimbal!!!! ");
|
||||
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3);
|
||||
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case CMD_REQUEST_UPDATE:
|
||||
//ROS_INFO("RC_Call: Update Fleet Status!!!!");
|
||||
rc_cmd = CMD_REQUEST_UPDATE;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
|
|
|
|||
Loading…
Reference in New Issue