corrects error (s/x/m_x) and add a parameter to inverse x and y axes

CMakeLists.txt

@@ -61,10 +61,12 @@ add_dependencies(commander hand_control_generate_messages_cpp)
 generate_dynamic_reconfigure_options(
   cfg/Filter.cfg
   cfg/Commander.cfg
+  cfg/Estimator.cfg
   )
 
 add_dependencies(filter ${PROJECT_NAME}_gencfg)
 add_dependencies(commander ${PROJECT_NAME}_gencfg)
+add_dependencies(estimator ${PROJECT_NAME}_gencfg)
 
   catkin_package(
                  CATKIN_DEPENDS message_runtime

cfg/Estimator.cfg

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+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")
+exit(gen.generate(PACKAGE, "hand_control", "Estimator"))

src/estimator.cpp

@@ -5,6 +5,8 @@
 #include <hand_control/Plan.h>
 #include <time.h>
 #include <math.h>
+#include <dynamic_reconfigure/server.h>
+#include <hand_control/EstimatorConfig.h>
 
 #include <pcl/common/pca.h>
 
@@ -43,9 +45,17 @@ class Callback {
         // this formula is good only if :
         //  -pi/2 <= th <= pi/2
         // ie cos(th) == m_x >= 0
-        float m_x(eg(0,0));
+        float m_x, m_y;
-        float m_y(eg(1,0));
+        if (!reverse)
-        if(x < 0) 
+        {
+           m_x = eg(0,0);
+           m_y = eg(1,0);
+        } else {
+           m_x = eg(1,0);
+           m_y = eg(0,0);
+        }
+
+        if(m_x < 0) 
         {
             m_x *= -1.;
             m_y *= -1.;
@@ -62,12 +72,18 @@ class Callback {
       }
     }
 
-    Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){};
+    Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)), reverse(false) {};
+
+
+  void reconfigure(const hand_control::EstimatorConfig& c, const uint32_t& level) { 
+    reverse = c.reverse ;
+  }
 
   private:
     ros::Publisher publisher;
     pcl::PCA<Point> analyser;
     const float _RAD2DEG;
+    bool reverse;
 
     inline const hand_control::Plan::ConstPtr
       to_Plan(const float& x, const float& y,
@@ -104,6 +120,11 @@ int main(int argc, char** argv)
   Callback callback(publisher);
   ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
 
+  dynamic_reconfigure::Server<hand_control::EstimatorConfig> server;
+  dynamic_reconfigure::Server<hand_control::EstimatorConfig>::CallbackType f;
+  f = boost::bind(&Callback::reconfigure, &callback, _1, _2);
+  server.setCallback(f);
+
   ROS_INFO("node started");
   ros::spin();
   ROS_INFO("exit");