feat: add support of displaying TrackedObjects (#39)

Signed-off-by: ktro2828 <[email protected]>

Author: ktro2828

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 <awviz_common/display.hpp>
+
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
+
+namespace awviz_plugin
+{
+class TrackedObjectsDisplay
+: public awviz_common::RosTopicDisplay<autoware_perception_msgs::msg::TrackedObjects>
+{
+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_

awviz_plugin/plugin_description.xml

@@ -26,4 +26,9 @@
         <description>Display from autoware_perception_msgs/msg/DetectedObjects message.</description>
         <message_type>autoware_perception_msgs/msg/DetectedObjects</message_type>
     </class>
+    <class name="awviz_plugin/TrackedObjectsDisplay" type="awviz_plugin::TrackedObjectsDisplay"
+        base_class_type="awviz_common::Display">
+        <description>Display from autoware_perception_msgs/msg/TrackedObjects message.</description>
+        <message_type>autoware_perception_msgs/msg/TrackedObjects</message_type>
+    </class>
 </library>

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 <rerun.hpp>
+
+namespace awviz_plugin
+{
+TrackedObjectsDisplay::TrackedObjectsDisplay()
+: awviz_common::RosTopicDisplay<autoware_perception_msgs::msg::TrackedObjects>()
+{
+}
+
+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<rerun::Position3D> centers;
+  std::vector<rerun::HalfSize3D> sizes;
+  std::vector<rerun::Rotation3D> rotations;
+  std::vector<rerun::components::ClassId> 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<uint16_t>(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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(awviz_plugin::TrackedObjectsDisplay, awviz_common::Display);

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