comments estimator.cpp

src/estimator.cpp

@@ -17,30 +17,36 @@ typedef Eigen::Matrix3f& Matrix;
 class Callback {
   public:
     void callback(const PointCloud::ConstPtr& msg)
+    // handles received messages and publish the plan estimation
     {
       ROS_INFO("PointCloud received");
 
       if (msg->width > 3){
+      // else, no plan can be estimated…
 
         analyser.setInputCloud(msg);
         Matrix eg = analyser.getEigenVectors();
 
+        // to build the "Plan" message to be published
         float x, y, z, th, h, c;
         x = y = z = th = h = c = 0.;
 
-        // we consider the whole PointCloud
+        // we want to 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(); // to have norm(v) == 1
-        v.normalize();
+
+        // x, y and z are the coords of the plan normal
         if (!reverse)
         {
           x = v(0); y = v(1);
         } else {
+          // if "x" and "y" axes are inverted
+          // (reverse is a parameter to set with dynamic_reconfigure)
           x = v(1); y = v(0);
         }
         z = v(2);
@@ -48,11 +54,15 @@ class Callback {
         // h is the altitude
         h = (analyser.getMean())(2);
   
-        // this formula is good only if :
+        // angle calculation
-        //  -pi/2 <= th <= pi/2
+  
-        // ie cos(th) == m_x >= 0
+        // m_x and m_y are the "x" and "y" coords
+        // of the first principal component
         float m_x, m_y;
+  
         if (reverse_angle) 
+        // parameter to set
+        // with dynamic_reconfigure                     
         {
           m_x = eg(0,0);
           m_y = eg(1,0);
@@ -61,13 +71,16 @@ class Callback {
           m_y = eg(0,0);
         }
 
+        // because we want "th" only between -90° and 90°
         if (m_x < 0.)
           m_y *= -1;
 
-        th = - asin(m_y / sqrt(pow(m_y,2)+ pow(m_x,2)));
+        th = - asin(m_y / sqrt(pow(m_y,2)+ pow(m_x,2))); // 0 <= th <= pi
-        // 0 <= th <= pi
+        th *= _RAD2DEG; // -90 <= th <= 90
-        th *= _RAD2DEG;
+
-        // -90 <= th <= 90
+        // TODO
+        // -> calculate "c" (the curvature)
+        // ( c == 0 for the moment)
 
         // publication
         ROS_INFO("Plan published");
@@ -77,8 +90,9 @@ class Callback {
 
     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) { 
+    // updates the parameters received from dynamic_reconfigure
+    { 
       reverse = c.reverse ;
       reverse_angle = c.reverse_angle;
     }
@@ -89,6 +103,8 @@ class Callback {
     const float _RAD2DEG;
     bool reverse, reverse_angle;
 
+    // return a "Plan" message build with
+    // the informations provided
     inline const hand_control::Plan::ConstPtr
       to_Plan(const float& x, const float& y,
               const float& z, const float& h,
@@ -104,7 +120,6 @@ class Callback {
         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;
@@ -119,16 +134,17 @@ int main(int argc, char** argv)
 {
   ros::init(argc, argv, "estimator");
   ros::NodeHandle node("estimator");
-
   ros::Publisher  publisher = node.advertise<hand_control::Plan>("output", 1);
   Callback callback(publisher);
   ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &callback);
 
+  // sets up dynamic_reconfigure
   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);
 
+  // begins working
   ROS_INFO("node started");
   ros::spin();
   ROS_INFO("exit");