fixed graph formation

include/buzz_utility.h

@@ -20,8 +20,15 @@ struct pos_struct
   pos_struct(double x,double y,double z):x(x),y(y),z(z){};
   pos_struct(){}
 };
+typedef struct pos_struct Pos_struct;
+struct rb_struct
+{
+  double r,b,la,lo;
+  rb_struct(double la, double lo, double r,double b):la(la),lo(lo),r(r),b(b){};
+  rb_struct(){}
+};
+typedef struct rb_struct RB_struct;
 
-typedef struct pos_struct Pos_struct ;
 
 uint16_t* u64_cvt_u16(uint64_t u64);
 

include/buzzuav_closures.h

@@ -87,7 +87,8 @@ int buzzuav_update_battery(buzzvm_t vm);
  * Updates current position in Buzz
  */
 int buzzuav_update_currentpos(buzzvm_t vm);
-int buzzuav_adduserRB(buzzvm_t vm);
+int buzzuav_update_targets(buzzvm_t vm);
+int buzzuav_addtargetRB(buzzvm_t vm);
 /*
  * Updates flight status and rc command in Buzz, put it in a tabel to acess it 
  * use flight.status for flight status

include/roscontroller.h

@@ -171,6 +171,7 @@ private:
 			 double longitude,
 			 double altitude);
 	/*convert from spherical to cartesian coordinate system callback */
+	float constrainAngle(float x);
 	void gps_rb(GPS nei_pos, double out[]);
 	void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
 	void gps_ned_home(float &ned_x, float &ned_y);

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 testWP {
+	rosservice call $1/buzzcmd 0 16 0 0 0 0 0 45.45782 -- -73.63608 10
+}
 function record {
 	rosbag record $1/flight_status $1/global_position $1/users_pos $1/local_position $1/neighbours_pos /power_status /guidance/obstacle_distance /guidance/front/depth/image_rect/compressedDepth /guidance/right/depth/image_rect/compressedDepth /guidance/front/depth/points /guidance/right/depth/points /guidance/front/right/image_rect/compressed /guidance/front/left/image_rect/compressed /guidance/right/right/image_rect/compressed /guidance/right/left/image_rect/compressed /guidance/front/left/camera_info /guidance/front/right/camera_info /guidance/right/right/camera_info /guidance/right/left/camera_info
 }

script/Graph_drone.graph

@@ -2,3 +2,4 @@
 1	0	1000.0	0.0
 2	0	1000.0	1.57
 3	0	1000.0	3.14
+4	0	1000.0	4.71

script/flock.bzz

@@ -6,7 +6,7 @@ include "vstigenv.bzz"
  
 # Lennard-Jones parameters
 TARGET     = 12.0
-EPSILON    = 14.0
+EPSILON    = 18.0
  
 # Lennard-Jones interaction magnitude
 function lj_magnitude(dist, target, epsilon) {

script/graphform.bzz

@@ -7,12 +7,7 @@ include "update.bzz"
 include "barrier.bzz"	# don't use a stigmergy id=11 with this header.
 include "uavstates.bzz"	# require an 'action' function to be defined here.
 
-#
+ROBOT_RADIUS=50
-#Constant parameters, need to be adjust
-#parameters for set_wheels
-m_sWheelTurningParams={.MaxSpeed=1.0}
-
-ROBOT_RADIUS=0.5
 ROBOT_DIAMETER=2.0*ROBOT_RADIUS
 ROBOT_SAFETYDIST=2.0*ROBOT_DIAMETER
 
@@ -98,34 +93,17 @@ EPSILON    = 3.5 #3.5
 # Lennard-Jones interaction magnitude
 
 function FlockInteraction(dist,target,epsilon){
-var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+	var mag = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
-return mag
+	return mag
-}
-
-function Norm(vector) {
-  var l = math.vec2.length(vector)
-  return {
-    .x = vector.x / l,
-    .y = vector.y / l
-  }
 }
 
 function LimitAngle(angle){
-if(angle>2*math.pi)
+	if(angle>2*math.pi)
-return angle-2*math.pi
+		return angle-2*math.pi
-else if (angle<0)
+	else if (angle<0)
-return angle+2*math.pi
+		return angle+2*math.pi
-else
+	else
-return angle
+		return angle
-}
-
-#
-# Calculates the length of the given vector2.
-# PARAM v: The vector2.
-# RETURN: The length of the vector.
-#
-Length = function(v) {
-  return math.sqrt(v.x * v.x + v.y * v.y)
 }
 
 #
@@ -141,43 +119,43 @@ Angle = function(v) {
 #return the number of value in table
 #
 function count(table,value){
-var number=0
+	var number=0
-var i=0
+	var i=0
-while(i<size(table)){
+	while(i<size(table)){
-if(table[i]==value){
+		if(table[i]==value){
-number=number+1
+			number=number+1
-}
+		}
-i=i+1
+		i=i+1
-}
+	}
-return number
+	return number
 }
 
 #
 #return the index of value
 #
 function find(table,value){
-var index=nil
+	var index=nil
-var i=0
+	var i=0
-while(i<size(table)){
+	while(i<size(table)){
-if(table[i]==value)
+		if(table[i]==value)
-index=i
+			index=i
-i=i+1
+		i=i+1
-}
+	}
-return index
+	return index
 }
 
 function pow(base,exponent){
-var i=0
+	var i=0
-var renturn_val=1
+	var renturn_val=1
-if(exponent==0)
+	if(exponent==0)
-return 1
+		return 1
-else{
+	else{
-    while(i<exponent){
+		while(i<exponent){
-    renturn_val=renturn_val*base
+			renturn_val=renturn_val*base
-    i=i+1
+			i=i+1
-    }
+		}
-    return renturn_val
+		return renturn_val
-    }
+	}
 }
 
 #
@@ -272,86 +250,86 @@ neighbors.listen("msg",
 #
 #Function used to get the station info of the sender of the message
 function Get_DisAndAzi(id){
-neighbors.foreach(
+	neighbors.foreach(
-  function(rid, data) {
+	function(rid, data) {
-	if(rid==id){
+		if(rid==id){
-    m_receivedMessage.Range=data.distance*100.0
+		m_receivedMessage.Range=data.distance*100.0
-    m_receivedMessage.Bearing=data.azimuth
+		m_receivedMessage.Bearing=data.azimuth
-    }
+		}
-    })
+		})
 }
 
 #
 #Update node info according to neighbour robots
 #
 function UpdateNodeInfo(){
-#Collect informaiton
+	#Collect informaiton
-#Update information
+	#Update information
-var i=0
+	var i=0
 
-while(i<m_neighbourCunt){
+	while(i<m_neighbourCunt){
-if(m_MessageState[i]=="STATE_JOINED"){
+		if(m_MessageState[i]=="STATE_JOINED"){
-	m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
+			m_vecNodes[m_MessageLable[i]].State="ASSIGNED"
-	m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
+			m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
-}
+		}
-else if(m_MessageState[i]=="STATE_JOINING"){
+		else if(m_MessageState[i]=="STATE_JOINING"){
-	m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
+			m_vecNodes[m_MessageLable[i]].State="ASSIGNING"
-	m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
+			m_vecNodes[m_MessageLable[i]].StateAge=m_unJoiningLostPeriod
-}
+		}
-i=i+1
+		i=i+1
-}
+	}
-#Forget old information
+	#Forget old information
-i=0
+	i=0
-while(i<size(m_vecNodes)){
+	while(i<size(m_vecNodes)){
-	if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
+		if((m_vecNodes[i].StateAge>0) and (m_vecNodes[i].State=="ASSIGNING")){
-	m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
+		m_vecNodes[i].StateAge=m_vecNodes[i].StateAge-1
-	if(m_vecNodes[i].StateAge==0)
+		if(m_vecNodes[i].StateAge==0)
-		m_vecNodes[i].State="UNASSIGNED"
+			m_vecNodes[i].State="UNASSIGNED"
+		}
+	i=i+1
 	}
-i=i+1
-}
 }
 
 #
 #Transistion to state free
 #
 function TransitionToFree(){
-m_eState="STATE_FREE"
+	m_eState="STATE_FREE"
-m_unWaitCount=m_unLabelSearchWaitTime
+	m_unWaitCount=m_unLabelSearchWaitTime
-m_selfMessage.State=m_eState
+	m_selfMessage.State=m_eState
 }
 
 #
 #Transistion to state asking
 #
 function TransitionToAsking(un_label){
-m_eState="STATE_ASKING"
+	m_eState="STATE_ASKING"
-m_nLabel=un_label
+	m_nLabel=un_label
-m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
+	m_unRequestId=rng.uniform(0,65536)+id#don't know why the random numbers are the same, add id to make the ReqID different
-m_selfMessage.State=m_eState
+	m_selfMessage.State=m_eState
-m_selfMessage.ReqLable=m_nLabel
+	m_selfMessage.ReqLable=m_nLabel
-m_selfMessage.ReqID=m_unRequestId
+	m_selfMessage.ReqID=m_unRequestId
 
-m_unWaitCount=m_unResponseTimeThreshold
+	m_unWaitCount=m_unResponseTimeThreshold
 }
 
 #
 #Transistion to state joining
 #
 function TransitionToJoining(){
-m_eState="STATE_JOINING"
+	m_eState="STATE_JOINING"
-m_selfMessage.State=m_eState 
+	m_selfMessage.State=m_eState 
-m_selfMessage.Lable=m_nLabel
+	m_selfMessage.Lable=m_nLabel
-m_unWaitCount=m_unJoiningLostPeriod
+	m_unWaitCount=m_unJoiningLostPeriod
 
-neighbors.listen("reply",
+	neighbors.listen("reply",
-    function(vid,value,rid){
+		function(vid,value,rid){
-    #store the received message
+		#store the received message
-    if(value.Lable==m_nLabel){
+		if(value.Lable==m_nLabel){
-	m_cMeToPred.GlobalBearing=value.GlobalBearing
+		m_cMeToPred.GlobalBearing=value.GlobalBearing
 
-	}
+		}
-})
+	})
 
 }
 
@@ -359,32 +337,32 @@ neighbors.listen("reply",
 #Transistion to state joined
 #
 function TransitionToJoined(){
-m_eState="STATE_JOINED"
+	m_eState="STATE_JOINED"
-m_selfMessage.State=m_eState
+	m_selfMessage.State=m_eState
-m_selfMessage.Lable=m_nLabel
+	m_selfMessage.Lable=m_nLabel
-m_vecNodes[m_nLabel].State="ASSIGNED"
+	m_vecNodes[m_nLabel].State="ASSIGNED"
-neighbors.ignore("reply")
+	neighbors.ignore("reply")
 
-#write statues
+	#write statues
-v_tag.put(m_nLabel, 1)
+	v_tag.put(m_nLabel, 1)
 
-m_navigation.x=0.0
+	m_navigation.x=0.0
-m_navigation.y=0.0
+	m_navigation.y=0.0
-uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
+	uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
 }
 
 #
 #Transistion to state Lock, lock the current formation
 #
 function TransitionToLock(){
-m_eState="STATE_LOCK"
+	m_eState="STATE_LOCK"
-m_selfMessage.State=m_eState
+	m_selfMessage.State=m_eState
-m_selfMessage.Lable=m_nLabel
+	m_selfMessage.Lable=m_nLabel
-m_vecNodes[m_nLabel].State="ASSIGNED"
+	m_vecNodes[m_nLabel].State="ASSIGNED"
 
-m_navigation.x=0.0
+	m_navigation.x=0.0
-m_navigation.y=0.0
+	m_navigation.y=0.0
-uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
+	uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
 }
 
 #
@@ -554,19 +532,18 @@ function DoJoining(){
 		#the vector from self to target in global coordinate
 		var S2Target=math.vec2.add(S2Pred,P2Target)
 		#change the vector to local coordinate of self
-		var S2Target_dis=Length(S2Target)
 		var S2Target_bearing=Angle(S2Target)
 		m_bias=m_cMeToPred.Bearing-S2PGlobalBearing
-		S2Target_bearing=S2Target_bearing+m_bias
+		#S2Target_bearing=S2Target_bearing+m_bias # commented out by DS'06/17
-		m_navigation=math.vec2.newp(S2Target_dis,S2Target_bearing)
+		m_navigation=math.vec2.newp(math.vec2.length(S2Target),S2Target_bearing)
 		uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
 
 		
 
 		#test if is already in desired position
 		if(math.abs(S2Target.x)<m_fTargetDistanceTolerance and math.abs(S2Target.y)<m_fTargetDistanceTolerance){
-		log(S2Target_dis,S2Target_bearing)	
+			#log(S2Target_dis,S2Target_bearing)	
-			#TransitionToJoined()
+			TransitionToJoined()
 			return
 		}
 	} else{ #miss pred, there is a change the another robot block the sight, keep moving as before for sometime
@@ -662,7 +639,7 @@ function DoJoined(){
 
 
 	if(v_tag.size()==ROBOTS){
-		#TransitionToLock()
+		TransitionToLock()
 	}
 }
 
@@ -733,6 +710,7 @@ uav_moveto(m_navigation.x/100.0,m_navigation.y/100.0)
 
 function action(){
   statef=action
+  UAVSTATE="GRAPH"
 }
 
 #
@@ -744,7 +722,7 @@ function init() {
 	#
 	m_unResponseTimeThreshold=10
 	m_unLabelSearchWaitTime=10
-	m_fTargetDistanceTolerance=150
+	m_fTargetDistanceTolerance=10
 	m_unJoiningLostPeriod=100
 
 	#
@@ -767,7 +745,7 @@ function step(){
 	#
 	#act according to current state
 	#
-	if(UAVSTATE=="IDLE"){
+	if(UAVSTATE=="GRAPH"){
 		if(m_eState=="STATE_FREE")
 			DoFree()
 		else if(m_eState=="STATE_ESCAPE")

script/include/uavstates.bzz

@@ -50,7 +50,7 @@ function land() {
 		uav_land()
 	}
 	else {
-		barrier_set(ROBOTS,idle,land)
+		barrier_set(ROBOTS,turnedoff,land)
 		barrier_ready()
 		timeW=0
 		#barrier = nil
@@ -58,6 +58,22 @@ function land() {
 	}
 }
 
+function follow() {
+	if(size(targets)>0) {
+		UAVSTATE = "FOLLOW"
+		statef=follow
+		attractor=math.vec2.newp(0,0)
+		foreach(targets, function(id, tab) {
+			force=(0.05)*(tab.range)^4
+			attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
+		})
+		uav_moveto(attractor.x, attractor.y)
+	} else {
+		log("No target in local table!")
+		#statef=idle
+	}
+}
+
 function uav_rccmd() {
     if(flight.rc_cmd==22) {
 		log("cmd 22")
@@ -74,9 +90,11 @@ function uav_rccmd() {
 		neighbors.broadcast("cmd", 21)
 	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
-		statef = idle
+		UAVSTATE = "FOLLOW"
+		log(rc_goto.latitude, " ", rc_goto.longitude)
+		add_targetrb(0,rc_goto.latitude,rc_goto.longitude)
+		statef = follow
 		#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

@@ -40,10 +40,10 @@ namespace buzz_utility{
 
 	void update_users(){
 		if(users_map.size()>0) {
-			/* Reset users information */
+			// Reset users information
-			buzzusers_reset();
+//			buzzusers_reset();
 //			create_stig_tables();
-			/* Get user id and update user information */
+			// Get user id and update user information
 			map< int, Pos_struct >::iterator it;
 			for (it=users_map.begin(); it!=users_map.end(); ++it){
 				//ROS_INFO("Buzz_utility will save user %i.", it->first);
@@ -52,21 +52,20 @@ namespace buzz_utility{
 									(it->second).y,
 									(it->second).z);
 			}
-		}/*else
+		}
-			ROS_INFO("[%i] No new users",Robot_id);*/
 	}
 
-	int buzzusers_reset() {
+	/*int buzzusers_reset() {
 		if(VM->state != BUZZVM_STATE_READY) return VM->state;
-		/* Make new table */
+		//Make new table
 		buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
 		//make_table(&t);
 		if(VM->state != BUZZVM_STATE_READY) return VM->state;
-		/* Register table as global symbol */
+		//Register table as global symbol
-		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
+		//buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
 		buzzvm_gload(VM);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
-        buzzvm_tget(VM);*/
+        buzzvm_tget(VM);
         buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
 		buzzvm_gload(VM);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
@@ -75,55 +74,47 @@ namespace buzz_utility{
         //buzzvm_pushi(VM, 2);
 		//buzzvm_callc(VM);
 		return VM->state;
-	}
+	}*/
 
 	int buzzusers_add(int id, double latitude, double longitude, double altitude) {
 		if(VM->state != BUZZVM_STATE_READY) return VM->state;
-		/* Get users "p" table */
+		// Get users "p" table 
 		/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
 		buzzvm_gload(VM);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
         buzzvm_tget(VM);*/
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
-		buzzvm_gload(VM);
-        //buzzvm_pushi(VM, 1);
-		//buzzvm_callc(VM);
-		buzzvm_type_assert(VM, 1, BUZZTYPE_TABLE);
-		buzzobj_t nbr = buzzvm_stack_at(VM, 1);
-		/* Get "data" field */
-		buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
 		buzzvm_tget(VM);
 		if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
 			//ROS_INFO("Empty data, create a new table");
 			buzzvm_pop(VM);
-			buzzvm_push(VM, nbr);
+			buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
-			buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
 			buzzvm_pusht(VM);
 			buzzobj_t data = buzzvm_stack_at(VM, 1);
 			buzzvm_tput(VM);
 			buzzvm_push(VM, data);
 		}
-		/* When we get here, the "data" table is on top of the stack */
+		// When we get here, the "data" table is on top of the stack 
-		/* Push user id */
+		// Push user id 
 		buzzvm_pushi(VM, id);
-		/* Create entry table */
+		// Create entry table
 		buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
-		/* Insert latitude */
+		// Insert latitude
 		buzzvm_push(VM, entry);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
 		buzzvm_pushf(VM, latitude);
 		buzzvm_tput(VM);
-		/* Insert longitude */
+		// Insert longitude
 		buzzvm_push(VM, entry);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
 		buzzvm_pushf(VM, longitude);
 		buzzvm_tput(VM);
-		/* Insert altitude */
+		// Insert altitude
 		buzzvm_push(VM, entry);
 		buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
 		buzzvm_pushf(VM, altitude);
 		buzzvm_tput(VM);
-		/* Save entry into data table */
+		// Save entry into data table
 		buzzvm_push(VM, entry);
 		buzzvm_tput(VM);
 		//ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
@@ -329,8 +320,8 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
    		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_user_rb", 1));
+   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_targetrb", 1));
-   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_adduserRB));
+   		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
    		buzzvm_gstore(VM);
 
    	return VM->state;
@@ -371,7 +362,7 @@ namespace buzz_utility{
    		buzzvm_pushs(VM,  buzzvm_string_register(VM, "uav_land", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
-   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_user_rb", 1));
+   		buzzvm_pushs(VM,  buzzvm_string_register(VM, "add_targetrb", 1));
    		buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
    		buzzvm_gstore(VM);
 
@@ -692,6 +683,7 @@ int create_stig_tables() {
 	   	buzzuav_closures::buzzuav_update_prox(VM);
 	   	buzzuav_closures::buzzuav_update_currentpos(VM);
 	   	buzzuav_closures::update_neighbors(VM);
+		buzzuav_closures::buzzuav_update_targets(VM);
 	   	//update_users();
 	   	buzzuav_closures::buzzuav_update_flight_status(VM);
 	}

src/buzzuav_closures.cpp

@@ -26,7 +26,7 @@ namespace buzzuav_closures{
 	static int buzz_cmd=0;
 	static float height=0;
 	
-
+	std::map< int,  buzz_utility::RB_struct> targets_map;
 	std::map< int,  buzz_utility::Pos_struct> neighbors_map;
 
 	/****************************************/
@@ -89,13 +89,20 @@ namespace buzzuav_closures{
 		out[2] = height; //constant height.
 	}
 
+	float constrainAngle(float x){
+			x = fmod(x,2*M_PI);
+			if (x < 0.0)
+				x += 2*M_PI;
+			return x;
+		}
+
 	void rb_from_gps(double nei[], double out[], double cur[]){   
        		double d_lon = nei[1] - cur[1];
        		double d_lat = nei[0] - cur[0];
         	double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
         	double ned_y = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
 		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
-		out[1] = atan2(ned_y,ned_x);
+		out[1] = constrainAngle(atan2(ned_y,ned_x));
 		out[2] = 0.0;
 	}
 
@@ -126,57 +133,57 @@ namespace buzzuav_closures{
 	   gps_from_rb(d, b, goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
        //printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
-	   //printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
+	   //ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], goto_pos[1], goto_pos[2]);
 	   buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
 	   return buzzvm_ret0(vm);
 	}
 
-	int users_add2localtable(buzzvm_t vm, int id, float range, float bearing) {
+	int buzzuav_update_targets(buzzvm_t vm) {
 		if(vm->state != BUZZVM_STATE_READY) return vm->state;
-		buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
-		buzzvm_gload(vm);
+		//buzzobj_t t = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
-		buzzvm_type_assert(vm, 1, BUZZTYPE_TABLE);
+		//buzzvm_push(vm, t);
-		buzzobj_t nbr = buzzvm_stack_at(vm, 1);
+		buzzvm_pusht(vm);
-		/* Get "data" field */
+		buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
-		buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
+		//buzzvm_tput(vm);
-		buzzvm_tget(vm);
+		//buzzvm_dup(vm);
-		if(buzzvm_stack_at(vm, 1)->o.type == BUZZTYPE_NIL) {
+		double rb[3], tmp[3];
-			//ROS_INFO("Empty data, create a new table");
+		map< int, buzz_utility::RB_struct >::iterator it;
-			buzzvm_pop(vm);
+		for (it=targets_map.begin(); it!=targets_map.end(); ++it){	   
-			buzzvm_push(vm, nbr);
+			tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
-			buzzvm_pushs(vm, buzzvm_string_register(vm, "dataL", 1));
+	   		rb_from_gps(tmp, rb, cur_pos);
-			buzzvm_pusht(vm);
+			ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
-			buzzobj_t data = buzzvm_stack_at(vm, 1);
+			buzzvm_push(vm, targettbl);
+			/* When we get here, the "targets" table is on top of the stack */
+			//ROS_INFO("Buzz_utility will save user %i.", it->first);
+			/* Push user id */
+			buzzvm_pushi(vm, it->first);
+			/* Create entry table */
+			buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
+			/* Insert range */
+			buzzvm_push(vm, entry);
+			buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
+			buzzvm_pushf(vm, rb[0]);
+			buzzvm_tput(vm);
+			/* Insert longitude */
+			buzzvm_push(vm, entry);
+			buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
+			buzzvm_pushf(vm, rb[1]);
+			buzzvm_tput(vm);
+			/* Save entry into data table */
+			buzzvm_push(vm, entry);
 			buzzvm_tput(vm);
-			buzzvm_push(vm, data);
 		}
-		/* When we get here, the "data" table is on top of the stack */
+		buzzvm_gstore(vm);
-		/* Push user id */
+
-		buzzvm_pushi(vm, id);
-		/* Create entry table */
-		buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
-		/* Insert range */
-		buzzvm_push(vm, entry);
-		buzzvm_pushs(vm, buzzvm_string_register(vm, "r", 1));
-		buzzvm_pushf(vm, range);
-		buzzvm_tput(vm);
-		/* Insert longitude */
-		buzzvm_push(vm, entry);
-		buzzvm_pushs(vm, buzzvm_string_register(vm, "b", 1));
-		buzzvm_pushf(vm, bearing);
-		buzzvm_tput(vm);
-		/* Save entry into data table */
-		buzzvm_push(vm, entry);
-		buzzvm_tput(vm);
-		//printf("\tBuzz_closure saved new user: %i (%f,%f)\n", id, range, bearing);
 		return vm->state;
 	}
 
-	int buzzuav_adduserRB(buzzvm_t vm) {
+	int buzzuav_addtargetRB(buzzvm_t vm) {
 	   buzzvm_lnum_assert(vm, 3);
-	   buzzvm_lload(vm, 1); /* longitude */
+	   buzzvm_lload(vm, 1); // longitude
-	   buzzvm_lload(vm, 2); /* latitude */
+	   buzzvm_lload(vm, 2); // latitude
-	   buzzvm_lload(vm, 3); /* id */
+	   buzzvm_lload(vm, 3); // id
 	   buzzvm_type_assert(vm, 3, BUZZTYPE_INT);
 	   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
 	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@@ -189,10 +196,20 @@ namespace buzzuav_closures{
 
 	   rb_from_gps(tmp, rb, cur_pos);
 	   if(fabs(rb[0])<100.0) {
-	   	//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
+			//printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
-		return users_add2localtable(vm, uid, rb[0], rb[1]);
+			buzz_utility::RB_struct RB_arr;
+			RB_arr.la=tmp[0];
+			RB_arr.lo=tmp[1];
+			RB_arr.r=rb[0];
+			RB_arr.b=rb[1];
+			map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(uid);
+			if(it!=targets_map.end())
+				targets_map.erase(it);
+			targets_map.insert(make_pair(uid, RB_arr));
+			//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
+			return vm->state;
 	   } else
-		printf(" ---------- User too far %f\n",rb[0]);
+		printf(" ---------- Target too far %f\n",rb[0]);
 
 		return 0;
 	}
@@ -427,7 +444,7 @@ namespace buzzuav_closures{
 
 	int buzzuav_update_prox(buzzvm_t vm) {
 
-	buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
+		buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
         buzzvm_pusht(vm);
         buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
         buzzvm_gstore(vm);

src/roscontroller.cpp

@@ -92,22 +92,28 @@ namespace rosbzz_node{
                         ///////////////////////////////////////////////////////
                         // MAIN LOOP
                         //////////////////////////////////////////////////////
-			ROS_INFO("[%i] STARTING MAIN LOOP!", robot_id);
+			//ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
 			while (ros::ok() && !buzz_utility::buzz_script_done())
   			{
       			/*Update neighbors position inside Buzz*/
      			//buzz_closure::neighbour_pos_callback(neighbours_pos_map);
 				/*Neighbours of the robot published with id in respective topic*/
+				//ROS_WARN("[%i]-----------NEIG", robot_id);
 				neighbours_pos_publisher();
 				/*Check updater state and step code*/
+				//ROS_WARN("[%i]-----------UPD", robot_id);
   				update_routine(bcfname.c_str(), dbgfname.c_str());
 				/*Step buzz script */
+				//ROS_WARN("[%i]-----------STEP", robot_id);
       			buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
+				//ROS_WARN("[%i]-----------MSG", robot_id);
 		  		prepare_msg_and_publish();
 				/*call flight controler service to set command long*/
+				//ROS_WARN("[%i]-----------FC", robot_id);
 				flight_controller_service_call();
 				/*Set multi message available after update*/
+				//ROS_WARN("[%i]-----------UPD", robot_id);
 				if(get_update_status()){
 					set_read_update_status();
 					multi_msg=true;
@@ -122,6 +128,7 @@ namespace rosbzz_node{
 				}
 				/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
 				//no_of_robots=get_number_of_robots();
+				//ROS_WARN("[%i]-----------ROBOTS", robot_id);
 				get_number_of_robots();
 				buzz_utility::set_robot_var(no_of_robots);
 				//if(neighbours_pos_map.size() >0) no_of_robots =neighbours_pos_map.size()+1;
@@ -603,13 +610,20 @@ namespace rosbzz_node{
                 }
             }
 
+		float roscontroller::constrainAngle(float x){
+			x = fmod(x,2*M_PI);
+			if (x < 0.0)
+				x += 2*M_PI;
+			return x;
+		}
+
 	void roscontroller::gps_rb(GPS nei_pos, double out[])
     {
         float ned_x=0.0, ned_y=0.0;
 		gps_ned_cur(ned_x, ned_y, nei_pos);
 		out[0] = sqrt(ned_x*ned_x+ned_y*ned_y);
         //out[0] = std::floor(out[0] * 1000000) / 1000000;
-		out[1] = atan2(ned_y,ned_x);
+		out[1] = constrainAngle(atan2(ned_y,ned_x));
         //out[1] = std::floor(out[1] * 1000000) / 1000000;
 		out[2] = 0.0;
 	}