feat(awviz_plugin): add support of `autoware_perception_msgs::msg::Pr…

…edictedObjects`

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

awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp

@@ -0,0 +1,56 @@
+// 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__PREDICTED_OBJECTS_DISPLAY_HPP_
+#define AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__PREDICTED_OBJECTS_DISPLAY_HPP_
+
+#include <awviz_common/display.hpp>
+#include <rerun.hpp>
+
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+
+#include <string>
+#include <vector>
+
+namespace awviz_plugin
+{
+/**
+ * @brief Display plugin of `autoware_perception_msgs::msg::PredictedObjects`.
+ */
+class PredictedObjectsDisplay
+: public awviz_common::RosTopicDisplay<autoware_perception_msgs::msg::PredictedObjects>
+{
+public:
+  /**
+   * @brief Construct a new object.
+   */
+  PredictedObjectsDisplay();
+
+protected:
+  void log_message(autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) override;
+
+private:
+  /**
+   * @brief Log predicted paths with `rerun::LineStrips3D`.
+   *
+   * @param entity_path Entity path of log.
+   * @param predicted_paths List of predict path.
+   */
+  void log_predicted_paths(
+    const std::string & entity_path,
+    const std::vector<autoware_perception_msgs::msg::PredictedPath> & predicted_paths) const;
+};
+}  // namespace awviz_plugin
+
+#endif  // AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__PREDICTED_OBJECTS_DISPLAY_HPP_

awviz_plugin/plugin_description.xml

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

awviz_plugin/src/autoware_perception/predicted_objects_display.cpp

@@ -0,0 +1,84 @@
+// 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/predicted_objects_display.hpp"
+
+#include <awviz_plugin/color.hpp>
+#include <rerun.hpp>
+
+#include <array>
+
+namespace awviz_plugin
+{
+PredictedObjectsDisplay::PredictedObjectsDisplay()
+: awviz_common::RosTopicDisplay<autoware_perception_msgs::msg::PredictedObjects>()
+{
+}
+
+void PredictedObjectsDisplay::log_message(
+  autoware_perception_msgs::msg::PredictedObjects::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 & init_pose = object.kinematics.initial_pose_with_covariance.pose;
+    const auto & dimensions = object.shape.dimensions;
+    centers.emplace_back(init_pose.position.x, init_pose.position.y, init_pose.position.z);
+    sizes.emplace_back(dimensions.x, dimensions.y, dimensions.z);
+    rotations.emplace_back(rerun::Quaternion::from_wxyz(
+      init_pose.orientation.w, init_pose.orientation.x, init_pose.orientation.y,
+      init_pose.orientation.z));
+    class_ids.emplace_back(static_cast<uint16_t>(object.classification.front().label));
+
+    log_predicted_paths(entity_path.value(), object.kinematics.predicted_paths);
+  }
+
+  stream_->log(
+    entity_path.value(), rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes)
+                           .with_rotations(rotations)
+                           .with_class_ids(class_ids));
+}
+
+void PredictedObjectsDisplay::log_predicted_paths(
+  const std::string & entity_path,
+  const std::vector<autoware_perception_msgs::msg::PredictedPath> & predicted_paths) const
+{
+  std::vector<rerun::LineStrip3D> strips;
+  std::vector<float> confidences;
+  for (const auto & path : predicted_paths) {
+    std::vector<rerun::Vec3D> waypoints;
+    for (const auto & point : path.path) {
+      waypoints.emplace_back(point.position.x, point.position.y, point.position.z);
+    }
+    strips.emplace_back(waypoints);
+    confidences.emplace_back(path.confidence);
+  }
+  auto colors = colormap(confidences);
+  stream_->log(entity_path, rerun::LineStrips3D(strips).with_colors(colors).with_radii(0.05f));
+}
+}  // namespace awviz_plugin
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(awviz_plugin::PredictedObjectsDisplay, awviz_common::Display);