merge dev

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

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,21 +40,25 @@ 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) {
+function barrier_wait(threshold, transf, resumef, bc) {
-  barrier.put(id, 1)
+  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
@@ -62,7 +67,24 @@ function barrier_wait(threshold, transf, resumef) {
     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
+}

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

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_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
-			barrier_ready()
+			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_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
-		barrier_ready()
+		barrier_ready(22)
 	}
 }
 
+gohomeT=100
+function goinghome() {
+  BVMSTATE = "GOHOME"
+	if(gohomeT > 0) {	# TODO: Make a real check if home is reached
+		gohome()
+		gohomeT = gohomeT - 1
+	} else
+  	BVMSTATE = AUTO_LAUNCH_STATE
+}
+
 function stop() {
   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")
+			BVMSTATE = "TURNEDOFF"
-			barrier_ready()
+			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
+			#barrier_ready(21)
 		}
 	} else {
-			barrier_set(ROBOTS,"TURNEDOFF","STOP")
+			BVMSTATE = "TURNEDOFF"
-			barrier_ready()
+			#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,17 +186,21 @@ 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)
+	accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
-  goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
+  goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
 }
 
+# listens to commands from the remote control (web, commandline, etc)
 function rc_cmd_listen() {
   if(flight.rc_cmd==22) {
 		log("cmd 22")
@@ -172,74 +208,108 @@ function rc_cmd_listen() {
 		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==777){
+		flight.rc_cmd=0
+		reinit_time_sync()
+		neighbors.broadcast("cmd", 777)
 	}else if (flight.rc_cmd==900){
 		flight.rc_cmd=0
-		BVMSTATE = "TASK_ALLOCATE"
+		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") {
+				if(BVMSTATE!="BARRIERWAIT") {
+					if(value==22) {
 						BVMSTATE = "LAUNCH"
-				} else if(value==21 and BVMSTATE!="BARRIERWAIT") {
+					}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") {
-					uav_arm()
+						arm()
 					} else if(value==401 and BVMSTATE=="TURNEDOFF"){
-					uav_disarm()
+						disarm()
+					} else if(value==777 and BVMSTATE=="TURNEDOFF"){
+						reinit_time_sync()
+						#neighbors.broadcast("cmd", 777)
 					}else if(value==900){	# Shapes
-					BVMSTATE = "TASK_ALLOCATE"
+						barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
+						#barrier_ready(900)
+						neighbors.broadcast("cmd", 900)
 					} else if(value==901){	# Pursuit
 						destroyGraph()
-					BVMSTATE = "PURSUIT"
+						barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 901)
+						#barrier_ready(901)
+						neighbors.broadcast("cmd", 901)
 					} else if(value==902){	# Agreggate
 						destroyGraph()
-					BVMSTATE = "AGGREGATE"
+						barrier_set(ROBOTS, "AGGREGATE", BVMSTATE, 902)
+						#barrier_ready(902)
+						neighbors.broadcast("cmd", 902)
 					} else if(value==903){	# Formation
 						destroyGraph()
-					BVMSTATE = "FORMATION"
+						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
 					#  }) 
 					}
+				}
     })
 }

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

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
@@ -655,8 +655,7 @@ function DoJoined(){
 	      return
 	   }
 	}
-
+  barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED",941)
-  barrier_wait(ROBOTS, "GRAPH_TRANSTOLOCK", "GRAPH_JOINED")
   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(loop) {
-	  if(timeout_graph==0) {
+#	  if(timeout_graph==0) {
-		if(graphid < 3)
+#		if(graphid < 3)
-			graphid = graphid + 1
+#			graphid = graphid + 1
-		else
+#		else
-			graphid = 0
+#			graphid = 0
-		timeout_graph = 40
+#		timeout_graph = 40
-		BVMSTATE="TASK_ALLOCATE"
+#		BVMSTATE="TASK_ALLOCATE"
-	  }
+#	  }
-  	timeout_graph = timeout_graph - 1
+#  	timeout_graph = timeout_graph - 1
-  }
+#  }
 }
 
 #

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

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

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

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")

buzz_scripts/minimal.bzz

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

buzz_scripts/stand_by.bzz

@@ -1,5 +1,11 @@
+include "act/states.bzz"
+include "vstigenv.bzz"
+
 updated="update_ack"
 update_no=0
+BVMSTATE = "UPDATESTANDBY"
+
+# Executed once at init time.
 function init(){
 	barrier = stigmergy.create(101)
 	barrier.put(id,1)
@@ -8,28 +14,25 @@ barrier.onconflict(function (k,l,r) {
 	if(r. data < l. data or (r. data == l. data )) return l
 	else return r
 	})
-
+	init_swarm()
-s = swarm.create(1)
+	# start the swarm command listener
-s.join()
+	nei_cmd_listen()
 }
 
 function stand_by(){
-
 	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)
 	   }
 )
 
 }
 
-
+# Executed at each time step.
 function step() {
-
 	stand_by()
 
 }

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

include/buzz_update.h

@@ -1,22 +1,28 @@
 #ifndef BUZZ_UPDATE_H
 #define BUZZ_UPDATE_H
-/*Simulation or robot check*/
-//#define SIMULATION 1 // set in CMAKELIST
 
 #include <stdlib.h>
 #include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/time.h>
+#include <sys/inotify.h>
 #include <buzz/buzztype.h>
 #include <buzz/buzzdict.h>
 #include <buzz/buzzdarray.h>
 #include <buzz/buzzvstig.h>
+#include <buzz_utility.h>
 #include <fstream>
+
 #define delete_p(p)                                                                                                    \
   do                                                                                                                   \
   {                                                                                                                    \
     free(p);                                                                                                           \
     p = NULL;                                                                                                          \
   } while (0)
-
+namespace buzz_update
+{
   static const uint16_t CODE_REQUEST_PADDING = 250;
   static const uint16_t MIN_UPDATE_PACKET = 251;
   static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
@@ -97,7 +103,7 @@ void update_routine();
   /************************************************/
   /*Initalizes the updater */
   /************************************************/
-void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
+  int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
 
   /*********************************************************/
   /*Appends buffer of given size to in msg queue of updater*/
@@ -112,30 +118,29 @@ void code_message_inqueue_append(uint8_t* msg, uint16_t size);
   void code_message_inqueue_process();
 
   /*****************************************************/
-/* obtains messages from out msgs queue of the updater*/
+  /*Obtains messages from out msgs queue of the updater*/
   /*******************************************************/
   uint8_t* getupdater_out_msg();
 
   /******************************************************/
-/*obtains out msg queue size*/
+  /*Obtains out msg queue size*/
   /*****************************************************/
   uint8_t* getupdate_out_msg_size();
 
   /**************************************************/
-/*destroys the out msg queue*/
+  /*Destroys the out msg queue*/
   /*************************************************/
   void destroy_out_msg_queue();
 
-/***************************************************/
-/*obatins updater state*/
-/***************************************************/
-// int get_update_mode();
-
   // buzz_updater_elem_t get_updater();
   /***************************************************/
-/*sets bzz file name*/
+  /*Sets bzz file name*/
+  /***************************************************/
+  void set_bzz_file(const char* in_bzz_file, bool dbg);
+
+  /****************************************************/
+  /*Tests the code from a buffer*/
   /***************************************************/
-void set_bzz_file(const char* in_bzz_file);
 
   int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
 
@@ -145,17 +150,22 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
 
   void destroy_updater();
 
+  /****************************************************/
+  /*Checks for updater message*/
+  /***************************************************/
+
   int is_msg_present();
 
-int get_update_status();
+  /****************************************************/
-
+  /*Compiles a bzz script to bo and bdbg*/
-void set_read_update_status();
+  /***************************************************/
 
   int compile_bzz(std::string bzz_file);
 
+  /****************************************************/
+  /*Set number of robots in the updater*/
+  /***************************************************/
+
   void updates_set_robots(int robots);
-
+}
-// void set_packet_id(int packet_id);
-
-// void collect_data(std::ofstream& logger);
 #endif

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<uint8_t*> get_inmsg_vector();
+
+std::string getuavstate();
+
+int get_timesync_state();
+
+int get_timesync_itr();
 }

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<int, std::map<int,int>> getgrid();
-/*
+
- * returns the current Buzz state
+
- */
-std::string getuavstate();
 /*
  * returns the gimbal commands
  */
@@ -118,6 +120,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
  * update neighbors from in msgs
  */
 void update_neighbors(buzzvm_t vm);
+/*
+ *Clear neighbours struct
+ */
+void clear_neighbours_pos();
 /*
  * closure to add a neighbor status
  */

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 <signal.h>
 #include <ostream>
 #include <map>
+#include <cmath>
 #include "buzzuav_closures.h"
 
-#define UPDATER_MESSAGE_CONSTANT 987654321
+/*
-#define XBEE_MESSAGE_CONSTANT 586782343
+* ROSBuzz message types
-#define XBEE_STOP_TRANSMISSION 4355356352
+*/
+typedef enum {
+  ROS_BUZZ_MSG_NIL = 0,    // dummy msg
+  UPDATER_MESSAGE,         // Update msg
+  BUZZ_MESSAGE_WTO_TIME,   // Broadcast message wihout time info
+  BUZZ_MESSAGE_TIME,       // Broadcast message with time info
+} rosbuzz_msgtype;
+
+// Time sync algo. constants
+#define COM_DELAY 100000000 // in nano seconds i.e 100 ms
+#define TIME_SYNC_JUMP_THR 500000000 
+#define MOVING_AVERAGE_ALPHA 0.1
+#define MAX_NUMBER_OF_ROBOTS 10
+
 #define TIMEOUT 60
 #define BUZZRATE 10
 #define CMD_REQUEST_UPDATE 666
+#define CMD_SYNC_CLOCK 777
 
 using namespace std;
 
@@ -83,19 +99,42 @@ private:
   };
   typedef struct POSE ros_pose;
 
+
+  struct MsgData
+  {
+    int msgid;
+    uint16_t nid;
+    uint16_t size;
+    double sent_time;
+    uint64_t received_time;
+    MsgData(int mi, uint16_t ni, uint16_t s, double st, uint64_t rt): 
+            msgid(mi), nid(ni), size(s),sent_time(st), received_time(rt){};
+    MsgData(){};
+  };
+  typedef struct MsgData msg_data;
+
   ros_pose target, home, cur_pos;
 
   uint64_t payload;
   std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
   std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
+  std::map<int, buzz_utility::neighbor_time> neighbours_time_map;
   int timer_step = 0;
   int robot_id = 0;
+  ros::Time logical_clock;
+  ros::Time previous_step_time;
+  std::vector<msg_data> inmsgdata; 
+  uint64_t out_msg_time; 
+  double logical_time_rate;
+  bool time_sync_jumped;
   std::string robot_name = "";
 
   int rc_cmd;
   float fcu_timeout;
   int armstate;
   int barrier;
+  int update;
+  int statepub_active;
   int message_number = 0;
   uint8_t no_of_robots = 0;
   bool rcclient;
@@ -205,7 +244,7 @@ private:
                        double gps_r_lat);
 
   /*battery status callback */
-  void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
+  void battery(const sensor_msgs::BatteryState::ConstPtr& msg);
 
   /*flight extended status callback*/
   void flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg);
@@ -265,5 +304,9 @@ private:
   bool GetFilteredPacketLoss(const uint8_t short_id, float& result);
   void get_xbee_status();
 
+  void time_sync_step();
+  void push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr);
+  uint64_t get_logical_time();
+  void set_logical_time_correction(uint64_t cor);
 };
 }

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
+

launch/rosbuzz.launch

@@ -6,9 +6,10 @@
   <arg name="name" default="robot0"/>
   <arg name="xbee_plugged" default="true"/>
   <arg name="script" default="testalone"/>
+  <arg name="launch_config" default="topics"/>
 
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
-    <rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
+    <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
     <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
     <param name="rcclient" value="true" />
     <param name="rcservice_name" value="buzzcmd" />

launch/rosbuzzd.launch

@@ -6,9 +6,10 @@
 	<arg name="name" default="robot0"/>
   <arg name="xbee_plugged" default="true"/>
   <arg name="script" default="testalone"/>
+  <arg name="launch_config" default="topics"/>
 
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
-    <rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
+    <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
     <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
     <param name="rcclient" value="true" />
     <param name="rcservice_name" value="buzzcmd" />

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 {

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.
 

src/buzz_update.cpp

@@ -1,14 +1,14 @@
-#include <stdio.h>
+/** @file      buzz_utility.cpp
-#include <stdlib.h>
+ *  @version   1.0
-#include <fcntl.h>
+ *  @date      27.09.2016
-#include <unistd.h>
+ *  @brief     Buzz Implementation as a node in ROS.
-#include <sys/inotify.h>
+ *  @author    Vivek Shankar Varadharajan and David St-Onge
-#include "buzz_update.h"
+ *  @copyright 2016 MistLab. All rights reserved.
-#include <buzz_utility.h>
+ */
-#include <string.h>
-#include <buzz/buzzdebug.h>
-#include <sys/time.h>
 
+#include "buzz_update.h"
+
+namespace buzz_update{
   /*Temp for data collection*/
   // static int neigh=-1;
   static struct timeval t1, t2;
@@ -28,24 +28,29 @@ 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;
+  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)
+  int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
   {
+    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_INFO("Initializing file monitor...");
+      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);
       if (fd < 0)
       {
         perror("inotify_init error");
       }
-  /*If simulation set the file monitor only for robot 1*/
+      /*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 */
@@ -117,6 +122,12 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script, c
       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()
@@ -128,11 +139,10 @@ int check_update()
     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 */
+      /*Report file changes and self deletes*/
-    // fprintf(stdout,"inside file monitor.\n");
       if (event->mask & (IN_MODIFY | IN_DELETE_SELF))
       {
-      /*respawn watch if the file is self deleted */
+        /*Respawn watch if the file is self deleted */
         inotify_rm_watch(fd, wd);
         close(fd);
         fd = inotify_init1(IN_NONBLOCK);
@@ -144,7 +154,7 @@ int check_update()
           old_update = 1;
         }
       }
-    /* update index to start of next event */
+      /*Update index to start of next event*/
       i += sizeof(struct inotify_event) + event->len;
     }
     if (!check)
@@ -163,14 +173,14 @@ int test_patch(std::string& path, std::string& name1, size_t update_patch_size,
       /*Patch the old bo with new patch*/
       std::stringstream patch_writefile;
       patch_writefile << path << name1 << "tmp_patch.bo";
-    /*Write the patch to a file*/
+      /*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";
-    fprintf(stdout, "Launching bspatch command: %s \n", patch_exsisting.str().c_str());
+      if(debug) ROS_WARN("Launching bspatch command: %s", patch_exsisting.str().c_str());
       if (system(patch_exsisting.str().c_str()))
         return 0;
       else
@@ -185,8 +195,11 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
       /*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());
+      if(debug){ 
-    uint8_t* patched_BO_Buf = 0;
+        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)
       {
@@ -195,15 +208,15 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
       fseek(fp, 0, SEEK_END);
       size_t patched_size = ftell(fp);
       rewind(fp);
-    patched_BO_Buf = (uint8_t*)malloc(patched_size);
+      patched_bo_buf = (uint8_t*)malloc(patched_size);
-    if (fread(patched_BO_Buf, 1, patched_size, fp) < 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 = patched_bo_buf;
       update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
       *(uint16_t*)(update_code->bcode_size) = patched_size;
 
@@ -215,8 +228,11 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
       /*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());
+      if(debug) {
-    uint8_t* patched_BO_Buf = 0;
+        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)
       {
@@ -225,8 +241,8 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
       fseek(fp, 0, SEEK_END);
       size_t patched_size = ftell(fp);
       rewind(fp);
-    patched_BO_Buf = (uint8_t*)malloc(patched_size);
+      patched_bo_buf = (uint8_t*)malloc(patched_size);
-    if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
+      if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
       {
         perror(read_patched.str().c_str());
       }
@@ -238,7 +254,7 @@ updater_code_t obtain_patched_bo(std::string& path, std::string& name1)
 
       /*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 = patched_bo_buf;
       update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
       *(uint16_t*)(update_code->bcode_size) = patched_size;
 
@@ -288,13 +304,13 @@ void outqueue_append_code_request(uint16_t update_no)
     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);
+    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\n", (int) size);
+    // 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);
@@ -309,13 +325,13 @@ void code_message_inqueue_process()
   {
     int size = 0;
     updater_code_t out_code = NULL;
-
+    if(debug) {
-  ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode));
+      ROS_WARN("[Debug] Updater processing in msg with mode %d", *(int*)(updater->mode));
-  ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n", (*(uint16_t*)(updater->update_no)),
+      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_INFO("[Debug] Updater received patch of size %u \n",
+      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");
@@ -347,7 +363,7 @@ void code_message_inqueue_process()
 
             if (test_set_code((out_code->bcode), (char*)dbgf_name, (size_t)(*(uint16_t*)out_code->bcode_size)))
             {
-            printf("TEST PASSED!\n");
+              if(debug) ROS_WARN("Received patch PASSED tests!");
               *(uint16_t*)updater->update_no = update_no;
               /*Clear exisiting patch if any*/
               delete_p(updater->patch);
@@ -365,7 +381,7 @@ void code_message_inqueue_process()
           }
           else
           {
-          ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
+            ROS_ERROR("Patching the .bo file failed");
             update_try_counter++;
             outqueue_append_code_request(update_no);
           }
@@ -375,6 +391,7 @@ void code_message_inqueue_process()
     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))
       {
@@ -382,11 +399,11 @@ void code_message_inqueue_process()
         if (*(uint16_t*)(updater->inmsg_queue->queue + size) > update_try_counter)
         {
           update_try_counter = *(uint16_t*)(updater->inmsg_queue->queue + size);
-        ROS_ERROR("Code appended! update try : %u \n", update_try_counter);
+          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: ROLL BACK !!");
+          ROS_ERROR("TODO: Max rebroadcast retry reached, ROLL BACK !!");
       }
     }
     // fprintf(stdout,"in queue freed\n");
@@ -408,15 +425,12 @@ void create_update_patch()
 
     std::string name2 = name1 + "-update";
 
-  // CALL BSDIFF CMD
+    /*Launch bsdiff and create a patch*/
     genpatch << "bsdiff " << path << name1 << ".bo " << path << name2 << ".bo " << path << "diff.bo";
-  fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str());
+    if(debug) ROS_WARN("Launching bsdiff: %s", genpatch.str().c_str());
     system(genpatch.str().c_str());
 
-  // BETTER: CALL THE FUNCTION IN BSDIFF.CPP
+    /*Delete old files & rename new files*/
-  // 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());
@@ -460,15 +474,15 @@ void update_routine()
     // fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
     if (*(int*)updater->mode == CODE_RUNNING)
     {
-    buzzvm_function_call(VM, "updated_neigh", 0);
+      buzzvm_function_call(VM, "updated_no_bct", 0);
       // Check for update
       if (check_update())
       {
-      ROS_INFO("Update found \nUpdating script ...\n");
+        ROS_INFO("Update found \tUpdating script ...");
 
         if (compile_bzz(bzz_file))
         {
-        ROS_WARN("Errors in comipilg script so staying on old script\n");
+          ROS_ERROR("Error in compiling script, resuming old script.");
         }
         else
         {
@@ -479,7 +493,7 @@ void update_routine()
           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
+          FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb");  
           if (!fp)
           {
             perror(bzzfile_in_compile.str().c_str());
@@ -500,12 +514,12 @@ void update_routine()
 
             create_update_patch();
             VM = buzz_utility::get_vm();
-          ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no));
+            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;
-          ROS_INFO("Sending code... \n");
+            if(debug) ROS_INFO("Broadcasting patch ...");
             code_message_outqueue_append();
           }
           delete_p(BO_BUF);
@@ -520,25 +534,24 @@ void update_routine()
       buzzvm_gload(VM);
       buzzobj_t tObj = buzzvm_stack_at(VM, 1);
       buzzvm_pop(VM);
-    ROS_INFO("Barrier ..................... %i \n", tObj->i.value);
+      ROS_INFO("Barrier ............. No. of robots deployed: %i", tObj->i.value);
       if (tObj->i.value == no_of_robot)
       {
+        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);
-      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");
+        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);
-      updated = 1;
         update_try_counter = 0;
         timer_steps = 0;
       }
@@ -560,13 +573,13 @@ 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(debug) ROS_WARN("Initializtion test passed");
       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");
+        if(debug) ROS_WARN("Step test passed");
         *(int*)(updater->mode) = CODE_STANDBY;
         // fprintf(stdout,"updater value = %i\n",updater->mode);
         delete_p(updater->bcode);
@@ -587,13 +600,13 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
       {
         if (*(int*)(updater->mode) == CODE_RUNNING)
         {
-        ROS_ERROR("step test failed, stick to old script\n");
+          ROS_ERROR("Step test failed, resuming old script");
           buzz_utility::buzz_update_init_test((updater)->bcode, dbgfname, (size_t) * (size_t*)(updater->bcode_size));
         }
         else
         {
           /*You will never reach here*/
-        ROS_ERROR("step test failed, Return to stand by\n");
+          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();
@@ -608,13 +621,13 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
     {
       if (*(int*)(updater->mode) == CODE_RUNNING)
       {
-      ROS_ERROR("Initialization test failed, stick to old script\n");
+        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("Initialization 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();
@@ -634,18 +647,6 @@ void destroy_out_msg_queue()
     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()
   {
@@ -679,8 +680,9 @@ void destroy_updater()
     close(fd);
   }
 
-void set_bzz_file(const char* in_bzz_file)
+  void set_bzz_file(const char* in_bzz_file, bool dbg)
   {
+    debug=dbg;
     bzz_file = in_bzz_file;
   }
 
@@ -690,7 +692,7 @@ void updates_set_robots(int robots)
   }
 
   /*--------------------------------------------------------
-/ Create Buzz bytecode from the bzz script inputed
+  / Create Buzz bytecode from the bzz script input
   /-------------------------------------------------------*/
   int compile_bzz(std::string bzz_file)
   {
@@ -719,3 +721,4 @@ Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent
            << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
            << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
   }*/
+}

src/buzz_utility.cpp

@@ -564,4 +564,56 @@ int get_inmsg_size()
 {
   return IN_MSG.size();
 }
+
+std::vector<uint8_t*> get_inmsg_vector(){
+  return IN_MSG;
+}
+
+string getuavstate()
+/*
+/ return current BVM state
+---------------------------------------*/
+{
+  std::string uav_state = "Unknown";
+  if(VM && VM->strings){
+    buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
+    buzzvm_gload(VM);
+    buzzobj_t obj = buzzvm_stack_at(VM, 1);
+    uav_state = string(obj->s.value.str);
+    buzzvm_pop(VM);
+  }
+  return uav_state;
+}
+
+int get_timesync_state()
+/*
+/ return time sync state from bzz script
+--------------------------------------*/
+{
+  int timesync_state = 0;
+  if(VM){
+    buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_state", 1));
+    buzzvm_gload(VM);
+    buzzobj_t obj = buzzvm_stack_at(VM, 1);
+    if(obj->o.type == BUZZTYPE_INT) timesync_state = obj->i.value;
+    buzzvm_pop(VM);
+  }
+  return timesync_state;
+}
+int get_timesync_itr()
+/*
+/ return time sync iteration from bzz script
+--------------------------------------*/
+{
+  int timesync_itr = 0;
+  if(VM){
+    buzzvm_pushs(VM, buzzvm_string_register(VM, "timesync_itr", 1));
+    buzzvm_gload(VM);
+    buzzobj_t obj = buzzvm_stack_at(VM, 1);
+    if(obj->o.type == BUZZTYPE_INT) timesync_itr = obj->i.value;
+    buzzvm_pop(VM);
+  }
+  return timesync_itr;
+}
+
 }

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],
+   ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
-  // goto_pos[1], goto_pos[2]);
+   goto_pos[1], goto_pos[2]);
   buzz_cmd = COMMAND_MOVETO;  // TODO: standard mavros?
   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));

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<<ros::Time::now().toNSec()<<",";
+      get_logical_time();
+      // logical time now
+      log<<logical_clock.toNSec()<<",";
+      log<<buzz_utility::get_timesync_itr()<<",";
+      log<<buzz_utility::get_timesync_state()<<",";
+
+      log<<cur_pos.latitude * 100000 << "," << cur_pos.longitude * 100000 << ","
+            << cur_pos.altitude * 100000 << ",";
+      log << (int)no_of_robots<<",";
+      log  << neighbours_pos_map.size()<< ",";
+      log<<(int)inmsgdata.size()<<",";
+      log<< message_number<<",";
+      log<< out_msg_time<<",";
+      log <<buzz_utility::getuavstate();
+      // if(neighbours_pos_map.size() > 0)log<<",";
+      map<int, buzz_utility::Pos_struct>::iterator it =
+            neighbours_pos_map.begin();
+      for (; it != neighbours_pos_map.end(); ++it)
+      {
+        log<<","<< it->first<<",";
+        log<< (double)it->second.x << "," << (double)it->second.y
+            << "," << (double)it->second.z;
+      }
+      for (std::vector<msg_data>::iterator it = inmsgdata.begin() ; it != inmsgdata.end(); ++it){
+        log<<","<<(int)it->nid <<","<<(int)it->msgid<<","<<(int)it->size<<","<<it->sent_time
+            <<","<<it->received_time;
+      }
+      inmsgdata.clear();
+      log<<std::endl;
+
+      // time_sync_step();
+      // if(debug)
+      //   ROS_INFO("[Debug : %i ] logical Time %f , har time %f",robot_id, logical_clock.toSec(), ros::Time::now().toSec());
       //  Call Step from buzz script
       buzz_utility::buzz_script_step();
       //  Prepare messages and publish them
       prepare_msg_and_publish();
       // Call the flight controler service
       flight_controller_service_call();
-      //  Set multi message available after update
-      if (get_update_status())
-      {
-        set_read_update_status();
-      }
       //  Set ROBOTS variable (swarm size)
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
-      updates_set_robots(no_of_robots);
+      if(update) buzz_update::updates_set_robots(no_of_robots);
       // get_xbee_status();  // commented out because it may slow down the node too much, to be tested
 
       ros::spinOnce();
@@ -328,12 +392,12 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
 
   Subscribe(n_c);
 
-  payload_sub = n_c.subscribe(in_payload, 5, &roscontroller::payload_obt, this);
+  payload_sub = n_c.subscribe(in_payload, MAX_NUMBER_OF_ROBOTS, &roscontroller::payload_obt, this);
 
   // publishers
   payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 5);
   MPpayload_pub = n_c.advertise<mavros_msgs::Mavlink>("fleet_status", 5);
-  neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", 5);
+  neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("neighbours_pos", MAX_NUMBER_OF_ROBOTS);
   uavstate_pub = n_c.advertise<std_msgs::String>("uavstate", 5);
   grid_pub = n_c.advertise<nav_msgs::OccupancyGrid>("grid", 5);
   localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_name, 5);
@@ -448,7 +512,7 @@ void roscontroller::uavstate_publisher()
 /----------------------------------------------------*/
 {
   std_msgs::String uavstate_msg;
-  uavstate_msg.data = buzzuav_closures::getuavstate();
+  uavstate_msg.data = buzz_utility::getuavstate();
   uavstate_pub.publish(uavstate_msg);
 }
 
@@ -539,33 +603,68 @@ with size.........  |   /
   tmp[2] = cur_pos.altitude;
   memcpy(position, tmp, 3 * sizeof(uint64_t));
   mavros_msgs::Mavlink payload_out;
-  payload_out.payload64.push_back(XBEE_MESSAGE_CONSTANT);
+  //  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*)&ltrate64, (void*)&logical_time_rate, sizeof(uint64_t));
+    //payload_out.payload64.push_back(ltrate64);
+  }
+  else{
+    // prepare rosbuzz msg header
+    tmphead[0] = (uint8_t)BUZZ_MESSAGE_WTO_TIME;
+    memcpy(rheader, tmphead, sizeof(uint64_t));
+    // push header into the buffer
+    payload_out.payload64.push_back(rheader[0]);
+    payload_out.payload64.push_back(position[0]);
+    payload_out.payload64.push_back(position[1]);
+    payload_out.payload64.push_back(position[2]);
+  }
   //  Append Buzz message
   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
+  //Store out msg time
-  if (message_number < 0)
+  get_logical_time();
-    message_number = 0;
+  out_msg_time = ros::Time::now().toNSec(); //logical_clock.toNSec();
-  else
-    message_number++;
-  payload_out.sysid = (uint8_t)robot_id;
-  payload_out.msgid = (uint32_t)message_number;
-
   // publish prepared messages in respective topic
   payload_pub.publish(payload_out);
   delete[] out;
   delete[] payload_out_ptr;
-  //  Check for updater message if present send
+  ///  Check for updater message if present send
-  if (is_msg_present())
+  if (update && buzz_update::is_msg_present())
   {
     uint8_t* buff_send = 0;
-    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) {
+  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_WARN("Set stream rate call successful");
-  // ROS_INFO("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);
+      buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
-      code_message_inqueue_process();
+      buzz_update::code_message_inqueue_process();
     }
     free(pl);
   }
   //  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);
+     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 + 3));
+      buzz_utility::in_msg_append((message_obt + index));
+    }
+    else{
+      delete[] out;
+      buzz_utility::in_msg_append((message_obt + index));
+    }
   }
 }
 
@@ -1094,8 +1230,13 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
+#ifdef MAVROSKINETIC
+    case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
+      rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
+#else
     case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
       rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
+#endif
       armstate = req.param1;
       if (armstate)
       {
@@ -1123,10 +1264,18 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
+#ifdef MAVROSKINETIC
+    case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
+#else
     case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
+#endif
       ROS_INFO("RC_Call: Gimbal!!!! ");
       buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5);
+#ifdef MAVROSKINETIC
+      rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
+#else
       rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
+#endif
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
@@ -1135,9 +1284,15 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
+    case CMD_SYNC_CLOCK:
+      rc_cmd = CMD_SYNC_CLOCK;
+      buzzuav_closures::rc_call(rc_cmd);
+      ROS_INFO("<---------------- Time Sync requested ----------->");
+      res.success = true;
+      break;
     default:
       buzzuav_closures::rc_call(req.command);
-      ROS_INFO("----> Received unregistered command: ", req.command);
+      ROS_ERROR("----> Received unregistered command: ", req.command);
       res.success = true;
       break;
   }
@@ -1326,4 +1481,85 @@ void roscontroller::get_xbee_status()
    * TriggerAPIRssi(all_ids);
    */
 }
+
+void roscontroller::time_sync_step()
+/*
+ * Steps the time syncronization algorithm  
+ ------------------------------------------------------------------ */
+{
+  //ROS_INFO("Stepping time sync : %f ", logical_clock.toSec());
+  double avgRate = logical_time_rate;
+  double avgOffset = 0;
+  int offsetCount = 0;
+  map<int, buzz_utility::neighbor_time>::iterator it;
+  for (it = neighbours_time_map.begin(); it != neighbours_time_map.end(); )
+  {
+    avgRate += (it->second).relative_rate;
+    // estimate current offset
+    int64_t  offset = (int64_t)(it->second).nei_logical_time - (int64_t)(it->second).node_logical_time; 
+    avgOffset = avgOffset + offset;
+    offsetCount++;
+    if((it->second).age > BUZZRATE){
+      neighbours_time_map.erase(it++);
+    }
+    else{ 
+      (it->second).age++;
+      ++it;
+    }
+    ROS_INFO("Size of nei time %i",neighbours_time_map.size());
+  }
+  avgRate = avgRate/(neighbours_time_map.size()+1);
+  if(offsetCount>0 && !time_sync_jumped){
+    int64_t correction = (int64_t)ceil(avgOffset / (offsetCount+1));
+    if(correction < TIME_SYNC_JUMP_THR && correction > 0 ){
+      set_logical_time_correction(correction);
+    }
+  }
+  get_logical_time(); // just to update clock
+  time_sync_jumped = false;
+  //neighbours_time_map.clear();
+  logical_time_rate = avgRate;
+
+} 
+void roscontroller::push_timesync_nei_msg(int nid, uint64_t nh, uint64_t nl, double nr)
+/*
+ * pushes a time syncronization msg into its data slot  
+ ------------------------------------------------------------------ */
+{
+  map<int, buzz_utility::neighbor_time>::iterator it = neighbours_time_map.find(nid);
+  double relativeRate =1;
+  if (it != neighbours_time_map.end()){
+    int64_t deltaLocal = ros::Time::now().toNSec() - (it->second).node_hardware_time;
+    int64_t delatNei = round(nh - (it->second).nei_hardware_time);
+    double currentRate = 0;
+    if(deltaLocal !=0 && delatNei !=0) currentRate = ((double)delatNei - (double)deltaLocal)/(double)deltaLocal;
+    relativeRate =  MOVING_AVERAGE_ALPHA*((it->second).relative_rate) 
+                              + (1- MOVING_AVERAGE_ALPHA)*currentRate;
+    
+    ROS_INFO("SYNC MSG RECEIVED deltaLocal %" PRIu64 ", delatNei %" PRId64 " , currentrate %f , this relative rate %f, final relativeRate %f", 
+      deltaLocal, delatNei, currentRate, (it->second).relative_rate, relativeRate);
+    neighbours_time_map.erase(it);
+  }
+  int64_t offset = (int64_t)nl - (int64_t)logical_clock.toNSec();
+  if(offset > TIME_SYNC_JUMP_THR){
+    set_logical_time_correction(offset);// + time_diff * logical_time_rate );
+    time_sync_jumped = true;
+  }
+  buzz_utility::neighbor_time nt(nh, nl, ros::Time::now().toNSec(),
+                                    logical_clock.toNSec(), nr, relativeRate);
+  nt.age=0;
+  neighbours_time_map.insert(make_pair(nid, nt));
+}
+
+  uint64_t roscontroller::get_logical_time(){
+    uint64_t time_diff = ros::Time::now().toNSec() - previous_step_time.toNSec();
+    uint64_t old_time = logical_clock.toNSec();
+    logical_clock.fromNSec(old_time + time_diff);// + time_diff * logical_time_rate);
+    previous_step_time.fromNSec(ros::Time::now().toNSec());
+    return logical_clock.toNSec();
+  }
+  void roscontroller::set_logical_time_correction(uint64_t cor){
+    uint64_t l_time_now = get_logical_time();
+    logical_clock.fromNSec(l_time_now + cor);
+  }
 }