fix waypoint bug

src/roscontroller.cpp

@@ -155,22 +155,24 @@ namespace rosbzz_node{
 		
  		/* flight controller client call if requested from Buzz*/
 		/*FC call for takeoff,land and gohome*/
-		if (buzzuav_closures::bzz_cmd()==1){
+		int tmp = buzzuav_closures::bzz_cmd();
+		if (tmp == 1){
 			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"); 
-		}
-		/*FC call for goto*/	
-		else if (buzzuav_closures::bzz_cmd() == 2) {  
+		} else if (tmp == 2) { /*FC call for goto*/ 
 			/*prepare the goto publish message */
     			double* goto_pos = buzzuav_closures::getgoto();
-			cmd_srv.request.param1 = goto_pos[0];
-    			cmd_srv.request.param2 = goto_pos[1];
-    			cmd_srv.request.param3 = goto_pos[2];
+			cmd_srv.request.x = goto_pos[0]*pow(10,7);
+    			cmd_srv.request.y = goto_pos[1]*pow(10,7);
+    			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"); 
-		} 		
+    			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"); 
+		}
     		/*obtain Pay load to be sent*/  
    		uint64_t* payload_out_ptr= buzz_utility::out_msg_process();
     		uint64_t  position[3];

src/test1.bzz

@@ -1,4 +1,37 @@
-
+# We need this for 2D vectors
+# Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
+ 
+# Lennard-Jones parameters
+TARGET     = 28.3
+EPSILON    = 15.0
+ 
+# Lennard-Jones interaction magnitude
+function lj_magnitude(dist, target, epsilon) {
+  return -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+}
+ 
+# Neighbor data to LJ interaction vector
+function lj_vector(rid, data) {
+  return math.vec2.newp(lj_magnitude(data.distance, TARGET, EPSILON), data.azimuth)
+}
+ 
+# Accumulator of neighbor LJ interactions
+function lj_sum(rid, data, accum) {
+  return math.vec2.add(data, accum)
+}
+ 
+# Calculates and actuates the flocking interaction
+function hexagon() {
+  statef=hexagon
+  # Calculate accumulator
+  var accum = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
+  if(neighbors.count() > 0)
+    math.vec2.scale(accum, 1.0 / neighbors.count())
+  # Move according to vector
+  uav_goto(accum.x+position.latitude, accum.y+position.longitude, position.altitude)
+}
+ 
 ########################################
 #
 # BARRIER-RELATED FUNCTIONS
@@ -9,7 +42,7 @@
 # Constants
 #
 BARRIER_VSTIG = 1
-ROBOTS = 2 # number of robots in the swarm
+ROBOTS = 3 # number of robots in the swarm
 
 #
 # Sets a barrier
@@ -57,7 +90,7 @@ neighbors.listen("cmd",
 
 function takeoff() {
 	if( flight.status == 2) {
-		barrier_set(ROBOTS,idle)
+		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 	}
 	else if(flight.status!=3)

src/vec2.bzz

@@ -0,0 +1,107 @@
+#
+# Create a new namespace for vector2 functions
+#
+math.vec2 = {}
+
+#
+# Creates a new vector2.
+# PARAM x: The x coordinate.
+# PARAM y: The y coordinate.
+# RETURN: A new vector2.
+#
+math.vec2.new = function(x, y) {
+  return { .x = x, .y = y }
+}
+
+#
+# Creates a new vector2 from polar coordinates.
+# PARAM l: The length of the vector2.
+# PARAM a: The angle of the vector2.
+# RETURN: A new vector2.
+#
+math.vec2.newp = function(l, a) {
+  return {
+    .x = l * math.cos(a),
+    .y = l * math.sin(a)
+  }
+}
+
+#
+# Calculates the length of the given vector2.
+# PARAM v: The vector2.
+# RETURN: The length of the vector.
+#
+math.vec2.length = function(v) {
+  return math.sqrt(v.x * v.x + v.y * v.y)
+}
+
+#
+# Calculates the angle of the given vector2.
+# PARAM v: The vector2.
+# RETURN: The angle of the vector.
+#
+math.vec2.angle = function(v) {
+  return math.atan2(v.y, v.x)
+}
+
+#
+# Returns the normalized form of a vector2.
+# PARAM v: The vector2.
+# RETURN: The normalized form.
+#
+math.vec2.norm = function(v) {
+  var l = math.length(v)
+  return {
+    .x = v.x / l,
+    .y = v.y / l
+  }
+}
+
+#
+# Calculates v1 + v2.
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 + v2
+#
+math.vec2.add = function(v1, v2) {
+  return {
+    .x = v1.x + v2.x,
+    .y = v1.y + v2.y
+  }
+}
+
+#
+# Calculates v1 - v2.
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 + v2
+#
+math.vec2.sub = function(v1, v2) {
+  return {
+    .x = v1.x - v2.x,
+    .y = v1.y - v2.y
+  }
+}
+
+#
+# Scales a vector by a numeric constant.
+# PARAM v: A vector2.
+# PARAM s: A number (float or int).
+# RETURN: s * v
+#
+math.vec2.scale = function(v, s) {
+  return {
+    .x = v.x * s,
+    .y = v.y * s
+  }
+}
+
+#
+# Calculates v1 . v2 (the dot product)
+# PARAM v1: A vector2.
+# PARAM v2: A vector2.
+# RETURN: v1 . v2
+#
+math.vec2.dot = function(v1, v2) {
+  return v1.x * v2.x + v1.y * v2.y
+}