From d4ee8ef81e69cdde7d3feda9e06f9a9e591fea58 Mon Sep 17 00:00:00 2001 From: dave Date: Thu, 6 Sep 2018 12:47:38 -0400 Subject: [PATCH] merge dev --- CMakeLists.txt | 4 +- buzz_scripts/include/act/barrier.bzz | 46 +- buzz_scripts/include/act/old_barrier.bzz | 69 + buzz_scripts/include/act/states.bzz | 182 ++- buzz_scripts/include/plan/rrtstar.bzz | 2 +- .../include/taskallocate/graphformGPS.bzz | 34 +- buzz_scripts/include/timesync.bzz | 28 + buzz_scripts/include/update.bzz | 3 +- buzz_scripts/include/utils/conversions.bzz | 2 +- buzz_scripts/include/utils/string.bzz | 12 +- buzz_scripts/main.bzz | 21 +- buzz_scripts/minimal.bzz | 1 - buzz_scripts/stand_by.bzz | 45 +- buzz_scripts/testLJ.bzz | 72 + include/buzz_update.h | 248 ++-- include/buzz_utility.h | 26 + include/buzzuav_closures.h | 14 +- include/roscontroller.h | 51 +- launch/launch_config/solo.yaml | 12 + launch/rosbuzz.launch | 5 +- launch/rosbuzzd.launch | 3 +- misc/cmdlinectr.sh | 6 +- readme.md | 4 +- src/buzz_update.cpp | 1293 +++++++++-------- src/buzz_utility.cpp | 52 + src/buzzuav_closures.cpp | 48 +- src/roscontroller.cpp | 332 ++++- 27 files changed, 1654 insertions(+), 961 deletions(-) create mode 100644 buzz_scripts/include/act/old_barrier.bzz create mode 100644 buzz_scripts/include/timesync.bzz create mode 100755 buzz_scripts/testLJ.bzz create mode 100644 launch/launch_config/solo.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 672d68d..2b48f35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index 8b56f3c..27628b0 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -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 +} \ No newline at end of file diff --git a/buzz_scripts/include/act/old_barrier.bzz b/buzz_scripts/include/act/old_barrier.bzz new file mode 100644 index 0000000..ef3c5fc --- /dev/null +++ b/buzz_scripts/include/act/old_barrier.bzz @@ -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 +} + diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index 9668295..2edf7d3 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -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 + # }) + } } }) } diff --git a/buzz_scripts/include/plan/rrtstar.bzz b/buzz_scripts/include/plan/rrtstar.bzz index 54c69ab..a55aff1 100644 --- a/buzz_scripts/include/plan/rrtstar.bzz +++ b/buzz_scripts/include/plan/rrtstar.bzz @@ -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) } diff --git a/buzz_scripts/include/taskallocate/graphformGPS.bzz b/buzz_scripts/include/taskallocate/graphformGPS.bzz index 61e77fd..044228b 100644 --- a/buzz_scripts/include/taskallocate/graphformGPS.bzz +++ b/buzz_scripts/include/taskallocate/graphformGPS.bzz @@ -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 +# } } # diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..8b933ee --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -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 +} diff --git a/buzz_scripts/include/update.bzz b/buzz_scripts/include/update.bzz index f49fd7c..4a5c699 100644 --- a/buzz_scripts/include/update.bzz +++ b/buzz_scripts/include/update.bzz @@ -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) } \ No newline at end of file diff --git a/buzz_scripts/include/utils/conversions.bzz b/buzz_scripts/include/utils/conversions.bzz index 50a58bc..8cef004 100644 --- a/buzz_scripts/include/utils/conversions.bzz +++ b/buzz_scripts/include/utils/conversions.bzz @@ -66,4 +66,4 @@ function gps_from_vec(vec) { #goal.longitude = d_lon + pose.position.longitude; return Lgoal -} \ No newline at end of file +} diff --git a/buzz_scripts/include/utils/string.bzz b/buzz_scripts/include/utils/string.bzz index 4140a1b..91ceb63 100644 --- a/buzz_scripts/include/utils/string.bzz +++ b/buzz_scripts/include/utils/string.bzz @@ -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 diff --git a/buzz_scripts/main.bzz b/buzz_scripts/main.bzz index e87f015..d963fe4 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -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") diff --git a/buzz_scripts/minimal.bzz b/buzz_scripts/minimal.bzz index 8f0494b..a49329b 100644 --- a/buzz_scripts/minimal.bzz +++ b/buzz_scripts/minimal.bzz @@ -51,7 +51,6 @@ function step() { statef=action statef() - log("Current state: ", BVMSTATE) } diff --git a/buzz_scripts/stand_by.bzz b/buzz_scripts/stand_by.bzz index 1349089..c605089 100644 --- a/buzz_scripts/stand_by.bzz +++ b/buzz_scripts/stand_by.bzz @@ -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() } diff --git a/buzz_scripts/testLJ.bzz b/buzz_scripts/testLJ.bzz new file mode 100755 index 0000000..5240618 --- /dev/null +++ b/buzz_scripts/testLJ.bzz @@ -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() { +} diff --git a/include/buzz_update.h b/include/buzz_update.h index 5680f85..4b43e86 100644 --- a/include/buzz_update.h +++ b/include/buzz_update.h @@ -1,161 +1,171 @@ #ifndef BUZZ_UPDATE_H #define BUZZ_UPDATE_H -/*Simulation or robot check*/ -//#define SIMULATION 1 // set in CMAKELIST #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include #include + #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 diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 12cf011..047b575 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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 get_inmsg_vector(); + +std::string getuavstate(); + +int get_timesync_state(); + +int get_timesync_itr(); } diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index e6c3f48..7a5bfa2 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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> 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 */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 7599c58..781f298 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -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 #include #include +#include #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 neighbours_pos_map; std::map raw_neighbours_pos_map; + std::map neighbours_time_map; int timer_step = 0; int robot_id = 0; + ros::Time logical_clock; + ros::Time previous_step_time; + std::vector 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); }; } diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml new file mode 100644 index 0000000..c18075b --- /dev/null +++ b/launch/launch_config/solo.yaml @@ -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 + diff --git a/launch/rosbuzz.launch b/launch/rosbuzz.launch index c2531f4..7f3c63b 100644 --- a/launch/rosbuzz.launch +++ b/launch/rosbuzz.launch @@ -3,12 +3,13 @@ - + + - + diff --git a/launch/rosbuzzd.launch b/launch/rosbuzzd.launch index de7857d..a89b1af 100644 --- a/launch/rosbuzzd.launch +++ b/launch/rosbuzzd.launch @@ -6,9 +6,10 @@ + - + diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 00df45f..bea3e11 100644 --- a/misc/cmdlinectr.sh +++ b/misc/cmdlinectr.sh @@ -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 { diff --git a/readme.md b/readme.md index 82d3941..d61cdb4 100644 --- a/readme.md +++ b/readme.md @@ -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" } -``` \ No newline at end of file +``` diff --git a/src/buzz_update.cpp b/src/buzz_update.cpp index 1d169f9..8624c3a 100644 --- a/src/buzz_update.cpp +++ b/src/buzz_update.cpp @@ -1,599 +1,633 @@ -#include -#include -#include -#include -#include +/** @file buzz_utility.cpp + * @version 1.0 + * @date 27.09.2016 + * @brief Buzz Implementation as a node in ROS. + * @author Vivek Shankar Varadharajan and David St-Onge + * @copyright 2016 MistLab. All rights reserved. + */ + #include "buzz_update.h" -#include -#include -#include -#include -/*Temp for data collection*/ -// static int neigh=-1; -static struct timeval t1, t2; -static int timer_steps = 0; -// static clock_t end; +namespace buzz_update{ + /*Temp for data collection*/ + // static int neigh=-1; + static struct timeval t1, t2; + static int timer_steps = 0; + // static clock_t end; -/*Temp end */ -static int fd, wd = 0; -static int old_update = 0; -static buzz_updater_elem_t updater; -static int no_of_robot; -static const char* dbgf_name; -static const char* bcfname; -static const char* old_bcfname = "old_bcode.bo"; -static const char* bzz_file; -static int Robot_id = 0; -static int neigh = -1; -static int updater_msg_ready; -static uint16_t update_try_counter = 0; -static int updated = 0; -static const uint16_t MAX_UPDATE_TRY = 5; -static int packet_id_ = 0; -static size_t old_byte_code_size = 0; + /*Temp end */ + static int fd, wd = 0; + static int old_update = 0; + static buzz_updater_elem_t updater; + static int no_of_robot; + static const char* dbgf_name; + static const char* bcfname; + static const char* old_bcfname = "old_bcode.bo"; + static const char* bzz_file; + static int Robot_id = 0; + static int neigh = -1; + static int updater_msg_ready; + static uint16_t update_try_counter = 0; + static const uint16_t MAX_UPDATE_TRY = 5; + static size_t old_byte_code_size = 0; + static bool debug = false; -/*Initialize updater*/ -void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) -{ - Robot_id = robot_id; - dbgf_name = dbgfname; - bcfname = bo_filename; - ROS_INFO("Initializing file monitor..."); - fd = inotify_init1(IN_NONBLOCK); - if (fd < 0) + /*Initialize updater*/ + int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) { - perror("inotify_init error"); - } - /*If simulation set the file monitor only for robot 1*/ - if (SIMULATION == 1 && robot_id == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - else if (SIMULATION == 0) - { - /* watch /.bzz file for any activity and report it back to update */ - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - } - /*load the .bo under execution into the updater*/ - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bo_filename, "rb"); - if (!fp) - { - perror(bo_filename); - } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) - { - perror(bo_filename); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Load stand_by .bo file into the updater*/ - uint8_t* STD_BO_BUF = 0; - fp = fopen(stand_by_script, "rb"); - if (!fp) - { - perror(stand_by_script); - } - fseek(fp, 0, SEEK_END); - size_t stdby_bcode_size = ftell(fp); - rewind(fp); - STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); - if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) - { - perror(stand_by_script); - // fclose(fp); - // return 0; - } - fclose(fp); - /*Create the updater*/ - updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); - /*Intialize the updater with the required data*/ - updater->bcode = BO_BUF; - /*Store a copy of the Bcode for rollback*/ - updater->outmsg_queue = NULL; - updater->inmsg_queue = NULL; - updater->patch = NULL; - updater->patch_size = (size_t*)malloc(sizeof(size_t)); - updater->bcode_size = (size_t*)malloc(sizeof(size_t)); - updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->update_no) = 0; - *(size_t*)(updater->bcode_size) = bcode_size; - updater->standby_bcode = STD_BO_BUF; - updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); - *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; - updater->mode = (int*)malloc(sizeof(int)); - *(int*)(updater->mode) = CODE_RUNNING; - // no_of_robot=barrier; - updater_msg_ready = 0; - - /*Write the bcode to a file for rollback*/ - fp = fopen("old_bcode.bo", "wb"); - fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); - fclose(fp); -} -/*Check for .bzz file chages*/ -int check_update() -{ - char buf[1024]; - int check = 0; - int i = 0; - int len = read(fd, buf, 1024); - while (i < len) - { - struct inotify_event* event = (struct inotify_event*)&buf[i]; - /* file was modified this flag is true in nano and self delet in gedit and other editors */ - // fprintf(stdout,"inside file monitor.\n"); - if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) - { - /*respawn watch if the file is self deleted */ - inotify_rm_watch(fd, wd); - close(fd); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){ + Robot_id = robot_id; + dbgf_name = dbgfname; + bcfname = bo_filename; + ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)"); + if(debug) ROS_INFO("Initializing file monitor..."); fd = inotify_init1(IN_NONBLOCK); - wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); - /* To mask multiple writes from editors*/ - if (!old_update) + if (fd < 0) { - check = 1; - old_update = 1; + perror("inotify_init error"); + } + /*If simulation set the file monitor only for robot 0*/ + if (SIMULATION == 1 && robot_id == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + else if (SIMULATION == 0) + { + /* watch /.bzz file for any activity and report it back to update */ + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + } + /*load the .bo under execution into the updater*/ + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bo_filename, "rb"); + if (!fp) + { + perror(bo_filename); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bo_filename); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Load stand_by .bo file into the updater*/ + uint8_t* STD_BO_BUF = 0; + fp = fopen(stand_by_script, "rb"); + if (!fp) + { + perror(stand_by_script); + } + fseek(fp, 0, SEEK_END); + size_t stdby_bcode_size = ftell(fp); + rewind(fp); + STD_BO_BUF = (uint8_t*)malloc(stdby_bcode_size); + if (fread(STD_BO_BUF, 1, stdby_bcode_size, fp) < stdby_bcode_size) + { + perror(stand_by_script); + // fclose(fp); + // return 0; + } + fclose(fp); + /*Create the updater*/ + updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s)); + /*Intialize the updater with the required data*/ + updater->bcode = BO_BUF; + /*Store a copy of the Bcode for rollback*/ + updater->outmsg_queue = NULL; + updater->inmsg_queue = NULL; + updater->patch = NULL; + updater->patch_size = (size_t*)malloc(sizeof(size_t)); + updater->bcode_size = (size_t*)malloc(sizeof(size_t)); + updater->update_no = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->update_no) = 0; + *(size_t*)(updater->bcode_size) = bcode_size; + updater->standby_bcode = STD_BO_BUF; + updater->standby_bcode_size = (size_t*)malloc(sizeof(size_t)); + *(size_t*)(updater->standby_bcode_size) = stdby_bcode_size; + updater->mode = (int*)malloc(sizeof(int)); + *(int*)(updater->mode) = CODE_RUNNING; + // no_of_robot=barrier; + updater_msg_ready = 0; + + /*Write the bcode to a file for rollback*/ + fp = fopen("old_bcode.bo", "wb"); + fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp); + fclose(fp); + return 1; + } + else{ + ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)"); + return 0; + } + } + /*Check for .bzz file chages*/ + int check_update() + { + char buf[1024]; + int check = 0; + int i = 0; + int len = read(fd, buf, 1024); + while (i < len) + { + struct inotify_event* event = (struct inotify_event*)&buf[i]; + /*Report file changes and self deletes*/ + if (event->mask & (IN_MODIFY | IN_DELETE_SELF)) + { + /*Respawn watch if the file is self deleted */ + inotify_rm_watch(fd, wd); + close(fd); + fd = inotify_init1(IN_NONBLOCK); + wd = inotify_add_watch(fd, bzz_file, IN_ALL_EVENTS); + /*To mask multiple writes from editors*/ + if (!old_update) + { + check = 1; + old_update = 1; + } + } + /*Update index to start of next event*/ + i += sizeof(struct inotify_event) + event->len; + } + if (!check) + old_update = 0; + return check; + } + + int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) + { + if (SIMULATION == 1) + { + return 1; + } + else + { + /*Patch the old bo with new patch*/ + std::stringstream patch_writefile; + patch_writefile << path << name1 << "tmp_patch.bo"; + /*Write the patch to a file and call bsdiff to patch*/ + FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); + fwrite(patch, update_patch_size, 1, fp); + fclose(fp); + std::stringstream patch_exsisting; + patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 + << "tmp_patch.bo"; + if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str()); + if (system(patch_exsisting.str().c_str())) + return 0; + else + return 1; + } + } + + updater_code_t obtain_patched_bo(std::string& path, std::string& name1) + { + if (SIMULATION == 1) + { + /*Read the exsisting file to simulate the patching*/ + std::stringstream read_patched; + read_patched << path << name1 << ".bo"; + if(debug){ + ROS_WARN("Simulation patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + + else + { + /*Read the new patched file*/ + std::stringstream read_patched; + read_patched << path << name1 << "-patched.bo"; + if(debug) { + ROS_WARN("Robot patching ..."); + ROS_WARN("Read file for patching %s", read_patched.str().c_str()); + } + uint8_t* patched_bo_buf = 0; + FILE* fp = fopen(read_patched.str().c_str(), "rb"); + if (!fp) + { + perror(read_patched.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patched_size = ftell(fp); + rewind(fp); + patched_bo_buf = (uint8_t*)malloc(patched_size); + if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size) + { + perror(read_patched.str().c_str()); + } + fclose(fp); + + /* delete old bo file & rename new bo file */ + remove((path + name1 + ".bo").c_str()); + rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); + + /*Write the patched to a code struct and return*/ + updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); + update_code->bcode = patched_bo_buf; + update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(update_code->bcode_size) = patched_size; + + return update_code; + } + } + + void code_message_outqueue_append() + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + /* if size less than 250 Pad with zeors to assure transmission*/ + uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); + uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; + size += padding_size; + updater->outmsg_queue->queue = (uint8_t*)malloc(size); + memset(updater->outmsg_queue->queue, 0, size); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + *(uint16_t*)(updater->outmsg_queue->size) = size; + size = 0; + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); + size += sizeof(uint16_t); + memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); + // size += (uint16_t) * (size_t*)(updater->patch_size); + updater_msg_ready = 1; + } + + void outqueue_append_code_request(uint16_t update_no) + { + updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + uint16_t size = 0; + updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); + /*Append message type*/ + *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; + size += sizeof(uint8_t); + /*Append the update no, code size and code to out msg*/ + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; + size += sizeof(uint16_t); + *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; + updater_msg_ready = 1; + if(debug) ROS_WARN("[Debug] Requesting update no. %u for rebroadcast, try: %u", update_no, update_try_counter); + } + + void code_message_inqueue_append(uint8_t* msg, uint16_t size) + { + updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); + // ROS_INFO("[DEBUG] Updater append code of size %d", (int) size); + updater->inmsg_queue->queue = (uint8_t*)malloc(size); + updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); + memcpy(updater->inmsg_queue->queue, msg, size); + *(uint16_t*)(updater->inmsg_queue->size) = size; + } + /*Used for data logging*/ + /*void set_packet_id(int packet_id) + { + packet_id_ = packet_id; + }*/ + void code_message_inqueue_process() + { + int size = 0; + updater_code_t out_code = NULL; + if(debug) { + ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode)); + ROS_WARN("[Debug] Current update no: %u, Received update no: %u", (*(uint16_t*)(updater->update_no)), + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); + ROS_WARN("[Debug] Updater received patch of size %u", + (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); + } + if (*(int*)(updater->mode) == CODE_RUNNING) + { + // fprintf(stdout,"[debug]Inside inmsg code running"); + if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + { + size += sizeof(uint8_t); + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + { + // fprintf(stdout,"[debug]Inside update number comparision"); + uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); + size += sizeof(uint16_t); + /*Generate patch*/ + std::string bzzfile_name(bzz_file); + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + { + out_code = obtain_patched_bo(path, name1); + + // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); + // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); + // FILE *fp; + // fp=fopen("update.bo", "wb"); + // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); + // fclose(fp); + + if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) + { + if(debug) ROS_WARN("Received patch PASSED tests!"); + *(uint16_t*)updater->update_no = update_no; + /*Clear exisiting patch if any*/ + delete_p(updater->patch); + /*copy the patch into the updater*/ + updater->patch = (uint8_t*)malloc(update_patch_size); + memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); + *(size_t*)(updater->patch_size) = update_patch_size; + // code_message_outqueue_append(); + neigh = 1; + } + /*clear the temp code buff*/ + delete_p(out_code->bcode); + delete_p(out_code->bcode_size); + delete_p(out_code); + } + else + { + ROS_ERROR("Patching the .bo file failed"); + update_try_counter++; + outqueue_append_code_request(update_no); + } + } } } - /* update index to start of next event */ - i += sizeof(struct inotify_event) + event->len; - } - if (!check) - old_update = 0; - return check; -} - -int test_patch(std::string& path, std::string& name1, size_t update_patch_size, uint8_t* patch) -{ - if (SIMULATION == 1) - { - return 1; - } - else - { - /*Patch the old bo with new patch*/ - std::stringstream patch_writefile; - patch_writefile << path << name1 << "tmp_patch.bo"; - /*Write the patch to a file*/ - FILE* fp = fopen(patch_writefile.str().c_str(), "wb"); - fwrite(patch, update_patch_size, 1, fp); - fclose(fp); - std::stringstream patch_exsisting; - patch_exsisting << "bspatch " << path << name1 << ".bo " << path << name1 << "-patched.bo " << path << name1 - << "tmp_patch.bo"; - fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str()); - if (system(patch_exsisting.str().c_str())) - return 0; - else - return 1; - } -} - -updater_code_t obtain_patched_bo(std::string& path, std::string& name1) -{ - if (SIMULATION == 1) - { - /*Read the exsisting file to simulate the patching*/ - std::stringstream read_patched; - read_patched << path << name1 << ".bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } - - else - { - /*Read the new patched file*/ - std::stringstream read_patched; - read_patched << path << name1 << "-patched.bo"; - fprintf(stdout, "read file name %s \n", read_patched.str().c_str()); - uint8_t* patched_BO_Buf = 0; - FILE* fp = fopen(read_patched.str().c_str(), "rb"); - if (!fp) - { - perror(read_patched.str().c_str()); - } - fseek(fp, 0, SEEK_END); - size_t patched_size = ftell(fp); - rewind(fp); - patched_BO_Buf = (uint8_t*)malloc(patched_size); - if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size) - { - perror(read_patched.str().c_str()); - } - fclose(fp); - - /* delete old bo file & rename new bo file */ - remove((path + name1 + ".bo").c_str()); - rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str()); - - /*Write the patched to a code struct and return*/ - updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s)); - update_code->bcode = patched_BO_Buf; - update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(update_code->bcode_size) = patched_size; - - return update_code; - } -} - -void code_message_outqueue_append() -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - /* if size less than 250 Pad with zeors to assure transmission*/ - uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size); - uint16_t padding_size = (size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET - size; - size += padding_size; - updater->outmsg_queue->queue = (uint8_t*)malloc(size); - memset(updater->outmsg_queue->queue, 0, size); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - *(uint16_t*)(updater->outmsg_queue->size) = size; - size = 0; - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = SENT_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = *(uint16_t*)(updater->update_no); - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = (uint16_t) * (size_t*)(updater->patch_size); - size += sizeof(uint16_t); - memcpy(updater->outmsg_queue->queue + size, updater->patch, *(size_t*)(updater->patch_size)); - // size += (uint16_t) * (size_t*)(updater->patch_size); - updater_msg_ready = 1; -} - -void outqueue_append_code_request(uint16_t update_no) -{ - updater->outmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - uint16_t size = 0; - updater->outmsg_queue->queue = (uint8_t*)malloc(2 * sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) + CODE_REQUEST_PADDING); - /*Append message type*/ - *(uint8_t*)(updater->outmsg_queue->queue + size) = RESEND_CODE; - size += sizeof(uint8_t); - /*Append the update no, code size and code to out msg*/ - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_no; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->queue + size) = update_try_counter; - size += sizeof(uint16_t); - *(uint16_t*)(updater->outmsg_queue->size) = size + CODE_REQUEST_PADDING; - updater_msg_ready = 1; - ROS_INFO("[Debug] Requested update no. %u with try: %u \n", update_no, update_try_counter); -} - -void code_message_inqueue_append(uint8_t* msg, uint16_t size) -{ - updater->inmsg_queue = (updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s)); - // ROS_INFO("[DEBUG] Updater append code of size %d\n", (int) size); - updater->inmsg_queue->queue = (uint8_t*)malloc(size); - updater->inmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t)); - memcpy(updater->inmsg_queue->queue, msg, size); - *(uint16_t*)(updater->inmsg_queue->size) = size; -} -/*Used for data logging*/ -/*void set_packet_id(int packet_id) -{ - packet_id_ = packet_id; -}*/ -void code_message_inqueue_process() -{ - int size = 0; - updater_code_t out_code = NULL; - - ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode)); - ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)), - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint8_t)))); - ROS_INFO("[Debug] Updater received patch of size %u \n", - (*(uint16_t*)(updater->inmsg_queue->queue + sizeof(uint16_t) + sizeof(uint8_t)))); - - if (*(int*)(updater->mode) == CODE_RUNNING) - { - // fprintf(stdout,"[debug]Inside inmsg code running"); - if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE) + size = 0; + if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) { + if(debug) ROS_WARN("Patch rebroadcast request received."); size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no)) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) { - // fprintf(stdout,"[debug]Inside update number comparision"); - uint16_t update_no = *(uint16_t*)(updater->inmsg_queue->queue + size); size += sizeof(uint16_t); - uint16_t update_patch_size = *(uint16_t*)(updater->inmsg_queue->queue + size); - size += sizeof(uint16_t); - /*Generate patch*/ - std::string bzzfile_name(bzz_file); - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - if (test_patch(path, name1, update_patch_size, (updater->inmsg_queue->queue + size))) + if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) { - out_code = obtain_patched_bo(path, name1); + update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); + if(debug) ROS_WARN("Rebroadcasting patch, try : %u", update_try_counter); + code_message_outqueue_append(); + } + if (update_try_counter > MAX_UPDATE_TRY) + ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!"); + } + } + // fprintf(stdout,"in queue freed\n"); + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } - // fprintf(stdout,"in queue process Update no %d\n", (int) update_no); - // fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size); - // FILE *fp; - // fp=fopen("update.bo", "wb"); - // fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp); - // fclose(fp); + void create_update_patch() + { + std::stringstream genpatch; - if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size))) - { - printf("TEST PASSED!\n"); - *(uint16_t*)updater->update_no = update_no; - /*Clear exisiting patch if any*/ - delete_p(updater->patch); - /*copy the patch into the updater*/ - updater->patch = (uint8_t*)malloc(update_patch_size); - memcpy(updater->patch, (updater->inmsg_queue->queue + size), update_patch_size); - *(size_t*)(updater->patch_size) = update_patch_size; - // code_message_outqueue_append(); - neigh = 1; - } - /*clear the temp code buff*/ - delete_p(out_code->bcode); - delete_p(out_code->bcode_size); - delete_p(out_code); + std::string bzzfile_name(bzz_file); + + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + + std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name1 = name1.substr(0, name1.find_last_of(".")); + + std::string name2 = name1 + "-update"; + + /*Launch bsdiff and create a patch*/ + genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; + if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str()); + system(genpatch.str().c_str()); + + /*Delete old files & rename new files*/ + + remove((path + name1 + ".bo").c_str()); + remove((path + name1 + ".bdb").c_str()); + + rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); + rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); + + /*Read the diff file */ + std::stringstream patchfileName; + patchfileName << path << "diff.bo"; + + uint8_t* patch_buff = 0; + FILE* fp = fopen(patchfileName.str().c_str(), "rb"); + if (!fp) + { + perror(patchfileName.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t patch_size = ftell(fp); + rewind(fp); + patch_buff = (uint8_t*)malloc(patch_size); + if (fread(patch_buff, 1, patch_size, fp) < patch_size) + { + perror(patchfileName.str().c_str()); + } + fclose(fp); + delete_p(updater->patch); + updater->patch = patch_buff; + *(size_t*)(updater->patch_size) = patch_size; + + /* Delete the diff file */ + remove(patchfileName.str().c_str()); + } + + void update_routine() + { + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); + if (*(int*)updater->mode == CODE_RUNNING) + { + buzzvm_function_call(VM, "updated_no_bct", 0); + // Check for update + if (check_update()) + { + ROS_INFO("Update found \tUpdating script ..."); + + if (compile_bzz(bzz_file)) + { + ROS_ERROR("Error in compiling script, resuming old script."); } else { - ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n"); - update_try_counter++; - outqueue_append_code_request(update_no); + std::string bzzfile_name(bzz_file); + stringstream bzzfile_in_compile; + std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; + std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); + name = name.substr(0, name.find_last_of(".")); + bzzfile_in_compile << path << name << "-update.bo"; + uint8_t* BO_BUF = 0; + FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); + if (!fp) + { + perror(bzzfile_in_compile.str().c_str()); + } + fseek(fp, 0, SEEK_END); + size_t bcode_size = ftell(fp); + rewind(fp); + BO_BUF = (uint8_t*)malloc(bcode_size); + if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + { + perror(bcfname); + } + fclose(fp); + if (test_set_code(BO_BUF, dbgf_name, bcode_size)) + { + uint16_t update_no = *(uint16_t*)(updater->update_no); + *(uint16_t*)(updater->update_no) = update_no + 1; + + create_update_patch(); + VM = buzz_utility::get_vm(); + if(debug) ROS_INFO("Current Update no %d", *(uint16_t*)(updater->update_no)); + buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); + buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + buzzvm_gstore(VM); + neigh = -1; + if(debug) ROS_INFO("Broadcasting patch ..."); + code_message_outqueue_append(); + } + delete_p(BO_BUF); } } } - } - size = 0; - if (*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE) - { - size += sizeof(uint8_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) == *(uint16_t*)(updater->update_no)) + + else { - size += sizeof(uint16_t); - if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter) + timer_steps++; + buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); + buzzvm_gload(VM); + buzzobj_t tObj = buzzvm_stack_at(VM, 1); + buzzvm_pop(VM); + ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value); + if (tObj->i.value == no_of_robot) { - update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size); - ROS_ERROR("Code appended! update try : %u \n", update_try_counter); - code_message_outqueue_append(); + ROS_WARN("Patch deployment successful, rolling forward"); + *(int*)(updater->mode) = CODE_RUNNING; + gettimeofday(&t2, NULL); + // collect_data(); + buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); + // buzzvm_function_call(m_tBuzzVM, "updated", 0); + update_try_counter = 0; + timer_steps = 0; + } + else if (timer_steps > TIMEOUT_FOR_ROLLBACK) + { + ROS_ERROR("TIME OUT Reached, rolling back"); + /*Time out hit deceided to roll back*/ + *(int*)(updater->mode) = CODE_RUNNING; + buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); + update_try_counter = 0; + timer_steps = 0; } - if (update_try_counter > MAX_UPDATE_TRY) - ROS_ERROR("TODO: ROLL BACK !!"); } } - // fprintf(stdout,"in queue freed\n"); - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); -} -void create_update_patch() -{ - std::stringstream genpatch; - - std::string bzzfile_name(bzz_file); - - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - - std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name1 = name1.substr(0, name1.find_last_of(".")); - - std::string name2 = name1 + "-update"; - - // CALL BSDIFF CMD - genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo"; - fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str()); - system(genpatch.str().c_str()); - - // BETTER: CALL THE FUNCTION IN BSDIFF.CPP - // bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo"); - - /* delete old files & rename new files */ - - remove((path + name1 + ".bo").c_str()); - remove((path + name1 + ".bdb").c_str()); - - rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str()); - rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str()); - - /*Read the diff file */ - std::stringstream patchfileName; - patchfileName << path << "diff.bo"; - - uint8_t* patch_buff = 0; - FILE* fp = fopen(patchfileName.str().c_str(), "rb"); - if (!fp) + uint8_t* getupdater_out_msg() { - perror(patchfileName.str().c_str()); + return (uint8_t*)updater->outmsg_queue->queue; } - fseek(fp, 0, SEEK_END); - size_t patch_size = ftell(fp); - rewind(fp); - patch_buff = (uint8_t*)malloc(patch_size); - if (fread(patch_buff, 1, patch_size, fp) < patch_size) + + uint8_t* getupdate_out_msg_size() { - perror(patchfileName.str().c_str()); + // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); + return (uint8_t*)updater->outmsg_queue->size; } - fclose(fp); - delete_p(updater->patch); - updater->patch = patch_buff; - *(size_t*)(updater->patch_size) = patch_size; - /* Delete the diff file */ - remove(patchfileName.str().c_str()); -} - -void update_routine() -{ - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); - buzzvm_gstore(VM); - // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode); - if (*(int*)updater->mode == CODE_RUNNING) + int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) { - buzzvm_function_call(VM, "updated_neigh", 0); - // Check for update - if (check_update()) + if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) { - ROS_INFO("Update found \nUpdating script ...\n"); - - if (compile_bzz(bzz_file)) + if(debug) ROS_WARN("Initializtion test passed"); + if (buzz_utility::update_step_test()) { - ROS_WARN("Errors in comipilg script so staying on old script\n"); + /*data logging*/ + old_byte_code_size = *(size_t*)updater->bcode_size; + /*data logging*/ + if(debug) ROS_WARN("Step test passed"); + *(int*)(updater->mode) = CODE_STANDBY; + // fprintf(stdout,"updater value = %i\n",updater->mode); + delete_p(updater->bcode); + updater->bcode = (uint8_t*)malloc(bcode_size); + memcpy(updater->bcode, BO_BUF, bcode_size); + *(size_t*)updater->bcode_size = bcode_size; + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + gettimeofday(&t1, NULL); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); + buzzvm_gstore(VM); + return 1; } + /*Unable to step something wrong*/ else { - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << path << name << "-update.bo"; - uint8_t* BO_BUF = 0; - FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit - if (!fp) + if (*(int*)(updater->mode) == CODE_RUNNING) { - perror(bzzfile_in_compile.str().c_str()); + ROS_ERROR("Step test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); } - fseek(fp, 0, SEEK_END); - size_t bcode_size = ftell(fp); - rewind(fp); - BO_BUF = (uint8_t*)malloc(bcode_size); - if (fread(BO_BUF, 1, bcode_size, fp) < bcode_size) + else { - perror(bcfname); - } - fclose(fp); - if (test_set_code(BO_BUF, dbgf_name, bcode_size)) - { - uint16_t update_no = *(uint16_t*)(updater->update_no); - *(uint16_t*)(updater->update_no) = update_no + 1; - - create_update_patch(); - VM = buzz_utility::get_vm(); - ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no)); - buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1)); - buzzvm_pushi(VM, *(uint16_t*)(updater->update_no)); + /*You will never reach here*/ + ROS_ERROR("Step test failed, returning to standby"); + buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, + (size_t) * (size_t*)(updater->standby_bcode_size)); + buzzvm_t VM = buzz_utility::get_vm(); + buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); + buzzvm_pushi(VM, no_of_robot); buzzvm_gstore(VM); - neigh = -1; - ROS_INFO("Sending code... \n"); - code_message_outqueue_append(); } - delete_p(BO_BUF); + return 0; } } - } - - else - { - timer_steps++; - buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val", 1)); - buzzvm_gload(VM); - buzzobj_t tObj = buzzvm_stack_at(VM, 1); - buzzvm_pop(VM); - ROS_INFO("Barrier ..................... %i \n", tObj->i.value); - if (tObj->i.value == no_of_robot) - { - *(int*)(updater->mode) = CODE_RUNNING; - gettimeofday(&t2, NULL); - // collect_data(); - buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size)); - // buzzvm_function_call(m_tBuzzVM, "updated", 0); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - else if (timer_steps > TIMEOUT_FOR_ROLLBACK) - { - ROS_ERROR("TIME OUT Reached, decided to roll back"); - /*Time out hit deceided to roll back*/ - *(int*)(updater->mode) = CODE_RUNNING; - buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot); - updated = 1; - update_try_counter = 0; - timer_steps = 0; - } - } -} - -uint8_t* getupdater_out_msg() -{ - return (uint8_t*)updater->outmsg_queue->queue; -} - -uint8_t* getupdate_out_msg_size() -{ - // fprintf(stdout,"[Debug from get out size in util: ]size = %i \n",*(uint16_t*)updater->outmsg_queue->size); - return (uint8_t*)updater->outmsg_queue->size; -} - -int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) -{ - if (buzz_utility::buzz_update_init_test(BO_BUF, dbgfname, bcode_size)) - { - ROS_WARN("Initializtion of script test passed\n"); - if (buzz_utility::update_step_test()) - { - /*data logging*/ - old_byte_code_size = *(size_t*)updater->bcode_size; - /*data logging*/ - ROS_WARN("Step test passed\n"); - *(int*)(updater->mode) = CODE_STANDBY; - // fprintf(stdout,"updater value = %i\n",updater->mode); - delete_p(updater->bcode); - updater->bcode = (uint8_t*)malloc(bcode_size); - memcpy(updater->bcode, BO_BUF, bcode_size); - *(size_t*)updater->bcode_size = bcode_size; - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - gettimeofday(&t1, NULL); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - return 1; - } - /*Unable to step something wrong*/ else { if (*(int*)(updater->mode) == CODE_RUNNING) { - ROS_ERROR("step test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size)); + ROS_ERROR("Initialization test failed, resuming old script"); + buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); } else { /*You will never reach here*/ - ROS_ERROR("step test failed, Return to stand by\n"); + ROS_ERROR("Initialization test failed, returning to standby"); buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, (size_t) * (size_t*)(updater->standby_bcode_size)); buzzvm_t VM = buzz_utility::get_vm(); @@ -604,118 +638,87 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size) return 0; } } - else - { - if (*(int*)(updater->mode) == CODE_RUNNING) - { - ROS_ERROR("Initialization test failed, stick to old script\n"); - buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (int)*(size_t*)(updater->bcode_size)); - } - else - { - /*You will never reach here*/ - ROS_ERROR("Initialization test failed, Return to stand by\n"); - buzz_utility::buzz_update_init_test((updater)->standby_bcode, (char*)dbgfname, - (size_t) * (size_t*)(updater->standby_bcode_size)); - buzzvm_t VM = buzz_utility::get_vm(); - buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); - buzzvm_pushi(VM, no_of_robot); - buzzvm_gstore(VM); - } - return 0; - } -} -void destroy_out_msg_queue() -{ - // fprintf(stdout,"out queue freed\n"); - delete_p(updater->outmsg_queue->queue); - delete_p(updater->outmsg_queue->size); - delete_p(updater->outmsg_queue); - updater_msg_ready = 0; -} -int get_update_status() -{ - return updated; -} -void set_read_update_status() -{ - updated = 0; -} -/*int get_update_mode() -{ - return (int)*(int*)(updater->mode); -}*/ - -int is_msg_present() -{ - return updater_msg_ready; -} -/*buzz_updater_elem_t get_updater() -{ - return updater; -}*/ -void destroy_updater() -{ - delete_p(updater->bcode); - delete_p(updater->bcode_size); - delete_p(updater->standby_bcode); - delete_p(updater->standby_bcode_size); - delete_p(updater->mode); - delete_p(updater->update_no); - if (updater->outmsg_queue) + void destroy_out_msg_queue() { + // fprintf(stdout,"out queue freed\n"); delete_p(updater->outmsg_queue->queue); delete_p(updater->outmsg_queue->size); delete_p(updater->outmsg_queue); + updater_msg_ready = 0; } - if (updater->inmsg_queue) + + int is_msg_present() { - delete_p(updater->inmsg_queue->queue); - delete_p(updater->inmsg_queue->size); - delete_p(updater->inmsg_queue); + return updater_msg_ready; + } + /*buzz_updater_elem_t get_updater() + { + return updater; + }*/ + void destroy_updater() + { + delete_p(updater->bcode); + delete_p(updater->bcode_size); + delete_p(updater->standby_bcode); + delete_p(updater->standby_bcode_size); + delete_p(updater->mode); + delete_p(updater->update_no); + if (updater->outmsg_queue) + { + delete_p(updater->outmsg_queue->queue); + delete_p(updater->outmsg_queue->size); + delete_p(updater->outmsg_queue); + } + if (updater->inmsg_queue) + { + delete_p(updater->inmsg_queue->queue); + delete_p(updater->inmsg_queue->size); + delete_p(updater->inmsg_queue); + } + inotify_rm_watch(fd, wd); + close(fd); } - inotify_rm_watch(fd, wd); - close(fd); -} -void set_bzz_file(const char* in_bzz_file) -{ - bzz_file = in_bzz_file; -} + void set_bzz_file(const char* in_bzz_file, bool dbg) + { + debug=dbg; + bzz_file = in_bzz_file; + } -void updates_set_robots(int robots) -{ - no_of_robot = robots; -} + void updates_set_robots(int robots) + { + no_of_robot = robots; + } -/*-------------------------------------------------------- -/ Create Buzz bytecode from the bzz script inputed -/-------------------------------------------------------*/ -int compile_bzz(std::string bzz_file) -{ - /*Compile the buzz code .bzz to .bo*/ - std::string bzzfile_name(bzz_file); - stringstream bzzfile_in_compile; - std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"; - std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1); - name = name.substr(0, name.find_last_of(".")); - bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<bcode_size << "," << *(size_t*)updater->patch_size << "," - << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; -}*/ + /*void collect_data(std::ofstream& logger) + { + double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC; + time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0; + // RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number, + // + Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter + logger << (int)no_of_robot << "," << neigh << "," << (double)time_spent << "," << (int)timer_steps << "," + << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," + << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; + }*/ +} \ No newline at end of file diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 507e70c..ebedb52 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -564,4 +564,56 @@ int get_inmsg_size() { return IN_MSG.size(); } + +std::vector 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; +} + } diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 364f2f8..1e368c5 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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)); diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index cf54154..ddd71ea 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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< 0)log<<","; + map::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::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){ + log<<","<<(int)it->nid <<","<<(int)it->msgid<<","<<(int)it->size<<","<sent_time + <<","<received_time; + } + inmsgdata.clear(); + log<(out_payload, 5); MPpayload_pub = n_c.advertise("fleet_status", 5); - neigh_pos_pub = n_c.advertise("neighbours_pos", 5); + neigh_pos_pub = n_c.advertise("neighbours_pos", MAX_NUMBER_OF_ROBOTS); uavstate_pub = n_c.advertise("uavstate", 5); grid_pub = n_c.advertise("grid", 5); localsetpoint_nonraw_pub = n_c.advertise(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::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::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); + } }