Added ImageMarker publisher for foxglove

Author: lucasreljic

deep_object_detection/CMakeLists.txt

@@ -30,6 +30,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(vision_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 find_package(deep_core REQUIRED)
 find_package(deep_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
@@ -60,6 +61,7 @@ target_link_libraries(deep_object_detection_lib
     ${pluginlib_TARGETS}
     ${vision_msgs_TARGETS}
     ${deep_msgs_TARGETS}
+    ${visualization_msgs_TARGETS}
     deep_core::deep_core_lib
   PRIVATE
     ${rclcpp_components_TARGETS}

deep_object_detection/include/deep_object_detection/deep_object_detection_node.hpp

@@ -40,6 +40,7 @@
 #include <rclcpp_lifecycle/state.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <std_msgs/msg/header.hpp>
+#include <visualization_msgs/msg/image_marker.hpp>
 
 #include "deep_object_detection/backend_manager.hpp"
 #include "deep_object_detection/detection_types.hpp"
@@ -200,6 +201,18 @@ class DeepObjectDetectionNode : public rclcpp_lifecycle::LifecycleNode
     const std::vector<std_msgs::msg::Header> & headers,
     const std::vector<ImageMeta> & metas);
 
+  /**
+   * @brief Convert detections to ImageMarker annotations for Foxglove visualization
+   * @param header ROS header for the message
+   * @param detections Vector of detections for a single image
+   * @return ImageMarker message with bounding box and label annotations
+   *
+   * Converts SimpleDetection objects to visualization_msgs::msg::ImageMarker format
+   * for rendering in Foxglove and other visualization tools.
+   */
+  visualization_msgs::msg::ImageMarker detectionsToImageMarker(
+    const std_msgs::msg::Header & header, const std::vector<SimpleDetection> & detections) const;
+
   /**
    * @brief Load class names from file
    *
@@ -242,6 +255,10 @@ class DeepObjectDetectionNode : public rclcpp_lifecycle::LifecycleNode
   std::string input_topic_;
   /// Publisher for Detection2DArray messages
   rclcpp_lifecycle::LifecyclePublisher<Detection2DArrayMsg>::SharedPtr detection_pub_;
+  /// Publisher for ImageMarker annotations (for Foxglove visualization)
+  rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::ImageMarker>::SharedPtr image_marker_pub_;
+  /// Output annotations topic name for ImageMarker messages
+  std::string output_annotations_topic_;
   /// Timer for periodic batch processing checks (5ms period)
   rclcpp::TimerBase::SharedPtr batch_timer_;
 

deep_object_detection/package.xml

@@ -19,6 +19,7 @@
   <depend>sensor_msgs</depend>
   <depend>cv_bridge</depend>
   <depend>vision_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>deep_core</depend>
   <depend>deep_msgs</depend>
   <depend>deep_ort_backend_plugin</depend>

deep_object_detection/src/deep_object_detection_node.cpp

@@ -134,6 +134,7 @@ void DeepObjectDetectionNode::declareAndReadParameters()
   input_topic_ = this->declare_parameter<std::string>("input_topic", "");
   params_.input_qos_reliability = "best_effort";
   params_.output_detections_topic = this->declare_parameter<std::string>("output_detections_topic", "/detections");
+  output_annotations_topic_ = this->declare_parameter<std::string>("output_annotations_topic", "/image_annotations");
 
   params_.max_batch_size = this->declare_parameter<int>("max_batch_size", 3);
   params_.queue_size = this->declare_parameter<int>("queue_size", 10);
@@ -225,6 +226,11 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn DeepOb
     auto pub = this->create_publisher<Detection2DArrayMsg>(params_.output_detections_topic, rclcpp::SensorDataQoS{});
     detection_pub_ = std::static_pointer_cast<rclcpp_lifecycle::LifecyclePublisher<Detection2DArrayMsg>>(pub);
 
+    auto marker_pub =
+      this->create_publisher<visualization_msgs::msg::ImageMarker>(output_annotations_topic_, rclcpp::SensorDataQoS{});
+    image_marker_pub_ =
+      std::static_pointer_cast<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::ImageMarker>>(marker_pub);
+
     if (input_topic_.empty()) {
       RCLCPP_ERROR(this->get_logger(), "input_topic is empty. Please set input_topic to a valid MultiImage topic.");
       cleanupPartialConfiguration();
@@ -269,6 +275,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn DeepOb
       detection_pub_->on_activate();
     }
 
+    if (image_marker_pub_) {
+      image_marker_pub_->on_activate();
+    }
+
     RCLCPP_INFO(this->get_logger(), "Deep object detection node activated and ready to process images");
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
   } catch (const std::exception & e) {
@@ -283,6 +293,7 @@ void DeepObjectDetectionNode::cleanupPartialConfiguration()
   backend_manager_.reset();
   preprocessor_.reset();
   detection_pub_.reset();
+  image_marker_pub_.reset();
 }
 
 void DeepObjectDetectionNode::cleanupAllResources()
@@ -293,6 +304,7 @@ void DeepObjectDetectionNode::cleanupAllResources()
   postprocessor_.reset();
   params_.class_names.clear();
   detection_pub_.reset();
+  image_marker_pub_.reset();
 }
 
 void DeepObjectDetectionNode::stopSubscriptionsAndTimer()
@@ -321,6 +333,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn DeepOb
     detection_pub_->on_deactivate();
   }
 
+  if (image_marker_pub_) {
+    image_marker_pub_->on_deactivate();
+  }
+
   RCLCPP_INFO(this->get_logger(), "Deep object detection node deactivated");
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
@@ -554,6 +570,19 @@ void DeepObjectDetectionNode::publishDetections(
     postprocessor_->fillDetectionMessage(headers[i], batch_detections[i], metas[i], msg);
     detection_pub_->publish(msg);
     total_published += batch_detections[i].size();
+
+    RCLCPP_DEBUG(
+      this->get_logger(),
+      "  Published [%zu]: frame_id=%s, %zu detections",
+      i,
+      headers[i].frame_id.c_str(),
+      batch_detections[i].size());
+
+    // Also publish ImageMarker annotations if publisher is available
+    if (image_marker_pub_) {
+      auto marker_msg = detectionsToImageMarker(headers[i], batch_detections[i]);
+      image_marker_pub_->publish(marker_msg);
+    }
   }
   RCLCPP_INFO_THROTTLE(
     this->get_logger(),
@@ -589,6 +618,86 @@ void DeepObjectDetectionNode::loadClassNames()
     this->get_logger(), "Loaded %zu class names from %s", params_.class_names.size(), class_names_path.c_str());
 }
 
+visualization_msgs::msg::ImageMarker DeepObjectDetectionNode::detectionsToImageMarker(
+  const std_msgs::msg::Header & header, const std::vector<SimpleDetection> & detections) const
+{
+  visualization_msgs::msg::ImageMarker marker_msg;
+  marker_msg.header = header;
+  marker_msg.ns = "detections";
+  marker_msg.id = 0;
+  marker_msg.type = visualization_msgs::msg::ImageMarker::LINE_LIST;
+  marker_msg.action = visualization_msgs::msg::ImageMarker::ADD;
+
+  // Set marker lifetime
+  marker_msg.lifetime.sec = 0;
+  marker_msg.lifetime.nanosec = 50000000;  // 0.05 seconds - short lifetime to avoid ghosting
+  marker_msg.scale = 2.0;
+
+  // Set outline color to green
+  marker_msg.outline_color.r = 0.0;
+  marker_msg.outline_color.g = 1.0;
+  marker_msg.outline_color.b = 0.0;
+  marker_msg.outline_color.a = 1.0;
+
+  // Iterate through detections and add bounding box line segments
+  for (const auto & det : detections) {
+    // Skip detections with invalid coordinates
+    if (det.width <= 0 || det.height <= 0) {
+      continue;
+    }
+
+    // Calculate bounding box corners
+    float x_min = det.x;
+    float y_min = det.y;
+    float x_max = det.x + det.width;
+    float y_max = det.y + det.height;
+
+    // Add line points for bounding box rectangle (4 line segments = 8 points)
+    // Top-left to top-right
+    geometry_msgs::msg::Point pt1, pt2;
+    pt1.x = x_min;
+    pt1.y = y_min;
+    pt1.z = 0;
+    pt2.x = x_max;
+    pt2.y = y_min;
+    pt2.z = 0;
+    marker_msg.points.push_back(pt1);
+    marker_msg.points.push_back(pt2);
+
+    // Top-right to bottom-right
+    pt1.x = x_max;
+    pt1.y = y_min;
+    pt1.z = 0;
+    pt2.x = x_max;
+    pt2.y = y_max;
+    pt2.z = 0;
+    marker_msg.points.push_back(pt1);
+    marker_msg.points.push_back(pt2);
+
+    // Bottom-right to bottom-left
+    pt1.x = x_max;
+    pt1.y = y_max;
+    pt1.z = 0;
+    pt2.x = x_min;
+    pt2.y = y_max;
+    pt2.z = 0;
+    marker_msg.points.push_back(pt1);
+    marker_msg.points.push_back(pt2);
+
+    // Bottom-left to top-left
+    pt1.x = x_min;
+    pt1.y = y_max;
+    pt1.z = 0;
+    pt2.x = x_min;
+    pt2.y = y_min;
+    pt2.z = 0;
+    marker_msg.points.push_back(pt1);
+    marker_msg.points.push_back(pt2);
+  }
+
+  return marker_msg;
+}
+
 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> createDeepObjectDetectionNode(const rclcpp::NodeOptions & options)
 {
   return std::make_shared<DeepObjectDetectionNode>(options);