From d9e45f3adcfcb94107b5b0dddd77f6fa8893d9fa Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Wed, 17 Jul 2024 03:00:00 +0900 Subject: [PATCH] feat: update msg name and add topic option update (#13) Signed-off-by: ktro2828 --- awviz/config/awviz.param.yaml | 30 ++++++++++++++-------------- awviz/include/awviz/topic_option.hpp | 16 +++++++-------- awviz/src/rerun_logger_node.cpp | 17 ++++++++++++++-- awviz/src/rerun_logger_node.hpp | 5 +++++ 4 files changed, 43 insertions(+), 25 deletions(-) diff --git a/awviz/config/awviz.param.yaml b/awviz/config/awviz.param.yaml index f5ef08e..285a7d7 100644 --- a/awviz/config/awviz.param.yaml +++ b/awviz/config/awviz.param.yaml @@ -19,47 +19,47 @@ topic_options: /sensing/lidar/concatenated/pointcloud: - type: PointCloud + type: sensor_msgs/msg/PointCloud2 entity: /topics/sensing/lidar/concatenated/pointcloud /sensing/camera/camera0/image_rect_color/compressed: - type: CompressedImage + type: sensor_msgs/msg/CompressedImage entity: /topics/sensing/camera/camera0/image_rect_color/compressed /sensing/camera/camera0/camera_info: - type: CameraInfo + type: sensor_msgs/msg/CameraInfo entity: /topics/sensing/camera/camera0/image_rect_color/compressed /sensing/camera/camera1/image_rect_color/compressed: - type: CompressedImage + type: sensor_msgs/msg/CompressedImage entity: /topics/sensing/camera/camera1/image_rect_color/compressed /sensing/camera/camera1/camera_info: - type: CameraInfo + type: sensor_msgs/msg/CameraInfo entity: /topics/sensing/camera/camera1/image_rect_color/compressed /sensing/camera/camera2/image_rect_color/compressed: - type: CompressedImage + type: sensor_msgs/msg/CompressedImage entity: /topics/sensing/camera/camera2/image_rect_color/compressed /sensing/camera/camera2/camera_info: - type: CameraInfo + type: sensor_msgs/msg/CameraInfo entity: /topics/sensing/camera/camera2/image_rect_color/compressed /sensing/camera/camera3/image_rect_color/compressed: - type: CompressedImage + type: sensor_msgs/msg/CompressedImage entity: /topics/sensing/camera/camera3/image_rect_color/compressed /sensing/camera/camera3/camera_info: - type: CameraInfo + type: sensor_msgs/msg/CameraInfo entity: /topics/sensing/camera/camera3/image_rect_color/compressed /sensing/camera/camera4/image_rect_color/compressed: - type: CompressedImage + type: sensor_msgs/msg/CompressedImage entity: /topics/sensing/camera/camera4/image_rect_color/compressed /sensing/camera/camera4/camera_info: - type: CameraInfo + type: sensor_msgs/msg/CameraInfo entity: /topics/sensing/camera/camera4/image_rect_color/compressed /sensing/camera/camera5/image_rect_color/compressed: - type: CompressedImage + type: sensor_msgs/msg/CompressedImage entity: /topics/sensing/camera/camera5/image_rect_color/compressed /sensing/camera/camera5/camera_info: - type: CameraInfo + type: sensor_msgs/msg/CameraInfo entity: /topics/sensing/camera/camera5/image_rect_color/compressed /perception/object_recognition/detection/objects: - type: DetectedObjects + type: autoware_perception_msgs/msg/DetectedObjects entity: /topics/perception/object_recognition/detection/objects /perception/object_recognition/tracking/objects: - type: TrackedObjects + type: autoware_perception_msgs/msg/TrackedObjects entity: /topics/perception/object_recognition/tracking/objects diff --git a/awviz/include/awviz/topic_option.hpp b/awviz/include/awviz/topic_option.hpp index e04b040..6336c0b 100644 --- a/awviz/include/awviz/topic_option.hpp +++ b/awviz/include/awviz/topic_option.hpp @@ -29,7 +29,7 @@ namespace awviz */ enum MsgType { Unknown, - PointCloud, + PointCloud2, CameraInfo, Image, CompressedImage, @@ -44,17 +44,17 @@ enum MsgType { */ MsgType nameToMsgType(const std::string & name) { - if (name == "PointCloud") { - return MsgType::PointCloud; - } else if (name == "CameraInfo") { + if (name == "sensor_msgs/msg/PointCloud2") { + return MsgType::PointCloud2; + } else if (name == "sensor_msgs/msg/CameraInfo") { return MsgType::CameraInfo; - } else if (name == "Image") { + } else if (name == "sensor_msgs/msg/Image") { return MsgType::Image; - } else if (name == "CompressedImage") { + } else if (name == "sensor_msgs/msg/CompressedImage") { return MsgType::CompressedImage; - } else if (name == "DetectedObjects") { + } else if (name == "autoware_perception_msgs/msg/DetectedObjects") { return MsgType::DetectedObjects; - } else if (name == "TrackedObjects") { + } else if (name == "autoware_perception_msgs/msg/TrackedObjects") { return MsgType::TrackedObjects; } else { return MsgType::Unknown; diff --git a/awviz/src/rerun_logger_node.cpp b/awviz/src/rerun_logger_node.cpp index d9087f3..b355a49 100644 --- a/awviz/src/rerun_logger_node.cpp +++ b/awviz/src/rerun_logger_node.cpp @@ -20,6 +20,7 @@ #include "rclcpp/subscription.hpp" #include "tf2/exceptions.h" +#include #include namespace awviz @@ -41,15 +42,27 @@ RerunLoggerNode::RerunLoggerNode(const rclcpp::NodeOptions & node_options) 100ms, [&]() -> void { createSubscriptions(); }, parallel_callback_group_); } +void RerunLoggerNode::updateTopicOptions() +{ + for (const auto & [topic_name, topic_types] : get_topic_names_and_types()) { + const auto topic_type = nameToMsgType(topic_types.front()); + if (std::find_if(topic_options_.cbegin(), topic_options_.cend(), [&](const auto & option) { + return option.topic() == topic_name && option.type() == topic_type; + }) == topic_options_.cend()) { + topic_options_.emplace_back(topic_name, topic_type); + } + } +} + void RerunLoggerNode::createSubscriptions() { - // TODO(ktro2828): use rclcpp::Node::get_topic_names_and_types() + updateTopicOptions(); for (const auto & option : topic_options_) { if (topic_to_subscription_.find(option.topic()) != topic_to_subscription_.end()) { continue; } - if (option.type() == MsgType::PointCloud) { + if (option.type() == MsgType::PointCloud2) { topic_to_subscription_[option.topic()] = createPointCloudSubscription(option); } else if (option.type() == MsgType::CameraInfo) { topic_to_subscription_[option.topic()] = createCameraInfoSubscription(option); diff --git a/awviz/src/rerun_logger_node.hpp b/awviz/src/rerun_logger_node.hpp index b9d8663..06c38e6 100644 --- a/awviz/src/rerun_logger_node.hpp +++ b/awviz/src/rerun_logger_node.hpp @@ -45,6 +45,11 @@ class RerunLoggerNode : public rclcpp::Node explicit RerunLoggerNode(const rclcpp::NodeOptions & node_options); private: + /** + * @brief Update topic option container if the topic has not registered yet. + */ + void updateTopicOptions(); + /** * @brief Create subscribers. */