From ff255eb62ff8780db527cd2d3f2355997c7ad770 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 10 Nov 2023 19:20:43 +0900 Subject: [PATCH] refactor(goal_planner): separate thread safe data (#5493) * refactor(goal_planner): separate thread safe data Signed-off-by: kosuke55 * fix style(pre-commit): autofix fix fix --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 190 ++++++--- .../goal_planner/pull_over_planner_base.hpp | 22 ++ .../goal_planner/goal_planner_module.cpp | 360 +++++++++--------- 3 files changed, 344 insertions(+), 228 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index df21bad862cc9..e4044d7191805 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -68,38 +68,30 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using tier4_autoware_utils::Polygon2d; -enum class PathType { - NONE = 0, - SHIFT, - ARC_FORWARD, - ARC_BACKWARD, - FREESPACE, -}; +#define DEFINE_SETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + NAME##_ = value; \ + } -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } \ - \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ +#define DEFINE_GETTER(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + return NAME##_; \ } +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ + DEFINE_SETTER(TYPE, NAME) \ + DEFINE_GETTER(TYPE, NAME) + class PullOverStatus { public: - explicit PullOverStatus(std::recursive_mutex & mutex) : mutex_(mutex) {} - // Reset all data members to their initial states void reset() { - std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; lane_parking_pull_over_path_ = nullptr; current_path_idx_ = 0; require_increment_ = true; @@ -109,16 +101,12 @@ class PullOverStatus pull_over_lanes_.clear(); lanes_.clear(); has_decided_path_ = false; - is_safe_static_objects_ = false; is_safe_dynamic_objects_ = false; - prev_is_safe_ = false; + prev_found_path_ = false; prev_is_safe_dynamic_objects_ = false; has_decided_velocity_ = false; - has_requested_approval_ = false; - is_ready_ = false; } - DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) DEFINE_SETTER_GETTER(size_t, current_path_idx) DEFINE_SETTER_GETTER(bool, require_increment) @@ -128,24 +116,14 @@ class PullOverStatus DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) DEFINE_SETTER_GETTER(std::vector, lanes) DEFINE_SETTER_GETTER(bool, has_decided_path) - DEFINE_SETTER_GETTER(bool, is_safe_static_objects) DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) - DEFINE_SETTER_GETTER(bool, prev_is_safe) + DEFINE_SETTER_GETTER(bool, prev_found_path) DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) DEFINE_SETTER_GETTER(bool, has_decided_velocity) - DEFINE_SETTER_GETTER(bool, has_requested_approval) - DEFINE_SETTER_GETTER(bool, is_ready) - DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) - DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) - DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) - DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) - DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) - DEFINE_SETTER_GETTER(std::optional, closest_start_pose) private: - std::shared_ptr pull_over_path_{nullptr}; std::shared_ptr lane_parking_pull_over_path_{nullptr}; size_t current_path_idx_{0}; bool require_increment_{true}; @@ -155,32 +133,134 @@ class PullOverStatus lanelet::ConstLanelets pull_over_lanes_{}; std::vector lanes_{}; bool has_decided_path_{false}; - bool is_safe_static_objects_{false}; bool is_safe_dynamic_objects_{false}; - bool prev_is_safe_{false}; + bool prev_found_path_{false}; bool prev_is_safe_dynamic_objects_{false}; bool has_decided_velocity_{false}; - bool has_requested_approval_{false}; - bool is_ready_{false}; - // save last time and pose - std::shared_ptr last_increment_time_; - std::shared_ptr last_path_update_time_; - - // goal modification - std::optional modified_goal_pose_; Pose refined_goal_pose_{}; - GoalCandidates goal_candidates_{}; Pose closest_goal_candidate_pose_{}; +}; + +#undef DEFINE_SETTER +#undef DEFINE_GETTER +#undef DEFINE_SETTER_GETTER + +#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } + +#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) + +class ThreadSafeData +{ +public: + ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) + : mutex_(mutex), clock_(clock) + { + } + + bool incrementPathIndex() + { + const std::lock_guard lock(mutex_); + if (pull_over_path_->incrementPathIndex()) { + last_path_idx_increment_time_ = clock_->now(); + return true; + } + return false; + } + + void set_pull_over_path(const PullOverPath & value) + { + const std::lock_guard lock(mutex_); + pull_over_path_ = std::make_shared(value); + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path(const std::shared_ptr & value) + { + const std::lock_guard lock(mutex_); + pull_over_path_ = value; + last_path_update_time_ = clock_->now(); + } + + void clearPullOverPath() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + } + + bool foundPullOverPath() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + return pull_over_path_->isValidPath(); + } + + PullOverPlannerType getPullOverPlannerType() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return PullOverPlannerType::NONE; + } + + return pull_over_path_->type; + }; - // pull over path + void reset() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + pull_over_path_candidates_.clear(); + goal_candidates_.clear(); + modified_goal_pose_ = std::nullopt; + last_path_update_time_ = std::nullopt; + last_path_idx_increment_time_ = std::nullopt; + closest_start_pose_ = std::nullopt; + } + + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) + + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + +private: + std::shared_ptr pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; - std::optional closest_start_pose_; + GoalCandidates goal_candidates_{}; + std::optional modified_goal_pose_; + std::optional last_path_update_time_; + std::optional last_path_idx_increment_time_; + std::optional closest_start_pose_{}; std::recursive_mutex & mutex_; + rclcpp::Clock::SharedPtr clock_; }; -#undef DEFINE_SETTER_GETTER +#undef DEFINE_SETTER_WITH_MUTEX +#undef DEFINE_GETTER_WITH_MUTEX +#undef DEFINE_SETTER_GETTER_WITH_MUTEX struct FreespacePlannerDebugData { @@ -276,6 +356,7 @@ class GoalPlannerModule : public SceneModuleInterface std::recursive_mutex mutex_; PullOverStatus status_; + ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; @@ -329,6 +410,7 @@ class GoalPlannerModule : public SceneModuleInterface bool isStuck(); bool hasDecidedPath() const; void decideVelocity(); + bool foundPullOverPath() const; // validation bool hasEnoughDistance(const PullOverPath & pull_over_path) const; @@ -352,8 +434,6 @@ class GoalPlannerModule : public SceneModuleInterface const GoalCandidates & goal_candidates) const; // deal with pull over partial paths - PathWithLaneId getCurrentPath() const; - bool incrementPathIndex(); void transitionToNextPathIfFinishingCurrentPath(); // lanes and drivable area diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 54bba6aee2fc2..4267261fdfe84 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -46,6 +46,7 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + size_t path_idx{0}; // accelerate with constant acceleration to the target velocity std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; @@ -84,6 +85,27 @@ struct PullOverPath return parking_path; } + + PathWithLaneId getCurrentPath() const + { + if (partial_paths.empty()) { + return PathWithLaneId{}; + } else if (partial_paths.size() <= path_idx) { + return partial_paths.back(); + } + return partial_paths.at(path_idx); + } + + bool incrementPathIndex() + { + if (partial_paths.size() - 1 <= path_idx) { + return false; + } + path_idx += 1; + return true; + } + + bool isValidPath() const { return type != PullOverPlannerType::NONE; } }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 69c1fb911c4f9..0475cd92f1bec 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -58,7 +58,7 @@ GoalPlannerModule::GoalPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, - status_{mutex_} + thread_safe_data_{mutex_, clock_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -138,16 +138,12 @@ void GoalPlannerModule::updateOccupancyGrid() void GoalPlannerModule::onTimer() { // already generated pull over candidate paths - if (!status_.get_pull_over_path_candidates().empty()) { + if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { return; } // goals are not yet available. - if (status_.get_goal_candidates().empty()) { - return; - } - - if (!isExecutionRequested()) { + if (thread_safe_data_.get_goal_candidates().empty()) { return; } @@ -157,7 +153,11 @@ void GoalPlannerModule::onTimer() return; } - const auto goal_candidates = status_.get_goal_candidates(); + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + + const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( @@ -208,8 +208,8 @@ void GoalPlannerModule::onTimer() // set member variables { const std::lock_guard lock(mutex_); - status_.set_pull_over_path_candidates(path_candidates); - status_.set_closest_start_pose(closest_start_pose); + thread_safe_data_.set_pull_over_path_candidates(path_candidates); + thread_safe_data_.set_closest_start_pose(closest_start_pose); } } @@ -457,13 +457,13 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const bool GoalPlannerModule::planFreespacePath() { - goal_searcher_->setPlannerData(planner_data_); - auto goal_candidates = status_.get_goal_candidates(); - goal_searcher_->update(goal_candidates); - status_.set_goal_candidates(goal_candidates); - + GoalCandidates goal_candidates{}; { const std::lock_guard lock(mutex_); + goal_searcher_->setPlannerData(planner_data_); + goal_candidates = thread_safe_data_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + thread_safe_data_.set_goal_candidates(goal_candidates); debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); debug_data_.freespace_planner.is_planning = true; } @@ -487,11 +487,8 @@ bool GoalPlannerModule::planFreespacePath() { const std::lock_guard lock(mutex_); - status_.set_pull_over_path(std::make_shared(*freespace_path)); - status_.set_current_path_idx(0); - status_.set_is_safe_static_objects(true); - status_.set_modified_goal_pose(goal_candidate); - status_.set_last_path_update_time(std::make_shared(clock_->now())); + thread_safe_data_.set_pull_over_path(*freespace_path); + thread_safe_data_.set_modified_goal_pose(goal_candidate); debug_data_.freespace_planner.is_planning = false; } @@ -527,14 +524,8 @@ void GoalPlannerModule::returnToLaneParking() return; } - { - const std::lock_guard lock(mutex_); - status_.set_is_safe_static_objects(true); - status_.set_has_decided_path(false); - status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); - status_.set_current_path_idx(0); - status_.set_last_path_update_time(std::make_shared(clock_->now())); - } + status_.set_has_decided_path(false); + thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); RCLCPP_INFO(getLogger(), "return to lane parking"); } @@ -546,7 +537,7 @@ void GoalPlannerModule::generateGoalCandidates() // with old architecture, module instance is not cleared when new route is received // so need to reset status here. // todo: move this check out of this function after old architecture is removed - if (!status_.get_goal_candidates().empty()) { + if (!thread_safe_data_.get_goal_candidates().empty()) { return; } @@ -554,21 +545,23 @@ void GoalPlannerModule::generateGoalCandidates() const Pose goal_pose = route_handler->getOriginalGoalPose(); status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { + const std::lock_guard lock(mutex_); goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); - status_.set_goal_candidates(goal_searcher_->search()); + thread_safe_data_.set_goal_candidates(goal_searcher_->search()); const auto current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, false); status_.set_closest_goal_candidate_pose( - goal_searcher_->getClosetGoalCandidateAlongLanes(status_.get_goal_candidates()).goal_pose); + goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()) + .goal_pose); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; GoalCandidates goal_candidates{}; goal_candidates.push_back(goal_candidate); - status_.set_goal_candidates(goal_candidates); + thread_safe_data_.set_goal_candidates(goal_candidates); status_.set_closest_goal_candidate_pose(goal_pose); } } @@ -630,13 +623,13 @@ void GoalPlannerModule::selectSafePullOverPath() { const std::lock_guard lock(mutex_); goal_searcher_->setPlannerData(planner_data_); - goal_candidates = status_.get_goal_candidates(); + goal_candidates = thread_safe_data_.get_goal_candidates(); goal_searcher_->update(goal_candidates); - status_.set_goal_candidates(goal_candidates); - status_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( - status_.get_pull_over_path_candidates(), status_.get_goal_candidates())); - pull_over_path_candidates = status_.get_pull_over_path_candidates(); - status_.set_is_safe_static_objects(false); + thread_safe_data_.set_goal_candidates(goal_candidates); + thread_safe_data_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( + thread_safe_data_.get_pull_over_path_candidates(), thread_safe_data_.get_goal_candidates())); + pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); + thread_safe_data_.clearPullOverPath(); } for (const auto & pull_over_path : pull_over_path_candidates) { @@ -660,26 +653,24 @@ void GoalPlannerModule::selectSafePullOverPath() // found safe pull over path { const std::lock_guard lock(mutex_); - status_.set_is_safe_static_objects(true); - status_.set_pull_over_path(std::make_shared(pull_over_path)); - status_.set_current_path_idx(0); - status_.set_lane_parking_pull_over_path(status_.get_pull_over_path()); - status_.set_modified_goal_pose(*goal_candidate_it); - status_.set_last_path_update_time(std::make_shared(clock_->now())); + thread_safe_data_.set_pull_over_path(pull_over_path); + thread_safe_data_.set_modified_goal_pose(*goal_candidate_it); + status_.set_lane_parking_pull_over_path(thread_safe_data_.get_pull_over_path()); } break; } - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { return; } // decelerate before the search area start const auto search_start_offset_pose = calcLongitudinalOffsetPose( - status_.get_pull_over_path()->getFullPath().points, status_.get_refined_goal_pose().position, + thread_safe_data_.get_pull_over_path()->getFullPath().points, + status_.get_refined_goal_pose().position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - approximate_pull_over_distance_); - auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); if (search_start_offset_pose) { decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { @@ -714,7 +705,7 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path setStopPath(output); } else if ( @@ -732,7 +723,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) } // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); + auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); keepStoppedWithCurrentPath(current_path); output.path = std::make_shared(current_path); @@ -751,14 +742,14 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. const std::lock_guard lock(mutex_); - status_.set_prev_is_safe(status_.get_is_safe_static_objects()); + status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); status_.set_prev_is_safe_dynamic_objects( parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { - if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { + if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); const std::lock_guard lock(mutex_); @@ -766,10 +757,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) // set stop path as pull over path auto stop_pull_over_path = std::make_shared(); stop_pull_over_path->partial_paths.push_back(*output.path); - status_.set_pull_over_path(stop_pull_over_path); - status_.set_current_path_idx(0); - status_.set_last_path_update_time(std::make_shared(clock_->now())); - + thread_safe_data_.set_pull_over_path(stop_pull_over_path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { @@ -786,7 +774,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { - auto current_path = getCurrentPath(); + auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, @@ -796,12 +784,13 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { - output.path = std::make_shared(getCurrentPath()); + output.path = + std::make_shared(thread_safe_data_.get_pull_over_path()->getCurrentPath()); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - status_.set_last_path_update_time(std::make_shared(clock_->now())); + // status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = status_.get_prev_stop_path_after_approval(); @@ -813,7 +802,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::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; @@ -831,10 +820,10 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.get_is_safe_static_objects()) { + if (thread_safe_data_.foundPullOverPath()) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = status_.get_modified_goal_pose()->goal_pose; + modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose; modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -879,22 +868,25 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { return false; } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, current_pose, std::numeric_limits::max(), M_PI_2); + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose, + std::numeric_limits::max(), M_PI_2); if (!ego_segment_idx) { return false; } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.get_pull_over_path()->start_pose.position); + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, + thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( - getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, + *ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, + start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -904,7 +896,7 @@ void GoalPlannerModule::decideVelocity() // decide velocity to guarantee turn signal lighting time if (!status_.get_has_decided_velocity()) { - auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { @@ -917,13 +909,14 @@ void GoalPlannerModule::decideVelocity() CandidateOutput GoalPlannerModule::planCandidate() const { return CandidateOutput( - status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); + thread_safe_data_.get_pull_over_path() ? thread_safe_data_.get_pull_over_path()->getFullPath() + : PathWithLaneId()); } BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { // if pull over path candidates generation is not finished, use previous module output - if (status_.get_pull_over_path_candidates().empty()) { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); } @@ -944,10 +937,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() } transitionToNextPathIfFinishingCurrentPath(); } else if ( - !status_.get_pull_over_path_candidates().empty() && needPathUpdate(path_update_duration)) { + !thread_safe_data_.get_pull_over_path_candidates().empty() && + needPathUpdate(path_update_duration)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates - // and set it to status_.get_pull_over_path() + // and set it to thread_safe_data_.get_pull_over_path() selectSafePullOverPath(); } // else: stop path is generated and set by setOutput() @@ -959,27 +953,32 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { returnToLaneParking(); } + // For debug + setDebugData(); + if (parameters_->print_debug_info) { + // For evaluations + printParkingPositionError(); + } + + if (!thread_safe_data_.foundPullOverPath()) { + return output; + } + const auto distance_to_path_change = calcDistanceToPathChange(); if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } // TODO(tkhmy) add handle status TRYING updateSteeringFactor( - {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, + {thread_safe_data_.get_pull_over_path()->start_pose, + thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); - // For debug - setDebugData(); - if (parameters_->print_debug_info) { - // For evaluations - printParkingPositionError(); - } - - setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); return output; } @@ -1002,7 +1001,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() { // if pull over path candidates generation is not finished, use previous module output - if (status_.get_pull_over_path_candidates().empty()) { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); } @@ -1012,10 +1011,9 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; - const auto distance_to_path_change = calcDistanceToPathChange(); // generate drivable area info for new architecture - if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1029,21 +1027,31 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } + if (!thread_safe_data_.foundPullOverPath()) { + return out; + } + + const auto distance_to_path_change = calcDistanceToPathChange(); if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, + {thread_safe_data_.get_pull_over_path()->start_pose, + thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); return out; } std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto full_path = status_.get_pull_over_path()->getFullPath(); + if (!thread_safe_data_.foundPullOverPath()) { + return {std::numeric_limits::max(), std::numeric_limits::max()}; + } + + const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1053,15 +1061,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.get_pull_over_path()->start_pose.position); + full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.get_modified_goal_pose()->goal_pose.position); + full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); + thread_safe_data_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1103,17 +1111,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, status_.get_closest_goal_candidate_pose().position, -approximate_pull_over_distance_); if ( - !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && + !thread_safe_data_.foundPullOverPath() && !thread_safe_data_.get_closest_start_pose() && !search_start_offset_pose) { return generateFeasibleStopPath(); } const Pose stop_pose = [&]() -> Pose { - if (status_.get_is_safe_static_objects()) { - return status_.get_pull_over_path()->start_pose; + if (thread_safe_data_.foundPullOverPath()) { + return thread_safe_data_.get_pull_over_path()->start_pose; } - if (status_.get_closest_start_pose()) { - return status_.get_closest_start_pose().value(); + if (thread_safe_data_.get_closest_start_pose()) { + return thread_safe_data_.get_closest_start_pose().value(); } return *search_start_offset_pose; }(); @@ -1187,44 +1195,42 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && last_approval_data_) { - // if using arc_path and finishing current_path, get next path - // enough time for turn signal - const bool has_passed_enough_time = (clock_->now() - last_approval_data_->time).seconds() > - planner_data_->parameters.turn_signal_search_time; - - if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { - if (incrementPathIndex()) { - status_.set_last_increment_time(std::make_shared(clock_->now())); - } - } + if (!isActivated() || !last_approval_data_) { + return; } -} -bool GoalPlannerModule::incrementPathIndex() -{ - if (status_.get_current_path_idx() == status_.get_pull_over_path()->partial_paths.size() - 1) { - return false; + // if using arc_path and finishing current_path, get next path + // enough time for turn signal + const bool has_passed_enough_time_from_approval = + (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; + if (!has_passed_enough_time_from_approval) { + return; } - status_.set_current_path_idx(status_.get_current_path_idx() + 1); - return true; -} -PathWithLaneId GoalPlannerModule::getCurrentPath() const -{ - if (status_.get_pull_over_path() == nullptr) { - return PathWithLaneId{}; + // require increment only when the time passed is enough + // to prevent increment before driving + // when the end of the current path is close to the current pose + // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + constexpr double keep_current_idx_time = 4.0; + const bool has_passed_enough_time_from_increment = + (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > + keep_current_idx_time; + if (!has_passed_enough_time_from_increment) { + return; } - if (status_.get_pull_over_path()->partial_paths.size() <= status_.get_current_path_idx()) { - return PathWithLaneId{}; + if (!hasFinishedCurrentPath()) { + return; } - return status_.get_pull_over_path()->partial_paths.at(status_.get_current_path_idx()); + + thread_safe_data_.incrementPathIndex(); } bool GoalPlannerModule::isStopped( std::deque & odometry_buffer, const double time) { + const std::lock_guard lock(mutex_); odometry_buffer.push_back(planner_data_->self_odometry); // Delete old data in buffer while (rclcpp::ok()) { @@ -1254,6 +1260,7 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + const std::lock_guard lock(mutex_); if (isOnModifiedGoal()) { return false; } @@ -1264,21 +1271,22 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.get_is_safe_static_objects()) { + if (!thread_safe_data_.foundPullOverPath()) { return true; } // any path has never been found - if (!status_.get_pull_over_path()) { + if (!thread_safe_data_.get_pull_over_path()) { return false; } - return checkCollision(getCurrentPath()); + return checkCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath()); } bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto current_path_end = getCurrentPath().points.back(); + const auto current_path_end = + thread_safe_data_.get_pull_over_path()->getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1288,12 +1296,12 @@ bool GoalPlannerModule::hasFinishedCurrentPath() bool GoalPlannerModule::isOnModifiedGoal() const { - if (!status_.get_modified_goal_pose()) { + if (!thread_safe_data_.get_modified_goal_pose()) { return false; } const Pose current_pose = planner_data_->self_odometry->pose.pose; - return calcDistance2d(current_pose, status_.get_modified_goal_pose()->goal_pose) < + return calcDistance2d(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose) < parameters_->th_arrived_distance; } @@ -1302,9 +1310,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.get_pull_over_path()->start_pose; - const auto & end_pose = status_.get_pull_over_path()->end_pose; - const auto full_path = status_.get_pull_over_path()->getFullPath(); + const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; + const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; + const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); // calc TurnIndicatorsCommand { @@ -1435,20 +1443,18 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; - constexpr double keep_current_idx_buffer_time = 2.0; - if (status_.get_last_increment_time()) { - const auto time_diff = (clock_->now() - *status_.get_last_increment_time()).seconds(); - if (time_diff < keep_stop_time) { - status_.set_require_increment(false); - for (auto & p : path.points) { - p.point.longitudinal_velocity_mps = 0.0; - } - } else if (time_diff > keep_stop_time + keep_current_idx_buffer_time) { - // require increment only when the time passed is enough - // to prevent increment before driving - // when the end of the current path is close to the current pose - status_.set_require_increment(true); - } + if (!thread_safe_data_.get_last_path_idx_increment_time()) { + return; + } + + const auto time_diff = + (clock_->now() - *thread_safe_data_.get_last_path_idx_increment_time()).seconds(); + if (time_diff > keep_stop_time) { + return; + } + + for (auto & p : path.points) { + p.point.longitudinal_velocity_mps = 0.0; } } @@ -1499,7 +1505,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { - const auto stop_point = utils::insertStopPoint(stop_point_length, path); + utils::insertStopPoint(stop_point_length, path); } } @@ -1631,7 +1637,7 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( bool GoalPlannerModule::isSafePath() const { - const auto pull_over_path = getCurrentPath(); + const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto & current_pose = planner_data_->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data_->self_odometry->twist.twist.linear.x, @@ -1648,7 +1654,7 @@ bool GoalPlannerModule::isSafePath() const const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( - status_.get_pull_over_path()->pairs_terminal_velocity_and_accel, + thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, status_.get_current_path_idx()); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", @@ -1744,23 +1750,25 @@ void GoalPlannerModule::setDebugData() goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - const auto goal_candidates = status_.get_goal_candidates(); + const auto goal_candidates = thread_safe_data_.get_goal_candidates(); add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } // Visualize path and related pose - if (status_.get_is_safe_static_objects()) { + if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( - status_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, + 0.9)); add(createPoseMarkerArray( - status_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray( + thread_safe_data_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray( - status_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); + thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < status_.get_pull_over_path()->partial_paths.size(); ++i) { - const auto & partial_path = status_.get_pull_over_path()->partial_paths.at(i); + for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths.size(); ++i) { + const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths.at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -1782,7 +1790,15 @@ void GoalPlannerModule::setDebugData() } } debug_marker_.markers.push_back(marker); + + // Visualize debug poses + const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses; + for (size_t i = 0; i < debug_poses.size(); ++i) { + add(createPoseMarkerArray( + debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); + } } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (goal_planner_data_.ego_predicted_path.size() > 0) { @@ -1806,17 +1822,22 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.get_is_safe_static_objects() + const auto color = thread_safe_data_.foundPullOverPath() ? createMarkerColor(1.0, 1.0, 1.0, 0.99) : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = status_.get_modified_goal_pose() ? status_.get_modified_goal_pose()->goal_pose - : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(status_.get_pull_over_path()->type); - marker.text += " " + std::to_string(status_.get_current_path_idx()) + "/" + - std::to_string(status_.get_pull_over_path()->partial_paths.size() - 1); + marker.pose = thread_safe_data_.get_modified_goal_pose() + ? thread_safe_data_.get_modified_goal_pose()->goal_pose + : planner_data_->self_odometry->pose.pose; + marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); + if (thread_safe_data_.foundPullOverPath()) { + marker.text += + " " + std::to_string(status_.get_current_path_idx()) + "/" + + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); + } + if (isStuck()) { marker.text += " stuck"; } else if (isStopped()) { @@ -1832,13 +1853,6 @@ void GoalPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } - - // Visualize debug poses - const auto & debug_poses = status_.get_pull_over_path()->debug_poses; - for (size_t i = 0; i < debug_poses.size(); ++i) { - add(createPoseMarkerArray( - debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); - } } void GoalPlannerModule::printParkingPositionError() const @@ -1847,7 +1861,7 @@ void GoalPlannerModule::printParkingPositionError() const const double real_shoulder_to_map_shoulder = 0.0; const Pose goal_to_ego = - inverseTransformPose(current_pose, status_.get_modified_goal_pose()->goal_pose); + inverseTransformPose(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose); const double dx = goal_to_ego.position.x; const double dy = goal_to_ego.position.y; const double distance_from_real_shoulder = @@ -1856,7 +1870,7 @@ void GoalPlannerModule::printParkingPositionError() const getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, tier4_autoware_utils::rad2deg( tf2::getYaw(current_pose.orientation) - - tf2::getYaw(status_.get_modified_goal_pose()->goal_pose.orientation)), + tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); } @@ -1882,10 +1896,10 @@ bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const { - if (!status_.get_last_path_update_time()) { + if (!thread_safe_data_.get_last_path_update_time()) { return true; } - return (clock_->now() - *status_.get_last_path_update_time()).seconds() > duration; + return (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > duration; } } // namespace behavior_path_planner