feat: add support of sensor_msgs::msg::CameraInfo (#9)

* feat: add support of `CameraInfo` Signed-off-by: ktro2828 <[email protected]> * docs: update README Signed-off-by: ktro2828 <[email protected]> * feat: update entity path of camera info Signed-off-by: ktro2828 <[email protected]> * chore: pre-commit Signed-off-by: ktro2828 <[email protected]> --------- Signed-off-by: ktro2828 <[email protected]>
ktro2828 · Jul 16, 2024 · 326a710 · 326a710
1 parent 632acd3
commit 326a710
Show file tree

Hide file tree

Showing 7 changed files with 82 additions and 2 deletions.
diff --git a/README.md b/README.md
@@ -51,7 +51,7 @@ For details, please refer to [awviz/config/awviz.param.yaml](./awviz/config/awvi
 |   `PointCloud2`   |   ✅    |
 |      `Image`      |   ✅    |
 | `CompressedImage` |   ✅    |
-|   `CameraInfo`    |         |
+|   `CameraInfo`    |   ✅    |
 
 ### `autoware_msgs`
 

diff --git a/awviz/config/awviz.param.yaml b/awviz/config/awviz.param.yaml
@@ -3,11 +3,17 @@
     topic_names:
       - /sensing/lidar/concatenated/pointcloud
       - /sensing/camera/camera0/image_rect_color/compressed
+      - /sensing/camera/camera0/camera_info
       - /sensing/camera/camera1/image_rect_color/compressed
+      - /sensing/camera/camera1/camera_info
       - /sensing/camera/camera2/image_rect_color/compressed
+      - /sensing/camera/camera2/camera_info
       - /sensing/camera/camera3/image_rect_color/compressed
+      - /sensing/camera/camera3/camera_info
       - /sensing/camera/camera4/image_rect_color/compressed
+      - /sensing/camera/camera4/camera_info
       - /sensing/camera/camera5/image_rect_color/compressed
+      - /sensing/camera/camera5/camera_info
       - /perception/object_recognition/detection/objects
       - /perception/object_recognition/tracking/objects
 
@@ -18,21 +24,39 @@
       /sensing/camera/camera0/image_rect_color/compressed:
         type: CompressedImage
         entity: /topics/sensing/camera/camera0/image_rect_color/compressed
+      /sensing/camera/camera0/camera_info:
+        type: CameraInfo
+        entity: /topics/sensing/camera/camera0/image_rect_color/compressed
       /sensing/camera/camera1/image_rect_color/compressed:
         type: CompressedImage
         entity: /topics/sensing/camera/camera1/image_rect_color/compressed
+      /sensing/camera/camera1/camera_info:
+        type: CameraInfo
+        entity: /topics/sensing/camera/camera1/image_rect_color/compressed
       /sensing/camera/camera2/image_rect_color/compressed:
         type: CompressedImage
         entity: /topics/sensing/camera/camera2/image_rect_color/compressed
+      /sensing/camera/camera2/camera_info:
+        type: CameraInfo
+        entity: /topics/sensing/camera/camera2/image_rect_color/compressed
       /sensing/camera/camera3/image_rect_color/compressed:
         type: CompressedImage
         entity: /topics/sensing/camera/camera3/image_rect_color/compressed
+      /sensing/camera/camera3/camera_info:
+        type: CameraInfo
+        entity: /topics/sensing/camera/camera3/image_rect_color/compressed
       /sensing/camera/camera4/image_rect_color/compressed:
         type: CompressedImage
         entity: /topics/sensing/camera/camera4/image_rect_color/compressed
+      /sensing/camera/camera4/camera_info:
+        type: CameraInfo
+        entity: /topics/sensing/camera/camera4/image_rect_color/compressed
       /sensing/camera/camera5/image_rect_color/compressed:
         type: CompressedImage
         entity: /topics/sensing/camera/camera5/image_rect_color/compressed
+      /sensing/camera/camera5/camera_info:
+        type: CameraInfo
+        entity: /topics/sensing/camera/camera5/image_rect_color/compressed
       /perception/object_recognition/detection/objects:
         type: DetectedObjects
         entity: /topics/perception/object_recognition/detection/objects

diff --git a/awviz/include/awviz/rerun_ros_interface.hpp b/awviz/include/awviz/rerun_ros_interface.hpp
@@ -22,6 +22,7 @@
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -50,6 +51,16 @@ void logPointCloud(
   const rerun::RecordingStream & stream, const std::string & entity,
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg);
 
+/**
+ * @brief Log CameraInfo msg to rerun stream.
+ * @param stream Rerun recodring stream.
+ * @param entity Entity path of the record.
+ * @param msg CameraInfo msg pointer.
+ */
+void logCameraInfo(
+  const rerun::RecordingStream & stream, const std::string & entity,
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg);
+
 /**
  * @brief Log Image msg to rerun stream.
  * @param stream Rerun recording stream.

diff --git a/awviz/include/awviz/topic_option.hpp b/awviz/include/awviz/topic_option.hpp
@@ -27,7 +27,15 @@ namespace awviz
 /**
  * @brief Represent ROS msg types.
  */
-enum MsgType { Unknown, PointCloud, Image, CompressedImage, DetectedObjects, TrackedObjects };
+enum MsgType {
+  Unknown,
+  PointCloud,
+  CameraInfo,
+  Image,
+  CompressedImage,
+  DetectedObjects,
+  TrackedObjects
+};
 
 /**
  * @brief Convert string name of ROS msg into MsgType.
@@ -38,6 +46,8 @@ MsgType nameToMsgType(const std::string & name)
 {
   if (name == "PointCloud") {
     return MsgType::PointCloud;
+  } else if (name == "CameraInfo") {
+    return MsgType::CameraInfo;
   } else if (name == "Image") {
     return MsgType::Image;
   } else if (name == "CompressedImage") {

diff --git a/awviz/src/rerun_logger_node.cpp b/awviz/src/rerun_logger_node.cpp
@@ -51,6 +51,8 @@ void RerunLoggerNode::createSubscriptions()
 
     if (option.type() == MsgType::PointCloud) {
       topic_to_subscription_[option.topic()] = createPointCloudSubscription(option);
+    } else if (option.type() == MsgType::CameraInfo) {
+      topic_to_subscription_[option.topic()] = createCameraInfoSubscription(option);
     } else if (option.type() == MsgType::Image) {
       topic_to_subscription_[option.topic()] = createImageSubscription(option);
     } else if (option.type() == MsgType::CompressedImage) {
@@ -91,6 +93,15 @@ RerunLoggerNode::createPointCloudSubscription(const TopicOption & option)
     });
 }
 
+rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr
+RerunLoggerNode::createCameraInfoSubscription(const TopicOption & option)
+{
+  return this->create_subscription<sensor_msgs::msg::CameraInfo>(
+    option.topic(), 1000, [&](const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
+      awviz::logCameraInfo(stream_, option.entity(), msg);
+    });
+}
+
 rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr RerunLoggerNode::createImageSubscription(
   const TopicOption & option)
 {

diff --git a/awviz/src/rerun_logger_node.hpp b/awviz/src/rerun_logger_node.hpp
@@ -20,6 +20,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rerun.hpp>
 
+#include "sensor_msgs/msg/detail/camera_info__struct.hpp"
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <geometry_msgs/msg/transform_stamped.hpp>
@@ -60,10 +61,18 @@ class RerunLoggerNode : public rclcpp::Node
 
   /**
    * @brief Create a subscriber for PointCloud2 msg.
+   * @param option Topic option.
    */
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr createPointCloudSubscription(
     const TopicOption & option);
 
+  /**
+   * @brief Create a subscriber for CameraInfo msg.
+   * @param option Topic option.
+   */
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr createCameraInfoSubscription(
+    const TopicOption & option);
+
   /**
    * @brief Craete subscriber for Image msg.
    * @param option Topic option.

diff --git a/awviz/src/rerun_ros_interface.cpp b/awviz/src/rerun_ros_interface.cpp
@@ -17,6 +17,7 @@
 #include "awviz/uuid.hpp"
 #include "collection_adapters.hpp"
 #include "color.hpp"
+#include "rerun/archetypes/pinhole.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <rerun.hpp>
@@ -115,6 +116,20 @@ void logPointCloud(
   stream.log(entity, rerun::Points3D(points).with_colors(colors));
 }
 
+void logCameraInfo(
+  const rerun::RecordingStream & stream, const std::string & entity,
+  const sensor_msgs::msg::CameraInfo::ConstSharedPtr & msg)
+{
+  const std::array<float, 9> camera_intrinsic = {
+    static_cast<float>(msg->k[0]), static_cast<float>(msg->k[3]), static_cast<float>(msg->k[6]),
+    static_cast<float>(msg->k[1]), static_cast<float>(msg->k[4]), static_cast<float>(msg->k[7]),
+    static_cast<float>(msg->k[2]), static_cast<float>(msg->k[5]), static_cast<float>(msg->k[8])};
+
+  stream.log(
+    entity, rerun::Pinhole(camera_intrinsic)
+              .with_resolution(static_cast<int>(msg->width), static_cast<int>(msg->height)));
+}
+
 void logImage(
   const rerun::RecordingStream & stream, const std::string & entity,
   const sensor_msgs::msg::Image::ConstSharedPtr & msg)