feat(static_obstacle_avoidance): update envelope polygon creation met…

…hod (autowarefoundation#8551) * update envelope polygon by considering pose covariance Signed-off-by: Go Sakayori <[email protected]> * change parameter Signed-off-by: Go Sakayori <[email protected]> * modify schema json Signed-off-by: Go Sakayori <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp Co-authored-by: Satoshi OTA <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]> Signed-off-by: Go Sakayori <[email protected]> Co-authored-by: Satoshi OTA <[email protected]>
tier4 · Sep 3, 2024 · 0710343 · 0710343
1 parent 5d7305b
commit 0710343
Show file tree

Hide file tree

Showing 8 changed files with 107 additions and 8 deletions.
diff --git a/...ehavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/...ehavior_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
 

diff --git a/...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/...e_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};
 

diff --git a/...dule/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/...dule/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;
     };
 

diff --git a/...nce_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/...nce_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;

diff --git a/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/...voidance_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_
diff --git a/...havior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/...havior_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
             },

diff --git a/...vior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/...vior_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);
   };
 
   {

diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/...havior_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