feat(static_obstacle_avoidance): update envelope polygon creation method (autowarefoundation#8551)

* update envelope polygon by considering pose covariance

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

* change parameter

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* modify schema json

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

---------

Signed-off-by: Go Sakayori <gsakayori@gmail.com>
Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>

Author: go-sakayori

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml

@@ -28,6 +28,7 @@
             hard_margin_for_parked_vehicle: 0.7         # [m]
           max_expand_ratio: 0.0                         # [-] FOR DEVELOPER
           envelope_buffer_margin: 0.5                   # [m] FOR DEVELOPER
+          th_error_eclipse_long_radius : 0.6            # [m]
         truck:
           th_moving_speed: 1.0
           th_moving_time: 2.0
@@ -38,6 +39,7 @@
             hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
+          th_error_eclipse_long_radius : 0.6
         bus:
           th_moving_speed: 1.0
           th_moving_time: 2.0
@@ -48,6 +50,7 @@
             hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
+          th_error_eclipse_long_radius : 0.6
         trailer:
           th_moving_speed: 1.0
           th_moving_time: 2.0
@@ -58,6 +61,7 @@
             hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
+          th_error_eclipse_long_radius : 0.6
         unknown:
           th_moving_speed: 0.28
           th_moving_time: 1.0
@@ -68,6 +72,7 @@
             hard_margin_for_parked_vehicle: -0.2
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.1
+          th_error_eclipse_long_radius : 0.6
         bicycle:
           th_moving_speed: 0.28
           th_moving_time: 1.0
@@ -78,6 +83,7 @@
             hard_margin_for_parked_vehicle: 0.5
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
+          th_error_eclipse_long_radius : 0.6
         motorcycle:
           th_moving_speed: 1.0
           th_moving_time: 1.0
@@ -88,6 +94,7 @@
             hard_margin_for_parked_vehicle: 0.3
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
+          th_error_eclipse_long_radius : 0.6
         pedestrian:
           th_moving_speed: 0.28
           th_moving_time: 1.0
@@ -98,6 +105,7 @@
             hard_margin_for_parked_vehicle: 0.5
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
+          th_error_eclipse_long_radius : 0.6
         lower_distance_for_polygon_expansion: 30.0      # [m] FOR DEVELOPER
         upper_distance_for_polygon_expansion: 100.0     # [m] FOR DEVELOPER
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

@@ -90,6 +90,8 @@ struct ObjectParameter
   double lateral_hard_margin_for_parked_vehicle{1.0};
 
   double longitudinal_margin{0.0};
+
+  double th_error_eclipse_long_radius{0.0};
 };
 
 struct AvoidanceParameters
@@ -420,6 +422,9 @@ struct ObjectData  // avoidance target
   // to stop line distance
   double to_stop_line{std::numeric_limits<double>::infinity()};
 
+  // long radius of the covariance error ellipse
+  double error_eclipse_max{std::numeric_limits<double>::infinity()};
+
   // if lateral margin is NOT enough, the ego must avoid the object.
   bool avoid_required{false};
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp

@@ -69,6 +69,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       param.lateral_hard_margin_for_parked_vehicle =
         getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
       param.longitudinal_margin = getOrDeclareParameter<double>(*node, ns + "longitudinal_margin");
+      param.th_error_eclipse_long_radius =
+        getOrDeclareParameter<double>(*node, ns + "th_error_eclipse_long_radius");
       return param;
     };
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp

@@ -41,6 +41,7 @@ using tier4_planning_msgs::msg::PathWithLaneId;
 // ROS 2 general msgs
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseWithCovariance;
 using geometry_msgs::msg::TransformStamped;
 using geometry_msgs::msg::Vector3;
 using std_msgs::msg::ColorRGBA;

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp

@@ -174,6 +174,8 @@ double calcDistanceToAvoidStartLine(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+double calcErrorEclipseLongRadius(const PoseWithCovariance & pose);
+
 }  // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json

@@ -96,6 +96,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -104,7 +109,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -158,6 +164,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -166,7 +177,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -220,6 +232,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -228,7 +245,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -282,6 +300,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -290,7 +313,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -344,6 +368,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -352,7 +381,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -406,6 +436,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -414,7 +449,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -468,6 +504,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -476,7 +517,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },
@@ -530,6 +572,11 @@
                   "type": "number",
                   "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.",
                   "default": 0.0
+                },
+                "th_error_eclipse_long_radius": {
+                  "type": "number",
+                  "description": "This value will be applied to judge whether the eclipse error is to large",
+                  "default": 0.6
                 }
               },
               "required": [
@@ -538,7 +585,8 @@
                 "longitudinal_margin",
                 "lateral_margin",
                 "envelope_buffer_margin",
-                "max_expand_ratio"
+                "max_expand_ratio",
+                "th_error_eclipse_long_radius"
               ],
               "additionalProperties": false
             },

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp

@@ -60,6 +60,8 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
       parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle",
       config.lateral_hard_margin_for_parked_vehicle);
     updateParam<double>(parameters, ns + "longitudinal_margin", config.longitudinal_margin);
+    updateParam<double>(
+      parameters, ns + "th_error_eclipse_long_radius", config.th_error_eclipse_long_radius);
   };
 
   {

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

@@ -21,6 +21,7 @@
 #include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp"
 #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp"
 
+#include <Eigen/Dense>
 #include <lanelet2_extension/utility/message_conversion.hpp>
 
 #include <boost/geometry/algorithms/buffer.hpp>
@@ -1500,11 +1501,27 @@ void fillObjectEnvelopePolygon(
   if (same_id_obj == registered_objects.end()) {
     object_data.envelope_poly =
       createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
+    object_data.error_eclipse_max =
+      calcErrorEclipseLongRadius(object_data.object.kinematics.initial_pose_with_covariance);
     return;
   }
 
   const auto one_shot_envelope_poly =
     createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
+  const double error_eclipse_long_radius =
+    calcErrorEclipseLongRadius(object_data.object.kinematics.initial_pose_with_covariance);
+
+  if (error_eclipse_long_radius > object_parameter.th_error_eclipse_long_radius) {
+    if (error_eclipse_long_radius < object_data.error_eclipse_max) {
+      object_data.error_eclipse_max = error_eclipse_long_radius;
+      object_data.envelope_poly = one_shot_envelope_poly;
+      return;
+    }
+    object_data.envelope_poly = same_id_obj->envelope_poly;
+    return;
+  }
+
+  object_data.error_eclipse_max = error_eclipse_long_radius;
 
   // If the one_shot_envelope_poly is within the registered envelope, use the registered one
   if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) {
@@ -2512,4 +2529,18 @@ double calcDistanceToReturnDeadLine(
 
   return distance_to_return_dead_line;
 }
+
+double calcErrorEclipseLongRadius(const PoseWithCovariance & pose)
+{
+  Eigen::Matrix2d xy_covariance;
+  const auto cov = pose.covariance;
+  xy_covariance(0, 0) = cov[0 * 6 + 0];
+  xy_covariance(0, 1) = cov[0 * 6 + 1];
+  xy_covariance(1, 0) = cov[1 * 6 + 0];
+  xy_covariance(1, 1) = cov[1 * 6 + 1];
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
+
+  return std::sqrt(eigensolver.eigenvalues()(1));
+}
 }  // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance