fix(behavior_path_planner): fix lane change path validation (autoware…

…foundation#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> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
tier4 · Dec 8, 2022 · 5c642cf · 5c642cf
1 parent ee2e0d7
commit 5c642cf
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 7 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
@@ -18,7 +18,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/utilities.hpp"
-#include "lane_departure_checker/lane_departure_checker.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -38,7 +37,6 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using lane_departure_checker::LaneDepartureChecker;
 
 struct LaneChangeParameters
 {
@@ -127,7 +125,6 @@ class LaneChangeModule : public SceneModuleInterface
   LaneChangeParameters parameters_;
   LaneChangeStatus status_;
   PathShifter path_shifter_;
-  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
@@ -44,7 +44,6 @@ LaneChangeModule::LaneChangeModule(
   uuid_left_{generateUUID()},
   uuid_right_{generateUUID()}
 {
-  lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo());
 }
 
 BehaviorModuleOutput LaneChangeModule::run()
@@ -426,9 +425,20 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const
   const auto expanded_lanes = util::expandLanelets(
     drivable_lanes, drivable_area_left_bound_offset, 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
@@ -172,6 +172,11 @@ std::vector<LaneChangePath> getLaneChangePaths(
           target_lanelets, reference_path1.points.back().point.pose);
       double s_start = lane_change_start_arc_position.length;
       double s_end = s_start + straight_distance + lane_change_distance + forward_path_length;
+      if (route_handler.isInGoalRouteSection(target_lanelets.back())) {
+        const auto goal_arc_coordinates =
+          lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose());
+        s_end = std::min(s_end, goal_arc_coordinates.length);
+      }
       target_lane_reference_path = route_handler.getCenterLinePath(target_lanelets, s_start, s_end);
     }