feat: add support of TrackedObjects

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

awviz/config/awviz.param.yaml

@@ -9,6 +9,7 @@
       - /sensing/camera/camera4/image_rect_color/compressed
       - /sensing/camera/camera5/image_rect_color/compressed
       - /perception/object_recognition/detection/objects
+      - /perception/object_recognition/tracking/objects
 
     topic_options:
       /sensing/lidar/concatenated/pointcloud:
@@ -34,4 +35,7 @@
         entity: /topics/sensing/camera/camera5/image_rect_color/compressed
       /perception/object_recognition/detection/objects:
         type: DetectedObjects
-        entity: /topics/perception/object_recognition/detected_objects
+        entity: /topics/perception/object_recognition/detection/objects
+      /perception/object_recognition/tracking/objects:
+        type: TrackedObjects
+        entity: /topics/perception/object_recognition/tracking/objects

awviz/include/awviz/rerun_ros_interface.hpp

@@ -19,8 +19,8 @@
 
 #include <rerun.hpp>
 
-#include "autoware_perception_msgs/msg/detail/detected_objects__struct.hpp"
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -68,6 +68,16 @@ void logCompressedImage(
 void logDetectedObjects(
   const rerun::RecordingStream & stream, const std::string & entity,
   const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & msg);
+
+/**
+ * @brief Log TrackedObjects msg to rerun stream.
+ * @param stream Rerun recodring stream.
+ * @param entity Entity path of the record.
+ * @param msg TrackedObjects msg pointer.
+ */
+void logTrackedObjects(
+  const rerun::RecordingStream & stream, const std::string & entity,
+  const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg);
 }  // namespace awviz
 
 #endif  // AWVIZ__RERUN_ROS_INTERFACE_HPP_

awviz/include/awviz/topic_option.hpp

@@ -27,7 +27,7 @@ namespace awviz
 /**
  * @brief Represent ROS msg types.
  */
-enum MsgType { Unknown, PointCloud, Image, CompressedImage, DetectedObjects };
+enum MsgType { Unknown, PointCloud, Image, CompressedImage, DetectedObjects, TrackedObjects };
 
 /**
  * @brief Convert string name of ROS msg into MsgType.
@@ -44,6 +44,8 @@ MsgType nameToMsgType(const std::string & name)
     return MsgType::CompressedImage;
   } else if (name == "DetectedObjects") {
     return MsgType::DetectedObjects;
+  } else if (name == "TrackedObjects") {
+    return MsgType::TrackedObjects;
   } else {
     return MsgType::Unknown;
   }

awviz/include/awviz/uuid.hpp

@@ -0,0 +1,55 @@
+// 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__UUID_HPP_
+#define AWVIZ__UUID_HPP_
+
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <cstdint>
+#include <sstream>
+#include <string>
+
+namespace awviz
+{
+
+template <typename T>
+T uuid(const unique_identifier_msgs::msg::UUID & msg);
+
+/**
+ * @brief Convert UUID msg into a single uint16_t value.
+ * @param msg UUID msg.
+ * @param uint16_t A single uint16_t value.
+ */
+template <>
+uint16_t uuid<uint16_t>(const unique_identifier_msgs::msg::UUID & msg)
+{
+  uint16_t output = 0;
+  for (size_t i = 0; i < msg.uuid.size(); ++i) {
+    output |= static_cast<uint16_t>(msg.uuid[i]) << (i * 8);
+  }
+  return output;
+}
+
+template <>
+std::string uuid<std::string>(const unique_identifier_msgs::msg::UUID & msg)
+{
+  std::ostringstream ss;
+  for (const auto & v : msg.uuid) {
+    ss << static_cast<int>(v);
+  }
+  return ss.str();
+}
+}  // namespace awviz
+#endif  //  AWVIZ__UUID_HPP_

awviz/src/rerun_logger_node.cpp

@@ -18,8 +18,6 @@
 #include "awviz/topic_option.hpp"
 #include "rclcpp/subscription.hpp"
 
-#include "autoware_perception_msgs/msg/detail/detected_objects__struct.hpp"
-
 #include <chrono>
 
 namespace awviz
@@ -53,6 +51,8 @@ void RerunLoggerNode::createSubscriptions()
       topic_to_subscription_[option.topic()] = createCompressedImageSubscription(option);
     } else if (option.type() == MsgType::DetectedObjects) {
       topic_to_subscription_[option.topic()] = createDetectedObjectsSubscription(option);
+    } else if (option.type() == MsgType::TrackedObjects) {
+      topic_to_subscription_[option.topic()] = createTrackedObjectsSubscription(option);
     } else {
       RCLCPP_WARN_STREAM(this->get_logger(), "Unknown msg type of topic: " << option.topic());
     }
@@ -94,6 +94,15 @@ RerunLoggerNode::createDetectedObjectsSubscription(const TopicOption & option)
       awviz::logDetectedObjects(stream_, option.entity(), msg);
     });
 }
+
+rclcpp::Subscription<autoware_perception_msgs::msg::TrackedObjects>::SharedPtr
+RerunLoggerNode::createTrackedObjectsSubscription(const TopicOption & option)
+{
+  return this->create_subscription<autoware_perception_msgs::msg::TrackedObjects>(
+    option.topic(), 1000, [&](const autoware_perception_msgs::msg::TrackedObjects::SharedPtr msg) {
+      awviz::logTrackedObjects(stream_, option.entity(), msg);
+    });
+}
 }  // namespace awviz
 
 #include <rclcpp_components/register_node_macro.hpp>

awviz/src/rerun_logger_node.hpp

@@ -22,6 +22,7 @@
 #include <rerun.hpp>
 
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -66,9 +67,20 @@ class RerunLoggerNode : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
   createCompressedImageSubscription(const TopicOption & option);
 
+  /**
+   * @brief Create subscriber for DetectedObjects msg.
+   * @param option Topic option.
+   */
   rclcpp::Subscription<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr
   createDetectedObjectsSubscription(const TopicOption & option);
 
+  /**
+   * @brief Create subscriber for TrackedObjects msg.
+   * @param option Topic option.
+   */
+  rclcpp::Subscription<autoware_perception_msgs::msg::TrackedObjects>::SharedPtr
+  createTrackedObjectsSubscription(const TopicOption & option);
+
 private:
   const rerun::RecordingStream stream_;
 

awviz/src/rerun_ros_interface.cpp

@@ -14,6 +14,7 @@
 
 #include "awviz/rerun_ros_interface.hpp"
 
+#include "awviz/uuid.hpp"
 #include "collection_adapters.hpp"
 #include "color.hpp"
 
@@ -153,13 +154,41 @@ void logDetectedObjects(
   const rerun::RecordingStream & stream, const std::string & entity,
   const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr & msg)
 {
-  stream.set_time_sequence(
+  stream.set_time_seconds(
+    "timestamp", rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds());
+
+  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, rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes)
+              .with_rotations(rotations)
+              .with_class_ids(class_ids));
+}
+
+void logTrackedObjects(
+  const rerun::RecordingStream & stream, const std::string & entity,
+  const autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg)
+{
+  stream.set_time_seconds(
     "timestamp", rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds());
 
   std::vector<rerun::Position3D> centers;
   std::vector<rerun::HalfSize3D> sizes;
   std::vector<rerun::Rotation3D> rotations;
   std::vector<rerun::components::ClassId> class_ids;
+  std::vector<rerun::Text> uuids;
   for (const auto & object : msg->objects) {
     const auto & pose = object.kinematics.pose_with_covariance.pose;
     const auto & dimensions = object.shape.dimensions;
@@ -168,8 +197,10 @@ void logDetectedObjects(
     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));
+    uuids.emplace_back(uuid<std::string>(object.object_id));
   }
 
+  // TODO(ktro2828): use a shortened uuid of approximately length 8.
   stream.log(
     entity, rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes)
               .with_rotations(rotations)