it works :-) yaml with naked hand

cfg/Estimator.cfg

@@ -3,4 +3,5 @@ PACKAGE = "hand_control"
 from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 gen.add("reverse", bool_t, 0, "Pose the kinect in parallel with the arm")
+gen.add("reverse_angle", bool_t, 0, "Change the angle sign")
 exit(gen.generate(PACKAGE, "hand_control", "Estimator"))

cfg/Filter.cfg

@@ -2,8 +2,8 @@
 PACKAGE = "hand_control"
 from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
-gen.add("z_max", double_t, 0, "The maximal altitude of the hand", 50., 0.)
+gen.add("z_max", double_t, 0, "The maximal altitude of the hand", 1.9, 0., 50.)
-gen.add("hue", double_t, 0, "The color hue of the hand", 0., 0., 360.)
+gen.add("hue", double_t, 0, "The color hue of the hand", 140., 0., 360.)
 gen.add("delta_hue", double_t, 0, "The tolerance of the hue filter", 20., 0., 180.)
 gen.add("sat_min", double_t, 0, "The minimum color saturation of the hand", 0.1, 0., 1.)
 gen.add("sat_max", double_t, 0, "The maximum color saturation of the hand", 1., 0., 1.)

commander.yaml

@@ -0,0 +1,37 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  angle_vel: 0.01
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      angle_vel: 0.01
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      max_curvature: 0.5
+      min_points_number: 1000
+      name: Default
+      neutral_alt: 0.8
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      plan_vel: 0.5
+      state: true
+      theta_minimal_deviation: 10.0
+      type: ''
+      up_fact: 1.5
+      x_minimal_deviation: 0.2
+      y_minimal_deviation: 0.2
+      z_minimal_deviation: 0.05
+      z_vel: 2.0
+    state: []
+  max_curvature: 0.5
+  min_points_number: 1000
+  neutral_alt: 0.8
+  plan_vel: 0.5
+  theta_minimal_deviation: 10.0
+  up_fact: 1.5
+  x_minimal_deviation: 0.2
+  y_minimal_deviation: 0.2
+  z_minimal_deviation: 0.05
+  z_vel: 2.0
+state: []

estimator.yaml

@@ -0,0 +1,19 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      reverse: false
+      reverse_angle: true
+      state: true
+      type: ''
+    state: []
+  reverse: false
+  reverse_angle: true
+state: []

filter.yaml

@@ -0,0 +1,29 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  delta_hue: 10.0
+  groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+    dictitems:
+      delta_hue: 10.0
+      groups: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      hue: 0.0
+      id: 0
+      name: Default
+      parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
+        state: []
+      parent: 0
+      sat_max: 1.0
+      sat_min: 0.13
+      state: true
+      type: ''
+      val_max: 1.0
+      val_min: 0.31
+      z_max: 2.0
+    state: []
+  hue: 0.0
+  sat_max: 1.0
+  sat_min: 0.13
+  val_max: 1.0
+  val_min: 0.31
+  z_max: 2.0
+state: []

src/estimator.cpp

@@ -16,7 +16,7 @@ typedef Eigen::Matrix3f& Matrix;
 
 class Callback {
   public:
-    void operator()(const PointCloud::ConstPtr& msg)
+    void callback(const PointCloud::ConstPtr& msg)
     {
       ROS_INFO("PointCloud received");
 
@@ -37,7 +37,13 @@ class Callback {
         Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
         // norm(v) == 1
         v.normalize();
-        x = v(0); y=v(1); z=v(2);
+        if (!reverse)
+        {
+          x = v(0); y=v(1);
+        } else {
+          x = v(1); y = v(0);
+        }
+        z=v(2);
 
         // h is the altitude
         h = (analyser.getMean())(2);
@@ -46,23 +52,20 @@ class Callback {
         //  -pi/2 <= th <= pi/2
         // ie cos(th) == m_x >= 0
         float m_x, m_y;
-        if (!reverse)
+        if (!reverse_angle)
         {
-           m_x = eg(0,0);
+          m_x = eg(0,0);
-           m_y = eg(1,0);
+          m_y = eg(1,0);
         } else {
-           m_x = eg(1,0);
+          m_x = eg(1,0);
-           m_y = eg(0,0);
+          m_y = eg(0,0);
         }
 
-        if(m_x < 0) 
+        if (m_x < 0.)
-        {
+          m_y *= -1;
-            m_x *= -1.;
+
-            m_y *= -1.;
+        th = - asin(m_y / sqrt(pow(m_y,2)+ pow(m_x,2)));
-        }
+        // 0 <= th <= pi
-        th = 2 * atan(m_y / (1 + m_x));
-        // -pi/2 <= th <= pi/2
-        
         th *= _RAD2DEG;
         // -90 <= th <= 90
 
@@ -72,18 +75,19 @@ class Callback {
       }
     }
 
-    Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false) {};
+    Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false), reverse_angle(false) {};
 
 
-  void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level) { 
+    void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level) { 
-    reverse = c.reverse ;
+      reverse = c.reverse ;
-  }
+      reverse_angle = c.reverse_angle;
+    }
 
   private:
     ros::Publisher publisher;
     pcl::PCA<Point> analyser;
     const float _RAD2DEG;
-    bool reverse;
+    bool reverse, reverse_angle;
 
     inline const hand_control::Plan::ConstPtr
       to_Plan(const float& x, const float& y,
@@ -118,7 +122,7 @@ int main(int argc, char** argv)
 
   ros::Publisher  publisher = node.advertise<hand_control::Plan>("output", 1);
   Callback callback(publisher);
-  ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
+  ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback);
 
   dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
   dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;