From 5fa0141664553017980dfc7673c5faa323d7a4c7 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 24 Dec 2024 13:05:22 +0900 Subject: [PATCH] refactor(lane_change): separate path-related function to utils/path (#9633) * refactor(lane_change): separate path-related function to utils/path Signed-off-by: Zulfaqar Azmi * remove old terminal lane change computation Signed-off-by: Zulfaqar Azmi * doxygen comments Signed-off-by: Zulfaqar Azmi * remove frenet planner header Signed-off-by: Zulfaqar Azmi * minor refactoring by throwing instead Signed-off-by: Zulfaqar Azmi * minor refactoring Signed-off-by: Zulfaqar Azmi * fix docstring and remove redundant argument Signed-off-by: Zulfaqar Azmi * get logger in header Signed-off-by: Zulfaqar Azmi * add docstring Signed-off-by: Zulfaqar Azmi * rename function is_colliding Signed-off-by: Zulfaqar Azmi * Fix failing test Signed-off-by: Zulfaqar Azmi * fix for failing scenario caused by prepare velocity Signed-off-by: Zulfaqar Azmi * fix error message Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../CMakeLists.txt | 1 + .../base_class.hpp | 5 - .../scene.hpp | 48 +- .../structs/data.hpp | 2 + .../utils/path.hpp | 102 ++++ .../utils/utils.hpp | 95 ++-- .../src/scene.cpp | 438 +++--------------- .../src/utils/path.cpp | 292 ++++++++++++ .../src/utils/utils.cpp | 244 ++-------- .../test/test_lane_change_scene.cpp | 1 + 10 files changed, 561 insertions(+), 667 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index d1b6bfed868f0..0d56a28aaf450 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/calculation.cpp src/utils/markers.cpp src/utils/utils.cpp + src/utils/path.cpp ) if(BUILD_TESTING) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 5317b94db2d6b..ce2552a41d380 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -102,8 +102,6 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; - virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; - virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_trailing_object) = 0; @@ -235,9 +233,6 @@ class LaneChangeBase protected: virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - virtual bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 9bf66722dcec2..6367caaea6829 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -64,8 +64,6 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; - BehaviorModuleOutput getTerminalLaneChangePath() const override; - BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; @@ -128,14 +126,6 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const override; - - PathWithLaneId getTargetSegment( - const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, - const double target_lane_length, const double lane_changing_length, - const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, @@ -143,32 +133,19 @@ class NormalLaneChange : public LaneChangeBase bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - LaneChangePath get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const; - bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; - std::optional calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, - const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const; - - bool has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; + CollisionCheckDebugMap & debug_data) const; - bool is_collided( + bool is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, @@ -178,24 +155,6 @@ class NormalLaneChange : public LaneChangeBase bool is_ego_stuck() const; - /** - * @brief Checks if the given pose is a valid starting point for a lane change. - * - * This function determines whether the given pose (position) of the vehicle is within - * the area of either the target neighbor lane or the target lane itself. It uses geometric - * checks to see if the start point of the lane change is covered by the polygons representing - * these lanes. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes. - * @param pose The current pose of the vehicle - * - * @return `true` if the pose is within either the target neighbor lane or the target lane; - * `false` otherwise. - */ - bool is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const; - bool check_prepare_phase() const; void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); @@ -213,7 +172,6 @@ class NormalLaneChange : public LaneChangeBase std::vector path_after_intersection_; double stop_time_{0.0}; - static constexpr double floating_err_th{1e-3}; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index 4dcb1c7b685d0..5f36be806bba4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp @@ -160,6 +160,8 @@ struct TargetObjects : leading(std::move(leading)), trailing(std::move(trailing)) { } + + [[nodiscard]] bool empty() const { return leading.empty() && trailing.empty(); } }; enum class ModuleType { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp new file mode 100644 index 0000000000000..dcc327b4793e1 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 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_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" + +#include + +#include + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::LaneChangePath; +using behavior_path_planner::lane_change::CommonDataPtr; + +/** + * @brief Generates a prepare segment for a lane change maneuver. + * + * This function generates the "prepare segment" of the path by trimming it to the specified length, + * adjusting longitudinal velocity for acceleration or deceleration, and ensuring the starting point + * meets necessary constraints for a lane change. + * + * @param common_data_ptr Shared pointer to CommonData containing current and target lane + * information, vehicle parameters, and ego state. + * @param prev_module_path The input path from the previous module, which will be used + * as the base for the prepare segment. + * @param prep_metric Preparation metrics containing desired length and velocity for the segment. + * @param prepare_segment Output parameter where the resulting prepare segment path will be stored. + * + * @throws std::logic_error If the lane change start point is behind the target lanelet or + * if lane information is invalid. + * + * @return true if the prepare segment is successfully generated, false otherwise. + */ +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment); + +/** + * @brief Generates the candidate path for a lane change maneuver. + * + * This function calculates the candidate lane change path based on the provided preparation + * and lane change metrics. It resamples the target lane reference path, determines the start + * and end poses for the lane change, and constructs the shift line required for the maneuver. + * The function ensures that the resulting path satisfies length and distance constraints. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient data. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * @param lc_metric Metrics for the lane change phase, including path length and velocity. + * @param prep_segment The path segment prepared before initiating the lane change. + * @param sorted_lane_ids A vector of sorted lane IDs used for updating lane information in the + * path. + * @param lc_start_pose The pose where the lane change begins. + * @param shift_length The lateral distance to shift during the lane change maneuver. + * + * @throws std::logic_error If the target lane reference path is empty, candidate path generation + * fails, or the resulting path length exceeds terminal constraints. + * + * @return LaneChangePath The constructed candidate lane change path. + */ +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length); + +/** + * @brief Constructs a candidate path for a lane change maneuver. + * + * This function generates a candidate lane change path by shifting the target lane's reference path + * and combining it with the prepare segment. It verifies the path's validity by checking the yaw + * difference, adjusting longitudinal velocity, and updating lane IDs based on the provided lane + * sorting information. + * + * @param lane_change_info Information about the lane change, including shift line and target + * velocity. + * @param prepare_segment The path segment leading up to the lane change. + * @param target_lane_reference_path The reference path of the target lane. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the candidate path's lane IDs. + * + * @return std::optional The constructed candidate path if valid, or std::nullopt + * if the path fails any constraints. + */ +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids); +} // namespace autoware::behavior_path_planner::utils::lane_change +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 0adb9409fb4b0..69362994dbbac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -52,6 +52,7 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; @@ -61,12 +62,11 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; -bool is_mandatory_lane_change(const ModuleType lc_type); +rclcpp::Logger get_logger(); -double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity); +bool is_mandatory_lane_change(const ModuleType lc_type); -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); std::vector replaceWithSortedIds( @@ -79,27 +79,10 @@ lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type); -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes); - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids); - -ShiftLine get_lane_changing_shift_line( - const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, - const PathWithLaneId & reference_path, const double shift_length); - -PathWithLaneId get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval); - std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); @@ -141,8 +124,8 @@ bool isParkedObject( * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects * which pass isParkedObject() check will be considered. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, - * and transient data. + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, + * parameters, and transient data. * @param lane_change_path Candidate lane change path to apply checks on. * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include * target lane leading static objects). @@ -202,8 +185,8 @@ rclcpp::Logger getLogger(const std::string & type); * The footprint is determined by the vehicle's pose and its dimensions, including the distance * from the base to the front and rear ends of the vehicle, as well as its width. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions - * and pose information. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's + * dimensions and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ @@ -233,15 +216,15 @@ bool is_within_intersection( /** * @brief Determines if a polygon is within lanes designated for turning. * - * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). - * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's - * area. + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding + * 'straight'). It evaluates the lanelet's 'turn_direction' attribute and determines overlap with + * the lanelet's area. * * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. * @param polygon The polygon to be checked for its presence within turn direction lanes. * - * @return bool True if the polygon is within a lane designated for turning, false if it is within a - * straight lane or no turn direction is specified. + * @return bool True if the polygon is within a lane designated for turning, false if it is within + * a straight lane or no turn direction is specified. */ bool is_within_turn_direction_lanes( const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -276,8 +259,8 @@ double get_distance_to_next_regulatory_element( * * @param common_data_ptr Pointer to the common data structure containing parameters for lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the current - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * current lane. * @param dist_to_target_lane_start The distance to the start of the target lane from the ego * vehicle. * @param path The current path of the ego vehicle, containing path points and lane information. @@ -297,8 +280,8 @@ double get_min_dist_to_current_lanes_obj( * * @param common_data_ptr Pointer to the common data structure containing parameters for the lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the target - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * target lane. * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. * @param path The current path of the ego vehicle, containing path points and lane information. * @return true if there is an object in the target lane that influences the stop point decision; @@ -365,14 +348,15 @@ bool has_overtaking_turn_lane_object( * * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, * vehicle dimensions, lane polygons, and behavior parameters. - * @param object An extended predicted object representing a potential obstacle in the environment. + * @param object An extended predicted object representing a potential obstacle in the + * environment. * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the * current lanes. * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. - * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of - * the lane. - * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or - * outside boundaries). + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point + * of the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, + * or outside boundaries). * @param trailing_objects Reference to a collection for storing trailing objects. * * @return true if the object is classified as either leading or trailing, false otherwise. @@ -415,5 +399,38 @@ std::vector get_preceding_lanes(const CommonDataPtr & co */ bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); + +/** + * @brief Converts a lane change path into multiple predicted paths with varying acceleration + * profiles. + * + * This function generates a set of predicted paths for the ego vehicle during a lane change, + * using different acceleration values within the specified range. It accounts for deceleration + * sampling if the global minimum acceleration differs from the lane-changing acceleration. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and state information. + * @param lane_change_path The lane change path used to generate predicted paths. + * @param deceleration_sampling_num Number of samples for deceleration profiles to generate paths. + * + * @return std::vector> A collection of predicted paths, where + * each path is represented as a series of poses with associated velocity. + */ +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num); + +/** + * @brief Validates whether a given pose is a valid starting point for a lane change. + * + * This function checks if the specified pose lies within the polygons representing + * the target lane or its neighboring areas. This ensures that the starting point is + * appropriately positioned for initiating a lane change, even if previous paths were adjusted. + * + * @param common_data_ptr Shared pointer to CommonData containing lane polygon information. + * @param pose The pose to validate as a potential lane change starting point. + * + * @return true if the pose is within the target or target neighbor polygons, false otherwise. + */ +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index a1c2d8467d1fc..f8a200a9685a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -15,6 +15,7 @@ #include "autoware/behavior_path_lane_change_module/scene.hpp" #include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" +#include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -31,9 +32,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -379,48 +380,6 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } -BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const -{ - auto output = prev_module_output_; - - if (isAbortState() && abort_path_) { - output.path = abort_path_->path; - extendOutputDrivableArea(output); - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - return output; - } - - const auto & current_lanes = get_current_lanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); - return prev_module_output_; - } - - const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); - if (!terminal_path) { - RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); - return prev_module_output_; - } - - output.path = terminal_path->path; - output.turn_signal_info = updateOutputTurnSignal(); - - extendOutputDrivableArea(output); - - const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( - output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, - output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, - planner_data_->parameters.ego_nearest_yaw_threshold); - - return output; -} - BehaviorModuleOutput NormalLaneChange::generateOutput() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -922,43 +881,6 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } -bool NormalLaneChange::get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const -{ - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; - - if (current_lanes.empty() || target_lanes.empty()) { - return false; - } - - prepare_segment = prev_module_output_.path; - const size_t current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, getEgoPose(), 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - - if (prepare_segment.points.empty()) return false; - - const auto & lc_start_pose = prepare_segment.points.back().point.pose; - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; - - // lane changing start is at the end of prepare segment - const auto target_length_from_lane_change_start_pose = - utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { - throw std::logic_error("lane change start is behind target lanelet!"); - } - - return true; -} - lane_change::TargetObjects NormalLaneChange::get_target_objects( const FilteredLanesObjects & filtered_objects, [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const @@ -1107,41 +1029,6 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -PathWithLaneId NormalLaneChange::getTargetSegment( - const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, - const double target_lane_length, const double lane_changing_length, - const double lane_changing_velocity, const double buffer_for_next_lane_change) const -{ - const auto & route_handler = *getRouteHandler(); - const auto forward_path_length = planner_data_->parameters.forward_path_length; - - const double s_start = std::invoke([&lane_changing_start_pose, &target_lanes, - &lane_changing_length, &target_lane_length, - &buffer_for_next_lane_change]() { - const auto arc_to_start_pose = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - const double dist_from_front_target_lanelet = arc_to_start_pose.length + lane_changing_length; - const double end_of_lane_dist_without_buffer = target_lane_length - buffer_for_next_lane_change; - return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); - }); - - const double s_end = std::invoke( - [&s_start, &forward_path_length, &target_lane_length, &buffer_for_next_lane_change]() { - const double dist_from_start = s_start + forward_path_length; - const double dist_from_end = target_lane_length - buffer_for_next_lane_change; - return std::max( - std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); - }); - - PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); - for (auto & point : target_segment.points) { - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_velocity)); - } - - return target_segment; -} - std::vector NormalLaneChange::get_prepare_metrics() const { const auto & current_lanes = common_data_ptr_->lanes_ptr->current; @@ -1199,7 +1086,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto & current_lanes = get_current_lanes(); const auto & target_lanes = get_target_lanes(); - const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = get_target_objects(filtered_objects_, current_lanes); @@ -1239,7 +1125,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) PathWithLaneId prepare_segment; try { - if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, prep_metric, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1258,7 +1145,10 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto lane_changing_metrics = get_lane_changing_metrics( prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); + // set_prepare_velocity must only be called after computing lane change metrics, as lane change + // metrics rely on the prepare segment's original velocity as max_path_velocity. + utils::lane_change::set_prepare_velocity( + prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { const auto debug_print_lat = [&](const std::string & s) { @@ -1274,9 +1164,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath candidate_path; try { - candidate_path = get_candidate_path( - prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - shift_length); + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; @@ -1302,52 +1191,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return false; } -LaneChangePath NormalLaneChange::get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const -{ - const auto & route_handler = *getRouteHandler(); - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - - const auto resample_interval = - utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); - - if (target_lane_reference_path.points.empty()) { - throw std::logic_error("target_lane_reference_path is empty!"); - } - - const auto lc_end_pose = std::invoke([&]() { - const auto dist_to_lc_start = - lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; - const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; - return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); - }); - - const auto shift_line = utils::lane_change::get_lane_changing_shift_line( - lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - - LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - - const auto candidate_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - - if (!candidate_path) { - throw std::logic_error("failed to generate candidate path!"); - } - - if ( - candidate_path.value().info.length.sum() + - common_data_ptr_->transient_data.next_dist_buffer.min > - common_data_ptr_->transient_data.dist_to_terminal_end) { - throw std::logic_error("invalid candidate path length!"); - } - - return *candidate_path; -} - bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { @@ -1377,147 +1220,30 @@ bool NormalLaneChange::check_candidate_path_safety( throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); } + if ((target_objects.empty()) || candidate_path.path.points.empty()) { + RCLCPP_DEBUG(logger_, "There is nothing to check."); + return true; + } + constexpr size_t decel_sampling_num = 1; + const auto ego_predicted_paths = utils::lane_change::convert_to_predicted_paths( + common_data_ptr_, candidate_path, decel_sampling_num); + const auto safety_check_with_normal_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params, lane_change_debug_.collision_check_objects); if (!safety_check_with_normal_rss.is_safe && is_stuck) { const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, + lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } return safety_check_with_normal_rss.is_safe; } -std::optional NormalLaneChange::calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - - const auto minimum_lane_changing_velocity = - lane_change_parameters_->trajectory.min_lane_changing_velocity; - - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - - const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - - // lane changing start getEgoPose() is at the end of prepare segment - const auto current_lane_terminal_point = - lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); - - double distance_to_terminal_from_goal = 0; - if (is_goal_in_route) { - distance_to_terminal_from_goal = - utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); - } - - const auto lane_changing_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( - prev_module_output_.path.points, current_lane_terminal_point, - -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); - - if (!lane_changing_start_pose) { - RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); - return {}; - } - - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose.value()); - - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); - return {}; - } - - const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( - target_lanes, lane_changing_start_pose.value()); - - const auto [min_lateral_acc, max_lateral_acc] = - lane_change_parameters_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); - - const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( - shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); - - const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, - minimum_lane_changing_velocity, next_min_dist_buffer); - - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); - return {}; - } - - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; - lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{0.0, current_min_dist_buffer}; - lane_change_info.lane_changing_start = lane_changing_start_pose.value(); - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.lateral_acceleration = max_lateral_acc; - lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; - - if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { - RCLCPP_DEBUG( - logger_, - "Reject: lane changing points are not inside of the target preferred lanes or its " - "neighbors"); - return {}; - } - - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); - - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); - return {}; - } - - lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( - lane_changing_start_pose.value(), target_segment.points.front().point.pose, - target_lane_reference_path, shift_length); - - auto reference_segment = prev_module_output_.path; - const double length_to_lane_changing_start = autoware::motion_utils::calcSignedArcLength( - reference_segment.points, reference_segment.points.front().point.pose.position, - lane_changing_start_pose->position); - utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); - // remove terminal points because utils::clipPathLength() calculates extra long path - reference_segment.points.pop_back(); - reference_segment.points.back().point.longitudinal_velocity_mps = - static_cast(minimum_lane_changing_velocity); - - const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, - sorted_lane_ids); - - return terminal_lane_change_path; -} - PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1545,9 +1271,13 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, false}; } + const auto decel_sampling_num = + static_cast(lane_change_parameters_->cancel.deceleration_sampling_num); + const auto ego_predicted_paths = + utils::lane_change::convert_to_predicted_paths(common_data_ptr_, path, decel_sampling_num); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->safety.rss_params_for_abort, - static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); + path, ego_predicted_paths, target_objects, lane_change_parameters_->safety.rss_params_for_abort, + debug_data); { // only for debug purpose lane_change_debug_.collision_check_objects.clear(); @@ -1779,103 +1509,57 @@ bool NormalLaneChange::calcAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, - const utils::path_safety_checker::RSSparams & rss_params, const size_t deceleration_sampling_num, + const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); constexpr auto is_safe = true; constexpr auto is_object_behind_ego = true; - if (collision_check_objects.leading.empty() && collision_check_objects.trailing.empty()) { - RCLCPP_DEBUG(logger_, "There is nothing to check."); - return {is_safe, !is_object_behind_ego}; - } - const auto is_check_prepare_phase = check_prepare_phase(); - const auto all_decel_pattern_has_collision = - [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool { - return has_collision_with_decel_patterns( - lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase, - debug_data); + const auto all_paths_collide = [&](const auto & objects) { + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; + return ranges::all_of(ego_predicted_paths, [&](const auto & ego_predicted_path) { + const auto check_for_collision = [&](const auto & object) { + const auto selected_rss_param = (object.initial_twist.linear.x <= stopped_obj_vel_th) + ? lane_change_parameters_->safety.rss_params_for_parked + : rss_params; + return is_colliding( + lane_change_path, object, ego_predicted_path, selected_rss_param, is_check_prepare_phase, + debug_data); + }; + return ranges::any_of(objects, check_for_collision); + }); }; - if (all_decel_pattern_has_collision(collision_check_objects.trailing)) { + if (all_paths_collide(collision_check_objects.trailing)) { return {!is_safe, is_object_behind_ego}; } - if (all_decel_pattern_has_collision(collision_check_objects.leading)) { + if (all_paths_collide(collision_check_objects.leading)) { return {!is_safe, !is_object_behind_ego}; } return {is_safe, !is_object_behind_ego}; } -bool NormalLaneChange::has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const -{ - if (objects.empty()) { - return false; - } - - const auto & path = lane_change_path.path; - - if (path.points.empty()) { - return false; - } - - const auto bpp_param = *common_data_ptr_->bpp_param_ptr; - const auto global_min_acc = bpp_param.min_acc; - const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; - - const auto min_acc = std::min(lane_changing_acc, global_min_acc); - const auto sampling_num = - std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; - const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); - - std::vector acceleration_values(sampling_num); - std::iota(acceleration_values.begin(), acceleration_values.end(), 0); - - std::transform( - acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), - [&](double n) { return lane_changing_acc + n * acc_resolution; }); - - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - const auto all_collided = std::all_of( - acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { - const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( - common_data_ptr_, lane_change_path, acceleration); - - return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th) - ? lane_change_parameters_->safety.rss_params_for_parked - : rss_param; - return is_collided( - lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, - debug_data); - }); - }); - - return all_collided; -} - -bool NormalLaneChange::is_collided( +bool NormalLaneChange::is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const { - constexpr auto is_collided{true}; + constexpr auto is_colliding{true}; if (lane_change_path.path.points.empty()) { - return !is_collided; + return !is_colliding; } if (ego_predicted_path.empty()) { - return !is_collided; + return !is_colliding; } const auto & lanes_polygon_ptr = common_data_ptr_->lanes_polygon_ptr; @@ -1883,7 +1567,7 @@ bool NormalLaneChange::is_collided( const auto & expanded_target_polygon = lanes_polygon_ptr->target; if (current_polygon.empty() || expanded_target_polygon.empty()) { - return !is_collided; + return !is_colliding; } constexpr auto is_safe{true}; @@ -1934,10 +1618,10 @@ bool NormalLaneChange::is_collided( utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, !is_safe); - return is_collided; + return is_colliding; } utils::path_safety_checker::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); - return !is_collided; + return !is_colliding; } double NormalLaneChange::get_max_velocity_for_safety_check() const @@ -2004,18 +1688,6 @@ bool NormalLaneChange::is_ego_stuck() const return has_object_blocking; } -bool NormalLaneChange::is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const -{ - const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); - - const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; - const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; - - return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || - boost::geometry::covered_by(lc_start_point, target_lane_poly); -} - void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) { const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp new file mode 100644 index 0000000000000..d7303d7d1df2d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -0,0 +1,292 @@ +// Copyright 2024 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 "autoware/behavior_path_lane_change_module/utils/path.hpp" + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +using autoware::behavior_path_planner::LaneChangeInfo; +using autoware::behavior_path_planner::PathPointWithLaneId; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::PathWithLaneId; +using autoware::behavior_path_planner::ShiftedPath; +using autoware::behavior_path_planner::lane_change::CommonDataPtr; +using autoware::behavior_path_planner::lane_change::LCParamPtr; + +using autoware::behavior_path_planner::LaneChangePhaseMetrics; +using autoware::behavior_path_planner::ShiftLine; +using geometry_msgs::msg::Pose; + +double calc_resample_interval( + const double lane_changing_length, const double lane_changing_velocity) +{ + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + return std::max( + lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); +} + +PathWithLaneId get_reference_path_from_target_Lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + + const auto lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + + const double s_start = lane_change_start_arc_position.length; + const double s_end = std::invoke([&]() { + const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; + if (is_goal_in_route) { + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - + next_lc_buffer; + return std::min(dist_from_lc_start, s_goal); + } + return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); + }); + + constexpr double epsilon = 1e-4; + if (s_end - s_start + epsilon < lane_changing_length) { + return PathWithLaneId(); + } + + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + return autoware::behavior_path_planner::utils::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); +} + +ShiftLine get_lane_changing_shift_line( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ + ShiftLine shift_line; + shift_line.end_shift_length = shift_length; + shift_line.start = lane_changing_start_pose; + shift_line.end = lane_changing_end_pose; + shift_line.start_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_start_pose.position); + shift_line.end_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_end_pose.position); + + return shift_line; +} + +ShiftedPath get_shifted_path( + const PathWithLaneId & target_lane_reference_path, const LaneChangeInfo & lane_change_info) +{ + const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; + + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftLine(lane_change_info.shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); + const auto initial_lane_changing_velocity = lane_change_info.velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); + path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); + + constexpr auto offset_back = false; + ShiftedPath shifted_path; + [[maybe_unused]] const auto success = path_shifter.generate(&shifted_path, offset_back); + return shifted_path; +} + +std::optional exceed_yaw_threshold( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const double yaw_th_rad) +{ + const auto & prepare = prepare_segment.points; + const auto & lane_changing = lane_changing_segment.points; + + if (prepare.size() <= 1 || lane_changing.size() <= 1) { + return std::nullopt; + } + + const auto & p1 = std::prev(prepare.end() - 1)->point.pose; + const auto & p2 = std::next(lane_changing.begin())->point.pose; + const auto yaw_diff_rad = std::abs(autoware::universe_utils::normalizeRadian( + tf2::getYaw(p1.orientation) - tf2::getYaw(p2.orientation))); + if (yaw_diff_rad > yaw_th_rad) { + return yaw_diff_rad; + } + return std::nullopt; +} +}; // namespace + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::lane_change::CommonDataPtr; + +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const LaneChangePhaseMetrics prep_metric, PathWithLaneId & prepare_segment) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto backward_path_length = common_data_ptr->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + throw std::logic_error("Empty lanes!"); + } + + prepare_segment = prev_module_path; + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, common_data_ptr->get_ego_pose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prep_metric.length, backward_path_length); + + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_start_pose); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + return true; +} + +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + + const auto resample_interval = calc_resample_interval(lc_metric.length, prep_metric.velocity); + + if (prep_segment.points.empty()) { + throw std::logic_error("Empty prepare segment!"); + } + + const auto & lc_start_pose = prep_segment.points.back().point.pose; + const auto target_lane_reference_path = get_reference_path_from_target_Lane( + common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); + + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("Empty target reference!"); + } + + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metric.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); + + const auto shift_line = get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); + + LaneChangeInfo lane_change_info{prep_metric, lc_metric, lc_start_pose, lc_end_pose, shift_line}; + + if ( + lane_change_info.length.sum() + common_data_ptr->transient_data.next_dist_buffer.min > + common_data_ptr->transient_data.dist_to_terminal_end) { + throw std::logic_error("invalid candidate path length!"); + } + + return utils::lane_change::construct_candidate_path( + lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); +} + +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids) +{ + const auto & shift_line = lane_change_info.shift_line; + const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; + + auto shifted_path = get_shifted_path(target_lane_reference_path, lane_change_info); + + if (shifted_path.path.points.empty()) { + throw std::logic_error("Failed to generate shifted path."); + } + + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + throw std::logic_error("Path points are removed by PathShifter."); + } + + const auto lc_end_idx_opt = autoware::motion_utils::findNearestIndex( + shifted_path.path.points, lane_change_info.lane_changing_end); + + if (!lc_end_idx_opt) { + throw std::logic_error("Lane change end idx not found on target path."); + } + + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lc_end_idx_opt) { + point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); + continue; + } + const auto nearest_idx = + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; + } + + constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0); + if ( + const auto yaw_diff_opt = + exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) { + std::stringstream err_msg; + err_msg << "Excessive yaw difference " << yaw_diff_opt.value() << " which exceeds the " + << yaw_diff_th << " radian threshold."; + throw std::logic_error(err_msg.str()); + } + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + candidate_path.info = lane_change_info; + + return candidate_path; +} +} // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 2b1c744a926e3..147ef3856ac4e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -19,7 +19,6 @@ #include "autoware/behavior_path_planner_common/parameters.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" @@ -29,15 +28,14 @@ #include #include #include -#include #include +#include #include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include @@ -72,7 +70,6 @@ using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; -using lanelet::ArcCoordinates; using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() @@ -88,29 +85,20 @@ bool is_mandatory_lane_change(const ModuleType lc_type) lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; } -double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity) -{ - constexpr auto min_resampling_points{30.0}; - constexpr auto resampling_dt{0.2}; - return std::max( - lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); -} - -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { - if (current_velocity < prepare_velocity) { - // acceleration - for (auto & point : prepare_segment.points) { - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); - } - } else { + if (current_velocity >= prepare_velocity) { // deceleration prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( prepare_segment.points.back().point.longitudinal_velocity_mps, static_cast(prepare_velocity)); + return; + } + // acceleration + for (auto & point : prepare_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); } } @@ -132,34 +120,9 @@ lanelet::ConstLanelets get_target_neighbor_lanes( } } } - return neighbor_lanes; } -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) -{ - const auto current_lane_poly = - lanelet::utils::getPolygonFromArcLength(current_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly = - lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); - const auto current_lane_poly_2d = lanelet::utils::to2D(current_lane_poly).basicPolygon(); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_poly).basicPolygon(); - for (const auto & pt : path.points) { - const lanelet::BasicPoint2d ll_pt(pt.point.pose.position.x, pt.point.pose.position.y); - const auto is_in_current = boost::geometry::covered_by(ll_pt, current_lane_poly_2d); - if (is_in_current) { - continue; - } - const auto is_in_target = boost::geometry::covered_by(ll_pt, target_lane_poly_2d); - if (!is_in_target) { - return false; - } - } - return true; -} - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) @@ -190,152 +153,6 @@ bool path_footprint_exceeds_target_lane_bound( return false; } -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids) -{ - const auto & shift_line = lane_change_info.shift_line; - const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; - const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; - const auto lane_change_velocity = lane_change_info.velocity; - - PathShifter path_shifter; - path_shifter.setPath(target_lane_reference_path); - path_shifter.addShiftLine(shift_line); - path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); - ShiftedPath shifted_path; - - // offset front side - bool offset_back = false; - - const auto initial_lane_changing_velocity = lane_change_velocity.lane_changing; - path_shifter.setVelocity(initial_lane_changing_velocity); - path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); - - if (!path_shifter.generate(&shifted_path, offset_back)) { - RCLCPP_DEBUG(get_logger(), "Failed to generate shifted path."); - } - - // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path - // shifter. - if (shifted_path.path.points.size() < shift_line.end_idx + 1) { - RCLCPP_DEBUG(get_logger(), "Path points are removed by PathShifter."); - return std::nullopt; - } - - LaneChangePath candidate_path; - candidate_path.info = lane_change_info; - - const auto lane_change_end_idx = autoware::motion_utils::findNearestIndex( - shifted_path.path.points, candidate_path.info.lane_changing_end); - - if (!lane_change_end_idx) { - RCLCPP_DEBUG(get_logger(), "Lane change end idx not found on target path."); - return std::nullopt; - } - - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *lane_change_end_idx) { - point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); - continue; - } - const auto nearest_idx = - autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); - point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; - } - - // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster - const bool enable_path_check_in_lanelet = false; - - // check candidate path is in lanelet - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - if ( - enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, current_lanes, target_lanes)) { - return std::nullopt; - } - - if (prepare_segment.points.size() > 1 && shifted_path.path.points.size() > 1) { - const auto & prepare_segment_second_last_point = - std::prev(prepare_segment.points.end() - 1)->point.pose; - const auto & lane_change_start_from_shifted = - std::next(shifted_path.path.points.begin())->point.pose; - const auto yaw_diff2 = std::abs(autoware::universe_utils::normalizeRadian( - tf2::getYaw(prepare_segment_second_last_point.orientation) - - tf2::getYaw(lane_change_start_from_shifted.orientation))); - if (yaw_diff2 > autoware::universe_utils::deg2rad(5.0)) { - RCLCPP_DEBUG( - get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", - autoware::universe_utils::rad2deg(yaw_diff2)); - return std::nullopt; - } - } - - candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); - candidate_path.shifted_path = shifted_path; - - return std::optional{candidate_path}; -} - -PathWithLaneId get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval) -{ - const auto & route_handler = *common_data_ptr->route_handler_ptr; - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; - const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; - const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; - const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; - - const ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - - const double s_start = lane_change_start_arc_position.length; - const double s_end = std::invoke([&]() { - const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; - if (is_goal_in_route) { - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - - next_lc_buffer; - return std::min(dist_from_lc_start, s_goal); - } - return std::min(dist_from_lc_start, target_lane_length - next_lc_buffer); - }); - - constexpr double epsilon = 1e-4; - if (s_end - s_start + epsilon < lane_changing_length) { - return PathWithLaneId(); - } - - const auto lane_changing_reference_path = - route_handler.getCenterLinePath(target_lanes, s_start, s_end); - - return utils::resamplePathWithSpline( - lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); -} - -ShiftLine get_lane_changing_shift_line( - const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, - const PathWithLaneId & reference_path, const double shift_length) -{ - ShiftLine shift_line; - shift_line.end_shift_length = shift_length; - shift_line.start = lane_changing_start_pose; - shift_line.end = lane_changing_end_pose; - shift_line.start_idx = autoware::motion_utils::findNearestIndex( - reference_path.points, lane_changing_start_pose.position); - shift_line.end_idx = autoware::motion_utils::findNearestIndex( - reference_path.points, lane_changing_end_pose.position); - - return shift_line; -} - std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) @@ -631,6 +448,7 @@ std::vector convert_to_predicted_path( const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; std::vector predicted_path; + predicted_path.reserve(static_cast(std::ceil(duration / resolution))); // prepare segment for (double t = 0.0; t < prepare_time; t += resolution) { @@ -647,6 +465,7 @@ std::vector convert_to_predicted_path( initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare); const auto offset = initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; + for (double t = prepare_time; t < duration; t += resolution) { const auto delta_t = t - prepare_time; const auto velocity = std::clamp( @@ -1294,4 +1113,39 @@ bool object_path_overlaps_lanes( return !boost::geometry::disjoint(path, lanes_polygon); }); } + +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num) +{ + static constexpr double floating_err_th{1e-3}; + const auto bpp_param = *common_data_ptr->bpp_param_ptr; + const auto global_min_acc = bpp_param.min_acc; + const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; + + const auto min_acc = std::min(lane_changing_acc, global_min_acc); + const auto sampling_num = + std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; + const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); + + const auto ego_predicted_path = [&](size_t n) { + auto acc = lane_changing_acc + static_cast(n) * acc_resolution; + return utils::lane_change::convert_to_predicted_path(common_data_ptr, lane_change_path, acc); + }; + + return ranges::views::iota(0UL, sampling_num) | ranges::views::transform(ego_predicted_path) | + ranges::to(); +} + +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) +{ + const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); + + const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; + const auto & target_lane_poly = common_data_ptr->lanes_polygon_ptr->target; + + // Check the target lane because the previous approved path might be shifted by avoidance module + return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || + boost::geometry::covered_by(lc_start_point, target_lane_poly); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index 96b24d5aa9a9e..114a38a77876c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -258,6 +258,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) constexpr auto is_approved = true; ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); planner_data_->self_odometry = set_odometry(ego_pose_); + normal_lane_change_->setData(planner_data_); set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects();