[pull] main from autowarefoundation:main

common/autoware_auto_perception_rviz_plugin/CMakeLists.txt

@@ -13,18 +13,18 @@ set(OD_PLUGIN_LIB_SRC
 )
 
 set(OD_PLUGIN_LIB_HEADERS
-  include/visibility_control.hpp
+  include/autoware_auto_perception_rviz_plugin/visibility_control.hpp
 )
 set(OD_PLUGIN_LIB_HEADERS_TO_WRAP
-  include/object_detection/detected_objects_display.hpp
-  include/object_detection/tracked_objects_display.hpp
-  include/object_detection/predicted_objects_display.hpp
+  include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp
+  include/autoware_auto_perception_rviz_plugin/object_detection/tracked_objects_display.hpp
+  include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp
 )
 
 set(COMMON_HEADERS
-  include/common/color_alpha_property.hpp
-  include/object_detection/object_polygon_detail.hpp
-  include/object_detection/object_polygon_display_base.hpp
+  include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp
+  include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp
+  include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp
 )
 
 set(COMMON_SRC

common/autoware_auto_perception_rviz_plugin/include/common/color_alpha_property.hpp → common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp

@@ -11,13 +11,14 @@
 // 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 COMMON__COLOR_ALPHA_PROPERTY_HPP_
-#define COMMON__COLOR_ALPHA_PROPERTY_HPP_
+#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_
+#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_
+
+#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp"
 
 #include <rviz_common/display.hpp>
 #include <rviz_common/properties/color_property.hpp>
 #include <rviz_common/properties/float_property.hpp>
-#include <visibility_control.hpp>
 
 #include <std_msgs/msg/color_rgba.hpp>
 
@@ -55,4 +56,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ColorAlphaProperty
 }  // namespace rviz_plugins
 }  // namespace autoware
 
-#endif  // COMMON__COLOR_ALPHA_PROPERTY_HPP_
+#endif  // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__COMMON__COLOR_ALPHA_PROPERTY_HPP_

common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp → common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/detected_objects_display.hpp

@@ -11,10 +11,10 @@
 // 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 OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
-#define OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
+#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
+#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
 
-#include <object_detection/object_polygon_display_base.hpp>
+#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp"
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 
@@ -43,4 +43,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay
 }  // namespace rviz_plugins
 }  // namespace autoware
 
-#endif  // OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_
+#endif  // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__DETECTED_OBJECTS_DISPLAY_HPP_

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

@@ -12,12 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 /// \brief This file defines some helper functions used by ObjectPolygonDisplayBase class
-#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
-#define OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
+#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
+#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
 
+#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp"
+
+#include <Eigen/Core>
+#include <Eigen/Eigen>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <visibility_control.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_object.hpp>
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
@@ -112,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(
@@ -130,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,
@@ -141,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);
@@ -252,4 +285,4 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC
 }  // namespace rviz_plugins
 }  // namespace autoware
 
-#endif  // OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_
+#endif  // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DETAIL_HPP_

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp → common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp

@@ -11,19 +11,19 @@
 // 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 OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
-#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
+#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
+#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
 
-#include "common/color_alpha_property.hpp"
+#include "autoware_auto_perception_rviz_plugin/common/color_alpha_property.hpp"
+#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp"
+#include "autoware_auto_perception_rviz_plugin/visibility_control.hpp"
 
-#include <object_detection/object_polygon_detail.hpp>
 #include <rviz_common/display.hpp>
 #include <rviz_common/properties/color_property.hpp>
 #include <rviz_common/properties/enum_property.hpp>
 #include <rviz_common/properties/float_property.hpp>
 #include <rviz_default_plugins/displays/marker/marker_common.hpp>
 #include <rviz_default_plugins/displays/marker_array/marker_array_display.hpp>
-#include <visibility_control.hpp>
 
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
@@ -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
@@ -449,4 +564,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
 }  // namespace rviz_plugins
 }  // namespace autoware
 
-#endif  // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
+#endif  // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_

common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp → common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/predicted_objects_display.hpp

@@ -11,10 +11,10 @@
 // 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 OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
-#define OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
+#ifndef AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
+#define AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
 
-#include <object_detection/object_polygon_display_base.hpp>
+#include "autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp"
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 
@@ -153,4 +153,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
 }  // namespace rviz_plugins
 }  // namespace autoware
 
-#endif  // OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_
+#endif  // AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__PREDICTED_OBJECTS_DISPLAY_HPP_