Merge branch 'dev'

Conflicts:
	include/roscontroller.h
	src/buzz_utility.cpp
	src/roscontroller.cpp

include/buzzuav_closures.h

@@ -6,7 +6,8 @@
 #include "uav_utility.h"
 #include "mavros_msgs/CommandCode.h"
 #include "ros/ros.h"
-
+#include "buzz_utility.h"
+//#include "roscontroller.h"
 
 namespace buzzuav_closures{
 /*
@@ -46,8 +47,13 @@ void set_obstacle_dist(float dist[]);
  * Commands the UAV to takeoff
  */
 int buzzuav_takeoff(buzzvm_t vm);
+/*
+ * Arm command from Buzz 
+ */
 int buzzuav_arm(buzzvm_t vm);
-
+/*
+ * Disarm from buzz
+ */
 int buzzuav_disarm(buzzvm_t vm) ;
 /* Commands the UAV to land
  */
@@ -66,8 +72,6 @@ int buzzuav_update_battery(buzzvm_t vm);
  */
 int buzzuav_update_currentpos(buzzvm_t vm);
 
-/*update obstacles in Buzz*/
-int buzzuav_update_obstacle(buzzvm_t vm);
 
 /*
  * Updates flight status and rc command in Buzz, put it in a tabel to acess it 

include/roscontroller.h

@@ -11,6 +11,7 @@
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
+#include "mavros_msgs/PositionTarget.h"
 #include "sensor_msgs/NavSatStatus.h"
 #include "mavros_msgs/WaypointPush.h"
 #include "mavros_msgs/Waypoint.h"
@@ -65,6 +66,7 @@ private:
 	ros::ServiceClient mav_client;
 	ros::Publisher payload_pub;
 	ros::Publisher neigh_pos_pub;
+	ros::Publisher localsetpoint_pub;
 	ros::ServiceServer service;
 	ros::Subscriber current_position_sub;
 	ros::Subscriber battery_sub;
@@ -86,8 +88,9 @@ private:
 
   	mavros_msgs::SetMode m_cmdSetMode;
   	ros::ServiceClient mode_client;
-
+	
-  	ros::ServiceClient mission_client;
+	/*Initialize publisher and subscriber, done in the constructor*/
+	void Initialize_pub_sub(ros::NodeHandle n_c);
 
   	std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
 
@@ -99,6 +102,12 @@ private:
 	/*compiles buzz script from the specified .bzz file*/
 	void Compile_bzz();
 
+	/*Flight controller service call*/
+	void flight_controler_service_call();
+	
+	/*Neighbours pos publisher*/
+	void neighbours_pos_publisher();
+
 	/*Prepare messages and publish*/
 	void prepare_msg_and_publish();
 
@@ -140,14 +149,19 @@ private:
 	/*robot id sub callback*/
 	void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
 
+	/*Obstacle distance table callback*/
 	void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
 
+	/*Get publisher and subscriber from YML file*/
 	void GetSubscriptionParameters(ros::NodeHandle node_handle);
 
+	/*Arm/disarm method that can be called from buzz*/
 	void Arm();
 
-	void SetMode(std::string mode, int delay_miliseconds);
+	/*set mode like guided for solo*/
+	void SetMode();
 
+	/*Robot independent subscribers*/
 	void Subscribe(ros::NodeHandle n_c);
 
 	void WaypointMissionSetup(float lat, float lng, float alt);

launch/rosbuzz_2_parallel.launch

@@ -2,7 +2,7 @@
 
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
-		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev.bzz" />
+		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/testflockfev_2parallel.bzz" />
 		<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="rc_cmd" />

src/buzz_utility.cpp

@@ -76,15 +76,9 @@ namespace buzz_utility{
    		memcpy(pl, payload ,size);
 		/*size and robot id read*/
    		size_t tot = sizeof(uint32_t);
-      		/*for(int i=0;i<data[0];i++){
-			uint16_t* out = u64_cvt_u16(payload[i]);
-			for(int k=0;k<4;k++){
-				cout<<" [Debug inside buzz util: obt msg:] "<<out[k]<<endl;
-			}
-		}*/
 		/* Go through the messages until there's nothing else to read */
-		//fprintf(stdout,"Total data size  : utils : %d\n",(int)size);
       		uint16_t unMsgSize=0;
+<<<<<<< HEAD
 		//uint8_t is_msg_pres=*(uint8_t*)(pl + tot);
 		//tot+=sizeof(uint8_t);
 		 //fprintf(stdout,"is_updater msg present : %i\n",(int)is_msg_pres);
@@ -102,6 +96,9 @@ namespace buzz_utility{
 		//}
 	      	//unMsgSize=0;
 
+=======
+		
+>>>>>>> dev
 			/*Obtain Buzz messages only when they are present*/
 	      			do {
 		 			/* Get payload size */
@@ -131,6 +128,7 @@ namespace buzz_utility{
    		/* Send robot id */
    		*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
    		tot += sizeof(uint16_t);
+<<<<<<< HEAD
 		//uint8_t updater_msg_pre = 0;
    		//uint16_t updater_msgSize= 0;
 		//if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1){
@@ -155,6 +153,8 @@ namespace buzz_utility{
 		//   *(uint8_t*)(buff_send + tot) = (uint8_t)updater_msg_pre;
       		//   tot += sizeof(uint8_t);
 		//}
+=======
+>>>>>>> dev
  			/* Send messages from FIFO */
 	   		do {
 				/* Are there more messages? */
@@ -507,7 +507,6 @@ namespace buzz_utility{
 	   buzzuav_closures::buzzuav_update_prox(VM);
 	   buzzuav_closures::buzzuav_update_currentpos(VM);
 	   buzzuav_closures::buzzuav_update_flight_status(VM);
-	   buzzuav_closures::buzzuav_update_obstacle(VM);
 	    /*
 	    * Call Buzz step() function
 	    */
@@ -518,7 +517,7 @@ namespace buzz_utility{
 	      buzzvm_dump(VM);
 	   }
 	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
-	   /* look into this currently we don't have swarm feature at all */
+	   /* look into this currently we don't have swarm feature tested */
 	   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
 	   /*Print swarm*/
 	   //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
@@ -571,7 +570,6 @@ int update_step_test(){
   buzzuav_closures::buzzuav_update_prox(VM);
   buzzuav_closures::buzzuav_update_currentpos(VM);
   buzzuav_closures::buzzuav_update_flight_status(VM);
-  buzzuav_closures::buzzuav_update_obstacle(VM);
 
  int a = buzzvm_function_call(VM, "step", 0);
 if(a != BUZZVM_STATE_READY){

src/buzzuav_closures.cpp

@@ -7,362 +7,346 @@
  */
 //#define _GNU_SOURCE
 #include "buzzuav_closures.h"
-#include "buzz_utility.h"
+//#include "roscontroller.h"
 namespace buzzuav_closures{
 
-// TODO: Minimize the required global variables and put them in the header
+	// TODO: Minimize the required global variables and put them in the header
+	//static const rosbzz_node::roscontroller* roscontroller_ptr;
+	/*forward declaration of ros controller ptr storing function*/
+	//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
+	static double goto_pos[3];
+	static double rc_goto_pos[3];
+	static float batt[3];
+	static float obst[5]={0,0,0,0,0};
+	static double cur_pos[3];
+	static uint8_t status;
+	static int cur_cmd = 0;
+	static int rc_cmd=0;
+	static int buzz_cmd=0;
+	static float height=0;
+	/****************************************/
+	/****************************************/
 
-static double goto_pos[3];
+	int buzzros_print(buzzvm_t vm) {
-static double rc_goto_pos[3];
+	int i;
-static float batt[3];
+	   for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
-static float obst[5]={0,0,0,0,0};
+	      buzzvm_lload(vm, i);
-static double cur_pos[3];
+	      buzzobj_t o = buzzvm_stack_at(vm, 1);
-static uint8_t status;
+	      buzzvm_pop(vm);
-static int cur_cmd = 0;
+	      switch(o->o.type) {
-static int rc_cmd=0;
+		 case BUZZTYPE_NIL:
-static int buzz_cmd=0;
+		    ROS_INFO("BUZZ - [nil]");
-static float height=0;
+		    break;
-/****************************************/
+		 case BUZZTYPE_INT:
-/****************************************/
+		    ROS_INFO("%d", o->i.value);
+		    //fprintf(stdout, "%d", o->i.value);
+		    break;
+		 case BUZZTYPE_FLOAT:
+		    ROS_INFO("%f", o->f.value);
+		    break;
+		 case BUZZTYPE_TABLE:
+		    ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
+		    break;
+		 case BUZZTYPE_CLOSURE:
+		    if(o->c.value.isnative)
+			ROS_INFO("[n-closure @%d]", o->c.value.ref);
+		    else
+			ROS_INFO("[c-closure @%d]", o->c.value.ref);
+		    break;
+		 case BUZZTYPE_STRING:
+		    ROS_INFO("%s", o->s.value.str);
+		    break;
+		 case BUZZTYPE_USERDATA:
+		    ROS_INFO("[userdata @%p]", o->u.value);
+		    break;
+		 default:
+		    break;
+	      }
+	   }
+	   //fprintf(stdout, "\n");
+	   return buzzvm_ret0(vm);
+	}
 
-int buzzros_print(buzzvm_t vm) {
+	/*----------------------------------------/
-   int i;
+	/ Compute GPS destination from current position and desired Range and Bearing
-   for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
+	/----------------------------------------*/
-      buzzvm_lload(vm, i);
+	#define EARTH_RADIUS (double) 6371000.0
-      buzzobj_t o = buzzvm_stack_at(vm, 1);
+	void gps_from_rb(double  range, double bearing, double out[3]) {
-      buzzvm_pop(vm);
+		double lat = cur_pos[0]*M_PI/180.0;
-      switch(o->o.type) {
+		double lon = cur_pos[1]*M_PI/180.0;
-         case BUZZTYPE_NIL:
+	 	out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
-	    ROS_INFO("BUZZ - [nil]");
+		out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
-            break;
+		out[0] = out[0]*180.0/M_PI;
-         case BUZZTYPE_INT:
+		out[1] = out[1]*180.0/M_PI;
-	    ROS_INFO("%d", o->i.value);
+		out[2] = height; //constant height.
-            //fprintf(stdout, "%d", o->i.value);
+	}
-            break;
-         case BUZZTYPE_FLOAT:
-	    ROS_INFO("%f", o->f.value);
-            break;
-         case BUZZTYPE_TABLE:
-            ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
-            break;
-         case BUZZTYPE_CLOSURE:
-            if(o->c.value.isnative)
-		ROS_INFO("[n-closure @%d]", o->c.value.ref);
-            else
-		ROS_INFO("[c-closure @%d]", o->c.value.ref);
-            break;
-         case BUZZTYPE_STRING:
-	    ROS_INFO("%s", o->s.value.str);
-            break;
-         case BUZZTYPE_USERDATA:
-            ROS_INFO("[userdata @%p]", o->u.value);
-            break;
-         default:
-            break;
-      }
-   }
-   //fprintf(stdout, "\n");
-   return buzzvm_ret0(vm);
-}
 
-/*----------------------------------------/
+	// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
-/ Compute GPS destination from current position and desired Range and Bearing
+	double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
-/----------------------------------------*/
+	double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
-#define EARTH_RADIUS (double) 6371000.0
+	double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
-void gps_from_rb(double  range, double bearing, double out[3]) {
-	double lat = cur_pos[0]*M_PI/180.0;
-	double lon = cur_pos[1]*M_PI/180.0;
- 	out[0] = asin(sin(lat) * cos(range/EARTH_RADIUS) + cos(lat) * sin(range/EARTH_RADIUS) * cos(bearing));
-	out[1] = lon + atan2(sin(bearing) * sin(range/EARTH_RADIUS) * cos(lat), cos(range/EARTH_RADIUS) - sin(lat)*sin(out[0]));
-	out[0] = out[0]*180.0/M_PI;
-	out[1] = out[1]*180.0/M_PI;
-	out[2] = height; //constant height.
-}
 
-// Hard coded GPS position in Park Maisonneuve, Montreal, Canada for simulation tests
+	/*----------------------------------------/
-double hcpos1[4] = {45.564489, -73.562537, 45.564140, -73.562336};
+	/ Buzz closure to move following a 2D vector
-double hcpos2[4] = {45.564729, -73.562060, 45.564362, -73.562958};
+	/----------------------------------------*/
-double hcpos3[4] = {45.564969, -73.562838, 45.564636, -73.563677};
+	int buzzuav_moveto(buzzvm_t vm) {
+	   buzzvm_lnum_assert(vm, 2);
+	   buzzvm_lload(vm, 1); /* dx */
+	   buzzvm_lload(vm, 2); /* dy */
+	   //buzzvm_lload(vm, 3); /* Latitude */
+	   //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
+	   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
+	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+	   float dy = buzzvm_stack_at(vm, 1)->f.value;
+	   float dx = buzzvm_stack_at(vm, 2)->f.value;
+	   double d = sqrt(dx*dx+dy*dy);	//range
+	   goto_pos[0]=dx;
+           goto_pos[1]=dy;
+           goto_pos[2]=0;
+	   /*double b = atan2(dy,dx);		//bearing
+	   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
+	   gps_from_rb(d, b, goto_pos);
+	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
+	   printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
+	   buzz_cmd=5;
+	   return buzzvm_ret0(vm);
+	}
 
-/*----------------------------------------/
+	/*----------------------------------------/
-/ Buzz closure to move following a 2D vector
+	/ Buzz closure to go directly to a GPS destination from the Mission Planner
-/----------------------------------------*/
+	/----------------------------------------*/
-int buzzuav_moveto(buzzvm_t vm) {
+	int buzzuav_goto(buzzvm_t vm) {
-   buzzvm_lnum_assert(vm, 2);
+	   rc_goto_pos[2]=height;
-   buzzvm_lload(vm, 1); /* dx */
+	   set_goto(rc_goto_pos);
-   buzzvm_lload(vm, 2); /* dy */
+	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
-   //buzzvm_lload(vm, 3); /* Latitude */
+	   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
-   //buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
+	   buzz_cmd=2;
-   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
+	   return buzzvm_ret0(vm);
-   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
+	}
-   float dy = buzzvm_stack_at(vm, 1)->f.value;
-   float dx = buzzvm_stack_at(vm, 2)->f.value;
-   double d = sqrt(dx*dx+dy*dy);	//range
-   double b = atan2(dy,dx);		//bearing
-   printf(" Vector for Goto: %.7f,%.7f\n",dx,dy);
-   gps_from_rb(d, b, goto_pos);
-   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
-   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
-   buzz_cmd=2;
-   return buzzvm_ret0(vm);
-}
 
-/*----------------------------------------/
+	/*----------------------------------------/
-/ Buzz closure to go directly to a GPS destination from the Mission Planner
+	/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
-/----------------------------------------*/
+	/----------------------------------------*/
-int buzzuav_goto(buzzvm_t vm) {
+	int buzzuav_arm(buzzvm_t vm) {
-   rc_goto_pos[2]=height;
+	   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
-   set_goto(rc_goto_pos);
+	   printf(" Buzz requested Arm \n");
-   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
+	   buzz_cmd=3;
-   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
+	   return buzzvm_ret0(vm);
-   buzz_cmd=2;
+	}
-   return buzzvm_ret0(vm);
+	int buzzuav_disarm(buzzvm_t vm) {
-}
+	   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
+	   printf(" Buzz requested Disarm  \n");
+	   buzz_cmd=4;
+	   return buzzvm_ret0(vm);
+	}
 
-/*----------------------------------------/
+	/*---------------------------------------/
-/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and runing
+	/ Buss closure for basic UAV commands
-/----------------------------------------*/
+	/---------------------------------------*/
-int buzzuav_arm(buzzvm_t vm) {
+	int buzzuav_takeoff(buzzvm_t vm) {
-   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
+	   buzzvm_lnum_assert(vm, 1);
-   printf(" Buzz requested Arm \n");
+	   buzzvm_lload(vm, 1); /* Altitude */
-   buzz_cmd=3;
+	   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-   return buzzvm_ret0(vm);
+	   goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
-}
+	   height = goto_pos[2];
-int buzzuav_disarm(buzzvm_t vm) {
+	   cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
-   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
+	   printf(" Buzz requested Take off !!! \n");
-   printf(" Buzz requested Disarm  \n");
+	   buzz_cmd = 1;
-   buzz_cmd=4;
+	   return buzzvm_ret0(vm);
-   return buzzvm_ret0(vm);
+	}
-}
 
-/*---------------------------------------/
+	int buzzuav_land(buzzvm_t vm) {
-/ Buss closure for basic UAV commands
+	   cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
-/---------------------------------------*/
+	   printf(" Buzz requested Land !!! \n");
-int buzzuav_takeoff(buzzvm_t vm) {
+	   buzz_cmd = 1;
-   buzzvm_lnum_assert(vm, 1);
+	   return buzzvm_ret0(vm);
-   buzzvm_lload(vm, 1); /* Altitude */
+	}
-   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-   goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
-   height = goto_pos[2];
-   cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
-   printf(" Buzz requested Take off !!! \n");
-   buzz_cmd = 1;
-   return buzzvm_ret0(vm);
-}
 
-int buzzuav_land(buzzvm_t vm) {
+	int buzzuav_gohome(buzzvm_t vm) {
-   cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
+	   cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
-   printf(" Buzz requested Land !!! \n");
+	   printf(" Buzz requested gohome !!! \n");
-   buzz_cmd = 1;
+	   buzz_cmd = 1;
-   return buzzvm_ret0(vm);
+	   return buzzvm_ret0(vm);
-}
+	}
-
-int buzzuav_gohome(buzzvm_t vm) {
-   cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
-   printf(" Buzz requested gohome !!! \n");
-   buzz_cmd = 1;
-   return buzzvm_ret0(vm);
-}
 
 
-/*-------------------------------
+	/*-------------------------------
-/ Get/Set to transfer variable from Roscontroller to buzzuav
+	/ Get/Set to transfer variable from Roscontroller to buzzuav
-/------------------------------------*/
+	/------------------------------------*/
-double* getgoto() {
+	double* getgoto() {
-	return goto_pos;
+		return goto_pos;
-}
+	}
 
-int getcmd() {
+	int getcmd() {
-	return cur_cmd;
+		return cur_cmd;
-}
+	}
 
-void set_goto(double pos[]) {
+	void set_goto(double pos[]) {
-	goto_pos[0] = pos[0];
+		goto_pos[0] = pos[0];
-	goto_pos[1] = pos[1];
+		goto_pos[1] = pos[1];
-	goto_pos[2] = pos[2];
+		goto_pos[2] = pos[2];
-
+
-}
+	}
-
+
-int bzz_cmd() {
+	int bzz_cmd() {
-	int cmd = buzz_cmd;
+		int cmd = buzz_cmd;
-	buzz_cmd = 0;
+		buzz_cmd = 0;
-	return cmd;
+		return cmd;
-}
+	}
-
+
-void rc_set_goto(double pos[]) {
+	void rc_set_goto(double pos[]) {
-	rc_goto_pos[0] = pos[0];
+		rc_goto_pos[0] = pos[0];
-	rc_goto_pos[1] = pos[1];
+		rc_goto_pos[1] = pos[1];
-	rc_goto_pos[2] = pos[2];
+		rc_goto_pos[2] = pos[2];
-
+
-}
+	}
-void rc_call(int rc_cmd_in) {
+	void rc_call(int rc_cmd_in) {
-	rc_cmd = rc_cmd_in;
+		rc_cmd = rc_cmd_in;
-}
+	}
-
+
-void set_obstacle_dist(float dist[]) {
+	void set_obstacle_dist(float dist[]) {
-	for (int i = 0; i < 5; i++)
+		for (int i = 0; i < 5; i++)
-		obst[i] = dist[i];
+			obst[i] = dist[i];
-}
+	}
-
+
-void set_battery(float voltage,float current,float remaining){
+	void set_battery(float voltage,float current,float remaining){
- batt[0]=voltage;
+	 batt[0]=voltage;
- batt[1]=current;
+	 batt[1]=current;
- batt[2]=remaining;
+	 batt[2]=remaining;
-}
+	}
-
+
-int buzzuav_update_battery(buzzvm_t vm) {
+	int buzzuav_update_battery(buzzvm_t vm) {
-   //static char BATTERY_BUF[256];
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
+	   buzzvm_pusht(vm);
-   buzzvm_pusht(vm);
+	   buzzvm_dup(vm);
-   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "voltage", 1));
+	   buzzvm_pushf(vm, batt[0]);
-   buzzvm_pushf(vm, batt[0]);
+	   buzzvm_tput(vm);
-   buzzvm_tput(vm);
+	   buzzvm_dup(vm);
-   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "current", 1));
+	   buzzvm_pushf(vm, batt[1]);
-   buzzvm_pushf(vm, batt[1]);
+	   buzzvm_tput(vm);
-   buzzvm_tput(vm);
+	   buzzvm_dup(vm);
-   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "capacity", 1));
+	   buzzvm_pushf(vm, batt[2]);
-   buzzvm_pushf(vm, batt[2]);
+	   buzzvm_tput(vm);
-   buzzvm_tput(vm);
+	   buzzvm_gstore(vm);
-   buzzvm_gstore(vm);
+	   return vm->state;
-   return vm->state;
+	}
-}
+	/****************************************/
-/****************************************/
+	/*current pos update*/
-/*current pos update*/
+	/***************************************/
-/***************************************/
+	void set_currentpos(double latitude, double longitude, double altitude){
-void set_currentpos(double latitude, double longitude, double altitude){
+	   cur_pos[0]=latitude;
-   cur_pos[0]=latitude;
+	   cur_pos[1]=longitude;
-   cur_pos[1]=longitude;
+	   cur_pos[2]=altitude;
-   cur_pos[2]=altitude;
+	}
-}
+	/****************************************/
-/****************************************/
+	int buzzuav_update_currentpos(buzzvm_t vm) {
-int buzzuav_update_currentpos(buzzvm_t vm) {
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
+	   buzzvm_pusht(vm);
-   buzzvm_pusht(vm);
+	   buzzvm_dup(vm);
-   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
+	   buzzvm_pushf(vm, cur_pos[0]);
-   buzzvm_pushf(vm, cur_pos[0]);
+	   buzzvm_tput(vm);
-   buzzvm_tput(vm);
+	   buzzvm_dup(vm);
-   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
+	   buzzvm_pushf(vm, cur_pos[1]);
-   buzzvm_pushf(vm, cur_pos[1]);
+	   buzzvm_tput(vm);
-   buzzvm_tput(vm);
+	   buzzvm_dup(vm);
-   buzzvm_dup(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
+	   buzzvm_pushf(vm, cur_pos[2]);
-   buzzvm_pushf(vm, cur_pos[2]);
+	   buzzvm_tput(vm);
-   buzzvm_tput(vm);
+	   buzzvm_gstore(vm);
-   buzzvm_gstore(vm);
+	   return vm->state;
-   return vm->state;
+	}
-}
+
-/*-----------------------------------------
+	void flight_status_update(uint8_t state){
-/ Create an obstacle Buzz table from proximity sensors
+	   status=state;
-/ TODO: swap to proximity function below
+	}
---------------------------------------------*/
+
-
+	/*----------------------------------------------------
-int buzzuav_update_obstacle(buzzvm_t vm) {
+	/ Create the generic robot table with status, remote controller current comand and destination
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "obstacle", 1));
+	/ and current position of the robot
-   buzzvm_pusht(vm);
+	/ TODO: change global name for robot
-   buzzvm_dup(vm);
+	/------------------------------------------------------*/
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
+	int buzzuav_update_flight_status(buzzvm_t vm) {
-   buzzvm_pushf(vm, obst[0]);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
-   buzzvm_tput(vm);
+	   buzzvm_pusht(vm);
-   buzzvm_dup(vm);
+	   buzzvm_dup(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
-   buzzvm_pushf(vm, obst[1]);
+	   buzzvm_pushi(vm, rc_cmd);
-   buzzvm_tput(vm);
+	   buzzvm_tput(vm);
-   buzzvm_dup(vm);
+	   buzzvm_dup(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
+	   rc_cmd=0;
-   buzzvm_pushf(vm, obst[2]);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
-   buzzvm_tput(vm);
+	   buzzvm_pushi(vm, status);
-   buzzvm_dup(vm);
+	   buzzvm_tput(vm); 
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
+	   buzzvm_gstore(vm);
-   buzzvm_pushf(vm, obst[3]);
+	   //also set rc_controllers goto
-   buzzvm_tput(vm);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
-   buzzvm_dup(vm);
+	   buzzvm_pusht(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
+	   buzzvm_dup(vm);
-   buzzvm_pushf(vm, obst[4]);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
-   buzzvm_tput(vm);
+	   buzzvm_pushf(vm, rc_goto_pos[0]);
-   buzzvm_gstore(vm);
+	   buzzvm_tput(vm);
-   return vm->state;
+	   buzzvm_dup(vm);
-}
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
-
+	   buzzvm_pushf(vm, rc_goto_pos[1]);
-void flight_status_update(uint8_t state){
+	   buzzvm_tput(vm);
-   status=state;
+	   buzzvm_dup(vm);
-}
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
-
+	   buzzvm_pushf(vm, rc_goto_pos[2]);
-/*----------------------------------------------------
+	   buzzvm_tput(vm);
-/ Create the generic robot table with status, remote controller current comand and destination
+	   buzzvm_gstore(vm);
-/ and current position of the robot
+	   return vm->state;
-/ TODO: change global name for robot
+	}
-/------------------------------------------------------*/
+
-int buzzuav_update_flight_status(buzzvm_t vm) {
+
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
+
-   buzzvm_pusht(vm);
+	/******************************************************/
-   buzzvm_dup(vm);
+	/*Create an obstacle Buzz table from proximity sensors*/
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_cmd", 1));
+	/******************************************************/
-   buzzvm_pushi(vm, rc_cmd);
+
-   buzzvm_tput(vm);
+	int buzzuav_update_prox(buzzvm_t vm) {
-   buzzvm_dup(vm);
+	  buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
-   rc_cmd=0;
+	   buzzvm_pusht(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
+	   buzzvm_dup(vm);
-   buzzvm_pushi(vm, status);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "bottom", 1));
-   buzzvm_tput(vm); 
+	   buzzvm_pushf(vm, obst[0]);
-   buzzvm_gstore(vm);
+	   buzzvm_tput(vm);
-   //also set rc_controllers goto
+	   buzzvm_dup(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "front", 1));
-   buzzvm_pusht(vm);
+	   buzzvm_pushf(vm, obst[1]);
-   buzzvm_dup(vm);
+	   buzzvm_tput(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "latitude", 1));
+	   buzzvm_dup(vm);
-   buzzvm_pushf(vm, rc_goto_pos[0]);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "right", 1));
-   buzzvm_tput(vm);
+	   buzzvm_pushf(vm, obst[2]);
-   buzzvm_dup(vm);
+	   buzzvm_tput(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "longitude", 1));
+	   buzzvm_dup(vm);
-   buzzvm_pushf(vm, rc_goto_pos[1]);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "back", 1));
-   buzzvm_tput(vm);
+	   buzzvm_pushf(vm, obst[3]);
-   buzzvm_dup(vm);
+	   buzzvm_tput(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "altitude", 1));
+	   buzzvm_dup(vm);
-   buzzvm_pushf(vm, rc_goto_pos[2]);
+	   buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
-   buzzvm_tput(vm);
+	   buzzvm_pushf(vm, obst[4]);
-   buzzvm_gstore(vm);
+	   buzzvm_tput(vm);
-   return vm->state;
+	   buzzvm_gstore(vm);
-}
+	   return vm->state;
-
+	}
-
+
-
+	/**********************************************/
-/****************************************/
+	/*Dummy closure for use during update testing */
-/*To do !!!!!*/
+	/**********************************************/
-/****************************************/
+
-
+	int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
-int buzzuav_update_prox(buzzvm_t vm) {
+
- /*  static char PROXIMITY_BUF[256];
+	/***********************************************/
-   int i;
+	/* Store Ros controller object pointer           */
-   //kh4_proximity_ir(PROXIMITY_BUF, DSPIC);
+	/***********************************************/
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity_ir", 1));
+	
-   buzzvm_pusht(vm);
+	//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin){
-   for(i = 0; i < 8; i++) {
+	//roscontroller_ptr = roscontroller_ptrin;
-      buzzvm_dup(vm);
+	//} 
-      buzzvm_pushi(vm, i+1);
-      buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
-      buzzvm_tput(vm);
-   }
-   buzzvm_gstore(vm);
-   buzzvm_pushs(vm, buzzvm_string_register(vm, "ground_ir", 1));
-   buzzvm_pusht(vm);
-   for(i = 8; i < 12; i++) {
-      buzzvm_dup(vm);
-      buzzvm_pushi(vm, i-7);
-      buzzvm_pushi(vm, (PROXIMITY_BUF[i*2] | PROXIMITY_BUF[i*2+1] << 8));
-      buzzvm_tput(vm);
-   }
-   buzzvm_gstore(vm);*/
-   return vm->state;
-}
-
-/****************************************/
-/****************************************/
-
-int dummy_closure(buzzvm_t vm){ return buzzvm_ret0(vm);}
 
 }

src/rosbuzz.cpp

@@ -18,6 +18,8 @@ int main(int argc, char **argv)
   ros::init(argc, argv, "rosBuzz"); 
   ros::NodeHandle n_c("~");
   rosbzz_node::roscontroller RosController(n_c);
+  /*Register ros controller object inside buzz*/
+  //buzzuav_closures::set_ros_controller_ptr(&RosController);
   RosController.RosControllerRun(); 
   return 0;
 }

src/roscontroller.cpp

@@ -2,7 +2,9 @@
 #include <thread>
 namespace rosbzz_node{
 
-	/***Constructor***/
+	/*---------------
+	/Constructor
+	---------------*/
 	roscontroller::roscontroller(ros::NodeHandle n_c)	
 	{
 		ROS_INFO("Buzz_node");
@@ -20,7 +22,9 @@ namespace rosbzz_node{
 		multi_msg = true;
 	}
 
-	/***Destructor***/
+	/*---------------------
+	/Destructor
+	---------------------*/
 	roscontroller::~roscontroller()
 	{
 		/* All done */  
@@ -30,7 +34,9 @@ namespace rosbzz_node{
    		uav_done();
 	}
 
-	/*rosbuzz_node run*/
+	/*-------------------------------------------------
+	/rosbuzz_node loop method executed once every step
+	/--------------------------------------------------*/
 	void roscontroller::RosControllerRun(){
 		/* Set the Buzz bytecode */
 		if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
@@ -40,34 +46,16 @@ namespace rosbzz_node{
   			{
       				/*Update neighbors position inside Buzz*/
      				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
-				auto current_time = ros::Time::now();
+				/*Neighbours of the robot published with id in respective topic*/
-				map< int, buzz_utility::Pos_struct >::iterator it;
+				neighbours_pos_publisher();
-				rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); 
-				neigh_pos_array.header.frame_id = "/world";
-				neigh_pos_array.header.stamp = current_time;
-				for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
-					sensor_msgs::NavSatFix neigh_tmp;
-					//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;				
-					neigh_tmp.header.stamp = current_time;
-					neigh_tmp.header.frame_id = "/world";
-					neigh_tmp.position_covariance_type=it->first; //custom robot id storage
-                        		neigh_tmp.latitude=(it->second).x;
-                        		neigh_tmp.longitude=(it->second).y;
-                        		neigh_tmp.altitude=(it->second).z;
-    					neigh_pos_array.pos_neigh.push_back(neigh_tmp); 
-					//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;  					
-					}
-				neigh_pos_pub.publish(neigh_pos_array); 
-
-			
 				/*Check updater state and step code*/
   				update_routine(bcfname.c_str(), dbgfname.c_str());
-      				
 				/*Step buzz script */
       			buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
-	
 		  		prepare_msg_and_publish();
+				/*call flight controler service to set command long*/
+				flight_controler_service_call();
 				/*Set multi message available after update*/
 				if(get_update_status()){
 					set_read_update_status();
@@ -127,7 +115,9 @@ namespace rosbzz_node{
 		// initialize topics to null?
 
 	}
-
+	/*-----------------------------------------------------------------------------------
+	/Obtains publisher, subscriber and services from yml file based on the robot used
+	/-----------------------------------------------------------------------------------*/
 	void roscontroller::GetSubscriptionParameters(ros::NodeHandle node_handle){
 		//todo: make it as an array in yaml?
 		m_sMySubscriptions.clear();
@@ -177,7 +167,8 @@ namespace rosbzz_node{
   		//mavros_msgs::PositionTarget message;
 
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
-		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
+		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
+		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>("/setpoint_raw/local",1000);	
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
@@ -185,7 +176,9 @@ namespace rosbzz_node{
 
 		multi_msg=true;
 	}
-	
+	/*---------------------------------------
+	/Robot independent subscribers
+	/--------------------------------------*/
 	void roscontroller::Subscribe(ros::NodeHandle n_c){
 
   		for(std::map<std::string, std::string>::iterator it = m_smTopic_infos.begin(); it != m_smTopic_infos.end(); ++it){
@@ -210,6 +203,7 @@ namespace rosbzz_node{
 	/ Create Buzz bytecode from the bzz script inputed
 	/-------------------------------------------------------*/
 	void roscontroller::Compile_bzz(){
+		/*TODO: change to bzzc instead of bzzparse and also add -I for includes*/
 		/*Compile the buzz code .bzz to .bo*/
 		stringstream bzzfile_in_compile;
 	        std::string  path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/"));
@@ -219,7 +213,6 @@ namespace rosbzz_node{
 		std::string  name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
  		name = name.substr(0,name.find_last_of("."));
 		bzzfile_in_compile << "bzzparse "<<bzzfile_name<<" "<<path<< name<<".basm";
-   		//system("rm ../catkin_ws/src/rosbuzz/src/out.basm ../catkin_ws/src/rosbuzz/src/out.bo ../catkin_ws/src/rosbuzz/src/out.bdbg");
    		system(bzzfile_in_compile.str().c_str());
 		bzzfile_in_compile.str("");
            	bzzfile_in_compile <<"bzzasm "<<path<<name<<".basm "<<path<<name<<".bo "<<path<<name<<".bdbg";
@@ -232,9 +225,32 @@ namespace rosbzz_node{
 		dbgfname = bzzfile_in_compile.str();
    		
 	}
+	/*----------------------------------------------------
+	/ Publish neighbours pos and id in neighbours pos topic
+	/----------------------------------------------------*/
+	void roscontroller::neighbours_pos_publisher(){
+		auto current_time = ros::Time::now();
+		map< int, buzz_utility::Pos_struct >::iterator it;
+		rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); 
+		neigh_pos_array.header.frame_id = "/world";
+		neigh_pos_array.header.stamp = current_time;
+		for (it=raw_neighbours_pos_map.begin(); it!=raw_neighbours_pos_map.end(); ++it){
+			sensor_msgs::NavSatFix neigh_tmp;
+			//cout<<"iterator it val: "<< it-> first << " After convertion: " <<(uint8_t) buzz_utility::get_rid_uint8compac(it->first)<<endl;				
+			neigh_tmp.header.stamp = current_time;
+			neigh_tmp.header.frame_id = "/world";
+			neigh_tmp.position_covariance_type=it->first; //custom robot id storage
+        		neigh_tmp.latitude=(it->second).x;
+        		neigh_tmp.longitude=(it->second).y;
+        		neigh_tmp.altitude=(it->second).z;
+			neigh_pos_array.pos_neigh.push_back(neigh_tmp); 
+			//std::cout<<"long obt"<<neigh_tmp.longitude<<endl;  					
+			}
+		neigh_pos_pub.publish(neigh_pos_array); 	
+	}
 
 	/*--------------------------------------------------------
-	/ Fonctions handling the local MAV ROS fligh controller
+	/ Functions handling the local MAV ROS flight controller
 	/-------------------------------------------------------*/
 	void roscontroller::Arm(){
 		mavros_msgs::CommandBool arming_message;
@@ -248,80 +264,33 @@ namespace rosbzz_node{
 			ROS_INFO("FC Arm Service call failed!");
 		}
 	}
-
+	/*---------------------------------------------------------
-	/* Prepare Buzz messages payload for each step and publish */
+	/ Set mode for the solos
-	/*******************************************************************************************************/
+	/---------------------------------------------------------*/
-	/* Message format of payload (Each slot is uint64_t)						      					   */
+	void roscontroller::SetMode(){
-	/* _________________________________________________________________________________________________   */
+		mavros_msgs::SetMode set_mode_message;
-	/*|	|     |	    |						     |			            | 								   */
+		set_mode_message.request.base_mode = 0;
-	/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |  */
+		set_mode_message.request.custom_mode = "GUIDED"; // shit!
-	/*|_____|_____|_____|________________________________________________|______________________________|  */
+		if(mode_client.call(set_mode_message)) {
-	/*******************************************************************************************************/	
+			ROS_INFO("Set Mode Service call successful!");
-	void roscontroller::prepare_msg_and_publish(){
+		} else {
-		
+			ROS_INFO("Set Mode Service call failed!");
- 		/* flight controller client call if requested from Buzz*/
-		/*FC call for takeoff,land and gohome*/
-		/* TODO: this should go in a separate function and be called by the main Buzz step */
-
-		int tmp = buzzuav_closures::bzz_cmd();
-		std::cout<< "Going: " << tmp << std::endl;
-    	double* goto_pos = buzzuav_closures::getgoto();
-    	// TODO: Make sure that land and takeoff use the right statuses in bzz
-		if (tmp == 1){
-			cmd_srv.request.param7 = goto_pos[2];
-			cmd_srv.request.command =  buzzuav_closures::getcmd();
-			std::cout << " CMD: " << buzzuav_closures::getcmd() << std::endl;
-			// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND mode
-
-			switch(buzzuav_closures::getcmd()){
-			case mavros_msgs::CommandCode::NAV_LAND:
-				if(current_mode != "LAND")
-					SetMode("LAND", 0);
-				break;
-			case mavros_msgs::CommandCode::NAV_TAKEOFF:
-				if(current_mode != "GUIDED")
-					SetMode("GUIDED", 0); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
-				break;
-			}
-
-			if (mav_client.call(cmd_srv))
-				{ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
-			else
-				ROS_ERROR("Failed to call service from flight controller");
-
-		} else if (tmp == 2) { /*FC call for goto*/
-			/*prepare the goto publish message */
-			cmd_srv.request.param5 = goto_pos[0];
-			cmd_srv.request.param6 = goto_pos[1];
-			cmd_srv.request.param7 = goto_pos[2];
-			cmd_srv.request.command =  buzzuav_closures::getcmd();
-
-			if (mav_client.call(cmd_srv)) {
-				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
-			}
-	    	else
-	    		ROS_ERROR("Failed to call service from flight controller");
-
-			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
-
-			if (mav_client.call(cmd_srv)){
-				ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
-			}
-	    	else
-	    		ROS_ERROR("Failed to call service from flight controller");
-
-		} else if (tmp == 3) { /*FC call for arm*/
-			// SOLO SPECIFIC: when you land, mode is LANDED, which is not armable, therefore change it to LOITER
-			SetMode("LOITER", 0);
-			armstate=1;
-			Arm(); 
-		} else if (tmp == 4){
-			SetMode("LOITER", 0);
-			armstate=0;
-			Arm();
 		}
-    		/*obtain Pay load to be sent*/
+	}
-		//fprintf(stdout, "before getting msg from utility\n");
+
+
+	/*-----------------------------------------------------------------------------------------------------
+	/Prepare Buzz messages payload for each step and publish
+	/-----------------------------------------------------------------------------------------------------*/
+	/*----------------------------------------------------------------------------------------------------*/
+	/* Message format of payload (Each slot is uint64_t)						       /
+	/ _________________________________________________________________________________________________    /
+	/|	|     |	    |						     |			           |   /
+	/|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |   /
+	/|_____|_____|_____|________________________________________________|______________________________|  */
+	/*----------------------------------------------------------------------------------------------------*/	
+	void roscontroller::prepare_msg_and_publish(){
+    		/*obtain Pay load to be sent*/  
    		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
     		uint64_t  position[3];
   		/*Appened current position to message*/
@@ -335,21 +304,6 @@ namespace rosbzz_node{
     		for(int i=0;i<out[0];i++){
     			payload_out.payload64.push_back(payload_out_ptr[i]);
     		}	
-    		/*int i=0;
-		uint64_t message_obt[payload_out.payload64.size()];
-    		for(std::vector<long unsigned int>::const_iterator it = payload_out.payload64.begin();
-			it != payload_out.payload64.end(); ++it){
-			message_obt[i] =(uint64_t) *it;
-			i++;
-        	}*/
-		/*for(int i=0;i<payload_out.payload64.size();i++){
-			cout<<" [Debug:] sent message "<<payload_out.payload64[i]<<endl;
-			out = buzz_utility::u64_cvt_u16(message_obt[i]);
-			for(int k=0;k<4;k++){
-				cout<<" [Debug:] sent message "<<out[k]<<endl;
-			}
-		}*/
-
 		/*Add Robot id and message number to the published message*/
 		if (message_number < 0) message_number=0;
 		else message_number++;
@@ -360,8 +314,7 @@ namespace rosbzz_node{
     		payload_pub.publish(payload_out);
     		delete[] out;
     		delete[] payload_out_ptr;
-
+		/*Check for updater message if present send*/
-
 		if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
 			uint8_t* buff_send = 0;
 	   		uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
@@ -399,36 +352,74 @@ namespace rosbzz_node{
 		    	multi_msg=false;
 		    	delete[] payload_64;
 		}
-		/*Request xbee to stop any multi transmission updated not needed any more*/
-		//if(get_update_status()){
-		//	set_read_update_status();
-		//	mavros_msgs::Mavlink stop_req_packet;
-		//	stop_req_packet.payload64.push_back((uint64_t) XBEE_STOP_TRANSMISSION);
-		//	payload_pub.publish(stop_req_packet);
-		//	std::cout << "request xbee to stop multi-packet transmission" << std::endl;	
 		
-		//}
+	}
-		
+	/*---------------------------------------------------------------------------------
-		}
+	/Flight controller service call every step if there is a command set from bzz script
-
+	/-------------------------------------------------------------------------------- */
-
+	void roscontroller::flight_controler_service_call(){
-	/*Refresh neighbours Position for every ten step*/
+		/* flight controller client call if requested from Buzz*/
+		/*FC call for takeoff,land, gohome and arm/disarm*/
+		int tmp = buzzuav_closures::bzz_cmd();
+    		double* goto_pos = buzzuav_closures::getgoto();
+		if (tmp == 1){
+			cmd_srv.request.param7 = goto_pos[2];
+			//cmd_srv.request.z = goto_pos[2];
+			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
+			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+	    		else ROS_ERROR("Failed to call service from flight controller"); 
+		} else if (tmp == 2) { /*FC call for goto*/ 
+			/*prepare the goto publish message */
+			cmd_srv.request.param5 = goto_pos[0];
+    			cmd_srv.request.param6 = goto_pos[1];
+    			cmd_srv.request.param7 = goto_pos[2];
+    			cmd_srv.request.command =  buzzuav_closures::getcmd();
+			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+	    		else ROS_ERROR("Failed to call service from flight controller"); 
+    			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
+	    		else ROS_ERROR("Failed to call service from flight controller"); 
+		} else if (tmp == 3) { /*FC call for arm*/
+			armstate=1;
+			Arm(); 
+		} else if (tmp == 4){
+			armstate=0;
+			Arm();
+		} else if (tmp == 5) { /*Buzz call for moveto*/ 
+			/*prepare the goto publish message */
+			mavros_msgs::PositionTarget pt;
+			pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX && 				mavros_msgs::PositionTarget::IGNORE_VY && mavros_msgs::PositionTarget::IGNORE_VZ && mavros_msgs::PositionTarget::IGNORE_AFX && mavros_msgs::PositionTarget::IGNORE_AFY && mavros_msgs::PositionTarget::IGNORE_AFZ && mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
+			pt.coordinate_frame =  mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
+			pt.position.x = goto_pos[0];
+    			pt.position.y = goto_pos[1];
+    			pt.position.z = goto_pos[2];
+    			pt.yaw = 0;//goto_pos[3];
+			localsetpoint_pub.publish(pt);
+ 		}
+	}
+	/*----------------------------------------------
+	/Refresh neighbours Position for every ten step
+	/---------------------------------------------*/
 	void roscontroller::maintain_pos(int tim_step){
 		if(timer_step >=10){
 		neighbours_pos_map.clear();
-		//raw_neighbours_pos_map.clear();
+		//raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but have to clear !
 		timer_step=0;
 		}
 	}
 
-	/*Puts neighbours position*/
+	/*---------------------------------------------------------------------------------
+	/Puts neighbours position into local struct storage that is cleared every 10 step
+	/---------------------------------------------------------------------------------*/
 	void roscontroller::neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
 		map< int, buzz_utility::Pos_struct >::iterator it = neighbours_pos_map.find(id);
 		if(it!=neighbours_pos_map.end())
 		neighbours_pos_map.erase(it);
 		neighbours_pos_map.insert(make_pair(id, pos_arr));
 		}
-	/*Puts raw neighbours position*/
+	/*-----------------------------------------------------------------------------------
+	/Puts raw neighbours position into local storage for neighbours pos publisher
+	/-----------------------------------------------------------------------------------*/
 	void roscontroller::raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr ){
 		map< int, buzz_utility::Pos_struct >::iterator it = raw_neighbours_pos_map.find(id);
 		if(it!=raw_neighbours_pos_map.end())
@@ -436,14 +427,15 @@ namespace rosbzz_node{
 		raw_neighbours_pos_map.insert(make_pair(id, pos_arr));
 		}
 
-	/*Set the current position of the robot callback*/
+	/*--------------------------------------------------------
+	/Set the current position of the robot callback
+	/--------------------------------------------------------*/
 	void roscontroller::set_cur_pos(double latitude,
 			 double longitude,
 			 double altitude){
 		cur_pos [0] =latitude;
 		cur_pos [1] =longitude;
 		cur_pos [2] =altitude;
-
 	}
 
 
@@ -464,13 +456,17 @@ namespace rosbzz_node{
 		out[2] = 0.0;
 	}
 
-	/*battery status callback*/ 
+	/*------------------------------------------------------
+	/ Update battery status into BVM from subscriber
+	/------------------------------------------------------*/ 
 	void roscontroller::battery(const mavros_msgs::BatteryStatus::ConstPtr& msg){
 		buzzuav_closures::set_battery(msg->voltage,msg->current,msg->remaining);
 		//ROS_INFO("voltage : %d  current : %d  remaining : %d",msg->voltage, msg->current, msg ->remaining);
 	}
 
-	/*flight extended status update*/
+	/*----------------------------------------------------------------------
+	/Update flight extended status into BVM from subscriber for solos
+	/---------------------------------------------------------------------*/
 	void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
 		// http://wiki.ros.org/mavros/CustomModes
 		// TODO: Handle landing
@@ -483,47 +479,47 @@ namespace rosbzz_node{
 			buzzuav_closures::flight_status_update(1);//?
 	}
 
-	/*flight extended status update*/
+	/*------------------------------------------------------------
+	/Update flight extended status into BVM from subscriber
+	------------------------------------------------------------*/
 	void roscontroller::flight_extended_status_update(const mavros_msgs::ExtendedState::ConstPtr& msg){
 		buzzuav_closures::flight_status_update(msg->landed_state);
 	}
-	/*current position callback*/
+	/*-------------------------------------------------------------
+	/ Update current position into BVM from subscriber
+	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
 		set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
 		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
 	}
-	/*Obstacle distance call back*/
+	/*-------------------------------------------------------------
+	/Set obstacle Obstacle distance table into BVM from subscriber
+	/-------------------------------------------------------------*/	
 	void roscontroller::obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg){
 		float obst[5];
 		for(int i=0;i<5;i++)
 			obst[i]=msg->ranges[i];
 		buzzuav_closures::set_obstacle_dist(obst);
 	}
-	/*payload callback callback*/
 
+	/*-------------------------------------------------------------
+	/Push payload into BVM FIFO
+	/-------------------------------------------------------------*/
 	/*******************************************************************************************************/
 	/* Message format of payload (Each slot is uint64_t)						       */
 	/* _________________________________________________________________________________________________   */
 	/*|	|     |	    |						     |			            |  */
 	/*|Pos x|Pos y|Pos z|Size in Uint64_t|robot_id|Buzz_msg_size|Buzz_msg|Buzz_msgs with size.........  |  */
 	/*|_____|_____|_____|________________________________________________|______________________________|  */
-	/*******************************************************************************************************/
+	/*******************************************************************************************************/		
-
 	void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg){
-
+		/*Check for Updater message, if updater message push it into updater FIFO*/
-		/*Ack from xbee about its transfer complete of multi packet*/
+		if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 5){
-		/*if((uint64_t)msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT && msg->payload64.size() == 1){
-			multi_msg=true;
-			std::cout << "ACK From xbee after transimssion of code " << std::endl;
-		}*/
-		if((uint64_t)msg->payload64[0]==(uint64_t)UPDATER_MESSAGE_CONSTANT && msg->payload64.size() > 10){
 			uint16_t obt_msg_size=sizeof(uint64_t)*(msg->payload64.size());
 			uint64_t message_obt[obt_msg_size];
 			/* Go throught the obtained payload*/
 			for(int i=0;i < (int)msg->payload64.size();i++){
 				message_obt[i] =(uint64_t)msg->payload64[i];
-		        	//cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
-				//i++;
 			}
 
 			uint8_t* pl =(uint8_t*)malloc(obt_msg_size);
@@ -533,7 +529,7 @@ namespace rosbzz_node{
 			uint16_t unMsgSize = *(uint16_t*)(pl);
 			//uint16_t tot;
 			//tot+=sizeof(uint16_t);
-     			fprintf(stdout,"Update packet read msg size : %u \n",unMsgSize);	
+     			fprintf(stdout,"Update packet, read msg size : %u \n",unMsgSize);	
 			if(unMsgSize>0){
 				code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)),unMsgSize);
 				//fprintf(stdout,"before in queue process : utils\n");
@@ -541,10 +537,8 @@ namespace rosbzz_node{
 				//fprintf(stdout,"after in queue process : utils\n");
 			}
    			delete[] pl;
-
-
 		}
-
+		/*BVM FIFO message*/
 		else if(msg->payload64.size() > 3){
 			uint64_t message_obt[msg->payload64.size()];
 			/* Go throught the obtained payload*/
@@ -636,6 +630,10 @@ namespace rosbzz_node{
    		}
    		return true;
 	}
+	/*-----------------------------------------------------
+	/Obtain robot id by subscribing to xbee robot id topic 
+	/ TODO: check for integrity of this subscriber call back
+	/----------------------------------------------------*/
 	void roscontroller::set_robot_id(const std_msgs::UInt8::ConstPtr& msg){
 	robot_id=(int)msg->data;
 

src/testflockfev.bzz

@@ -78,13 +78,20 @@ function barrier_ready() {
 #
 # Executes the barrier
 #
+WAIT_TIMEOUT = 100
+timeW=0
 function barrier_wait(threshold, transf) {
   barrier.get(id)
   CURSTATE = "BARRIERWAIT"
   if(barrier.size() >= threshold) {
     barrier = nil
     transf()
-  }
+   } else if(timeW>=WAIT_TIMEOUT) {
+     barrier = nil
+     statef=hexagon #idle
+     timeW=0
+   }
+   timeW = timeW+1
 }
 
 # flight status
@@ -144,7 +151,6 @@ function init() {
 
 # Executed at each time step.
 function step() {
-
 	if(flight.rc_cmd==22) {
 		log("cmd 22")
 		flight.rc_cmd=0
@@ -166,6 +172,7 @@ function step() {
 		uav_arm()
 		neighbors.broadcast("cmd", 400)
 	} else if (flight.rc_cmd==401){
+		flight.rc_cmd=0
 		uav_disarm()
 		neighbors.broadcast("cmd", 401)
 	}