feat(awviz_plugin): add support of autoware_perception_msgs::msg::PredictedObjects

awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp

@@ -0,0 +1,42 @@
+// 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>
+
+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;
+};
+}  // namespace awviz_plugin
+
+#endif  // AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__PREDICTED_OBJECTS_DISPLAY_HPP_

awviz_plugin/include/awviz_plugin/image/collection_adapter.hpp → awviz_plugin/include/awviz_plugin/collection_adapter.hpp

@@ -12,13 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef AWVIZ_PLUGIN__IMAGE__COLLECTION_ADAPTER_HPP_
-#define AWVIZ_PLUGIN__IMAGE__COLLECTION_ADAPTER_HPP_
+#ifndef AWVIZ_PLUGIN__COLLECTION_ADAPTER_HPP_
+#define AWVIZ_PLUGIN__COLLECTION_ADAPTER_HPP_
 
+#include <eigen3/Eigen/Core>
 #include <opencv2/opencv.hpp>
 #include <rerun.hpp>
 #include <rerun/collection.hpp>
 
+#include <cstring>
 #include <utility>
 #include <vector>
 
@@ -30,22 +32,69 @@ namespace rerun
 template <typename TElement>
 struct CollectionAdapter<TElement, cv::Mat>
 {
-  /// Borrow for non-temporary.
+  /**
+   * @brief Borrow for non-temporary.
+   *
+   * @param img OpenCV matrix.
+   * @return Collection<TElement>
+   */
   Collection<TElement> operator()(const cv::Mat & img)
   {
     return Collection<TElement>::borrow(
       reinterpret_cast<TElement *>(img.data), img.total() * img.channels());
   }
 
-  // Do a full copy for temporaries (otherwise the data might be deleted when the temporary is
-  // destroyed).
+  /**
+   * @brief Do a full copy for temporaries (otherwise the data might be deleted when the temporary
+   * is destroyed).
+   *
+   * @param img OpenCV matrix.
+   * @return Collection<TElement>
+   */
   Collection<TElement> operator()(cv::Mat && img)
   {
     std::vector<TElement> img_vec(img.total() * img.channels());
     img_vec.assign(img.data, img.data + img.total() * img.channels());
     return Collection<TElement>::take_ownership(std::move(img_vec));
   }
 };
+
+/**
+ * @brief An adaptor to be able to borrow an Eigen matrix into Rerun 3D position without copying.
+ */
+template <>
+struct CollectionAdapter<rerun::Position3D, std::vector<Eigen::Vector3f>>
+{
+  // Sanity check that this is binary compatible.
+  static_assert(
+    sizeof(rerun::Position3D) ==
+    sizeof(Eigen::Matrix3Xf::Scalar) * Eigen::Matrix3Xf::RowsAtCompileTime);
+
+  /**
+   * @brief Borrow for non-temporary.
+   *
+   * @param container Eigen 3D float vector.
+   * @return Collection<rerun::Position3D>
+   */
+  Collection<rerun::Position3D> operator()(const std::vector<Eigen::Vector3f> & container)
+  {
+    return Collection<rerun::Position3D>::borrow(container.data(), container.size());
+  }
+
+  /**
+   * @brief Do a full copy for temporaries (otherwise the data might be deleted when the temporary
+   * is destroyed).
+   *
+   * @param container Eigen 3D float vector.
+   * @return Collection<rerun::Position3D>
+   */
+  Collection<rerun::Position3D> operator()(std::vector<Eigen::Vector3f> && container)
+  {
+    std::vector<rerun::Position3D> positions(container.size());
+    memcpy(positions.data(), container.data(), container.size() * sizeof(Eigen::Vector3f));
+    return Collection<rerun::Position3D>::take_ownership(std::move(positions));
+  }
+};
 }  // namespace rerun
 
 namespace awviz_plugin
@@ -64,4 +113,4 @@ inline rerun::Collection<rerun::TensorDimension> tensor_shape(const cv::Mat & im
 }
 }  // namespace awviz_plugin
 
-#endif  // AWVIZ_PLUGIN__IMAGE__IMAGE_COMPRESSED_IMAGE_DISPLAY_HPP_
+#endif

awviz_plugin/plugin_description.xml

@@ -42,4 +42,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,114 @@
+// 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 <rerun.hpp>
+#include <rerun/archetypes/line_strips3d.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/convert.h>
+
+#include <vector>
+
+namespace awviz_plugin
+{
+namespace
+{
+/**
+ * @brief Transform pose from `init_pose` origin coordinate to world coordinate.
+ *
+ * @param origin Pose origin.
+ * @param pose Pose which is with respect to `origin` origin coordinate.
+ * @return rerun::Vec3D
+ */
+rerun::Vec3D do_transform(
+  const geometry_msgs::msg::Pose & origin, const geometry_msgs::msg::Pose & pose)
+{
+  tf2::Quaternion oq;
+  oq.setValue(
+    origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w);
+
+  tf2::Vector3 op, pp;
+  op.setValue(origin.position.x, origin.position.y, origin.position.z);
+  pp.setValue(pose.position.x, pose.position.y, pose.position.z);
+
+  tf2::Matrix3x3 rotation(oq);
+
+  auto ret = rotation * pp + op;
+  return {static_cast<float>(ret.x()), static_cast<float>(ret.y()), static_cast<float>(ret.z())};
+}
+
+/**
+ * @brief Convert predicted paths to the sequence of waypoints.
+ *
+ * @param origin Pose origin.
+ * @param path Sequence of predicted poses.
+ * @return std::vector<rerun::Vec3D>
+ */
+std::vector<rerun::Vec3D> to_waypoints(
+  const geometry_msgs::msg::Pose & origin,
+  const autoware_perception_msgs::msg::PredictedPath & path)
+{
+  std::vector<rerun::Vec3D> waypoints;
+  for (const auto & pose : path.path) {
+    // waypoints.emplace_back(do_transform(origin, pose));
+    waypoints.emplace_back(rerun::Vec3D(pose.position.x, pose.position.y, pose.position.z));
+  }
+  return waypoints;
+}
+
+}  // namespace
+
+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::LineStrip3D> linestrips;
+  std::vector<rerun::components::ClassId> class_ids;
+  for (const auto & object : msg->objects) {
+    const auto & origin = object.kinematics.initial_pose_with_covariance.pose;
+    const auto class_id = static_cast<uint16_t>(object.classification.front().label);
+
+    for (const auto & path : object.kinematics.predicted_paths) {
+      const auto waypoints = to_waypoints(origin, path);
+      linestrips.emplace_back(rerun::LineStrip3D(waypoints));
+
+      class_ids.emplace_back(class_id);
+    }
+  }
+
+  stream_->log(entity_path.value(), rerun::LineStrips3D(linestrips).with_class_ids(class_ids));
+}
+}  // namespace awviz_plugin
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(awviz_plugin::PredictedObjectsDisplay, awviz_common::Display);

awviz_plugin/src/image/compressed_image_display.cpp

@@ -14,7 +14,7 @@
 
 #include "awviz_plugin/image/compressed_image_display.hpp"
 
-#include "awviz_plugin/image/collection_adapter.hpp"
+#include "awviz_plugin/collection_adapter.hpp"
 
 #include <opencv2/opencv.hpp>
 

awviz_plugin/src/image/image_display.cpp

@@ -14,7 +14,7 @@
 
 #include "awviz_plugin/image/image_display.hpp"
 
-#include "awviz_plugin/image/collection_adapter.hpp"
+#include "awviz_plugin/collection_adapter.hpp"
 
 #include <rerun.hpp>