Merge branch 'autowarefoundation:main' into main

Author: isouf

.cspell-partial.json

@@ -5,5 +5,5 @@
     "perception/bytetrack/lib/**"
   ],
   "ignoreRegExpList": [],
-  "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"]
+  "words": ["dltype", "tvmgen"]
 }

.github/CODEOWNERS

@@ -36,6 +36,9 @@ jobs:
         with:
           fetch-depth: 0
 
+      - name: Check disk space before build
+        run: df -h
+
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 
@@ -69,6 +72,9 @@ jobs:
           verbose: true
           flags: differential
 
+      - name: Check disk space after build
+        run: df -h
+
   clang-tidy-differential:
     runs-on: [self-hosted, linux, X64]
     container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda

.github/workflows/build-and-test-differential.yaml

@@ -5,6 +5,7 @@ on:
     paths:
       - "**/schema/*.schema.json"
       - "**/config/*.param.yaml"
+  workflow_dispatch:
 
 jobs:
   json-schema-check:

.github/workflows/json-schema-check.yaml

@@ -5,8 +5,6 @@
   <version>0.0.0</version>
   <description>The autoware_ad_api_specs package</description>
   <maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
-  <maintainer email="makoto.yabuta@tier4.jp">yabuta</maintainer>
-  <maintainer email="kahhooi.tan@tier4.jp">Kah Hooi Tan</maintainer>
   <maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer>
   <license>Apache License 2.0</license>
 

.github/workflows/spell-check-differential.yaml

@@ -17,6 +17,8 @@
 
 #include "autoware_auto_perception_rviz_plugin/visibility_control.hpp"
 
+#include <Eigen/Core>
+#include <Eigen/Eigen>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -113,8 +115,14 @@ get_uuid_marker_ptr(
   const std_msgs::msg::ColorRGBA & color_rgba);
 
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
-get_pose_with_covariance_marker_ptr(
-  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance);
+get_pose_covariance_marker_ptr(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+  const double & confidence_interval_coefficient);
+
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_yaw_covariance_marker_ptr(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length,
+  const double & confidence_interval_coefficient, const double & line_width);
 
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_velocity_text_marker_ptr(
@@ -131,6 +139,23 @@ get_twist_marker_ptr(
   const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
   const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_twist_covariance_marker_ptr(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
+  const double & confidence_interval_coefficient);
+
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_yaw_rate_marker_ptr(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width);
+
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_yaw_rate_covariance_marker_ptr(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+  const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
+  const double & confidence_interval_coefficient, const double & line_width);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_predicted_path_marker_ptr(
   const autoware_auto_perception_msgs::msg::Shape & shape,
@@ -142,10 +167,17 @@ get_path_confidence_marker_ptr(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const std_msgs::msg::ColorRGBA & path_confidence_color);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip(
+  const double start_angle, const double end_angle, const double radius,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points(
   const double point_list[][3], const int point_pairs[][2], const int & num_pairs,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors(
+  const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
@@ -154,6 +186,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_lin
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
@@ -162,6 +198,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_dir
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list(
+  const autoware_auto_perception_msgs::msg::Shape & shape,
+  std::vector<geometry_msgs::msg::Point> & points);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list(
   const autoware_auto_perception_msgs::msg::Shape & shape,
   std::vector<geometry_msgs::msg::Point> & points);

common/autoware_ad_api_specs/package.xml

@@ -64,14 +64,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display 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_pose_with_covariance_property{
-      "Display PoseWithCovariance", true, "Enable/disable pose with covariance visualization",
-      this},
     m_display_velocity_text_property{
       "Display Velocity", true, "Enable/disable velocity text visualization", this},
     m_display_acceleration_text_property{
       "Display Acceleration", true, "Enable/disable acceleration text visualization", this},
+    m_display_pose_covariance_property{
+      "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this},
+    m_display_yaw_covariance_property{
+      "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this},
     m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this},
+    m_display_twist_covariance_property{
+      "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this},
+    m_display_yaw_rate_property{
+      "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this},
+    m_display_yaw_rate_covariance_property{
+      "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization",
+      this},
     m_display_predicted_paths_property{
       "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this},
     m_display_path_confidence_property{
@@ -96,6 +104,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
       "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this);
     m_simple_visualize_mode_property->addOption("Normal", 0);
     m_simple_visualize_mode_property->addOption("Simple", 1);
+    // Confidence interval property
+    m_confidence_interval_property = new rviz_common::properties::EnumProperty(
+      "Confidence Interval", "95%", "Confidence interval of state estimations.", this);
+    m_confidence_interval_property->addOption("70%", 0);
+    m_confidence_interval_property->addOption("85%", 1);
+    m_confidence_interval_property->addOption("95%", 2);
+    m_confidence_interval_property->addOption("99%", 3);
 
     // iterate over default values to create and initialize the properties.
     for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
@@ -238,11 +253,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     }
   }
 
-  std::optional<Marker::SharedPtr> get_pose_with_covariance_marker_ptr(
+  std::optional<Marker::SharedPtr> get_pose_covariance_marker_ptr(
     const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const
   {
-    if (m_display_pose_with_covariance_property.getBool()) {
-      return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance);
+    if (m_display_pose_covariance_property.getBool()) {
+      return detail::get_pose_covariance_marker_ptr(pose_with_covariance, get_confidence_region());
+    } else {
+      return std::nullopt;
+    }
+  }
+
+  std::optional<Marker::SharedPtr> get_yaw_covariance_marker_ptr(
+    const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length,
+    const double & line_width) const
+  {
+    if (m_display_yaw_covariance_property.getBool()) {
+      return detail::get_yaw_covariance_marker_ptr(
+        pose_with_covariance, length, get_confidence_interval(), line_width);
     } else {
       return std::nullopt;
     }
@@ -286,6 +313,44 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     }
   }
 
+  std::optional<Marker::SharedPtr> get_twist_covariance_marker_ptr(
+    const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+    const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const
+  {
+    if (m_display_twist_covariance_property.getBool()) {
+      return detail::get_twist_covariance_marker_ptr(
+        pose_with_covariance, twist_with_covariance, get_confidence_region());
+    } else {
+      return std::nullopt;
+    }
+  }
+
+  std::optional<Marker::SharedPtr> get_yaw_rate_marker_ptr(
+    const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+    const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
+    const double & line_width) const
+  {
+    if (m_display_yaw_rate_property.getBool()) {
+      return detail::get_yaw_rate_marker_ptr(
+        pose_with_covariance, twist_with_covariance, line_width);
+    } else {
+      return std::nullopt;
+    }
+  }
+
+  std::optional<Marker::SharedPtr> get_yaw_rate_covariance_marker_ptr(
+    const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
+    const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance,
+    const double & line_width) const
+  {
+    if (m_display_yaw_rate_covariance_property.getBool()) {
+      return detail::get_yaw_rate_covariance_marker_ptr(
+        pose_with_covariance, twist_with_covariance, get_confidence_interval(), line_width);
+    } else {
+      return std::nullopt;
+    }
+  }
+
   std::optional<Marker::SharedPtr> get_predicted_path_marker_ptr(
     const unique_identifier_msgs::msg::UUID & uuid,
     const autoware_auto_perception_msgs::msg::Shape & shape,
@@ -408,6 +473,46 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 
   double get_line_width() { return m_line_width_property.getFloat(); }
 
+  double get_confidence_interval() const
+  {
+    switch (m_confidence_interval_property->getOptionInt()) {
+      case 0:
+        // 70%
+        return 1.036;
+      case 1:
+        // 85%
+        return 1.440;
+      case 2:
+        // 95%
+        return 1.960;
+      case 3:
+        // 99%
+        return 2.576;
+      default:
+        return 1.960;
+    }
+  }
+
+  double get_confidence_region() const
+  {
+    switch (m_confidence_interval_property->getOptionInt()) {
+      case 0:
+        // 70%
+        return 1.552;
+      case 1:
+        // 85%
+        return 1.802;
+      case 2:
+        // 95%
+        return 2.448;
+      case 3:
+        // 99%
+        return 3.035;
+      default:
+        return 2.448;
+    }
+  }
+
 private:
   // All rviz plugins should have this. Should be initialized with pointer to this class
   MarkerCommon m_marker_common;
@@ -419,18 +524,28 @@ class AUTOWARE_AUTO_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 set confidence interval of state estimations
+  rviz_common::properties::EnumProperty * m_confidence_interval_property;
   // Property to enable/disable label visualization
   rviz_common::properties::BoolProperty m_display_label_property;
   // Property to enable/disable uuid visualization
   rviz_common::properties::BoolProperty m_display_uuid_property;
-  // Property to enable/disable pose with covariance visualization
-  rviz_common::properties::BoolProperty m_display_pose_with_covariance_property;
   // Property to enable/disable velocity text visualization
   rviz_common::properties::BoolProperty m_display_velocity_text_property;
   // Property to enable/disable acceleration text visualization
   rviz_common::properties::BoolProperty m_display_acceleration_text_property;
+  // Property to enable/disable pose with covariance visualization
+  rviz_common::properties::BoolProperty m_display_pose_covariance_property;
+  // Property to enable/disable yaw covariance visualization
+  rviz_common::properties::BoolProperty m_display_yaw_covariance_property;
   // Property to enable/disable twist visualization
   rviz_common::properties::BoolProperty m_display_twist_property;
+  // Property to enable/disable twist covariance visualization
+  rviz_common::properties::BoolProperty m_display_twist_covariance_property;
+  // Property to enable/disable yaw rate visualization
+  rviz_common::properties::BoolProperty m_display_yaw_rate_property;
+  // Property to enable/disable yaw rate covariance visualization
+  rviz_common::properties::BoolProperty m_display_yaw_rate_covariance_property;
   // Property to enable/disable predicted paths visualization
   rviz_common::properties::BoolProperty m_display_predicted_paths_property;
   // Property to enable/disable predicted path confidence visualization

common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

@@ -8,7 +8,8 @@
   <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
   <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
   <maintainer email="takeshi.miura@tier4.jp">Takeshi Miura</maintainer>
-
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
+  <maintainer email="yoshi.ri@tier4.jp">Yoshi Ri</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>