From a567bb2177b00c2c48c2d12587d838727b731460 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 27 Jul 2023 13:45:29 +0900 Subject: [PATCH] refactor(avoidance): use common safety checker (#4388) * refactor(avoidance): use common safety checker Signed-off-by: satoshi-ota * docs(avoidance): rename params Signed-off-by: satoshi-ota * chore(avoidance): rename variables Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 21 +- .../behavior_path_planner_avoidance_design.md | 44 +- .../avoidance/avoidance_module.hpp | 61 +-- .../utils/avoidance/avoidance_module_data.hpp | 37 +- .../utils/avoidance/utils.hpp | 21 + .../avoidance/avoidance_module.cpp | 377 ++---------------- .../src/scene_module/avoidance/manager.cpp | 20 +- .../src/utils/avoidance/utils.cpp | 177 ++++++++ 8 files changed, 295 insertions(+), 463 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 3a36fcea62e41..3b9165194a3bf 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -13,7 +13,6 @@ enable_bound_clipping: false enable_update_path_when_object_is_gone: false enable_force_avoidance_for_stopped_vehicle: false - enable_safety_check: true enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false disable_path_update: false @@ -133,10 +132,18 @@ # For safety check safety_check: + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] + time_resolution: 0.5 # [s] + time_horizon: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] @@ -186,12 +193,6 @@ max_jerk: 1.0 # [m/sss] max_acceleration: 1.0 # [m/ss] - target_velocity_matrix: - col_size: 2 - matrix: - [2.78, 13.9, # velocity [m/s] - 0.50, 1.00] # margin [m] - shift_line_pipeline: trim: quantize_filter_threshold: 0.2 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 7436c26533000..9be99f705f560 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -485,15 +485,22 @@ The hatched road marking is defined on Lanelet map. See [here](https://github.co ### Safety check -The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable_safety_check` as `true`. +The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable` as `true`. ```yaml -enable_safety_check: false - -# For safety check +# safety check configuration +enable: true # [-] +check_current_lane: false # [-] +check_shift_side_lane: true # [-] +check_other_side_lane: false # [-] +check_unavoidable_object: false # [-] +check_other_object: true # [-] + +# collision check parameters +check_all_predicted_path: false # [-] +time_horizon: 10.0 # [s] +idling_time: 1.5 # [s] safety_check_backward_distance: 50.0 # [m] -safety_check_time_horizon: 10.0 # [s] -safety_check_idling_time: 1.5 # [s] safety_check_accel_for_rss: 2.5 # [m/ss] ``` @@ -621,12 +628,9 @@ namespace: `avoidance.` | detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | | detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | | enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_safety_check | [-] | bool | Flag to enable safety check. | false | | enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | | enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | -**NOTE:** It has to set both `enable_safety_check` and `enable_yield_maneuver` to enable yield maneuver. - | Name | Unit | Type | Description | Default value | | :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | | enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | @@ -695,14 +699,20 @@ namespace: `avoidance.target_filtering.` namespace: `avoidance.safety_check.` -| Name | Unit | Type | Description | Default value | -| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | -| safety_check_time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | -| safety_check_idling_time | [t] | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5 | -| safety_check_accel_for_rss | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS. | 2.5 | -| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | -| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | +| enable | [-] | bool | Enable to use safety check feature. | true | +| check_current_lane | [-] | bool | Check objects on current driving lane. | false | +| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true | +| check_other_side_lane | [-] | bool | Check objects on other side lane. | false | +| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false | +| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false | +| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false | +| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | +| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 | +| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | +| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | +| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | ### Avoidance maneuver parameters diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index ee81915ea7944..743f0b3b1b804 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" #include @@ -256,12 +257,6 @@ class AvoidanceModule : public SceneModuleInterface ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; - /** - * @brief get objects that are driving on adjacent lanes. - * @param left or right lanelets. - */ - ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const; - /** * @brief fill additional data so that the module judges target objects. * @param avoidance data. @@ -481,65 +476,13 @@ class AvoidanceModule : public SceneModuleInterface // safety check - /** - * @brief get shift side adjacent lanes. - * @param path shifter. - * @param forward distance to get. - * @param backward distance to get. - * @return adjacent lanes. - */ - lanelet::ConstLanelets getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const; - - /** - * @brief calculate lateral margin from avoidance velocity for safety check. - * @param target velocity. - */ - double getLateralMarginFromVelocity(const double velocity) const; - - /** - * @brief calculate rss longitudinal distance for safety check. - * @param ego velocity. - * @param object velocity. - * @param option for rss calculation. - * @return rss longitudinal distance. - */ - double getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const; - - /** - * @brief check avoidance path safety for surround moving objects. - * @param path shifter. - * @param avoidance path. - * @param debug data. - * @return result. - */ - bool isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const; - /** * @brief check avoidance path safety for surround moving objects. * @param avoidance path. - * @param shift side lanes. * @param debug data. * @return result. */ - bool isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, - DebugData & debug) const; - - /** - * @brief check predicted points safety. - * @param predicted points. - * @param future time. - * @param object data. - * @param margin data for debug. - * @return result. - */ - bool isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const; + bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; // post process diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 3c01ad20bcebd..b590d452ae03e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -46,6 +47,8 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using marker_utils::CollisionCheckDebug; + struct ObjectParameter { bool is_target{false}; @@ -95,9 +98,6 @@ struct AvoidanceParameters // enable avoidance for all parking vehicle bool enable_force_avoidance_for_stopped_vehicle{false}; - // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver - bool enable_safety_check{false}; - // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -177,20 +177,23 @@ struct AvoidanceParameters // for longitudinal direction double longitudinal_collision_margin_time; - // find adjacent lane vehicles - double safety_check_backward_distance; - - // minimum longitudinal margin for vehicles in adjacent lane - double safety_check_min_longitudinal_margin; + // parameters for safety check area + bool enable_safety_check{false}; + bool check_current_lane{false}; + bool check_shift_side_lane{false}; + bool check_other_side_lane{false}; - // safety check time horizon - double safety_check_time_horizon; + // parameters for safety check target. + bool check_unavoidable_object{false}; + bool check_other_object{false}; - // use in RSS calculation - double safety_check_idling_time; + // parameters for collision check. + bool check_all_predicted_path{false}; + double safety_check_time_horizon{0.0}; + double safety_check_time_resolution{0.0}; - // use in RSS calculation - double safety_check_accel_for_rss; + // find adjacent lane vehicles + double safety_check_backward_distance; // transit hysteresis (unsafe to safe) double safety_check_hysteresis_factor; @@ -280,12 +283,6 @@ struct AvoidanceParameters // Maximum lateral acceleration limitation map. std::vector lateral_max_accel_map; - // target velocity matrix - std::vector target_velocity_matrix; - - // matrix col size - size_t col_size; - // parameters depend on object class std::unordered_map object_parameters; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 69803e4cc4dec..8c229b4f26949 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" +#include #include #include #include @@ -25,6 +27,10 @@ namespace behavior_path_planner::utils::avoidance { using behavior_path_planner::PlannerData; +using behavior_path_planner::utils::safety_check::ExtendedPredictedObject; +using behavior_path_planner::utils::safety_check::PoseWithVelocityAndPolygonStamped; +using behavior_path_planner::utils::safety_check::PoseWithVelocityStamped; +using behavior_path_planner::utils::safety_check::PredictedPathWithPolygon; bool isOnRight(const ObjectData & obj); @@ -79,11 +85,19 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); + lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset); @@ -139,6 +153,13 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters); + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 34977b0933019..06ebd6e01b48b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -44,6 +44,7 @@ namespace behavior_path_planner { +using marker_utils::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -465,7 +466,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * Check avoidance path safety. For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.safe = isSafePath(path_shifter, data.candidate_path, debug); + data.safe = isSafePath(data.candidate_path, debug); } void AvoidanceModule::fillEgoStatus( @@ -1832,371 +1833,55 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) } bool AvoidanceModule::isSafePath( - const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const + ShiftedPath & shifted_path, [[maybe_unused]] DebugData & debug) const { - const auto & p = parameters_; + const auto & p = planner_data_->parameters; - if (!p->enable_safety_check) { + if (!parameters_->enable_safety_check) { return true; // if safety check is disabled, it always return safe. } - const auto & forward_check_distance = p->object_check_forward_distance; - const auto & backward_check_distance = p->safety_check_backward_distance; - const auto check_lanes = - getAdjacentLane(path_shifter, forward_check_distance, backward_check_distance); - - auto path_with_current_velocity = shifted_path.path; - - const size_t ego_idx = planner_data_->findEgoIndex(path_with_current_velocity.points); - utils::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0); - - constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38; // 5km/h - for (auto & p : path_with_current_velocity.points) { - p.point.longitudinal_velocity_mps = std::max(getEgoSpeed(), MIN_EGO_VEL_IN_PREDICTION); - } - - { - debug_data_.path_with_planned_velocity = path_with_current_velocity; - } - - return isSafePath(path_with_current_velocity, check_lanes, debug); -} - -bool AvoidanceModule::isSafePath( - const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes, DebugData & debug) const -{ - if (path.points.empty()) { - return true; - } - - const auto path_with_time = [&path]() { - std::vector> ret{}; - - float travel_time = 0.0; - ret.emplace_back(path.points.front(), travel_time); + const auto ego_predicted_path = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); - for (size_t i = 1; i < path.points.size(); ++i) { - const auto & p1 = path.points.at(i - 1); - const auto & p2 = path.points.at(i); + const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + const auto is_right_shift = [&]() -> std::optional { + for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { + const auto length = shifted_path.shift_length.at(i); - const auto v = std::max(p1.point.longitudinal_velocity_mps, float{1.0}); - const auto ds = calcDistance2d(p1, p2); - - travel_time += ds / v; - - ret.emplace_back(p2, travel_time); - } - - return ret; - }(); - - const auto move_objects = getAdjacentLaneObjects(check_lanes); - - { - debug.unsafe_objects.clear(); - debug.margin_data_array.clear(); - debug.exist_adjacent_objects = !move_objects.empty(); - } - - bool is_safe = true; - for (const auto & p : path_with_time) { - MarginData margin_data{}; - margin_data.pose = getPose(p.first); - - if (p.second > parameters_->safety_check_time_horizon) { - break; - } - - for (const auto & o : move_objects) { - const auto is_enough_margin = isEnoughMargin(p.first, p.second, o, margin_data); - - if (!is_enough_margin) { - debug.unsafe_objects.push_back(o); + if (parameters_->lateral_avoid_check_threshold < length) { + return false; } - is_safe = is_safe && is_enough_margin; - } - - debug.margin_data_array.push_back(margin_data); - } - - return is_safe; -} - -bool AvoidanceModule::isEnoughMargin( - const PathPointWithLaneId & p_ego, const double t, const ObjectData & object, - MarginData & margin_data) const -{ - const auto & common_param = planner_data_->parameters; - const auto & vehicle_width = common_param.vehicle_width; - const auto & base_link2front = common_param.base_link2front; - const auto & base_link2rear = common_param.base_link2rear; - - const auto p_ref = [this, &p_ego]() { - const auto idx = findNearestIndex(avoidance_data_.reference_path.points, getPoint(p_ego)); - return getPose(avoidance_data_.reference_path.points.at(idx)); - }(); - - const auto & v_ego = p_ego.point.longitudinal_velocity_mps; - const auto & v_ego_lon = utils::avoidance::getLongitudinalVelocity(p_ref, getPose(p_ego), v_ego); - const auto & v_obj = object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - - if (!utils::avoidance::isTargetObjectType(object.object, parameters_)) { - return true; - } - - // | centerline - // | ^ x - // | +-------+ | - // | | | | - // | | | D1 | D2 D4 - // | | obj |<-->|<---------->|<->| - // | | | D3 | +-------+ - // | | |<----------->| | - // | +-------+ | | | - // | | | ego | - // | | | | - // | | | | - // | | +-------+ - // | y <----+ - // D1: overhang_dist (signed value) - // D2: shift_length (signed value) - // D3: lateral_distance (should be larger than margin that's calculated from relative velocity.) - // D4: vehicle_width (unsigned value) - - const auto reliable_path = std::max_element( - object.object.kinematics.predicted_paths.begin(), - object.object.kinematics.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); - - if (reliable_path == object.object.kinematics.predicted_paths.end()) { - return true; - } - - const auto p_obj = [&t, &reliable_path]() { - boost::optional ret{boost::none}; - - const auto dt = rclcpp::Duration(reliable_path->time_step).seconds(); - const auto idx = static_cast(std::floor(t / dt)); - const auto res = t - dt * idx; - - if (idx > reliable_path->path.size() - 2) { - return ret; + if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { + return true; + } } - const auto & p_src = reliable_path->path.at(idx); - const auto & p_dst = reliable_path->path.at(idx + 1); - ret = calcInterpolatedPose(p_src, p_dst, res / dt); - return ret; + return std::nullopt; }(); - if (!p_obj) { - return true; - } - - const auto v_obj_lon = utils::avoidance::getLongitudinalVelocity(p_ref, p_obj.get(), v_obj); - - double hysteresis_factor = 1.0; - if (avoidance_data_.state == AvoidanceState::YIELD) { - hysteresis_factor = parameters_->safety_check_hysteresis_factor; - } - - const auto shift_length = calcLateralDeviation(p_ref, getPoint(p_ego)); - const auto lateral_distance = std::abs(object.overhang_dist - shift_length) - 0.5 * vehicle_width; - const auto lateral_margin = getLateralMarginFromVelocity(std::abs(v_ego_lon - v_obj_lon)); - - if (lateral_distance > lateral_margin * hysteresis_factor) { - return true; - } - - const auto lon_deviation = calcLongitudinalDeviation(getPose(p_ego), p_obj.get().position); - const auto is_front_object = lon_deviation > 0.0; - const auto longitudinal_margin = - getRSSLongitudinalDistance(v_ego_lon, v_obj_lon, is_front_object); - const auto vehicle_offset = is_front_object ? base_link2front : base_link2rear; - const auto longitudinal_distance = - std::abs(lon_deviation) - vehicle_offset - 0.5 * object.object.shape.dimensions.x; - - { - margin_data.pose.orientation = p_ref.orientation; - margin_data.enough_lateral_margin = false; - margin_data.longitudinal_distance = - std::min(margin_data.longitudinal_distance, longitudinal_distance); - margin_data.longitudinal_margin = - std::max(margin_data.longitudinal_margin, longitudinal_margin); - margin_data.vehicle_width = vehicle_width; - margin_data.base_link2front = base_link2front; - margin_data.base_link2rear = base_link2rear; - } - - if (longitudinal_distance > longitudinal_margin * hysteresis_factor) { + if (!is_right_shift.has_value()) { return true; } - return false; -} - -double AvoidanceModule::getLateralMarginFromVelocity(const double velocity) const -{ - const auto & p = parameters_; - - if (p->col_size < 2 || p->col_size * 2 != p->target_velocity_matrix.size()) { - throw std::logic_error("invalid matrix col size."); - } - - if (velocity < p->target_velocity_matrix.front()) { - return p->target_velocity_matrix.at(p->col_size); - } - - if (velocity > p->target_velocity_matrix.at(p->col_size - 1)) { - return p->target_velocity_matrix.back(); - } - - for (size_t i = 1; i < p->col_size; ++i) { - if (velocity < p->target_velocity_matrix.at(i)) { - const auto v1 = p->target_velocity_matrix.at(i - 1); - const auto v2 = p->target_velocity_matrix.at(i); - const auto m1 = p->target_velocity_matrix.at(i - 1 + p->col_size); - const auto m2 = p->target_velocity_matrix.at(i + p->col_size); - - const auto v_clamp = std::clamp(velocity, v1, v2); - return m1 + (m2 - m1) * (v_clamp - v1) / (v2 - v1); - } - } - - return p->target_velocity_matrix.back(); -} + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( + avoidance_data_, planner_data_, parameters_, is_right_shift.value()); -double AvoidanceModule::getRSSLongitudinalDistance( - const double v_ego, const double v_obj, const bool is_front_object) const -{ - const auto & accel_for_rss = parameters_->safety_check_accel_for_rss; - const auto & idling_time = parameters_->safety_check_idling_time; - - const auto opposite_lane_vehicle = v_obj < 0.0; - - /** - * object and ego already pass each other. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (!is_front_object && opposite_lane_vehicle) { - return 0.0; - } - - /** - * object drive opposite direction. - * ======================================= - * Ego--> - * --------------------------------------- - * <--Obj - * ======================================= - */ - if (is_front_object && opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) + - std::abs(v_obj) * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is in front of ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (is_front_object && !opposite_lane_vehicle) { - return v_ego * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_ego + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_obj, 2.0) / (2.0 * accel_for_rss); - } - - /** - * object is behind ego, and drive same direction. - * ======================================= - * Ego--> - * --------------------------------------- - * Obj--> - * ======================================= - */ - if (!is_front_object && !opposite_lane_vehicle) { - return v_obj * idling_time + 0.5 * accel_for_rss * std::pow(idling_time, 2.0) + - std::pow(v_obj + accel_for_rss * idling_time, 2.0) / (2.0 * accel_for_rss) - - std::pow(v_ego, 2.0) / (2.0 * accel_for_rss); - } - - return 0.0; -} - -lanelet::ConstLanelets AvoidanceModule::getAdjacentLane( - const PathShifter & path_shifter, const double forward_distance, - const double backward_distance) const -{ - const auto & rh = planner_data_->route_handler; - - bool has_left_shift = false; - bool has_right_shift = false; - - for (const auto & sp : path_shifter.getShiftLines()) { - if (sp.end_shift_length > 0.01) { - has_left_shift = true; - continue; - } - - if (sp.end_shift_length < -0.01) { - has_right_shift = true; - continue; - } - } - - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - - const auto ego_succeeding_lanes = - rh->getLaneletSequence(current_lane, getEgoPose(), backward_distance, forward_distance); - - lanelet::ConstLanelets check_lanes{}; - for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (has_left_shift && opt_left_lane) { - check_lanes.push_back(opt_left_lane.get()); - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (has_right_shift && opt_right_lane) { - check_lanes.push_back(opt_right_lane.get()); - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (has_right_shift && !right_opposite_lanes.empty()) { - check_lanes.push_back(right_opposite_lanes.front()); - } - } - - return check_lanes; -} - -ObjectDataArray AvoidanceModule::getAdjacentLaneObjects( - const lanelet::ConstLanelets & adjacent_lanes) const -{ - ObjectDataArray objects; - for (const auto & o : avoidance_data_.other_objects) { - if (utils::avoidance::isCentroidWithinLanelets(o.object, adjacent_lanes)) { - objects.push_back(o); + for (const auto & object : safety_check_target_objects) { + const auto obj_predicted_paths = + utils::getPredictedPathFromObj(object, parameters_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::safety_check::checkCollision( + shifted_path.path, ego_predicted_path, object, obj_path, p, + p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + return false; + } } } - return objects; + return true; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 1ba7c588c84d6..29b3535331547 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -59,7 +59,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "enable_update_path_when_object_is_gone"); p.enable_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "enable_force_avoidance_for_stopped_vehicle"); - p.enable_safety_check = get_parameter(node, ns + "enable_safety_check"); p.enable_yield_maneuver = get_parameter(node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); @@ -140,11 +139,17 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check { std::string ns = "avoidance.safety_check."; + p.enable_safety_check = get_parameter(node, ns + "enable"); + p.check_current_lane = get_parameter(node, ns + "check_current_lane"); + p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); + p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); + p.check_other_object = get_parameter(node, ns + "check_other_object"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); + p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_time_horizon = get_parameter(node, ns + "safety_check_time_horizon"); - p.safety_check_idling_time = get_parameter(node, ns + "safety_check_idling_time"); - p.safety_check_accel_for_rss = get_parameter(node, ns + "safety_check_accel_for_rss"); p.safety_check_hysteresis_factor = get_parameter(node, ns + "safety_check_hysteresis_factor"); p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); @@ -227,13 +232,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( } } - // velocity matrix - { - std::string ns = "avoidance.target_velocity_matrix."; - p.col_size = get_parameter(node, ns + "col_size"); - p.target_velocity_matrix = get_parameter>(node, ns + "matrix"); - } - // shift line pipeline { std::string ns = "avoidance.shift_line_pipeline."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d3c31d688557d..58beaf9cf3a87 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1367,4 +1367,181 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } + +std::vector convertToPredictedPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (path.points.empty()) { + return {}; + } + + const auto & acceleration = parameters->max_acceleration; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); + + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const double velocity = + std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); + const double length = initial_velocity * t + 0.5 * acceleration * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +ExtendedPredictedObject transform( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_resolution = parameters->safety_check_time_resolution; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths.at(i); + extended_object.predicted_paths.at(i).confidence = path.confidence; + + // create path + for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { + const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths.at(i).path.emplace_back( + t, *obj_pose, obj_velocity, obj_polygon); + } + } + } + + return extended_object; +} + +lanelet::ConstLanelets getAdjacentLane( + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & rh = planner_data->route_handler; + const auto & forward_distance = parameters->object_check_forward_distance; + const auto & backward_distance = parameters->safety_check_backward_distance; + const auto & vehicle_pose = planner_data->self_odometry->pose.pose; + + lanelet::ConstLanelet current_lane; + if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Satoshi Ota) + } + + const auto ego_succeeding_lanes = + rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); + + lanelet::ConstLanelets lanes{}; + for (const auto & lane : ego_succeeding_lanes) { + const auto opt_left_lane = rh->getLeftLanelet(lane); + if (!is_right_shift && opt_left_lane) { + lanes.push_back(opt_left_lane.get()); + } + + const auto opt_right_lane = rh->getRightLanelet(lane); + if (is_right_shift && opt_right_lane) { + lanes.push_back(opt_right_lane.get()); + } + + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); + if (is_right_shift && !right_opposite_lanes.empty()) { + lanes.push_back(right_opposite_lanes.front()); + } + } + + return lanes; +} + +std::vector getSafetyCheckTargetObjects( + const AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters, const bool is_right_shift) +{ + const auto & p = parameters; + const auto check_right_lanes = + (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + const auto check_left_lanes = + (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + + std::vector target_objects; + + const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { + std::for_each(objects.begin(), objects.end(), [&](const auto & object) { + if (isCentroidWithinLanelets(object.object, check_lanes)) { + target_objects.push_back(utils::avoidance::transform(object.object, p)); + } + }); + }; + + const auto unavoidable_objects = [&data]() { + ObjectDataArray ret; + std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) { + ret.push_back(object); + } + }); + return ret; + }(); + + // check right lanes + if (check_right_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, true); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check left lanes + if (check_left_lanes) { + const auto check_lanes = getAdjacentLane(planner_data, p, false); + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + // check current lanes + if (p->check_current_lane) { + const auto check_lanes = data.current_lanelets; + + if (p->check_other_object) { + append_target_objects(check_lanes, data.other_objects); + } + + if (p->check_unavoidable_object) { + append_target_objects(check_lanes, unavoidable_objects); + } + } + + return target_objects; +} } // namespace behavior_path_planner::utils::avoidance