diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index e996d54..78e67cf 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -12,6 +12,17 @@ #include #include +/* + * 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_(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(0, 0), rot_cv.at(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()));