move rosbuzz headers and reneabled pursuit and graph formation from webcontrol

buzz_scripts/include/act/CA.bzz

@@ -1,10 +1,10 @@
 # Lightweight collision avoidance
 function LCA( vel_vec ) {
-	var safety_radius = 3.0
+	var safety_radius = 2.0
 	collide = 0
 
-	var k_v = 5   # x axis gain
+	var k_v = 4   # x axis gain
-	var k_w = 5   # y axis gain
+	var k_w = 4   # y axis gain
 
 	cart = neighbors.map(
 		function(rid, data) {
@@ -17,7 +17,7 @@ function LCA( vel_vec ) {
 	})
 	if (collide) {
 		log("")
-		log("------>  AVOIDING NEIGHBOR!  <------")
+		log("------>  AVOIDING NEIGHBOR! (LCA) <------")
 		log("")
 		result = cart.reduce(function(rid, data, accum) {
 			if(data.distance < accum.distance and data.distance > 0.0){
@@ -48,4 +48,187 @@ function LCA( vel_vec ) {
 
 	}	else
 		return vel_vec
-}
+}
+
+robot_radius = 1.0
+safety_radius = 2.0
+combined_radius = 2 * robot_radius + safety_radius
+vel_sample_count = 50
+
+velocities = {}
+
+function in_between(theta_right, theta_dif, theta_left) {
+  if(math.abs(theta_right - theta_left) < (math.pi - 0.000001)) {
+    if((theta_right <= theta_dif) and (theta_dif <= theta_left)) {
+      return 1
+    } else {
+      return 0
+    }    
+  } else if(math.abs(theta_right - theta_left) > (math.pi + 0.000001)) {
+    if((theta_right <= theta_dif) or (theta_dif <= theta_left)) {
+      return 1
+    } else {
+      return 0
+    }
+  } else {
+    # Exactly pi rad between right and left
+    if(theta_right <= 0) {
+      if((theta_right <= theta_dif) and (theta_dif <= theta_left)) {
+        return 1
+      } else {
+        return 0
+      }
+    } else if(theta_left <= 0) { 
+      if((theta_right <= theta_dif) or (theta_dif <= theta_left)) {
+        return 1
+      } else {
+        return 0
+      }
+    }
+  }
+}
+
+# VO  magic happens here
+function RVO(preferedVelocity) {
+
+  #data_string = string.concat(data_string, ",", string.tostring(preferedVelocity.x), ",", string.tostring(preferedVelocity.y), ",", string.tostring(preferedVelocity.z))
+
+  final_V = math.vec2.new(0.0, 0.0)
+  collision = 0
+  suitable_V = {}
+
+  var VO_all = {}
+
+  neighbors.foreach(
+    function(rid, data) {
+
+      var angle = data.azimuth#-(data.azimuth * 0.017454)
+
+      #data_string = string.concat(data_string, ",", string.tostring(data.distance), ",", string.tostring(angle))
+
+      var distance = data.distance
+      if(distance <= combined_radius) distance = combined_radius
+
+      theta_BA_ort = math.asin(combined_radius / distance)
+      theta_BA_left = angle + theta_BA_ort
+      if(theta_BA_left > math.pi) {
+        theta_BA_left = theta_BA_left - 2 * math.pi
+      } else if(theta_BA_left < -math.pi) {
+        theta_BA_left = theta_BA_left + 2 * math.pi
+      }
+      theta_BA_right = angle - theta_BA_ort
+      if(theta_BA_right > math.pi) {
+        theta_BA_right = theta_BA_right - 2 * math.pi
+      } else if(theta_BA_right < -math.pi) {
+        theta_BA_right = theta_BA_right + 2 * math.pi
+      }
+
+      neighbor_velocity = velocities[rid]
+      if(neighbor_velocity == nil) {
+        neighbor_velocity = math.vec2.new(0.0, 0.0)
+      }
+
+      #data_string = string.concat(data_string, ",", string.tostring(neighbor_velocity.x), ",", string.tostring(neighbor_velocity.y), ",", string.tostring(neighbor_velocity.z))
+
+      VO_all[rid] = { 
+        .velocity = math.vec2.new(neighbor_velocity.x, neighbor_velocity.y),
+        .theta_left = theta_BA_left,
+        .theta_right = theta_BA_right
+      }
+
+    }
+  )
+
+  # Detect collision
+  foreach(VO_all, function(rid, vo) {
+    vAB = math.vec2.sub(preferedVelocity, vo.velocity)
+    var vel_angle = math.acos(vAB.x / math.vec2.length(vAB))
+    if(vAB.y < 0) vel_angle = vel_angle * -1
+
+    if(in_between(vo.theta_right, vel_angle, vo.theta_left)) {
+      collision = 1
+    }
+  })
+
+  # Calculate suitable velocities
+  if(collision) {
+
+	log("")
+	log("------>  AVOIDING NEIGHBOR! (RVO) <------")
+	log("")
+
+    var idx = 0
+	var n = 0
+    while (n < vel_sample_count) {
+
+      v_cand = math.vec2.new( 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL, 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL)
+      while(math.vec2.length(v_cand) > GOTO_MAXVEL) {
+        v_cand = math.vec2.new( 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL, 2.0 * math.rng.uniform(GOTO_MAXVEL) - GOTO_MAXVEL)
+      }
+
+      suit = 1
+
+      foreach(VO_all, function(rid, vo) {
+        #vAB = new_V
+        vAB = math.vec2.sub(v_cand, vo.velocity)
+        #vAB = math.vec3.sub(new_V, math.vec3.scale(math.vec3.add(preferedVelocity, vo.velocity), 0.5))
+        var vel_angle = math.acos(vAB.x / math.vec2.length(vAB))
+        if(vAB.y < 0) vel_angle = vel_angle * -1
+
+        if(in_between(vo.theta_right, vel_angle, vo.theta_left)) {
+          suit = 0
+        }
+      })
+
+      if(suit) {
+        suitable_V[idx] = v_cand
+        idx = idx + 1
+      }
+	  n = n + 1
+    }
+
+    # Chose a velocity closer to the desired one
+    if(size(suitable_V) > 0) {
+      min_dist = 99999
+	  sv = 0
+      while(sv < size(suitable_V)) {
+        var RVOdist = math.vec2.length(math.vec2.sub(suitable_V[sv], preferedVelocity))
+        if(RVOdist < min_dist) {
+          min_dist = RVOdist
+          final_V = suitable_V[sv]
+        }
+		sv = sv + 1
+      }
+    }
+
+    #data_string = string.concat(data_string, ",", string.tostring(final_V.x), ",", string.tostring(final_V.y), ",", string.tostring(final_V.z))
+	
+	broadcast_velocity(final_V)
+
+    return final_V
+  } else {
+
+    #data_string = string.concat(data_string, ",", string.tostring(preferedVelocity.x), ",", string.tostring(preferedVelocity.y), ",", string.tostring(preferedVelocity.z))
+
+	broadcast_velocity(preferedVelocity)
+    return preferedVelocity
+  }
+}
+
+function broadcast_velocity(velocity) {
+    neighbors.broadcast(string.concat("v", string.tostring(id)), velocity)
+}
+
+function setup_velocity_callbacks() {
+	r = 1
+    while(r <= ROBOTS) {
+        if(r != id) {
+            neighbors.listen(string.concat("v", string.tostring(r)),
+                function(vid, value, rid) {
+                    velocities[rid] = value
+                }
+            )
+        }
+		r = r + 1
+    }    
+}

buzz_scripts/include/act/barrier.bzz

@@ -18,21 +18,7 @@ hvs = 0;
 # Sets a barrier
 #
 function barrier_create() {
-  # reset
   timeW = 1
-  # create barrier vstig
-  #log("---> Prev. br. ", barrier, " ", BARRIER_VSTIG)
-  #if(barrier!=nil) {
-  #  barrier=nil
-    #if(hvs) {
-    #  BARRIER_VSTIG = BARRIER_VSTIG -1
-    #  hvs = 0
-    #}else{
-    #  BARRIER_VSTIG = BARRIER_VSTIG +1
-    #  hvs = 1
-    #}
-  #}
-  #log("---> New. br. ", barrier, " ", BARRIER_VSTIG)
   if(barrier==nil)
     barrier = stigmergy.create(BARRIER_VSTIG)
 }
@@ -95,6 +81,43 @@ function barrier_wait(threshold, transf, resumef, bc) {
   timeW = timeW+1
 }
 
+function barrier_wait_graph(threshold, transf, resumef, bc) {
+  if(barrier==nil) #failsafe
+    barrier_create()
+  
+  barrier.put(id, bc)
+  if(barrier.get("n")<threshold)
+    barrier.put("n", threshold)
+  if(threshold>1) {
+    neighbors.foreach(function (nei,data){
+      barrier.get(nei)
+    })
+  }
+
+  # Not to miss any RC inputs, for waypoints, potential and deploy states
+	check_rc_wp()
+
+  #log("--->BS: ", barrier.size(), " / ", threshold, " (", BARRIER_VSTIG, " - ", barrier.get("d"), ") t= ", timeW)
+  if(barrier.size()-2 >= barrier.get("n")) {
+    if(barrier_allgood(barrier,bc)) {
+      barrier.put("d", 1)
+      timeW = 0
+      GRAPHSTATE = transf
+    } else 
+      barrier.put("d", 0)
+  }
+
+  if(timeW >= BARRIER_TIMEOUT) {
+    log("------> Barrier Timeout !!!!")
+    #barrier = nil
+    timeW = 0
+    GRAPHSTATE = resumef
+  } else if(timeW % 10 == 0 and bc > 0)
+    neighbors.broadcast("cmd", bc)
+
+  timeW = timeW+1
+}
+
 # Check all members state
 function barrier_allgood(barrier, bc) {
   barriergood = 1

buzz_scripts/include/act/naviguation.bzz

@@ -1,23 +1,9 @@
-GOTO_MAXVEL = 12.5 # m/steps
+GOTO_MAXVEL = 2.5 # m/steps
 GOTO_MAXDIST = 250 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
-# Geofence polygon
+
-# CEPSUM field
+include "utils/field_config.bzz"
-#GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
-#			.2={.lat=45.510896, .lng=-73.608731},
-#			.3={.lat=45.510355, .lng=-73.608404},
-#			.4={.lat=45.509840, .lng=-73.610072}}
-# Pangeae final site
-#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315},
-#			.2={.lat=29.068724, .lng=-13.662634},
-#			.3={.lat=29.068113, .lng=-13.661427},
-#			.4={.lat=29.067014, .lng=-13.661564}}
-# Pangeae first site
-GPSlimit = {.1={.lat=29.021055, .lng=-13.715155},
-		.2={.lat=29.021055, .lng=-13.714132},
-		.3={.lat=29.019470, .lng=-13.714132},
-		.4={.lat=29.019470, .lng=-13.715240}} 
 
 # Core naviguation function to travel to a GPS target location.
 function goto_gps(transf) {
@@ -30,8 +16,12 @@ function goto_gps(transf) {
 	} else {
 		m_navigation = LimitSpeed(m_navigation, 1.0)
 		gf = {.0=m_navigation, .1=vec_from_gps(GPSlimit[1].lat, GPSlimit[1].lng, 0), .2=vec_from_gps(GPSlimit[2].lat, GPSlimit[2].lng, 0), .3=vec_from_gps(GPSlimit[3].lat, GPSlimit[3].lng, 0), .4=vec_from_gps(GPSlimit[4].lat, GPSlimit[4].lng, 0)}
+		#TODO: ENSURE THAT IF WE GO OUT WE CAN GET BACK IN!!!
 		geofence(gf)
-		#m_navigation = LCA(m_navigation)
+		if(CA_ON==1)
+			m_navigation = LCA(m_navigation)
+		if(CA_ON==2)
+			m_navigation = RVO(m_navigation)
 		goto_abs(m_navigation.x, m_navigation.y, cur_goal.altitude - pose.position.altitude, 0.0)
 	}
 }

buzz_scripts/include/act/neighborcomm.bzz

@@ -59,7 +59,7 @@ function rc_cmd_listen() {
 				destroyGraph()
 				resetWP()
 				check_rc_wp()
-				barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
+				barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 903)
 				barrier_ready(903)
 				neighbors.broadcast("cmd", 903)
 			} else if (flight.rc_cmd==904){
@@ -100,11 +100,11 @@ function nei_cmd_listen() {
 							if(BVMSTATE!=AUTO_LAUNCH_STATE)
 								barrier_set(ROBOTS, "GOHOME", BVMSTATE, 20)
 							#barrier_ready(20)
-						} else if(value==900){	# Shapes
+						} else if(value==900 and BVMSTATE!="TASK_ALLOCATE"){	# Shapes
 							barrier_set(ROBOTS, "TASK_ALLOCATE", BVMSTATE, 900)
 							#barrier_ready(900)
 							#neighbors.broadcast("cmd", 900)
-						} else if(value==901 and BVMSTATE!="DEPLOY" ){	# Pursuit
+						} else if(value==901 and BVMSTATE!="DEPLOY" ){	# Voronoi
 							destroyGraph()
 							barrier_set(ROBOTS, "DEPLOY", BVMSTATE, 901)
 							#barrier_ready(901)
@@ -114,9 +114,9 @@ function nei_cmd_listen() {
 							barrier_set(ROBOTS, "WAYPOINT", BVMSTATE, 902)
 							#barrier_ready(902)
 							#neighbors.broadcast("cmd", 902)
-						} else if(value==903 and BVMSTATE!="POTENTIAL" and BVMSTATE!="TURNEDOFF"){	# Formation
+						} else if(value==903 and BVMSTATE!="PURSUIT" and BVMSTATE!="TURNEDOFF"){	# Pursuit
 							destroyGraph()
-							barrier_set(ROBOTS, "POTENTIAL", BVMSTATE, 903)
+							barrier_set(ROBOTS, "PURSUIT", BVMSTATE, 903)
 							#barrier_ready(903)
 							#neighbors.broadcast("cmd", 903)
 						} else if(value==904 and BVMSTATE!="IDLE" and BVMSTATE!="TURNEDOFF"){	# idle

buzz_scripts/include/act/states.bzz

@@ -276,7 +276,7 @@ function aggregate() {
 # State fucntion to circle all together around centroid
 function pursuit() {
   BVMSTATE="PURSUIT"
-	rd = 20.0
+	rd = 30.0
 	pc = 3.2
 	vd = 15.0
   r_vec = neighbors.reduce(function(rid, data, r_vec) {

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -11,8 +11,9 @@ include "taskallocate/graphs/shapes_Y.bzz"
 ROBOT_RADIUS = 50
 ROBOT_DIAMETER = 2.0*ROBOT_RADIUS
 ROBOT_SAFETYDIST = 2.0*ROBOT_DIAMETER
-ROOT_ID = 2
+GRAPHSTATE="NONE"
-graph_id = 3
+ROOT_ID = 0
+graph_id = 0
 graph_loop = 1
 
 #
@@ -32,12 +33,12 @@ m_MessageBearing={}#store received neighbour message
 m_neighbourCount=0#used to cunt neighbours
 #Save message from one neighbour
 #the indexes are as State(received state),Label(received Label),ReqLabel,ReqID,Response,Range,Bearing
-m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
+m_receivedMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
 
 #
 #Save the message to send
 #The keys of the talbe is State(current state),Label(current Label),ReqLabel(requested Label),ReqID(request id),Response(reply message{REQ_NONE,REQ_GRANTED,REQ_RESEND})
-m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
+m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
 
 #navigation vector
 m_navigation={.x=0,.y=0}
@@ -240,7 +241,7 @@ neighbors.listen("m",
     Get_DisAndAzi(temp_id)
     #add the received message
     #
-    m_MessageState[m_neighbourCount]=i2s(recv_val.State)
+    m_MessageState[m_neighbourCount]=i2s_graph(recv_val.State)
     m_MessageLabel[m_neighbourCount]=recv_val.Label
     m_MessageReqLabel[m_neighbourCount]=recv_val.ReqLabel
     m_MessageReqID[m_neighbourCount]=recv_val.ReqID
@@ -249,7 +250,7 @@ neighbors.listen("m",
     m_MessageBearing[m_neighbourCount]=m_receivedMessage.Bearing
 	m_messageID[m_neighbourCount]=rid
 
-#log(rid, " is in ",  m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
+log(rid, " is in ",  m_MessageState[m_neighbourCount], " ", m_MessageLabel[m_neighbourCount])
 
     m_neighbourCount=m_neighbourCount+1
 })
@@ -270,7 +271,7 @@ function Get_DisAndAzi(t_id){
 #Update node info according to neighbour robots
 #
 function UpdateNodeInfo(){
-	#Collect informaiton
+	#Collect information
 	#Update information
 	i=0
 
@@ -301,13 +302,13 @@ function UpdateNodeInfo(){
 # Do free
 #
 function DoFree() {
-  if(BVMSTATE!="GRAPH_FREE"){
+  if(GRAPHSTATE!="GRAPH_FREE"){
-	BVMSTATE="GRAPH_FREE"
+	GRAPHSTATE="GRAPH_FREE"
 	m_unWaitCount=m_unLabelSearchWaitTime
-	m_selfMessage.State=s2i(BVMSTATE)
+	m_selfMessage.State=s2i_graph(GRAPHSTATE)
   }else{
 	UpdateGraph()
-	m_selfMessage.State=s2i(BVMSTATE)
+	m_selfMessage.State=s2i_graph(GRAPHSTATE)
 
 	#wait for a while before looking for a Label
 	if(m_unWaitCount>0)
@@ -316,28 +317,28 @@ function DoFree() {
 	#find a set of joined robots
 	var setJoinedLabels={}
 	var setJoinedIndexes={}
-	i=0
+	neighbor_it=0
-	j=0
+	joined_it=0
-	while(i<m_neighbourCount){
+	while(neighbor_it<m_neighbourCount){
-		if(m_MessageState[i]=="GRAPH_JOINED"){
+		if(m_MessageState[neighbor_it]=="GRAPH_JOINED"){
-		setJoinedLabels[j]=m_MessageLabel[i]
+		setJoinedLabels[joined_it]=m_MessageLabel[neighbor_it]
-		setJoinedIndexes[j]=i
+		setJoinedIndexes[joined_it]=neighbor_it
-		j=j+1
+		joined_it=joined_it+1
 		}
-		i=i+1
+		neighbor_it=neighbor_it+1
 	}
 
 	#go through the graph to look for a proper Label
 	var unFoundLabel=0
 #	var IDofPred=0
-	i=1
+	vecNode_it=1
-	while(i<size(m_vecNodes) and (unFoundLabel==0)){
+	while(vecNode_it<size(m_vecNodes) and (unFoundLabel==0)){
 		#if the node is unassigned and the predecessor is insight
-		if(m_vecNodes[i].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[i].Pred)==1){
+		if(m_vecNodes[vecNode_it].State=="UNASSIGNED" and count(setJoinedLabels,m_vecNodes[vecNode_it].Pred)==1){
-			unFoundLabel=m_vecNodes[i].Label
+			unFoundLabel=m_vecNodes[vecNode_it].Label
 #			IDofPred=find(m_MessageLabel,m_vecNodes[unFoundLabel].Pred)
 		}
-		i=i+1
+		vecNode_it=vecNode_it+1
 	}
 
 	if(unFoundLabel>0){
@@ -347,7 +348,7 @@ function DoFree() {
 	}
 
 	#set message
-	m_selfMessage.State=s2i(BVMSTATE)
+	m_selfMessage.State=s2i_graph(GRAPHSTATE)
 
 	BroadcastGraph()
   }
@@ -356,11 +357,11 @@ function DoFree() {
 #Do asking
 #
 function DoAsking(){
-	if(BVMSTATE!="GRAPH_ASKING"){
+	if(GRAPHSTATE!="GRAPH_ASKING"){
-	  BVMSTATE="GRAPH_ASKING"
+	  GRAPHSTATE="GRAPH_ASKING"
 	  #math.rng.setseed(id)
 	  m_unRequestId=id#math.rng.uniform(0,10)	#don't know why the random numbers are the same, add id to make the ReqID different
-	  m_selfMessage.State=s2i(BVMSTATE)
+	  m_selfMessage.State=s2i_graph(GRAPHSTATE)
 	  m_selfMessage.ReqLabel=m_nLabel
 	  m_selfMessage.ReqID=m_unRequestId
 	  m_unWaitCount=m_unResponseTimeThreshold
@@ -390,7 +391,7 @@ function DoAsking(){
 			#no response, wait
 
 			m_unWaitCount=m_unWaitCount-1
-			m_selfMessage.State=s2i(BVMSTATE)
+			m_selfMessage.State=s2i_graph(GRAPHSTATE)
 			m_selfMessage.ReqLabel=m_nLabel
 			m_selfMessage.ReqID=m_unRequestId
 			#if(m_unWaitCount==0){
@@ -428,9 +429,9 @@ function DoAsking(){
 #Do joining
 #
 function DoJoining(){
-  if(BVMSTATE!="GRAPH_JOINING") {
+  if(GRAPHSTATE!="GRAPH_JOINING") {
-	BVMSTATE="GRAPH_JOINING"
+	GRAPHSTATE="GRAPH_JOINING"
-    m_selfMessage.State=s2i(BVMSTATE)
+    m_selfMessage.State=s2i_graph(GRAPHSTATE)
     m_selfMessage.Label=m_nLabel
 	m_unWaitCount=m_unJoiningLostPeriod
   } else {
@@ -440,7 +441,7 @@ function DoJoining(){
 	else
 		goto_gps(DoJoined)
 		#pack the communication package
-		m_selfMessage.State=s2i(BVMSTATE)
+		m_selfMessage.State=s2i_graph(GRAPHSTATE)
 		m_selfMessage.Label=m_nLabel
 		BroadcastGraph()
   }
@@ -477,9 +478,9 @@ function set_rc_goto() {
 #Do joined
 #
 function DoJoined(){
-	if(BVMSTATE!="GRAPH_JOINED"){
+	if(GRAPHSTATE!="GRAPH_JOINED"){
-	  BVMSTATE="GRAPH_JOINED"
+	  GRAPHSTATE="GRAPH_JOINED"
-	  m_selfMessage.State=s2i(BVMSTATE)
+	  m_selfMessage.State=s2i_graph(GRAPHSTATE)
 	  m_selfMessage.Label=m_nLabel
 	  m_vecNodes[m_nLabel].State="ASSIGNED"
 
@@ -492,7 +493,7 @@ function DoJoined(){
 	  goto_abs(m_navigation.x/100.0, m_navigation.y/100.0, 0.0, 0.0)
 	} else {
 		UpdateGraph()
-		m_selfMessage.State=s2i(BVMSTATE)
+		m_selfMessage.State=s2i_graph(GRAPHSTATE)
 		m_selfMessage.Label=m_nLabel
 
 		#collect all requests
@@ -571,7 +572,7 @@ function DoJoined(){
 		}
 		}
 	# using JOINED as the resume state kinds of just reset the barrier timeout, should be IDLE/ALLOCATE...
-	barrier_wait(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
+	barrier_wait_graph(ROBOTS, "GRAPH_LOCK", "GRAPH_JOINED", -1)
 	BroadcastGraph()
   }
 }
@@ -581,8 +582,8 @@ function DoJoined(){
 timeout_graph = 40
 function DoLock() {
   #[transition to lock...
-	BVMSTATE="GRAPH_LOCK"
+	GRAPHSTATE="GRAPH_LOCK"
-	m_selfMessage.State=s2i(BVMSTATE)
+	m_selfMessage.State=s2i_graph(GRAPHSTATE)
 	m_selfMessage.Label=m_nLabel
 	m_vecNodes[m_nLabel].State="ASSIGNED"
 	#record neighbor distance
@@ -602,7 +603,7 @@ function DoLock() {
 	neighbors.ignore("m")
   #..]
 	UpdateGraph()
-	m_selfMessage.State=s2i(BVMSTATE)
+	m_selfMessage.State=s2i_graph(GRAPHSTATE)
 	m_selfMessage.Label=m_nLabel
 	m_navigation.x=0.0
 	m_navigation.y=0.0
@@ -621,12 +622,12 @@ function DoLock() {
 
 	if(graph_loop) {
 	  if(timeout_graph==0) {
-		if(graph_id < 3)
+		if(graph_id < 1)
 			graph_id = graph_id + 1
 		else
 			graph_id = 0
 		timeout_graph = 40
-		BVMSTATE="TASK_ALLOCATE"
+		GRAPHSTATE="NONE"
 	  }
 	 	timeout_graph = timeout_graph - 1
 	}
@@ -652,7 +653,7 @@ function UpdateGraph() {
 	#update the graph
 	UpdateNodeInfo()
 	#reset message package to be sent
-	m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
+	m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
 }
 
 function BroadcastGraph() {
@@ -676,8 +677,8 @@ function BroadcastGraph() {
 # Executed when reset
 #
 function resetGraph(){
-	m_receivedMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
+	m_receivedMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE"),.Range=0,.Bearing=0}
-	m_selfMessage={.State=s2i("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
+	m_selfMessage={.State=s2i_graph("GRAPH_FREE"),.Label=0,.ReqLabel=0,.ReqID=0,.Response=r2i("REQ_NONE")}
 	m_navigation={.x=0,.y=0}
 	m_nLabel=-1
 	m_messageID={}
@@ -695,15 +696,15 @@ function resetGraph(){
 	if(graph_id==0){
 		log("Loading P graph")
 		Read_GraphP()
+#	} else if(graph_id==1) {
+#		log("Loading O graph")
+#		Read_GraphO()
 	} else if(graph_id==1) {
-		log("Loading O graph")
-		Read_GraphO()
-	} else if(graph_id==2) {
 		log("Loading L graph")
 		Read_GraphL()
-	} else if(graph_id==3) {
+#	} else if(graph_id==3) {
-		log("Loading Y graph")
+#		log("Loading Y graph")
-		Read_GraphY()
+#		Read_GraphY()
 	}
 
 	#start listening
@@ -719,6 +720,25 @@ function resetGraph(){
 		DoFree()
 	}
 }
+
+function graph_state(){
+	if(GRAPHSTATE=="GRAPH_FREE")
+		stateG=DoFree
+	else if(GRAPHSTATE=="GRAPH_ASKING")
+		stateG=DoAsking
+	else if(GRAPHSTATE=="GRAPH_JOINING")
+		stateG=DoJoining
+	else if(GRAPHSTATE=="GRAPH_JOINED")
+		stateG=DoJoined
+	else if(GRAPHSTATE=="GRAPH_LOCK")	# move all together (TODO: not tested), but auto-loop in graph list
+		stateG=DoLock
+	else
+		stateG=resetGraph
+	
+	stateG()
+
+	log("Current graph state: ", GRAPHSTATE)
+}
 #
 # Executed upon destroy
 #

buzz_scripts/include/utils/conversions.bzz

@@ -62,17 +62,14 @@ function gps_from_vec(vec) {
   return Lgoal
 }
 
-GPSoffset = {.lat=45.50, .lng=-73.61}
-GPSoffsetL = {.lat=29.01, .lng=-13.70}
-
 function packWP2i(in_lat, in_long, processed) {
-  var dlat = math.round((in_lat - GPSoffsetL.lat)*1000000)
+  var dlat = math.round((in_lat - GPSoffset.lat)*1000000)
-  var dlon = math.round((in_long - GPSoffsetL.lng)*1000000)
+  var dlon = math.round((in_long - GPSoffset.lng)*1000000)
   return {.dla=dlat, .dlo=dlon*10+processed}
 }
 
 function unpackWP2i(wp_int){
   var dlon = (wp_int.dlo-wp_int.dlo%10)/10.0
   var pro = wp_int.dlo-dlon*10
-  return {.lat=wp_int.dla/1000000.0+GPSoffsetL.lat, .lng=dlon/1000000.0+GPSoffsetL.lng, .pro=pro}
+  return {.lat=wp_int.dla/1000000.0+GPSoffset.lat, .lng=dlon/1000000.0+GPSoffset.lng, .pro=pro}
 }

buzz_scripts/include/utils/field_config.bzz

@@ -0,0 +1,20 @@
+# Geofence polygon
+# CEPSUM field
+#GPSlimit = {.1={.lat=45.510400, .lng=-73.610421},
+#			.2={.lat=45.510896, .lng=-73.608731},
+#			.3={.lat=45.510355, .lng=-73.608404},
+#			.4={.lat=45.509840, .lng=-73.610072}}
+# Pangeae final site
+#GPSlimit = {.1={.lat=29.067746, .lng=-13.663315},
+#			.2={.lat=29.068724, .lng=-13.662634},
+#			.3={.lat=29.068113, .lng=-13.661427},
+#			.4={.lat=29.067014, .lng=-13.661564}}
+# Pangeae first site
+GPSlimit = {.1={.lat=29.021055, .lng=-13.715155},
+		.2={.lat=29.021055, .lng=-13.714132},
+		.3={.lat=29.019470, .lng=-13.714132},
+		.4={.lat=29.019470, .lng=-13.715240}} 
+
+
+#GPSoffset = {.lat=45.50, .lng=-73.61}
+GPSoffset = {.lat=29.01, .lng=-13.70}

buzz_scripts/include/utils/table.bzz

@@ -34,7 +34,9 @@ function count(table,value){
 # map from int to state - vstig serialization limits to 9....
 #
 function i2s(value){
-	if(value==1){
+	if(value==0){
+	return "PURSUIT"
+	}else if(value==1){
 	return "IDLE"
 	}
 	else if(value==2){
@@ -50,7 +52,7 @@ function i2s(value){
 	return "BARRIERWAIT"
 	}
   else if(value==6){
-	return "INDIWP"
+	return "WAYPOINT"
 	}
   else if(value==7){
 	return "GOHOME"
@@ -59,7 +61,27 @@ function i2s(value){
 	return "LAUNCH"
 	}
   else if(value==9){
-	return "FORMATION"
+	return "TASK_ALLOCATE"
+	}
+  else {
+	return "UNDETERMINED"
+	}
+}
+function i2s_graph(value){
+	if(value==1){
+	return "GRAPH_FREE"
+	}
+	else if(value==2){
+	return "GRAPH_ASKING"
+	}
+	else if(value==3){
+	return "GRAPH_JOINING"
+	}
+	else if(value==4){
+	return "GRAPH_JOINED"
+	}
+	else if(value==5){
+	return "GRAPH_LOCK"
 	}
   else {
 	return "UNDETERMINED"
@@ -69,7 +91,9 @@ function i2s(value){
 # map from state to int - vstig serialization limits to 9....
 #
 function s2i(value){
-	if(value=="IDLE"){
+	if(value=="PURSUIT"){
+	return 0
+	}else if(value=="IDLE"){
 	return 1
 	}
 	else if(value=="DEPLOY"){
@@ -93,9 +117,29 @@ function s2i(value){
 	else if(value=="LAUNCH"){
 	return 8
 	}
-	else if(value=="POTENTIAL"){
+	else if(value=="TASK_ALLOCATE"){
 	return 9
 	}
   else
-    return 0
+    return -1
+}
+function s2i_graph(value){
+	if(value=="GRAPH_FREE"){
+	return 1
+	}
+	else if(value=="GRAPH_ASKING"){
+	return 2
+	}
+	else if(value=="GRAPH_JOINING"){
+	return 3
+	}
+	else if(value=="GRAPH_JOINED"){
+	return 4
+	}
+	else if(value=="GRAPH_LOCK"){
+	return 5
+	}
+  else {
+	return 0
+	}
 }

buzz_scripts/main.bzz

@@ -80,21 +80,11 @@ function step() {
 	else if(BVMSTATE=="PURSUIT")
 		statef=pursuit
 	else if(BVMSTATE=="TASK_ALLOCATE")	# or bidding ?
-		statef=resetGraph
+		statef=graph_state
 	else if(BVMSTATE=="BIDDING")	# check the absolute path of the waypointlist csv file in bidding.bzz
 		statef=bidding
 	else if(BVMSTATE=="DEPLOY")	# check the absolute path of the waypointlist csv file in bidding.bzz
 		statef=voronoicentroid
-	else if(BVMSTATE=="GRAPH_FREE")
-		statef=DoFree
-	else if(BVMSTATE=="GRAPH_ASKING")
-		statef=DoAsking
-	else if(BVMSTATE=="GRAPH_JOINING")
-		statef=DoJoining
-	else if(BVMSTATE=="GRAPH_JOINED")
-		statef=DoJoined
-	else if(BVMSTATE=="GRAPH_LOCK")	# move all together (TODO: not tested), but auto-loop in graph list
-		statef=DoLock
 	else if(BVMSTATE=="PATHPLAN")	# ends on navigate, defined in rrtstar
 		statef=rrtstar
 	else if(BVMSTATE=="NAVIGATE")	# ends on idle, defined in rrtstar

include/rosbuzz/buzz_update.h

@@ -12,7 +12,7 @@
 #include <buzz/buzzdict.h>
 #include <buzz/buzzdarray.h>
 #include <buzz/buzzvstig.h>
-#include <buzz_utility.h>
+#include <rosbuzz/buzz_utility.h>
 #include <fstream>
 
 #define delete_p(p)                                                                                                    \

include/rosbuzz/buzz_utility.h

@@ -1,8 +1,8 @@
 #pragma once
 #include <stdio.h>
-#include "buzz_utility.h"
+#include <rosbuzz/buzz_utility.h>
-#include "buzzuav_closures.h"
+#include <rosbuzz/buzzuav_closures.h>
-#include "buzz_update.h"
+#include <rosbuzz/buzz_update.h>
 #include <buzz/buzzdebug.h>
 #include <stdlib.h>
 #include <string.h>
@@ -100,6 +100,8 @@ buzzvm_t get_vm();
 
 void set_robot_var(int ROBOTS);
 
+void set_ca_on_var(int CA_ON);
+
 int get_inmsg_size();
 
 std::vector<uint8_t*> get_inmsg_vector();

include/rosbuzz/buzzuav_closures.h

@@ -5,8 +5,8 @@
 #include "mavros_msgs/CommandCode.h"
 #include "mavros_msgs/Mavlink.h"
 #include "ros/ros.h"
-#include "buzz_utility.h"
+#include <rosbuzz/buzz_utility.h>
-#include "rosbuzz/mavrosCC.h"
+#include <rosbuzz/mavrosCC.h>
 
 #define EARTH_RADIUS (double)6371000.0
 #define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))

include/rosbuzz/roscontroller.h

@@ -35,8 +35,8 @@
 #include <signal.h>
 #include <ostream>
 #include <map>
-#include "buzzuav_closures.h"
+#include <rosbuzz/buzzuav_closures.h>
-#include "rosbuzz/mavrosCC.h"
+#include <rosbuzz/mavrosCC.h>
 
 /*
 * ROSBuzz message types
@@ -131,6 +131,7 @@ private:
   int rc_cmd;
   float fcu_timeout;
   int armstate;
+  int ca_on;
   int barrier;
   int update;
   int message_number = 0;

launch/rosbuzz.launch

@@ -12,6 +12,7 @@
   <arg name="setmode" default="false" />
   <arg name="latitude" default="0" />
   <arg name="longitude" default="0" />
+  <arg name="ca_on" default="2" />
 
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
     <rosparam file="$(find rosbuzz)/launch/launch_config/$(arg launch_config).yaml"/>
@@ -24,5 +25,6 @@
     <param name="latitude" value="$(arg latitude)"/>
     <param name="longitude" value="$(arg longitude)"/>
     <param name="stand_by" value="$(find rosbuzz)/buzz_scripts/stand_by.bzz"/>
+    <param name="ca_on" value="$(arg ca_on)"/>
   </node>
 </launch>

src/VoronoiDiagramGenerator.cpp

@@ -27,7 +27,7 @@
 * OF THIS SOFTWARE OR ITS FITNESS FOR ANY PARTICULAR PURPOSE.
 */
 
-#include "VoronoiDiagramGenerator.h"
+#include <rosbuzz/VoronoiDiagramGenerator.h>
 
 VoronoiDiagramGenerator::VoronoiDiagramGenerator()
 {

src/buzz_update.cpp

@@ -6,7 +6,7 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include "buzz_update.h"
+#include <rosbuzz/buzz_update.h>
 
 namespace buzz_update
 {

src/buzz_utility.cpp

@@ -6,7 +6,7 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include "buzz_utility.h"
+#include <rosbuzz/buzz_utility.h>
 
 namespace buzz_utility
 {
@@ -565,6 +565,16 @@ void set_robot_var(int ROBOTS)
   buzzvm_gstore(VM);
 }
 
+void set_ca_on_var(int CA_ON)
+/*
+/ set swarm size in the BVM
+-----------------------------*/
+{
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "CA_ON", 1));
+  buzzvm_pushi(VM, CA_ON);
+  buzzvm_gstore(VM);
+}
+
 int get_inmsg_size()
 /*
 / get the incoming msgs size

src/buzzuav_closures.cpp

@@ -6,9 +6,9 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include "buzzuav_closures.h"
+#include <rosbuzz/buzzuav_closures.h>
 #include "math.h"
-#include "VoronoiDiagramGenerator.h"
+#include <rosbuzz/VoronoiDiagramGenerator.h>
 
 namespace buzzuav_closures
 {

src/rosbuzz.cpp

@@ -6,7 +6,7 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include "roscontroller.h"
+#include <rosbuzz/roscontroller.h>
 
 int main(int argc, char** argv)
 /*

src/roscontroller.cpp

@@ -6,7 +6,7 @@
  *  @copyright 2016 MistLab. All rights reserved.
  */
 
-#include "roscontroller.h"
+#include <rosbuzz/roscontroller.h>
 #include <thread>
 namespace rosbuzz_node
 {
@@ -130,6 +130,7 @@ void roscontroller::RosControllerRun()
       ROS_ERROR("BVMSTATE undeclared in .bzz file, please set BVMSTATE.");
     //  DEBUG
     // ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
+    buzz_utility::set_ca_on_var(ca_on);
     while (ros::ok() && !buzz_utility::buzz_script_done())
     {
       //  Publish topics
@@ -300,6 +301,9 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
   //  Obtain standby script to run during update
   n_c.getParam("stand_by", stand_by);
 
+ //  Obtain collision avoidance mode
+  n_c.getParam("ca_on", ca_on);
+
   if (n_c.getParam("xbee_plugged", xbeeplugged))
     ;
   else