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

…or lane change (autowarefoundation#2379) (autowarefoundation#192) * fix(behavior_path_planner): check lane departure and relative angle for 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> * feat: extend lanes for lane_departure_checker Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * fix: extend lanes Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * feat: expand lane Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
badai-nguyen · Nov 30, 2022 · 52fd315 · 52fd315
1 parent ac57471
commit 52fd315
Show file tree

Hide file tree

Showing 11 changed files with 336 additions and 17 deletions.
diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/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);

diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/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_);

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -61,6 +61,13 @@ struct Approval
   ModuleNameStamped is_force_approved{};
 };
 
+struct DrivableLanes
+{
+  lanelet::ConstLanelet right_lane;
+  lanelet::ConstLanelet left_lane;
+  lanelet::ConstLanelets middle_lanes;
+};
+
 struct PlannerData
 {
   PoseStamped::ConstSharedPtr self_pose{};

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

diff --git a/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
@@ -75,6 +75,9 @@ bool hasEnoughDistance(
   const bool isInGoalRouteSection, const Pose & goal_pose,
   const lanelet::routing::RoutingGraphContainer & overall_graphs);
 bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
+std::vector<DrivableLanes> generateDrivableLanes(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & lane_change_lanes);
 }  // namespace lane_change_utils
 }  // namespace behavior_path_planner
 

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -184,6 +184,15 @@ PredictedObjects filterObjectsByVelocity(
   const PredictedObjects & objects, double min_v, double max_v);
 
 // drivable area generation
+lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes);
+lanelet::ConstLanelets transformToLanelets(const std::vector<DrivableLanes> & drivable_lanes);
+boost::optional<lanelet::ConstLanelet> getRightLanelet(
+  const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes);
+boost::optional<lanelet::ConstLanelet> getLeftLanelet(
+  const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes);
+std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
+std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
 
 void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image);
 
@@ -196,6 +205,10 @@ OccupancyGrid generateDrivableArea(
   const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double resolution,
   const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data);
 
+std::vector<DrivableLanes> expandLanelets(
+  const std::vector<DrivableLanes> & drivable_lanes, const double left_bound_offset,
+  const double right_bound_offset, const std::vector<std::string> & types_to_skip = {});
+
 lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
   const std::shared_ptr<const PlannerData> & planner_data);
 // goal management
@@ -292,9 +305,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
 

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,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,33 @@ 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;
+  const auto drivable_area_left_bound_offset = 0.5;
+  const auto drivable_area_right_bound_offset = 0.5;
+
+  // 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, 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 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;

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
@@ -498,5 +498,49 @@ bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose)
   return obj_from_ego.position.x > 0;
 }
 
+std::vector<DrivableLanes> generateDrivableLanes(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & lane_change_lanes)
+{
+  size_t current_lc_idx = 0;
+  std::vector<DrivableLanes> drivable_lanes(current_lanes.size());
+  for (size_t i = 0; i < current_lanes.size(); ++i) {
+    const auto & current_lane = current_lanes.at(i);
+    drivable_lanes.at(i).left_lane = current_lane;
+    drivable_lanes.at(i).right_lane = current_lane;
+
+    const auto left_lane = route_handler.getLeftLanelet(current_lane);
+    const auto right_lane = route_handler.getRightLanelet(current_lane);
+    if (!left_lane && !right_lane) {
+      continue;
+    }
+
+    for (size_t lc_idx = current_lc_idx; lc_idx < lane_change_lanes.size(); ++lc_idx) {
+      const auto & lc_lane = lane_change_lanes.at(lc_idx);
+      if (left_lane && lc_lane.id() == left_lane->id()) {
+        drivable_lanes.at(i).left_lane = lc_lane;
+        current_lc_idx = lc_idx;
+        break;
+      }
+
+      if (right_lane && lc_lane.id() == right_lane->id()) {
+        drivable_lanes.at(i).right_lane = lc_lane;
+        current_lc_idx = lc_idx;
+        break;
+      }
+    }
+  }
+
+  for (size_t i = current_lc_idx + 1; i < lane_change_lanes.size(); ++i) {
+    const auto & lc_lane = lane_change_lanes.at(i);
+    DrivableLanes drivable_lane;
+    drivable_lane.left_lane = lc_lane;
+    drivable_lane.right_lane = lc_lane;
+    drivable_lanes.push_back(drivable_lane);
+  }
+
+  return drivable_lanes;
+}
+
 }  // namespace lane_change_utils
 }  // namespace behavior_path_planner