comments commander.cpp

src/commander.cpp

@@ -19,38 +19,34 @@
 class Run
 {
   private:
-    float xx, yy, zz, theta;
-    // xx < 0 : forward
-    // xx > 0 : backward
+    float xx, yy, zz, theta; // read coords
 
-    // yy > 0 : right
-    // yy < 0 : left
-
-    // zz > 0 : up
-    // zz < 0 : down
-
-    float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z;
-
-    float max_curv;
-    float z_dev_min, x_dev_min, y_dev_min, th_dev_min;
-    uint64_t min_number;
-
-    bool no_diag;
+    // see README.md to know what are the parameters
+    float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z;  // parameters
+    float max_curv; // not used yet
+    float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds
+    uint64_t min_number; // parameter
+    bool no_diag; // parameter
 
     ros::Publisher pub;
 
     void publish()
+    // build and publish a message from the "xx", "yy", "zz" and "theta" informations
     {
       geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
 
       if (fabs(zz) > z_dev_min)
       {
+        // up_factor to balance out the gravity effect
         if (zz > 0)
           mvt->linear.z = zz * z_vel * up_factor ;
         else
           mvt->linear.z = zz * z_vel;
       }
-      
+
+
+      // no_diag true : the drone can only translate on the "x" axis
+      // or the "y" axis but not on a linear combination of these axes.
       if (no_diag)
       {
         if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
@@ -61,7 +57,7 @@ class Run
         {
           mvt->linear.x = - xx * x_vel;
         }
-      } else
+      } else // no_diag false : the drone can translate on any possible direction
       {
         if (fabs(yy) > y_dev_min)
           mvt->linear.y = yy * y_vel;
@@ -73,21 +69,20 @@ class Run
         mvt->angular.z = theta * angle_vel;
       }
 
-      assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
       pub.publish(mvt);
       ROS_INFO("cmd published");
-    }//end publish
+    }
 
   public:
     Run(const ros::Publisher& cmd_publisher) :
-      pub(cmd_publisher)
-  {
-  }
+      pub(cmd_publisher) {}
 
     void callback(const hand_control::Plan::ConstPtr& msg)
+    // handle received messages
     {
       ROS_INFO("plan received");
       if (msg->curvature < max_curv && msg->number > min_number)
+      // we ever have msg->curvature == 0 in fact (not implemented yet)
       {
 
         if(msg->normal.z > 0)
@@ -113,7 +108,9 @@ class Run
       publish();
     };
 
-    void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level) {
+    void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level)
+    // updates the parameters (received with dynamic_reconfigure)
+    {
       max_curv = c.max_curvature;
       x_dev_min = c.x_minimal_deviation;
       y_dev_min = c.y_minimal_deviation;
@@ -130,6 +127,7 @@ class Run
     }  
 
     void run()
+    // runs the callbacks and publications process
     {
       ros::spin();
     }
@@ -140,16 +138,17 @@ int main(int argc, char** argv)
 {
   ros::init(argc, argv, "commander");
   ros::NodeHandle node("commander");
-
   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);
 
+  // setting up dynamic_reconfigure (for rqt_reconfigure)
   dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
   dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
   f = boost::bind(&Run::reconfigure, &run, _1, _2);
   server.setCallback(f);
 
+  // starts working
   run.run();
   return 0;
 }