diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4db2390e5d91c..ef1775f314bbd 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -433,16 +433,17 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral #### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | -| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | false | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | +| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | +| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | ### **geometric pull out** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 2ff17bc508e89..5d82fbbfdf401 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -13,6 +13,7 @@ # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: true + allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 563856c87db70..5b41edaee0da8 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -72,6 +72,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + bool allow_check_shift_path_lane_departure_override{false}; double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index aaf711b855e7a..1368124929367 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -178,6 +178,21 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; + uint16_t getSteeringFactorDirection( + const behavior_path_planner::BehaviorModuleOutput & output) const + { + switch (output.turn_signal_info.turn_signal.command) { + case TurnIndicatorsCommand::ENABLE_LEFT: + return SteeringFactor::LEFT; + + case TurnIndicatorsCommand::ENABLE_RIGHT: + return SteeringFactor::RIGHT; + + default: + return SteeringFactor::STRAIGHT; + } + }; + /** * @brief Check if there are no moving objects around within a certain radius. * diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index bd96a095eb767..f921df83120ae 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -54,6 +54,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); p.shift_collision_check_distance_from_end = node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = @@ -390,6 +392,9 @@ void StartPlannerModuleManager::updateModuleParams( p->geometric_collision_check_distance_from_end); updateParam( parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); + updateParam( + parameters, ns + "allow_check_shift_path_lane_departure_override", + p->allow_check_shift_path_lane_departure_override); updateParam(parameters, ns + "search_priority", p->search_priority); updateParam(parameters, ns + "max_back_distance", p->max_back_distance); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index d21a68048e34f..f5674cfb288d0 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -74,13 +74,27 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // check lane departure - // The method for lane departure checking verifies if the footprint of each point on the path is - // contained within a lanelet using `boost::geometry::within`, which incurs a high computational - // cost. const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + // if lane departure check override is true, and if the initial pose is not fully within a lane, + // cancel lane departure check + const bool is_lane_departure_check_required = std::invoke([&]() -> bool { + if (!parameters_.allow_check_shift_path_lane_departure_override) + return parameters_.check_shift_path_lane_departure; + + PathWithLaneId path_with_only_first_pose{}; + path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0)); + return !lane_departure_checker_->checkPathWillLeaveLane( + lanelet_map_ptr, path_with_only_first_pose); + }); + + // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path + // is contained within a lanelet using `boost::geometry::within`, which incurs a high + // computational cost. + if ( - parameters_.check_shift_path_lane_departure && + is_lane_departure_check_required && lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) { continue; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index ed81f39135181..04c10a3d18c86 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -182,18 +182,14 @@ void StartPlannerModule::updateData() status_.first_engaged_time = clock_->now(); } - if (hasFinishedBackwardDriving()) { + status_.backward_driving_complete = hasFinishedBackwardDriving(); + if (status_.backward_driving_complete) { updateStatusAfterBackwardDriving(); DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); - } else { - status_.backward_driving_complete = false; } - if (requiresDynamicObjectsCollisionDetection()) { - status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); - } else { - status_.is_safe_dynamic_objects = true; - } + status_.is_safe_dynamic_objects = + (!requiresDynamicObjectsCollisionDetection()) ? true : !hasCollisionWithDynamicObjects(); } bool StartPlannerModule::hasFinishedBackwardDriving() const @@ -364,20 +360,17 @@ bool StartPlannerModule::isStopped() bool StartPlannerModule::isExecutionReady() const { - bool is_safe = true; // Evaluate safety. The situation is not safe if any of the following conditions are met: // 1. pull out path has not been found // 2. there is a moving objects around ego // 3. waiting for approval and there is a collision with dynamic objects - if (!status_.found_pull_out_path) { - is_safe = false; - } else if (isWaitingApproval()) { - if (!noMovingObjectsAround()) { - is_safe = false; - } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) { - is_safe = false; - } - } + + const bool is_safe = [&]() -> bool { + if (!status_.found_pull_out_path) return false; + if (!isWaitingApproval()) return true; + if (!noMovingObjectsAround()) return false; + return !(requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()); + }(); if (!is_safe) { stop_pose_ = planner_data_->self_odometry->pose.pose; @@ -459,14 +452,7 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( @@ -480,15 +466,16 @@ BehaviorModuleOutput StartPlannerModule::plan() {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - const double distance = motion_utils::calcSignedArcLength( - path.points, planner_data_->self_odometry->pose.pose.position, - status_.pull_out_path.start_pose.position); - updateRTCStatus(0.0, distance); - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); + setDebugData(); + return output; } + const double distance = motion_utils::calcSignedArcLength( + path.points, planner_data_->self_odometry->pose.pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); setDebugData(); @@ -569,14 +556,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( @@ -590,15 +570,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); - } else { - const double distance = motion_utils::calcSignedArcLength( - stop_path.points, planner_data_->self_odometry->pose.pose.position, - status_.pull_out_path.start_pose.position); - updateRTCStatus(0.0, distance); - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); + setDebugData(); + + return output; } + const double distance = motion_utils::calcSignedArcLength( + stop_path.points, planner_data_->self_odometry->pose.pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); setDebugData(); @@ -658,16 +640,20 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( order_priority.emplace_back(i, planner); } } - } else if (search_priority == "short_back_distance") { + return order_priority; + } + + if (search_priority == "short_back_distance") { for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } } - } else { - RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); - throw std::domain_error("[start_planner] invalid search_priority"); + return order_priority; } + + RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); + throw std::domain_error("[start_planner] invalid search_priority"); return order_priority; } @@ -1099,12 +1085,7 @@ bool StartPlannerModule::isStuck() return false; } - if (status_.planner_type == PlannerType::STOP) { - return true; - } - - // not found safe path - if (!status_.found_pull_out_path) { + if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) { return true; } @@ -1197,30 +1178,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return false; }); - if (is_near_intersection) { - // offset required end pose with ration to activate turn signal for intersection - turn_signal.required_end_point = std::invoke([&]() { - const double length_start_to_end = - motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); - const auto ratio = std::clamp( - parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); - - const double required_end_length = length_start_to_end * ratio; - double accumulated_length = 0.0; - const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - for (size_t i = start_idx; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > required_end_length) { - return path.points.at(i).point.pose; - } + turn_signal.required_end_point = std::invoke([&]() { + if (is_near_intersection) return end_pose; + const double length_start_to_end = + motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); + const auto ratio = std::clamp( + parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); + + const double required_end_length = length_start_to_end * ratio; + double accumulated_length = 0.0; + const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); + for (size_t i = start_idx; i < path.points.size() - 1; ++i) { + accumulated_length += + tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (accumulated_length > required_end_length) { + return path.points.at(i).point.pose; } - // not found required end point - return end_pose; - }); - } else { - turn_signal.required_end_point = end_pose; - } + } + // not found required end point + return end_pose; + }); return turn_signal; } @@ -1389,22 +1366,27 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.planner_type == PlannerType::FREESPACE) { - const double drivable_area_margin = planner_data_->parameters.vehicle_width; - output.drivable_area_info.drivable_margin = - planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - } else { - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - output.path, generateDrivableLanes(output.path), - planner_data_->drivable_area_expansion_parameters); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = - status_.driving_forward - ? utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) - : current_drivable_area_info; + switch (status_.planner_type) { + case PlannerType::FREESPACE: { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + break; + } + default: { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + output.path, generateDrivableLanes(output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = + status_.driving_forward + ? utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) + : current_drivable_area_info; + break; + } } }