From fe41742427ec73fbbd858fe51c32782f57be8678 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 31 Jan 2025 09:10:40 +0900 Subject: [PATCH 1/7] refactor(goal_planner): refactor goal_searcher and goal_candidates (#10049) Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 6 +- .../goal_candidate.hpp | 36 +++++ .../goal_planner_module.hpp | 6 +- .../goal_searcher.hpp | 45 ++++-- .../goal_searcher_base.hpp | 75 ---------- .../pull_over_planner_base.hpp | 2 +- .../util.hpp | 2 +- .../src/decision_state.cpp | 6 +- .../src/goal_planner_module.cpp | 64 +++++---- .../src/goal_searcher.cpp | 132 +++++++++--------- 10 files changed, 182 insertions(+), 192 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp delete mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index 1092047e65030..df17becd10360 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" @@ -56,7 +56,7 @@ class PathDecisionStateController const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path, std::vector & ego_polygons_expanded); @@ -77,7 +77,7 @@ class PathDecisionStateController const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp new file mode 100644 index 0000000000000..a9fe54ac36d80 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_candidate.hpp @@ -0,0 +1,36 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_ + +#include + +#include + +namespace autoware::behavior_path_planner +{ +struct GoalCandidate +{ + geometry_msgs::msg::Pose goal_pose{}; + double distance_from_original_goal{0.0}; + double lateral_offset{0.0}; + size_t id{0}; + bool is_safe{true}; + size_t num_objects_to_avoid{0}; +}; +using GoalCandidates = std::vector; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_CANDIDATE_HPP_ 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 fb278fd65b7a3..feac0bb526f44 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 @@ -335,7 +335,7 @@ class GoalPlannerModule : public SceneModuleInterface std::unique_ptr fixed_goal_planner_; // goal searcher - std::shared_ptr goal_searcher_; + std::optional goal_searcher_{}; GoalCandidates goal_candidates_{}; bool use_bus_stop_area_{false}; @@ -364,7 +364,8 @@ class GoalPlannerModule : public SceneModuleInterface mutable GoalPlannerDebugData debug_data_; // goal seach - GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) const; + GoalCandidates generateGoalCandidates( + GoalSearcher & goal_searcher, const bool use_bus_stop_area) const; /* * state transitions and plan function used in each state @@ -433,7 +434,6 @@ class GoalPlannerModule : public SceneModuleInterface std::optional selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const std::optional> sorted_bezier_indices_opt) const; // lanes and drivable area diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 19231ed26b5c6..89b440c5bb795 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -15,7 +15,12 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include #include #include @@ -24,32 +29,45 @@ namespace autoware::behavior_path_planner { using autoware::universe_utils::LinearRing2d; using BasicPolygons2d = std::vector; +using autoware::universe_utils::MultiPolygon2d; -class GoalSearcher : public GoalSearcherBase +class GoalSearcher { public: - GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint); + static GoalSearcher create( + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const std::shared_ptr & planner_data); +public: GoalCandidates search( - const std::shared_ptr & planner_data, const bool use_bus_stop_area) override; + const std::shared_ptr & planner_data, const bool use_bus_stop_area); void update( GoalCandidates & goal_candidates, const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, - const PredictedObjects & objects) const override; + const PredictedObjects & objects) const; // todo(kosuke55): Functions for this specific use should not be in the interface, // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, - const std::shared_ptr & planner_data) const override; + const std::shared_ptr & planner_data) const; bool isSafeGoalWithMarginScaleFactor( const GoalCandidate & goal_candidate, const double margin_scale_factor, const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, - const PredictedObjects & objects) const override; + const PredictedObjects & objects) const; + + MultiPolygon2d getAreaPolygons() const { return area_polygons_; } private: + GoalSearcher( + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const bool left_side_parking, const lanelet::ConstLanelets & pull_over_lanes, + const lanelet::BasicPolygons2d & no_parking_area_polygons, + const lanelet::BasicPolygons2d & no_stopping_area_polygons, + const lanelet::BasicPolygons2d & bus_stop_area_polygons); + void countObjectsToAvoid( GoalCandidates & goal_candidates, const PredictedObjects & objects, const std::shared_ptr & planner_data, @@ -64,11 +82,16 @@ class GoalSearcher : public GoalSearcherBase const Pose & ego_pose, const PredictedObjects & objects, const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data) const; - BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; - BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; - LinearRing2d vehicle_footprint_{}; - bool left_side_parking_{true}; + const GoalPlannerParameters parameters_; + const LinearRing2d vehicle_footprint_; + const bool left_side_parking_; + const lanelet::ConstLanelets pull_over_lanes_; + const lanelet::BasicPolygons2d no_parking_area_polygons_; + const lanelet::BasicPolygons2d no_stopping_area_polygons_; + const lanelet::BasicPolygons2d bus_stop_area_polygons_; + + MultiPolygon2d area_polygons_{}; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp deleted file mode 100644 index cef9981e3cb14..0000000000000 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ - -#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware/behavior_path_planner_common/data_manager.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" - -#include - -#include -#include - -namespace autoware::behavior_path_planner -{ -using autoware::universe_utils::MultiPolygon2d; -using geometry_msgs::msg::Pose; - -struct GoalCandidate -{ - Pose goal_pose{}; - double distance_from_original_goal{0.0}; - double lateral_offset{0.0}; - size_t id{0}; - bool is_safe{true}; - size_t num_objects_to_avoid{0}; -}; -using GoalCandidates = std::vector; - -class GoalSearcherBase -{ -public: - explicit GoalSearcherBase(const GoalPlannerParameters & parameters) : parameters_{parameters} {} - virtual ~GoalSearcherBase() = default; - - MultiPolygon2d getAreaPolygons() const { return area_polygons_; } - virtual GoalCandidates search( - const std::shared_ptr & planner_data, const bool use_bus_stop_area) = 0; - virtual void update( - [[maybe_unused]] GoalCandidates & goal_candidates, - [[maybe_unused]] const std::shared_ptr occupancy_grid_map, - [[maybe_unused]] const std::shared_ptr & planner_data, - [[maybe_unused]] const PredictedObjects & objects) const - { - return; - } - virtual GoalCandidate getClosetGoalCandidateAlongLanes( - const GoalCandidates & goal_candidates, - const std::shared_ptr & planner_data) const = 0; - virtual bool isSafeGoalWithMarginScaleFactor( - const GoalCandidate & goal_candidate, const double margin_scale_factor, - const std::shared_ptr occupancy_grid_map, - const std::shared_ptr & planner_data, - const PredictedObjects & objects) const = 0; - -protected: - const GoalPlannerParameters parameters_; - MultiPolygon2d area_polygons_{}; -}; -} // namespace autoware::behavior_path_planner - -#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index 651d326061eb1..541596fdd3c3b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -14,8 +14,8 @@ #pragma once +#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index e6a345be6bddc..5f0710382e46d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_candidate.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index 1a4a6e5a25589..ca55cbd993650 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -32,7 +32,7 @@ void PathDecisionStateController::transit_state( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path, std::vector & ego_polygons_expanded) { @@ -50,7 +50,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, + const GoalSearcher & goal_searcher, const bool is_activated, const std::optional & pull_over_path_opt, std::vector & ego_polygons_expanded) const { @@ -105,7 +105,7 @@ PathDecisionState PathDecisionStateController::get_next_state( // check goal pose collision if ( - modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( + modified_goal_opt && !goal_searcher.isSafeGoalWithMarginScaleFactor( modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data, static_target_objects)) { RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); 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 bf722d19744c9..e4d5344790fac 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 @@ -89,10 +89,6 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - // set selected goal searcher - // currently there is only one goal_searcher_type - goal_searcher_ = std::make_shared(parameters_, vehicle_footprint_); - // timer callback for generating lane parking candidate paths const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); lane_parking_timer_cb_group_ = @@ -210,8 +206,9 @@ std::optional planFreespacePath( std::shared_ptr freespace_planner) { auto goal_candidates = req.goal_candidates_; - auto goal_searcher = std::make_shared(req.parameters_, req.vehicle_footprint_); - goal_searcher->update( + auto goal_searcher = + GoalSearcher::create(req.parameters_, req.vehicle_footprint_, req.get_planner_data()); + goal_searcher.update( goal_candidates, req.get_occupancy_grid_map(), req.get_planner_data(), static_target_objects); for (size_t i = 0; i < goal_candidates.size(); i++) { @@ -712,7 +709,11 @@ void GoalPlannerModule::updateData() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // update goal searcher and generate goal candidates + if (!goal_searcher_) { + goal_searcher_.emplace(GoalSearcher::create(parameters_, vehicle_footprint_, planner_data_)); + } + const auto & goal_searcher = goal_searcher_.value(); + if (goal_candidates_.empty()) { const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, @@ -726,7 +727,7 @@ void GoalPlannerModule::updateData() return boost::geometry::within( universe_utils::Point2d{goal_position.x, goal_position.y}, area); }); - goal_candidates_ = generateGoalCandidates(use_bus_stop_area_); + goal_candidates_ = generateGoalCandidates(goal_searcher_.value(), use_bus_stop_area_); } const lanelet::ConstLanelets current_lanes = @@ -788,7 +789,7 @@ void GoalPlannerModule::updateData() path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, - goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); + goal_searcher, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (context_data_) { @@ -808,17 +809,20 @@ void GoalPlannerModule::updateData() parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } - auto & ctx_data = context_data_.value(); + const auto & ctx_data = context_data_.value(); + goal_searcher.update( + goal_candidates_, occupancy_grid_map_, planner_data_, ctx_data.static_target_objects); + auto & ctx_data_mut = context_data_.value(); if (!isActivated()) { return; } - if (hasFinishedCurrentPath(ctx_data)) { - if (ctx_data.pull_over_path_opt) { - auto & pull_over_path = ctx_data.pull_over_path_opt.value(); + if (hasFinishedCurrentPath(ctx_data_mut)) { + if (ctx_data_mut.pull_over_path_opt) { + auto & pull_over_path = ctx_data_mut.pull_over_path_opt.value(); if (pull_over_path.incrementPathIndex()) { - ctx_data.last_path_idx_increment_time = clock_->now(); + ctx_data_mut.last_path_idx_increment_time = clock_->now(); } } } @@ -827,7 +831,7 @@ void GoalPlannerModule::updateData() last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); // TODO(soblin): do not "plan" in updateData - if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value()); + if (ctx_data_mut.pull_over_path_opt) decideVelocity(ctx_data_mut.pull_over_path_opt.value()); } } @@ -995,14 +999,15 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return true; } -GoalCandidates GoalPlannerModule::generateGoalCandidates(const bool use_bus_stop_area) const +GoalCandidates GoalPlannerModule::generateGoalCandidates( + GoalSearcher & goal_searcher, const bool use_bus_stop_area) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // calculate goal candidates const auto & route_handler = planner_data_->route_handler; if (utils::isAllowedGoalModification(route_handler)) { - return goal_searcher_->search(planner_data_, use_bus_stop_area); + return goal_searcher.search(planner_data_, use_bus_stop_area); } // NOTE: @@ -1026,8 +1031,8 @@ BehaviorModuleOutput GoalPlannerModule::plan() RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, " [pull_over] plan() is called without valid context_data"); } else { - auto & context_data = context_data_.value(); - return planPullOver(context_data); + auto & context_data_mut = context_data_.value(); + return planPullOver(context_data_mut); } } @@ -1212,7 +1217,6 @@ void sortPullOverPaths( std::optional GoalPlannerModule::selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates, const std::optional> sorted_bezier_indices_opt) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1233,7 +1237,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP1-1: Extract paths which have safe goal // Create a map of goal_id to GoalCandidate for quick access std::unordered_map goal_candidate_map; - for (const auto & goal_candidate : goal_candidates) { + for (const auto & goal_candidate : goal_candidates_) { goal_candidate_map[goal_candidate.id] = goal_candidate; } for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { @@ -1270,7 +1274,7 @@ std::optional GoalPlannerModule::selectPullOverPath( sorted_path_indices.end()); sortPullOverPaths( - planner_data_, parameters_, pull_over_path_candidates, goal_candidates, + planner_data_, parameters_, pull_over_path_candidates, goal_candidates_, context_data.static_target_objects, getLogger(), sorted_path_indices); // STEP3: Select the final pull over path by checking collision to make it as high priority as @@ -1324,6 +1328,7 @@ void GoalPlannerModule::setOutput( if (!selected_pull_over_path_with_velocity_opt) { // situation : not safe against static objects use stop_path + // TODO(soblin): goal_candidates_.empty() is impossible output.path = generateStopPath( context_data, (goal_candidates_.empty() ? "no goal candidate" : "no static safe path")); RCLCPP_INFO_THROTTLE( @@ -1538,16 +1543,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData context_data.last_path_update_time = std::nullopt; context_data.last_path_idx_increment_time = std::nullopt; - // update goal candidates - auto goal_candidates = goal_candidates_; - goal_searcher_->update( - goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); - // Select a path that is as safe as possible and has a high priority. const auto & pull_over_path_candidates = context_data.lane_parking_response.pull_over_path_candidates; const auto lane_pull_over_path_opt = selectPullOverPath( - context_data, pull_over_path_candidates, goal_candidates, + context_data, pull_over_path_candidates, context_data.lane_parking_response.sorted_bezier_indices_opt); // update thread_safe_data_ @@ -1656,8 +1656,8 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() " [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal " "planner"); } else { - auto & context_data = context_data_.value(); - return planPullOverAsCandidate(context_data, "waiting approval"); + auto & context_data_mut = context_data_.value(); + return planPullOverAsCandidate(context_data_mut, "waiting approval"); } } @@ -1702,6 +1702,8 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const PullOverContextData & context_data, const std::string & detail) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(!goal_searcher_); + const auto & goal_searcher = goal_searcher_.value(); const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1746,7 +1748,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible // stop point is found, stop at this position. const auto closest_goal_candidate = - goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); + goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); 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 6124955a39433..70bf3132362a4 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 @@ -18,6 +18,7 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" @@ -93,13 +94,59 @@ struct SortByWeightedDistance }; GoalSearcher::GoalSearcher( - const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint) -: GoalSearcherBase{parameters}, - vehicle_footprint_{vehicle_footprint}, - left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const bool left_side_parking, const lanelet::ConstLanelets & pull_over_lanes, + const lanelet::BasicPolygons2d & no_parking_area_polygons, + const lanelet::BasicPolygons2d & no_stopping_area_polygons, + const lanelet::BasicPolygons2d & bus_stop_area_polygons) +: parameters_(parameters), + vehicle_footprint_(vehicle_footprint), + left_side_parking_(left_side_parking), + pull_over_lanes_(pull_over_lanes), + no_parking_area_polygons_(no_parking_area_polygons), + no_stopping_area_polygons_(no_stopping_area_polygons), + bus_stop_area_polygons_(bus_stop_area_polygons) { } +GoalSearcher GoalSearcher::create( + const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, + const std::shared_ptr & planner_data) +{ + const auto left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *planner_data->route_handler, left_side_parking, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + + const auto no_parking_areas = lanelet::utils::query::noParkingAreas(pull_over_lanes); + lanelet::BasicPolygons2d no_parking_area_polygons; + for (const auto & no_parking_area : no_parking_areas) { + for (const auto & area : no_parking_area->noParkingAreas()) { + no_parking_area_polygons.push_back(lanelet::utils::to2D(area).basicPolygon()); + } + } + + const auto no_stopping_areas = lanelet::utils::query::noStoppingAreas(pull_over_lanes); + lanelet::BasicPolygons2d no_stopping_area_polygons; + for (const auto & no_stopping_area : no_stopping_areas) { + for (const auto & area : no_stopping_area->noStoppingAreas()) { + no_stopping_area_polygons.push_back(lanelet::utils::to2D(area).basicPolygon()); + } + } + + const auto bus_stop_areas = lanelet::utils::query::busStopAreas(pull_over_lanes); + lanelet::BasicPolygons2d bus_stop_area_polygons; + for (const auto & bus_stop_area : bus_stop_areas) { + for (const auto & area : bus_stop_area->busStopAreas()) { + bus_stop_area_polygons.push_back(lanelet::utils::to2D(area).basicPolygon()); + } + } + + return GoalSearcher( + parameters, vehicle_footprint, left_side_parking, pull_over_lanes, no_parking_area_polygons, + no_stopping_area_polygons, bus_stop_area_polygons); +} + GoalCandidates GoalSearcher::search( const std::shared_ptr & planner_data, const bool use_bus_stop_area) { @@ -129,24 +176,17 @@ GoalCandidates GoalSearcher::search( const double base_link2front = planner_data->parameters.base_link2front; const double base_link2rear = planner_data->parameters.base_link2rear; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( - pull_over_lanes, *route_handler, left_side_parking_); + pull_over_lanes_, *route_handler, left_side_parking_); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes_, reference_goal_pose); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; const double longitudinal_interval = use_bus_stop_area ? parameters_.bus_stop_area.goal_search_interval : parameters_.goal_search_interval; auto center_line_path = utils::resamplePathWithSpline( - route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); - - 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 = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); + route_handler->getCenterLinePath(pull_over_lanes_, s_start, s_end), longitudinal_interval); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -158,13 +198,13 @@ GoalCandidates GoalSearcher::search( // ignore goal_pose near lane start const double distance_from_lane_start = - lanelet::utils::getArcCoordinates(pull_over_lanes, center_pose).length; + lanelet::utils::getArcCoordinates(pull_over_lanes_, center_pose).length; if (distance_from_lane_start < ignore_distance_from_lane_start) { continue; } const auto distance_from_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + pull_over_lanes_, vehicle_width, base_link2front, base_link2rear, center_pose, left_side_parking_); if (!distance_from_bound) continue; @@ -188,18 +228,19 @@ GoalCandidates GoalSearcher::search( if ( parameters_.bus_stop_area.use_bus_stop_area && - !goal_planner_utils::isWithinAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) { + !goal_planner_utils::isWithinAreas( + transformed_vehicle_footprint, bus_stop_area_polygons_)) { continue; } if (goal_planner_utils::isIntersectingAreas( - transformed_vehicle_footprint, no_parking_area_polygons)) { + transformed_vehicle_footprint, no_parking_area_polygons_)) { // break here to exclude goals located laterally in no_parking_areas break; } if (goal_planner_utils::isIntersectingAreas( - transformed_vehicle_footprint, no_stopping_area_polygons)) { + transformed_vehicle_footprint, no_stopping_area_polygons_)) { // break here to exclude goals located laterally in no_stopping_areas break; } @@ -215,7 +256,7 @@ GoalCandidates GoalSearcher::search( (transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontLeftIndex) + transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontRightIndex)) / 2.0; - const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes); + const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes_); const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose( left_side_parking_ ? pull_over_lanelet.leftBound() : pull_over_lanelet.rightBound(), autoware::universe_utils::createPoint( @@ -250,15 +291,12 @@ void GoalSearcher::countObjectsToAvoid( // calculate search start/end pose in pull over lanes const auto search_start_end_poses = std::invoke([&]() -> std::pair { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters_.backward_goal_search_length, - parameters_.forward_goal_search_length); const auto goal_arc_coords = - lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose); + lanelet::utils::getArcCoordinates(pull_over_lanes_, reference_goal_pose); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; const auto center_line_path = utils::resamplePathWithSpline( - route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + route_handler->getCenterLinePath(pull_over_lanes_, s_start, s_end), parameters_.goal_search_interval); return std::make_pair( center_line_path.points.front().point.pose, center_line_path.points.back().point.pose); @@ -371,10 +409,6 @@ bool GoalSearcher::isSafeGoalWithMarginScaleFactor( const std::shared_ptr occupancy_grid_map, const std::shared_ptr & planner_data, const PredictedObjects & objects) const { - if (!parameters_.use_object_recognition) { - return true; - } - const Pose goal_pose = goal_candidate.goal_pose; const double margin = parameters_.object_recognition_collision_check_hard_margins.back() * margin_scale_factor; @@ -455,13 +489,11 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( } } - if (parameters_.use_object_recognition) { - if ( - utils::calcLongitudinalDistanceFromEgoToObjects( - ego_pose, planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, - objects) < parameters_.longitudinal_margin) { - return true; - } + if ( + utils::calcLongitudinalDistanceFromEgoToObjects( + ego_pose, planner_data->parameters.base_link2front, planner_data->parameters.base_link2rear, + objects) < parameters_.longitudinal_margin) { + return true; } return false; } @@ -518,34 +550,6 @@ void GoalSearcher::createAreaPolygons( } } -BasicPolygons2d GoalSearcher::getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const -{ - BasicPolygons2d area_polygons{}; - for (const auto & ll : lanes) { - for (const auto & reg_elem : ll.regulatoryElementsAs()) { - for (const auto & area : reg_elem->noParkingAreas()) { - const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); - area_polygons.push_back(area_poly); - } - } - } - return area_polygons; -} - -BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const -{ - BasicPolygons2d area_polygons{}; - for (const auto & ll : lanes) { - for (const auto & reg_elem : ll.regulatoryElementsAs()) { - for (const auto & area : reg_elem->noStoppingAreas()) { - const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); - area_polygons.push_back(area_poly); - } - } - } - return area_polygons; -} - GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const From 094e9bb06deb1e74e7b04b8e7030e1ec84d39418 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 31 Jan 2025 09:43:32 +0900 Subject: [PATCH 2/7] fix(goal_planner): fix goal_searcher assert (#10055) 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 e4d5344790fac..f4d960a868cb2 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 @@ -1702,7 +1702,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const PullOverContextData & context_data, const std::string & detail) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - assert(!goal_searcher_); + assert(goal_searcher_); const auto & goal_searcher = goal_searcher_.value(); const auto & route_handler = planner_data_->route_handler; From cc9776807564308d15cb5e2cf0d6d7b7e12908e3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 31 Jan 2025 13:35:59 +0900 Subject: [PATCH 3/7] refactor(goal_planner): remove enable_safety_check because it is default (#10052) Signed-off-by: Mamoru Sobue --- .../README.md | 1 - .../src/decision_state.cpp | 29 ++++++++----------- .../src/goal_planner_module.cpp | 5 ---- .../src/manager.cpp | 7 +++-- 4 files changed, 16 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 928e53b1ccff5..11a2a92024f4f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -384,7 +384,6 @@ In addition, the safety check has a time hysteresis, and if the path is judged " | Name | Unit | Type | Description | Default value | | :----------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | -| enable_safety_check | [-] | bool | flag whether to use safety check | true | | method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | | keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | | check_all_predicted_path | - | bool | Flag to check all predicted paths | true | diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index ca55cbd993650..a5451c9f4be12 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -57,22 +57,18 @@ PathDecisionState PathDecisionStateController::get_next_state( auto next_state = current_state_; // update safety - if (!parameters.safety_check_params.enable_safety_check) { - next_state.is_stable_safe = true; - } else { - if (is_current_safe) { - if (!next_state.safe_start_time) { - next_state.safe_start_time = now; - next_state.is_stable_safe = false; - } else { - next_state.is_stable_safe = - ((now - next_state.safe_start_time.value()).seconds() > - parameters.safety_check_params.keep_unsafe_time); - } - } else { - next_state.safe_start_time = std::nullopt; + if (is_current_safe) { + if (!next_state.safe_start_time) { + next_state.safe_start_time = now; next_state.is_stable_safe = false; + } else { + next_state.is_stable_safe = + ((now - next_state.safe_start_time.value()).seconds() > + parameters.safety_check_params.keep_unsafe_time); } + } else { + next_state.safe_start_time = std::nullopt; + next_state.is_stable_safe = false; } // Once this function returns true, it will continue to return true thereafter @@ -87,10 +83,9 @@ PathDecisionState PathDecisionStateController::get_next_state( } const auto & pull_over_path = pull_over_path_opt.value(); - const bool enable_safety_check = parameters.safety_check_params.enable_safety_check; // 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 && !next_state.is_stable_safe && !is_activated) { + if (!next_state.is_stable_safe && !is_activated) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -132,7 +127,7 @@ PathDecisionState PathDecisionStateController::get_next_state( return next_state; } - if (enable_safety_check && !next_state.is_stable_safe) { + if (!next_state.is_stable_safe) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); 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 f4d960a868cb2..c615150b581a5 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 @@ -79,11 +79,6 @@ GoalPlannerModule::GoalPlannerModule( is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false} { - // TODO(soblin): remove safety_check_params.enable_safety_check because it does not make sense - if (!parameters_.safety_check_params.enable_safety_check) { - throw std::domain_error("goal_planner never works without safety check"); - } - occupancy_grid_map_ = std::make_shared(); // planner when goal modification is not allowed diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index aeaf6836cdba3..b0710cdd50d10 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -369,6 +369,10 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + // NOTE(soblin): remove safety_check_params.enable_safety_check because it does not make sense + if (!p.safety_check_params.enable_safety_check) { + throw std::domain_error("goal_planner never works without safety check"); + } p.safety_check_params.keep_unsafe_time = node->declare_parameter(safety_check_ns + "keep_unsafe_time"); p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); @@ -788,9 +792,6 @@ void GoalPlannerModuleManager::updateModuleParams( // SafetyCheckParams const std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; { - updateParam( - parameters, safety_check_ns + "enable_safety_check", - p->safety_check_params.enable_safety_check); updateParam( parameters, safety_check_ns + "keep_unsafe_time", p->safety_check_params.keep_unsafe_time); updateParam(parameters, safety_check_ns + "method", p->safety_check_params.method); From a961a1e32a66536ff4e73228a2bec81241a1efc4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 31 Jan 2025 16:45:39 +0900 Subject: [PATCH 4/7] feat(goal_planner): do not use isActivated() in deciding state transition (#10056) Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 12 +++----- .../src/decision_state.cpp | 29 ++++++++----------- .../src/goal_planner_module.cpp | 13 ++------- 3 files changed, 19 insertions(+), 35 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp index df17becd10360..f4152d5da67b7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -49,15 +49,13 @@ class PathDecisionStateController * @brief update current state and save old current state to prev state */ void transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const autoware_perception_msgs::msg::PredictedObjects & static_target_objects, const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded); PathDecisionState get_current_state() const { return current_state_; } @@ -71,14 +69,12 @@ class PathDecisionStateController * @brief update current state and save old current state to prev state */ PathDecisionState get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded) const; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index a5451c9f4be12..2e865ea59f7c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -26,32 +26,27 @@ namespace autoware::behavior_path_planner using autoware::motion_utils::calcSignedArcLength; void PathDecisionStateController::transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded) { const auto next_state = get_next_state( - found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, - planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, - pull_over_path, ego_polygons_expanded); + pull_over_path_opt, now, static_target_objects, dynamic_target_objects, planner_data, + occupancy_grid_map, is_current_safe, parameters, goal_searcher, ego_polygons_expanded); current_state_ = next_state; } PathDecisionState PathDecisionStateController::get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, + const std::optional & pull_over_path_opt, const rclcpp::Time & now, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, const std::shared_ptr planner_data, const std::shared_ptr occupancy_grid_map, const bool is_current_safe, const GoalPlannerParameters & parameters, - const GoalSearcher & goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, + const GoalSearcher & goal_searcher, std::vector & ego_polygons_expanded) const { auto next_state = current_state_; @@ -77,7 +72,7 @@ PathDecisionState PathDecisionStateController::get_next_state( } // if path is not safe, not decided - if (!found_pull_over_path || !pull_over_path_opt) { + if (!pull_over_path_opt) { next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; } @@ -85,7 +80,7 @@ PathDecisionState PathDecisionStateController::get_next_state( const auto & pull_over_path = pull_over_path_opt.value(); // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if (!next_state.is_stable_safe && !is_activated) { + if (!next_state.is_stable_safe) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -98,11 +93,11 @@ PathDecisionState PathDecisionStateController::get_next_state( if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { const double hysteresis_factor = 0.9; + const auto & modified_goal = pull_over_path.modified_goal(); // check goal pose collision - if ( - modified_goal_opt && !goal_searcher.isSafeGoalWithMarginScaleFactor( - modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, - planner_data, static_target_objects)) { + if (!goal_searcher.isSafeGoalWithMarginScaleFactor( + modified_goal, hysteresis_factor, occupancy_grid_map, planner_data, + static_target_objects)) { RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; next_state.deciding_start_time = std::nullopt; 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 c615150b581a5..91df188995a1f 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 @@ -761,13 +761,6 @@ void GoalPlannerModule::updateData() ? std::make_optional(context_data_.value().pull_over_path_opt.value()) : std::nullopt; - const auto modified_goal_pose = [&]() -> std::optional { - if (!pull_over_path_recv) { - return std::nullopt; - } - return pull_over_path_recv.value().modified_goal(); - }(); - // save "old" state const auto prev_decision_state = path_decision_controller_.get_current_state(); const auto [is_current_safe, collision_check_map] = @@ -782,9 +775,9 @@ void GoalPlannerModule::updateData() dynamic_target_objects, parameters_.th_moving_object_velocity); path_decision_controller_.transit_state( - found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, - modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, - goal_searcher, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); + pull_over_path_recv, clock_->now(), static_target_objects, dynamic_target_objects, + planner_data_, occupancy_grid_map_, is_current_safe, parameters_, goal_searcher, + debug_data_.ego_polygons_expanded); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); if (context_data_) { From a5b5c58e7fe796bbc0aebf0b0ca94ce7b6c6833d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 6 Feb 2025 17:04:46 +0900 Subject: [PATCH 5/7] feat(goal_planner): replace LastApprovalData with the time changed to DECIDED (#10066) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 10 +---- .../src/goal_planner_module.cpp | 38 +++++++++---------- 2 files changed, 20 insertions(+), 28 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 feac0bb526f44..0dbec17f890b3 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 @@ -73,14 +73,6 @@ struct GoalPlannerDebugData utils::path_safety_checker::CollisionCheckDebugMap collision_check{}; }; -struct LastApprovalData -{ - LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {} - - rclcpp::Time time{}; - Pose pose{}; -}; - struct PullOverContextData { PullOverContextData() = delete; @@ -352,7 +344,7 @@ class GoalPlannerModule : public SceneModuleInterface std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() PathDecisionStateController path_decision_controller_{getLogger()}; - std::unique_ptr last_approval_data_{nullptr}; + std::optional decided_time_{}; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. 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 91df188995a1f..aa0ea062f33c7 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 @@ -778,30 +778,38 @@ void GoalPlannerModule::updateData() pull_over_path_recv, clock_->now(), static_target_objects, dynamic_target_objects, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, goal_searcher, debug_data_.ego_polygons_expanded); + const auto new_decision_state = path_decision_controller_.get_current_state(); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); + + // NOTE: currently occupancy_grid_map_ must be used after syncWithThreads + goal_searcher.update(goal_candidates_, occupancy_grid_map_, planner_data_, static_target_objects); + if (context_data_) { context_data_.value().update( - path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects, prev_decision_state, + new_decision_state.is_stable_safe, static_target_objects, dynamic_target_objects, + prev_decision_state, isStopped( odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } else { context_data_.emplace( - path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects, prev_decision_state, + new_decision_state.is_stable_safe, static_target_objects, dynamic_target_objects, + prev_decision_state, isStopped( odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } - const auto & ctx_data = context_data_.value(); - goal_searcher.update( - goal_candidates_, occupancy_grid_map_, planner_data_, ctx_data.static_target_objects); auto & ctx_data_mut = context_data_.value(); + if (!decided_time_ && new_decision_state.state == PathDecisionState::DecisionKind::DECIDED) { + decided_time_ = clock_->now(); + // TODO(soblin): do not "plan" in updateData + if (ctx_data_mut.pull_over_path_opt) decideVelocity(ctx_data_mut.pull_over_path_opt.value()); + } + if (!isActivated()) { return; } @@ -814,13 +822,6 @@ void GoalPlannerModule::updateData() } } } - - if (!last_approval_data_) { - last_approval_data_ = - std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); - // TODO(soblin): do not "plan" in updateData - if (ctx_data_mut.pull_over_path_opt) decideVelocity(ctx_data_mut.pull_over_path_opt.value()); - } } void GoalPlannerModule::processOnExit() @@ -829,7 +830,6 @@ void GoalPlannerModule::processOnExit() resetPathReference(); debug_marker_.markers.clear(); context_data_ = std::nullopt; - last_approval_data_.reset(); } bool GoalPlannerModule::isExecutionRequested() const @@ -1875,7 +1875,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (!last_approval_data_) { + if (!decided_time_) { return false; } @@ -1885,10 +1885,10 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d // check if enough time has passed since last approval // this is necessary to give turn signal for enough time - const bool has_passed_enough_time_from_approval = - (clock_->now() - last_approval_data_->time).seconds() > + const bool has_passed_enough_time_from_decided = + (clock_->now() - decided_time_.value()).seconds() > planner_data_->parameters.turn_signal_search_time; - if (!has_passed_enough_time_from_approval) { + if (!has_passed_enough_time_from_decided) { return false; } From 6efa8787c1034b23737a79d7d8a5a483ff6bf4ae Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 7 Feb 2025 15:15:57 +0900 Subject: [PATCH 6/7] refactor(goal_planner): fix updateData continuation condition (#10079) refactor(goal_planner): fix updateData contiuation condition 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 aa0ea062f33c7..3c676707ad55b 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 @@ -746,7 +746,7 @@ void GoalPlannerModule::updateData() } } - if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } From 4eef2c74cd575061e7f562d7e4a75f73616abdf6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 12 Feb 2025 19:21:44 +0900 Subject: [PATCH 7/7] feat(goal_planner): ensure stop while path candidates are empty (#10101) Signed-off-by: Mamoru Sobue --- .../goal_searcher.hpp | 2 +- .../src/goal_planner_module.cpp | 26 +++++++++++++------ .../src/goal_searcher.cpp | 2 +- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index 89b440c5bb795..8744fc1d12f43 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -49,7 +49,7 @@ class GoalSearcher // todo(kosuke55): Functions for this specific use should not be in the interface, // so it is better to consider interface design when we implement other goal searchers. - GoalCandidate getClosetGoalCandidateAlongLanes( + std::optional getClosestGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const; bool isSafeGoalWithMarginScaleFactor( 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 3c676707ad55b..411695c63f8dc 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 @@ -1464,7 +1464,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( // if pull over path candidates generation is not finished, use previous module output if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { - return getPreviousModuleOutput(); + auto stop_path = getPreviousModuleOutput(); + stop_path.path = generateStopPath(context_data, detail); + return stop_path; } BehaviorModuleOutput output{}; @@ -1735,11 +1737,13 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible // stop point is found, stop at this position. - const auto closest_goal_candidate = - goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto closest_searched_goal_candidate = + goal_searcher.getClosestGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto closest_goal_candidate = closest_searched_goal_candidate + ? closest_searched_goal_candidate.value().goal_pose + : route_handler->getOriginalGoalPose(); const auto decel_pose = calcLongitudinalOffsetPose( - extended_prev_path.points, closest_goal_candidate.goal_pose.position, - -approximate_pull_over_distance_); + extended_prev_path.points, closest_goal_candidate.position, -approximate_pull_over_distance_); // if not approved stop road lane. // stop point priority is @@ -2066,12 +2070,18 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + assert(goal_searcher_); + const auto & goal_searcher = goal_searcher_.value(); // decelerate before the search area start - const auto closest_goal_candidate = - goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto & route_handler = planner_data_->route_handler; + const auto closest_searched_goal_candidate = + goal_searcher.getClosestGoalCandidateAlongLanes(goal_candidates_, planner_data_); + const auto closest_goal_candidate = closest_searched_goal_candidate + ? closest_searched_goal_candidate.value().goal_pose + : route_handler->getOriginalGoalPose(); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, + pull_over_path.full_path().points, closest_goal_candidate.position, -approximate_pull_over_distance_); auto & first_path = pull_over_path.partial_paths().front(); if (decel_pose) { 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 70bf3132362a4..cea726235238d 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 @@ -550,7 +550,7 @@ void GoalSearcher::createAreaPolygons( } } -GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( +std::optional GoalSearcher::getClosestGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const {