implement vstig users

include/buzz_utility.h

@@ -30,6 +30,8 @@ int buzz_listen(const char* type,
 
 void neighbour_pos_callback(std::map< int,  Pos_struct> neighbours_pos_map);
 
+int buzz_update_users_stigmergy(buzzobj_t tbl);
+
 void in_msg_process(uint64_t* payload);
 
 uint64_t* out_msg_process();

include/buzzuav_closures.h

@@ -46,6 +46,7 @@ void rc_call(int rc_cmd);
 void set_battery(float voltage,float current,float remaining);
 /* sets current position */
 void set_currentpos(double latitude, double longitude, double altitude);
+void set_userspos(double latitude, double longitude, double altitude);
 /*retuns the current go to position */
 double* getgoto();
 /* updates flight status*/
@@ -82,7 +83,7 @@ int buzzuav_update_battery(buzzvm_t vm);
  * Updates current position in Buzz
  */
 int buzzuav_update_currentpos(buzzvm_t vm);
-
+buzzobj_t buzzuav_update_userspos(buzzvm_t vm);
 
 /*
  * Updates flight status and rc command in Buzz, put it in a tabel to acess it 

include/roscontroller.h

@@ -39,19 +39,6 @@
 #define XBEE_STOP_TRANSMISSION 4355356352
 #define TIMEOUT	60
 
-/** Semi-major axis of the Earth, \f$ a \f$, in meters.
- * This is a defining parameter of the WGS84 ellipsoid. */
-#define WGS84_A 6378137.0
-/** Inverse flattening of the Earth, \f$ 1/f \f$.
- * This is a defining parameter of the WGS84 ellipsoid. */
-#define WGS84_IF 298.257223563
-/** The flattening of the Earth, \f$ f \f$. */
-#define WGS84_F (1/WGS84_IF)
-/** Semi-minor axis of the Earth in meters, \f$ b = a(1-f) \f$. */
-#define WGS84_B (WGS84_A*(1-WGS84_F))
-/** Eccentricity of the Earth, \f$ e \f$ where \f$ e^2 = 2f - f^2 \f$ */
-#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F))
-
 using namespace std;
 
 namespace rosbzz_node{
@@ -117,6 +104,7 @@ private:
 	ros::Publisher localsetpoint_nonraw_pub;
 	ros::ServiceServer service;
 	ros::Subscriber current_position_sub;
+	ros::Subscriber users_sub;
 	ros::Subscriber battery_sub;
 	ros::Subscriber payload_sub;
 	ros::Subscriber flight_status_sub;
@@ -189,6 +177,7 @@ private:
 	
 	/*current position callback*/
 	void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
+        void users_pos(const rosbuzz::neigh_pos msg);
 
 	/*current relative altitude callback*/
 	void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);

src/buzz_utility.cpp

@@ -20,6 +20,8 @@ namespace buzz_utility{
 	static uint8_t      MSG_SIZE        = 50;   // Only 100 bytes of Buzz messages every step
 	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
 	static int 	    Robot_id        = 0;
+        buzzobj_t           usersvstig;
+        buzzobj_t           key;
 	
 	/****************************************/
 	/*adds neighbours position*/
@@ -302,6 +304,30 @@ namespace buzz_utility{
       		fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
       		return 0;
    	}
+
+   	
+buzzvm_dup(VM);
+   	// usersvstig = stigmergy.create(123)
+        buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
+        // value of the stigmergy id
+        buzzvm_pushi(VM, 5);
+        // get the stigmergy table from the global scope
+//        buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
+//        buzzvm_gload(VM);
+        // get the create method from the stigmergy table
+//        buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
+//        buzzvm_tget(VM);
+        // call the stigmergy.create() method
+//        buzzvm_closure_call(VM, 1);
+        // now the stigmergy is at the top of the stack - save it for future reference
+//        usersvstig = buzzvm_stack_at(VM, 0);
+//buzzvm_pop(VM);
+        // assign the virtual stigmergy to the global symbol v
+        // create also a global variable for it, so the garbage collector does not remove it
+//buzzvm_pushs(VM, buzzvm_string_register(VM, "v", 1));
+//buzzvm_push(VM, usersvstig);
+        buzzvm_gstore(VM);
+        
    	/* Save bytecode file name */
    	BO_FNAME = strdup(bo_filename);
    	/* Execute the global part of the script */
@@ -309,9 +335,25 @@ namespace buzz_utility{
    	/* Call the Init() function */
    	buzzvm_function_call(VM, "init", 0);
    	/* All OK */
+
    	return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
 	}
-
+        
+        int buzz_update_users_stigmergy(buzzobj_t tbl){
+            // push the key (here it's an int with value 46)
+            buzzvm_pushi(VM, 46);
+            // push the table
+            buzzvm_push(VM, tbl);
+            // the stigmergy is stored in the vstig variable
+            // let's push it on the stack
+            buzzvm_push(VM, usersvstig);
+            // get the put method from myvstig
+            buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
+            buzzvm_tget(VM);
+            // call the vstig.put() method
+            buzzvm_closure_call(VM, 2);
+            return 1;
+        }
 	/****************************************/
 	/*Sets a new update                     */
 	/****************************************/
@@ -447,7 +489,12 @@ namespace buzz_utility{
 	   buzzuav_closures::buzzuav_update_battery(VM);
 	   buzzuav_closures::buzzuav_update_prox(VM);
 	   buzzuav_closures::buzzuav_update_currentpos(VM);
+	   buzzobj_t tbl = buzzuav_closures::buzzuav_update_userspos(VM);
 	   buzzuav_closures::buzzuav_update_flight_status(VM);
+           
+           //buzz_update_users_stigmergy(tbl);
+           
+           
 	    /*
 	    * Call Buzz step() function
 	    */

src/buzzuav_closures.cpp

@@ -19,6 +19,7 @@ namespace buzzuav_closures{
 	static float batt[3];
 	static float obst[5]={0,0,0,0,0};
 	static double cur_pos[3];
+	static double users_pos[3];
 	static uint8_t status;
 	static int cur_cmd = 0;
 	static int rc_cmd=0;
@@ -244,6 +245,11 @@ namespace buzzuav_closures{
 	   cur_pos[1]=longitude;
 	   cur_pos[2]=altitude;
 	}
+	void set_userspos(double latitude, double longitude, double altitude){
+	   users_pos[0]=latitude;
+	   users_pos[1]=longitude;
+	   users_pos[2]=altitude;
+	}
 	/****************************************/
 	int buzzuav_update_currentpos(buzzvm_t vm) {
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
@@ -263,6 +269,26 @@ namespace buzzuav_closures{
 	   buzzvm_gstore(vm);
 	   return vm->state;
 	}
+	buzzobj_t buzzuav_update_userspos(buzzvm_t vm) {
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "users", 1));
+	   buzzvm_pusht(vm);
+	   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
+	   buzzvm_pushf(vm, users_pos[0]);
+	   buzzvm_tput(vm);
+	   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
+	   buzzvm_pushf(vm, users_pos[1]);
+	   buzzvm_tput(vm);
+	   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "height", 1));
+	   buzzvm_pushf(vm, users_pos[2]);
+	   buzzvm_tput(vm);
+	   buzzvm_gstore(vm);
+           
+           return buzzvm_stack_at(vm, 0);
+	   //return vm->state;
+	}
 
 	void flight_status_update(uint8_t state){
 	   status=state;

src/roscontroller.cpp

@@ -223,7 +223,9 @@ namespace rosbzz_node{
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>("/"+robot_name+fcclient_name);
 		xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>("/"+robot_name+xbeesrv_name);
 		stream_client = n_c.serviceClient<mavros_msgs::StreamRate>("/"+robot_name+stream_client_name);
-
+                
+  		users_sub = n_c.subscribe("/"+robot_name+"/users_pos", 1000, &roscontroller::users_pos,this);
+                
 		multi_msg=true;
 	}
 	/*---------------------------------------
@@ -424,6 +426,8 @@ namespace rosbzz_node{
 					Arm();
 					ros::Duration(0.5).sleep();
 				}
+				// Registering HOME POINT.
+				home[0] = cur_pos[0];home[1] = cur_pos[1];home[2] = cur_pos[2];
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
 				if (mav_client.call(cmd_srv))
@@ -578,20 +582,32 @@ namespace rosbzz_node{
                 double M[3][3];
                 ecef2ned_matrix(ref_ecef, M);
                 double ned[3];
-                matrix_multiply(3, 3, 1, (double *)M, ecef, ned);*/
+                matrix_multiply(3, 3, 1, (double *)M, ecef, ned);   
+                
+                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[0] = std::floor(out[0] * 1000000) / 1000000;
+		out[1] = atan2(ned_y,ned_x);
+                out[1] = std::floor(out[1] * 1000000) / 1000000;
+		out[2] = 0.0;*/
+                
 		out[0] = sqrt(ned[0]*ned[0]+ned[1]*ned[1]);
                 out[0] = std::floor(out[0] * 1000000) / 1000000;
 		out[1] = atan2(ned[1],ned[0]);
                 out[1] = std::floor(out[1] * 1000000) / 1000000;
 		out[2] = 0.0;
+
 	}
 
 	void roscontroller::cvt_ned_coordinates(double nei[], double out[], double cur[]){
-              // calculate earth radii
+                // calculate earth radii
-  double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
+                double temp = 1.0 / (1.0 - excentrity2 * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)) * sin(DEG2RAD(DEFAULT_REFERENCE_LATITUDE)));
-  double prime_vertical_radius = equatorial_radius * sqrt(temp);
+                double prime_vertical_radius = equatorial_radius * sqrt(temp);
-  double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
+                double radius_north = prime_vertical_radius * (1 - excentrity2) * temp;
-  double radius_east  = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
+                double radius_east  = prime_vertical_radius * cos(DEG2RAD(DEFAULT_REFERENCE_LATITUDE));
   
  		double d_lon = nei[1] - cur[1];
         	double d_lat = nei[0] - cur[0];
@@ -617,7 +633,15 @@ namespace rosbzz_node{
                 ref_ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
                 double M[3][3];
                 ecef2ned_matrix(ref_ecef, M);
-                matrix_multiply(3, 3, 1, (double *)M, ecef, out);*/
+                matrix_multiply(3, 3, 1, (double *)M, ecef, out);
+                
+                double d_lon = nei[1] - cur[1];
+        	double d_lat = nei[0] - cur[0];
+        	out[0] = DEG2RAD(d_lat) * EARTH_RADIUS;
+                out[0] = std::floor(out[0] * 1000000) / 1000000;
+        	out[1] = DEG2RAD(d_lon) * EARTH_RADIUS * cos(DEG2RAD(nei[0]));
+                out[1] = std::floor(out[1] * 1000000) / 1000000;
+		out[2] = 0.0;*/
 	}
 
 	/*------------------------------------------------------
@@ -657,14 +681,35 @@ namespace rosbzz_node{
 		fcu_timeout = TIMEOUT;
                 double lat = std::floor(msg->latitude * 1000000) / 1000000;
                 double lon = std::floor(msg->longitude * 1000000) / 1000000;
-		if(cur_rel_altitude<0.2){
+		/*if(cur_rel_altitude<1.2){
 			home[0]=lat;
 			home[1]=lon;
 			home[2]=cur_rel_altitude;
-		}
+		}*/
 		set_cur_pos(lat,lon, cur_rel_altitude);//msg->altitude);
 		buzzuav_closures::set_currentpos(lat,lon, cur_rel_altitude);//msg->altitude);
 	}
+	void roscontroller::users_pos(const rosbuzz::neigh_pos data){
+		//ROS_INFO("Altitude out: %f", cur_rel_altitude);
+
+                double us[3];
+                int n = data.pos_neigh.size();
+        //	ROS_INFO("Neighbors array size: %i\n", n);
+                if(n>0)
+                {
+                        for(int it=0; it<n; ++it)
+                        {
+                                us[0] = data.pos_neigh[it].latitude;
+                                us[1] = data.pos_neigh[it].longitude;
+                                us[2] = data.pos_neigh[it].altitude;
+                                double out[3];
+                                cvt_rangebearing_coordinates(us, out, cur_pos);
+                                buzzuav_closures::set_userspos(out[0], out[1], out[2]);
+                        }
+
+                }
+		
+	}
 	/*-------------------------------------------------------------
 	/ Update altitude into BVM from subscriber
 	/-------------------------------------------------------------*/
@@ -694,7 +739,8 @@ namespace rosbzz_node{
 		double local_pos[3];
 		cvt_ned_coordinates(cur_pos,local_pos,home);
                 ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]);
-			/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
+                    
+                /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
 		moveMsg.pose.position.x = local_pos[1]+y;
 		moveMsg.pose.position.y = local_pos[0]+x;
 		moveMsg.pose.position.z = z;
@@ -704,11 +750,11 @@ namespace rosbzz_node{
 		moveMsg.pose.orientation.z = 0;
 		moveMsg.pose.orientation.w = 1;
 
-
+                // To prevent drifting from stable position.
-		//if(fabs(x)>0.01 || fabs(y)>0.01) {
+		if(fabs(x)>0.01 || fabs(y)>0.01) {
                     localsetpoint_nonraw_pub.publish(moveMsg);
                     ROS_INFO("Sent local NON RAW position message!");
-                //}
+                }
 	}
 
 	void roscontroller::SetMode(std::string mode, int delay_miliseconds){

src/testflocksim.bzz

@@ -11,11 +11,11 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 5.0
+TARGET_ALTITUDE = 10.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
-TARGET     = 10.0
+TARGET     = 12.0
 EPSILON    = 8.0
  
 # Lennard-Jones interaction magnitude
@@ -196,6 +196,12 @@ neighbors.listen("cmd",
 	statef()
 	log("Current state: ", CURSTATE)
 	log("Swarm size: ",ROBOTS)
+	log("User position: ", users.range)
+	
+	# Read a value from the structure
+        #x = usersvstig.get(46)
+        # Get the number of keys in the structure
+        #log("The vstig has ", usersvstig.size(), " elements")
 }
  
 # Executed once when the robot (or the simulator) is reset.