From 7b0469d581e9240b8a14d85ad32dec9d5e5c9e57 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 1 Dec 2023 02:12:58 +0900 Subject: [PATCH 1/4] refactor(start_planner): refactor debug and safety check logic This commit refactors the start_planner module in several ways: - The verbose parameter is removed from the YAML configuration file and replaced with a print_debug_info parameter under a new debug section. - A new function, initVariables(), is introduced to initialize member variables, which is called during processOnEntry() and processOnExit(). - The function isBackwardDrivingComplete() is renamed to hasFinishedBackwardDriving() to better reflect its purpose. It also includes additional debug prints. - A new function, checkCollisionWithDynamicObjects(), is introduced to encapsulate the logic for checking collisions with dynamic objects. - The function isExecutionReady() is modified to use the new checkCollisionWithDynamicObjects() function. - The function resetStatus() is simplified by directly assigning an empty PullOutStatus object to status_. - The function updatePullOutStatus() now uses the new hasFinishedBackwardDriving() function. - The logging of pull out status is now controlled by the new print_debug_info parameter rather than the removed verbose parameter. These changes improve the clarity of the code and make it easier to control debug output. Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner.param.yaml | 10 +- .../start_planner/start_planner_module.hpp | 8 +- .../start_planner_parameters.hpp | 3 +- .../scene_module/start_planner/manager.cpp | 7 +- .../start_planner/start_planner_module.cpp | 156 +++++++++--------- 5 files changed, 96 insertions(+), 88 deletions(-) diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 3a94a18e011b9..faf57c1e4ffbd 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -2,15 +2,13 @@ ros__parameters: start_planner: - verbose: false - th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 - th_distance_to_middle_of_the_road: 0.1 + th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true @@ -24,7 +22,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true - divide_pull_out_path: false + divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 @@ -140,3 +138,7 @@ # temporary backward_path_length: 30.0 forward_path_length: 100.0 + + # debug + debug: + print_debug_info: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 1e839989a0f99..7e4387bfe2e7c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -129,6 +129,11 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override; + /** + * @brief init member variables. + */ + void initVariables(); + void initializeSafetyCheckParameters(); bool receivedNewRoute() const; @@ -200,7 +205,8 @@ class StartPlannerModule : public SceneModuleInterface PredictedObjects filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; bool hasFinishedPullOut() const; - bool isBackwardDrivingComplete() const; + bool hasFinishedBackwardDriving() const; + bool checkCollisionWithDynamicObjects() const; bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index f77b15997dabd..46b72cacac651 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -46,7 +46,6 @@ struct StartPlannerParameters double collision_check_distance_from_end{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; - bool verbose{false}; // shift pull out bool enable_shift_pull_out{false}; @@ -93,6 +92,8 @@ struct StartPlannerParameters utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; utils::path_safety_checker::SafetyCheckParams safety_check_params{}; + + bool print_debug_info{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index fdc28139aa8f0..4b66377d0de72 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -33,7 +33,6 @@ StartPlannerModuleManager::StartPlannerModuleManager( std::string ns = "start_planner."; - p.verbose = node->declare_parameter(ns + "verbose"); p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); @@ -276,6 +275,12 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); } + // debug + std::string debug_ns = ns + "debug."; + { + p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index a8332359a73ab..e42106e5a58c4 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -38,6 +38,12 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; +using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap; + +// set as macro so that calling function name will be printed. +// debug print is heavy. turn on only when debugging. +#define DEBUG_PRINT(...) \ + RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__) namespace behavior_path_planner { @@ -107,38 +113,57 @@ BehaviorModuleOutput StartPlannerModule::run() void StartPlannerModule::processOnEntry() { - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - start_planner_data_.collision_check); - } + initVariables(); } void StartPlannerModule::processOnExit() +{ + initVariables(); +} + +void StartPlannerModule::initVariables() { resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + initializeSafetyCheckParameters(); + initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } void StartPlannerModule::updateData() { - if (isBackwardDrivingComplete()) { + if (receivedNewRoute()) { + resetStatus(); + DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); + } + + if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); + DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); } else { status_.backward_driving_complete = false; } - if (receivedNewRoute()) { - status_ = PullOutStatus(); - } - // check safety status when driving forward - if (parameters_->safety_check_params.enable_safety_check && status_.driving_forward) { - status_.is_safe_dynamic_objects = isSafePath(); - } else { - status_.is_safe_dynamic_objects = true; + status_.is_safe_dynamic_objects = checkCollisionWithDynamicObjects(); +} + +bool StartPlannerModule::hasFinishedBackwardDriving() const +{ + // check ego car is close enough to pull out start pose and stopped + const auto current_pose = planner_data_->self_odometry->pose.pose; + const auto distance = + tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); + + const bool is_near = distance < parameters_->th_arrived_distance; + const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); + const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; + + const bool back_finished = !status_.driving_forward && is_near && is_stopped; + if (back_finished) { + RCLCPP_INFO(getLogger(), "back finished"); } + + return back_finished; } bool StartPlannerModule::receivedNewRoute() const @@ -147,6 +172,12 @@ bool StartPlannerModule::receivedNewRoute() const *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); } +bool StartPlannerModule::checkCollisionWithDynamicObjects() const +{ + return !(parameters_->safety_check_params.enable_safety_check && status_.driving_forward) || + isSafePath(); +} + bool StartPlannerModule::isExecutionRequested() const { if (isModuleRunning()) { @@ -211,25 +242,35 @@ bool StartPlannerModule::isMoving() const parameters_->th_stopped_velocity; } -bool StartPlannerModule::isExecutionReady() const +bool StartPlannerModule::isStopped() { - // when found_pull_out_path is false,the path is not generated and approval shouldn't be - // allowed - if (!status_.found_pull_out_path) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found"); - return false; - } - - if ( - parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - isWaitingApproval()) { - if (!isSafePath()) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); - stop_pose_ = planner_data_->self_odometry->pose.pose; - return false; + odometry_buffer_.push_back(planner_data_->self_odometry); + // Delete old data in buffer + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - + rclcpp::Time(odometry_buffer_.front()->header.stamp); + if (time_diff.seconds() < parameters_->th_stopped_time) { + break; } + odometry_buffer_.pop_front(); } - return true; + return !std::any_of( + odometry_buffer_.begin(), odometry_buffer_.end(), [this](const auto & odometry) { + return utils::l2Norm(odometry->twist.twist.linear) > parameters_->th_stopped_velocity; + }); +} + +bool StartPlannerModule::isExecutionReady() const +{ + // Evaluate safety. The situation is not safe if any of the following conditions are met: + // 1. pull out path has not been found + // 2. waiting for approval and there is a collision with dynamic objects + const bool is_safe = + status_.found_pull_out_path && (!isWaitingApproval() || checkCollisionWithDynamicObjects()); + + if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose; + + return is_safe; } bool StartPlannerModule::canTransitSuccessState() @@ -461,8 +502,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { - PullOutStatus initial_status; - status_ = initial_status; + status_ = PullOutStatus{}; } void StartPlannerModule::incrementPathIndex() @@ -722,7 +762,7 @@ void StartPlannerModule::updatePullOutStatus() const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - if (isBackwardDrivingComplete()) { + if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); } else { status_.backward_path = start_planner_utils::getBackwardPath( @@ -866,48 +906,6 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -bool StartPlannerModule::isBackwardDrivingComplete() const -{ - // check ego car is close enough to pull out start pose and stopped - const auto current_pose = planner_data_->self_odometry->pose.pose; - const auto distance = - tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); - - const bool is_near = distance < parameters_->th_arrived_distance; - const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); - const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - - const bool back_finished = !status_.driving_forward && is_near && is_stopped; - if (back_finished) { - RCLCPP_INFO(getLogger(), "back finished"); - } - - return back_finished; -} - -bool StartPlannerModule::isStopped() -{ - odometry_buffer_.push_back(planner_data_->self_odometry); - // Delete old data in buffer - while (rclcpp::ok()) { - const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - - rclcpp::Time(odometry_buffer_.front()->header.stamp); - if (time_diff.seconds() < parameters_->th_stopped_time) { - break; - } - odometry_buffer_.pop_front(); - } - bool is_stopped = true; - for (const auto & odometry : odometry_buffer_) { - const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); - if (ego_vel > parameters_->th_stopped_velocity) { - is_stopped = false; - break; - } - } - return is_stopped; -} - bool StartPlannerModule::isStuck() { if (!isStopped()) { @@ -1262,8 +1260,7 @@ void StartPlannerModule::setDebugData() const add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } // Visualize planner type text @@ -1286,9 +1283,6 @@ void StartPlannerModule::setDebugData() const planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } - if (parameters_->verbose) { - logPullOutStatus(); - } } void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const From cffa796cd4c1df15fc608fdfc4077dd8c3dd0549 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 1 Dec 2023 09:12:10 +0000 Subject: [PATCH 2/4] style(pre-commit): autofix Signed-off-by: kyoichi-sugahara --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e42106e5a58c4..2f404a8b5c62b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -36,9 +36,9 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; -using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap; // set as macro so that calling function name will be printed. // debug print is heavy. turn on only when debugging. From f2142fb48e50eede9705d5812661cdc339669351 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 5 Dec 2023 12:20:57 +0900 Subject: [PATCH 3/4] - define requiresDynamicObjectsCollisionDetection() - rename checkCollisionWithDynamicObjects() to hasCollisionWithDynamicObjects() Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.hpp | 3 ++- .../start_planner/start_planner_module.cpp | 27 ++++++++++++++----- 2 files changed, 23 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 7e4387bfe2e7c..c01fa1681a678 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -136,6 +136,7 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); + bool requiresDynamicObjectsCollisionDetection() const; bool receivedNewRoute() const; bool isModuleRunning() const; @@ -206,7 +207,7 @@ class StartPlannerModule : public SceneModuleInterface const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; - bool checkCollisionWithDynamicObjects() const; + bool hasCollisionWithDynamicObjects() const; bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 2f404a8b5c62b..7bbee7fe0a10c 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -144,7 +144,9 @@ void StartPlannerModule::updateData() status_.backward_driving_complete = false; } - status_.is_safe_dynamic_objects = checkCollisionWithDynamicObjects(); + if (requiresDynamicObjectsCollisionDetection()) { + status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); + } } bool StartPlannerModule::hasFinishedBackwardDriving() const @@ -172,10 +174,16 @@ bool StartPlannerModule::receivedNewRoute() const *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); } -bool StartPlannerModule::checkCollisionWithDynamicObjects() const +bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { - return !(parameters_->safety_check_params.enable_safety_check && status_.driving_forward) || - isSafePath(); + return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; +} + +bool StartPlannerModule::hasCollisionWithDynamicObjects() const +{ + // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid + // mutable + return !isSafePath(); } bool StartPlannerModule::isExecutionRequested() const @@ -262,11 +270,18 @@ 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. waiting for approval and there is a collision with dynamic objects - const bool is_safe = - status_.found_pull_out_path && (!isWaitingApproval() || checkCollisionWithDynamicObjects()); + if (!status_.found_pull_out_path) { + is_safe = false; + } + + if (requiresDynamicObjectsCollisionDetection()) { + is_safe = !hasCollisionWithDynamicObjects(); + } if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose; From 51bb1153bc71190cb08dbf68897003f215ccebf5 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 5 Dec 2023 12:50:54 +0900 Subject: [PATCH 4/4] replace acceleration and max_velocity with min_acceleration Signed-off-by: kyoichi-sugahara --- .../config/goal_planner/goal_planner.param.yaml | 3 +-- .../config/start_planner/start_planner.param.yaml | 3 +-- .../src/scene_module/goal_planner/manager.cpp | 4 +--- .../src/scene_module/start_planner/manager.cpp | 4 +--- .../src/utils/start_goal_planner_common/utils.cpp | 3 +-- .../path_safety_checker/path_safety_checker_parameters.hpp | 1 + 6 files changed, 6 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index cf5bf683f7b73..0ab617a1a7ab9 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -119,8 +119,7 @@ # EgoPredictedPath ego_predicted_path: min_velocity: 0.0 - acceleration: 1.0 - max_velocity: 1.0 + min_acceleration: 1.0 time_horizon_for_front_object: 10.0 time_horizon_for_rear_object: 10.0 time_resolution: 0.5 diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index faf57c1e4ffbd..4bcb847d1280c 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -83,8 +83,7 @@ # EgoPredictedPath ego_predicted_path: min_velocity: 0.0 - acceleration: 1.0 - max_velocity: 1.0 + min_acceleration: 1.0 time_horizon_for_front_object: 10.0 time_horizon_for_rear_object: 10.0 time_resolution: 0.5 diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 83c0d7002936c..22d7e31211397 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -255,9 +255,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.ego_predicted_path_params.min_velocity = node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = - node->declare_parameter(ego_path_ns + "acceleration"); - p.ego_predicted_path_params.max_velocity = - node->declare_parameter(ego_path_ns + "max_velocity"); + node->declare_parameter(ego_path_ns + "min_acceleration"); p.ego_predicted_path_params.time_horizon_for_front_object = node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 4b66377d0de72..cb963decc1da0 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -171,9 +171,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.ego_predicted_path_params.min_velocity = node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = - node->declare_parameter(ego_path_ns + "acceleration"); - p.ego_predicted_path_params.max_velocity = - node->declare_parameter(ego_path_ns + "max_velocity"); + node->declare_parameter(ego_path_ns + "min_acceleration"); p.ego_predicted_path_params.time_horizon_for_front_object = node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index 1901785354cb8..cbc6d6a4a0b8d 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -133,8 +133,7 @@ void updatePathProperty( { // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is // necessary to ensure a reasonable path length. - // TODO(Sugahara): define them as parameter - const double min_accel_for_ego_predicted_path = 1.0; + const double min_accel_for_ego_predicted_path = ego_predicted_path_params->min_acceleration; const double acceleration = std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index a4e895cb0069e..d10a2f38fb975 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -150,6 +150,7 @@ struct IntegralPredictedPolygonParams struct EgoPredictedPathParams { double min_velocity{0.0}; ///< Minimum velocity. + double min_acceleration{0.0}; ///< Minimum acceleration double acceleration{0.0}; ///< Acceleration value. double max_velocity{0.0}; ///< Maximum velocity. double time_horizon_for_front_object{0.0}; ///< Time horizon for front object.