merge dev
This commit is contained in:
dave
parent
a99d2a6496
commit
d4ee8ef81e
|
|
@ -6,9 +6,9 @@ if(UNIX)
|
|||
endif()
|
||||
|
||||
if(SIM)
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1")
|
||||
else()
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0")
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1")
|
||||
endif()
|
||||
## Find catkin macros and libraries
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
|
|
|
|||
|
|
@ -7,10 +7,11 @@
|
|||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_TIMEOUT = 1200 # in steps
|
||||
BARRIER_TIMEOUT = 200 # in steps
|
||||
BARRIER_VSTIG = 80
|
||||
timeW = 0
|
||||
barrier = nil
|
||||
bc = 0;
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
|
|
@ -22,15 +23,15 @@ function barrier_create() {
|
|||
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
|
||||
if(barrier!=nil) {
|
||||
barrier=nil
|
||||
BARRIER_VSTIG = BARRIER_VSTIG +1
|
||||
# BARRIER_VSTIG = BARRIER_VSTIG +1
|
||||
}
|
||||
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
function barrier_set(threshold, transf, resumef) {
|
||||
function barrier_set(threshold, transf, resumef, bc) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf, resumef);
|
||||
barrier_wait(threshold, transf, resumef, bc);
|
||||
}
|
||||
BVMSTATE = "BARRIERWAIT"
|
||||
barrier_create()
|
||||
|
|
@ -39,30 +40,51 @@ function barrier_set(threshold, transf, resumef) {
|
|||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready() {
|
||||
function barrier_ready(bc) {
|
||||
#log("BARRIER READY -------")
|
||||
barrier.put(id, 1)
|
||||
barrier.put(id, bc)
|
||||
barrier.put("d", 0)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
function barrier_wait(threshold, transf, resumef) {
|
||||
barrier.put(id, 1)
|
||||
|
||||
function barrier_wait(threshold, transf, resumef, bc) {
|
||||
barrier.put(id, bc)
|
||||
barrier.get(id)
|
||||
#log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
|
||||
var allgood = 0
|
||||
log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
|
||||
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
|
||||
allgood = barrier_allgood(barrier,bc)
|
||||
}
|
||||
|
||||
if(allgood) {
|
||||
barrier.put("d", 1)
|
||||
timeW = 0
|
||||
BVMSTATE = transf
|
||||
} else if(timeW >= BARRIER_TIMEOUT) {
|
||||
log("------> Barrier Timeout !!!!")
|
||||
barrier=nil
|
||||
barrier = nil
|
||||
timeW = 0
|
||||
BVMSTATE = resumef
|
||||
}
|
||||
} else if(timeW % 100 == 0 and bc > 0)
|
||||
neighbors.broadcast("cmd", bc)
|
||||
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
barriergood = 1
|
||||
|
||||
# Barrer check all entries
|
||||
function barrier_allgood(barrier, bc) {
|
||||
barriergood = 1
|
||||
barrier.foreach(
|
||||
function(key, value, robot){
|
||||
#log("VS entry : ", key, " ", value, " ", robot)
|
||||
if(value != bc and key != "d"){
|
||||
barriergood = 0
|
||||
}
|
||||
}
|
||||
)
|
||||
return barriergood
|
||||
}
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
########################################
|
||||
#
|
||||
# BARRIER-RELATED FUNCTIONS
|
||||
#
|
||||
########################################
|
||||
|
||||
#
|
||||
# Constants
|
||||
#
|
||||
BARRIER_TIMEOUT = 1200 # in steps
|
||||
BARRIER_VSTIG = 80
|
||||
timeW = 0
|
||||
barrier = nil
|
||||
|
||||
#
|
||||
# Sets a barrier
|
||||
#
|
||||
function barrier_create() {
|
||||
# reset
|
||||
timeW = 0
|
||||
# create barrier vstig
|
||||
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
|
||||
if(barrier!=nil) {
|
||||
barrier=nil
|
||||
BARRIER_VSTIG = BARRIER_VSTIG +1
|
||||
}
|
||||
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
|
||||
barrier = stigmergy.create(BARRIER_VSTIG)
|
||||
}
|
||||
|
||||
function barrier_set(threshold, transf, resumef,bc) {
|
||||
statef = function() {
|
||||
barrier_wait(threshold, transf, resumef,bc);
|
||||
}
|
||||
BVMSTATE = "BARRIERWAIT"
|
||||
barrier_create()
|
||||
}
|
||||
|
||||
#
|
||||
# Make yourself ready
|
||||
#
|
||||
function barrier_ready(bc) {
|
||||
#log("BARRIER READY -------")
|
||||
barrier.put(id, 1)
|
||||
barrier.put("d", 0)
|
||||
}
|
||||
|
||||
#
|
||||
# Executes the barrier
|
||||
#
|
||||
function barrier_wait(threshold, transf, resumef,bc) {
|
||||
barrier.put(id, 1)
|
||||
|
||||
barrier.get(id)
|
||||
log("--->BS: ", barrier.size(), " (", BARRIER_VSTIG, ")")
|
||||
if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
|
||||
barrier.put("d", 1)
|
||||
timeW = 0
|
||||
BVMSTATE = transf
|
||||
} else if(timeW >= BARRIER_TIMEOUT) {
|
||||
log("------> Barrier Timeout !!!!")
|
||||
barrier=nil
|
||||
timeW = 0
|
||||
BVMSTATE = resumef
|
||||
}
|
||||
|
||||
timeW = timeW+1
|
||||
}
|
||||
|
||||
|
|
@ -5,17 +5,19 @@
|
|||
########################################
|
||||
include "utils/vec2.bzz"
|
||||
include "act/barrier.bzz"
|
||||
#include "act/old_barrier.bzz"
|
||||
include "utils/conversions.bzz"
|
||||
|
||||
TARGET_ALTITUDE = 15.0 # m.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
PICTURE_WAIT = 20 # steps
|
||||
GOTO_MAXVEL = 2.5 # m/steps
|
||||
|
||||
GOTO_MAXVEL = 0.5 # m/steps
|
||||
GOTO_MAXDIST = 150 # m.
|
||||
GOTODIST_TOL = 0.5 # m.
|
||||
GOTODIST_TOL = 1.0 # m.
|
||||
GOTOANG_TOL = 0.1 # rad.
|
||||
path_it = 0
|
||||
graphid = 0
|
||||
graphid = 3
|
||||
pic_time = 0
|
||||
g_it = 0
|
||||
|
||||
|
|
@ -27,44 +29,72 @@ function idle() {
|
|||
BVMSTATE = "IDLE"
|
||||
}
|
||||
|
||||
|
||||
# Custom function for the user.
|
||||
function cusfun(){
|
||||
BVMSTATE="CUSFUN"
|
||||
log("cusfun: yay!!!")
|
||||
LOCAL_TARGET = math.vec2.new(5 ,0 )
|
||||
local_set_point = math.vec2.new(LOCAL_TARGET.x - pose.position.x ,LOCAL_TARGET.y - pose.position.y ) # has to move 5 meters in x from the home location
|
||||
if(math.vec2.length(local_set_point) > GOTODIST_TOL){
|
||||
local_set_point = LimitSpeed(local_set_point, 1.0)
|
||||
goto_abs(local_set_point.x, local_set_point.y, 0.0, 0.0)
|
||||
}
|
||||
else{
|
||||
log("TARGET REACHED")
|
||||
}
|
||||
}
|
||||
|
||||
function launch() {
|
||||
BVMSTATE = "LAUNCH"
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so TAKE_OFF
|
||||
homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
|
||||
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
|
||||
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
|
||||
barrier_ready()
|
||||
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
|
||||
barrier_ready(22)
|
||||
} else {
|
||||
log("Altitude: ", pose.position.altitude)
|
||||
neighbors.broadcast("cmd", 22)
|
||||
uav_takeoff(TARGET_ALTITUDE)
|
||||
}
|
||||
} else {
|
||||
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH")
|
||||
barrier_ready()
|
||||
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
|
||||
barrier_ready(22)
|
||||
}
|
||||
}
|
||||
|
||||
gohomeT=100
|
||||
function goinghome() {
|
||||
BVMSTATE = "GOHOME"
|
||||
if(gohomeT > 0) { # TODO: Make a real check if home is reached
|
||||
gohome()
|
||||
gohomeT = gohomeT - 1
|
||||
} else
|
||||
BVMSTATE = AUTO_LAUNCH_STATE
|
||||
}
|
||||
|
||||
function stop() {
|
||||
BVMSTATE = "STOP"
|
||||
BVMSTATE = "STOP"
|
||||
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
|
||||
neighbors.broadcast("cmd", 21)
|
||||
uav_land()
|
||||
if(flight.status != 2 and flight.status != 3) {
|
||||
barrier_set(ROBOTS,"TURNEDOFF","STOP")
|
||||
barrier_ready()
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
|
||||
#barrier_ready(21)
|
||||
}
|
||||
} else {
|
||||
barrier_set(ROBOTS,"TURNEDOFF","STOP")
|
||||
barrier_ready()
|
||||
}
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
|
||||
#barrier_ready(21)
|
||||
}
|
||||
}
|
||||
|
||||
function take_picture() {
|
||||
BVMSTATE="PICTURE"
|
||||
uav_setgimbal(0.0, 0.0, -90.0, 20.0)
|
||||
setgimbal(0.0, 0.0, -90.0, 20.0)
|
||||
if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
|
||||
uav_takepicture()
|
||||
takepicture()
|
||||
} else if(pic_time>=PICTURE_WAIT) { # wait for the picture
|
||||
BVMSTATE="IDLE"
|
||||
pic_time=0
|
||||
|
|
@ -73,6 +103,7 @@ function take_picture() {
|
|||
}
|
||||
|
||||
function goto_gps(transf) {
|
||||
log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude)
|
||||
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
||||
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
|
||||
if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
|
||||
|
|
@ -80,7 +111,7 @@ function goto_gps(transf) {
|
|||
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
|
||||
transf()
|
||||
else {
|
||||
LimitSpeed(m_navigation, 1.0)
|
||||
m_navigation = LimitSpeed(m_navigation, 1.0)
|
||||
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
|
||||
}
|
||||
}
|
||||
|
|
@ -117,7 +148,7 @@ function aggregate() {
|
|||
# circle all together around centroid
|
||||
function pursuit() {
|
||||
BVMSTATE="PURSUIT"
|
||||
rd = 15.0
|
||||
rd = 20.0
|
||||
pc = 3.2
|
||||
vd = 15.0
|
||||
r_vec = neighbors.reduce(function(rid, data, r_vec) {
|
||||
|
|
@ -127,7 +158,8 @@ function pursuit() {
|
|||
if(neighbors.count() > 0)
|
||||
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
|
||||
r = math.vec2.length(r_vec)
|
||||
gamma = vd / math.sqrt((r-rd)*(r-rd)+pc*pc*r*r)
|
||||
var sqr = (r-rd)*(r-rd)+pc*pc*r*r
|
||||
gamma = vd / math.sqrt(sqr)
|
||||
dr = -gamma * (r-rd)
|
||||
dT = gamma * pc
|
||||
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1)
|
||||
|
|
@ -137,9 +169,9 @@ function pursuit() {
|
|||
|
||||
# Lennard-Jones interaction magnitude
|
||||
TARGET = 8.0
|
||||
EPSILON = 3.0
|
||||
EPSILON = 30.0
|
||||
function lj_magnitude(dist, target, epsilon) {
|
||||
lj = -(epsilon / dist) * ((target / dist)^4 - 1.2 * (target / dist)^2)
|
||||
lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
|
||||
return lj
|
||||
}
|
||||
|
||||
|
|
@ -154,92 +186,130 @@ function lj_sum(rid, data, accum) {
|
|||
}
|
||||
|
||||
# Calculates and actuates the flocking interaction
|
||||
<<<<<<< HEAD
|
||||
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
|
||||
=======
|
||||
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
|
||||
function formation() {
|
||||
BVMSTATE="FORMATION"
|
||||
# Calculate accumulator
|
||||
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
|
||||
if(neighbors.count() > 0)
|
||||
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
|
||||
old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/4.0)
|
||||
goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
|
||||
accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
|
||||
goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
|
||||
}
|
||||
|
||||
# listens to commands from the remote control (web, commandline, etc)
|
||||
function rc_cmd_listen() {
|
||||
if(flight.rc_cmd==22) {
|
||||
if(flight.rc_cmd==22) {
|
||||
log("cmd 22")
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "LAUNCH"
|
||||
neighbors.broadcast("cmd", 22)
|
||||
} else if(flight.rc_cmd==21) {
|
||||
log("cmd 21")
|
||||
flight.rc_cmd=0
|
||||
AUTO_LAUNCH_STATE = "TURNEDOFF"
|
||||
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
|
||||
#barrier_ready(21)
|
||||
BVMSTATE = "STOP"
|
||||
neighbors.broadcast("cmd", 21)
|
||||
} else if(flight.rc_cmd==20) {
|
||||
flight.rc_cmd=0
|
||||
AUTO_LAUNCH_STATE = "IDLE"
|
||||
barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
|
||||
barrier_ready(20)
|
||||
neighbors.broadcast("cmd", 20)
|
||||
} else if(flight.rc_cmd==16) {
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "PATHPLAN"
|
||||
} else if(flight.rc_cmd==400) {
|
||||
flight.rc_cmd=0
|
||||
uav_arm()
|
||||
arm()
|
||||
neighbors.broadcast("cmd", 400)
|
||||
} else if (flight.rc_cmd==401){
|
||||
flight.rc_cmd=0
|
||||
uav_disarm()
|
||||
disarm()
|
||||
neighbors.broadcast("cmd", 401)
|
||||
} else if (flight.rc_cmd==666){
|
||||
flight.rc_cmd=0
|
||||
stattab_send()
|
||||
} else if (flight.rc_cmd==900){
|
||||
} else if (flight.rc_cmd==777){
|
||||
flight.rc_cmd=0
|
||||
BVMSTATE = "TASK_ALLOCATE"
|
||||
reinit_time_sync()
|
||||
neighbors.broadcast("cmd", 777)
|
||||
}else if (flight.rc_cmd==900){
|
||||
flight.rc_cmd=0
|
||||
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
|
||||
barrier_ready(900)
|
||||
neighbors.broadcast("cmd", 900)
|
||||
} else if (flight.rc_cmd==901){
|
||||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
BVMSTATE = "PURSUIT"
|
||||
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
|
||||
barrier_ready(901)
|
||||
neighbors.broadcast("cmd", 901)
|
||||
} else if (flight.rc_cmd==902){
|
||||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
BVMSTATE = "AGGREGATE"
|
||||
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
|
||||
barrier_ready(902)
|
||||
neighbors.broadcast("cmd", 902)
|
||||
} else if (flight.rc_cmd==903){
|
||||
flight.rc_cmd=0
|
||||
destroyGraph()
|
||||
BVMSTATE = "FORMATION"
|
||||
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
|
||||
barrier_ready(903)
|
||||
neighbors.broadcast("cmd", 903)
|
||||
}
|
||||
}
|
||||
|
||||
# listens to other fleet members broadcasting commands
|
||||
function nei_cmd_listen() {
|
||||
neighbors.listen("cmd",
|
||||
function(vid, value, rid) {
|
||||
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
|
||||
if(value==22 and BVMSTATE!="BARRIERWAIT") {
|
||||
BVMSTATE = "LAUNCH"
|
||||
} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
|
||||
BVMSTATE = "STOP"
|
||||
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
|
||||
uav_arm()
|
||||
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||
uav_disarm()
|
||||
} else if(value==900){ # Shapes
|
||||
BVMSTATE = "TASK_ALLOCATE"
|
||||
} else if(value==901){ # Pursuit
|
||||
destroyGraph()
|
||||
BVMSTATE = "PURSUIT"
|
||||
} else if(value==902){ # Agreggate
|
||||
destroyGraph()
|
||||
BVMSTATE = "AGGREGATE"
|
||||
} else if(value==903){ # Formation
|
||||
destroyGraph()
|
||||
BVMSTATE = "FORMATION"
|
||||
} else if(value==16 and BVMSTATE=="IDLE"){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
# # if(gt.id == id) statef=goto
|
||||
# })
|
||||
if(BVMSTATE!="BARRIERWAIT") {
|
||||
if(value==22) {
|
||||
BVMSTATE = "LAUNCH"
|
||||
}else if(value==20) {
|
||||
AUTO_LAUNCH_STATE = "IDLE"
|
||||
BVMSTATE = "GOHOME"
|
||||
} else if(value==21) {
|
||||
AUTO_LAUNCH_STATE = "TURNEDOFF"
|
||||
BVMSTATE = "STOP"
|
||||
} else if(value==400 and BVMSTATE=="TURNEDOFF") {
|
||||
arm()
|
||||
} else if(value==401 and BVMSTATE=="TURNEDOFF"){
|
||||
disarm()
|
||||
} else if(value==777 and BVMSTATE=="TURNEDOFF"){
|
||||
reinit_time_sync()
|
||||
#neighbors.broadcast("cmd", 777)
|
||||
}else if(value==900){ # Shapes
|
||||
barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
|
||||
#barrier_ready(900)
|
||||
neighbors.broadcast("cmd", 900)
|
||||
} else if(value==901){ # Pursuit
|
||||
destroyGraph()
|
||||
barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
|
||||
#barrier_ready(901)
|
||||
neighbors.broadcast("cmd", 901)
|
||||
} else if(value==902){ # Agreggate
|
||||
destroyGraph()
|
||||
barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
|
||||
#barrier_ready(902)
|
||||
neighbors.broadcast("cmd", 902)
|
||||
} else if(value==903){ # Formation
|
||||
destroyGraph()
|
||||
barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
|
||||
#barrier_ready(903)
|
||||
neighbors.broadcast("cmd", 903)
|
||||
} else if(value==16 and BVMSTATE=="IDLE"){
|
||||
# neighbors.listen("gt",function(vid, value, rid) {
|
||||
# print("Got (", vid, ",", value, ") from robot #", rid)
|
||||
# # if(gt.id == id) statef=goto
|
||||
# })
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,7 +20,7 @@ mapgoal = {}
|
|||
function navigate() {
|
||||
BVMSTATE="NAVIGATE"
|
||||
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz
|
||||
uav_storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
|
||||
storegoal(goal_list[g_it].x, goal_list[g_it].y, pose.position.altitude)
|
||||
cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
|
||||
m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
|
||||
}
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ include "taskallocate/graphs/shapes_Y.bzz"
|
|||
ROBOT_RADIUS = 50
|
||||
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
|
||||
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
|
||||
ROOT_ID = 2
|
||||
ROOT_ID = 5
|
||||
|
||||
#
|
||||
# Global variables
|
||||
|
|
@ -389,7 +389,7 @@ function TransitionToJoined(){
|
|||
#write statues
|
||||
#v_tag.put(m_nLabel, m_lockstig)
|
||||
barrier_create()
|
||||
barrier_ready()
|
||||
barrier_ready(940)
|
||||
|
||||
m_navigation.x=0.0
|
||||
m_navigation.y=0.0
|
||||
|
|
@ -537,7 +537,7 @@ function DoJoining(){
|
|||
|
||||
if(m_gotjoinedparent!=1)
|
||||
set_rc_goto()
|
||||
else
|
||||
else
|
||||
goto_gps(TransitionToJoined)
|
||||
#pack the communication package
|
||||
m_selfMessage.State=s2i(BVMSTATE)
|
||||
|
|
@ -655,8 +655,7 @@ function DoJoined(){
|
|||
return
|
||||
}
|
||||
}
|
||||
|
||||
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
|
||||
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941)
|
||||
BroadcastGraph()
|
||||
}
|
||||
#
|
||||
|
|
@ -678,20 +677,21 @@ function DoLock() {
|
|||
m_navigation=motion_vector()
|
||||
}
|
||||
# #move
|
||||
goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
|
||||
# goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
|
||||
BroadcastGraph()
|
||||
|
||||
if(loop) {
|
||||
if(timeout_graph==0) {
|
||||
if(graphid < 3)
|
||||
graphid = graphid + 1
|
||||
else
|
||||
graphid = 0
|
||||
timeout_graph = 40
|
||||
BVMSTATE="TASK_ALLOCATE"
|
||||
}
|
||||
timeout_graph = timeout_graph - 1
|
||||
}
|
||||
# if(loop) {
|
||||
# if(timeout_graph==0) {
|
||||
# if(graphid < 3)
|
||||
# graphid = graphid + 1
|
||||
# else
|
||||
# graphid = 0
|
||||
# timeout_graph = 40
|
||||
# BVMSTATE="TASK_ALLOCATE"
|
||||
# }
|
||||
# timeout_graph = timeout_graph - 1
|
||||
# }
|
||||
}
|
||||
|
||||
#
|
||||
|
|
|
|||
|
|
@ -0,0 +1,28 @@
|
|||
TIME_TO_SYNC = 100
|
||||
sync_timer = 0
|
||||
timesync_old_state = 0
|
||||
timesync_itr = 0
|
||||
timesync_state = 0
|
||||
|
||||
# Function to sync. algo
|
||||
function check_time_sync(){
|
||||
# sync_timer = sync_timer + 1
|
||||
# if(sync_timer < TIME_TO_SYNC){
|
||||
# log(" Synchronizing Clock : ",sync_timer,"/",TIME_TO_SYNC)
|
||||
# timesync_state = 1
|
||||
# }
|
||||
# else{
|
||||
# timesync_state = 0
|
||||
# }
|
||||
# if(timesync_old_state == 0 and timesync_state == 1){
|
||||
# timesync_itr = timesync_itr + 1
|
||||
# timesync_old_state = 0
|
||||
# }
|
||||
# timesync_old_state = timesync_state
|
||||
}
|
||||
|
||||
# Function to set sync timer to zero and reinitiate sync. algo
|
||||
|
||||
function reinit_time_sync(){
|
||||
sync_timer = 0
|
||||
}
|
||||
|
|
@ -4,6 +4,7 @@
|
|||
####################################################################################################
|
||||
updated="update_ack"
|
||||
update_no=0
|
||||
function updated_neigh(){
|
||||
updates_active = 1
|
||||
function updated_no_bct(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
||||
|
|
@ -66,4 +66,4 @@ function gps_from_vec(vec) {
|
|||
#goal.longitude = d_lon + pose.position.longitude;
|
||||
|
||||
return Lgoal
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,10 +17,10 @@ string.charat = function(s, n) {
|
|||
# RETURN: The position of the first match, or nil
|
||||
#
|
||||
string.indexoffirst = function(s, m) {
|
||||
var ls = string.length(s)
|
||||
var las = string.length(s)
|
||||
var lm = string.length(m)
|
||||
var i = 0
|
||||
while(i < ls-lm+1) {
|
||||
while(i < las-lm+1) {
|
||||
if(string.sub(s, i, i+lm) == m) return i
|
||||
i = i + 1
|
||||
}
|
||||
|
|
@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) {
|
|||
# RETURN: The position of the last match, or nil
|
||||
#
|
||||
string.indexoflast = function(s, m) {
|
||||
var ls = string.length(s)
|
||||
var las = string.length(s)
|
||||
var lm = string.length(m)
|
||||
var i = ls - lm + 1
|
||||
var i = las - lm + 1
|
||||
while(i >= 0) {
|
||||
if(string.sub(s, i, i+lm) == m) return i
|
||||
i = i - 1
|
||||
|
|
@ -56,10 +56,10 @@ string.split = function(s, d) {
|
|||
var i2 = 0 # index to move along s (token end)
|
||||
var c = 0 # token count
|
||||
var t = {} # token list
|
||||
var ls = string.length(s)
|
||||
var las = string.length(s)
|
||||
var ld = string.length(d)
|
||||
# Go through string s
|
||||
while(i2 < ls) {
|
||||
while(i2 < las) {
|
||||
# Try every delimiter
|
||||
var j = 0 # index to move along d
|
||||
var f = nil # whether the delimiter was found or not
|
||||
|
|
|
|||
|
|
@ -5,9 +5,15 @@ include "act/states.bzz"
|
|||
include "plan/rrtstar.bzz"
|
||||
include "taskallocate/graphformGPS.bzz"
|
||||
include "vstigenv.bzz"
|
||||
include "timesync.bzz"
|
||||
|
||||
#State launched after takeoff
|
||||
AUTO_LAUNCH_STATE = "PURSUIT"
|
||||
AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
|
||||
#AUTO_LAUNCH_STATE = "CUSFUN"
|
||||
#Lowest robot id in the network
|
||||
LOWEST_ROBOT_ID = 1
|
||||
TARGET = 9.0
|
||||
EPSILON = 30.0
|
||||
|
||||
#####
|
||||
# Vehicule type:
|
||||
|
|
@ -26,7 +32,10 @@ function init() {
|
|||
init_stig()
|
||||
init_swarm()
|
||||
|
||||
TARGET_ALTITUDE = 10 + id*2.0 # m
|
||||
# initGraph()
|
||||
|
||||
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
|
||||
|
||||
loop = 1
|
||||
|
||||
# start the swarm command listener
|
||||
|
|
@ -38,6 +47,10 @@ function init() {
|
|||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
|
||||
# check time sync algorithm state
|
||||
check_time_sync()
|
||||
|
||||
# listen to Remote Controller
|
||||
rc_cmd_listen()
|
||||
|
||||
|
|
@ -49,10 +62,14 @@ function step() {
|
|||
#
|
||||
if(BVMSTATE=="TURNEDOFF")
|
||||
statef=turnedoff
|
||||
else if(BVMSTATE=="CUSFUN")
|
||||
statef=cusfun
|
||||
else if(BVMSTATE=="STOP") # ends on turnedoff
|
||||
statef=stop
|
||||
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
|
||||
statef=launch
|
||||
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
|
||||
statef=goinghome
|
||||
else if(BVMSTATE=="IDLE")
|
||||
statef=idle
|
||||
else if(BVMSTATE=="AGGREGATE")
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ function step() {
|
|||
statef=action
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,35 +1,38 @@
|
|||
include "act/states.bzz"
|
||||
include "vstigenv.bzz"
|
||||
|
||||
updated="update_ack"
|
||||
update_no=0
|
||||
function init(){
|
||||
barrier = stigmergy.create(101)
|
||||
barrier.put(id,1)
|
||||
barrier_val=0
|
||||
barrier.onconflict(function (k,l,r) {
|
||||
if(r. data < l. data or (r. data == l. data )) return l
|
||||
else return r
|
||||
})
|
||||
BVMSTATE = "UPDATESTANDBY"
|
||||
|
||||
s = swarm.create(1)
|
||||
s.join()
|
||||
# Executed once at init time.
|
||||
function init(){
|
||||
barrier = stigmergy.create(101)
|
||||
barrier.put(id,1)
|
||||
barrier_val=0
|
||||
barrier.onconflict(function (k,l,r) {
|
||||
if(r. data < l. data or (r. data == l. data )) return l
|
||||
else return r
|
||||
})
|
||||
init_swarm()
|
||||
# start the swarm command listener
|
||||
nei_cmd_listen()
|
||||
}
|
||||
|
||||
function stand_by(){
|
||||
barrier.get(id)
|
||||
barrier_val = barrier.size()
|
||||
|
||||
barrier.get(id)
|
||||
barrier_val = barrier.size()
|
||||
|
||||
neighbors.listen(updated,
|
||||
function(vid, value, rid) {
|
||||
print(" got from",vid," ", " value = ",value," ","rid"," " )
|
||||
if(value==update_no) barrier.put(rid,1)
|
||||
}
|
||||
neighbors.listen(updated,
|
||||
function(vid, value, rid) {
|
||||
if(value==update_no) barrier.put(rid,1)
|
||||
}
|
||||
)
|
||||
|
||||
}
|
||||
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
|
||||
stand_by()
|
||||
stand_by()
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,72 @@
|
|||
include "update.bzz"
|
||||
# don't use a stigmergy id=11 with this header, for barrier
|
||||
# it requires an 'action' function to be defined here.
|
||||
include "act/states.bzz"
|
||||
include "vstigenv.bzz"
|
||||
|
||||
V_TYPE = 0
|
||||
|
||||
#State launched after takeoff
|
||||
AUTO_LAUNCH_STATE = "FORMATION"
|
||||
|
||||
|
||||
TARGET = 8.0
|
||||
EPSILON = 3.0
|
||||
GOTO_MAXVEL = 2.5 # m/steps
|
||||
|
||||
# Executed once at init time.
|
||||
function init() {
|
||||
init_stig()
|
||||
init_swarm()
|
||||
|
||||
# start the swarm command listener
|
||||
nei_cmd_listen()
|
||||
|
||||
# Starting state: TURNEDOFF to wait for user input.
|
||||
BVMSTATE = "TURNEDOFF"
|
||||
TAKEOFF_COUNTER = 20
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
rc_cmd_listen()
|
||||
|
||||
# update the vstig (status/net/batt/...)
|
||||
# uav_updatestig()
|
||||
|
||||
#
|
||||
# State machine
|
||||
#
|
||||
if(BVMSTATE=="TURNEDOFF")
|
||||
statef=turnedoff
|
||||
else if(BVMSTATE=="STOP") # ends on turnedoff
|
||||
statef=stop
|
||||
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
|
||||
statef=launch
|
||||
else if(BVMSTATE=="IDLE")
|
||||
statef=idle
|
||||
else if(BVMSTATE=="FORMATION")
|
||||
statef=formation
|
||||
|
||||
statef()
|
||||
|
||||
log("Current state: ", BVMSTATE)
|
||||
|
||||
# Auto-takeoff (delayed for simulator boot)
|
||||
if(id == 0) {
|
||||
if(TAKEOFF_COUNTER>0)
|
||||
TAKEOFF_COUNTER = TAKEOFF_COUNTER - 1
|
||||
else if(TAKEOFF_COUNTER == 0) {
|
||||
BVMSTATE="LAUNCH"
|
||||
TAKEOFF_COUNTER = -1
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
# Executed once when the robot (or the simulator) is reset.
|
||||
function reset() {
|
||||
}
|
||||
|
||||
# Executed once at the end of experiment.
|
||||
function destroy() {
|
||||
}
|
||||
|
|
@ -1,161 +1,171 @@
|
|||
#ifndef BUZZ_UPDATE_H
|
||||
#define BUZZ_UPDATE_H
|
||||
/*Simulation or robot check*/
|
||||
//#define SIMULATION 1 // set in CMAKELIST
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/inotify.h>
|
||||
#include <buzz/buzztype.h>
|
||||
#include <buzz/buzzdict.h>
|
||||
#include <buzz/buzzdarray.h>
|
||||
#include <buzz/buzzvstig.h>
|
||||
#include <buzz_utility.h>
|
||||
#include <fstream>
|
||||
|
||||
#define delete_p(p) \
|
||||
do \
|
||||
{ \
|
||||
free(p); \
|
||||
p = NULL; \
|
||||
} while (0)
|
||||
|
||||
static const uint16_t CODE_REQUEST_PADDING = 250;
|
||||
static const uint16_t MIN_UPDATE_PACKET = 251;
|
||||
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
|
||||
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
|
||||
/*********************/
|
||||
/* Updater states */
|
||||
/********************/
|
||||
|
||||
typedef enum {
|
||||
CODE_RUNNING = 0, // Code executing
|
||||
CODE_STANDBY, // Standing by for others to update
|
||||
} code_states_e;
|
||||
|
||||
/*********************/
|
||||
/*Message types */
|
||||
/********************/
|
||||
|
||||
typedef enum {
|
||||
SENT_CODE = 0, // Broadcast code
|
||||
RESEND_CODE, // ReBroadcast request
|
||||
} code_message_e;
|
||||
|
||||
/*************************/
|
||||
/*Updater message queue */
|
||||
/*************************/
|
||||
|
||||
struct updater_msgqueue_s
|
||||
namespace buzz_update
|
||||
{
|
||||
uint8_t* queue;
|
||||
uint8_t* size;
|
||||
};
|
||||
typedef struct updater_msgqueue_s* updater_msgqueue_t;
|
||||
static const uint16_t CODE_REQUEST_PADDING = 250;
|
||||
static const uint16_t MIN_UPDATE_PACKET = 251;
|
||||
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
|
||||
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
|
||||
/*********************/
|
||||
/* Updater states */
|
||||
/********************/
|
||||
|
||||
struct updater_code_s
|
||||
{
|
||||
uint8_t* bcode;
|
||||
uint8_t* bcode_size;
|
||||
};
|
||||
typedef struct updater_code_s* updater_code_t;
|
||||
typedef enum {
|
||||
CODE_RUNNING = 0, // Code executing
|
||||
CODE_STANDBY, // Standing by for others to update
|
||||
} code_states_e;
|
||||
|
||||
/**************************/
|
||||
/*Updater data*/
|
||||
/**************************/
|
||||
/*********************/
|
||||
/*Message types */
|
||||
/********************/
|
||||
|
||||
struct buzz_updater_elem_s
|
||||
{
|
||||
/* robot id */
|
||||
// uint16_t robotid;
|
||||
/*current Bytecode content */
|
||||
uint8_t* bcode;
|
||||
/*old Bytecode name */
|
||||
const char* old_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* bcode_size;
|
||||
/*Update patch*/
|
||||
uint8_t* patch;
|
||||
/* Update patch size*/
|
||||
size_t* patch_size;
|
||||
/*current Bytecode content */
|
||||
uint8_t* standby_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* standby_bcode_size;
|
||||
/*updater out msg queue */
|
||||
updater_msgqueue_t outmsg_queue;
|
||||
/*updater in msg queue*/
|
||||
updater_msgqueue_t inmsg_queue;
|
||||
/*Current state of the updater one in code_states_e ENUM*/
|
||||
int* mode;
|
||||
uint8_t* update_no;
|
||||
};
|
||||
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
|
||||
typedef enum {
|
||||
SENT_CODE = 0, // Broadcast code
|
||||
RESEND_CODE, // ReBroadcast request
|
||||
} code_message_e;
|
||||
|
||||
/**************************************************************************/
|
||||
/*Updater routine from msg processing to file checks to be called from main*/
|
||||
/**************************************************************************/
|
||||
void update_routine();
|
||||
/*************************/
|
||||
/*Updater message queue */
|
||||
/*************************/
|
||||
|
||||
/************************************************/
|
||||
/*Initalizes the updater */
|
||||
/************************************************/
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
|
||||
struct updater_msgqueue_s
|
||||
{
|
||||
uint8_t* queue;
|
||||
uint8_t* size;
|
||||
};
|
||||
typedef struct updater_msgqueue_s* updater_msgqueue_t;
|
||||
|
||||
/*********************************************************/
|
||||
/*Appends buffer of given size to in msg queue of updater*/
|
||||
/*********************************************************/
|
||||
struct updater_code_s
|
||||
{
|
||||
uint8_t* bcode;
|
||||
uint8_t* bcode_size;
|
||||
};
|
||||
typedef struct updater_code_s* updater_code_t;
|
||||
|
||||
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
|
||||
/**************************/
|
||||
/*Updater data*/
|
||||
/**************************/
|
||||
|
||||
/*********************************************************/
|
||||
/*Processes messages inside the queue of the updater*/
|
||||
/*********************************************************/
|
||||
struct buzz_updater_elem_s
|
||||
{
|
||||
/* robot id */
|
||||
// uint16_t robotid;
|
||||
/*current Bytecode content */
|
||||
uint8_t* bcode;
|
||||
/*old Bytecode name */
|
||||
const char* old_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* bcode_size;
|
||||
/*Update patch*/
|
||||
uint8_t* patch;
|
||||
/* Update patch size*/
|
||||
size_t* patch_size;
|
||||
/*current Bytecode content */
|
||||
uint8_t* standby_bcode;
|
||||
/*current bcode size*/
|
||||
size_t* standby_bcode_size;
|
||||
/*updater out msg queue */
|
||||
updater_msgqueue_t outmsg_queue;
|
||||
/*updater in msg queue*/
|
||||
updater_msgqueue_t inmsg_queue;
|
||||
/*Current state of the updater one in code_states_e ENUM*/
|
||||
int* mode;
|
||||
uint8_t* update_no;
|
||||
};
|
||||
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
|
||||
|
||||
void code_message_inqueue_process();
|
||||
/**************************************************************************/
|
||||
/*Updater routine from msg processing to file checks to be called from main*/
|
||||
/**************************************************************************/
|
||||
void update_routine();
|
||||
|
||||
/*****************************************************/
|
||||
/* obtains messages from out msgs queue of the updater*/
|
||||
/*******************************************************/
|
||||
uint8_t* getupdater_out_msg();
|
||||
/************************************************/
|
||||
/*Initalizes the updater */
|
||||
/************************************************/
|
||||
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
|
||||
|
||||
/******************************************************/
|
||||
/*obtains out msg queue size*/
|
||||
/*****************************************************/
|
||||
uint8_t* getupdate_out_msg_size();
|
||||
/*********************************************************/
|
||||
/*Appends buffer of given size to in msg queue of updater*/
|
||||
/*********************************************************/
|
||||
|
||||
/**************************************************/
|
||||
/*destroys the out msg queue*/
|
||||
/*************************************************/
|
||||
void destroy_out_msg_queue();
|
||||
void code_message_inqueue_append(uint8_t* msg, uint16_t size);
|
||||
|
||||
/***************************************************/
|
||||
/*obatins updater state*/
|
||||
/***************************************************/
|
||||
// int get_update_mode();
|
||||
/*********************************************************/
|
||||
/*Processes messages inside the queue of the updater*/
|
||||
/*********************************************************/
|
||||
|
||||
// buzz_updater_elem_t get_updater();
|
||||
/***************************************************/
|
||||
/*sets bzz file name*/
|
||||
/***************************************************/
|
||||
void set_bzz_file(const char* in_bzz_file);
|
||||
void code_message_inqueue_process();
|
||||
|
||||
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
|
||||
/*****************************************************/
|
||||
/*Obtains messages from out msgs queue of the updater*/
|
||||
/*******************************************************/
|
||||
uint8_t* getupdater_out_msg();
|
||||
|
||||
/****************************************************/
|
||||
/*Destroys the updater*/
|
||||
/***************************************************/
|
||||
/******************************************************/
|
||||
/*Obtains out msg queue size*/
|
||||
/*****************************************************/
|
||||
uint8_t* getupdate_out_msg_size();
|
||||
|
||||
void destroy_updater();
|
||||
/**************************************************/
|
||||
/*Destroys the out msg queue*/
|
||||
/*************************************************/
|
||||
void destroy_out_msg_queue();
|
||||
|
||||
int is_msg_present();
|
||||
// buzz_updater_elem_t get_updater();
|
||||
/***************************************************/
|
||||
/*Sets bzz file name*/
|
||||
/***************************************************/
|
||||
void set_bzz_file(const char* in_bzz_file, bool dbg);
|
||||
|
||||
int get_update_status();
|
||||
/****************************************************/
|
||||
/*Tests the code from a buffer*/
|
||||
/***************************************************/
|
||||
|
||||
void set_read_update_status();
|
||||
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
|
||||
|
||||
int compile_bzz(std::string bzz_file);
|
||||
/****************************************************/
|
||||
/*Destroys the updater*/
|
||||
/***************************************************/
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
void destroy_updater();
|
||||
|
||||
// void set_packet_id(int packet_id);
|
||||
/****************************************************/
|
||||
/*Checks for updater message*/
|
||||
/***************************************************/
|
||||
|
||||
// void collect_data(std::ofstream& logger);
|
||||
int is_msg_present();
|
||||
|
||||
/****************************************************/
|
||||
/*Compiles a bzz script to bo and bdbg*/
|
||||
/***************************************************/
|
||||
|
||||
int compile_bzz(std::string bzz_file);
|
||||
|
||||
/****************************************************/
|
||||
/*Set number of robots in the updater*/
|
||||
/***************************************************/
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -43,6 +43,24 @@ struct neiStatus
|
|||
};
|
||||
typedef struct neiStatus neighbors_status;
|
||||
|
||||
struct neitime
|
||||
{
|
||||
uint64_t nei_hardware_time;
|
||||
uint64_t nei_logical_time;
|
||||
uint64_t node_hardware_time;
|
||||
uint64_t node_logical_time;
|
||||
double nei_rate;
|
||||
double relative_rate;
|
||||
int age;
|
||||
neitime(uint64_t nht, uint64_t nlt, uint64_t mht, uint64_t mlt, double nr, double mr)
|
||||
: nei_hardware_time(nht), nei_logical_time(nlt), node_hardware_time(mht), node_logical_time(mlt),
|
||||
nei_rate(nr), relative_rate(mr) {};
|
||||
neitime()
|
||||
{
|
||||
}
|
||||
};
|
||||
typedef struct neitime neighbor_time;
|
||||
|
||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||
|
||||
int buzz_listen(const char* type, int msg_size);
|
||||
|
|
@ -77,4 +95,12 @@ buzzvm_t get_vm();
|
|||
void set_robot_var(int ROBOTS);
|
||||
|
||||
int get_inmsg_size();
|
||||
|
||||
std::vector<uint8_t*> get_inmsg_vector();
|
||||
|
||||
std::string getuavstate();
|
||||
|
||||
int get_timesync_state();
|
||||
|
||||
int get_timesync_itr();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -89,6 +89,10 @@ void set_filtered_packet_loss(float value);
|
|||
/*
|
||||
* sets current position
|
||||
*/
|
||||
|
||||
void set_currentNEDpos(double x, double y);
|
||||
|
||||
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
|
||||
void set_currentpos(double latitude, double longitude, float altitude, float yaw);
|
||||
/*
|
||||
* returns the current go to position
|
||||
|
|
@ -98,10 +102,8 @@ double* getgoto();
|
|||
* returns the current grid
|
||||
*/
|
||||
std::map<int, std::map<int,int>> getgrid();
|
||||
/*
|
||||
* returns the current Buzz state
|
||||
*/
|
||||
std::string getuavstate();
|
||||
|
||||
|
||||
/*
|
||||
* returns the gimbal commands
|
||||
*/
|
||||
|
|
@ -118,6 +120,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
|
|||
* update neighbors from in msgs
|
||||
*/
|
||||
void update_neighbors(buzzvm_t vm);
|
||||
/*
|
||||
*Clear neighbours struct
|
||||
*/
|
||||
void clear_neighbours_pos();
|
||||
/*
|
||||
* closure to add a neighbor status
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@
|
|||
#include "mavros_msgs/SetMode.h"
|
||||
#include "mavros_msgs/State.h"
|
||||
#include "mavros_msgs/BatteryStatus.h"
|
||||
#include "sensor_msgs/BatteryState.h"
|
||||
#include "mavros_msgs/Mavlink.h"
|
||||
#include "mavros_msgs/PositionTarget.h"
|
||||
#include "sensor_msgs/NavSatStatus.h"
|
||||
|
|
@ -34,14 +35,29 @@
|
|||
#include <signal.h>
|
||||
#include <ostream>
|
||||
#include <map>
|
||||
#include <cmath>
|
||||
#include "buzzuav_closures.h"
|
||||
|
||||
#define UPDATER_MESSAGE_CONSTANT 987654321
|
||||
#define XBEE_MESSAGE_CONSTANT 586782343
|
||||
#define XBEE_STOP_TRANSMISSION 4355356352
|
||||
/*
|
||||
* ROSBuzz message types
|
||||
*/
|
||||
typedef enum {
|
||||
ROS_BUZZ_MSG_NIL = 0, // dummy msg
|
||||
UPDATER_MESSAGE, // Update msg
|
||||
BUZZ_MESSAGE_WTO_TIME, // Broadcast message wihout time info
|
||||
BUZZ_MESSAGE_TIME, // Broadcast message with time info
|
||||
} rosbuzz_msgtype;
|
||||
|
||||
// Time sync algo. constants
|
||||
#define COM_DELAY 100000000 // in nano seconds i.e 100 ms
|
||||
#define TIME_SYNC_JUMP_THR 500000000
|
||||
#define MOVING_AVERAGE_ALPHA 0.1
|
||||
#define MAX_NUMBER_OF_ROBOTS 10
|
||||
|
||||
#define TIMEOUT 60
|
||||
#define BUZZRATE 10
|
||||
#define CMD_REQUEST_UPDATE 666
|
||||
#define CMD_SYNC_CLOCK 777
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -83,19 +99,42 @@ private:
|
|||
};
|
||||
typedef struct POSE ros_pose;
|
||||
|
||||
|
||||
struct MsgData
|
||||
{
|
||||
int msgid;
|
||||
uint16_t nid;
|
||||
uint16_t size;
|
||||
double sent_time;
|
||||
uint64_t received_time;
|
||||
MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt):
|
||||
msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
|
||||
MsgData(){};
|
||||
};
|
||||
typedef struct MsgData msg_data;
|
||||
|
||||
ros_pose target, home, cur_pos;
|
||||
|
||||
uint64_t payload;
|
||||
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
|
||||
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
|
||||
std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
|
||||
int timer_step = 0;
|
||||
int robot_id = 0;
|
||||
ros::Time logical_clock;
|
||||
ros::Time previous_step_time;
|
||||
std::vector<msg_data> inmsgdata;
|
||||
uint64_t out_msg_time;
|
||||
double logical_time_rate;
|
||||
bool time_sync_jumped;
|
||||
std::string robot_name = "";
|
||||
|
||||
int rc_cmd;
|
||||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int update;
|
||||
int statepub_active;
|
||||
int message_number = 0;
|
||||
uint8_t no_of_robots = 0;
|
||||
bool rcclient;
|
||||
|
|
@ -205,7 +244,7 @@ private:
|
|||
double gps_r_lat);
|
||||
|
||||
/*battery status callback */
|
||||
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
|
||||
void battery(const sensor_msgs::BatteryState::ConstPtr& msg);
|
||||
|
||||
/*flight extended status callback*/
|
||||
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
|
||||
|
|
@ -265,5 +304,9 @@ private:
|
|||
bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
|
||||
void get_xbee_status();
|
||||
|
||||
void time_sync_step();
|
||||
void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr);
|
||||
uint64_t get_logical_time();
|
||||
void set_logical_time_correction(uint64_t cor);
|
||||
};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,12 @@
|
|||
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
|
||||
|
||||
|
|
@ -3,12 +3,13 @@
|
|||
<!-- This file is included in all ROS workspace launch files -->
|
||||
<!-- Modify with great care! -->
|
||||
<launch>
|
||||
<arg name="name" default="robot0"/>
|
||||
<arg name="name" default="robot0"/>
|
||||
<arg name="xbee_plugged" default="true"/>
|
||||
<arg name="script" default="testalone"/>
|
||||
<arg name="launch_config" default="topics"/>
|
||||
|
||||
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
|
|
|
|||
|
|
@ -6,9 +6,10 @@
|
|||
<arg name="name" default="robot0"/>
|
||||
<arg name="xbee_plugged" default="true"/>
|
||||
<arg name="script" default="testalone"/>
|
||||
<arg name="launch_config" default="topics"/>
|
||||
|
||||
<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"/>
|
||||
<rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
|
||||
<param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
|
||||
<param name="rcclient" value="true" />
|
||||
<param name="rcservice_name" value="buzzcmd" />
|
||||
|
|
|
|||
|
|
@ -11,6 +11,9 @@ function arm {
|
|||
function disarm {
|
||||
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0
|
||||
}
|
||||
function timesync {
|
||||
rosservice call $1/buzzcmd 0 777 0 0 0 0 0 0 0 0
|
||||
}
|
||||
function testWP {
|
||||
rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
|
||||
}
|
||||
|
|
@ -31,7 +34,8 @@ function stoprobot {
|
|||
}
|
||||
function updaterobot {
|
||||
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
|
||||
rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100buzzy.launch
|
||||
rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXHeavenbuzzy.launch
|
||||
# rosrun robot_upstart install --logdir /media/key/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXxbeebuzzy.launch
|
||||
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch
|
||||
}
|
||||
function uavstate {
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its st
|
|||
|
||||
References
|
||||
------
|
||||
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
|
||||
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
|
||||
|
||||
* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
|
||||
|
||||
|
|
@ -108,4 +108,4 @@ To activate highlights of the code in Visual Studio Code or Roboware add the fol
|
|||
"files.associations": {
|
||||
"*.bzz":"python"
|
||||
}
|
||||
```
|
||||
```
|
||||
|
|
|
|||
1293
src/buzz_update.cpp
1293
src/buzz_update.cpp
File diff suppressed because it is too large
Load Diff
|
|
@ -564,4 +564,56 @@ int get_inmsg_size()
|
|||
{
|
||||
return IN_MSG.size();
|
||||
}
|
||||
|
||||
std::vector<uint8_t*> get_inmsg_vector(){
|
||||
return IN_MSG;
|
||||
}
|
||||
|
||||
string getuavstate()
|
||||
/*
|
||||
/ return current BVM state
|
||||
---------------------------------------*/
|
||||
{
|
||||
std::string uav_state = "Unknown";
|
||||
if(VM && VM->strings){
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzobj_t obj = buzzvm_stack_at(VM, 1);
|
||||
uav_state = string(obj->s.value.str);
|
||||
buzzvm_pop(VM);
|
||||
}
|
||||
return uav_state;
|
||||
}
|
||||
|
||||
int get_timesync_state()
|
||||
/*
|
||||
/ return time sync state from bzz script
|
||||
--------------------------------------*/
|
||||
{
|
||||
int timesync_state = 0;
|
||||
if(VM){
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzobj_t obj = buzzvm_stack_at(VM, 1);
|
||||
if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value;
|
||||
buzzvm_pop(VM);
|
||||
}
|
||||
return timesync_state;
|
||||
}
|
||||
int get_timesync_itr()
|
||||
/*
|
||||
/ return time sync iteration from bzz script
|
||||
--------------------------------------*/
|
||||
{
|
||||
int timesync_itr = 0;
|
||||
if(VM){
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzobj_t obj = buzzvm_stack_at(VM, 1);
|
||||
if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value;
|
||||
buzzvm_pop(VM);
|
||||
}
|
||||
return timesync_itr;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@ static float rc_gimbal[4];
|
|||
static float batt[3];
|
||||
static float obst[5] = { 0, 0, 0, 0, 0 };
|
||||
static double cur_pos[4];
|
||||
static double cur_NEDpos[2];
|
||||
static uint8_t status;
|
||||
static int cur_cmd = 0;
|
||||
static int rc_cmd = 0;
|
||||
|
|
@ -228,8 +229,8 @@ int buzzuav_moveto(buzzvm_t vm)
|
|||
goto_pos[2] = height + dh;
|
||||
goto_pos[3] = dyaw;
|
||||
// DEBUG
|
||||
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
|
||||
// goto_pos[1], goto_pos[2]);
|
||||
ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
|
||||
goto_pos[1], goto_pos[2]);
|
||||
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
|
||||
return buzzvm_ret0(vm);
|
||||
}
|
||||
|
|
@ -400,7 +401,11 @@ int buzzuav_arm(buzzvm_t vm)
|
|||
/ Buzz closure to arm
|
||||
/---------------------------------------*/
|
||||
{
|
||||
#ifdef MAVROSKINETIC
|
||||
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
|
||||
#else
|
||||
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
|
||||
#endif
|
||||
printf(" Buzz requested Arm \n");
|
||||
buzz_cmd = COMMAND_ARM;
|
||||
return buzzvm_ret0(vm);
|
||||
|
|
@ -411,7 +416,11 @@ int buzzuav_disarm(buzzvm_t vm)
|
|||
/ Buzz closure to disarm
|
||||
/---------------------------------------*/
|
||||
{
|
||||
#ifdef MAVROSKINETIC
|
||||
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1;
|
||||
#else
|
||||
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
|
||||
#endif
|
||||
printf(" Buzz requested Disarm \n");
|
||||
buzz_cmd = COMMAND_DISARM;
|
||||
return buzzvm_ret0(vm);
|
||||
|
|
@ -479,19 +488,6 @@ float* getgimbal()
|
|||
return rc_gimbal;
|
||||
}
|
||||
|
||||
string getuavstate()
|
||||
/*
|
||||
/ return current BVM state
|
||||
---------------------------------------*/
|
||||
{
|
||||
static buzzvm_t VM = buzz_utility::get_vm();
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
|
||||
buzzvm_pop(VM);
|
||||
return uav_state->s.value.str;
|
||||
}
|
||||
|
||||
int getcmd()
|
||||
/*
|
||||
/ return current mavros command to the FC
|
||||
|
|
@ -642,6 +638,15 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
|
|||
return vm->state;
|
||||
}
|
||||
|
||||
void set_currentNEDpos(double x, double y)
|
||||
/*
|
||||
/ update interface position array
|
||||
-----------------------------------*/
|
||||
{
|
||||
cur_NEDpos[0] = x;
|
||||
cur_NEDpos[1] = y;
|
||||
}
|
||||
|
||||
void set_currentpos(double latitude, double longitude, float altitude, float yaw)
|
||||
/*
|
||||
/ update interface position array
|
||||
|
|
@ -678,6 +683,11 @@ void update_neighbors(buzzvm_t vm)
|
|||
}
|
||||
}
|
||||
|
||||
// Clear neighbours pos
|
||||
void clear_neighbours_pos(){
|
||||
neighbors_map.clear();
|
||||
}
|
||||
|
||||
int buzzuav_update_currentpos(buzzvm_t vm)
|
||||
/*
|
||||
/ Update the BVM position table
|
||||
|
|
@ -705,6 +715,14 @@ int buzzuav_update_currentpos(buzzvm_t vm)
|
|||
buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 0));
|
||||
buzzvm_pushf(vm, cur_pos[2]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tPosition);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "x", 0));
|
||||
buzzvm_pushf(vm, cur_NEDpos[0]);
|
||||
buzzvm_tput(vm);
|
||||
buzzvm_push(vm, tPosition);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "y", 0));
|
||||
buzzvm_pushf(vm, cur_NEDpos[1]);
|
||||
buzzvm_tput(vm);
|
||||
// Store read table in the proximity table
|
||||
buzzvm_push(vm, tPoseTable);
|
||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0));
|
||||
|
|
|
|||
|
|
@ -11,9 +11,10 @@
|
|||
namespace rosbzz_node
|
||||
{
|
||||
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
|
||||
const bool debug = false;
|
||||
const bool debug = true;
|
||||
|
||||
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
|
||||
roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv):
|
||||
logical_clock(ros::Time()), previous_step_time(ros::Time())
|
||||
/*
|
||||
/ roscontroller class Constructor
|
||||
---------------*/
|
||||
|
|
@ -27,7 +28,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
|
|||
std::string fname = Compile_bzz(bzzfile_name);
|
||||
bcfname = fname + ".bo";
|
||||
dbgfname = fname + ".bdb";
|
||||
set_bzz_file(bzzfile_name.c_str());
|
||||
buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
|
||||
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
|
||||
// Initialize variables
|
||||
SetMode("LOITER", 0);
|
||||
|
|
@ -60,6 +61,24 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
|
|||
{
|
||||
robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
|
||||
}
|
||||
// time sync algo. parameter intialization
|
||||
previous_step_time.fromNSec(ros::Time::now().toNSec());
|
||||
logical_clock.fromSec(0);
|
||||
logical_time_rate = 0;
|
||||
time_sync_jumped = false;
|
||||
out_msg_time=0;
|
||||
// Create log dir and open log file
|
||||
std::string path =
|
||||
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
|
||||
std::string folder_check="mkdir -p "+path;
|
||||
system(folder_check.c_str());
|
||||
for(int i=5;i>0;i--){
|
||||
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
|
||||
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
|
||||
}
|
||||
path += "logger_"+std::to_string(robot_id)+"_0.log";
|
||||
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
|
||||
}
|
||||
|
||||
roscontroller::~roscontroller()
|
||||
|
|
@ -69,6 +88,8 @@ roscontroller::~roscontroller()
|
|||
{
|
||||
// Destroy the BVM
|
||||
buzz_utility::buzz_script_destroy();
|
||||
// Close the data logging file
|
||||
log.close();
|
||||
}
|
||||
|
||||
void roscontroller::GetRobotId()
|
||||
|
|
@ -107,20 +128,31 @@ void roscontroller::RosControllerRun()
|
|||
ROS_INFO("[%i] Bytecode file found and set", robot_id);
|
||||
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
|
||||
// Intialize the update monitor
|
||||
init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id);
|
||||
update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id);
|
||||
// set ROS loop rate
|
||||
ros::Rate loop_rate(BUZZRATE);
|
||||
// check for BVMSTATE variable
|
||||
buzzvm_t VM = buzz_utility::get_vm();
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzobj_t obj = buzzvm_stack_at(VM, 1);
|
||||
if(obj->o.type == BUZZTYPE_STRING) statepub_active = 1;
|
||||
else
|
||||
{
|
||||
statepub_active = 0;
|
||||
ROS_ERROR("BVMSTATE undeclared in .bzz file, BVMSTATE pusblisher disabled.");
|
||||
}
|
||||
// DEBUG
|
||||
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done())
|
||||
{
|
||||
// Publish topics
|
||||
neighbours_pos_publisher();
|
||||
uavstate_publisher();
|
||||
if(statepub_active) uavstate_publisher();
|
||||
grid_publisher();
|
||||
send_MPpayload();
|
||||
// Check updater state and step code
|
||||
update_routine();
|
||||
if(update) buzz_update::update_routine();
|
||||
if (time_step == BUZZRATE)
|
||||
{
|
||||
time_step = 0;
|
||||
|
|
@ -138,21 +170,53 @@ void roscontroller::RosControllerRun()
|
|||
}
|
||||
if (debug)
|
||||
ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss);
|
||||
|
||||
// log data
|
||||
// hardware time now
|
||||
log<<ros::Time::now().toNSec()<<",";
|
||||
get_logical_time();
|
||||
// logical time now
|
||||
log<<logical_clock.toNSec()<<",";
|
||||
log<<buzz_utility::get_timesync_itr()<<",";
|
||||
log<<buzz_utility::get_timesync_state()<<",";
|
||||
|
||||
log<<cur_pos.latitude * 100000 << "," << cur_pos.longitude * 100000 << ","
|
||||
<< cur_pos.altitude * 100000 << ",";
|
||||
log << (int)no_of_robots<<",";
|
||||
log << neighbours_pos_map.size()<< ",";
|
||||
log<<(int)inmsgdata.size()<<",";
|
||||
log<< message_number<<",";
|
||||
log<< out_msg_time<<",";
|
||||
log <<buzz_utility::getuavstate();
|
||||
// if(neighbours_pos_map.size() > 0)log<<",";
|
||||
map<int, buzz_utility::Pos_struct>::iterator it =
|
||||
neighbours_pos_map.begin();
|
||||
for (; it != neighbours_pos_map.end(); ++it)
|
||||
{
|
||||
log<<","<< it->first<<",";
|
||||
log<< (double)it->second.x << "," << (double)it->second.y
|
||||
<< "," << (double)it->second.z;
|
||||
}
|
||||
for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){
|
||||
log<<","<<(int)it->nid <<","<<(int)it->msgid<<","<<(int)it->size<<","<<it->sent_time
|
||||
<<","<<it->received_time;
|
||||
}
|
||||
inmsgdata.clear();
|
||||
log<<std::endl;
|
||||
|
||||
// time_sync_step();
|
||||
// if(debug)
|
||||
// ROS_INFO("[Debug : %i ] logical Time %f , har time %f",robot_id, logical_clock.toSec(), ros::Time::now().toSec());
|
||||
// Call Step from buzz script
|
||||
buzz_utility::buzz_script_step();
|
||||
// Prepare messages and publish them
|
||||
prepare_msg_and_publish();
|
||||
// Call the flight controler service
|
||||
flight_controller_service_call();
|
||||
// Set multi message available after update
|
||||
if (get_update_status())
|
||||
{
|
||||
set_read_update_status();
|
||||
}
|
||||
// Set ROBOTS variable (swarm size)
|
||||
get_number_of_robots();
|
||||
buzz_utility::set_robot_var(no_of_robots);
|
||||
updates_set_robots(no_of_robots);
|
||||
if(update) buzz_update::updates_set_robots(no_of_robots);
|
||||
// get_xbee_status(); // commented out because it may slow down the node too much, to be tested
|
||||
|
||||
ros::spinOnce();
|
||||
|
|
@ -328,12 +392,12 @@ 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, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, 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);
|
||||
neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS);
|
||||
uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
|
||||
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
|
||||
localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
|
||||
|
|
@ -448,7 +512,7 @@ void roscontroller::uavstate_publisher()
|
|||
/----------------------------------------------------*/
|
||||
{
|
||||
std_msgs::String uavstate_msg;
|
||||
uavstate_msg.data = buzzuav_closures::getuavstate();
|
||||
uavstate_msg.data = buzz_utility::getuavstate();
|
||||
uavstate_pub.publish(uavstate_msg);
|
||||
}
|
||||
|
||||
|
|
@ -539,33 +603,68 @@ with size......... | /
|
|||
tmp[2] = cur_pos.altitude;
|
||||
memcpy(position, tmp, 3 * sizeof(uint64_t));
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
payload_out.payload64.push_back(XBEE_MESSAGE_CONSTANT);
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
payload_out.payload64.push_back(position[1]);
|
||||
payload_out.payload64.push_back(position[2]);
|
||||
// Add Robot id and message number to the published message
|
||||
if (message_number < 0)
|
||||
message_number = 0;
|
||||
else
|
||||
message_number++;
|
||||
|
||||
//header variables
|
||||
uint16_t tmphead[4];
|
||||
tmphead[1] = (uint16_t)message_number;
|
||||
get_logical_time();
|
||||
uint32_t stime = (uint32_t)(ros::Time::now().toSec()); //(uint32_t)(logical_clock.toSec() * 100000);
|
||||
memcpy((void*)(tmphead+2),(void*)&stime,sizeof(uint32_t));
|
||||
uint64_t rheader[1];
|
||||
rheader[0]=0;
|
||||
payload_out.sysid = (uint8_t)robot_id;
|
||||
payload_out.msgid = (uint32_t)message_number;
|
||||
|
||||
if(buzz_utility::get_timesync_state()){
|
||||
// prepare rosbuzz msg header
|
||||
tmphead[0] = (uint8_t)BUZZ_MESSAGE_TIME;
|
||||
memcpy(rheader, tmphead, sizeof(uint64_t));
|
||||
// push header into the buffer
|
||||
payload_out.payload64.push_back(rheader[0]);
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
payload_out.payload64.push_back(position[1]);
|
||||
payload_out.payload64.push_back(position[2]);
|
||||
//payload_out.payload64.push_back((uint64_t)message_number);
|
||||
// add time sync algo data
|
||||
payload_out.payload64.push_back(ros::Time::now().toNSec());
|
||||
payload_out.payload64.push_back(logical_clock.toNSec());
|
||||
//uint64_t ltrate64 = 0;
|
||||
//memcpy((void*)<rate64, (void*)&logical_time_rate, sizeof(uint64_t));
|
||||
//payload_out.payload64.push_back(ltrate64);
|
||||
}
|
||||
else{
|
||||
// prepare rosbuzz msg header
|
||||
tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
|
||||
memcpy(rheader, tmphead, sizeof(uint64_t));
|
||||
// push header into the buffer
|
||||
payload_out.payload64.push_back(rheader[0]);
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
payload_out.payload64.push_back(position[1]);
|
||||
payload_out.payload64.push_back(position[2]);
|
||||
}
|
||||
// Append Buzz message
|
||||
uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);
|
||||
for (int i = 0; i < out[0]; i++)
|
||||
{
|
||||
payload_out.payload64.push_back(payload_out_ptr[i]);
|
||||
}
|
||||
// Add Robot id and message number to the published message
|
||||
if (message_number < 0)
|
||||
message_number = 0;
|
||||
else
|
||||
message_number++;
|
||||
payload_out.sysid = (uint8_t)robot_id;
|
||||
payload_out.msgid = (uint32_t)message_number;
|
||||
|
||||
//Store out msg time
|
||||
get_logical_time();
|
||||
out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec();
|
||||
// publish prepared messages in respective topic
|
||||
payload_pub.publish(payload_out);
|
||||
delete[] out;
|
||||
delete[] payload_out_ptr;
|
||||
// Check for updater message if present send
|
||||
if (is_msg_present())
|
||||
/// Check for updater message if present send
|
||||
if (update && buzz_update::is_msg_present())
|
||||
{
|
||||
uint8_t* buff_send = 0;
|
||||
uint16_t updater_msgSize = *(uint16_t*)(getupdate_out_msg_size());
|
||||
uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size());
|
||||
;
|
||||
int tot = 0;
|
||||
mavros_msgs::Mavlink update_packets;
|
||||
|
|
@ -578,16 +677,16 @@ with size......... | /
|
|||
*(uint16_t*)(buff_send + tot) = updater_msgSize;
|
||||
tot += sizeof(uint16_t);
|
||||
// Append updater msgs
|
||||
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
|
||||
memcpy(buff_send + tot, (uint8_t*)(buzz_update::getupdater_out_msg()), updater_msgSize);
|
||||
tot += updater_msgSize;
|
||||
// Destroy the updater out msg queue
|
||||
destroy_out_msg_queue();
|
||||
buzz_update::destroy_out_msg_queue();
|
||||
uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t)));
|
||||
uint64_t* payload_64 = new uint64_t[total_size];
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
|
||||
free(buff_send);
|
||||
// Send a constant number to differenciate updater msgs
|
||||
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
|
||||
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE);
|
||||
for (int i = 0; i < total_size; i++)
|
||||
{
|
||||
update_packets.payload64.push_back(payload_64[i]);
|
||||
|
|
@ -682,7 +781,11 @@ script
|
|||
}
|
||||
else
|
||||
ROS_ERROR("Failed to call service from flight controller");
|
||||
#ifdef MAVROSKINETIC
|
||||
cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START;
|
||||
#else
|
||||
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
|
||||
#endif
|
||||
if (mav_client.call(cmd_srv))
|
||||
{
|
||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||
|
|
@ -720,7 +823,11 @@ script
|
|||
cmd_srv.request.param2 = gimbal[1];
|
||||
cmd_srv.request.param3 = gimbal[2];
|
||||
cmd_srv.request.param4 = gimbal[3];
|
||||
#ifdef MAVROSKINETIC
|
||||
cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
|
||||
#else
|
||||
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
|
||||
#endif
|
||||
if (mav_client.call(cmd_srv))
|
||||
{
|
||||
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
|
||||
|
|
@ -750,6 +857,7 @@ void roscontroller::maintain_pos(int tim_step)
|
|||
if (timer_step >= BUZZRATE)
|
||||
{
|
||||
neighbours_pos_map.clear();
|
||||
buzzuav_closures::clear_neighbours_pos();
|
||||
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
|
||||
// have to clear !
|
||||
timer_step = 0;
|
||||
|
|
@ -836,12 +944,12 @@ void roscontroller::gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon
|
|||
ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(gps_t_lat));
|
||||
};
|
||||
|
||||
void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg)
|
||||
void roscontroller::battery(const sensor_msgs::BatteryState::ConstPtr& msg)
|
||||
/*
|
||||
/ Update battery status into BVM from subscriber
|
||||
/------------------------------------------------------*/
|
||||
{
|
||||
buzzuav_closures::set_battery(msg->voltage, msg->current, msg->remaining);
|
||||
buzzuav_closures::set_battery(msg->voltage, msg->current, msg->percentage);
|
||||
// DEBUG
|
||||
// ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage,
|
||||
// msg->current, msg ->remaining);
|
||||
|
|
@ -888,6 +996,8 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
|
|||
{
|
||||
cur_pos.x = msg->pose.position.x;
|
||||
cur_pos.y = msg->pose.position.y;
|
||||
|
||||
buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
|
||||
// cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
|
||||
tf::Quaternion q(
|
||||
msg->pose.orientation.x,
|
||||
|
|
@ -948,9 +1058,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
|
|||
moveMsg.pose.orientation.w = q[3];
|
||||
|
||||
// To prevent drifting from stable position, uncomment
|
||||
// if(fabs(x)>0.005 || fabs(y)>0.005) {
|
||||
localsetpoint_nonraw_pub.publish(moveMsg);
|
||||
// }
|
||||
if(fabs(x)>0.005 || fabs(y)>0.005) {
|
||||
localsetpoint_nonraw_pub.publish(moveMsg);
|
||||
}
|
||||
}
|
||||
|
||||
void roscontroller::SetMode(std::string mode, int delay_miliseconds)
|
||||
|
|
@ -994,8 +1104,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();
|
||||
}
|
||||
// DEBUG
|
||||
// ROS_INFO("Set stream rate call successful");
|
||||
ROS_WARN("Set stream rate call successful");
|
||||
}
|
||||
|
||||
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
||||
|
|
@ -1012,8 +1121,18 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
|||
|
||||
-----------------------------------------------------------------------------------------------------*/
|
||||
{
|
||||
// decode msg header
|
||||
uint64_t rhead = msg->payload64[0];
|
||||
uint16_t r16head[4];
|
||||
memcpy(r16head,&rhead, sizeof(uint64_t));
|
||||
uint16_t mtype = r16head[0];
|
||||
uint16_t mid = r16head[1];
|
||||
uint32_t temptime=0;
|
||||
memcpy(&temptime, r16head+2, sizeof(uint32_t));
|
||||
float stime = (float)temptime/(float)100000;
|
||||
// if(debug) ROS_INFO("Received Msg: sent time %f for id %u",stime, mid);
|
||||
// Check for Updater message, if updater message push it into updater FIFO
|
||||
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT)
|
||||
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE)
|
||||
{
|
||||
uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
|
||||
uint64_t message_obt[obt_msg_size];
|
||||
|
|
@ -1031,13 +1150,14 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
|||
fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
|
||||
if (unMsgSize > 0)
|
||||
{
|
||||
code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
|
||||
code_message_inqueue_process();
|
||||
buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
|
||||
buzz_update::code_message_inqueue_process();
|
||||
}
|
||||
free(pl);
|
||||
}
|
||||
// BVM FIFO message
|
||||
else if (msg->payload64[0] == (uint64_t)XBEE_MESSAGE_CONSTANT)
|
||||
else if ((int)mtype == (int)BUZZ_MESSAGE_TIME ||
|
||||
(int)mtype == (int)BUZZ_MESSAGE_WTO_TIME)
|
||||
{
|
||||
uint64_t message_obt[msg->payload64.size() - 1];
|
||||
// Go throught the obtained payload
|
||||
|
|
@ -1056,8 +1176,15 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
|||
nei_pos.altitude = neighbours_pos_payload[2];
|
||||
double cvt_neighbours_pos_payload[3];
|
||||
gps_rb(nei_pos, cvt_neighbours_pos_payload);
|
||||
int index = 3;
|
||||
if((int)mtype == (int)BUZZ_MESSAGE_TIME) index = 5;
|
||||
// Extract robot id of the neighbour
|
||||
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));
|
||||
uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + index));
|
||||
//get_logical_time();
|
||||
// store in msg data for data log
|
||||
msg_data inm(mid,out[1],out[0]*sizeof(uint64_t),stime,ros::Time::now().toNSec());
|
||||
inmsgdata.push_back(inm);
|
||||
|
||||
if (debug)
|
||||
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
|
||||
|
|
@ -1068,8 +1195,17 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
|||
// TODO: remove roscontroller local map array for neighbors
|
||||
neighbours_pos_put((int)out[1], n_pos);
|
||||
buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z);
|
||||
delete[] out;
|
||||
buzz_utility::in_msg_append((message_obt + 3));
|
||||
if((int)mtype == (int)BUZZ_MESSAGE_TIME){
|
||||
// update time struct for sync algo
|
||||
double nr = 0;
|
||||
push_timesync_nei_msg((int)out[1], message_obt[3],message_obt[4],nr);
|
||||
delete[] out;
|
||||
buzz_utility::in_msg_append((message_obt + index));
|
||||
}
|
||||
else{
|
||||
delete[] out;
|
||||
buzz_utility::in_msg_append((message_obt + index));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1094,8 +1230,13 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
|
|||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
#ifdef MAVROSKINETIC
|
||||
case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
|
||||
rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
|
||||
#else
|
||||
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
|
||||
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
|
||||
#endif
|
||||
armstate = req.param1;
|
||||
if (armstate)
|
||||
{
|
||||
|
|
@ -1123,10 +1264,18 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
|
|||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
#ifdef MAVROSKINETIC
|
||||
case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
|
||||
#else
|
||||
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
|
||||
#endif
|
||||
ROS_INFO("RC_Call: Gimbal!!!! ");
|
||||
buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5);
|
||||
#ifdef MAVROSKINETIC
|
||||
rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
|
||||
#else
|
||||
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
|
||||
#endif
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
|
|
@ -1135,9 +1284,15 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
|
|||
buzzuav_closures::rc_call(rc_cmd);
|
||||
res.success = true;
|
||||
break;
|
||||
case CMD_SYNC_CLOCK:
|
||||
rc_cmd = CMD_SYNC_CLOCK;
|
||||
buzzuav_closures::rc_call(rc_cmd);
|
||||
ROS_INFO("<---------------- Time Sync requested ----------->");
|
||||
res.success = true;
|
||||
break;
|
||||
default:
|
||||
buzzuav_closures::rc_call(req.command);
|
||||
ROS_INFO("----> Received unregistered command: ", req.command);
|
||||
ROS_ERROR("----> Received unregistered command: ", req.command);
|
||||
res.success = true;
|
||||
break;
|
||||
}
|
||||
|
|
@ -1326,4 +1481,85 @@ void roscontroller::get_xbee_status()
|
|||
* TriggerAPIRssi(all_ids);
|
||||
*/
|
||||
}
|
||||
|
||||
void roscontroller::time_sync_step()
|
||||
/*
|
||||
* Steps the time syncronization algorithm
|
||||
------------------------------------------------------------------ */
|
||||
{
|
||||
//ROS_INFO("Stepping time sync : %f ", logical_clock.toSec());
|
||||
double avgRate = logical_time_rate;
|
||||
double avgOffset = 0;
|
||||
int offsetCount = 0;
|
||||
map<int, buzz_utility::neighbor_time>::iterator it;
|
||||
for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); )
|
||||
{
|
||||
avgRate += (it->second).relative_rate;
|
||||
// estimate current offset
|
||||
int64_t offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time;
|
||||
avgOffset = avgOffset + offset;
|
||||
offsetCount++;
|
||||
if((it->second).age > BUZZRATE){
|
||||
neighbours_time_map.erase(it++);
|
||||
}
|
||||
else{
|
||||
(it->second).age++;
|
||||
++it;
|
||||
}
|
||||
ROS_INFO("Size of nei time %i",neighbours_time_map.size());
|
||||
}
|
||||
avgRate = avgRate/(neighbours_time_map.size()+1);
|
||||
if(offsetCount>0 && !time_sync_jumped){
|
||||
int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1));
|
||||
if(correction < TIME_SYNC_JUMP_THR && correction > 0 ){
|
||||
set_logical_time_correction(correction);
|
||||
}
|
||||
}
|
||||
get_logical_time(); // just to update clock
|
||||
time_sync_jumped = false;
|
||||
//neighbours_time_map.clear();
|
||||
logical_time_rate = avgRate;
|
||||
|
||||
}
|
||||
void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr)
|
||||
/*
|
||||
* pushes a time syncronization msg into its data slot
|
||||
------------------------------------------------------------------ */
|
||||
{
|
||||
map<int, buzz_utility::neighbor_time>::iterator it = neighbours_time_map.find(nid);
|
||||
double relativeRate =1;
|
||||
if (it != neighbours_time_map.end()){
|
||||
int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time;
|
||||
int64_t delatNei = round(nh - (it->second).nei_hardware_time);
|
||||
double currentRate = 0;
|
||||
if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal;
|
||||
relativeRate = MOVING_AVERAGE_ALPHA*((it->second).relative_rate)
|
||||
+ (1- MOVING_AVERAGE_ALPHA)*currentRate;
|
||||
|
||||
ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f",
|
||||
deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate);
|
||||
neighbours_time_map.erase(it);
|
||||
}
|
||||
int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec();
|
||||
if(offset > TIME_SYNC_JUMP_THR){
|
||||
set_logical_time_correction(offset);// + time_diff * logical_time_rate );
|
||||
time_sync_jumped = true;
|
||||
}
|
||||
buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(),
|
||||
logical_clock.toNSec(), nr, relativeRate);
|
||||
nt.age=0;
|
||||
neighbours_time_map.insert(make_pair(nid, nt));
|
||||
}
|
||||
|
||||
uint64_t roscontroller::get_logical_time(){
|
||||
uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec();
|
||||
uint64_t old_time = logical_clock.toNSec();
|
||||
logical_clock.fromNSec(old_time + time_diff);// + time_diff * logical_time_rate);
|
||||
previous_step_time.fromNSec(ros::Time::now().toNSec());
|
||||
return logical_clock.toNSec();
|
||||
}
|
||||
void roscontroller::set_logical_time_correction(uint64_t cor){
|
||||
uint64_t l_time_now = get_logical_time();
|
||||
logical_clock.fromNSec(l_time_now + cor);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue