tuned user tracking

include/roscontroller.h

@@ -38,7 +38,7 @@
 #define XBEE_MESSAGE_CONSTANT 586782343
 #define XBEE_STOP_TRANSMISSION 4355356352
 #define TIMEOUT	60
-#define BUZZRATE 50
+#define BUZZRATE 10
 
 using namespace std;
 

script/tentacle.bzz

@@ -0,0 +1,430 @@
+include "vec2.bzz"
+updated="update_ack"
+update_no=0
+function updated_neigh(){
+	neighbors.broadcast(updated, update_no)
+}
+TARGET_ALTITUDE = 5.0
+CURSTATE = "TURNEDOFF"
+ 
+# Lennard-Jones parameters
+TARGET     = 12.0
+EPSILON    = 10.0
+
+#################################################
+### UTILITY FUNCTIONS ###########################
+#################################################
+
+# Write a table as if it was a matrix
+function write_knowledge(k, row, col, val) {
+    var key = string.concat(string.tostring(row),"-",string.tostring(col))
+    k[key] = val
+    log("Writing knowledge:", val, " to ", row, " ", col)
+}
+
+# Read a table as if it was a matrix
+function read_knowledge(k, row, col) {
+    var key = string.concat(string.tostring(row),"-",string.tostring(col))
+    if (k[key] == nil) {
+        log("Warning: reading 'nil' value from the knowledge table at", row, " ", col, ", returning -1")
+        return -1
+    } else {
+        return k[key]
+    }
+}
+
+# Int to String 
+function itos(i) {
+    
+    log("Use 'string.tostring(OJB)' instead")
+    
+    if (i==0) { return "0" }
+    if (i==1) { return "1" }
+    if (i==2) { return "2" }
+    if (i==3) { return "3" }
+    if (i==4) { return "4" }
+    if (i==5) { return "5" }
+    if (i==6) { return "6" }
+    if (i==7) { return "7" }
+    if (i==8) { return "8" }
+    if (i==9) { return "9" }
+    
+    log("Function 'itos' out of bounds, returning the answer (42)")
+    return "42"
+}
+
+# String to Int
+function stoi(s) {
+   if (s=='0') { return 0 }
+   if (s=='1') { return 1 }
+   if (s=='2') { return 2 }
+   if (s=='3') { return 3 }
+   if (s=='4') { return 4 }
+   if (s=='5') { return 5 }
+   if (s=='6') { return 6 }
+   if (s=='7') { return 7 }
+   if (s=='8') { return 8 }
+   if (s=='9') { return 9 }
+   
+   log("Function 'stoi' out of bounds, returning the answer (42)")
+   return 42
+   
+}
+
+# Rads to degrees
+function rtod(r) {
+   return (r*(180.0/math.pi))
+}
+
+# Degrees to rads
+function dtor(d) {
+   return (math.pi*(d/180.0))
+}
+
+# Force angles in the (-180,180) interval
+function degrees_interval(a) {
+    var temp = a
+    while ((temp>360.0) or (temp<0.0)) {
+        if (temp > 360.0) {
+            temp = temp - 360.0
+        } else if (temp < 0.0){
+            temp = temp + 360.0
+        }
+    }
+    if (temp > 180.0) {
+        temp = temp - 360.0
+    }
+    return temp
+}
+
+# Force angles in the (-pi,pi) interval
+function radians_interval(a) {
+    var temp = a
+    while ((temp>2.0*math.pi) or (temp<0.0)) {
+        if (temp > 2.0*math.pi) {
+            temp = temp - 2.0*math.pi
+        } else if (temp < 0.0){
+            temp = temp + 2.0*math.pi
+        }
+    }
+    if (temp > math.pi) {
+        temp = temp - 2.0*math.pi
+    }
+    return temp
+}
+
+#################################################
+### MOVEMENT/COMMUNICATION PRIMITIVES ###########
+#################################################
+
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  moveto(accum.x, accum.y)
+}
+
+function inform_your_neighborhood() {    
+    # Reset to 0 the visibility of all neighbors
+    foreach(knowledge, function(key, value) {
+          column = string.sub(key, string.length(key)-1,string.length(key))
+          if (column=='3') { 
+              knowledge[key] = 0 
+          }     
+    })    
+    neighbors.foreach( function(rid, data) {               
+        # For each neighbor, send a message with its azimuth, as seen by the broadcasting robot
+        message_id = string.tostring(rid)
+        neighbors.broadcast(message_id, rtod(data.azimuth))        
+        # Record the neighbor azimuth in my own knowledge table
+        write_knowledge(knowledge, rid, 0, rtod(data.azimuth))        
+        # Record the neighbor distance in my own knowledge table
+        write_knowledge(knowledge, rid, 2, data.distance)        
+        # Set neighbor as visible
+        write_knowledge(knowledge, rid, 3, 1)     
+    })    
+    # Send a message with the desired direction, as seen by the broadcasting robot
+    neighbors.broadcast("direction", local_dir)
+    
+}
+
+function listen_to_your_neighborhood() {    
+    # For all "senders" in my neighborhood, record my azimuth, as seen by them
+    message_id = string.tostring(id)
+    neighbors.listen(message_id, function(vid, value, rid) {
+        write_knowledge(knowledge, rid, 1, value)
+    })    
+}
+
+#################################################
+### ACTUAL CONTROLLERS ##########################
+#################################################
+
+function zero() {   
+  # Do not move
+  moveto(0.0,0.0) 
+  # Tell the neighbors of the center where to go
+  inform_your_neighborhood()       
+}
+
+function onetwo() {
+  if (id == 1) {
+      angle = 45
+  } else {
+      angle = -135
+  }
+  # Broadcast information
+  inform_your_neighborhood()   
+  # If the center 0 is in sight
+  if (read_knowledge(knowledge, 0, 3) == 1) {
+      arm_offset = degrees_interval(read_knowledge(knowledge, 0, 1) - angle)
+      if (arm_offset<3 and arm_offset>(-3)) {
+          hexagon()  # Underlying Lennard-Jones potential behavior
+      } else {
+      
+          local_rotation = degrees_interval( read_knowledge(knowledge, 0, 1) + (180.0 - read_knowledge(knowledge, 0, 0)) )
+          local_arm = degrees_interval(angle - local_rotation)
+          
+          if (read_knowledge(knowledge, 0, 2) > 250.0) { 
+              x_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0)))
+              y_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))       
+          } else if (read_knowledge(knowledge, 0, 2) < 30.0) {
+              x_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0)))
+              y_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))              
+          } else {
+              spiraling = 2.0+(id/10.0) # Fun stuff but be careful with this, it affects how a robots turns around a central node, use random number generation, eventually
+              if (arm_offset > 0) { # Clockwise
+
+                  x_mov = -math.sin(dtor(read_knowledge(knowledge, 0, 0)))
+                  y_mov = math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
+              } else { # Counterclockwise
+                  x_mov = math.sin(dtor(read_knowledge(knowledge, 0, 0)))
+                  y_mov = -math.cos(dtor(read_knowledge(knowledge, 0, 0))) * spiraling
+              }
+          }
+          speed = 100
+          moveto(speed * x_mov,speed * y_mov)
+      }
+  } else {
+      hexagon()            
+  }            
+}
+
+########################################
+#
+# BARRIER-RELATED FUNCTIONS
+#
+########################################
+
+#
+# Constants
+#
+BARRIER_VSTIG = 1
+
+#
+# Sets a barrier
+#
+function barrier_set(threshold, transf) {
+  statef = function() {
+    barrier_wait(threshold, transf);
+  }
+  barrier = stigmergy.create(BARRIER_VSTIG)
+}
+
+#
+# Make yourself ready
+#
+function barrier_ready() {
+  barrier.put(id, 1)
+}
+
+#
+# Executes the barrier
+#
+WAIT_TIMEOUT = 200
+timeW=0
+function barrier_wait(threshold, transf) {
+  barrier.get(id)
+  barrier.put(id, 1)
+  CURSTATE = "BARRIERWAIT"
+  if(barrier.size() >= threshold) {
+    #barrier = nil
+    transf()
+  } else if(timeW>=WAIT_TIMEOUT) {
+    barrier = nil
+    statef=land
+    timeW=0
+  }
+  timeW = timeW+1
+}
+
+# flight status
+
+function idle() {
+statef=idle
+CURSTATE = "IDLE"
+   
+}
+
+function takeoff() {
+	CURSTATE = "TAKEOFF"
+	statef=takeoff
+	#log("TakeOff: ", flight.status)
+	#log("Relative position: ", position.altitude)
+	
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		if(id==0)
+            barrier_set(ROBOTS, zero)
+        else
+            barrier_set(ROBOTS, onetwo)
+		barrier_ready()
+		#statef=hexagon
+	}
+	else {
+		log("Altitude: ", TARGET_ALTITUDE)
+		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
+	}
+}
+function land() {
+	CURSTATE = "LAND"
+	statef=land
+	#log("Land: ", flight.status)
+	if(flight.status == 2 or flight.status == 3){
+		neighbors.broadcast("cmd", 21)
+		uav_land()
+	}
+	else {
+		barrier_set(ROBOTS,idle)
+		barrier_ready()
+		timeW=0
+		#barrier = nil
+		#statef=idle
+	}
+}
+
+function users_save(t) {
+	if(size(t)>0) {
+		foreach(t, function(id, tab) {
+				#log("id: ",id," Latitude ", tab.la, "Longitude ", tab.lo)
+				add_user_rb(id,tab.la,tab.lo)
+			})
+	}
+}
+
+# printing the contents of a table: a custom function
+function table_print(t) {
+  if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+			})
+	}
+}
+
+#################################################
+### BUZZ FUNCTIONS ##############################
+#################################################
+
+# Executed at init time
+function init() {
+  s = swarm.create(0)
+  s.join()
+  # Local knowledge table
+  knowledge = {}
+  # Update local knowledge with information from the neighbors
+  listen_to_your_neighborhood()
+  # Variables initialization
+  iteration = 0 
+
+    vt = stigmergy.create(5)
+	t = {}
+	vt.put("p",t)
+	statef=idle
+	CURSTATE = "IDLE"
+}
+
+# Executed every time step
+function step() {
+    if(flight.rc_cmd==22) {
+		log("cmd 22")
+		flight.rc_cmd=0
+		statef = takeoff
+		CURSTATE = "TAKEOFF"		
+		neighbors.broadcast("cmd", 22)
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
+		flight.rc_cmd=0
+		statef = land
+		CURSTATE = "LAND"
+		neighbors.broadcast("cmd", 21)
+	} else if(flight.rc_cmd==16) {
+		flight.rc_cmd=0
+		statef = idle
+		#uav_goto()
+		add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
+	} else if(flight.rc_cmd==400) {
+		flight.rc_cmd=0
+		uav_arm()
+		neighbors.broadcast("cmd", 400)
+	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
+		uav_disarm()
+		neighbors.broadcast("cmd", 401)
+	}
+neighbors.listen("cmd",
+   function(vid, value, rid) {
+      print("Got (", vid, ",", value, ") from robot #", rid)
+	if(value==22 and CURSTATE=="IDLE") {
+		statef=takeoff
+	} else if(value==21) {
+		statef=land
+	} else if(value==400 and CURSTATE=="IDLE") {
+		uav_arm()
+	} else if(value==401 and CURSTATE=="IDLE"){
+		uav_disarm()
+	}
+   }
+
+)
+	statef()
+	log("Current state: ", CURSTATE)
+	log("Swarm size: ",ROBOTS)
+	
+	# Read a value from the structure
+		#		log(users)      
+				#users_print(users.dataG)
+				if(size(users.dataG)>0)
+					vt.put("p", users.dataG)
+				
+        # Get the number of keys in the structure
+        #log("The vstig has ", vt.size(), " elements")
+				users_save(vt.get("p"))
+				#table_print(users.dataL)
+
+  # Count the number of steps
+  iteration = iteration + 1   
+}
+
+# Executed once when the robot (or the simulator) is reset.
+function reset() {
+}
+# Execute at exit
+function destroy() {
+}

script/testflocksim.bzz

@@ -16,11 +16,12 @@ CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
 TARGET     = 12.0
-EPSILON    = 0.5
+EPSILON    = 14.0
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {
   return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+  #return -(4 * epsilon) * ((target / dist)^12 - (target / dist)^6)
 }
  
 # Neighbor data to LJ interaction vector
@@ -32,25 +33,51 @@ function lj_vector(rid, data) {
 function lj_sum(rid, data, accum) {
   return math.vec2.add(data, accum)
 }
+
+function user_attract(t) {
+	fus = math.vec2.new(0.0, 0.0)
+	if(size(t)>0) {
+		foreach(t, function(u, tab) {
+				#log("id: ",u," Range ", tab.r, "Bearing ", tab.b)
+				fus = math.vec2.add(fus, math.vec2.newp(lj_magnitude(tab.r, TARGET / 2.0, EPSILON * 2.0), tab.b))
+			})
+		math.vec2.scale(fus, 1.0 / size(t))
+	}
+	#print("User attract:", fus.x," ", fus.y, " [", size(t), "]")
+	return fus
+}
  
 # Calculates and actuates the flocking interaction
 function hexagon() {
   statef=hexagon
-  CURSTATE = "HEXAGON"
   # Calculate accumulator
   var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
-    math.vec2.scale(accum, 1.0 / neighbors.count())
+    accum = math.vec2.scale(accum, 1.0 / neighbors.count())
+
+  accum = math.vec2.add(accum, user_attract(users.dataL))
+  accum = math.vec2.scale(accum, 1.0 / 2.0)
+
+  if(math.vec2.length(accum) > 0.75) {
+  	  accum = math.vec2.scale(accum, 0.75 / math.vec2.length(accum))
+  }
+
   # Move according to vector
-  #print("Robot ", id, "must push ",accum.length, "; ", accum.angle)
+  print("Robot ", id, "must push ", math.vec2.length(accum) )#, "; ", math.vec2.angle(accum))
-  uav_moveto(accum.x,accum.y)
+  uav_moveto(accum.x, accum.y)
+  CURSTATE = "LENNARDJONES"
 
 #  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
 #  	timeW =0
 #  	statef=land
-#  	} else {
+#  	} else if(timeW>=WAIT_TIMEOUT/2) {
+#		  CURSTATE ="GOEAST"
 #  	timeW = timeW+1
-#  	uav_moveto(0.0,0.0)
+#  	uav_moveto(0.0,5.0)
+#  	} else {
+#		  CURSTATE ="GONORTH"
+#  	timeW = timeW+1
+#  	uav_moveto(5.0,0.0)
 #  	}
 }
  
@@ -89,9 +116,10 @@ WAIT_TIMEOUT = 200
 timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)
+  barrier.put(id, 1)
   CURSTATE = "BARRIERWAIT"
   if(barrier.size() >= threshold) {
-    barrier = nil
+    #barrier = nil
     transf()
   } else if(timeW>=WAIT_TIMEOUT) {
     barrier = nil
@@ -112,8 +140,8 @@ CURSTATE = "IDLE"
 function takeoff() {
 	CURSTATE = "TAKEOFF"
 	statef=takeoff
-	log("TakeOff: ", flight.status)
+	#log("TakeOff: ", flight.status)
-	log("Relative position: ", position.altitude)
+	#log("Relative position: ", position.altitude)
 	
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
@@ -129,15 +157,17 @@ function takeoff() {
 function land() {
 	CURSTATE = "LAND"
 	statef=land
-	log("Land: ", flight.status)
+	#log("Land: ", flight.status)
 	if(flight.status == 2 or flight.status == 3){
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 	}
 	else {
+		barrier_set(ROBOTS,idle)
+		barrier_ready()
 		timeW=0
-		barrier = nil
+		#barrier = nil
-		statef=idle
+		#statef=idle
 	}
 }
 
@@ -194,7 +224,8 @@ function step() {
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
 		statef = idle
-		uav_goto()
+		#uav_goto()
+		add_user_rb(10,rc_goto.latitude,rc_goto.longitude)
 	} else if(flight.rc_cmd==400) {
 		flight.rc_cmd=0
 		uav_arm()

src/buzz_utility.cpp

@@ -41,8 +41,8 @@ namespace buzz_utility{
 	void update_users(){
 		if(users_map.size()>0) {
 			/* Reset users information */
-//			buzzusers_reset();
+			buzzusers_reset();
-			create_stig_tables();
+//			create_stig_tables();
 			/* Get user id and update user information */
 			map< int, Pos_struct >::iterator it;
 			for (it=users_map.begin(); it!=users_map.end(); ++it){
@@ -68,6 +68,8 @@ namespace buzz_utility{
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
         buzzvm_tget(VM);*/
         buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
+		buzzvm_gload(VM);
+		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
 		buzzvm_push(VM, t);
 		buzzvm_gstore(VM);
         //buzzvm_pushi(VM, 2);

src/roscontroller.cpp

@@ -683,7 +683,7 @@ namespace rosbzz_node{
                 {
                         for(int it=0; it<n; ++it)
                         {
-				buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
+							buzz_utility::add_user(data.pos_neigh[it].position_covariance_type,data.pos_neigh[it].latitude, data.pos_neigh[it].longitude, data.pos_neigh[it].altitude);
                         }
 
                 }
@@ -715,8 +715,8 @@ namespace rosbzz_node{
 		moveMsg.header.stamp = ros::Time::now();
 		moveMsg.header.seq = setpoint_counter++;
 		moveMsg.header.frame_id = 1;
-		float ned_x, ned_y;
+//		float ned_x, ned_y;
-		gps_ned_home(ned_x, ned_y);
+//		gps_ned_home(ned_x, ned_y);
        //         ROS_INFO("[%i] ROSBuzz Home: %.7f, %.7f", robot_id, home[0], home[1]);
        //         ROS_INFO("[%i] ROSBuzz LocalPos: %.7f, %.7f", robot_id, local_pos[0], local_pos[1]);
                     
@@ -827,7 +827,7 @@ namespace rosbzz_node{
 			gps_rb(nei_pos, cvt_neighbours_pos_payload);
 			/*Extract robot id of the neighbour*/
 	 		uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t)*(message_obt+3));
-			//cout << "Rel Pos of " << (int)out[1] << ": " << cvt_neighbours_pos_payload[0] << ", "<< cvt_neighbours_pos_payload[1] << ", "<< cvt_neighbours_pos_payload[2] << endl;
+			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*/
 			buzz_utility::Pos_struct n_pos(cvt_neighbours_pos_payload[0],cvt_neighbours_pos_payload[1],cvt_neighbours_pos_payload[2]);
 			/*Put RID and pos*/