feat(start_planner): yaw threshold for rss check (autowarefoundation#…

…7657) * add param to customize yaw th Signed-off-by: Daniel Sanchez <[email protected]> * add param to other modules Signed-off-by: Daniel Sanchez <[email protected]> * docs Signed-off-by: Daniel Sanchez <[email protected]> * update READMEs with params Signed-off-by: Daniel Sanchez <[email protected]> * fix LC README Signed-off-by: Daniel Sanchez <[email protected]> * use normalized yaw diff Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]>
TaikiYamada4 · Jun 27, 2024 · c129187 · c129187
1 parent 23d857d
commit c129187
Show file tree

Hide file tree

Showing 23 changed files with 114 additions and 51 deletions.
diff --git a/...ning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/...ning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
@@ -382,13 +382,14 @@ In addition, the safety check has a time hysteresis, and if the path is judged "
 
 ### Parameters for safety check
 
-| Name                     | Unit | Type   | Description                                                                                              | Default value                |
-| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- |
-| enable_safety_check      | [-]  | bool   | flag whether to use safety check                                                                         | true                         |
-| method                   | [-]  | string | method for safety check. `RSS` or `integral_predicted_polygon`                                           | `integral_predicted_polygon` |
-| keep_unsafe_time         | [s]  | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0                          |
-| check_all_predicted_path | -    | bool   | Flag to check all predicted paths                                                                        | true                         |
-| publish_debug_marker     | -    | bool   | Flag to publish debug markers                                                                            | false                        |
+| Name                                 | Unit  | Type   | Description                                                                                              | Default value                |
+| :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- |
+| enable_safety_check                  | [-]   | bool   | flag whether to use safety check                                                                         | true                         |
+| method                               | [-]   | string | method for safety check. `RSS` or `integral_predicted_polygon`                                           | `integral_predicted_polygon` |
+| keep_unsafe_time                     | [s]   | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0                          |
+| check_all_predicted_path             | -     | bool   | Flag to check all predicted paths                                                                        | true                         |
+| publish_debug_marker                 | -     | bool   | Flag to publish debug markers                                                                            | false                        |
+| `collision_check_yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking                | 3.1416                       |
 
 #### Parameters for RSS safety check
 

diff --git a/...or_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/...or_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
@@ -180,6 +180,7 @@
             time_horizon: 10.0
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
+          collision_check_yaw_diff_threshold: 3.1416
           # temporary
           backward_path_length: 100.0
           forward_path_length: 100.0

diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -2319,8 +2319,10 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath(
       return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
         pull_over_path, ego_predicted_path, filtered_objects, collision_check,
         planner_data->parameters, safety_check_params->rss_params,
-        objects_filtering_params->use_all_predicted_path, hysteresis_factor);
-    } else if (parameters.safety_check_params.method == "integral_predicted_polygon") {
+        objects_filtering_params->use_all_predicted_path, hysteresis_factor,
+        safety_check_params->collision_check_yaw_diff_threshold);
+    }
+    if (parameters.safety_check_params.method == "integral_predicted_polygon") {
       return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
         ego_predicted_path, vehicle_info_, filtered_objects,
         objects_filtering_params->check_all_predicted_path,

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
@@ -357,6 +357,8 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
     p.safety_check_params.method = node->declare_parameter<std::string>(safety_check_ns + "method");
     p.safety_check_params.hysteresis_factor_expand_rate =
       node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
+    p.safety_check_params.collision_check_yaw_diff_threshold =
+      node->declare_parameter<double>(safety_check_ns + "collision_check_yaw_diff_threshold");
     p.safety_check_params.backward_path_length =
       node->declare_parameter<double>(safety_check_ns + "backward_path_length");
     p.safety_check_params.forward_path_length =
@@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, safety_check_ns + "hysteresis_factor_expand_rate",
       p->safety_check_params.hysteresis_factor_expand_rate);
+    updateParam<double>(
+      parameters, safety_check_ns + "collision_check_yaw_diff_threshold",
+      p->safety_check_params.collision_check_yaw_diff_threshold);
     updateParam<double>(
       parameters, safety_check_ns + "backward_path_length",
       p->safety_check_params.backward_path_length);

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
@@ -725,6 +725,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi
 | `check_objects_on_current_lanes`                         | [-]   | boolean | If true, the lane change module check objects on current lanes when performing collision assessment.                                                                                                       | false         |
 | `check_objects_on_other_lanes`                           | [-]   | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment                                                                                                       | false         |
 | `use_all_predicted_path`                                 | [-]   | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                                                                     | true          |
+| `safety_check.collision_check_yaw_diff_threshold`        | [rad] | double  | Maximum yaw difference between ego and object when executing rss-based collision checking                                                                                                                  | 3.1416        |
 
 #### safety constraints during lane change path is computed
 

diff --git a/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -30,6 +30,7 @@
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true
+        collision_check_yaw_diff_threshold: 3.1416
         execution:
           expected_front_deceleration: -1.0
           expected_rear_deceleration: -1.0

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -137,6 +137,7 @@ struct LaneChangeParameters
 
   // safety check
   bool allow_loose_check_for_cancel{true};
+  double collision_check_yaw_diff_threshold{3.1416};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_abort{};
   utils::path_safety_checker::RSSparams rss_params_for_stuck{};

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp
@@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   // safety check
   p.allow_loose_check_for_cancel =
     getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
+  p.collision_check_yaw_diff_threshold = getOrDeclareParameter<double>(
+    *node, parameter("safety_check.collision_check_yaw_diff_threshold"));
 
   p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.execution.longitudinal_distance_min_threshold"));

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -30,6 +30,7 @@
 #include <lanelet2_core/geometry/Polygon.h>
 
 #include <algorithm>
+#include <cmath>
 #include <limits>
 #include <memory>
 #include <utility>
@@ -2048,6 +2049,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     lane_change_parameters_->lane_expansion_left_offset,
     lane_change_parameters_->lane_expansion_right_offset);
 
+  constexpr double collision_check_yaw_diff_threshold{M_PI};
+
   for (const auto & obj : collision_check_objects) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
@@ -2056,7 +2059,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     for (const auto & obj_path : obj_predicted_paths) {
       const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
         path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
-        get_max_velocity_for_safety_check(), current_debug_data.second);
+        get_max_velocity_for_safety_check(), collision_check_yaw_diff_threshold,
+        current_debug_data.second);
 
       if (collided_polygons.empty()) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(

diff --git a/...utoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/...utoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md
@@ -48,15 +48,15 @@ $$
 rss_{dist} = v_{rear} (t_{reaction} + t_{margin}) + \frac{v_{rear}^2}{2|a_{rear, decel}|} - \frac{v_{front}^2}{2|a_{front, decel|}}
 $$
 
-where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration.
+where $V_{front}$, $v_{rear}$ are front and rear vehicle velocity respectively and $a_{rear, front}$, $a_{rear, decel}$ are front and rear vehicle deceleration. Note that RSS distance is normally used for objects traveling in the same direction, if the yaw difference between a given ego pose and object pose is more than a user-defined yaw difference threshold, the rss collision check will be skipped for that specific pair of poses.
 
 #### 5. Create extended ego and target object polygons
 
 In this step, we compute extended ego and target object polygons. The extended polygons can be described as:
 
 ![extended_polygons](../images/path_safety_checker/extended_polygons.drawio.svg)
 
-As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin
+As the picture shows, we expand the rear object polygon. For the longitudinal side, we extend it with the RSS distance, and for the lateral side, we extend it by the lateral margin.
 
 #### 6. Check overlap
 

diff --git a/...behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/...behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp
@@ -23,6 +23,7 @@
 
 #include <boost/uuid/uuid_hash.hpp>
 
+#include <cmath>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -203,7 +204,9 @@ struct SafetyCheckParams
   /// possible values: ["RSS", "integral_predicted_polygon"]
   double keep_unsafe_time{0.0};  ///< Time to keep unsafe before changing to safe.
   double hysteresis_factor_expand_rate{
-    0.0};                            ///< Hysteresis factor to expand/shrink polygon with the value.
+    0.0};  ///< Hysteresis factor to expand/shrink polygon with the value.
+  double collision_check_yaw_diff_threshold{
+    3.1416};                         ///< threshold yaw difference between ego and object.
   double backward_path_length{0.0};  ///< Length of the backward lane for path generation.
   double forward_path_length{0.0};   ///< Length of the forward path lane for path generation.
   RSSparams rss_params{};            ///< Parameters related to the RSS model.

diff --git a/.../include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/.../include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
@@ -26,6 +26,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 
+#include <cmath>
 #include <vector>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -98,7 +99,8 @@ bool checkSafetyWithRSS(
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & debug_map,
   const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params,
-  const bool check_all_predicted_path, const double hysteresis_factor);
+  const bool check_all_predicted_path, const double hysteresis_factor,
+  const double yaw_difference_th);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -111,6 +113,8 @@ bool checkSafetyWithRSS(
  * @param common_parameters The common parameters used in behavior path planner.
  * @param front_object_deceleration The deceleration of the object in the front.(used in RSS)
  * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS)
+ * @param yaw_difference_th maximum yaw difference between any given ego path pose and object
+ * predicted path pose.
  * @param debug The debug information for collision checking.
  * @return true if distance is safe.
  */
@@ -120,7 +124,7 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug);
+  const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and
@@ -142,7 +146,8 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug);
+  const double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
+  CollisionCheckDebug & debug);
 
 bool checkPolygonsIntersects(
   const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);

diff --git a/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
@@ -26,6 +26,9 @@
 #include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
 
+#include <tf2/utils.h>
+
+#include <cmath>
 #include <limits>
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker
@@ -482,7 +485,8 @@ bool checkSafetyWithRSS(
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
   const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & debug_map,
   const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params,
-  const bool check_all_predicted_path, const double hysteresis_factor)
+  const bool check_all_predicted_path, const double hysteresis_factor,
+  const double yaw_difference_th)
 {
   // Check for collisions with each predicted path of the object
   const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) {
@@ -495,7 +499,7 @@ bool checkSafetyWithRSS(
       obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) {
         const bool has_collision = !utils::path_safety_checker::checkCollision(
           planned_path, ego_predicted_path, object, obj_path, parameters, rss_params,
-          hysteresis_factor, current_debug_data.second);
+          hysteresis_factor, yaw_difference_th, current_debug_data.second);
 
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug_map, current_debug_data, !has_collision);
@@ -559,11 +563,12 @@ bool checkCollision(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug)
+  const double hysteresis_factor, const double yaw_difference_th, CollisionCheckDebug & debug)
 {
   const auto collided_polygons = getCollidedPolygons(
     planned_path, predicted_ego_path, target_object, target_object_path, common_parameters,
-    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), debug);
+    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), yaw_difference_th,
+    debug);
   return collided_polygons.empty();
 }
 
@@ -573,7 +578,8 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug)
+  double hysteresis_factor, const double max_velocity_limit, const double yaw_difference_th,
+  CollisionCheckDebug & debug)
 {
   {
     debug.ego_predicted_path = predicted_ego_path;
@@ -604,6 +610,11 @@ std::vector<Polygon2d> getCollidedPolygons(
     const auto & ego_polygon = interpolated_data->poly;
     const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit);
 
+    const double ego_yaw = tf2::getYaw(ego_pose.orientation);
+    const double object_yaw = tf2::getYaw(obj_pose.orientation);
+    const double yaw_difference = autoware::universe_utils::normalizeRadian(ego_yaw - object_yaw);
+    if (std::abs(yaw_difference) > yaw_difference_th) continue;
+
     // check overlap
     if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {
       debug.unsafe_reason = "overlap_polygon";