From 326a7107779e2f919c70a5c87819b3ad86438605 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Wed, 17 Jul 2024 02:07:56 +0900 Subject: [PATCH] feat: add support of `sensor_msgs::msg::CameraInfo` (#9) * feat: add support of `CameraInfo` Signed-off-by: ktro2828 * docs: update README Signed-off-by: ktro2828 * feat: update entity path of camera info Signed-off-by: ktro2828 * chore: pre-commit Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 --- README.md | 2 +- awviz/config/awviz.param.yaml | 24 +++++++++++++++++++++ awviz/include/awviz/rerun_ros_interface.hpp | 11 ++++++++++ awviz/include/awviz/topic_option.hpp | 12 ++++++++++- awviz/src/rerun_logger_node.cpp | 11 ++++++++++ awviz/src/rerun_logger_node.hpp | 9 ++++++++ awviz/src/rerun_ros_interface.cpp | 15 +++++++++++++ 7 files changed, 82 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 105dfc6..6810a72 100644 --- 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 index 6161aa1..f5ef08e 100644 --- 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 index 12c7ab1..20bdd7b 100644 --- a/awviz/include/awviz/rerun_ros_interface.hpp +++ b/awviz/include/awviz/rerun_ros_interface.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -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 index b191085..e04b040 100644 --- 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 index a1dd5fe..d9087f3 100644 --- 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::SharedPtr +RerunLoggerNode::createCameraInfoSubscription(const TopicOption & option) +{ + return this->create_subscription( + option.topic(), 1000, [&](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + awviz::logCameraInfo(stream_, option.entity(), msg); + }); +} + rclcpp::Subscription::SharedPtr RerunLoggerNode::createImageSubscription( const TopicOption & option) { diff --git a/awviz/src/rerun_logger_node.hpp b/awviz/src/rerun_logger_node.hpp index b7b8c38..b9d8663 100644 --- a/awviz/src/rerun_logger_node.hpp +++ b/awviz/src/rerun_logger_node.hpp @@ -20,6 +20,7 @@ #include #include +#include "sensor_msgs/msg/detail/camera_info__struct.hpp" #include #include #include @@ -60,10 +61,18 @@ class RerunLoggerNode : public rclcpp::Node /** * @brief Create a subscriber for PointCloud2 msg. + * @param option Topic option. */ rclcpp::Subscription::SharedPtr createPointCloudSubscription( const TopicOption & option); + /** + * @brief Create a subscriber for CameraInfo msg. + * @param option Topic option. + */ + rclcpp::Subscription::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 index 7a0af6c..ae5b399 100644 --- 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 #include @@ -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 camera_intrinsic = { + static_cast(msg->k[0]), static_cast(msg->k[3]), static_cast(msg->k[6]), + static_cast(msg->k[1]), static_cast(msg->k[4]), static_cast(msg->k[7]), + static_cast(msg->k[2]), static_cast(msg->k[5]), static_cast(msg->k[8])}; + + stream.log( + entity, rerun::Pinhole(camera_intrinsic) + .with_resolution(static_cast(msg->width), static_cast(msg->height))); +} + void logImage( const rerun::RecordingStream & stream, const std::string & entity, const sensor_msgs::msg::Image::ConstSharedPtr & msg)