feat(pull_out): support pull out normal lane

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

planning/behavior_path_planner/config/pull_out/pull_out.param.yaml

@@ -9,8 +9,8 @@
       # shift pull out
       enable_shift_pull_out: true
       shift_pull_out_velocity: 2.0
-      pull_out_sampling_num: 4
       minimum_shift_pull_out_distance: 20.0
+      pull_out_sampling_num: 4
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.0

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -57,6 +57,7 @@ using nav_msgs::msg::Odometry;
 using route_handler::RouteHandler;
 using tier4_planning_msgs::msg::LateralOffset;
 using PlanResult = PathWithLaneId::SharedPtr;
+using unique_identifier_msgs::msg::UUID;
 
 struct BoolStamped
 {
@@ -140,6 +141,7 @@ struct PlannerData
   PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
   std::optional<PoseWithUuidStamped> prev_modified_goal{};
+  std::optional<UUID> prev_route_id{};
   lanelet::ConstLanelets current_lanes{};
   std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
   BehaviorPathPlannerParameters parameters{};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp

@@ -47,18 +47,20 @@ using lane_departure_checker::LaneDepartureChecker;
 
 struct PullOutStatus
 {
-  PullOutPath pull_out_path;
-  size_t current_path_idx = 0;
-  PlannerType planner_type = PlannerType::NONE;
-  PathWithLaneId backward_path;
-  lanelet::ConstLanelets current_lanes;
-  lanelet::ConstLanelets pull_out_lanes;
-  std::vector<DrivableLanes> lanes;
-  std::vector<uint64_t> lane_follow_lane_ids;
-  std::vector<uint64_t> pull_out_lane_ids;
-  bool is_safe = false;
-  bool back_finished = false;
-  Pose pull_out_start_pose;
+  PullOutPath pull_out_path{};
+  size_t current_path_idx{0};
+  PlannerType planner_type{PlannerType::NONE};
+  PathWithLaneId backward_path{};
+  lanelet::ConstLanelets current_lanes{};
+  lanelet::ConstLanelets pull_out_lanes{};
+  std::vector<DrivableLanes> lanes{};
+  std::vector<uint64_t> lane_follow_lane_ids{};
+  std::vector<uint64_t> pull_out_lane_ids{};
+  bool is_safe{false};
+  bool back_finished{false};
+  Pose pull_out_start_pose{};
+
+  PullOutStatus() {}
 };
 
 class PullOutModule : public SceneModuleInterface
@@ -107,13 +109,14 @@ class PullOutModule : public SceneModuleInterface
   vehicle_info_util::VehicleInfo vehicle_info_;
 
   std::vector<std::shared_ptr<PullOutPlannerBase>> pull_out_planners_;
-  PullOutStatus status_;
+  mutable PullOutStatus status_;
 
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
 
   std::unique_ptr<rclcpp::Time> last_route_received_time_;
   std::unique_ptr<rclcpp::Time> last_pull_out_start_update_time_;
   std::unique_ptr<Pose> last_approved_pose_;
+  mutable bool has_received_new_route_{false};
 
   std::shared_ptr<PullOutPlannerBase> getCurrentPlanner() const;
   PathWithLaneId getFullPath() const;
@@ -141,6 +144,11 @@ class PullOutModule : public SceneModuleInterface
   bool hasFinishedCurrentPath();
 
   void setDebugData() const;
+
+// tempolary for old architecture
+#ifdef USE_OLD_ARCHITECTURE
+  mutable bool is_executed_{false};
+#endif
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1264,6 +1264,8 @@ void BehaviorPathPlannerNode::run()
     modified_goal_publisher_->publish(modified_goal);
   }
 
+  planner_data_->prev_route_id = planner_data_->route_handler->getRouteUuid();
+
   if (planner_data_->parameters.visualize_maximum_drivable_area) {
     const auto maximum_drivable_area = marker_utils::createFurthestLineStringMarkerArray(
       utils::getMaximumDrivableArea(planner_data_));

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -104,13 +104,37 @@ void PullOutModule::processOnExit()
 
 bool PullOutModule::isExecutionRequested() const
 {
+  has_received_new_route_ =
+    !planner_data_->prev_route_id ||
+    *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid();
+
+  if (has_received_new_route_) {
+    status_ = PullOutStatus();
+  }
+
+#ifdef USE_OLD_ARCHITECTURE
+  if (is_executed_) {
+    return true;
+  }
+#endif
+
   if (current_state_ == ModuleStatus::RUNNING) {
     return true;
   }
 
+  if (!has_received_new_route_) {
+#ifdef USE_OLD_ARCHITECTURE
+    is_executed_ = false;
+#endif
+    return false;
+  }
+
   const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) <
                           parameters_->th_arrived_distance;
   if (!is_stopped) {
+#ifdef USE_OLD_ARCHITECTURE
+    is_executed_ = false;
+#endif
     return false;
   }
 
@@ -126,19 +150,15 @@ bool PullOutModule::isExecutionRequested() const
   auto lanes = current_lanes;
   lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end());
   if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) {
+#ifdef USE_OLD_ARCHITECTURE
+    is_executed_ = false;
+#endif
     return false;
   }
 
-  // Check if any of the footprint points are in the shoulder lane
-  lanelet::Lanelet closest_shoulder_lanelet;
-  if (!lanelet::utils::query::getClosestLanelet(
-        pull_out_lanes, planner_data_->self_odometry->pose.pose, &closest_shoulder_lanelet)) {
-    return false;
-  }
-  if (!isOverlappedWithLane(closest_shoulder_lanelet, vehicle_footprint)) {
-    return false;
-  }
-
+#ifdef USE_OLD_ARCHITECTURE
+  is_executed_ = true;
+#endif
   return true;
 }
 
@@ -532,19 +552,8 @@ PathWithLaneId PullOutModule::generateStopPath() const
 
 void PullOutModule::updatePullOutStatus()
 {
-  // if new route is received, reset status
-  const bool has_received_new_route =
-    last_route_received_time_ == nullptr ||
-    *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp;
-  if (has_received_new_route) {
-    RCLCPP_INFO(getLogger(), "Receive new route, so reset status");
-    resetStatus();
-  }
-  last_route_received_time_ =
-    std::make_unique<rclcpp::Time>(planner_data_->route_handler->getRouteHeader().stamp);
-
   // skip updating if enough time has not passed for preventing chattering between back and pull_out
-  if (!has_received_new_route && !status_.back_finished) {
+  if (!has_received_new_route_ && !status_.back_finished) {
     if (last_pull_out_start_update_time_ == nullptr) {
       last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
     }
@@ -711,7 +720,14 @@ bool PullOutModule::hasFinishedPullOut() const
     lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose);
   const auto arclength_pull_out_end =
     lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose);
-  return arclength_current.length - arclength_pull_out_end.length > 0.0;
+
+  const bool has_finished = arclength_current.length - arclength_pull_out_end.length > 0.0;
+
+#ifdef USE_OLD_ARCHITECTURE
+  is_executed_ = !has_finished;
+#endif
+
+  return has_finished;
 }
 
 void PullOutModule::checkBackFinished()

planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp

@@ -166,6 +166,10 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
     // calculate after/before shifted pull out distance
     // lateral distance from road center to start pose
     const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
+    std::cerr << "shift_length: " << shift_length << std::endl;
+
+    // const bool use_minimum_shift_length = shift_length < 1.0;
+
     const double pull_out_distance = std::max(
       PathShifter::calcLongitudinalDistFromJerk(
         abs(shift_length), lateral_jerk, shift_pull_out_velocity),

planning/behavior_path_planner/src/utils/pull_out/util.cpp

@@ -96,23 +96,22 @@ Pose getBackedPose(
   return backed_pose;
 }
 
-// getShoulderLanesOnCurrentPose
 lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr<const PlannerData> & planner_data)
 {
-  const double pull_out_lane_length = 200.0;
   const double & vehicle_width = planner_data->parameters.vehicle_width;
   const auto & route_handler = planner_data->route_handler;
-  const auto current_pose = planner_data->self_odometry->pose.pose;
+  const auto & current_pose = planner_data->self_odometry->pose.pose;
 
   lanelet::ConstLanelet current_shoulder_lane;
   lanelet::ConstLanelets shoulder_lanes;
   if (route_handler->getPullOutStartLane(
         route_handler->getShoulderLanelets(), current_pose, vehicle_width,
         &current_shoulder_lane)) {
-    shoulder_lanes = route_handler->getShoulderLaneletSequence(
-      current_shoulder_lane, current_pose, pull_out_lane_length, pull_out_lane_length);
+    // pull out from shoulder lane
+    return route_handler->getShoulderLaneletSequence(current_shoulder_lane, current_pose);
   }
 
-  return shoulder_lanes;
+  // pull out from road lane
+  return utils::getExtendedCurrentLanes(planner_data);
 }
 }  // namespace behavior_path_planner::pull_out_utils