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("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 3000, 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", 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("angle_vel", double_t, 0, "Angular velocity", 0.5, 0., 10.)
+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.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
 
@@ -62,9 +60,9 @@ class Run
         mvt->linear.x = - xx * plan_vel;
       }
 
-     if (fabs(theta) > th_dev_min) {
-       mvt->angular.z = theta * angle_vel;
-     }
+      if (fabs(theta) > th_dev_min) {
+        mvt->angular.z = theta * angle_vel;
+      }
 
       assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
       pub.publish(mvt);
@@ -74,9 +72,8 @@ class Run
   public:
     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)
         {
@@ -127,7 +125,7 @@ class Run
 
     void run()
     {
-        ros::spin();
+      ros::spin();
     }
 
 };

src/estimator.cpp

@@ -14,118 +14,96 @@ typedef Eigen::Matrix3f& Matrix;
 
 class Callback {
   public:
-    void
-    operator()(const PointCloud::ConstPtr& msg)
+    void operator()(const PointCloud::ConstPtr& msg)
     {
-      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
-      std::vector<int> indices;
-      for (int i = 0; i < msg->points.size(); ++i)
-        indices.push_back(i);
-
-
-
-      // 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));
-           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 = (analyser.getMean())(2);
-      
-      //h = altitude(msg);
-      th = 
-            2 * atan (eg(1,0)
-                       /(1 + eg(0,0)));
-
-      // publication
-      ROS_INFO("Plan published");
-      publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
+      if (msg->width > 3){
+
+        analyser.setInputCloud(msg);
+        Matrix eg = analyser.getEigenVectors();
+
+        float x, y, z, th, h, c;
+        x = y = z = th = h = c = 0.;
+
+        // 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
+        Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
+        // norm(v) == 1
+        v.normalize();
+        x = v(0); y=v(1); z=v(2);
+
+        // h is the altitude
+        h = (analyser.getMean())(2);
+
+        // 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_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");
+        publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
       }
     }
 
-    Callback(ros::Publisher& pub) :
-      publisher(pub) {}
+    Callback(ros::Publisher& pub):publisher(pub), _RAD2DEG(45.f/atan(1.)){};
 
   private:
     ros::Publisher publisher;
     pcl::PCA<Point> analyser;
-    //pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
-    
-    inline
-    const hand_control::Plan::ConstPtr
-    to_Plan(const float& x, const float& y,
-            const float& z, const float& h,
-            const float& th,
-            const float& c, const uint32_t& seq,
-            const uint64_t& msec64, const uint64_t& number)
-    {
-      hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
-      ros_msg->normal.x = x; 
-      ros_msg->normal.y = y; 
-      ros_msg->normal.z = z; 
-      ros_msg->altitude = h;
-      ros_msg->angle = th;
-      ros_msg->curvature = c; 
-      ros_msg->number = number;
-      // uint64_t msec64 is in ms (10-6)
-      uint64_t sec64 = msec64 / 1000000;
-      uint64_t nsec64 = (msec64 % 1000000) * 1000;
-      ros_msg->header.stamp.sec = (uint32_t) sec64;
-      ros_msg->header.stamp.nsec = (uint32_t) nsec64;
-      ros_msg->header.seq = seq;
-      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
-    }*/
+    const float _RAD2DEG;
+
+    inline const hand_control::Plan::ConstPtr
+      to_Plan(const float& x, const float& y,
+              const float& z, const float& h,
+              const float& th,
+              const float& c, const uint32_t& seq,
+              const uint64_t& msec64, const uint64_t& number)
+      {
+        hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
+        ros_msg->normal.x = x; 
+        ros_msg->normal.y = y; 
+        ros_msg->normal.z = z; 
+        ros_msg->altitude = h;
+        ros_msg->angle = th;
+        ros_msg->curvature = c; 
+        ros_msg->number = number;
+        // uint64_t msec64 is in ms (10-6)
+        uint64_t sec64 = msec64 / 1000000;
+        uint64_t nsec64 = (msec64 % 1000000) * 1000;
+        ros_msg->header.stamp.sec = (uint32_t) sec64;
+        ros_msg->header.stamp.nsec = (uint32_t) nsec64;
+        ros_msg->header.seq = seq;
+        ros_msg->header.frame_id = "0";
+        return ros_msg;
+      }
 };
 
-int
-main(int argc, char** argv)
+int main(int argc, char** argv)
 {
-  ros::init(argc, argv, "normal_estimator_pca");
-  ros::NodeHandle node("estimator");//`A vérifier ?
+  ros::init(argc, argv, "estimator");
+  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");