From 23615eb36041bb6eb35c15079131d34dd35ddf2d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Sep 2024 18:37:49 +0900 Subject: [PATCH 1/8] feat(map_loader): visualize BusStopArea Signed-off-by: Mamoru Sobue --- .../src/load_info_from_lanelet2_map.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp index ce4cda5c2c677..feb9fbb5047ab 100644 --- a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -30,7 +30,11 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); + if (!errors.empty()) { + for (const auto & error : errors) { + std::cout << error << std::endl; + } throw std::runtime_error("Error occurred while loading lanelet2 map"); } From 958c2fd5db50cbf3cad41edeee1f375e20e8193f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 6 Sep 2024 13:57:20 +0900 Subject: [PATCH 2/8] feat(goal_planner): add getBusStopArea function Signed-off-by: Mamoru Sobue --- .../src/load_info_from_lanelet2_map.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp index feb9fbb5047ab..ce4cda5c2c677 100644 --- a/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/autoware_map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -30,11 +30,7 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; const lanelet::LaneletMapPtr map = lanelet::load(filename, projector, &errors); - if (!errors.empty()) { - for (const auto & error : errors) { - std::cout << error << std::endl; - } throw std::runtime_error("Error occurred while loading lanelet2 map"); } From c88e8abdeb006b701e3ec13d342a5d88909e2fc4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 6 Sep 2024 14:21:42 +0900 Subject: [PATCH 3/8] add bus_stop_area param Signed-off-by: Mamoru Sobue --- .../src/goal_searcher.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index b9351e4588d5a..d7f9ae4bebb6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -138,6 +138,10 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); + const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); + const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); + const auto bus_stop_area_polygons = getBusStopAreaPolygons(pull_over_lanes); + std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { From 2636737a56e75ef7423a9022c93b5314e079971c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 6 Sep 2024 14:21:42 +0900 Subject: [PATCH 4/8] add bus_stop_area param Signed-off-by: Mamoru Sobue --- .../src/goal_searcher.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index d7f9ae4bebb6a..3b77ec04512d9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -142,6 +142,10 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); const auto bus_stop_area_polygons = getBusStopAreaPolygons(pull_over_lanes); + const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); + const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); + const auto bus_stop_area_polygons = getBusStopAreaPolygons(pull_over_lanes); + std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { From d42f6fd0a2022799ad03d83506a601cef6b9a2b6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 9 Sep 2024 18:43:51 +0900 Subject: [PATCH 5/8] save isSafePath to PullOverContextData Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 41 ++--- .../src/goal_planner_module.cpp | 143 +++++++++--------- 2 files changed, 96 insertions(+), 88 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 2adac879a6bb2..fe859b458c7bb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -309,6 +309,17 @@ struct PoseWithString } }; +struct PullOverContextData +{ + PullOverContextData() = delete; + explicit PullOverContextData(const std::pair & is_safe_path) + : is_safe_path_latched(is_safe_path.first), is_safe_path_instant(is_safe_path.second) + { + } + bool is_safe_path_latched{true}; + bool is_safe_path_instant{true}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -505,6 +516,9 @@ class GoalPlannerModule : public SceneModuleInterface // TODO(Mamoru Sobue): isSafePath() modifies ThreadSafeData::check_collision, avoid this mutable mutable ThreadSafeData thread_safe_data_; + // TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData + // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() + std::optional context_data_{std::nullopt}; std::unique_ptr last_approval_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. @@ -572,26 +586,17 @@ class GoalPlannerModule : public SceneModuleInterface bool hasDecidedPath( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, + const PullOverContextData & context_data, const GoalPlannerParameters & parameters, const std::shared_ptr goal_searcher) const; bool hasNotDecidedPath( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, + const PullOverContextData & context_data, const GoalPlannerParameters & parameters, const std::shared_ptr goal_searcher) const; DecidingPathStatusWithStamp checkDecidingPathStatus( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, + const PullOverContextData & context_data, const GoalPlannerParameters & parameters, const std::shared_ptr goal_searcher) const; void decideVelocity(); bool foundPullOverPath() const; @@ -615,9 +620,9 @@ class GoalPlannerModule : public SceneModuleInterface bool canReturnToLaneParking(); // plan pull over path - BehaviorModuleOutput planPullOver(); - BehaviorModuleOutput planPullOverAsOutput(); - BehaviorModuleOutput planPullOverAsCandidate(); + BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); + BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); + BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data); std::optional> selectPullOverPath( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; @@ -627,8 +632,8 @@ class GoalPlannerModule : public SceneModuleInterface void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // output setter - void setOutput(BehaviorModuleOutput & output); - void updatePreviousData(); + void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); + void updatePreviousData(const PullOverContextData & context_data); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output); @@ -676,7 +681,7 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & safety_check_params) const; // debug - void setDebugData(); + void setDebugData(const PullOverContextData & context_data); void printParkingPositionError() const; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 56c7c42eec6ae..b33f40bbb3136 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -276,11 +276,15 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } + // TODO(soblin): never call isSafePath on thread pool sode + const auto local_context_data = isSafePath( + local_planner_data, parameters, ego_predicted_path_params, objects_filtering_params, + safety_check_params); if ( hasDeviatedFromLastPreviousModulePath(local_planner_data) && + // TODO(soblin): !hasDecidedPath( - local_planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params, goal_searcher)) { + local_planner_data, occupancy_grid_map, local_context_data, parameters, goal_searcher)) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; } @@ -546,6 +550,10 @@ void GoalPlannerModule::updateData() resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + context_data_ = PullOverContextData(isSafePath( + planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, + safety_check_params_)); + // update goal searcher and generate goal candidates if (thread_safe_data_.get_goal_candidates().empty()) { goal_searcher_->setReferenceGoal( @@ -875,8 +883,15 @@ BehaviorModuleOutput GoalPlannerModule::plan() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!context_data_) { + RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); + } + const auto & context_data = context_data_.has_value() + ? context_data_.value() + : PullOverContextData(std::make_pair(true, true)); + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOver(); + return planPullOver(context_data); } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -1120,7 +1135,8 @@ std::vector GoalPlannerModule::generateDrivableLanes() const return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } -void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) +void GoalPlannerModule::setOutput( + const PullOverContextData & context_data, BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1136,11 +1152,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) } if ( - parameters_->safety_check_params.enable_safety_check && - !isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_) - .first && + parameters_->safety_check_params.enable_safety_check && !context_data.is_safe_path_latched && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk @@ -1166,8 +1178,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // set hazard and turn signal if ( hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_) && + planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) && isActivated()) { setTurnSignalInfo(output); } @@ -1237,40 +1248,29 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, + const PullOverContextData & context_data, const GoalPlannerParameters & parameters, const std::shared_ptr goal_searcher) const { return checkDecidingPathStatus( - planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params, goal_searcher) + planner_data, occupancy_grid_map, context_data, parameters, goal_searcher) .first == DecidingPathStatus::DECIDED; } bool GoalPlannerModule::hasNotDecidedPath( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, + const PullOverContextData & context_data, const GoalPlannerParameters & parameters, const std::shared_ptr goal_searcher) const { return checkDecidingPathStatus( - planner_data, occupancy_grid_map, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params, goal_searcher) + planner_data, occupancy_grid_map, context_data, parameters, goal_searcher) .first == DecidingPathStatus::NOT_DECIDED; } DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params, + const PullOverContextData & context_data, const GoalPlannerParameters & parameters, const std::shared_ptr goal_searcher) const { const auto & prev_status = thread_safe_data_.get_prev_data().deciding_path_status; @@ -1288,13 +1288,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if ( - enable_safety_check && - !isSafePath( - planner_data, parameters, ego_predicted_path_params, objects_filtering_params, - safety_check_params) - .first && - !isActivated()) { + if (enable_safety_check && !context_data.is_safe_path_latched && !isActivated()) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -1333,11 +1327,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus( return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } - if ( - enable_safety_check && !isSafePath( - planner_data, parameters, ego_predicted_path_params, - objects_filtering_params, safety_check_params) - .first) { + if (enable_safety_check && !context_data.is_safe_path_latched) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); @@ -1401,20 +1391,21 @@ void GoalPlannerModule::decideVelocity() } } -BehaviorModuleOutput GoalPlannerModule::planPullOver() +BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + // TODO(soblin): move DecidingPathStatus to context_data if (!hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_)) { - return planPullOverAsCandidate(); + planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_)) { + return planPullOverAsCandidate(context_data); } - return planPullOverAsOutput(); + return planPullOverAsOutput(context_data); } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() +BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( + const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1424,7 +1415,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() } BehaviorModuleOutput output{}; - const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(); + const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(context_data); output.modified_goal = pull_over_output.modified_goal; output.path = generateStopPath(); output.reference_path = getPreviousModuleOutput().reference_path; @@ -1441,12 +1432,13 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() return output; } - setDebugData(); + setDebugData(context_data); return output; } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() +BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( + const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1460,8 +1452,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() if ( hasNotDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_) && + planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, @@ -1496,7 +1487,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() // set output and status BehaviorModuleOutput output{}; - setOutput(output); + setOutput(context_data, output); // return to lane parking if it is possible if ( @@ -1506,7 +1497,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() } // For debug - setDebugData(); + setDebugData(context_data); if (parameters_->print_debug_info) { // For evaluations @@ -1520,7 +1511,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(); + updatePreviousData(context_data); return output; } @@ -1529,18 +1520,26 @@ void GoalPlannerModule::postProcess() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!context_data_) { + RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); + } + const auto & context_data = context_data_.has_value() + ? context_data_.value() + : PullOverContextData(std::make_pair(true, true)); + + // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a + // waste of time because it gives same result throughout the main thread. + const bool has_decided_path = + hasDecidedPath(planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_); + + context_data_ = std::nullopt; + if (!thread_safe_data_.foundPullOverPath()) { return; } const auto distance_to_path_change = calcDistanceToPathChange(); - // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a - // waste of time because it gives same result throughout the main thread. - const bool has_decided_path = hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_); - if (has_decided_path) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } @@ -1554,7 +1553,7 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updatePreviousData() +void GoalPlannerModule::updatePreviousData(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1564,8 +1563,7 @@ void GoalPlannerModule::updatePreviousData() auto prev_data = thread_safe_data_.get_prev_data(); prev_data.deciding_path_status = checkDecidingPathStatus( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_); + planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_); // This is related to safety check, so if it is disabled, return here. if (!parameters_->safety_check_params.enable_safety_check) { @@ -1573,9 +1571,8 @@ void GoalPlannerModule::updatePreviousData() } else { // Even if the current path is safe, it will not be safe unless it stands for a certain period // of time. Record the time when the current path has become safe - const auto [is_safe, current_is_safe] = isSafePath( - planner_data_, *parameters_, ego_predicted_path_params_, objects_filtering_params_, - safety_check_params_); + const auto is_safe = context_data.is_safe_path_latched, + current_is_safe = context_data.is_safe_path_instant; if (current_is_safe) { if (!prev_data.safety_status.safe_start_time) { prev_data.safety_status.safe_start_time = clock_->now(); @@ -1594,8 +1591,16 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!context_data_) { + RCLCPP_ERROR( + getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); + } + const auto & context_data = context_data_.has_value() + ? context_data_.value() + : PullOverContextData(std::make_pair(true, true)); + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planPullOverAsCandidate(); + return planPullOverAsCandidate(context_data); } fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -2503,7 +2508,7 @@ std::pair GoalPlannerModule::isSafePath( return {false /*is_safe*/, current_is_safe}; } -void GoalPlannerModule::setDebugData() +void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2532,9 +2537,7 @@ void GoalPlannerModule::setDebugData() if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = - hasDecidedPath( - planner_data_, occupancy_grid_map_, *parameters_, ego_predicted_path_params_, - objects_filtering_params_, safety_check_params_, goal_searcher_) + hasDecidedPath(planner_data_, occupancy_grid_map_, context_data, *parameters_, goal_searcher_) ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green const double z = planner_data_->route_handler->getGoalPose().position.z; From bc838216ea551e82ecdec0b0e59bcc31fdb7c49b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 10 Sep 2024 10:49:04 +0900 Subject: [PATCH 6/8] migrate Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 4 ++-- .../src/goal_searcher.cpp | 8 -------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index b33f40bbb3136..9feaf0a62d580 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -277,9 +277,9 @@ void GoalPlannerModule::onTimer() return true; } // TODO(soblin): never call isSafePath on thread pool sode - const auto local_context_data = isSafePath( + const auto local_context_data = PullOverContextData(isSafePath( local_planner_data, parameters, ego_predicted_path_params, objects_filtering_params, - safety_check_params); + safety_check_params)); if ( hasDeviatedFromLastPreviousModulePath(local_planner_data) && // TODO(soblin): diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 3b77ec04512d9..b9351e4588d5a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -138,14 +138,6 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); - const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); - const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); - const auto bus_stop_area_polygons = getBusStopAreaPolygons(pull_over_lanes); - - const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); - const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); - const auto bus_stop_area_polygons = getBusStopAreaPolygons(pull_over_lanes); - std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { From 9f512501b095db755f538b8497485ab31b359c30 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 10 Sep 2024 16:54:06 +0900 Subject: [PATCH 7/8] fix spell Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9feaf0a62d580..b39b6acf36ae8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -276,7 +276,7 @@ void GoalPlannerModule::onTimer() RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } - // TODO(soblin): never call isSafePath on thread pool sode + // TODO(soblin): never call isSafePath on thread pool side const auto local_context_data = PullOverContextData(isSafePath( local_planner_data, parameters, ego_predicted_path_params, objects_filtering_params, safety_check_params)); From 3f6db4b7452917a0bdcb2d385d5ee259766baaeb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 10 Sep 2024 23:50:00 +0900 Subject: [PATCH 8/8] reflect comment Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index b39b6acf36ae8..dde3a554fca3d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -282,7 +282,7 @@ void GoalPlannerModule::onTimer() safety_check_params)); if ( hasDeviatedFromLastPreviousModulePath(local_planner_data) && - // TODO(soblin): + // TODO(soblin): use DecidingPathStatus in ThreadInputData !hasDecidedPath( local_planner_data, occupancy_grid_map, local_context_data, parameters, goal_searcher)) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); @@ -886,9 +886,9 @@ BehaviorModuleOutput GoalPlannerModule::plan() if (!context_data_) { RCLCPP_ERROR(getLogger(), " [pull_over] plan() is called without valid context_data"); } - const auto & context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + const auto context_data = context_data_.has_value() + ? context_data_.value() + : PullOverContextData(std::make_pair(true, true)); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(context_data); @@ -1523,9 +1523,9 @@ void GoalPlannerModule::postProcess() if (!context_data_) { RCLCPP_ERROR(getLogger(), " [pull_over] postProcess() is called without valid context_data"); } - const auto & context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + const auto context_data = context_data_.has_value() + ? context_data_.value() + : PullOverContextData(std::make_pair(true, true)); // TODO(Mamoru Sobue): repetitive call to checkDecidingPathStatus() in the main thread is a // waste of time because it gives same result throughout the main thread. @@ -1595,9 +1595,9 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() RCLCPP_ERROR( getLogger(), " [pull_over] planWaitingApproval() is called without valid context_data"); } - const auto & context_data = context_data_.has_value() - ? context_data_.value() - : PullOverContextData(std::make_pair(true, true)); + const auto context_data = context_data_.has_value() + ? context_data_.value() + : PullOverContextData(std::make_pair(true, true)); if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(context_data);