From a99cc2fa5ec3dfe8bf63c8ce7a2cf1f72a7fcc82 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 May 2023 10:07:36 +0900 Subject: [PATCH] fix(lane_change): get lane change lanes actual length (#3687) (#493) Signed-off-by: Muhammad Zulfaqar Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --- .../config/lane_change/lane_change.param.yaml | 2 +- .../scene_module/lane_change/base_class.hpp | 3 -- .../lane_change/lane_change_module_data.hpp | 1 + .../src/behavior_path_planner_node.cpp | 1 + .../src/scene_module/lane_change/normal.cpp | 52 ++++++++++++------- 5 files changed, 36 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index f5df89e33c58f..827e974252fd0 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: lane_change: - lane_changing_safety_check_duration: 8.0 # [s] + backward_lane_length: 200.0 #[m] prepare_duration: 4.0 # [s] backward_length_buffer_for_end_of_lane: 3.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index ce5bd8be284ce..465fe2851f16a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -222,9 +222,6 @@ class LaneChangeBase PathWithLaneId prev_approved_path_{}; - double lane_change_lane_length_{200.0}; - double check_length_{100.0}; - bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; bool is_activated_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index a0a903c6f4f9f..4a95244c8aaf3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -28,6 +28,7 @@ namespace behavior_path_planner struct LaneChangeParameters { // trajectory generation + double backward_lane_length{200.0}; double lane_change_finish_judge_buffer{3.0}; double prediction_time_resolution{0.5}; int lane_change_sampling_num{10}; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5c320e2319efd..ed06dd198c650 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -708,6 +708,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation + p.backward_lane_length = declare_parameter(parameter("backward_lane_length")); p.lane_change_finish_judge_buffer = declare_parameter(parameter("lane_change_finish_judge_buffer")); p.prediction_time_resolution = declare_parameter(parameter("prediction_time_resolution")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 93e83654cd698..ea0e9f87aa4f4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/utils.hpp" +#include #include #include @@ -203,30 +205,40 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( return {}; } // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, getEgoPose(), ¤t_lane); - - const auto minimum_prepare_length = planner_data_->parameters.minimum_prepare_length; - - const auto lane_change_prepare_length = std::max( - getEgoVelocity() * planner_data_->parameters.lane_change_prepare_duration, - minimum_prepare_length); - const auto & route_handler = getRouteHandler(); - const auto current_check_lanes = - route_handler->getLaneletSequence(current_lane, getEgoPose(), 0.0, lane_change_prepare_length); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( *getRouteHandler(), current_lanes, type_, direction); - const auto lane_change_lane_length = std::max(lane_change_lane_length_, getEgoVelocity() * 10.0); - if (lane_change_lane) { - return route_handler->getLaneletSequence( - lane_change_lane.get(), getEgoPose(), lane_change_lane_length, lane_change_lane_length); + if (!lane_change_lane) { + return {}; } - return {}; + const auto front_pose = std::invoke([&lane_change_lane]() { + const auto & p = lane_change_lane->centerline().front(); + const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); + const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); + geometry_msgs::msg::Pose front_pose; + front_pose.position = front_point; + tf2::Quaternion quat; + quat.setRPY(0, 0, front_yaw); + front_pose.orientation = tf2::toMsg(quat); + return front_pose; + }); + + const auto forward_length = std::invoke([&]() { + const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); + const auto forward_path_length = planner_data_->parameters.forward_path_length; + if (signed_distance <= 0.0) { + return forward_path_length; + } + + return signed_distance + forward_path_length; + }); + const auto backward_length = lane_change_parameters_->backward_lane_length; + + return route_handler->getLaneletSequence( + lane_change_lane.get(), getEgoPose(), backward_length, forward_length); } bool NormalLaneChange::isNearEndOfLane() const @@ -580,9 +592,10 @@ bool NormalLaneChange::getLaneChangePaths( if (candidate_paths->empty()) { // only compute dynamic object indices once + const auto backward_length = lane_change_parameters_->backward_lane_length; const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( - route_handler, target_lanelets, getEgoPose(), check_length_); + route_handler, target_lanelets, getEgoPose(), backward_length); dynamic_object_indices = utils::lane_change::filterObjectIndices( {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, @@ -622,7 +635,8 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const // get lanes used for detection const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( - *route_handler, path.target_lanelets, current_pose, check_length_); + *route_handler, path.target_lanelets, current_pose, + lane_change_parameters.backward_lane_length); CollisionCheckDebugMap debug_data; const auto lateral_buffer =