diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index fac0e921c1072..0352096d02b2b 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -44,6 +44,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/start_planner/util.cpp src/utils/start_planner/shift_pull_out.cpp src/utils/start_planner/geometric_pull_out.cpp + src/utils/start_planner/freespace_pull_out.cpp src/utils/path_shifter/path_shifter.cpp src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 3c5846edf3ee9..39d05a7fb761b 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -31,7 +31,8 @@ # object recognition object_recognition: use_object_recognition: true - object_recognition_collision_check_margin: 1.0 + object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker + object_recognition_collision_check_max_extra_stopping_margin: 1.0 # pull over pull_over: diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 60f292744025d..bea78664c65a3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -36,3 +36,39 @@ th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 008087ee32b4b..99dadc959f5a5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -150,10 +150,11 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for object recognition based collision check -| Name | Unit | Type | Description | Default value | -| :---------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | -| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | -| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 1.0 | +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :----- | :--------------------------------------------------------- | :------------ | ---------------------------------------------------------------------------------------------------------- | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | | 1.0 | 　maximum value when adding longitudinal distance margin for collision check considering stopping distance | ## **Goal Search** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 0c3d257806fd3..5177feaaf61f4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -170,7 +170,6 @@ class GoalPlannerModule : public SceneModuleInterface // for parking policy bool left_side_parking_{true}; - mutable bool allow_goal_modification_{false}; // need to be set in isExecutionRequested // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index c55bd38a9e3c1..24fd27d9a7a78 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" @@ -94,7 +95,9 @@ class StartPlannerModule : public SceneModuleInterface { } + // Condition to disable simultaneous execution bool isBackFinished() const { return status_.back_finished; } + bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: std::shared_ptr parameters_; @@ -109,7 +112,15 @@ class StartPlannerModule : public SceneModuleInterface std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; - std::shared_ptr getCurrentPlanner() const; + // generate freespace pull out paths in a separate thread + std::unique_ptr freespace_planner_; + rclcpp::TimerBase::SharedPtr freespace_planner_timer_; + rclcpp::CallbackGroup::SharedPtr freespace_planner_timer_cb_group_; + // TODO(kosuke55) + // Currently, we only do lock when updating a member of status_. + // However, we need to ensure that the value does not change when referring to it. + std::mutex mutex_; + PathWithLaneId getFullPath() const; std::vector searchPullOutStartPoses(); @@ -133,7 +144,9 @@ class StartPlannerModule : public SceneModuleInterface bool hasFinishedPullOut() const; void checkBackFinished(); bool isStopped(); + bool isStuck(); bool hasFinishedCurrentPath(); + void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. bool IsGoalBehindOfEgoInSameRouteSegment() const; @@ -141,6 +154,10 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + // freespace planner + void onFreespacePlannerTimer(); + bool planFreespacePath(); + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index 1378be5461a64..b698892907789 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -47,6 +47,7 @@ class FreespacePullOver : public PullOverPlannerBase std::unique_ptr planner_; double velocity_; bool use_back_; + bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f4a3a2bc5a88b..f87944641f59b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -68,6 +68,7 @@ struct GoalPlannerParameters // object recognition bool use_object_recognition; double object_recognition_collision_check_margin; + double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params double pull_over_velocity; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index edfec6a7db579..8606f2cd2397d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -101,6 +101,15 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, const double rear_object_deceleration, CollisionCheckDebug & debug); +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double margin, const double max_stopping_margin); } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index bdca6a2fdf085..191cd3e10def5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -75,7 +75,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity); + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp new file mode 100644 index 0000000000000..abdf17c9c6cfe --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ + +#include "behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using freespace_planning_algorithms::AbstractPlanningAlgorithm; +using freespace_planning_algorithms::AstarSearch; +using freespace_planning_algorithms::RRTStar; + +class FreespacePullOut : public PullOutPlannerBase +{ +public: + FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info); + + PlannerType getPlannerType() override { return PlannerType::FREESPACE; } + + boost::optional plan(Pose start_pose, Pose end_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 3a2087083b427..6e6e5111e5dd9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -40,6 +40,7 @@ enum class PlannerType { SHIFT = 1, GEOMETRIC = 2, STOP = 3, + FREESPACE = 4, }; class PullOutPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 9abad1a2fbec6..1e26bef0c85be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -18,11 +18,20 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include +#include +#include + #include #include namespace behavior_path_planner { + +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + struct StartPlannerParameters { double th_arrived_distance; @@ -56,6 +65,17 @@ struct StartPlannerParameters double backward_search_resolution; double backward_path_update_duration; double ignore_distance_from_lane_end; + // freespace planner + bool enable_freespace_planner; + std::string freespace_planner_algorithm; + double end_pose_search_start_distance; + double end_pose_search_end_distance; + double end_pose_search_interval; + double freespace_planner_velocity; + double vehicle_shape_margin; + PlannerCommonParam freespace_planner_common_parameters; + AstarParam astar_parameters; + RRTStarParam rrt_star_parameters; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index a8e22df5614bf..5ab03c34e7e14 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -164,6 +164,16 @@ double getDistanceBetweenPredictedPathAndObject( const PredictedObject & object, const PredictedPath & path, const double start_time, const double end_time, const double resolution); +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double margin, const double max_stopping_margin); + /** * @brief Check collision between ego path footprints and objects. * @return Has collision or not @@ -399,6 +409,9 @@ void makeBoundLongitudinallyMonotonic( std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, const std::string & polygon_name); + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index b1bff882c94cd..97e047be749b5 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -273,10 +273,6 @@ bool GoalPlannerModule::isExecutionRequested() const const Pose & current_pose = planner_data_->self_odometry->pose.pose; const Pose & goal_pose = route_handler->getGoalPose(); - // if goal is shoulder lane, allow goal modification - allow_goal_modification_ = - route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); - // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); @@ -553,7 +549,9 @@ void GoalPlannerModule::selectSafePullOverPath() } // check if path is valid and safe - if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { + if ( + !hasEnoughDistance(pull_over_path) || + checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) { continue; } @@ -1169,9 +1167,14 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path, *(planner_data_->dynamic_object), - parameters_->object_recognition_collision_check_margin)) { + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( + path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, + parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; } } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 488604407df33..9e94a234a7f48 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -88,6 +88,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); + p.object_recognition_collision_check_max_extra_stopping_margin = + node->declare_parameter( + ns + "object_recognition_collision_check_max_extra_stopping_margin"); } // pull over general params diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index baa23e71ec5c3..5fdef6f467950 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -78,6 +78,75 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "backward_path_update_duration"); p.ignore_distance_from_lane_end = node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // freespace planner general params + { + std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + node->declare_parameter(ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + node->declare_parameter(ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + node->declare_parameter(ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); + p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_planner_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_planner_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_planner_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_planner_common_parameters.maximum_turning_radius = std::max( + p.freespace_planner_common_parameters.maximum_turning_radius, + p.freespace_planner_common_parameters.minimum_turning_radius); + p.freespace_planner_common_parameters.turning_radius_size = + std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + } + // freespace planner search config + { + std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + // freespace planner astar + { + std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.only_behind_solutions = + node->declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + node->declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); + } // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { @@ -112,7 +181,13 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const if (!module->isBackFinished()) { return false; } - return enable_simultaneous_execution_as_candidate_module_; + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return enable_simultaneous_execution_as_approved_module_; }; return std::all_of(registered_modules_.begin(), registered_modules_.end(), checker); @@ -125,6 +200,12 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons if (!module->isBackFinished()) { return false; } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + return enable_simultaneous_execution_as_candidate_module_; }; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 11341b3f171f0..c7541dc8af544 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -58,6 +58,34 @@ StartPlannerModule::StartPlannerModule( if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } + + if (parameters_->enable_freespace_planner) { + freespace_planner_ = std::make_unique(node, *parameters, vehicle_info_); + const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); + freespace_planner_timer_cb_group_ = + node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + freespace_planner_timer_ = rclcpp::create_timer( + &node, clock_, freespace_planner_period_ns, + std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), + freespace_planner_timer_cb_group_); + } +} + +void StartPlannerModule::onFreespacePlannerTimer() +{ + if (!planner_data_) { + return; + } + + if (!planner_data_->costmap) { + return; + } + + const bool is_new_costmap = + (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; + if (isStuck() && is_new_costmap) { + planFreespacePath(); + } } BehaviorModuleOutput StartPlannerModule::run() @@ -200,20 +228,14 @@ BehaviorModuleOutput StartPlannerModule::plan() path = status_.backward_path; } - const auto target_drivable_lanes = - getNonOverlappingExpandedLanes(path, generateDrivableLanes(path)); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -257,23 +279,8 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } -std::shared_ptr StartPlannerModule::getCurrentPlanner() const -{ - for (const auto & planner : start_planners_) { - if (status_.planner_type == planner->getPlannerType()) { - return planner; - } - } - return nullptr; -} - PathWithLaneId StartPlannerModule::getFullPath() const { - const auto pull_out_planner = getCurrentPlanner(); - if (pull_out_planner == nullptr) { - return PathWithLaneId{}; - } - // combine partial pull out path PathWithLaneId pull_out_path; for (const auto & partial_path : status_.pull_out_path.partial_paths) { @@ -334,17 +341,14 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = expanded_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - output.path = std::make_shared(stop_path); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); path_candidate_ = std::make_shared(getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; + setDrivableAreaInfo(output); + const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return SteeringFactor::LEFT; @@ -405,18 +409,12 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & goal_pose, const std::string search_priority) { - status_.is_safe = false; - status_.planner_type = PlannerType::NONE; - // check if start pose candidates are valid if (start_pose_candidates.empty()) { return; } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); @@ -428,6 +426,7 @@ void StartPlannerModule::planWithPriority( } // use current path if back is not needed if (status_.back_finished) { + const std::lock_guard lock(mutex_); status_.is_safe = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; @@ -447,10 +446,13 @@ void StartPlannerModule::planWithPriority( } // Update status variables with the next path information - status_.is_safe = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); + { + const std::lock_guard lock(mutex_); + status_.is_safe = true; + status_.pull_out_path = *pull_out_path_next; + status_.pull_out_start_pose = pull_out_start_pose_next; + status_.planner_type = planner->getPlannerType(); + } return true; }; @@ -490,7 +492,19 @@ void StartPlannerModule::planWithPriority( } for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) break; + if (is_safe_with_pose_planner(p.first, p.second)) { + const std::lock_guard lock(mutex_); + // Set back_finished flag based on the current index + status_.back_finished = p.first == 0; + return; + } + } + + // not found safe path + if (status_.planner_type != PlannerType::FREESPACE) { + const std::lock_guard lock(mutex_); + status_.is_safe = false; + status_.planner_type = PlannerType::NONE; } } @@ -515,9 +529,6 @@ PathWithLaneId StartPlannerModule::generateStopPath() const path.points.push_back(toPathPointWithLaneId(current_pose)); path.points.push_back(toPathPointWithLaneId(moved_pose)); - // generate drivable area - const auto target_drivable_lanes = - getNonOverlappingExpandedLanes(path, generateDrivableLanes(path)); return path; } @@ -707,6 +718,10 @@ bool StartPlannerModule::hasFinishedPullOut() const } const auto current_pose = planner_data_->self_odometry->pose.pose; + if (status_.planner_type == PlannerType::FREESPACE) { + return tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_path.end_pose) < + parameters_->th_arrived_distance; + } // check that ego has passed pull out end point const double backward_path_length = @@ -774,6 +789,24 @@ bool StartPlannerModule::isStopped() return is_stopped; } +bool StartPlannerModule::isStuck() +{ + if (!isStopped()) { + return false; + } + + if (status_.planner_type == PlannerType::STOP) { + return true; + } + + // not found safe path + if (!status_.is_safe) { + return true; + } + + return false; +} + bool StartPlannerModule::hasFinishedCurrentPath() { const auto current_path = getCurrentPath(); @@ -814,10 +847,17 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const if (path.points.empty()) { return {}; } - const auto closest_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - const auto lane_id = path.points.at(closest_idx).lane_ids.front(); - const auto lane = planner_data_->route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id); - const double lateral_offset = lanelet::utils::getLateralDistanceToCenterline(lane, start_pose); + + // calculate lateral offset from pull out target lane center line + lanelet::ConstLanelet closest_road_lane; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); + const double lateral_offset = + lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; @@ -913,21 +953,21 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() const PathWithLaneId stop_path = generateStopPath(); output.path = std::make_shared(stop_path); - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = generateDrivableLanes(*output.path); - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + setDrivableAreaInfo(output); output.reference_path = getPreviousModuleOutput().reference_path; - status_.back_finished = true; - status_.planner_type = PlannerType::STOP; - status_.pull_out_path.partial_paths.clear(); - status_.pull_out_path.partial_paths.push_back(stop_path); - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - status_.pull_out_start_pose = current_pose; - status_.pull_out_path.start_pose = current_pose; - status_.pull_out_path.end_pose = current_pose; + { + const std::lock_guard lock(mutex_); + status_.back_finished = true; + status_.planner_type = PlannerType::STOP; + status_.pull_out_path.partial_paths.clear(); + status_.pull_out_path.partial_paths.push_back(stop_path); + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + status_.pull_out_start_pose = current_pose; + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; + } path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; @@ -935,6 +975,68 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() return output; } +bool StartPlannerModule::planFreespacePath() +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto & route_handler = planner_data_->route_handler; + + const double end_pose_search_start_distance = parameters_->end_pose_search_start_distance; + const double end_pose_search_end_distance = parameters_->end_pose_search_end_distance; + const double end_pose_search_interval = parameters_->end_pose_search_interval; + + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + + const double s_start = std::max(0.0, current_arc_coords.length + end_pose_search_start_distance); + const double s_end = current_arc_coords.length + end_pose_search_end_distance; + + auto center_line_path = utils::resamplePathWithSpline( + route_handler->getCenterLinePath(current_lanes, s_start, s_end), end_pose_search_interval); + + for (const auto & p : center_line_path.points) { + const Pose end_pose = p.point.pose; + freespace_planner_->setPlannerData(planner_data_); + auto freespace_path = freespace_planner_->plan(current_pose, end_pose); + + if (!freespace_path) { + continue; + } + + const std::lock_guard lock(mutex_); + status_.pull_out_path = *freespace_path; + status_.pull_out_start_pose = current_pose; + status_.planner_type = freespace_planner_->getPlannerType(); + status_.is_safe = true; + status_.back_finished = true; + return true; + } + + return false; +} + +void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const +{ + if (status_.planner_type == PlannerType::FREESPACE) { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + } else { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, generateDrivableLanes(*output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + } +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index ebfac8a347134..f4aabb13b8916 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -25,7 +25,9 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} +: PullOverPlannerBase{node, parameters}, + velocity_{parameters.freespace_parking_velocity}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); @@ -58,8 +60,18 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) return {}; } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); + PathWithLaneId path = - utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_); + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); const auto reverse_indices = utils::getReversingIndices(path); std::vector partial_paths = utils::dividePath(path, reverse_indices); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index ee23ecc005142..34ba607938385 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -303,4 +303,28 @@ bool checkCollision( return true; } + +bool checkCollisionWithExtraStoppingMargin( + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, + const double base_to_front, const double base_to_rear, const double width, + const double maximum_deceleration, const double collision_check_margin, + const double max_extra_stopping_margin) +{ + for (const auto & p : ego_path.points) { + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, + max_extra_stopping_margin); + + const auto ego_polygon = tier4_autoware_utils::toFootprint( + p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + + for (const auto & object : dynamic_objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, ego_polygon); + if (distance < collision_check_margin) return true; + } + } + + return false; +} } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index d2d70b0a46dfc..ec14a064bf51b 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -336,14 +336,28 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity) + const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, + const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; - for (const auto & waypoint : waypoints.waypoints) { + for (size_t i = 0; i < waypoints.waypoints.size(); ++i) { + const auto & waypoint = waypoints.waypoints.at(i); PathPointWithLaneId point{}; point.point.pose = waypoint.pose.pose; - // point.lane_id = // todo + // put the lane that contain waypoints in lane_ids. + bool is_in_lanes = false; + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + is_in_lanes = true; + } + } + // If none of them corresponds, assign the previous lane_ids. + if (!is_in_lanes && i > 0) { + point.lane_ids = path.points.at(i - 1).lane_ids; + } + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; path.points.push_back(point); } diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp new file mode 100644 index 0000000000000..ba34901d9df6a --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -0,0 +1,115 @@ +// Copyright 2023 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. + +#include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" + +#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner/utils/utils.hpp" + +#include + +#include +#include + +namespace behavior_path_planner +{ +FreespacePullOut::FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const vehicle_info_util::VehicleInfo & vehicle_info) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} +{ + freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + if (parameters.freespace_planner_algorithm == "astar") { + use_back_ = parameters.astar_parameters.use_back; + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.freespace_planner_algorithm == "rrtstar") { + use_back_ = true; // no option for disabling back in rrtstar + planner_ = std::make_unique( + parameters.freespace_planner_common_parameters, vehicle_shape, + parameters.rrt_star_parameters); + } +} + +boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +{ + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + + planner_->setMap(*planner_data_->costmap); + + const bool found_path = planner_->makePlan(start_pose, end_pose); + if (!found_path) { + return {}; + } + + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // find candidate paths + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); + + const PathWithLaneId path = + utils::convertWayPointsToPathWithLaneId(planner_->getWaypoints(), velocity_, lanes); + + const auto reverse_indices = utils::getReversingIndices(path); + std::vector partial_paths = utils::dividePath(path, reverse_indices); + + // remove points which are near the end pose + PathWithLaneId & last_path = partial_paths.back(); + const double th_end_distance = 1.0; + for (auto it = last_path.points.begin(); it != last_path.points.end(); ++it) { + const size_t index = std::distance(last_path.points.begin(), it); + if (index == 0) continue; + const double distance = + tier4_autoware_utils::calcDistance2d(end_pose.position, it->point.pose.position); + if (distance < th_end_distance) { + last_path.points.erase(it, last_path.points.end()); + break; + } + } + + // push back generate road lane path between end pose and goal pose to last path + const auto & goal_pose = route_handler->getGoalPose(); + constexpr double offset_from_end_pose = 1.0; + const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); + const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); + const auto path_end_info = + start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose); + const double s_end = path_end_info.first; + const bool path_terminal_is_goal = path_end_info.second; + + const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + + const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; + utils::correctDividedPathVelocity(partial_paths); + if (!path_terminal_is_goal) { + // not need to stop at terminal + last_path.points.back().point.longitudinal_velocity_mps = original_terminal_velocity; + } + + PullOutPath pull_out_path{}; + pull_out_path.partial_paths = partial_paths; + pull_out_path.start_pose = start_pose; + pull_out_path.end_pose = end_pose; + + return pull_out_path; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 5eb22fbfe14ee..506b3ff2d634b 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3180,4 +3180,21 @@ void extractObstaclesFromDrivableArea( bound = drivable_area_processing::updateBoundary(bound, unique_polygons); } } + +lanelet::ConstLanelets combineLanelets( + const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes) +{ + lanelet::ConstLanelets combined_lanes = base_lanes; + for (const auto & added_lane : added_lanes) { + const auto it = std::find_if( + combined_lanes.begin(), combined_lanes.end(), + [&added_lane](const lanelet::ConstLanelet & lane) { return lane.id() == added_lane.id(); }); + if (it == combined_lanes.end()) { + combined_lanes.push_back(added_lane); + } + } + + return combined_lanes; +} + } // namespace behavior_path_planner::utils