avec ou sans diagonale

cfg/Commander.cfg

@@ -14,4 +14,5 @@ gen.add("angle_vel", double_t, 0, "Angular velocity", 0.01, 0., 10.)
 gen.add("x_vel", double_t, 0, "X Translation velocity", 0.5, 0., 10.)
 gen.add("y_vel", double_t, 0, "Y Translation velocity", 0.3, 0., 10.)
 gen.add("z_vel", double_t, 0, "Vertical translation velocity", 1.5, 0., 10.)
+gen.add("no_diag", bool_t, 0, "Drone cannot translate on a diagonal direction", True)
 exit(gen.generate(PACKAGE, "hand_control", "Commander"))

src/commander.cpp

@@ -35,7 +35,7 @@ class Run
     float z_dev_min, x_dev_min, y_dev_min, th_dev_min;
     uint64_t min_number;
 
-    bool first_msg;
+    bool no_diag;
 
     ros::Publisher pub;
 
@@ -50,14 +50,23 @@ class Run
         else
           mvt->linear.z = zz * z_vel;
       }
-
-      if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
+      
+      if (no_diag)
       {
-        mvt->linear.y = yy * y_vel;
-      }
-      else if (fabs(xx) > x_dev_min)
+        if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
+        {
+          mvt->linear.y = yy * y_vel;
+        }
+        else if (fabs(xx) > x_dev_min)
+        {
+          mvt->linear.x = - xx * x_vel;
+        }
+      } else
       {
-        mvt->linear.x = - xx * x_vel;
+        if (fabs(yy) > y_dev_min)
+          mvt->linear.y = yy * y_vel;
+        if (fabs(xx) > x_dev_min)
+          mvt->linear.x = - xx * x_vel;
       }
 
       if (fabs(theta) > th_dev_min) {
@@ -97,11 +106,6 @@ class Run
         theta = msg->angle;
         // theta between -90 and 90
 
-        if (first_msg)
-        {
-          first_msg = false;
-          ROS_INFO("first msg received");
-        }
         ROS_INFO("coords updated");
       } else {
         xx = yy = zz = 0.;
@@ -122,6 +126,7 @@ class Run
       y_vel = c.y_vel;
       z_vel = c.z_vel;
       angle_vel = c.angle_vel;
+      no_diag = c.no_diag;
     }  
 
     void run()