feat: add landmark topic publishing and configurable detection outputs

Author: kluge7

aruco-detector/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rclcpp_components REQUIRED)
 find_package(OpenCV 4.5.4 REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(Eigen3 REQUIRED)
@@ -50,6 +51,7 @@ ament_target_dependencies(${LIB_NAME} PUBLIC
   rclcpp_components
   sensor_msgs
   geometry_msgs
+  vortex_msgs
   tf2
   cv_bridge
   std_srvs

aruco-detector/config/aruco_detector_params.yaml

@@ -3,19 +3,22 @@
   ros__parameters:
 
     subs:
-      image_topic: "/image"
-      camera_info_topic: "/camera_info"
+      image_topic: "/cam/image_color"
+      camera_info_topic: "/cam/camera_info"
 
     pubs:
       aruco_image: "/aruco_detector/image"
       aruco_poses: "/aruco_detector/markers"
       board_pose: "/aruco_detector/board"
+      landmarks: "/aruco_detector/landmarks"
 
     logger_service_name: "/toggle_marker_logger"
 
     detect_board: true  # Set to true to detect aruco boards
     visualize: true  # Set to true to draw the detected markers and board on the image
     log_markers: false  # Set to true to log the detected markers
+    publish_detections: false
+    publish_landmarks: true
 
       # TAC ARUCO PARAMETERS
     aruco:

aruco-detector/include/aruco_detector/aruco_detector_ros.hpp

@@ -18,6 +18,9 @@
 #include <map>
 #include <string>
 
+#include <vortex_msgs/msg/landmark.hpp>
+#include <vortex_msgs/msg/landmark_array.hpp>
+
 #include <cv_bridge/cv_bridge.h>
 
 #include "aruco_detector.hpp"
@@ -94,6 +97,10 @@ class ArucoDetectorNode : public rclcpp::Node {
      */
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr
         board_pose_pub_;
+    /**
+     * @brief Publishes detected landmarks as a LandmarkArray
+     */
+    rclcpp::Publisher<vortex_msgs::msg::LandmarkArray>::SharedPtr landmark_pub_;
 
     /**
      * @brief Initialize the detector. Sets dictionary from ros param. Also
@@ -151,6 +158,8 @@ class ArucoDetectorNode : public rclcpp::Node {
     bool detect_board_;
     bool visualize_;
     bool log_markers_;
+    bool publish_detections_;
+    bool publish_landmarks_;
 
     float marker_size_;
     float xDist_, yDist_;

aruco-detector/package.xml

@@ -17,6 +17,7 @@
   <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>eigen</depend>
+  <depend>vortex_msgs</depend>
 
 
   <export>

aruco-detector/src/aruco_detector_ros.cpp

@@ -12,6 +12,10 @@ ArucoDetectorNode::ArucoDetectorNode(const rclcpp::NodeOptions& options)
     detect_board_ = this->declare_parameter<bool>("detect_board");
     visualize_ = this->declare_parameter<bool>("visualize");
     log_markers_ = this->declare_parameter<bool>("log_markers");
+    publish_detections_ =
+        this->declare_parameter<bool>("publish_detections", true);
+    publish_landmarks_ =
+        this->declare_parameter<bool>("publish_landmarks", true);
 
     this->declare_parameter<float>("aruco.marker_size");
     this->declare_parameter<std::string>("aruco.dictionary");
@@ -46,6 +50,9 @@ ArucoDetectorNode::ArucoDetectorNode(const rclcpp::NodeOptions& options)
     std::string board_pose_topic =
         this->declare_parameter<std::string>("pubs.board_pose");
 
+    std::string landmarks_topic =
+        this->declare_parameter<std::string>("pubs.landmarks");
+
     marker_image_pub_ = this->create_publisher<sensor_msgs::msg::Image>(
         aruco_image_topic, qos_sensor_data);
 
@@ -55,6 +62,9 @@ ArucoDetectorNode::ArucoDetectorNode(const rclcpp::NodeOptions& options)
     board_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
         board_pose_topic, qos_sensor_data);
 
+    landmark_pub_ = this->create_publisher<vortex_msgs::msg::LandmarkArray>(
+        landmarks_topic, qos_sensor_data);
+
     std::string logger_service_name =
         this->declare_parameter<std::string>("logger_service_name");
 
@@ -154,6 +164,7 @@ void ArucoDetectorNode::imageCallback(
     }
     cv::Mat input_image;
     cv::Mat input_image_gray;
+    vortex_msgs::msg::LandmarkArray landmark_array;
     try {
         auto cv_ptr =
             cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
@@ -201,6 +212,20 @@ void ArucoDetectorNode::imageCallback(
                                             msg->header);
             board_pose_pub_->publish(pose_msg);
 
+            vortex_msgs::msg::Landmark board_landmark;
+            board_landmark.header = msg->header;
+            board_landmark.type = vortex_msgs::msg::Landmark::ARUCO_BOARD;
+            board_landmark.subtype =
+                vortex_msgs::msg::Landmark::ARUCO_BOARD_CAMERA;
+
+            geometry_msgs::msg::PoseWithCovariance board_pose_cov;
+            board_pose_cov.pose = pose_msg.pose;
+            std::fill(board_pose_cov.covariance.begin(),
+                      board_pose_cov.covariance.end(), 0.0);
+            board_landmark.pose = board_pose_cov;
+
+            landmark_array.landmarks.push_back(board_landmark);
+
             if (visualize_) {
                 float length =
                     cv::norm(board_->objPoints[0][0] - board_->objPoints[0][1]);
@@ -221,9 +246,8 @@ void ArucoDetectorNode::imageCallback(
     geometry_msgs::msg::PoseArray pose_array;
 
     for (size_t i = 0; i < marker_ids.size(); i++) {
+        int id = marker_ids[i];
         if (log_markers_) {
-            int id = marker_ids[i];
-
             // id 0 is blacklisted, too many false positives
             if (id == 0) {
                 continue;
@@ -233,17 +257,32 @@ void ArucoDetectorNode::imageCallback(
             log_marker_ids(id, time);
         }
 
-        cv::Vec3d rvec = rvecs[i];
-        cv::Vec3d tvec = tvecs[i];
-        tf2::Quaternion quat = rvec_to_quat(rvec);
+        const cv::Vec3d& rvec = rvecs[i];
+        const cv::Vec3d& tvec = tvecs[i];
+        const tf2::Quaternion quat = rvec_to_quat(rvec);
 
         auto pose_msg = cv_pose_to_ros_pose_stamped(tvec, quat, msg->header);
         pose_array.poses.push_back(pose_msg.pose);
+
+        vortex_msgs::msg::Landmark landmark;
+        landmark.header = msg->header;
+        landmark.type = vortex_msgs::msg::Landmark::ARUCO_MARKER;
+        landmark.subtype = static_cast<uint16_t>(id);
+        geometry_msgs::msg::PoseWithCovariance pose_cov;
+        pose_cov.pose = pose_msg.pose;
+        std::fill(pose_cov.covariance.begin(), pose_cov.covariance.end(), 0.0);
+        landmark.pose = pose_cov;
+        landmark_array.landmarks.push_back(landmark);
     }
 
     pose_array.header = msg->header;
-    marker_pose_pub_->publish(pose_array);
-
+    landmark_array.header = msg->header;
+    if (publish_detections_) {
+        marker_pose_pub_->publish(pose_array);
+    }
+    if (publish_landmarks_ && !landmark_array.landmarks.empty()) {
+        landmark_pub_->publish(landmark_array);
+    }
     if (visualize_) {
         cv::aruco::drawDetectedMarkers(input_image, marker_corners, marker_ids);
         cv::aruco::drawDetectedMarkers(input_image, rejected_candidates,