fix(behavior_path_planner): check lane departure and relative angle f…

…or lane change (autowarefoundation#2379)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -102,7 +102,8 @@ class LaneDepartureChecker
     vehicle_info_ptr_ = std::make_shared<vehicle_info_util::VehicleInfo>(vehicle_info);
   }
 
-  bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path);
+  bool checkPathWillLeaveLane(
+    const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
 
   vehicle_info_util::VehicleInfo vehicle_info_public_;
 
@@ -122,7 +123,7 @@ class LaneDepartureChecker
   std::vector<LinearRing2d> createVehicleFootprints(
     const geometry_msgs::msg::PoseWithCovariance & covariance, const TrajectoryPoints & trajectory,
     const Param & param);
-  std::vector<LinearRing2d> createVehicleFootprints(const PathWithLaneId & path);
+  std::vector<LinearRing2d> createVehicleFootprints(const PathWithLaneId & path) const;
 
   static std::vector<LinearRing2d> createVehiclePassingAreas(
     const std::vector<LinearRing2d> & vehicle_footprints);

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -145,7 +145,7 @@ Output LaneDepartureChecker::update(const Input & input)
 }
 
 bool LaneDepartureChecker::checkPathWillLeaveLane(
-  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path)
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const
 {
   std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
@@ -242,7 +242,8 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
   return vehicle_footprints;
 }
 
-std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path)
+std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
+  const PathWithLaneId & path) const
 {
   // Create vehicle footprint in base_link coordinate
   const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_);

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

@@ -18,6 +18,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/utilities.hpp"
+#include "lane_departure_checker/lane_departure_checker.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -37,6 +38,7 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using lane_departure_checker::LaneDepartureChecker;
 
 struct LaneChangeParameters
 {
@@ -125,6 +127,7 @@ 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};
@@ -185,6 +188,7 @@ class LaneChangeModule : public SceneModuleInterface
 
   bool isSafe() const;
   bool isLaneBlocked(const lanelet::ConstLanelets & lanes) 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

@@ -292,9 +292,14 @@ 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);
 
+bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
+
 }  // namespace util
 }  // namespace behavior_path_planner
 

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

@@ -44,6 +44,7 @@ LaneChangeModule::LaneChangeModule(
   uuid_left_{generateUUID()},
   uuid_right_{generateUUID()}
 {
+  lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo());
 }
 
 BehaviorModuleOutput LaneChangeModule::run()
@@ -52,6 +53,12 @@ BehaviorModuleOutput LaneChangeModule::run()
   current_state_ = BT::NodeStatus::RUNNING;
   is_activated_ = isActivated();
   const auto output = plan();
+
+  if (!isSafe()) {
+    current_state_ = BT::NodeStatus::SUCCESS;  // for breaking loop
+    return output;
+  }
+
   const auto turn_signal_info = output.turn_signal_info;
   if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
     waitApprovalLeft(turn_signal_info.signal_distance);
@@ -77,7 +84,7 @@ void LaneChangeModule::onExit()
 {
   clearWaitingApproval();
   removeRTCStatus();
-  current_state_ = BT::NodeStatus::IDLE;
+  current_state_ = BT::NodeStatus::SUCCESS;
   RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit");
 }
 
@@ -122,6 +129,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;
@@ -143,6 +155,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{};
+  }
   // Generate drivable area
   {
     const auto common_parameters = planner_data_->parameters;
@@ -397,6 +413,27 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
 
 bool LaneChangeModule::isSafe() const { return status_.is_safe; }
 
+bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const
+{
+  // check lane departure
+  lanelet::ConstLanelets lanes;
+  lanes.reserve(status_.current_lanes.size() + status_.lane_change_lanes.size());
+  lanes.insert(lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end());
+  lanes.insert(lanes.end(), status_.lane_change_lanes.begin(), status_.lane_change_lanes.end());
+  if (lane_departure_checker_.checkPathWillLeaveLane(lanes, 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 = planner_data_->self_pose->pose;

planning/behavior_path_planner/src/utilities.cpp

@@ -1934,12 +1934,33 @@ 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)
 {
   const auto & route_handler = planner_data->route_handler;
   const auto current_pose = planner_data->self_pose->pose;
   const auto common_parameters = planner_data->parameters;
+  const auto routing_graph_ptr = route_handler->getRoutingGraphPtr();
 
   lanelet::ConstLanelet current_lane;
   if (!route_handler->getClosestLaneletWithinRoute(current_pose, &current_lane)) {
@@ -1950,25 +1971,56 @@ 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_lanes
-  const auto next_lanes = route_handler->getNextLanelets(current_lanes.back());
-  if (!next_lanes.empty()) {
-    // TODO(kosuke55) which lane should be added?
-    current_lanes.push_back(next_lanes.front());
+  return extendLanes(route_handler, current_lanes);
+}
+
+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;
   }
 
-  // Add prev_lane
-  lanelet::ConstLanelets prev_lanes;
-  if (route_handler->getPreviousLaneletsWithinRoute(current_lanes.front(), &prev_lanes)) {
-    // TODO(kosuke55) which lane should be added?
-    current_lanes.insert(current_lanes.begin(), 0, prev_lanes.front());
+  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 current_lanes;
+  return true;
 }
 
 }  // namespace util

planning/route_handler/include/route_handler/route_handler.hpp

@@ -123,6 +123,7 @@ class RouteHandler
    */
   boost::optional<lanelet::ConstLanelet> getLeftLanelet(
     const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const;
 
   /**

planning/route_handler/src/route_handler.cpp

@@ -771,6 +771,12 @@ bool RouteHandler::getClosestLaneletWithinRoute(
   return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet);
 }
 
+lanelet::ConstLanelets RouteHandler::getPreviousLanelets(
+  const lanelet::ConstLanelet & lanelet) const
+{
+  return routing_graph_ptr_->previous(lanelet);
+}
+
 bool RouteHandler::getNextLaneletWithinRoute(
   const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const
 {