feat(autoware_perception_rviz_plugin): object mesh marker visualization (#32)

* Add meshes with license

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Modify cmake

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Modify code for mesh, vehicle light and prediction with offset visualization

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Update license and readme

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Update license reference in readme

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Update license in package.xml

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Fix casting style

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* Add missing import

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* style(pre-commit): autofix

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* style(pre-commit): autofix

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* style(pre-commit): autofix

* Ignore certain spell checks

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>

* fix skipping of cspell check for LICENSE file

Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>

---------

Signed-off-by: Jonas Krug <jonas.krug@dai-labor.de>
Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Jonas Krug <jonas.krug@dai-labor.de>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>

Author: 4 people

autoware_perception_rviz_plugin/CMakeLists.txt

@@ -67,4 +67,5 @@ pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
 ament_auto_package(
   INSTALL_TO_SHARE
   icons
+  meshes
 )

autoware_perception_rviz_plugin/README.md

@@ -55,6 +55,22 @@ Overwrite prediction results with tracking results.
 
 ![predicted-object-visualization-description](./images/predicted-object-visualization-description.jpg)
 
+### Mesh Visualization
+
+For all object types mentioned above it is possible to additionally visualize a mesh file for each object based on its class. For this the mesh namespace can be activated in RViz.
+For cars the visualization dynamically switches between a sedan and SUV model, based on the ratio between height and length.
+
+#### Visualization Result
+
+Visualization of only the mesh files for different vehicle types, pedestrians and cyclists.
+
+![predicted-object-visualization-description](./images/mesh-object-visualization-description.jpg)
+
+##### Customizable Features
+
+- Set visualization offset for predicted objects (n steps in to the future)
+- Visualize vehicle lights (brake, left indicator, right indicator)
+
 ### TrafficLight
 
 #### Input Types
@@ -83,6 +99,12 @@ The plugin visualizes traffic light states in two ways:
 - Timeout for stale data
 - Topic names for both input types
 
+## Resources / License
+
+<!-- cspell:ignore Preuße sebastianpreusse -->
+
+The 3D models in the 'meshes' directory are provided by Sebastian Preuße ([LinkedIn](https://www.linkedin.com/in/sebastianpreusse), [sebastianpreusse.com](sebastianpreusse.com)) and are licensed under [Creative Commons Attribution-ShareAlike 4.0 International License](https://creativecommons.org/licenses/by-sa/4.0/legalcode.en).
+
 ## References/External links
 
 [1] <https://gitlab.com/autowarefoundation/autoware.auto/AutowareAuto/-/tree/master/src/tools/visualization/autoware_rviz_plugins>

autoware_perception_rviz_plugin/images/mesh-object-visualization-description.jpg

@@ -31,6 +31,7 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
 #include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <algorithm>
 #include <map>
@@ -98,6 +99,18 @@ get_2d_shape_marker_ptr(
   const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
   const bool & is_orientation_available = true);
 
+AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_mesh_marker_ptr(
+  const autoware_perception_msgs::msg::Shape & shape_msg,
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification);
+
+AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::MarkerArray::SharedPtr
+get_indicator_marker_ptr(
+  const autoware_perception_msgs::msg::Shape & shape_msg,
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const std::vector<autoware_perception_msgs::msg::ObjectClassification> & classification);
+
 /// \brief Convert the given polygon into a marker representing the shape in 3d
 /// \param centroid Centroid position of the shape in Object.header.frame_id frame
 /// \return Marker ptr. Id and header will have to be set by the caller
@@ -277,9 +290,10 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC
   autoware_perception_msgs::msg::ObjectClassification::_label_type
   get_best_label(ClassificationContainerT labels, const std::string & logger_name)
 {
-  const auto best_class_label = std::max_element(
-    labels.begin(), labels.end(),
-    [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; });
+  const auto best_class_label =
+    std::max_element(labels.begin(), labels.end(), [](const auto & a, const auto & b) -> bool {
+      return a.label >= 100 || (a.probability < b.probability && b.label < 100);
+    });  // label > 100 is reserved for status labels
   if (best_class_label == labels.end()) {
     RCLCPP_WARN(
       rclcpp::get_logger(logger_name),

autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -52,6 +52,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 public:
   using Color = std::array<float, 3U>;
   using Marker = visualization_msgs::msg::Marker;
+  using MarkerArray = visualization_msgs::msg::MarkerArray;
   using MarkerCommon = rviz_default_plugins::displays::MarkerCommon;
   using ObjectClassificationMsg = autoware_perception_msgs::msg::ObjectClassification;
   using RosTopicDisplay = rviz_common::RosTopicDisplay<MsgT>;
@@ -62,6 +63,10 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   explicit ObjectPolygonDisplayBase(const std::string & default_topic)
   : m_marker_common(this),
     // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this},
+    m_display_mesh_property{
+      "Display Mesh", false, "Enable/disable mesh visualization of the object", this},
+    m_display_indicator_property{
+      "Display Indicator", false, "Enable/disable indicator visualization of the object", this},
     m_display_label_property{"Display Label", true, "Enable/disable label visualization", this},
     m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this},
     m_display_velocity_text_property{
@@ -217,6 +222,37 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
     const bool & is_orientation_available);
 
+  template <typename ClassificationContainerT>
+  std::optional<Marker::SharedPtr> get_mesh_marker_ptr(
+    const autoware_perception_msgs::msg::Shape & shape_msg,
+    const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+    const ClassificationContainerT & labels) const
+  {
+    if (m_display_mesh_property.getBool()) {
+      auto marker_ptr = detail::get_mesh_marker_ptr(shape_msg, centroid, orientation, labels);
+      if (marker_ptr) {
+        return marker_ptr;
+      }
+    }
+    return std::nullopt;
+  }
+
+  template <typename ClassificationContainerT>
+  std::optional<MarkerArray::SharedPtr> get_indicator_marker_ptr(
+    const autoware_perception_msgs::msg::Shape & shape_msg,
+    const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+    const ClassificationContainerT & labels) const
+  {
+    if (m_display_indicator_property.getBool()) {
+      static const std::string kLoggerName("ObjectPolygonDisplayBase");
+      auto marker_ptr = detail::get_indicator_marker_ptr(shape_msg, centroid, orientation, labels);
+      if (marker_ptr) {
+        return marker_ptr;
+      }
+    }
+    return std::nullopt;
+  }
+
   /// \brief Convert given shape msg into a Marker to visualize label name
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg
   /// \param centroid Centroid position of the shape in Object.header.frame_id frame
@@ -534,6 +570,10 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   rviz_common::properties::EnumProperty * m_display_type_property;
   // Property to choose simplicity of visualization polygon
   rviz_common::properties::EnumProperty * m_simple_visualize_mode_property;
+  // Property to enable/disable mesh visualization of the object
+  rviz_common::properties::BoolProperty m_display_mesh_property;
+  // Property to enable/disable mesh visualization of the object
+  rviz_common::properties::BoolProperty m_display_indicator_property;
   // Property to set confidence interval of state estimations
   rviz_common::properties::EnumProperty * m_confidence_interval_property;
   // Property to set visualization type

autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp

@@ -16,6 +16,8 @@
 
 #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp"
 
+#include <rviz_common/properties/int_property.hpp>
+
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 
 #include <boost/functional/hash.hpp>
@@ -127,6 +129,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
 
   void update(float wall_dt, float ros_dt) override;
 
+  rviz_common::properties::IntProperty m_offset_property;
+
   std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
   // std::unordered_map<boost::uuids::uuid, int32_t> id_map;
   std::list<int32_t> unused_marker_ids;