with «angle» & «theta» between -90 and 90

cfg/Commander.cfg

@@ -6,11 +6,11 @@ gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane"
 gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.)
 gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.)
 gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.)
-gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 1.5, 0.)
+gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.)
-gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 0)
+gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 2000, 0)
 gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1)
-gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 180.)
+gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.)
-gen.add("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.)
+gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 0., 10.)
 gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.)
 gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.)
 exit(gen.generate(PACKAGE, "hand_control", "Commander"))

src/commander.cpp

@@ -20,8 +20,6 @@ class Run
 {
   private:
     float xx, yy, zz, theta;
-    float DEMI_PI;
-
     // xx < 0 : forward
     // xx > 0 : backward
 
@@ -75,7 +73,6 @@ class Run
     Run(const ros::Publisher& cmd_publisher) :
       pub(cmd_publisher)
   {
-        DEMI_PI = 2*atan(1.);
   }
 
     void callback(const hand_control::Plan::ConstPtr& msg)
@@ -97,7 +94,8 @@ class Run
 
         zz = msg->altitude - neutral_z;
 
-	theta = msg->angle - DEMI_PI;
+        theta = msg->angle;
+        // theta between -90 and 90
 
         if (first_msg)
         {

src/estimator.cpp

@@ -14,49 +14,47 @@ typedef Eigen::Matrix3f& Matrix;
 
 class Callback {
   public:
-    void
+    void operator()(const PointCloud::ConstPtr& msg)
-    operator()(const PointCloud::ConstPtr& msg)
     {
-      if (msg->width >3){
+      ROS_INFO("PointCloud received");
+
+      if (msg->width > 3){
+
         analyser.setInputCloud(msg);
         Matrix eg = analyser.getEigenVectors();
-      ROS_INFO("PointCloud received");
 
         float x, y, z, th, h, c;
         x = y = z = th = h = c = 0.;
 
-      // indices : tout le PointCloud
+        // we consider the whole PointCloud
         std::vector<int> indices;
         for (int i = 0; i < msg->points.size(); ++i)
           indices.push_back(i);
 
-
+        // v = eg_1 ^ eg_2 is the plan normal
-
-      // vérifier signature
-      //estimator.computePointNormal(*msg, indices, x, y, z, c);
-           //Produit vectoriel des deux première colonnes de Matrix3f
-          /* x = eg(2,1)*eg(3,2)
-                 - eg(3,1)*eg(2,2);
-           y = eg(3,1)*eg(1,2)
-                 - eg(1,1)*eg(3,2);
-           z = eg(1,1)*eg(2,2)
-                 - eg(2,1)*eg(1,2);*/
         Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
+        // norm(v) == 1
         v.normalize();
-           /*
-           x = x/sqrt(x*x+y*y+z*z);
-           y = x/sqrt(x*x+y*y+z*z);
-           z = x/sqrt(x*x+y*y+z*z);
-           */
         x = v(0); y=v(1); z=v(2);
 
-      
+        // h is the altitude
         h = (analyser.getMean())(2);
 
-      //h = altitude(msg);
+        // this formula is good only if :
-      th = 
+        //  -pi/2 <= th <= pi/2
-            2 * atan (eg(1,0)
+        // ie cos(th) == m_x >= 0
-                       /(1 + eg(0,0)));
+        float m_x(eg(0,0));
+        float m_y(eg(1,0));
+        if(x < 0) 
+        {
+            m_x *= -1.;
+            m_y *= -1.;
+        }
+        th = 2 * atan(m_y / (1 + m_x));
+        // -pi/2 <= th <= pi/2
+        
+        th *= _RAD2DEG;
+        // -90 <= th <= 90
 
         // publication
         ROS_INFO("Plan published");
@@ -64,16 +62,14 @@ class Callback {
       }
     }
 
-    Callback(ros::Publisher& pub) :
+    Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){};
-      publisher(pub) {}
 
   private:
     ros::Publisher publisher;
     pcl::PCA<Point> analyser;
-    //pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
+    const float _RAD2DEG;
 
-    inline
+    inline const hand_control::Plan::ConstPtr
-    const hand_control::Plan::ConstPtr
       to_Plan(const float& x, const float& y,
               const float& z, const float& h,
               const float& th,
@@ -97,35 +93,17 @@ class Callback {
         ros_msg->header.frame_id = "0";
         return ros_msg;
       }
-
-/*    inline
-    float
-    altitude(const PointCloud::ConstPtr& pcl)
-    {
-      int s = pcl->points.size();
-      float h(0);
-      for (int i = 0; i < s; ++i)
-        h += pcl->points[i].z;
-      return h/( (float) s );
-      
-      
-      getmean
-      getVector4fMap
-    }*/
 };
 
-int
+int main(int argc, char** argv)
-main(int argc, char** argv)
 {
-  ros::init(argc, argv, "normal_estimator_pca");
+  ros::init(argc, argv, "estimator");
-  ros::NodeHandle node("estimator");//`A vérifier ?
+  ros::NodeHandle node("estimator");
 
-  // initialisation
   ros::Publisher  publisher = node.advertise<hand_control::Plan>("output", 1);
   Callback callback(publisher);
   ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
 
-  // démarrage
   ROS_INFO("node started");
   ros::spin();
   ROS_INFO("exit");