bringup debug mode to launch file, fine tuned LJ params and rename hooks (generic hardware)

buzz_scripts/include/act/states.bzz

@@ -45,8 +45,13 @@ function launch() {
 	}
 }
 
+gohomeT=100
 function stop() {
   BVMSTATE = "STOP"
+	if(gohomeT > 0) {	# TODO: Make a real check if home is reached
+		gohome()
+		gohomeT = gohomeT - 1
+	}
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		neighbors.broadcast("cmd", 21)
 		uav_land()
@@ -64,7 +69,7 @@ function take_picture() {
   BVMSTATE="PICTURE"
   uav_setgimbal(0.0, 0.0, -90.0, 20.0)
 	if(pic_time==PICTURE_WAIT/2) { # wait for the drone to stabilize
-	  uav_takepicture()
+	  takepicture()
   } else if(pic_time>=PICTURE_WAIT) { # wait for the picture
     BVMSTATE="IDLE"
     pic_time=0 
@@ -137,9 +142,10 @@ function pursuit() {
 
 # Lennard-Jones interaction magnitude
 TARGET     = 8.0
-EPSILON    = 0.1
+EPSILON    = 30.0
 function lj_magnitude(dist, target, epsilon) {
-	lj = -(epsilon / dist) * ((target / dist)^4 - 1.02 * (target / dist)^2)
+	lj = -(epsilon / dist) * ((target / dist)^4 - (target / dist)^2)
+	log(target, dist, epsilon, lj)
   return lj
 }
  
@@ -154,17 +160,17 @@ function lj_sum(rid, data, accum) {
 }
  
 # Calculates and actuates the flocking interaction
-old_lj = math.vec2.newp(0.0, 0.0)	# 1-step filter, to smoothen dir. changes
 function formation() {
   BVMSTATE="FORMATION"
   # Calculate accumulator
   accum_lj = neighbors.map(lj_vector).reduce(lj_sum, math.vec2.new(0.0, 0.0))
   if(neighbors.count() > 0)
     accum_lj = math.vec2.scale(accum_lj, 1.0 / neighbors.count())
-	old_lj = LimitSpeed(math.vec2.add(old_lj, accum_lj), 1.0/2.0)
+	accum_lj = LimitSpeed(accum_lj, 1.0/3.0)
-  goto_abs(old_lj.x, old_lj.y, 0.0, 0.0)
+  goto_abs(accum_lj.x, accum_lj.y, 0.0, 0.0)
 }
 
+# listens to commands from the remote control (web, commandline, etc)
 function rc_cmd_listen() {
   if(flight.rc_cmd==22) {
 		log("cmd 22")
@@ -216,6 +222,7 @@ function rc_cmd_listen() {
 	}
 }
 
+# listens to other fleet members broadcasting commands
 function nei_cmd_listen() {
     neighbors.listen("cmd",
 			function(vid, value, rid) {

buzz_scripts/main.bzz

@@ -7,7 +7,7 @@ include "taskallocate/graphformGPS.bzz"
 include "vstigenv.bzz"
 
 #State launched after takeoff
-AUTO_LAUNCH_STATE = "IDLE"
+AUTO_LAUNCH_STATE = "FORMATION"
 
 #####
 # Vehicule type:

include/roscontroller.h

@@ -103,6 +103,7 @@ private:
   bool multi_msg;
   uint8_t no_cnt = 0;
   uint8_t old_val = 0;
+  bool debug = false;
   std::string bzzfile_name;
   std::string fcclient_name;
   std::string armclient;

launch/rosbuzz.launch

@@ -10,6 +10,7 @@
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >
     <rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
     <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
+    <param name="debug" value="true" />
     <param name="rcclient" value="true" />
     <param name="rcservice_name" value="buzzcmd" />
     <param name="in_payload" value="inMavlink"/>

launch/rosbuzzd.launch

@@ -10,6 +10,7 @@
   <node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" launch-prefix="gdb -ex run --args">
     <rosparam file="$(find rosbuzz)/launch/launch_config/topics.yaml"/>
     <param name="bzzfile_name" value="$(find rosbuzz)/buzz_scripts/$(arg script).bzz" />
+    <param name="debug" value="true" />
     <param name="rcclient" value="true" />
     <param name="rcservice_name" value="buzzcmd" />
     <param name="in_payload" value="inMavlink"/>

src/buzz_utility.cpp

@@ -204,25 +204,25 @@ static int buzz_register_hooks()
   buzzvm_pushs(VM, buzzvm_string_register(VM, "goto_abs", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "storegoal", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_setgimbal", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "setgimbal", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_setgimbal));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takepicture", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "takepicture", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takepicture));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "arm", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "disarm", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
   buzzvm_gstore(VM);
-  buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
+  buzzvm_pushs(VM, buzzvm_string_register(VM, "gohome", 1));
   buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
   buzzvm_gstore(VM);
   buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));

src/buzzuav_closures.cpp

@@ -104,10 +104,10 @@ float constrainAngle(float x)
 / Wrap the angle between -pi, pi
 ----------------------------------------------------------- */
 {
-  x = fmod(x, 2 * M_PI);
+  x = fmod(x + M_PI, 2 * M_PI);
   if (x < 0.0)
     x += 2 * M_PI;
-  return x;
+  return x - M_PI;
 }
 
 void rb_from_gps(double nei[], double out[], double cur[])

src/roscontroller.cpp

@@ -11,7 +11,6 @@
 namespace rosbzz_node
 {
 const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
-const bool debug = false;
 
 roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
 /*
@@ -188,6 +187,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
     ROS_ERROR("Provide a .bzz file to run in Launch file");
     system("rosnode kill rosbuzz_node");
   }
+  // Obtain debug mode from launch file parameter
+  if (n_c.getParam("debug", debug))
+    ;
+  else
+  {
+    ROS_ERROR("Provide a debug mode in Launch file");
+    system("rosnode kill rosbuzz_node");
+  }
   // Obtain rc service option from parameter server
   if (n_c.getParam("rcclient", rcclient))
   {
@@ -670,7 +677,7 @@ script
         ROS_ERROR("Failed to call service from flight controller");
       break;
 
-    case buzzuav_closures::COMMAND_GOTO:  // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
+    case buzzuav_closures::COMMAND_GOTO:  // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
       goto_pos = buzzuav_closures::getgoto();
       cmd_srv.request.param5 = goto_pos[0];
       cmd_srv.request.param6 = goto_pos[1];
@@ -794,10 +801,10 @@ float roscontroller::constrainAngle(float x)
 / Wrap the angle between -pi, pi
 ----------------------------------------------------------- */
 {
-  x = fmod(x, 2 * M_PI);
+  x = fmod(x + M_PI, 2 * M_PI);
   if (x < 0.0)
     x += 2 * M_PI;
-  return x;
+  return x - M_PI;
 }
 
 void roscontroller::gps_rb(POSE nei_pos, double out[])
@@ -809,10 +816,7 @@ void roscontroller::gps_rb(POSE nei_pos, double out[])
   float ned_x = 0.0, ned_y = 0.0;
   gps_ned_cur(ned_x, ned_y, nei_pos);
   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] = constrainAngle(atan2(ned_y, ned_x));
-  // out[1] = std::floor(out[1] * 1000000) / 1000000;
   out[2] = 0.0;
 }
 
@@ -1055,6 +1059,7 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
     nei_pos.longitude = neighbours_pos_payload[1];
     nei_pos.altitude = neighbours_pos_payload[2];
     double cvt_neighbours_pos_payload[3];
+    // Compute RB in robot body ref. frame
     gps_rb(nei_pos, cvt_neighbours_pos_payload);
     //  Extract robot id of the neighbour
     uint16_t* out = buzz_utility::u64_cvt_u16((uint64_t) * (message_obt + 3));