From 6304214b61d94fe2ea35d491a28150d20622d035 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Sat, 17 Aug 2024 06:50:41 +0900 Subject: [PATCH] feat: add support of displaying `TrackedObjects` (#39) Signed-off-by: ktro2828 --- .../tracked_objects_display.hpp | 41 +++++++++++++ awviz_plugin/plugin_description.xml | 5 ++ .../tracked_objects_display.cpp | 60 +++++++++++++++++++ docs/PLUGIN.md | 1 + 4 files changed, 107 insertions(+) create mode 100644 awviz_plugin/include/awviz_plugin/autoware_perception/tracked_objects_display.hpp create mode 100644 awviz_plugin/src/autoware_perception/tracked_objects_display.cpp diff --git a/awviz_plugin/include/awviz_plugin/autoware_perception/tracked_objects_display.hpp b/awviz_plugin/include/awviz_plugin/autoware_perception/tracked_objects_display.hpp new file mode 100644 index 0000000..2170fcc --- /dev/null +++ b/awviz_plugin/include/awviz_plugin/autoware_perception/tracked_objects_display.hpp @@ -0,0 +1,41 @@ +// Copyright 2024 Kotaro Uetake. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__TRACKED_OBJECTS_DISPLAY_HPP_ +#define AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__TRACKED_OBJECTS_DISPLAY_HPP_ + +#include + +#include + +namespace awviz_plugin +{ +class TrackedObjectsDisplay +: public awviz_common::RosTopicDisplay +{ +public: + /** + * @brief Construct a new object. + */ + TrackedObjectsDisplay(); + +protected: + /** + * @todo Add support of logging shortened uuids of approximately length 8. + */ + void log_message(autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) override; +}; +} // namespace awviz_plugin + +#endif // AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/awviz_plugin/plugin_description.xml b/awviz_plugin/plugin_description.xml index 07b392c..2a58efb 100644 --- a/awviz_plugin/plugin_description.xml +++ b/awviz_plugin/plugin_description.xml @@ -26,4 +26,9 @@ Display from autoware_perception_msgs/msg/DetectedObjects message. autoware_perception_msgs/msg/DetectedObjects + + Display from autoware_perception_msgs/msg/TrackedObjects message. + autoware_perception_msgs/msg/TrackedObjects + diff --git a/awviz_plugin/src/autoware_perception/tracked_objects_display.cpp b/awviz_plugin/src/autoware_perception/tracked_objects_display.cpp new file mode 100644 index 0000000..ef60b4f --- /dev/null +++ b/awviz_plugin/src/autoware_perception/tracked_objects_display.cpp @@ -0,0 +1,60 @@ +// Copyright 2024 Kotaro Uetake. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awviz_plugin/autoware_perception/tracked_objects_display.hpp" + +#include + +namespace awviz_plugin +{ +TrackedObjectsDisplay::TrackedObjectsDisplay() +: awviz_common::RosTopicDisplay() +{ +} + +void TrackedObjectsDisplay::log_message( + autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr msg) +{ + stream_->set_time_seconds( + TIMELINE_NAME, rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds()); + + const auto entity_path = property_.entity(msg->header.frame_id); + if (!entity_path) { + stream_->log(property_.topic(), rerun::TextLog("There is no corresponding entity path")); + return; + } + + std::vector centers; + std::vector sizes; + std::vector rotations; + std::vector class_ids; + for (const auto & object : msg->objects) { + const auto & pose = object.kinematics.pose_with_covariance.pose; + const auto & dimensions = object.shape.dimensions; + centers.emplace_back(pose.position.x, pose.position.y, pose.position.z); + sizes.emplace_back(dimensions.x, dimensions.y, dimensions.z); + rotations.emplace_back(rerun::Quaternion::from_wxyz( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z)); + class_ids.emplace_back(static_cast(object.classification.front().label)); + } + + stream_->log( + entity_path.value(), rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes) + .with_rotations(rotations) + .with_class_ids(class_ids)); +} +} // namespace awviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(awviz_plugin::TrackedObjectsDisplay, awviz_common::Display); diff --git a/docs/PLUGIN.md b/docs/PLUGIN.md index 28ae1e1..d1484ab 100644 --- a/docs/PLUGIN.md +++ b/docs/PLUGIN.md @@ -20,6 +20,7 @@ Even if there are ROS messages that are not supported by built-in plugins, we ca | ROS Package | Message Type | Plugin Type | | :------------------------- | :---------------- | :----------------------- | | `autoware_perception_msgs` | `DetectedObjects` | `DetectedObjectsDisplay` | +| | `TrackedObjects` | `TrackedObjectsDisplay` | ## Custom Plugin Definition