local setpoint

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 <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
@@ -62,6 +63,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;

src/buzzuav_closures.cpp

@@ -97,13 +97,16 @@ int buzzuav_moveto(buzzvm_t 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
+   goto_pos[0]=dx;
+   goto_pos[1]=dy;
+   goto_pos[2]=0;
+   /*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;
+   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);
 }
 

src/roscontroller.cpp

@@ -168,6 +168,7 @@ namespace rosbzz_node{
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 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);
@@ -292,6 +293,16 @@ namespace rosbzz_node{
 		} 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);
 		}
     		/*obtain Pay load to be sent*/  
 		//fprintf(stdout, "before getting msg from utility\n");

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