Revert "fix(behavior_path_planner): check lane departure and relative…

… angle for lane change (autowarefoundation#2379) (#192)"

This reverts commit 52fd315.

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -102,8 +102,7 @@ class LaneDepartureChecker
     vehicle_info_ptr_ = std::make_shared<vehicle_info_util::VehicleInfo>(vehicle_info);
   }
 
-  bool checkPathWillLeaveLane(
-    const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const;
+  bool checkPathWillLeaveLane(const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path);
 
   vehicle_info_util::VehicleInfo vehicle_info_public_;
 
@@ -123,7 +122,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) const;
+  std::vector<LinearRing2d> createVehicleFootprints(const PathWithLaneId & path);
 
   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
+  const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path)
 {
   std::vector<LinearRing2d> vehicle_footprints = createVehicleFootprints(path);
   lanelet::ConstLanelets candidate_lanelets = getCandidateLanelets(lanelets, vehicle_footprints);
@@ -242,8 +242,7 @@ std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
   return vehicle_footprints;
 }
 
-std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(
-  const PathWithLaneId & path) const
+std::vector<LinearRing2d> LaneDepartureChecker::createVehicleFootprints(const PathWithLaneId & path)
 {
   // Create vehicle footprint in base_link coordinate
   const auto local_vehicle_footprint = createVehicleFootprint(*vehicle_info_ptr_);

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -61,13 +61,6 @@ 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{};

planning/behavior_path_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};
@@ -188,7 +185,6 @@ 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/scene_module/lane_change/util.hpp

@@ -75,9 +75,6 @@ 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
 

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -184,15 +184,6 @@ 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);
 
@@ -205,10 +196,6 @@ 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
@@ -305,14 +292,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);
 
-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,7 +44,6 @@ LaneChangeModule::LaneChangeModule(
   uuid_left_{generateUUID()},
   uuid_right_{generateUUID()}
 {
-  lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo());
 }
 
 BehaviorModuleOutput LaneChangeModule::run()
@@ -53,12 +52,6 @@ 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);
@@ -84,7 +77,7 @@ void LaneChangeModule::onExit()
 {
   clearWaitingApproval();
   removeRTCStatus();
-  current_state_ = BT::NodeStatus::SUCCESS;
+  current_state_ = BT::NodeStatus::IDLE;
   RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onExit");
 }
 
@@ -129,11 +122,6 @@ 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;
@@ -155,10 +143,6 @@ 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;
@@ -413,33 +397,6 @@ 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;

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

@@ -498,49 +498,5 @@ 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