diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 1be3e04914cda..3e155aba0af2e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/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/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 915d256aba1db..53e06631a81d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_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/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2ac0b9493e67a..6caeb411f6680 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2319,8 +2319,10 @@ std::pair 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 index 901fe351fc68b..b079db9babf31 100644 --- 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(safety_check_ns + "method"); p.safety_check_params.hysteresis_factor_expand_rate = node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = @@ -778,6 +780,9 @@ void GoalPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "hysteresis_factor_expand_rate", p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); updateParam( 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 index a0870fe428c8a..5790be377b7aa 100644 --- 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/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 1ab33514c5f24..d2f695071649a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_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/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index e2afb6abeec03..ffd2754acc38f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_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 index 61ff54e8f99f3..22c93f7d4bfd5 100644 --- 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(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.collision_check_yaw_diff_threshold = getOrDeclareParameter( + *node, parameter("safety_check.collision_check_yaw_diff_threshold")); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *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 index 505bb3ef93340..fd96701fe088e 100644 --- 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 #include +#include #include #include #include @@ -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/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md index 4733fe7832da6..5511202c390b1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md @@ -48,7 +48,7 @@ $$ 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 @@ -56,7 +56,7 @@ In this step, we compute extended ego and target object polygons. The extended p ![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/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 313f9df471938..0e7d1cee65f02 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -23,6 +23,7 @@ #include +#include #include #include #include @@ -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/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index 3f1094dfd70b6..d0d19b6b8fed2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -98,7 +99,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & 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 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 & polys_1, const std::vector & polys_2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 7bb8c13aa65fb..b05d869cca6b5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -26,6 +26,9 @@ #include #include +#include + +#include #include namespace autoware::behavior_path_planner::utils::path_safety_checker @@ -482,7 +485,8 @@ bool checkSafetyWithRSS( const std::vector & ego_predicted_path, const std::vector & 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::max(), debug); + rss_parameters, hysteresis_factor, std::numeric_limits::max(), yaw_difference_th, + debug); return collided_polygons.empty(); } @@ -573,7 +578,8 @@ std::vector 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 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"; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md index 066708891652a..dba4a0cfce5e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/README.md @@ -436,17 +436,18 @@ Parameters under `target_filtering` are related to filtering target objects for Parameters under `safety_check_params` define the configuration for safety check. -| Name | Unit | Type | Description | Default value | -| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | -| enable_safety_check | - | bool | Flag to enable safety check | true | -| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | -| publish_debug_marker | - | bool | Flag to publish debug markers | false | -| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | -| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | -| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | -| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | -| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | -| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :---------------------------------------------------------------------------------------- | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | +| collision_check_yaw_diff_threshold | - | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 1.578 | ## **Path Generation** diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml index ce7ead87b54c7..d39a320a19e14 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml @@ -143,8 +143,10 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 1.578 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 7554098414442..b07d6497463ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -77,7 +77,7 @@ struct PullOutStatus // record if the ego has departed from the start point bool has_departed{false}; - PullOutStatus() {} + PullOutStatus() = default; }; class StartPlannerModule : public SceneModuleInterface @@ -90,7 +90,7 @@ class StartPlannerModule : public SceneModuleInterface std::unordered_map> & objects_of_interest_marker_interface_ptr_map); - ~StartPlannerModule() + ~StartPlannerModule() override { if (freespace_planner_timer_) { freespace_planner_timer_->cancel(); @@ -296,16 +296,16 @@ class StartPlannerModule : public SceneModuleInterface PathWithLaneId getCurrentPath() const; void planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority); + const Pose & goal_pose, const std::string & search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( - const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, - const double velocity_threshold, const double object_check_backward_distance, - const double object_check_forward_distance) const; + const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, + const double velocity_threshold, const double object_check_forward_distance, + const double object_check_backward_distance) const; bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasReachedFreespaceEnd() const; bool hasReachedPullOutEnd() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 9451bb3870648..873bab1043485 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -283,6 +283,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(safety_check_ns + "forward_path_length"); p.safety_check_params.publish_debug_marker = node->declare_parameter(safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); } // RSSparams @@ -298,6 +300,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); p.safety_check_params.rss_params.longitudinal_velocity_delta_time = node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + node->declare_parameter(rss_ns + "extended_polygon_policy"); } // surround moving obstacle check @@ -367,7 +371,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "extra_width_margin_for_rear_obstacle", p->extra_width_margin_for_rear_obstacle); - updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( @@ -658,6 +661,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, safety_check_ns + "publish_debug_marker", p->safety_check_params.publish_debug_marker); + updateParam( + parameters, safety_check_ns + "collision_check_yaw_diff_threshold", + p->safety_check_params.collision_check_yaw_diff_threshold); } const std::string rss_ns = safety_check_ns + "rss_params."; { @@ -676,6 +682,9 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, rss_ns + "longitudinal_velocity_delta_time", p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + updateParam( + parameters, rss_ns + "extended_polygon_policy", + p->safety_check_params.rss_params.extended_polygon_policy); } std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 9400f3ff40286..08d5ea4365575 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -34,7 +34,9 @@ #include #include +#include #include +#include #include #include #include @@ -65,7 +67,7 @@ StartPlannerModule::StartPlannerModule( { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params{}; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; @@ -104,7 +106,7 @@ void StartPlannerModule::onFreespacePlannerTimer() std::optional current_status_opt{std::nullopt}; std::optional parameters_opt{std::nullopt}; std::optional pull_out_status_opt{std::nullopt}; - bool is_stopped; + bool is_stopped{false}; // making a local copy of thread sensitive data { @@ -501,7 +503,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const // Get the closest target obj width in the relevant lanes const auto closest_object_width = std::invoke([&]() -> std::optional { double arc_length_to_closet_object = std::numeric_limits::max(); - double closest_object_width = -1.0; + std::optional closest_object_width = std::nullopt; std::for_each( target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { @@ -512,7 +514,6 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const arc_length_to_closet_object = arc_length; closest_object_width = o.shape.dimensions.y; }); - if (closest_object_width < 0.0) return std::nullopt; return closest_object_width; }); if (!closest_object_width) return false; @@ -837,7 +838,7 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, - const Pose & goal_pose, const std::string search_priority) + const Pose & goal_pose, const std::string & search_priority) { if (start_pose_candidates.empty()) return; @@ -1112,8 +1113,8 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() waitApproval(); // To enable approval of the forward path, the RTC status is removed. removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); + for (auto & itr : uuid_map_) { + itr.second = generateUUID(); } } @@ -1303,7 +1304,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() return ignore_signal_.value() == id; }; - const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) { return is_ignore ? std::make_optional(id) : std::nullopt; }; @@ -1422,10 +1423,12 @@ bool StartPlannerModule::isSafePath() const merged_target_object.insert( merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), target_objects_on_lane.on_shoulder_lane.end()); + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, - objects_filtering_params_->use_all_predicted_path, hysteresis_factor); + objects_filtering_params_->use_all_predicted_path, hysteresis_factor, + safety_check_params_->collision_check_yaw_diff_threshold); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1693,7 +1696,7 @@ void StartPlannerModule::setDebugData() // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (debug_data_.ego_predicted_path.size() > 0) { + if (!debug_data_.ego_predicted_path.empty()) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add( @@ -1702,7 +1705,7 @@ void StartPlannerModule::setDebugData() debug_marker_); } - if (debug_data_.filtered_objects.objects.size() > 0) { + if (!debug_data_.filtered_objects.objects.empty()) { add( createObjectsMarkerArray( debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9), diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 324be9a73439e..80d2128bf70e0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -174,6 +174,7 @@ safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index f05230f4ec19d..dbedbfe1f70cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -223,6 +223,8 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + double collision_check_yaw_diff_threshold{3.1416}; + bool consider_front_overhang{true}; bool consider_rear_overhang{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index b761939589968..a79a67d5fdd4a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -199,6 +199,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); p.hysteresis_factor_safe_count = getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + p.collision_check_yaw_diff_threshold = + getOrDeclareParameter(*node, ns + "collision_check_yaw_diff_threshold"); } // safety check predicted path params diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 266a4cf8a23ab..1e0753a288a28 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -927,6 +927,11 @@ "description": "Hysteresis count that be used for chattering prevention.", "default": 10 }, + "collision_check_yaw_diff_threshold": { + "type": "number", + "description": "Max yaw difference between ego and object when doing collision check", + "default": 3.1416 + }, "min_velocity": { "type": "number", "description": "Minimum velocity of the ego vehicle's predicted path.", diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index b0fd1b5c1b8ab..824466a1ad3ad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -781,7 +781,8 @@ bool StaticObstacleAvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, current_debug_data.second)) { + hysteresis_factor, parameters_->collision_check_yaw_diff_threshold, + current_debug_data.second)) { utils::path_safety_checker::updateCollisionCheckDebugMap( debug.collision_check, current_debug_data, false);