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
 
@@ -61,10 +59,10 @@ class Run
       {
         mvt->linear.x = - xx * plan_vel;
       }
-      
+
-     if (fabs(theta) > th_dev_min) {
+      if (fabs(theta) > th_dev_min) {
-       mvt->angular.z = theta * angle_vel;
+        mvt->angular.z = theta * angle_vel;
-     }
+      }
 
       assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
       pub.publish(mvt);
@@ -74,16 +72,15 @@ 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)
     {
       ROS_INFO("plan received");
       if (msg->curvature < max_curv && msg->number > min_number)
       {
-        
+
         if(msg->normal.z > 0)
         {
           yy = msg->normal.x;
@@ -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();
     }
 
 };
@@ -140,7 +138,7 @@ int main(int argc, char** argv)
   ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
   Run run(cmd_pub);
   ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
-  
+
   dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
   dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
   f = boost::bind(&Run::reconfigure, &run, _1, _2);

src/estimator.cpp

@@ -14,118 +14,96 @@ typedef Eigen::Matrix3f& Matrix;
 
 class Callback {
   public:
-    void
+    void operator()(const PointCloud::ConstPtr& msg)
-    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;
+      if (msg->width > 3){
-      x = y = z = th = h = c = 0.;
 
-      // indices : tout le PointCloud
+        analyser.setInputCloud(msg);
-      std::vector<int> indices;
+        Matrix eg = analyser.getEigenVectors();
-      for (int i = 0; i < msg->points.size(); ++i)
-        indices.push_back(i);
 
+        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érifier signature
+        // v = eg_1 ^ eg_2 is the plan normal
-      //estimator.computePointNormal(*msg, indices, x, y, z, c);
+        Eigen::Vector3f v = eg.col(0).cross(eg.col(1));
-           //Produit vectoriel des deux première colonnes de Matrix3f
+        // norm(v) == 1
-          /* x = eg(2,1)*eg(3,2)
+        v.normalize();
-                 - eg(3,1)*eg(2,2);
+        x = v(0); y=v(1); z=v(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
+        // h is the altitude
-      ROS_INFO("Plan published");
+        h = (analyser.getMean())(2);
-      publisher.publish(to_Plan(x, y, z, h, th, c, msg->header.seq, msg->header.stamp, msg->width));
+
+        // 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) :
+    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
-    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
+    inline const hand_control::Plan::ConstPtr
-    float
+      to_Plan(const float& x, const float& y,
-    altitude(const PointCloud::ConstPtr& pcl)
+              const float& z, const float& h,
-    {
+              const float& th,
-      int s = pcl->points.size();
+              const float& c, const uint32_t& seq,
-      float h(0);
+              const uint64_t& msec64, const uint64_t& number)
-      for (int i = 0; i < s; ++i)
+      {
-        h += pcl->points[i].z;
+        hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
-      return h/( (float) s );
+        ros_msg->normal.x = x; 
-      
+        ros_msg->normal.y = y; 
-      
+        ros_msg->normal.z = z; 
-      getmean
+        ros_msg->altitude = h;
-      getVector4fMap
+        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
+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");