document the aruco_detector

src/aruco_detector.cpp

@@ -12,6 +12,17 @@
 #include <aruco/aruco.h>
 #include <teleoperation/utils.hpp>
 
+/*
+ * This is the ROS Node for detecting the ArUco markers.
+ *
+ * Node: aruco_detector
+ *
+ * It subscribes to a webcam, detects the markers, transforms them
+ * into the correct coordinate frame (z-up) and publishes on TF
+ * relatively to the odom frame
+ *
+ */
+
 static const std::string ARUCO_WINDOW = "Aruco window";
 
 class ImageConverter {
@@ -41,6 +52,7 @@ class ImageConverter {
     }
 
     void imageCb(const sensor_msgs::ImageConstPtr& msg) {
+        // This function gets called when an image arrives
         cv_bridge::CvImageConstPtr cv_ptr;
         try
         {
@@ -54,7 +66,7 @@ class ImageConverter {
             return;
         }
 
-        // Do Aruco
+        // Define parameters of markers and camera
         cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
                 690., 0.0, 320,
                 0.0, 690., 225,
@@ -70,7 +82,7 @@ class ImageConverter {
         float m_size = 0.15;
         detector.detect(cv_ptr->image, markers, cam, m_size);
 
-        // Draw and print aruco
+        // Draw all aruco into the window and publish on tf
         cv::Mat aruco_demo = cv_ptr->image.clone();
         for (int i = 0; i < markers.size(); i++) {
 
@@ -79,6 +91,8 @@ class ImageConverter {
             cv::Mat rot_cv;
             cv::Rodrigues(markers[i].Rvec, rot_cv);
 
+            // Transform the coordinates of the aruco markers so that they
+            // are in the Z-up, X-forward frame
             tf::Matrix3x3 rot(
                     rot_cv.at<float>(0, 0),
                     rot_cv.at<float>(0, 1),
@@ -106,7 +120,7 @@ class ImageConverter {
                     0, -1, 0
                     );
 
-            rot = hmat * rot * another;  // This all somehow works as expected
+            rot = hmat * rot * another;  // This transforms the coords
             trans = hmat * trans;
 
             tf::Transform aruco_tf(rot, trans);
@@ -115,6 +129,7 @@ class ImageConverter {
             id += stuff_to_str(markers[i].id);
             id += "_frame";
 
+            // Publish the transform
             this->aruco_pub.sendTransform(tf::StampedTransform(
                         aruco_tf, ros::Time::now(), "odom",
                         id.c_str()));