fix(behavior_path_planner): check lane departure and relative angle for lane change

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -20,6 +20,7 @@
 #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>
 
@@ -43,6 +44,7 @@ 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;
@@ -103,6 +105,7 @@ 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};
@@ -169,6 +172,7 @@ class LaneChangeModule : public SceneModuleInterface
   void updateSteeringFactorPtr(
     const CandidateOutput & output, const LaneChangePath & selected_path) const;
   bool isSafe() const;
+  bool isValidPath(const PathWithLaneId & path) const;
   bool isNearEndOfLane() const;
   bool isCurrentSpeedLow() const;
   bool isAbortConditionSatisfied() const;

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -404,6 +404,9 @@ std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & class
 
 lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr<const PlannerData> & planner_data);
 
+lanelet::ConstLanelets extendLanes(
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
+
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data);
 
@@ -476,6 +479,8 @@ bool isSafeInFreeSpaceCollisionCheck(
   const double check_start_time, const double check_end_time, const double check_time_resolution,
   const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters,
   const double front_decel, const double rear_decel, CollisionCheckDebug & debug);
+
+bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
 }  // namespace behavior_path_planner::util
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -50,6 +50,7 @@ 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()
@@ -58,6 +59,12 @@ BehaviorModuleOutput LaneChangeModule::run()
   current_state_ = BT::NodeStatus::RUNNING;
   is_activated_ = isActivated();
   auto output = plan();
+
+  if (!isSafe()) {
+    current_state_ = BT::NodeStatus::SUCCESS;  // for breaking loop
+    return output;
+  }
+
   updateSteeringFactorPtr(output);
 
   return output;
@@ -73,7 +80,7 @@ void LaneChangeModule::onEntry()
 void LaneChangeModule::onExit()
 {
   resetParameters();
-  current_state_ = BT::NodeStatus::IDLE;
+  current_state_ = BT::NodeStatus::SUCCESS;
   RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit");
 }
 
@@ -112,6 +119,11 @@ bool LaneChangeModule::isExecutionReady() const
 BT::NodeStatus LaneChangeModule::updateState()
 {
   RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState");
+  if (!isSafe()) {
+    current_state_ = BT::NodeStatus::SUCCESS;
+    return current_state_;
+  }
+
   if (isAbortConditionSatisfied()) {
     if (isNearEndOfLane() && isCurrentSpeedLow()) {
       current_state_ = BT::NodeStatus::RUNNING;
@@ -134,6 +146,10 @@ 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{};
+  }
   generateExtendedDrivableArea(path);
 
   if (isAbortConditionSatisfied()) {
@@ -359,6 +375,32 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
 
 bool LaneChangeModule::isSafe() const { return status_.is_safe; }
 
+bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const
+{
+  const auto & route_handler = planner_data_->route_handler;
+
+  // check lane departure
+  const auto drivable_lanes = lane_change_utils::generateDrivableLanes(
+    *route_handler, util::extendLanes(route_handler, status_.current_lanes),
+    util::extendLanes(route_handler, status_.lane_change_lanes));
+  const auto expanded_lanes = util::expandLanelets(
+    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 relative angle
+  if (!util::checkPathRelativeAngle(path, M_PI)) {
+    RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid");
+    return false;
+  }
+
+  return true;
+}
+
 bool LaneChangeModule::isNearEndOfLane() const
 {
   const auto & current_pose = getEgoPose();

planning/behavior_path_planner/src/utilities.cpp

@@ -2189,6 +2189,26 @@ lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr<const PlannerData>
     common_parameters.forward_path_length);
 }
 
+lanelet::ConstLanelets extendLanes(
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes)
+{
+  auto extended_lanes = lanes;
+
+  // Add next lane
+  const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
+  if (!next_lanes.empty()) {
+    extended_lanes.push_back(next_lanes.front());
+  }
+
+  // Add previous lane
+  const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
+  if (!prev_lanes.empty()) {
+    extended_lanes.insert(extended_lanes.begin(), prev_lanes.front());
+  }
+
+  return extended_lanes;
+}
+
 lanelet::ConstLanelets getExtendedCurrentLanes(
   const std::shared_ptr<const PlannerData> & planner_data)
 {
@@ -2206,23 +2226,11 @@ lanelet::ConstLanelets getExtendedCurrentLanes(
   }
 
   // For current_lanes with desired length
-  auto current_lanes = route_handler->getLaneletSequence(
+  const auto current_lanes = route_handler->getLaneletSequence(
     current_lane, current_pose, common_parameters.backward_path_length,
     common_parameters.forward_path_length);
 
-  // Add next lane
-  const auto next_lanes = route_handler->getNextLanelets(current_lanes.back());
-  if (!next_lanes.empty()) {
-    current_lanes.push_back(next_lanes.front());
-  }
-
-  // Add previous lane
-  const auto prev_lanes = route_handler->getPreviousLanelets(current_lanes.front());
-  if (!prev_lanes.empty()) {
-    current_lanes.insert(current_lanes.begin(), prev_lanes.front());
-  }
-
-  return current_lanes;
+  return extendLanes(route_handler, current_lanes);
 }
 
 lanelet::ConstLanelets calcLaneAroundPose(
@@ -2645,4 +2653,50 @@ bool isSafeInFreeSpaceCollisionCheck(
   }
   return true;
 }
+
+bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold)
+{
+  // We need at least three points to compute relative angle
+  constexpr size_t relative_angle_points_num = 3;
+  if (path.points.size() < relative_angle_points_num) {
+    return true;
+  }
+
+  for (size_t p1_id = 0; p1_id <= path.points.size() - relative_angle_points_num; ++p1_id) {
+    // Get Point1
+    const auto & p1 = path.points.at(p1_id).point.pose.position;
+
+    // Get Point2
+    const auto & p2 = path.points.at(p1_id + 1).point.pose.position;
+
+    // Get Point3
+    const auto & p3 = path.points.at(p1_id + 2).point.pose.position;
+
+    // ignore invert driving direction
+    if (
+      path.points.at(p1_id).point.longitudinal_velocity_mps < 0 ||
+      path.points.at(p1_id + 1).point.longitudinal_velocity_mps < 0 ||
+      path.points.at(p1_id + 2).point.longitudinal_velocity_mps < 0) {
+      continue;
+    }
+
+    // convert to p1 coordinate
+    const double x3 = p3.x - p1.x;
+    const double x2 = p2.x - p1.x;
+    const double y3 = p3.y - p1.y;
+    const double y2 = p2.y - p1.y;
+
+    // calculate relative angle of vector p3 based on p1p2 vector
+    const double th = std::atan2(y2, x2);
+    const double th2 =
+      std::atan2(-x3 * std::sin(th) + y3 * std::cos(th), x3 * std::cos(th) + y3 * std::sin(th));
+    if (std::abs(th2) > angle_threshold) {
+      // invalid angle
+      return false;
+    }
+  }
+
+  return true;
+}
+
 }  // namespace behavior_path_planner::util