From 3d1f6c01c75ad66d9b03084aa67361f110aadcda Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Wed, 10 Aug 2022 16:33:29 +0900 Subject: [PATCH] refactor(behavior_path_planner): lane change refactoring Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module.hpp | 34 +- .../lane_change/lane_change_module_data.hpp | 59 +++ .../lane_change/lane_change_path.hpp | 4 + .../scene_module/lane_change/util.hpp | 56 ++- .../lane_change/lane_change_module.cpp | 169 ++++---- .../src/scene_module/lane_change/util.cpp | 397 +++++++++++------- .../behavior_path_planner/src/utilities.cpp | 2 +- 7 files changed, 442 insertions(+), 279 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 5770c16beb42c..a331269871215 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_HPP_ #include "behavior_path_planner/scene_module/lane_change/debug.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" @@ -36,38 +37,6 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using marker_utils::CollisionCheckDebug; -struct LaneChangeParameters -{ - double lane_change_prepare_duration; - double lane_changing_duration; - double minimum_lane_change_prepare_distance; - double lane_change_finish_judge_buffer; - double minimum_lane_change_velocity; - double prediction_time_resolution; - double maximum_deceleration; - int lane_change_sampling_num; - double abort_lane_change_velocity_thresh; - double abort_lane_change_angle_thresh; - double abort_lane_change_distance_thresh; - bool enable_abort_lane_change; - bool enable_collision_check_at_prepare_phase; - bool use_predicted_path_outside_lanelet; - bool use_all_predicted_path; - bool publish_debug_marker; -}; - -struct LaneChangeStatus -{ - PathWithLaneId lane_follow_path; - LaneChangePath lane_change_path; - lanelet::ConstLanelets current_lanes; - lanelet::ConstLanelets lane_change_lanes; - std::vector lane_follow_lane_ids; - std::vector lane_change_lane_ids; - bool is_safe; - double start_distance; -}; - class LaneChangeModule : public SceneModuleInterface { public: @@ -159,7 +128,6 @@ class LaneChangeModule : public SceneModuleInterface } PathWithLaneId getReferencePath() const; - lanelet::ConstLanelets getCurrentLanes() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; std::pair getSafePath( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp new file mode 100644 index 0000000000000..c67f3c17d152b --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" +#include "lanelet2_core/geometry/Lanelet.h" + +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" + +#include + +namespace behavior_path_planner +{ +struct LaneChangeParameters +{ + double lane_change_prepare_duration; + double lane_changing_duration; + double minimum_lane_change_prepare_distance; + double lane_change_finish_judge_buffer; + double minimum_lane_change_velocity; + double prediction_time_resolution; + double maximum_deceleration; + int lane_change_sampling_num; + double abort_lane_change_velocity_thresh; + double abort_lane_change_angle_thresh; + double abort_lane_change_distance_thresh; + bool enable_abort_lane_change; + bool enable_collision_check_at_prepare_phase; + bool use_predicted_path_outside_lanelet; + bool use_all_predicted_path; + bool publish_debug_marker; +}; + +struct LaneChangeStatus +{ + PathWithLaneId lane_follow_path; + LaneChangePath lane_change_path; + lanelet::ConstLanelets current_lanes; + lanelet::ConstLanelets lane_change_lanes; + std::vector lane_follow_lane_ids; + std::vector lane_change_lane_ids; + bool is_safe; + double start_distance; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp index fad9860e830f9..9d24060e60eb7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_path.hpp @@ -19,6 +19,8 @@ #include +#include + namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -31,5 +33,7 @@ struct LaneChangePath double preparation_length{0.0}; double lane_change_length{0.0}; }; +using LaneChangePaths = std::vector; + } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index dcd9d97c3efc2..7d1664a48eec5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -43,47 +43,89 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); +PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); + double getExpectedVelocityWhenDecelerate( const double & current_velocity, const double & expected_acceleration, const double & lane_change_prepare_duration); + double getDistanceWhenDecelerate( const double & velocity, const double & expected_acceleration, const double & duration, const double & minimum_distance); -std::vector getLaneChangePaths( + +std::optional constructCandidatePath( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point, + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + const double & acceleration, const double & prepare_distance, const double & lane_change_distance, + const double & lane_changing_duration, const double & minimum_lane_change_velocity); + +LaneChangePaths getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter); -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, + const BehaviorPathPlannerParameters & common_parameter, + const behavior_path_planner::LaneChangeParameters & parameter); + +LaneChangePaths selectValidPaths( + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); + bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & ros_parameters, LaneChangePath * selected_path, std::unordered_map & debug_data); + bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const size_t current_seg_idx, const Twist & current_twist, + const size_t & current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & lane_change_parameters, std::unordered_map & debug_data, const bool use_buffer = true, const double acceleration = 0.0); + bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs); + +ShiftPoint getLaneChangeShiftPoint( + const PathWithLaneId & path1, const PathWithLaneId & path2, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path); + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double & prepare_distance, + const double & lane_changing_distance, const double & forward_path_length); + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & in_target_front_pose, const Pose & in_target_end_pose); + +PathWithLaneId getLaneChangePathPrepareSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, + const Pose & current_pose, const double & backward_path_length, const double & prepare_distance, + const double & prepare_duration, const double & minimum_lane_change_velocity); + +PathWithLaneId getLaneChangePathLaneChangingSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, + const Pose & current_pose, const double & forward_path_length, const double & prepare_distance, + const double & lane_change_distance, const double & minimum_lane_change_length, + const double & lane_change_distance_buffer, const double & lane_changing_duration, + const double & minimum_lane_change_velocity); + } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 703984da93564..e8c67a6de5471 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -53,7 +53,7 @@ BehaviorModuleOutput LaneChangeModule::run() RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); current_state_ = BT::NodeStatus::RUNNING; is_activated_ = isActivated(); - const auto output = plan(); + auto output = plan(); const auto turn_signal_info = output.turn_signal_info; const auto current_pose = planner_data_->self_pose->pose; const double start_distance = motion_utils::calcSignedArcLength( @@ -97,7 +97,7 @@ bool LaneChangeModule::isExecutionRequested() const } // Get lane change lanes - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); // Find lane change path @@ -115,7 +115,7 @@ bool LaneChangeModule::isExecutionReady() const } // Get lane change lanes - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); LaneChangePath selected_path; @@ -147,16 +147,17 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { - constexpr double RESAMPLE_INTERVAL = 1.0; + constexpr double RESAMPLE_INTERVAL{1.0}; auto path = util::resamplePathWithSpline(status_.lane_change_path.path, RESAMPLE_INTERVAL); // Generate drivable area { - const auto common_parameters = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; lanelet::ConstLanelets lanes; + lanes.reserve(status_.current_lanes.size() + status_.lane_change_lanes.size()); lanes.insert(lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end()); lanes.insert(lanes.end(), status_.lane_change_lanes.begin(), status_.lane_change_lanes.end()); - const double resolution = common_parameters.drivable_area_resolution; + const double & resolution = common_parameters.drivable_area_resolution; path.drivable_area = util::generateDrivableArea( path, lanes, resolution, common_parameters.vehicle_length, planner_data_); } @@ -183,7 +184,7 @@ CandidateOutput LaneChangeModule::planCandidate() const CandidateOutput output; // Get lane change lanes - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); LaneChangePath selected_path; @@ -195,8 +196,8 @@ CandidateOutput LaneChangeModule::planCandidate() const return output; } - const auto start_idx = selected_path.shift_point.start_idx; - const auto end_idx = selected_path.shift_point.end_idx; + const auto & start_idx = selected_path.shift_point.start_idx; + const auto & end_idx = selected_path.shift_point.end_idx; output.path_candidate = selected_path.path; output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - @@ -224,7 +225,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() void LaneChangeModule::updateLaneChangeStatus() { - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); status_.current_lanes = current_lanes; // Get lane change lanes @@ -256,12 +257,12 @@ PathWithLaneId LaneChangeModule::getReferencePath() const const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; // Set header reference_path.header = route_handler->getRouteHeader(); - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); if (current_lanes.empty()) { return reference_path; @@ -281,7 +282,7 @@ PathWithLaneId LaneChangeModule::getReferencePath() const *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length, common_parameters, optional_lengths); } - const double buffer = + const double & buffer = common_parameters.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length const int num_lane_change = std::abs(route_handler->getNumLaneToPreferredLane(current_lanes.back())); @@ -299,30 +300,13 @@ PathWithLaneId LaneChangeModule::getReferencePath() const return reference_path; } -lanelet::ConstLanelets LaneChangeModule::getCurrentLanes() const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - RCLCPP_ERROR_THROTTLE( - getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!"); - return {}; // TODO(Horibe) what should be returned? - } - - // For current_lanes with desired length - return route_handler->getLaneletSequence( - current_lane, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length); -} - lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const { lanelet::ConstLanelets lane_change_lanes; const auto & route_handler = planner_data_->route_handler; + const auto & minimum_lane_change_length = planner_data_->parameters.minimum_lane_change_length; + const auto & lane_change_prepare_duration = parameters_->lane_change_prepare_duration; const auto current_pose = planner_data_->self_pose->pose; const auto current_twist = planner_data_->self_odometry->twist.twist; @@ -332,11 +316,9 @@ lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( // Get lane change lanes lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet( - current_lanes, planner_data_->self_pose->pose, ¤t_lane); - const double lane_change_prepare_length = std::max( - current_twist.linear.x * parameters_->lane_change_prepare_duration, - planner_data_->parameters.minimum_lane_change_length); + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); + const double lane_change_prepare_length = + std::max(current_twist.linear.x * lane_change_prepare_duration, minimum_lane_change_length); lanelet::ConstLanelets current_check_lanes = route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); lanelet::ConstLanelet lane_change_lane; @@ -354,14 +336,12 @@ std::pair LaneChangeModule::getSafePath( const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, LaneChangePath & safe_path) const { - std::vector valid_paths; - const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; + const auto & common_parameters = planner_data_->parameters; - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = util::getCurrentLanes(planner_data_); if (!lane_change_lanes.empty()) { // find candidate paths @@ -381,7 +361,7 @@ std::pair LaneChangeModule::getSafePath( } // select valid path - valid_paths = lane_change_utils::selectValidPaths( + const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( lane_change_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose, route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose()); @@ -393,7 +373,7 @@ std::pair LaneChangeModule::getSafePath( // select safe path object_debug_.clear(); - bool found_safe_path = lane_change_utils::selectSafePath( + const bool found_safe_path = lane_change_utils::selectSafePath( valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, current_twist, common_parameters, *parameters_, &safe_path, object_debug_); @@ -413,8 +393,8 @@ bool LaneChangeModule::isSafe() const { return status_.is_safe; } bool LaneChangeModule::isNearEndOfLane() const { - const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & common_parameters = planner_data_->parameters; const double threshold = 5 + common_parameters.minimum_lane_change_length; return std::max(0.0, util::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < @@ -423,8 +403,8 @@ bool LaneChangeModule::isNearEndOfLane() const bool LaneChangeModule::isCurrentSpeedLow() const { - const auto current_twist = planner_data_->self_odometry->twist.twist; - const double threshold_kmph = 10; + const auto & current_twist = planner_data_->self_odometry->twist.twist; + constexpr double threshold_kmph = 10.0; return util::l2Norm(current_twist.linear) < threshold_kmph * 1000 / 3600; } @@ -433,10 +413,10 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto & route_handler = planner_data_->route_handler; const auto current_pose = planner_data_->self_pose->pose; const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto objects = planner_data_->dynamic_object; - const auto common_parameters = planner_data_->parameters; + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & common_parameters = planner_data_->parameters; - const auto current_lanes = status_.current_lanes; + const auto & current_lanes = status_.current_lanes; // check abort enable flag if (!parameters_->enable_abort_lane_change) { @@ -458,8 +438,8 @@ bool LaneChangeModule::isAbortConditionSatisfied() const } // check if lane change path is still safe - bool is_path_safe{false}; - { + const bool is_path_safe = std::invoke([this, &route_handler, &dynamic_objects, ¤t_lanes, + ¤t_pose, ¤t_twist, &common_parameters]() { constexpr double check_distance = 100.0; // get lanes used for detection const auto & path = status_.lane_change_path; @@ -468,53 +448,58 @@ bool LaneChangeModule::isAbortConditionSatisfied() const const auto check_lanes = route_handler->getCheckTargetLanesFromPath( path.path, status_.lane_change_lanes, check_distance_with_path); - const size_t current_seg_idx = findEgoSegmentIndex(path.path.points); std::unordered_map debug_data; - is_path_safe = lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, objects, current_pose, current_seg_idx, current_twist, - common_parameters, *parameters_, debug_data, false, status_.lane_change_path.acceleration); - } - - // check vehicle velocity thresh - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; - - // check if vehicle is within lane - bool is_within_original_lane = false; - { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto vehicle_poly = util::getVehiclePolygon( - current_pose, common_parameters.vehicle_width, common_parameters.base_link2front); - is_within_original_lane = boost::geometry::within( - lanelet::utils::to2D(vehicle_poly).basicPolygon(), - lanelet::utils::to2D(lane_poly).basicPolygon()); - } - - // check distance from original lane's centerline - bool is_distance_small = false; - { - const auto centerline2d = lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); - lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); - const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); - is_distance_small = distance < parameters_->abort_lane_change_distance_thresh; - } - // check angle thresh from original lane - bool is_angle_diff_small = false; - { - const double lane_angle = - lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); - const double vehicle_yaw = tf2::getYaw(current_pose.orientation); - const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); - is_angle_diff_small = std::abs(yaw_diff) < parameters_->abort_lane_change_angle_thresh; - } + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + return lane_change_utils::isLaneChangePathSafe( + path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, *parameters_, debug_data, false, + status_.lane_change_path.acceleration); + }); // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { + // check vehicle velocity thresh + const bool is_velocity_low = + util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; + + // check if vehicle is within lane + const bool is_within_original_lane = + std::invoke([¤t_lanes, current_pose, common_parameters]() { + const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); + const auto lane_poly = + lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); + const auto vehicle_poly = util::getVehiclePolygon( + current_pose, common_parameters.vehicle_width, common_parameters.base_link2front); + return boost::geometry::within( + lanelet::utils::to2D(vehicle_poly).basicPolygon(), + lanelet::utils::to2D(lane_poly).basicPolygon()); + }); + if (is_velocity_low && is_within_original_lane) { return true; } + + // check distance from original lane's centerline + const bool is_distance_small = std::invoke([this, &closest_lanelet, ¤t_pose]() { + const auto centerline2d = + lanelet::utils::to2D(closest_lanelet.centerline()).basicLineString(); + lanelet::BasicPoint2d vehicle_pose2d(current_pose.position.x, current_pose.position.y); + const double distance = lanelet::geometry::distance2d(centerline2d, vehicle_pose2d); + return distance < parameters_->abort_lane_change_distance_thresh; + }); + + // check angle thresh from original lane + const bool is_angle_diff_small = std::invoke([this, closest_lanelet, ¤t_pose]() { + const double lane_angle = + lanelet::utils::getLaneletAngle(closest_lanelet, current_pose.position); + const double vehicle_yaw = tf2::getYaw(current_pose.orientation); + const double yaw_diff = tier4_autoware_utils::normalizeRadian(lane_angle - vehicle_yaw); + return std::abs(yaw_diff) < parameters_->abort_lane_change_angle_thresh; + }); + if (is_distance_small && is_angle_diff_small) { return true; } @@ -529,7 +514,7 @@ bool LaneChangeModule::isAbortConditionSatisfied() const bool LaneChangeModule::hasFinishedLaneChange() const { - const auto current_pose = planner_data_->self_pose->pose; + const auto & current_pose = planner_data_->self_pose->pose; const auto arclength_current = lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); const double travel_distance = arclength_current.length - status_.start_distance; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index bae73ba013254..44a521734934b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -33,7 +33,10 @@ namespace behavior_path_planner::lane_change_utils { -PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2) +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using lanelet::ArcCoordinates; + +PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) { PathWithLaneId path; path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); @@ -79,12 +82,85 @@ double getDistanceWhenDecelerate( const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2); return std::max(distance, minimum_distance); } -std::vector getLaneChangePaths( + +std::optional constructCandidatePath( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const PathWithLaneId & target_lane_reference_path, const ShiftPoint & shift_point, + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + const double & acceleration, const double & prepare_distance, const double & lane_change_distance, + const double & lane_changing_duration, const double & minimum_lane_change_velocity) +{ + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftPoint(shift_point); + ShiftedPath shifted_path; + + // offset front side + bool offset_back = false; + if (!path_shifter.generate(&shifted_path, offset_back)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "failed to generate shifted path."); + } + + LaneChangePath candidate_path; + candidate_path.acceleration = acceleration; + candidate_path.preparation_length = prepare_distance; + candidate_path.lane_change_length = lane_change_distance; + candidate_path.shift_point = shift_point; + + const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); + const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); + const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; + const auto lanechange_end_idx = + motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); + if (lanechange_end_idx) { + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lanechange_end_idx) { + point.lane_ids.insert( + point.lane_ids.end(), lane_changing_start_point.lane_ids.begin(), + lane_changing_start_point.lane_ids.end()); + point.lane_ids.insert( + point.lane_ids.end(), lane_changing_end_point.lane_ids.begin(), + lane_changing_end_point.lane_ids.end()); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + lane_changing_start_point.point.longitudinal_velocity_mps); + continue; + } + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast( + std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + const auto nearest_idx = + motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); + point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; + } + + candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "lane change end idx not found on target path."); + return std::nullopt; + } + + // check candidate path is in lanelet + if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { + return std::nullopt; + } + + return std::optional{candidate_path}; +} + +LaneChangePaths getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter) { - std::vector candidate_paths; + LaneChangePaths candidate_paths{}; if (original_lanelets.empty() || target_lanelets.empty()) { return candidate_paths; @@ -113,13 +189,13 @@ std::vector getLaneChangePaths( for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - const double lane_change_prepare_velocity = getExpectedVelocityWhenDecelerate( + const double prepare_speed = getExpectedVelocityWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration); - const double lane_changing_velocity = getExpectedVelocityWhenDecelerate( - lane_change_prepare_velocity, acceleration, lane_changing_duration); + const double lane_changing_speed = + getExpectedVelocityWhenDecelerate(prepare_speed, acceleration, lane_changing_duration); // skip if velocity becomes less than zero before finishing lane change - if (lane_changing_velocity < 0.0) { + if (lane_changing_speed < 0.0) { continue; } @@ -127,150 +203,63 @@ std::vector getLaneChangePaths( const double prepare_distance = getDistanceWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration, minimum_lane_change_prepare_distance); + const double lane_changing_distance = getDistanceWhenDecelerate( - lane_change_prepare_velocity, acceleration, lane_changing_duration, - minimum_lane_change_length); + prepare_speed, acceleration, lane_changing_duration, minimum_lane_change_length); if (prepare_distance < target_distance) { continue; } - PathWithLaneId reference_path1; - { - const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose); - const double s_start = arc_position.length - backward_path_length; - const double s_end = arc_position.length + prepare_distance; - reference_path1 = route_handler.getCenterLinePath(original_lanelets, s_start, s_end); - } - - reference_path1.points.back().point.longitudinal_velocity_mps = std::min( - reference_path1.points.back().point.longitudinal_velocity_mps, - static_cast( - std::max(prepare_distance / lane_change_prepare_duration, minimum_lane_change_velocity))); - - PathWithLaneId reference_path2{}; - { - const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); - const auto arc_position = lanelet::utils::getArcCoordinates(target_lanelets, pose); - const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); - double s_start = arc_position.length + prepare_distance + lane_changing_distance; - s_start = std::min(s_start, lane_length - num * minimum_lane_change_length); - double s_end = s_start + forward_path_length; - s_end = std::min(s_end, lane_length - num * (minimum_lane_change_length + buffer)); - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - reference_path2 = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); - } + const PathWithLaneId prepare_segment_reference = getLaneChangePathPrepareSegment( + route_handler, original_lanelets, pose, backward_path_length, prepare_distance, + lane_change_prepare_duration, minimum_lane_change_velocity); - for (auto & point : reference_path2.points) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast( - std::max(lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); - } + const PathWithLaneId lane_changing_segment_reference = getLaneChangePathLaneChangingSegment( + route_handler, target_lanelets, pose, forward_path_length, prepare_distance, + lane_changing_distance, minimum_lane_change_length, buffer, lane_changing_duration, + minimum_lane_change_velocity); - if (reference_path1.points.empty() || reference_path2.points.empty()) { + if ( + prepare_segment_reference.points.empty() || lane_changing_segment_reference.points.empty()) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "reference path is empty!! something wrong..."); continue; } - LaneChangePath candidate_path; - candidate_path.acceleration = acceleration; - candidate_path.preparation_length = prepare_distance; - candidate_path.lane_change_length = lane_changing_distance; - - PathWithLaneId target_lane_reference_path; - { - const lanelet::ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates( - target_lanelets, reference_path1.points.back().point.pose); - double s_start = lane_change_start_arc_position.length; - double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; - target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end); - } + const Pose & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; - ShiftPoint shift_point; - { - const Pose lane_change_start_on_self_lane = reference_path1.points.back().point.pose; - const Pose lane_change_end_on_target_lane = reference_path2.points.front().point.pose; - const lanelet::ArcCoordinates lane_change_start_on_self_lane_arc = - lanelet::utils::getArcCoordinates(target_lanelets, lane_change_start_on_self_lane); - shift_point.length = lane_change_start_on_self_lane_arc.distance; - shift_point.start = lane_change_start_on_self_lane; - shift_point.end = lane_change_end_on_target_lane; - shift_point.start_idx = motion_utils::findNearestIndex( - target_lane_reference_path.points, lane_change_start_on_self_lane.position); - shift_point.end_idx = motion_utils::findNearestIndex( - target_lane_reference_path.points, lane_change_end_on_target_lane.position); - } - - PathShifter path_shifter; - path_shifter.setPath(target_lane_reference_path); - path_shifter.addShiftPoint(shift_point); - ShiftedPath shifted_path; + const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( + route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, + lane_changing_distance, forward_path_length); - // offset front side - bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "failed to generate shifted path."); - } - const auto lanechange_end_idx = motion_utils::findNearestIndex( - shifted_path.path.points, reference_path2.points.front().point.pose); - if (lanechange_end_idx) { - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *lanechange_end_idx) { - point.lane_ids.insert( - point.lane_ids.end(), reference_path1.points.back().lane_ids.begin(), - reference_path1.points.back().lane_ids.end()); - point.lane_ids.insert( - point.lane_ids.end(), reference_path2.points.front().lane_ids.begin(), - reference_path2.points.front().lane_ids.end()); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - reference_path1.points.back().point.longitudinal_velocity_mps); - continue; - } - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - static_cast(std::max( - lane_changing_distance / lane_changing_duration, minimum_lane_change_velocity))); - const auto nearest_idx = - motion_utils::findNearestIndex(reference_path2.points, point.point.pose); - point.lane_ids = reference_path2.points.at(*nearest_idx).lane_ids; - } + const ShiftPoint shift_point = getLaneChangeShiftPoint( + prepare_segment_reference, lane_changing_segment_reference, target_lanelets, + target_lane_reference_path); - candidate_path.path = combineReferencePath(reference_path1, shifted_path.path); - candidate_path.shifted_path = shifted_path; - candidate_path.shift_point = shift_point; - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "lane change end idx not found on target path."); - continue; - } + const auto candidate_path = constructCandidatePath( + prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, + shift_point, original_lanelets, target_lanelets, acceleration, prepare_distance, + lane_changing_distance, lane_changing_duration, minimum_lane_change_velocity); - // check candidate path is in lanelet - if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { + if (!candidate_path) { continue; } - candidate_paths.push_back(candidate_path); + candidate_paths.push_back(*candidate_path); } return candidate_paths; } -std::vector selectValidPaths( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, +LaneChangePaths selectValidPaths( + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose) { - std::vector available_paths; + LaneChangePaths available_paths; for (const auto & path : paths) { if (hasEnoughDistance( @@ -284,7 +273,7 @@ std::vector selectValidPaths( } bool selectSafePath( - const std::vector & paths, const lanelet::ConstLanelets & current_lanes, + const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, @@ -316,8 +305,8 @@ bool hasEnoughDistance( const bool isInGoalRouteSection, const Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs) { - const double lane_change_prepare_distance = path.preparation_length; - const double lane_changing_distance = path.lane_change_length; + const double & lane_change_prepare_distance = path.preparation_length; + const double & lane_changing_distance = path.lane_change_length; const double lane_change_total_distance = lane_change_prepare_distance + lane_changing_distance; if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { @@ -340,6 +329,12 @@ bool hasEnoughDistance( util::getDistanceToCrosswalk(current_pose, current_lanes, overall_graphs)) { return false; } + + // return is there is no target lanes. Else continue checking + if (target_lanes.empty()) { + return true; + } + return true; } @@ -347,7 +342,7 @@ bool isLaneChangePathSafe( const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const size_t current_seg_idx, const Twist & current_twist, + const size_t & current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, std::unordered_map & debug_data, const bool use_buffer, @@ -363,21 +358,16 @@ bool isLaneChangePathSafe( const double time_resolution = lane_change_parameters.prediction_time_resolution; const auto & lane_change_prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const auto & enable_collision_check_at_prepare_phase = + lane_change_parameters.enable_collision_check_at_prepare_phase; const auto & lane_changing_duration = lane_change_parameters.lane_changing_duration; - const double current_lane_check_start_time = - (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration - : 0.0; - const double current_lane_check_end_time = - lane_change_prepare_duration + lane_changing_duration / 2; - double target_lane_check_start_time = - (!lane_change_parameters.enable_collision_check_at_prepare_phase) ? lane_change_prepare_duration - : 0.0; - const double target_lane_check_end_time = - lane_change_prepare_duration + lane_changing_duration / 2; + const double check_start_time = + (enable_collision_check_at_prepare_phase) ? 0.0 : lane_change_prepare_duration; + const double check_end_time = lane_change_prepare_duration + lane_changing_duration; constexpr double ego_predicted_path_min_speed{1.0}; const auto vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, current_seg_idx, target_lane_check_end_time, time_resolution, - acceleration, ego_predicted_path_min_speed); + path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, + time_resolution, acceleration, ego_predicted_path_min_speed); const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); @@ -425,9 +415,9 @@ bool isLaneChangePathSafe( util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, - current_lane_check_start_time, current_lane_check_end_time, time_resolution, obj, - obj_path, common_parameters, current_debug_data.second)) { + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, obj_path, common_parameters, + current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -456,18 +446,17 @@ bool isLaneChangePathSafe( if (is_object_in_target) { for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, - target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, - obj_path, common_parameters, current_debug_data.second)) { + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, obj_path, common_parameters, + current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } } } else { if (!util::isSafeInFreeSpaceCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, - target_lane_check_start_time, target_lane_check_end_time, time_resolution, obj, - common_parameters, current_debug_data.second)) { + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, common_parameters, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } @@ -476,4 +465,120 @@ bool isLaneChangePathSafe( } return true; } + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double & prepare_distance, + const double & lane_changing_distance, const double & 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 = s_start + prepare_distance + lane_changing_distance + forward_path_length; + return route_handler.getCenterLinePath(target_lanes, s_start, s_end); +} + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & in_target_front_pose, const Pose & in_target_end_pose) +{ + const ArcCoordinates lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, in_target_front_pose); + + const ArcCoordinates lane_change_end_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, in_target_end_pose); + + const double & s_start = lane_change_start_arc_position.length; + const double & s_end = lane_change_end_arc_position.length; + return route_handler.getCenterLinePath(target_lanes, s_start, s_end); +} + +ShiftPoint getLaneChangeShiftPoint( + const PathWithLaneId & path1, const PathWithLaneId & path2, + const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path) +{ + const Pose & lane_change_start_on_self_lane = path1.points.back().point.pose; + const Pose & lane_change_end_on_target_lane = path2.points.front().point.pose; + const ArcCoordinates lane_change_start_on_self_lane_arc = + lanelet::utils::getArcCoordinates(target_lanes, lane_change_start_on_self_lane); + + ShiftPoint shift_point; + shift_point.length = lane_change_start_on_self_lane_arc.distance; + shift_point.start = lane_change_start_on_self_lane; + shift_point.end = lane_change_end_on_target_lane; + shift_point.start_idx = + motion_utils::findNearestIndex(reference_path.points, lane_change_start_on_self_lane.position); + shift_point.end_idx = + motion_utils::findNearestIndex(reference_path.points, lane_change_end_on_target_lane.position); + + return shift_point; +} + +PathWithLaneId getLaneChangePathPrepareSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, + const Pose & current_pose, const double & backward_path_length, const double & prepare_distance, + const double & prepare_duration, const double & minimum_lane_change_velocity) +{ + if (original_lanelets.empty()) { + return PathWithLaneId(); + } + + const ArcCoordinates arc_position = + lanelet::utils::getArcCoordinates(original_lanelets, current_pose); + const double s_start = arc_position.length - backward_path_length; + const double s_end = arc_position.length + prepare_distance; + + PathWithLaneId prepare_segment = + route_handler.getCenterLinePath(original_lanelets, s_start, s_end); + + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast( + std::max(prepare_distance / prepare_duration, minimum_lane_change_velocity))); + + return prepare_segment; +} + +PathWithLaneId getLaneChangePathLaneChangingSegment( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, + const Pose & current_pose, const double & forward_path_length, const double & prepare_distance, + const double & lane_change_distance, const double & minimum_lane_change_length, + const double & lane_change_distance_buffer, const double & lane_changing_duration, + const double & minimum_lane_change_velocity) +{ + if (target_lanelets.empty()) { + return PathWithLaneId(); + } + + const ArcCoordinates arc_position = + lanelet::utils::getArcCoordinates(target_lanelets, current_pose); + const double lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); + const int num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); + + const double s_start = std::invoke([&arc_position, &prepare_distance, &lane_change_distance, + &lane_length, &num, &minimum_lane_change_length]() { + const double s_start = arc_position.length + prepare_distance + lane_change_distance; + return std::min(s_start, lane_length - num * minimum_lane_change_length); + }); + + const double s_end = std::invoke([&s_start, &forward_path_length, &lane_length, &num, + &minimum_lane_change_length, &lane_change_distance_buffer]() { + const double s_end = std::min( + s_start + forward_path_length, + lane_length - num * (minimum_lane_change_length + lane_change_distance_buffer)); + return std::max(s_end, s_start + std::numeric_limits::epsilon()); + }); + + PathWithLaneId lane_changing_segment = + route_handler.getCenterLinePath(target_lanelets, s_start, s_end); + for (auto & point : lane_changing_segment.points) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast( + std::max(lane_change_distance / lane_changing_duration, minimum_lane_change_velocity))); + } + + return lane_changing_segment; +} + } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 50feab9677745..a4b2a9759536d 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1878,7 +1878,7 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr { const auto & route_handler = planner_data->route_handler; const auto current_pose = planner_data->self_pose->pose; - const auto common_parameters = planner_data->parameters; + const auto & common_parameters = planner_data->parameters; lanelet::ConstLanelet current_lane; if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) {