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

@@ -4,13 +4,14 @@
       th_arrived_distance: 1.0
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
+      th_blinker_on_lateral_offset: 1.0
       collision_check_margin: 1.0
       collision_check_distance_from_end: 1.0
       # 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/docs/behavior_path_planner_pull_out_design.md

@@ -54,6 +54,7 @@ PullOutPath --o PullOutPlannerBase
 | th_arrived_distance_m             | [m]   | double | distance threshold for arrival of path termination                   | 1.0           |
 | th_stopped_velocity_mps           | [m/s] | double | velocity threshold for arrival of path termination                   | 0.01          |
 | th_stopped_time_sec               | [s]   | double | time threshold for arrival of path termination                       | 1.0           |
+| th_blinker_on_lateral_offset      | [m]   | double | lateral distance threshold for turning on blinker                    | 1.0           |
 | collision_check_margin            | [m]   | double | Obstacle collision check margin                                      | 1.0           |
 | collision_check_distance_from_end | [m]   | double | collision check distance from end point. currently only for pull out | 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;
+
+// temporary for old architecture
+#ifdef USE_OLD_ARCHITECTURE
+  mutable bool is_executed_{false};
+#endif
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -44,6 +44,7 @@ struct GoalPlannerParameters
   double th_arrived_distance;
   double th_stopped_velocity;
   double th_stopped_time;
+  double th_blinker_on_lateral_offset;
 
   // goal search
   std::string search_priority;   // "efficient_path" or "close_goal"

planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp

@@ -28,6 +28,7 @@ struct PullOutParameters
   double th_arrived_distance;
   double th_stopped_velocity;
   double th_stopped_time;
+  double th_blinker_on_lateral_offset;
   double collision_check_margin;
   double collision_check_distance_from_end;
   // shift pull out

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1034,6 +1034,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam()
   p.th_arrived_distance = declare_parameter<double>(ns + "th_arrived_distance");
   p.th_stopped_velocity = declare_parameter<double>(ns + "th_stopped_velocity");
   p.th_stopped_time = declare_parameter<double>(ns + "th_stopped_time");
+  p.th_blinker_on_lateral_offset = declare_parameter<double>(ns + "th_blinker_on_lateral_offset");
   p.collision_check_margin = declare_parameter<double>(ns + "collision_check_margin");
   p.collision_check_distance_from_end =
     declare_parameter<double>(ns + "collision_check_distance_from_end");
@@ -1267,6 +1268,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

@@ -31,6 +31,7 @@
 
 using motion_utils::calcLongitudinalOffsetPose;
 using tier4_autoware_utils::calcOffsetPose;
+using tier4_autoware_utils::inverseTransformPoint;
 
 namespace behavior_path_planner
 {
@@ -104,13 +105,33 @@ 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();
+
+#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 +147,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;
 }
 
@@ -390,6 +407,9 @@ void PullOutModule::incrementPathIndex()
 
 PathWithLaneId PullOutModule::getCurrentPath() const
 {
+  if (status_.pull_out_path.partial_paths.size() <= status_.current_path_idx) {
+    return PathWithLaneId{};
+  }
   return status_.pull_out_path.partial_paths.at(status_.current_path_idx);
 }
 
@@ -532,28 +552,21 @@ 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();
+  if (has_received_new_route_) {
+    status_ = PullOutStatus();
   }
-  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 (last_pull_out_start_update_time_ == nullptr) {
+  if (!has_received_new_route_ && !last_pull_out_start_update_time_ && !status_.back_finished) {
+    if (!last_pull_out_start_update_time_) {
       last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
     }
     const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds();
     if (elapsed_time < parameters_->backward_path_update_duration) {
       return;
     }
-    last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
   }
+  last_pull_out_start_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
 
   const auto & route_handler = planner_data_->route_handler;
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
@@ -711,7 +724,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()
@@ -776,7 +796,10 @@ bool PullOutModule::hasFinishedCurrentPath()
 TurnSignalInfo PullOutModule::calcTurnSignalInfo() const
 {
   TurnSignalInfo turn_signal{};  // output
-  const auto & current_pose = planner_data_->self_odometry->pose.pose;
+
+  const Pose & current_pose = planner_data_->self_odometry->pose.pose;
+  const Pose & start_pose = status_.pull_out_path.start_pose;
+  const Pose & end_pose = status_.pull_out_path.end_pose;
 
   // turn on hazard light when backward driving
   if (!status_.back_finished) {
@@ -785,26 +808,31 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo() const
     turn_signal.desired_start_point = back_start_pose;
     turn_signal.required_start_point = back_start_pose;
     // pull_out start_pose is same to backward driving end_pose
-    turn_signal.required_end_point = status_.pull_out_path.start_pose;
-    turn_signal.desired_end_point = status_.pull_out_path.start_pose;
+    turn_signal.required_end_point = start_pose;
+    turn_signal.desired_end_point = start_pose;
     return turn_signal;
   }
 
   // turn on right signal until passing pull_out end point
   const auto path = getFullPath();
   // pull out path does not overlap
-  const double distance_from_end = motion_utils::calcSignedArcLength(
-    path.points, status_.pull_out_path.end_pose.position, current_pose.position);
-  if (distance_from_end < 0.0) {
+  const double distance_from_end =
+    motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
+  const double lateral_offset = inverseTransformPoint(end_pose.position, start_pose).y;
+
+  if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) {
+    turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
+  } else if (
+    distance_from_end < 0.0 && lateral_offset < -parameters_->th_blinker_on_lateral_offset) {
     turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
   } else {
     turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE;
   }
 
-  turn_signal.desired_start_point = status_.pull_out_path.start_pose;
-  turn_signal.required_start_point = status_.pull_out_path.start_pose;
-  turn_signal.required_end_point = status_.pull_out_path.end_pose;
-  turn_signal.desired_end_point = status_.pull_out_path.end_pose;
+  turn_signal.desired_start_point = start_pose;
+  turn_signal.required_start_point = start_pose;
+  turn_signal.required_end_point = end_pose;
+  turn_signal.desired_end_point = end_pose;
 
   return turn_signal;
 }

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

@@ -158,14 +158,27 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   PathWithLaneId road_lane_reference_path =
     utils::resamplePathWithSpline(route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0);
 
+  bool has_non_shifted_path = false;
   for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk;
        lateral_jerk += jerk_resolution) {
     PathShifter path_shifter{};
     path_shifter.setPath(road_lane_reference_path);
 
     // calculate after/before shifted pull out distance
     // lateral distance from road center to start pose
+    constexpr double minimum_shift_length = 0.01;
     const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
+    // if shift length is too short, add non sifted path
+    if (shift_length < minimum_shift_length && !has_non_shifted_path) {
+      PullOutPath non_shifted_path{};
+      non_shifted_path.partial_paths.push_back(road_lane_reference_path);
+      non_shifted_path.start_pose = start_pose;
+      non_shifted_path.end_pose = start_pose;
+      candidate_paths.push_back(non_shifted_path);
+      has_non_shifted_path = true;
+      continue;
+    }
+
     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