merge dev

This commit is contained in:
dave 2018-09-06 12:47:38 -04:00
parent a99d2a6496
commit d4ee8ef81e
27 changed files with 1654 additions and 961 deletions

View File

@ -6,9 +6,9 @@ if(UNIX)
endif() endif()
if(SIM) if(SIM)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1")
else() else()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1")
endif() endif()
## Find catkin macros and libraries ## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS

View File

@ -7,10 +7,11 @@
# #
# Constants # Constants
# #
BARRIER_TIMEOUT = 1200 # in steps BARRIER_TIMEOUT = 200 # in steps
BARRIER_VSTIG = 80 BARRIER_VSTIG = 80
timeW = 0 timeW = 0
barrier = nil barrier = nil
bc = 0;
# #
# Sets a barrier # Sets a barrier
@ -22,15 +23,15 @@ function barrier_create() {
#log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG) #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
if(barrier!=nil) { if(barrier!=nil) {
barrier=nil barrier=nil
BARRIER_VSTIG = BARRIER_VSTIG +1 # BARRIER_VSTIG = BARRIER_VSTIG +1
} }
#log("---> New. br. ", barrier, " ", BARRIER_VSTIG) #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
barrier = stigmergy.create(BARRIER_VSTIG) barrier = stigmergy.create(BARRIER_VSTIG)
} }
function barrier_set(threshold, transf, resumef) { function barrier_set(threshold, transf, resumef, bc) {
statef = function() { statef = function() {
barrier_wait(threshold, transf, resumef); barrier_wait(threshold, transf, resumef, bc);
} }
BVMSTATE = "BARRIERWAIT" BVMSTATE = "BARRIERWAIT"
barrier_create() barrier_create()
@ -39,30 +40,51 @@ function barrier_set(threshold, transf, resumef) {
# #
# Make yourself ready # Make yourself ready
# #
function barrier_ready() { function barrier_ready(bc) {
#log("BARRIER READY -------") #log("BARRIER READY -------")
barrier.put(id, 1) barrier.put(id, bc)
barrier.put("d", 0) barrier.put("d", 0)
} }
# #
# Executes the barrier # Executes the barrier
# #
function barrier_wait(threshold, transf, resumef) { function barrier_wait(threshold, transf, resumef, bc) {
barrier.put(id, 1) barrier.put(id, bc)
barrier.get(id) 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) { if(barrier.size() - 1 >= threshold or barrier.get("d") == 1) {
allgood = barrier_allgood(barrier,bc)
}
if(allgood) {
barrier.put("d", 1) barrier.put("d", 1)
timeW = 0 timeW = 0
BVMSTATE = transf BVMSTATE = transf
} else if(timeW >= BARRIER_TIMEOUT) { } else if(timeW >= BARRIER_TIMEOUT) {
log("------> Barrier Timeout !!!!") log("------> Barrier Timeout !!!!")
barrier=nil barrier = nil
timeW = 0 timeW = 0
BVMSTATE = resumef BVMSTATE = resumef
} } else if(timeW % 100 == 0 and bc > 0)
neighbors.broadcast("cmd", bc)
timeW = timeW+1 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
}

View File

@ -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
}

View File

@ -5,17 +5,19 @@
######################################## ########################################
include "utils/vec2.bzz" include "utils/vec2.bzz"
include "act/barrier.bzz" include "act/barrier.bzz"
#include "act/old_barrier.bzz"
include "utils/conversions.bzz" include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 2.5 # m/steps
GOTO_MAXVEL = 0.5 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 0.5 # m. GOTODIST_TOL = 1.0 # m.
GOTOANG_TOL = 0.1 # rad. GOTOANG_TOL = 0.1 # rad.
path_it = 0 path_it = 0
graphid = 0 graphid = 3
pic_time = 0 pic_time = 0
g_it = 0 g_it = 0
@ -27,44 +29,72 @@ function idle() {
BVMSTATE = "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() { function launch() {
BVMSTATE = "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} homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) { if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready() barrier_ready(22)
} else { } else {
log("Altitude: ", pose.position.altitude) log("Altitude: ", pose.position.altitude)
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE) uav_takeoff(TARGET_ALTITUDE)
} }
} else { } else {
barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "LAUNCH") barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
barrier_ready() 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() { function stop() {
BVMSTATE = "STOP" BVMSTATE = "STOP"
if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND if(V_TYPE == 0 or V_TYPE == 1) { # flying vehicle so LAND
neighbors.broadcast("cmd", 21) neighbors.broadcast("cmd", 21)
uav_land() uav_land()
if(flight.status != 2 and flight.status != 3) { if(flight.status != 2 and flight.status != 3) {
barrier_set(ROBOTS,"TURNEDOFF","STOP") BVMSTATE = "TURNEDOFF"
barrier_ready() #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
#barrier_ready(21)
} }
} else { } else {
barrier_set(ROBOTS,"TURNEDOFF","STOP") BVMSTATE = "TURNEDOFF"
barrier_ready() #barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
} #barrier_ready(21)
}
} }
function take_picture() { function take_picture() {
BVMSTATE="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 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 } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
BVMSTATE="IDLE" BVMSTATE="IDLE"
pic_time=0 pic_time=0
@ -73,6 +103,7 @@ function take_picture() {
} }
function goto_gps(transf) { 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) 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)) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) 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 else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf() transf()
else { 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) 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 # circle all together around centroid
function pursuit() { function pursuit() {
BVMSTATE="PURSUIT" BVMSTATE="PURSUIT"
rd = 15.0 rd = 20.0
pc = 3.2 pc = 3.2
vd = 15.0 vd = 15.0
r_vec = neighbors.reduce(function(rid, data, r_vec) { r_vec = neighbors.reduce(function(rid, data, r_vec) {
@ -127,7 +158,8 @@ function pursuit() {
if(neighbors.count() > 0) if(neighbors.count() > 0)
r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count()) r_vec = math.vec2.scale(r_vec, 1.0 / neighbors.count())
r = math.vec2.length(r_vec) 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) dr = -gamma * (r-rd)
dT = gamma * pc dT = gamma * pc
vfg = math.vec2.newp(r+dr*0.1, math.vec2.angle(r_vec)+dT*0.1) 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 # Lennard-Jones interaction magnitude
TARGET = 8.0 TARGET = 8.0
EPSILON = 3.0 EPSILON = 30.0
function lj_magnitude(dist, target, epsilon) { 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 return lj
} }
@ -154,92 +186,130 @@ function lj_sum(rid, data, accum) {
} }
# Calculates and actuates the flocking interaction # Calculates and actuates the flocking interaction
<<<<<<< HEAD
old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes old_lj = math.vec2.newp(0.0, 0.0) # 1-step filter, to smoothen dir. changes
=======
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
function formation() { function formation() {
BVMSTATE="FORMATION" BVMSTATE="FORMATION"
# Calculate accumulator # Calculate accumulator
accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0)) accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
if(neighbors.count() > 0) if(neighbors.count() > 0)
accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count()) 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) accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
goto_abs(old_lj.x, old_lj.y, 0.0, 0.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() { function rc_cmd_listen() {
if(flight.rc_cmd==22) { if(flight.rc_cmd==22) {
log("cmd 22") log("cmd 22")
flight.rc_cmd=0 flight.rc_cmd=0
BVMSTATE = "LAUNCH" BVMSTATE = "LAUNCH"
neighbors.broadcast("cmd", 22) neighbors.broadcast("cmd", 22)
} else if(flight.rc_cmd==21) { } else if(flight.rc_cmd==21) {
log("cmd 21")
flight.rc_cmd=0 flight.rc_cmd=0
AUTO_LAUNCH_STATE = "TURNEDOFF"
#barrier_set(ROBOTS, "GOHOME", BVMSTATE, 21)
#barrier_ready(21)
BVMSTATE = "STOP" BVMSTATE = "STOP"
neighbors.broadcast("cmd", 21) 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) { } else if(flight.rc_cmd==16) {
flight.rc_cmd=0 flight.rc_cmd=0
BVMSTATE = "PATHPLAN" BVMSTATE = "PATHPLAN"
} else if(flight.rc_cmd==400) { } else if(flight.rc_cmd==400) {
flight.rc_cmd=0 flight.rc_cmd=0
uav_arm() arm()
neighbors.broadcast("cmd", 400) neighbors.broadcast("cmd", 400)
} else if (flight.rc_cmd==401){ } else if (flight.rc_cmd==401){
flight.rc_cmd=0 flight.rc_cmd=0
uav_disarm() disarm()
neighbors.broadcast("cmd", 401) neighbors.broadcast("cmd", 401)
} else if (flight.rc_cmd==666){ } else if (flight.rc_cmd==666){
flight.rc_cmd=0 flight.rc_cmd=0
stattab_send() stattab_send()
} else if (flight.rc_cmd==900){ } else if (flight.rc_cmd==777){
flight.rc_cmd=0 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) neighbors.broadcast("cmd", 900)
} else if (flight.rc_cmd==901){ } else if (flight.rc_cmd==901){
flight.rc_cmd=0 flight.rc_cmd=0
destroyGraph() destroyGraph()
BVMSTATE = "PURSUIT" barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
barrier_ready(901)
neighbors.broadcast("cmd", 901) neighbors.broadcast("cmd", 901)
} else if (flight.rc_cmd==902){ } else if (flight.rc_cmd==902){
flight.rc_cmd=0 flight.rc_cmd=0
destroyGraph() destroyGraph()
BVMSTATE = "AGGREGATE" barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
barrier_ready(902)
neighbors.broadcast("cmd", 902) neighbors.broadcast("cmd", 902)
} else if (flight.rc_cmd==903){ } else if (flight.rc_cmd==903){
flight.rc_cmd=0 flight.rc_cmd=0
destroyGraph() destroyGraph()
BVMSTATE = "FORMATION" barrier_set(ROBOTS, "FORMATION", BVMSTATE, 903)
barrier_ready(903)
neighbors.broadcast("cmd", 903) neighbors.broadcast("cmd", 903)
} }
} }
# listens to other fleet members broadcasting commands
function nei_cmd_listen() { function nei_cmd_listen() {
neighbors.listen("cmd", neighbors.listen("cmd",
function(vid, value, rid) { function(vid, value, rid) {
print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")") print("Got (", vid, ",", value, ") #", rid, "(", BVMSTATE, ")")
if(value==22 and BVMSTATE!="BARRIERWAIT") { if(BVMSTATE!="BARRIERWAIT") {
BVMSTATE = "LAUNCH" if(value==22) {
} else if(value==21 and BVMSTATE!="BARRIERWAIT") { BVMSTATE = "LAUNCH"
BVMSTATE = "STOP" }else if(value==20) {
} else if(value==400 and BVMSTATE=="TURNEDOFF") { AUTO_LAUNCH_STATE = "IDLE"
uav_arm() BVMSTATE = "GOHOME"
} else if(value==401 and BVMSTATE=="TURNEDOFF"){ } else if(value==21) {
uav_disarm() AUTO_LAUNCH_STATE = "TURNEDOFF"
} else if(value==900){ # Shapes BVMSTATE = "STOP"
BVMSTATE = "TASK_ALLOCATE" } else if(value==400 and BVMSTATE=="TURNEDOFF") {
} else if(value==901){ # Pursuit arm()
destroyGraph() } else if(value==401 and BVMSTATE=="TURNEDOFF"){
BVMSTATE = "PURSUIT" disarm()
} else if(value==902){ # Agreggate } else if(value==777 and BVMSTATE=="TURNEDOFF"){
destroyGraph() reinit_time_sync()
BVMSTATE = "AGGREGATE" #neighbors.broadcast("cmd", 777)
} else if(value==903){ # Formation }else if(value==900){ # Shapes
destroyGraph() barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
BVMSTATE = "FORMATION" #barrier_ready(900)
} else if(value==16 and BVMSTATE=="IDLE"){ neighbors.broadcast("cmd", 900)
# neighbors.listen("gt",function(vid, value, rid) { } else if(value==901){ # Pursuit
# print("Got (", vid, ",", value, ") from robot #", rid) destroyGraph()
# # if(gt.id == id) statef=goto 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
# })
}
} }
}) })
} }

View File

@ -20,7 +20,7 @@ mapgoal = {}
function navigate() { function navigate() {
BVMSTATE="NAVIGATE" BVMSTATE="NAVIGATE"
if(V_TYPE == 0) { # TODO: place these in a generic function in conversion.bzz 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) 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) m_pos = math.vec2.scale(vec_from_gps(homegps.lat, homegps.long, 0),-1)
} }

View File

@ -10,7 +10,7 @@ include "taskallocate/graphs/shapes_Y.bzz"
ROBOT_RADIUS = 50 ROBOT_RADIUS = 50
ROBOT_DIAMETER = 2.0*ROBOT_RADIUS ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
ROOT_ID = 2 ROOT_ID = 5
# #
# Global variables # Global variables
@ -389,7 +389,7 @@ function TransitionToJoined(){
#write statues #write statues
#v_tag.put(m_nLabel, m_lockstig) #v_tag.put(m_nLabel, m_lockstig)
barrier_create() barrier_create()
barrier_ready() barrier_ready(940)
m_navigation.x=0.0 m_navigation.x=0.0
m_navigation.y=0.0 m_navigation.y=0.0
@ -537,7 +537,7 @@ function DoJoining(){
if(m_gotjoinedparent!=1) if(m_gotjoinedparent!=1)
set_rc_goto() set_rc_goto()
else else
goto_gps(TransitionToJoined) goto_gps(TransitionToJoined)
#pack the communication package #pack the communication package
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)
@ -655,8 +655,7 @@ function DoJoined(){
return return
} }
} }
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941)
barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
BroadcastGraph() BroadcastGraph()
} }
# #
@ -678,20 +677,21 @@ function DoLock() {
m_navigation=motion_vector() m_navigation=motion_vector()
} }
# #move # #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() BroadcastGraph()
if(loop) { # if(loop) {
if(timeout_graph==0) { # if(timeout_graph==0) {
if(graphid < 3) # if(graphid < 3)
graphid = graphid + 1 # graphid = graphid + 1
else # else
graphid = 0 # graphid = 0
timeout_graph = 40 # timeout_graph = 40
BVMSTATE="TASK_ALLOCATE" # BVMSTATE="TASK_ALLOCATE"
} # }
timeout_graph = timeout_graph - 1 # timeout_graph = timeout_graph - 1
} # }
} }
# #

View File

@ -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
}

View File

@ -4,6 +4,7 @@
#################################################################################################### ####################################################################################################
updated="update_ack" updated="update_ack"
update_no=0 update_no=0
function updated_neigh(){ updates_active = 1
function updated_no_bct(){
neighbors.broadcast(updated, update_no) neighbors.broadcast(updated, update_no)
} }

View File

@ -66,4 +66,4 @@ function gps_from_vec(vec) {
#goal.longitude = d_lon + pose.position.longitude; #goal.longitude = d_lon + pose.position.longitude;
return Lgoal return Lgoal
} }

View File

@ -17,10 +17,10 @@ string.charat = function(s, n) {
# RETURN: The position of the first match, or nil # RETURN: The position of the first match, or nil
# #
string.indexoffirst = function(s, m) { string.indexoffirst = function(s, m) {
var ls = string.length(s) var las = string.length(s)
var lm = string.length(m) var lm = string.length(m)
var i = 0 var i = 0
while(i < ls-lm+1) { while(i < las-lm+1) {
if(string.sub(s, i, i+lm) == m) return i if(string.sub(s, i, i+lm) == m) return i
i = i + 1 i = i + 1
} }
@ -36,9 +36,9 @@ string.indexoffirst = function(s, m) {
# RETURN: The position of the last match, or nil # RETURN: The position of the last match, or nil
# #
string.indexoflast = function(s, m) { string.indexoflast = function(s, m) {
var ls = string.length(s) var las = string.length(s)
var lm = string.length(m) var lm = string.length(m)
var i = ls - lm + 1 var i = las - lm + 1
while(i >= 0) { while(i >= 0) {
if(string.sub(s, i, i+lm) == m) return i if(string.sub(s, i, i+lm) == m) return i
i = i - 1 i = i - 1
@ -56,10 +56,10 @@ string.split = function(s, d) {
var i2 = 0 # index to move along s (token end) var i2 = 0 # index to move along s (token end)
var c = 0 # token count var c = 0 # token count
var t = {} # token list var t = {} # token list
var ls = string.length(s) var las = string.length(s)
var ld = string.length(d) var ld = string.length(d)
# Go through string s # Go through string s
while(i2 < ls) { while(i2 < las) {
# Try every delimiter # Try every delimiter
var j = 0 # index to move along d var j = 0 # index to move along d
var f = nil # whether the delimiter was found or not var f = nil # whether the delimiter was found or not

View File

@ -5,9 +5,15 @@ include "act/states.bzz"
include "plan/rrtstar.bzz" include "plan/rrtstar.bzz"
include "taskallocate/graphformGPS.bzz" include "taskallocate/graphformGPS.bzz"
include "vstigenv.bzz" include "vstigenv.bzz"
include "timesync.bzz"
#State launched after takeoff #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: # Vehicule type:
@ -26,7 +32,10 @@ function init() {
init_stig() init_stig()
init_swarm() init_swarm()
TARGET_ALTITUDE = 10 + id*2.0 # m # initGraph()
TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*4.0 # m
loop = 1 loop = 1
# start the swarm command listener # start the swarm command listener
@ -38,6 +47,10 @@ function init() {
# Executed at each time step. # Executed at each time step.
function step() { function step() {
# check time sync algorithm state
check_time_sync()
# listen to Remote Controller # listen to Remote Controller
rc_cmd_listen() rc_cmd_listen()
@ -49,10 +62,14 @@ function step() {
# #
if(BVMSTATE=="TURNEDOFF") if(BVMSTATE=="TURNEDOFF")
statef=turnedoff statef=turnedoff
else if(BVMSTATE=="CUSFUN")
statef=cusfun
else if(BVMSTATE=="STOP") # ends on turnedoff else if(BVMSTATE=="STOP") # ends on turnedoff
statef=stop statef=stop
else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE else if(BVMSTATE=="LAUNCH") # ends on AUTO_LAUNCH_STATE
statef=launch statef=launch
else if(BVMSTATE=="GOHOME") # ends on AUTO_LAUNCH_STATE
statef=goinghome
else if(BVMSTATE=="IDLE") else if(BVMSTATE=="IDLE")
statef=idle statef=idle
else if(BVMSTATE=="AGGREGATE") else if(BVMSTATE=="AGGREGATE")

View File

@ -51,7 +51,6 @@ function step() {
statef=action statef=action
statef() statef()
log("Current state: ", BVMSTATE) log("Current state: ", BVMSTATE)
} }

View File

@ -1,35 +1,38 @@
include "act/states.bzz"
include "vstigenv.bzz"
updated="update_ack" updated="update_ack"
update_no=0 update_no=0
function init(){ BVMSTATE = "UPDATESTANDBY"
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
})
s = swarm.create(1) # Executed once at init time.
s.join() 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(){ function stand_by(){
barrier.get(id)
barrier_val = barrier.size()
barrier.get(id) neighbors.listen(updated,
barrier_val = barrier.size() function(vid, value, rid) {
if(value==update_no) barrier.put(rid,1)
neighbors.listen(updated, }
function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier.put(rid,1)
}
) )
} }
# Executed at each time step.
function step() { function step() {
stand_by()
stand_by()
} }

72
buzz_scripts/testLJ.bzz Executable file
View File

@ -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() {
}

View File

@ -1,161 +1,171 @@
#ifndef BUZZ_UPDATE_H #ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H #define BUZZ_UPDATE_H
/*Simulation or robot check*/
//#define SIMULATION 1 // set in CMAKELIST
#include <stdlib.h> #include <stdlib.h>
#include <stdio.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/buzztype.h>
#include <buzz/buzzdict.h> #include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h> #include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h> #include <buzz/buzzvstig.h>
#include <buzz_utility.h>
#include <fstream> #include <fstream>
#define delete_p(p) \ #define delete_p(p) \
do \ do \
{ \ { \
free(p); \ free(p); \
p = NULL; \ p = NULL; \
} while (0) } while (0)
namespace buzz_update
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
{ {
uint8_t* queue; static const uint16_t CODE_REQUEST_PADDING = 250;
uint8_t* size; static const uint16_t MIN_UPDATE_PACKET = 251;
}; static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
typedef struct updater_msgqueue_s* updater_msgqueue_t; static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/
/* Updater states */
/********************/
struct updater_code_s typedef enum {
{ CODE_RUNNING = 0, // Code executing
uint8_t* bcode; CODE_STANDBY, // Standing by for others to update
uint8_t* bcode_size; } code_states_e;
};
typedef struct updater_code_s* updater_code_t;
/**************************/ /*********************/
/*Updater data*/ /*Message types */
/**************************/ /********************/
struct buzz_updater_elem_s typedef enum {
{ SENT_CODE = 0, // Broadcast code
/* robot id */ RESEND_CODE, // ReBroadcast request
// uint16_t robotid; } code_message_e;
/*current Bytecode content */
uint8_t* bcode;
/*old Bytecode name */
const char* old_bcode;
/*current bcode size*/
size_t* bcode_size;
/*Update patch*/
uint8_t* patch;
/* Update patch size*/
size_t* patch_size;
/*current Bytecode content */
uint8_t* standby_bcode;
/*current bcode size*/
size_t* standby_bcode_size;
/*updater out msg queue */
updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/
updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/ /*************************/
/*Updater routine from msg processing to file checks to be called from main*/ /*Updater message queue */
/**************************************************************************/ /*************************/
void update_routine();
/************************************************/ struct updater_msgqueue_s
/*Initalizes the updater */ {
/************************************************/ uint8_t* queue;
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); uint8_t* size;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
/*********************************************************/ struct updater_code_s
/*Appends buffer of given size to in msg queue of updater*/ {
/*********************************************************/ 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*/
/**************************/
/*********************************************************/ struct buzz_updater_elem_s
/*Processes messages inside the queue of the updater*/ {
/*********************************************************/ /* 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*/ /*Initalizes the updater */
/*******************************************************/ /************************************************/
uint8_t* getupdater_out_msg(); int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/******************************************************/ /*********************************************************/
/*obtains out msg queue size*/ /*Appends buffer of given size to in msg queue of updater*/
/*****************************************************/ /*********************************************************/
uint8_t* getupdate_out_msg_size();
/**************************************************/ void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
/***************************************************/ /*********************************************************/
/*obatins updater state*/ /*Processes messages inside the queue of the updater*/
/***************************************************/ /*********************************************************/
// int get_update_mode();
// buzz_updater_elem_t get_updater(); void code_message_inqueue_process();
/***************************************************/
/*sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); /*****************************************************/
/*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 #endif

View File

@ -43,6 +43,24 @@ struct neiStatus
}; };
typedef struct neiStatus neighbors_status; 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); uint16_t* u64_cvt_u16(uint64_t u64);
int buzz_listen(const char* type, int msg_size); int buzz_listen(const char* type, int msg_size);
@ -77,4 +95,12 @@ buzzvm_t get_vm();
void set_robot_var(int ROBOTS); void set_robot_var(int ROBOTS);
int get_inmsg_size(); int get_inmsg_size();
std::vector<uint8_t*> get_inmsg_vector();
std::string getuavstate();
int get_timesync_state();
int get_timesync_itr();
} }

View File

@ -89,6 +89,10 @@ void set_filtered_packet_loss(float value);
/* /*
* sets current position * sets current position
*/ */
void set_currentNEDpos(double x, double y);
>>>>>>> 064760108611591426d86c679c7789b1a95cebee
void set_currentpos(double latitude, double longitude, float altitude, float yaw); void set_currentpos(double latitude, double longitude, float altitude, float yaw);
/* /*
* returns the current go to position * returns the current go to position
@ -98,10 +102,8 @@ double* getgoto();
* returns the current grid * returns the current grid
*/ */
std::map<int, std::map<int,int>> getgrid(); std::map<int, std::map<int,int>> getgrid();
/*
* returns the current Buzz state
*/
std::string getuavstate();
/* /*
* returns the gimbal commands * 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 * update neighbors from in msgs
*/ */
void update_neighbors(buzzvm_t vm); void update_neighbors(buzzvm_t vm);
/*
*Clear neighbours struct
*/
void clear_neighbours_pos();
/* /*
* closure to add a neighbor status * closure to add a neighbor status
*/ */

View File

@ -12,6 +12,7 @@
#include "mavros_msgs/SetMode.h" #include "mavros_msgs/SetMode.h"
#include "mavros_msgs/State.h" #include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/BatteryStatus.h"
#include "sensor_msgs/BatteryState.h"
#include "mavros_msgs/Mavlink.h" #include "mavros_msgs/Mavlink.h"
#include "mavros_msgs/PositionTarget.h" #include "mavros_msgs/PositionTarget.h"
#include "sensor_msgs/NavSatStatus.h" #include "sensor_msgs/NavSatStatus.h"
@ -34,14 +35,29 @@
#include <signal.h> #include <signal.h>
#include <ostream> #include <ostream>
#include <map> #include <map>
#include <cmath>
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#define UPDATER_MESSAGE_CONSTANT 987654321 /*
#define XBEE_MESSAGE_CONSTANT 586782343 * ROSBuzz message types
#define XBEE_STOP_TRANSMISSION 4355356352 */
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 TIMEOUT 60
#define BUZZRATE 10 #define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666 #define CMD_REQUEST_UPDATE 666
#define CMD_SYNC_CLOCK 777
using namespace std; using namespace std;
@ -83,19 +99,42 @@ private:
}; };
typedef struct POSE ros_pose; 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; ros_pose target, home, cur_pos;
uint64_t payload; uint64_t payload;
std::map<int, buzz_utility::Pos_struct> neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map; std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
int timer_step = 0; int timer_step = 0;
int robot_id = 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 = ""; std::string robot_name = "";
int rc_cmd; int rc_cmd;
float fcu_timeout; float fcu_timeout;
int armstate; int armstate;
int barrier; int barrier;
int update;
int statepub_active;
int message_number = 0; int message_number = 0;
uint8_t no_of_robots = 0; uint8_t no_of_robots = 0;
bool rcclient; bool rcclient;
@ -205,7 +244,7 @@ private:
double gps_r_lat); double gps_r_lat);
/*battery status callback */ /*battery status callback */
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg); void battery(const sensor_msgs::BatteryState::ConstPtr& msg);
/*flight extended status callback*/ /*flight extended status callback*/
void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg); 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); bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
void get_xbee_status(); 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);
}; };
} }

View File

@ -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

View File

@ -3,12 +3,13 @@
<!-- This file is included in all ROS workspace launch files --> <!-- This file is included in all ROS workspace launch files -->
<!-- Modify with great care! --> <!-- Modify with great care! -->
<launch> <launch>
<arg name="name" default="robot0"/> <arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/> <arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/> <arg name="script" default="testalone"/>
<arg name="launch_config" default="topics"/>
<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" > <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="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" /> <param name="rcservice_name" value="buzzcmd" />

View File

@ -6,9 +6,10 @@
<arg name="name" default="robot0"/> <arg name="name" default="robot0"/>
<arg name="xbee_plugged" default="true"/> <arg name="xbee_plugged" default="true"/>
<arg name="script" default="testalone"/> <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"> <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="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
<param name="rcclient" value="true" /> <param name="rcclient" value="true" />
<param name="rcservice_name" value="buzzcmd" /> <param name="rcservice_name" value="buzzcmd" />

View File

@ -11,6 +11,9 @@ function arm {
function disarm { function disarm {
rosservice call $1/buzzcmd 0 400 0 0 0 0 0 0 0 0 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 { function testWP {
rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10 rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
} }
@ -31,7 +34,8 @@ function stoprobot {
} }
function updaterobot { function updaterobot {
# rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch # rosrun robot_upstart install --logdir ~/ROS_WS/log/ robot_upstart/launch/m100buzzynocam.launch
rosrun robot_upstart install --logdir /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 # rosrun robot_upstart install --logdir ~/ROS_WS/log/ dji_sdk_mistlab/launch_robot/m100TXbuzzy.launch
} }
function uavstate { function uavstate {

View File

@ -97,7 +97,7 @@ The package offers a mavros_msgs/CommandLong service "buzzcmd" to control its st
References 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. * 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": { "files.associations": {
"*.bzz":"python" "*.bzz":"python"
} }
``` ```

File diff suppressed because it is too large Load Diff

View File

@ -564,4 +564,56 @@ int get_inmsg_size()
{ {
return IN_MSG.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;
}
} }

View File

@ -19,6 +19,7 @@ static float rc_gimbal[4];
static float batt[3]; static float batt[3];
static float obst[5] = { 0, 0, 0, 0, 0 }; static float obst[5] = { 0, 0, 0, 0, 0 };
static double cur_pos[4]; static double cur_pos[4];
static double cur_NEDpos[2];
static uint8_t status; static uint8_t status;
static int cur_cmd = 0; static int cur_cmd = 0;
static int rc_cmd = 0; static int rc_cmd = 0;
@ -228,8 +229,8 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[2] = height + dh; goto_pos[2] = height + dh;
goto_pos[3] = dyaw; goto_pos[3] = dyaw;
// DEBUG // DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], 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]); goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -400,7 +401,11 @@ int buzzuav_arm(buzzvm_t vm)
/ Buzz closure to arm / Buzz closure to arm
/---------------------------------------*/ /---------------------------------------*/
{ {
#ifdef MAVROSKINETIC
cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
#else
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
#endif
printf(" Buzz requested Arm \n"); printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM; buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
@ -411,7 +416,11 @@ int buzzuav_disarm(buzzvm_t vm)
/ Buzz closure to disarm / 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; cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
#endif
printf(" Buzz requested Disarm \n"); printf(" Buzz requested Disarm \n");
buzz_cmd = COMMAND_DISARM; buzz_cmd = COMMAND_DISARM;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
@ -479,19 +488,6 @@ float* getgimbal()
return rc_gimbal; 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() int getcmd()
/* /*
/ return current mavros command to the FC / return current mavros command to the FC
@ -642,6 +638,15 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
return vm->state; 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) void set_currentpos(double latitude, double longitude, float altitude, float yaw)
/* /*
/ update interface position array / 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) int buzzuav_update_currentpos(buzzvm_t vm)
/* /*
/ Update the BVM position table / 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_pushs(vm, buzzvm_string_register(vm, "altitude", 0));
buzzvm_pushf(vm, cur_pos[2]); buzzvm_pushf(vm, cur_pos[2]);
buzzvm_tput(vm); 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 // Store read table in the proximity table
buzzvm_push(vm, tPoseTable); buzzvm_push(vm, tPoseTable);
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 0));

View File

@ -11,9 +11,10 @@
namespace rosbzz_node namespace rosbzz_node
{ {
const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image"; 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 / 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); std::string fname = Compile_bzz(bzzfile_name);
bcfname = fname + ".bo"; bcfname = fname + ".bo";
dbgfname = fname + ".bdb"; 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("\\/")) + "/"); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
// Initialize variables // Initialize variables
SetMode("LOITER", 0); 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); 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() roscontroller::~roscontroller()
@ -69,6 +88,8 @@ roscontroller::~roscontroller()
{ {
// Destroy the BVM // Destroy the BVM
buzz_utility::buzz_script_destroy(); buzz_utility::buzz_script_destroy();
// Close the data logging file
log.close();
} }
void roscontroller::GetRobotId() void roscontroller::GetRobotId()
@ -107,20 +128,31 @@ void roscontroller::RosControllerRun()
ROS_INFO("[%i] Bytecode file found and set", robot_id); ROS_INFO("[%i] Bytecode file found and set", robot_id);
std::string standby_bo = Compile_bzz(stand_by) + ".bo"; std::string standby_bo = Compile_bzz(stand_by) + ".bo";
// Intialize the update monitor // 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 // set ROS loop rate
ros::Rate loop_rate(BUZZRATE); 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 // DEBUG
// ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id); // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
// Publish topics // Publish topics
neighbours_pos_publisher(); neighbours_pos_publisher();
uavstate_publisher(); if(statepub_active) uavstate_publisher();
grid_publisher(); grid_publisher();
send_MPpayload(); send_MPpayload();
// Check updater state and step code // Check updater state and step code
update_routine(); if(update) buzz_update::update_routine();
if (time_step == BUZZRATE) if (time_step == BUZZRATE)
{ {
time_step = 0; time_step = 0;
@ -138,21 +170,53 @@ void roscontroller::RosControllerRun()
} }
if (debug) if (debug)
ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); 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 // Call Step from buzz script
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
// Prepare messages and publish them // Prepare messages and publish them
prepare_msg_and_publish(); prepare_msg_and_publish();
// Call the flight controler service // Call the flight controler service
flight_controller_service_call(); flight_controller_service_call();
// Set multi message available after update
if (get_update_status())
{
set_read_update_status();
}
// Set ROBOTS variable (swarm size) // Set ROBOTS variable (swarm size)
get_number_of_robots(); get_number_of_robots();
buzz_utility::set_robot_var(no_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 // get_xbee_status(); // commented out because it may slow down the node too much, to be tested
ros::spinOnce(); ros::spinOnce();
@ -328,12 +392,12 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
Subscribe(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 // publishers
payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5); payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 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); uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5); grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 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);
@ -448,7 +512,7 @@ void roscontroller::uavstate_publisher()
/----------------------------------------------------*/ /----------------------------------------------------*/
{ {
std_msgs::String uavstate_msg; std_msgs::String uavstate_msg;
uavstate_msg.data = buzzuav_closures::getuavstate(); uavstate_msg.data = buzz_utility::getuavstate();
uavstate_pub.publish(uavstate_msg); uavstate_pub.publish(uavstate_msg);
} }
@ -539,33 +603,68 @@ with size......... | /
tmp[2] = cur_pos.altitude; tmp[2] = cur_pos.altitude;
memcpy(position, tmp, 3 * sizeof(uint64_t)); memcpy(position, tmp, 3 * sizeof(uint64_t));
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(XBEE_MESSAGE_CONSTANT); // Add Robot id and message number to the published message
payload_out.payload64.push_back(position[0]); if (message_number < 0)
payload_out.payload64.push_back(position[1]); message_number = 0;
payload_out.payload64.push_back(position[2]); 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*)&ltrate64, (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 // Append Buzz message
uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]); uint16_t* out = buzz_utility::u64_cvt_u16(payload_out_ptr[0]);
for (int i = 0; i < out[0]; i++) for (int i = 0; i < out[0]; i++)
{ {
payload_out.payload64.push_back(payload_out_ptr[i]); payload_out.payload64.push_back(payload_out_ptr[i]);
} }
// Add Robot id and message number to the published message //Store out msg time
if (message_number < 0) get_logical_time();
message_number = 0; out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec();
else
message_number++;
payload_out.sysid = (uint8_t)robot_id;
payload_out.msgid = (uint32_t)message_number;
// publish prepared messages in respective topic // publish prepared messages in respective topic
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
delete[] out; delete[] out;
delete[] payload_out_ptr; delete[] payload_out_ptr;
// Check for updater message if present send /// Check for updater message if present send
if (is_msg_present()) if (update && buzz_update::is_msg_present())
{ {
uint8_t* buff_send = 0; 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; int tot = 0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
@ -578,16 +677,16 @@ with size......... | /
*(uint16_t*)(buff_send + tot) = updater_msgSize; *(uint16_t*)(buff_send + tot) = updater_msgSize;
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
// Append updater msgs // 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; tot += updater_msgSize;
// Destroy the updater out msg queue // 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))); uint16_t total_size = (ceil((float)(float)tot / (float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size]; uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t)); memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
free(buff_send); free(buff_send);
// Send a constant number to differenciate updater msgs // 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++) for (int i = 0; i < total_size; i++)
{ {
update_packets.payload64.push_back(payload_64[i]); update_packets.payload64.push_back(payload_64[i]);
@ -682,7 +781,11 @@ script
} }
else else
ROS_ERROR("Failed to call service from flight controller"); 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; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
#endif
if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
{ {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@ -720,7 +823,11 @@ script
cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param2 = gimbal[1];
cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param3 = gimbal[2];
cmd_srv.request.param4 = gimbal[3]; 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; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
#endif
if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
{ {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); 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) if (timer_step >= BUZZRATE)
{ {
neighbours_pos_map.clear(); neighbours_pos_map.clear();
buzzuav_closures::clear_neighbours_pos();
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
// have to clear ! // have to clear !
timer_step = 0; 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)); 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 / 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 // DEBUG
// ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage, // ROS_INFO("voltage : %d current : %d remaining : %d",msg->voltage,
// msg->current, msg ->remaining); // 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.x = msg->pose.position.x;
cur_pos.y = msg->pose.position.y; 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 // cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
tf::Quaternion q( tf::Quaternion q(
msg->pose.orientation.x, 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]; moveMsg.pose.orientation.w = q[3];
// To prevent drifting from stable position, uncomment // To prevent drifting from stable position, uncomment
// if(fabs(x)>0.005 || fabs(y)>0.005) { if(fabs(x)>0.005 || fabs(y)>0.005) {
localsetpoint_nonraw_pub.publish(moveMsg); localsetpoint_nonraw_pub.publish(moveMsg);
// } }
} }
void roscontroller::SetMode(std::string mode, int delay_miliseconds) 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_INFO("Set stream rate call failed!, trying again...");
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
} }
// DEBUG ROS_WARN("Set stream rate call successful");
// ROS_INFO("Set stream rate call successful");
} }
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg) 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 // 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()); uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
uint64_t message_obt[obt_msg_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); fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
if (unMsgSize > 0) if (unMsgSize > 0)
{ {
code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
code_message_inqueue_process(); buzz_update::code_message_inqueue_process();
} }
free(pl); free(pl);
} }
// BVM FIFO message // 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]; uint64_t message_obt[msg->payload64.size() - 1];
// Go throught the obtained payload // 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]; nei_pos.altitude = neighbours_pos_payload[2];
double cvt_neighbours_pos_payload[3]; double cvt_neighbours_pos_payload[3];
gps_rb(nei_pos, cvt_neighbours_pos_payload); 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 // 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) if (debug)
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 // 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 // TODO: remove roscontroller local map array for neighbors
neighbours_pos_put((int)out[1], n_pos); 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); buzzuav_closures::neighbour_pos_callback((int)out[1], n_pos.x, n_pos.y, n_pos.z);
delete[] out; if((int)mtype == (int)BUZZ_MESSAGE_TIME){
buzz_utility::in_msg_append((message_obt + 3)); // 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); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; 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: case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
#endif
armstate = req.param1; armstate = req.param1;
if (armstate) if (armstate)
{ {
@ -1123,10 +1264,18 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;
#ifdef MAVROSKINETIC
case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
#else
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL: case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
#endif
ROS_INFO("RC_Call: Gimbal!!!! "); ROS_INFO("RC_Call: Gimbal!!!! ");
buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5); 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; rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
#endif
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;
@ -1135,9 +1284,15 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; 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: default:
buzzuav_closures::rc_call(req.command); buzzuav_closures::rc_call(req.command);
ROS_INFO("----> Received unregistered command: ", req.command); ROS_ERROR("----> Received unregistered command: ", req.command);
res.success = true; res.success = true;
break; break;
} }
@ -1326,4 +1481,85 @@ void roscontroller::get_xbee_status()
* TriggerAPIRssi(all_ids); * 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);
}
} }