From 7261aabc408c31ec07d85cf06454c3516d16b6ba Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Wed, 11 Jul 2018 18:41:50 -0400 Subject: [PATCH] Time Sync algorithm and rc cmd implementation to sync time on request --- buzz_scripts/include/act/barrier.bzz | 2 +- buzz_scripts/include/act/states.bzz | 15 ++++- buzz_scripts/include/timesync.bzz | 86 +++++++++++++++++++++++++++ buzz_scripts/include/utils/string.bzz | 12 ++-- buzz_scripts/main.bzz | 7 +++ include/buzz_utility.h | 2 + include/roscontroller.h | 1 + misc/cmdlinectr.sh | 3 + src/buzz_utility.cpp | 17 ++++++ src/roscontroller.cpp | 10 +++- 10 files changed, 145 insertions(+), 10 deletions(-) create mode 100644 buzz_scripts/include/timesync.bzz diff --git a/buzz_scripts/include/act/barrier.bzz b/buzz_scripts/include/act/barrier.bzz index f7d5910..d7e141c 100644 --- a/buzz_scripts/include/act/barrier.bzz +++ b/buzz_scripts/include/act/barrier.bzz @@ -87,4 +87,4 @@ function barrier_allgood(barrier, bc) { } ) return barriergood -} +} \ No newline at end of file diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz index f08b0e8..6f94586 100644 --- a/buzz_scripts/include/act/states.bzz +++ b/buzz_scripts/include/act/states.bzz @@ -215,7 +215,13 @@ function rc_cmd_listen() { } 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 + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if (flight.rc_cmd==900){ flight.rc_cmd=0 barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) barrier_ready(900) @@ -259,7 +265,12 @@ function nei_cmd_listen() { arm() } else if(value==401 and BVMSTATE=="TURNEDOFF"){ disarm() - } else if(value==900){ # Shapes + } else if(value==777 and BVMSTATE=="TURNEDOFF"){ + if(logical_time != nil) reinit_time_sync() + barrier_set(ROBOTS, "TURNEDOFF", BVMSTATE, 777) + barrier_ready(777) + neighbors.broadcast("cmd", 777) + }else if(value==900){ # Shapes barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900) #barrier_ready(900) neighbors.broadcast("cmd", 900) diff --git a/buzz_scripts/include/timesync.bzz b/buzz_scripts/include/timesync.bzz new file mode 100644 index 0000000..c441bf5 --- /dev/null +++ b/buzz_scripts/include/timesync.bzz @@ -0,0 +1,86 @@ +include "utils/string.bzz" + +# Time to be sync +logical_time = 0 +# sync algo. constants +TIME_JUMP_THR = 5 +TIME_TO_FORGET = 20 +TIME_TO_SYNC = 100 +# table to store neighbor time data +time_nei_table = {} +# Algo. global parameters +diffMaxLogical = 0 +jumped = 0 +syncError = 99999 +sync_timer = 0 + +# Function to intialize sync algorithm +function init_time_sync(){ + neighbors.listen("time_sync", + function(vid, value, rid) { + log(" TIME SYNC Got (", vid, ",", value, ") #", rid) + var msg = string.split(value,",") + var msg_time = string.toint(msg[0]) + var msg_max = string.toint(msg[1]) + #log("msg: 1: ", msg_time, " 2: ", msg_max ) + diffMaxLogical = math.max(diffMaxLogical,msg_max-logical_time) + var time_offset = msg_time - logical_time + if(math.abs(time_offset) > math.abs(syncError)) syncError = time_offset + if(time_offset > TIME_JUMP_THR){ + logical_time = logical_time + time_offset + diffMaxLogical = math.max(diffMaxLogical-time_offset,0) + jumped = 1 + } + time_nei_table[rid] = { .time=msg_time, .age=0, .max=msg_max} + } + ) +} + +# Function to sync. algo +function step_time_sync(){ + logical_time = logical_time + 1 + sync_timer = sync_timer + 1 + if(sync_timer < TIME_TO_SYNC){ + log(" SYNC ALGO ACTIVE time:", sync_timer) + cnt = 0 + avg_offset = 0 + foreach(time_nei_table, function(key, value) { + if(value != nil){ + #log("ForEach neigh : id ", key, " time ", value.time, " , age ", value.age, " , diffmax ", value.max) + var local_offset = value.time - logical_time - value.age + if(local_offset > 0){ + avg_offset = avg_offset + 1 * local_offset + cnt = cnt + 1 + } + else{ + if(math.abs(local_offset) TIME_TO_FORGET) + time_nei_table[key] = nil + value.age = value.age + 1 + } + }) + if(cnt > 0 and jumped != 1){ + var correction = math.ceil(avg_offset / (cnt + 1) ) + if(math.abs(correction) < TIME_JUMP_THR){ + logical_time = logical_time + correction + } + } + + jumped = 0 + syncError=0 + var mstr = string.concat(string.tostring(logical_time + 1),",",string.tostring(logical_time + 1 + diffMaxLogical)) + neighbors.broadcast("time_sync",mstr) + } + log("Logical time now ", logical_time) +} + +# Function to set sync timer to zero and reinitiate sync. algo + +function reinit_time_sync(){ + sync_timer = 0 +} \ 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 d52fd95..0a3bc32 100644 --- a/buzz_scripts/main.bzz +++ b/buzz_scripts/main.bzz @@ -5,6 +5,7 @@ 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 = "CUSFUN" @@ -31,6 +32,8 @@ function init() { init_stig() init_swarm() + init_time_sync() + TARGET_ALTITUDE = 5 + (id-LOWEST_ROBOT_ID)*3.0 # m loop = 1 @@ -43,6 +46,10 @@ function init() { # Executed at each time step. function step() { + + # step time sync algorithm + step_time_sync() + # listen to Remote Controller rc_cmd_listen() diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 600b9e2..1c3c506 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -79,4 +79,6 @@ void set_robot_var(int ROBOTS); int get_inmsg_size(); std::string getuavstate(); + +int getlogicaltime(); } diff --git a/include/roscontroller.h b/include/roscontroller.h index 029eca5..566b1a0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -43,6 +43,7 @@ #define TIMEOUT 60 #define BUZZRATE 10 #define CMD_REQUEST_UPDATE 666 +#define CMD_SYNC_CLOCK 777 using namespace std; diff --git a/misc/cmdlinectr.sh b/misc/cmdlinectr.sh index 00df45f..68b4296 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 } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 0862533..c3986d0 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -564,6 +564,7 @@ int get_inmsg_size() { return IN_MSG.size(); } + string getuavstate() /* / return current BVM state @@ -579,4 +580,20 @@ string getuavstate() } return uav_state; } + +int getlogicaltime() +/* +/ return current logical time +--------------------------------------*/ +{ + int logical_time = 0; + if(VM){ + buzzvm_pushs(VM, buzzvm_string_register(VM, "logical_time", 1)); + buzzvm_gload(VM); + buzzobj_t obj = buzzvm_stack_at(VM, 1); + if(obj->o.type == BUZZTYPE_INT) logical_time = obj->i.value; + buzzvm_pop(VM); + } + return logical_time; +} } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5f71a82..cb00b3e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -165,6 +165,8 @@ void roscontroller::RosControllerRun() ROS_WARN("CURRENT PACKET DROP : %f ", cur_packet_loss); // log data log<"); + 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; }