fix(behavior_path_planner): fix lane change path validation (#2421)

* fix(behavior_path_planner): clip_path_for_goal Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_path_planner): clip path for validation Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_path_planner): use path point only to check lane departure Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> * fix(behavior_path_planner): remove unnecessary change Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · Dec 1, 2022 · 35b0dfb · 35b0dfb
1 parent 7aa9314
commit 35b0dfb
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 9 deletions.
diff --git a/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
@@ -20,7 +20,6 @@
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
-#include "lane_departure_checker/lane_departure_checker.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -44,7 +43,6 @@ namespace behavior_path_planner
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
-using lane_departure_checker::LaneDepartureChecker;
 using marker_utils::CollisionCheckDebug;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
@@ -105,7 +103,6 @@ class LaneChangeModule : public SceneModuleInterface
   LaneChangeStatus status_;
   PathShifter path_shifter_;
   mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
-  LaneDepartureChecker lane_departure_checker_;
 
   double lane_change_lane_length_{200.0};
   double check_distance_{100.0};

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
@@ -50,7 +50,6 @@ LaneChangeModule::LaneChangeModule(
   uuid_right_{generateUUID()}
 {
   steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, "lane_change");
-  lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo());
 }
 
 BehaviorModuleOutput LaneChangeModule::run()
@@ -146,6 +145,7 @@ BehaviorModuleOutput LaneChangeModule::plan()
 {
   constexpr double resample_interval{1.0};
   auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval);
+
   if (!isValidPath(path)) {
     status_.is_safe = false;
     return BehaviorModuleOutput{};
@@ -387,9 +387,20 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const
     drivable_lanes, parameters_->drivable_area_left_bound_offset,
     parameters_->drivable_area_right_bound_offset);
   const auto lanelets = util::transformToLanelets(expanded_lanes);
-  if (lane_departure_checker_.checkPathWillLeaveLane(lanelets, path)) {
-    RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes");
-    return false;
+
+  // check path points are in any lanelets
+  for (const auto & point : path.points) {
+    bool is_in_lanelet = false;
+    for (const auto & lanelet : lanelets) {
+      if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) {
+        is_in_lanelet = true;
+        break;
+      }
+    }
+    if (!is_in_lanelet) {
+      RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes");
+      return false;
+    }
   }
 
   // check relative angle

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
@@ -496,7 +496,12 @@ PathWithLaneId getReferencePathFromTargetLane(
   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;
+  double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length;
+  if (route_handler.isInGoalRouteSection(target_lanes.back())) {
+    const auto goal_arc_coordinates =
+      lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose());
+    s_end = std::min(s_end, goal_arc_coordinates.length);
+  }
   return route_handler.getCenterLinePath(target_lanes, s_start, s_end);
 }
 
@@ -511,7 +516,12 @@ PathWithLaneId getReferencePathFromTargetLane(
     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;
+  double s_end = lane_change_end_arc_position.length;
+  if (route_handler.isInGoalRouteSection(target_lanes.back())) {
+    const auto goal_arc_coordinates =
+      lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose());
+    s_end = std::min(s_end, goal_arc_coordinates.length);
+  }
   return route_handler.getCenterLinePath(target_lanes, s_start, s_end);
 }