added yaw control and states pursuit, agreggation and formation (still to tweak)

buzz_scripts/include/act/states.bzz

@@ -79,11 +79,11 @@ function goto_gps(transf) {
 		log("Sorry this is too far.")
   else if(math.vec2.length(m_navigation)>GOTO_MAXVEL) { # limit velocity
 		m_navigation = math.vec2.scale(m_navigation, GOTO_MAXVEL/math.vec2.length(m_navigation))
-		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
+		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
 	} else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
 		transf()
 	else
-		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude)
+		goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
 }
 
 function follow() {
@@ -94,13 +94,80 @@ function follow() {
 			force=(0.05)*(tab.range)^4
 			attractor=math.vec2.add(attractor, math.vec2.newp(force, tab.bearing))
 		})
-		goto_abs(attractor.x, attractor.y, 0.0)
+		goto_abs(attractor.x, attractor.y, 0.0, 0.0)
 	} else {
 		log("No target in local table!")
 		BVMSTATE = "IDLE"
 	}
 }
 
+# converge to centroid
+function aggregate() {
+  BVMSTATE="AGGREGATE"
+  centroid = neighbors.reduce(function(rid, data, centroid) {
+	  centroid = math.vec2.add(centroid, math.vec2.newp(data.distance, data.azimuth))
+	  return centroid
+  }, {.x=0, .y=0})
+  if(neighbors.count() > 0)
+    math.vec2.scale(centroid, 1.0 / neighbors.count())
+  if(math.vec2.length(centroid)>GOTO_MAXVEL)
+  	centroid = math.vec2.scale(centroid, GOTO_MAXVEL/math.vec2.length(centroid))
+  goto_abs(centroid.x, centroid.y, 0.0, 0.0) 
+}
+
+# follow one another
+rotang = 0.0
+function pursuit() {
+  BVMSTATE="PURSUIT"
+  insight = 0
+  leader = math.vec2.newp(0.0, 0.0)
+  var cmdbin = math.vec2.newp(0.0, 0.0)
+  neighbors.foreach(function(rid, data) {
+  	if(data.distance < 11.0 and data.azimuth < 3.2 and data.azimuth > 2.8) {
+		  insight = 1
+		  leader = math.vec2.newp(data.distance, data.azimuth)
+	  }
+  })
+  if(insight == 1) {
+	log("Leader in sight !")
+  	#cmdbin = math.vec2.newp(lj_magnitude(math.vec2.length(leader), 3.0, 0.01), math.vec2.angle(leader))
+	cmdbin = math.vec2.newp(2.0, math.vec2.angle(leader))
+  } else {
+	rotang = rotang + math.pi/60
+	cmdbin = math.vec2.newp(2.0, rotang)
+  }
+  goto_abs(cmdbin.x, cmdbin.y, 0.0, rotang)
+}
+
+# Lennard-Jones interaction magnitude
+TARGET     = 8.0
+EPSILON    = 0.000001
+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 formation() {
+  BVMSTATE="FORMATION"
+  # 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())
+  if(math.vec2.length(accum)>GOTO_MAXVEL*15)
+  	accum = math.vec2.scale(accum, 15*GOTO_MAXVEL/math.vec2.length(accum))
+  goto_abs(accum.x, accum.y, 0.0, 0.0)
+}
+
 function rc_cmd_listen() {
     if(flight.rc_cmd==22) {
 		log("cmd 22")

buzz_scripts/include/plan/rrtstar.bzz

@@ -53,7 +53,7 @@ function navigate() {
 			populateGrid(m_pos)
 			if(check_path(Path, path_it, m_pos, 0)) {
         # stop
-				goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude)
+				goto_abs(0.0, 0.0, rc_goto.altitude - pose.position.altitude, 0.0)
 				Path = nil
 				if(V_TYPE == 0)
 					cur_goal = vec_from_gps(rc_goto.latitude, rc_goto.longitude,0)
@@ -62,7 +62,7 @@ function navigate() {
 				pathPlanner(cur_goal, m_pos)
 			} else {
 				cur_path_pt = math.vec2.scale(cur_path_pt, GOTO_MAXVEL/math.vec2.length(cur_path_pt))
-				goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude)
+				goto_abs(cur_path_pt.x, cur_path_pt.y, rc_goto.altitude - pose.position.altitude, 0.0)
 			}
 		} else
 			path_it = path_it + 1

buzz_scripts/include/taskallocate/graphformGPS.bzz

@@ -694,7 +694,7 @@ function DoLock(){
       m_navigation=motion_vector()
   }
   #move
-  goto_abs(m_navigation.x, m_navigation.y, 0.0)
+  goto_abs(m_navigation.x, m_navigation.y, 0.0, 0.0)
   BroadcastGraph()
 }
 #

buzz_scripts/main.bzz

@@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 
 #State launched after takeoff
-AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
+AUTO_LAUNCH_STATE = "PURSUIT"
 
 #####
 # Vehicule type:
@@ -26,7 +26,7 @@ function init() {
   	init_stig()
 	init_swarm()
 
-	TARGET_ALTITUDE = 25.0 # m
+	TARGET_ALTITUDE = 10 + id*2.0 # m
 
 	# start the swarm command listener
 	nei_cmd_listen()
@@ -54,6 +54,12 @@ function step() {
 		statef=launch
 	else if(BVMSTATE=="IDLE")
 		statef=idle
+	else if(BVMSTATE=="AGGREGATE")
+		statef=aggregate
+	else if(BVMSTATE=="FORMATION")
+		statef=formation
+	else if(BVMSTATE=="PURSUIT")
+		statef=pursuit
 	else if(BVMSTATE=="TASK_ALLOCATE") {	# or bidding ?
 		startGraph()
 	} else if(BVMSTATE=="GRAPH_FREE") {

src/buzzuav_closures.cpp

@@ -13,7 +13,7 @@ namespace buzzuav_closures
 {
 // TODO: Minimize the required global variables and put them in the header
 // static const rosbzz_node::roscontroller* roscontroller_ptr;
-static double goto_pos[3];
+static double goto_pos[4];
 static double rc_goto_pos[3];
 static float rc_gimbal[4];
 static float batt[3];
@@ -207,22 +207,26 @@ int buzz_exportmap(buzzvm_t vm)
 
 int buzzuav_moveto(buzzvm_t vm)
 /*
-/ Buzz closure to move following a 2D vector
+/ Buzz closure to move following a 3D vector + Yaw
 /----------------------------------------*/
 {
-  buzzvm_lnum_assert(vm, 3);
+  buzzvm_lnum_assert(vm, 4);
   buzzvm_lload(vm, 1);  // dx
   buzzvm_lload(vm, 2);  // dy
-  buzzvm_lload(vm, 3);  //* dheight
+  buzzvm_lload(vm, 3);  // dheight
+  buzzvm_lload(vm, 4);  // dyaw
+  buzzvm_type_assert(vm, 4, BUZZTYPE_FLOAT);
   buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
   buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
   buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-  float dh = buzzvm_stack_at(vm, 1)->f.value;
+  float dyaw = buzzvm_stack_at(vm, 1)->f.value;
-  float dy = buzzvm_stack_at(vm, 2)->f.value;
+  float dh = buzzvm_stack_at(vm, 2)->f.value;
-  float dx = buzzvm_stack_at(vm, 3)->f.value;
+  float dy = buzzvm_stack_at(vm, 3)->f.value;
+  float dx = buzzvm_stack_at(vm, 4)->f.value;
   goto_pos[0] = dx;
   goto_pos[1] = dy;
   goto_pos[2] = height + dh;
+  goto_pos[3] = dyaw;
   //  DEBUG
   // 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]);

src/roscontroller.cpp

@@ -711,7 +711,7 @@ script
 
     case buzzuav_closures::COMMAND_MOVETO:
       goto_pos = buzzuav_closures::getgoto();
-      roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
+      roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
     case buzzuav_closures::COMMAND_GIMBAL:
@@ -935,15 +935,17 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
   moveMsg.header.frame_id = 1;
 
   //  DEBUG
-  // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f", cur_pos.x, cur_pos.y, x, y);
+  // ROS_INFO("Lp: %.3f, %.3f - Del: %.3f, %.3f, %.3f", cur_pos.x, cur_pos.y, x, y, yaw);
   moveMsg.pose.position.x = cur_pos.x + y;
   moveMsg.pose.position.y = cur_pos.y + x;
   moveMsg.pose.position.z = z;
 
-  moveMsg.pose.orientation.x = 0;
+  tf::Quaternion q;
-  moveMsg.pose.orientation.y = 0;
+  q.setRPY(0.0, 0.0, yaw);
-  moveMsg.pose.orientation.z = 0;
+  moveMsg.pose.orientation.x = q[0];
-  moveMsg.pose.orientation.w = 1;
+  moveMsg.pose.orientation.y = q[1];
+  moveMsg.pose.orientation.z = q[2];
+  moveMsg.pose.orientation.w = q[3];
 
   // To prevent drifting from stable position, uncomment
   // if(fabs(x)>0.005 || fabs(y)>0.005) {