fix(lane_change): get lane change lanes actual length (autowarefounda…

…tion#3687) (#493)

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

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]

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};

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};

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<double>(parameter("backward_lane_length"));
   p.lane_change_finish_judge_buffer =
     declare_parameter<double>(parameter("lane_change_finish_judge_buffer"));
   p.prediction_time_resolution = declare_parameter<double>(parameter("prediction_time_resolution"));

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 <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 
 #include <algorithm>
@@ -203,30 +205,40 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes(
     return {};
   }
   // Get lane change lanes
-  lanelet::ConstLanelet current_lane;
-  lanelet::utils::query::getClosestLanelet(current_lanes, getEgoPose(), &current_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 =