feat(goal_planner): consider lane changing previous module path

Signed-off-by: Mamoru Sobue <[email protected]>

Author: soblin

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp

@@ -55,8 +55,8 @@ class PathDecisionStateController
     const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects,
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const bool is_current_safe, const GoalPlannerParameters & parameters,
-    const GoalSearcher & goal_searcher,
+    const bool is_current_safe, const bool lane_change_status_changed,
+    const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher,
     std::vector<autoware_utils::Polygon2d> & ego_polygons_expanded);
 
   PathDecisionState get_current_state() const { return current_state_; }
@@ -75,8 +75,8 @@ class PathDecisionStateController
     const PredictedObjects & dynamic_target_objects,
     const std::shared_ptr<const PlannerData> planner_data,
     const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-    const bool is_current_safe, const GoalPlannerParameters & parameters,
-    const GoalSearcher & goal_searcher,
+    const bool is_current_safe, const bool lane_change_status_changed,
+    const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher,
     std::vector<autoware_utils::Polygon2d> & ego_polygons_expanded) const;
 };
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -302,6 +302,11 @@ class GoalPlannerModule : public SceneModuleInterface
   const bool left_side_parking_;
 
   bool trigger_thread_on_approach_{false};
+
+  // signal path generator and state manager to regenerate path candidates and remain NOT_DECIDED
+  std::optional<bool> prev_lane_change_status_{};
+  bool lane_change_status_changed_{false};
+
   // pre-generate lane parking paths in a separate thread
   rclcpp::TimerBase::SharedPtr lane_parking_timer_;
   rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp

@@ -91,6 +91,8 @@ struct PullOverPath
 
   std::vector<Pose> debug_poses{};
 
+  std::optional<PathWithLaneId> debug_processed_prev_module_path;
+
 private:
   PullOverPath(
     const PullOverPlannerType & type, const size_t id, const Pose & start_pose,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp

@@ -45,7 +45,7 @@ class LaneParkingRequest
     const PlannerData & planner_data, const ModuleStatus & current_status,
     const BehaviorModuleOutput & upstream_module_output,
     const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
-    const bool trigger_thread_on_approach);
+    const bool trigger_thread_on_approach, const bool lane_change_status_changed);
 
   const autoware_utils::LinearRing2d vehicle_footprint_;
   const GoalCandidates goal_candidates_;
@@ -60,6 +60,7 @@ class LaneParkingRequest
   const std::optional<PullOverPath> & get_pull_over_path() const { return pull_over_path_; }
   const PathDecisionState & get_prev_data() const { return prev_data_; }
   bool trigger_thread_on_approach() const { return trigger_thread_on_approach_; }
+  bool lane_change_status_changed() const { return lane_change_status_changed_; }
 
 private:
   std::shared_ptr<PlannerData> planner_data_;
@@ -68,6 +69,7 @@ class LaneParkingRequest
   std::optional<PullOverPath> pull_over_path_;  //<! pull over path selected by main thread
   PathDecisionState prev_data_;
   bool trigger_thread_on_approach_{false};
+  bool lane_change_status_changed_{false};
 };
 
 struct LaneParkingResponse

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp

@@ -252,6 +252,23 @@ bool hasDeviatedFromPath(
  */
 bool has_stopline_except_terminal(const PathWithLaneId & path);
 
+/**
+ * @brief find the lanelet that has changed "laterally" from previous lanelet on the routing graph
+ * @return the lanelet that changed "laterally" if the path is lane changing, otherwise nullopt
+ */
+std::optional<lanelet::ConstLanelet> find_lane_change_completed_lanelet(
+  const PathWithLaneId & path, const lanelet::LaneletMapConstPtr lanelet_map,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph);
+
+/**
+ * @brief generate lanelets with which pull over path is alinged
+ * @note if lane changing path is detected, this returns lanelets aligned with later part of the
+ * lane changing path
+ */
+lanelet::ConstLanelets get_reference_lanelets_for_pullover(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const double backward_length, const double forward_length);
+
 }  // namespace autoware::behavior_path_planner::goal_planner_utils
 
 #endif  // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp

@@ -31,14 +31,14 @@ void PathDecisionStateController::transit_state(
   const PredictedObjects & dynamic_target_objects,
   const std::shared_ptr<const PlannerData> planner_data,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const bool is_current_safe, const GoalPlannerParameters & parameters,
-  const GoalSearcher & goal_searcher,
+  const bool is_current_safe, const bool lane_change_status_changed,
+  const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher,
   std::vector<autoware_utils::Polygon2d> & ego_polygons_expanded)
 {
   const auto next_state = get_next_state(
     pull_over_path_opt, upstream_module_has_stopline, now, static_target_objects,
-    dynamic_target_objects, planner_data, occupancy_grid_map, is_current_safe, parameters,
-    goal_searcher, ego_polygons_expanded);
+    dynamic_target_objects, planner_data, occupancy_grid_map, is_current_safe,
+    lane_change_status_changed, parameters, goal_searcher, ego_polygons_expanded);
   current_state_ = next_state;
 }
 
@@ -48,8 +48,8 @@ PathDecisionState PathDecisionStateController::get_next_state(
   const PredictedObjects & dynamic_target_objects,
   const std::shared_ptr<const PlannerData> planner_data,
   const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
-  const bool is_current_safe, const GoalPlannerParameters & parameters,
-  const GoalSearcher & goal_searcher,
+  const bool is_current_safe, const bool lane_change_status_changed,
+  const GoalPlannerParameters & parameters, const GoalSearcher & goal_searcher,
   std::vector<autoware_utils::Polygon2d> & ego_polygons_expanded) const
 {
   auto next_state = current_state_;
@@ -74,6 +74,12 @@ PathDecisionState PathDecisionStateController::get_next_state(
     return next_state;
   }
 
+  // if lane change is triggered, reference path for pull over must be reset accordingly
+  if (lane_change_status_changed) {
+    next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED;
+    return next_state;
+  }
+
   // while upstream module path has stopline, goal_planner remains NOT_DECIDED
   if (upstream_module_has_stopline) {
     next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -269,6 +269,7 @@ void LaneParkingPlanner::onTimer()
   const auto & prev_data = local_request.get_prev_data();
   const auto trigger_thread_on_approach = local_request.trigger_thread_on_approach();
   const auto use_bus_stop_area = local_request.use_bus_stop_area_;
+  const auto lane_change_status_changed = local_request.lane_change_status_changed();
 
   if (!trigger_thread_on_approach) {
     return;
@@ -317,6 +318,10 @@ void LaneParkingPlanner::onTimer()
       RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path");
       return true;
     }
+    if (lane_change_status_changed) {
+      RCLCPP_INFO(getLogger(), "[LaneParkingPlanner]: lane_change_status changed, so replan");
+      return true;
+    }
     const bool upstream_module_has_stopline_except_terminal =
       goal_planner_utils::has_stopline_except_terminal(upstream_module_output.path);
     if (upstream_module_has_stopline_except_terminal) {
@@ -613,6 +618,12 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
         vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(),
         use_bus_stop_area_ && goal_searcher.bus_stop_area_available());
     }
+    const auto lane_change_status =
+      goal_planner_utils::find_lane_change_completed_lanelet(
+        getPreviousModuleOutput().path, planner_data_->route_handler->getLaneletMapPtr(),
+        planner_data_->route_handler->getRoutingGraphPtr())
+        .has_value();
+
     // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that
     // planner_data_ is not nullptr, so it is OK to copy as value
     // By copying PlannerData as value, the internal shared member variables are also copied
@@ -622,7 +633,8 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
     // count is affected
     lane_parking_request_.value().update(
       *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path,
-      path_decision_controller_.get_current_state(), trigger_thread_on_approach_);
+      path_decision_controller_.get_current_state(), trigger_thread_on_approach_,
+      lane_change_status);
     // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as
     // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since
     // behavior_path_planner::run() updates
@@ -685,28 +697,41 @@ void GoalPlannerModule::updateData()
     goal_candidates_ = generateGoalCandidates(goal_searcher_.value(), use_bus_stop_area_);
   }
 
+  const auto lane_change_status =
+    goal_planner_utils::find_lane_change_completed_lanelet(
+      getPreviousModuleOutput().path, planner_data_->route_handler->getLaneletMapPtr(),
+      planner_data_->route_handler->getRoutingGraphPtr())
+      .has_value();
+  lane_change_status_changed_ =
+    prev_lane_change_status_ && prev_lane_change_status_.value() != lane_change_status;
+  prev_lane_change_status_ = lane_change_status;
+
   const lanelet::ConstLanelets current_lanes =
     utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
 
-  if (
-    !trigger_thread_on_approach_ &&
-    utils::isAllowedGoalModification(planner_data_->route_handler) &&
-    goal_planner_utils::is_goal_reachable_on_path(
-      current_lanes, *(planner_data_->route_handler), left_side_parking_)) {
-    const double self_to_goal_arc_length = utils::getSignedDistance(
-      planner_data_->self_odometry->pose.pose, planner_data_->route_handler->getOriginalGoalPose(),
-      current_lanes);
-    if (self_to_goal_arc_length < parameters_.pull_over_prepare_length) {
-      trigger_thread_on_approach_ = true;
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
+    if (
+      !trigger_thread_on_approach_ &&
+      utils::isAllowedGoalModification(planner_data_->route_handler) &&
+      goal_planner_utils::is_goal_reachable_on_path(
+        current_lanes, *(planner_data_->route_handler), left_side_parking_)) {
+      const double self_to_goal_arc_length = utils::getSignedDistance(
+        planner_data_->self_odometry->pose.pose,
+        planner_data_->route_handler->getOriginalGoalPose(), current_lanes);
+      if (self_to_goal_arc_length < parameters_.pull_over_prepare_length) {
+        trigger_thread_on_approach_ = true;
+        [[maybe_unused]] const auto send_only_request = syncWithThreads();
+        RCLCPP_INFO(
+          getLogger(), "start preparing goal candidates once for goal ahead of %f meter",
+          self_to_goal_arc_length);
+        return;
+      }
+    }
+    if (lane_change_status_changed_) {
       [[maybe_unused]] const auto send_only_request = syncWithThreads();
-      RCLCPP_INFO(
-        getLogger(), "start preparing goal candidates once for goal ahead of %f meter",
-        self_to_goal_arc_length);
+      RCLCPP_INFO(getLogger(), "restart preparing goal candidates since lane_change is detected");
       return;
     }
-  }
-
-  if (getCurrentStatus() == ModuleStatus::IDLE) {
     return;
   }
 
@@ -737,10 +762,12 @@ void GoalPlannerModule::updateData()
 
   const bool upstream_module_has_stopline_except_terminal =
     goal_planner_utils::has_stopline_except_terminal(getPreviousModuleOutput().path);
+
   path_decision_controller_.transit_state(
     pull_over_path_recv, upstream_module_has_stopline_except_terminal, clock_->now(),
     static_target_objects, dynamic_target_objects, planner_data_, occupancy_grid_map_,
-    is_current_safe, parameters_, goal_searcher, debug_data_.ego_polygons_expanded);
+    is_current_safe, lane_change_status_changed_, parameters_, goal_searcher,
+    debug_data_.ego_polygons_expanded);
   const auto new_decision_state = path_decision_controller_.get_current_state();
 
   auto [lane_parking_response, freespace_parking_response] = syncWithThreads();
@@ -2561,6 +2588,11 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data)
       createPathMarkerArray(pull_over_path.full_path(), "full_path", 0, 0.0, 0.5, 0.9));
     add_debug_marker(
       createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0));
+    if (pull_over_path.debug_processed_prev_module_path) {
+      add_debug_marker(createPathMarkerArray(
+        pull_over_path.debug_processed_prev_module_path.value(), "processed_prev_module_path", 0,
+        0.9, 0.5, 0.9));
+    }
 
     // visualize each partial path
     for (size_t i = 0; i < pull_over_path.partial_paths().size(); ++i) {

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -54,9 +54,8 @@ std::optional<PullOverPath> ShiftPullOver::plan(
   const int shift_sampling_num = parameters_.shift_sampling_num;
   const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num;
 
-  const auto road_lanes = utils::getExtendedCurrentLanesFromPath(
-    upstream_module_output.path, planner_data, backward_search_length, forward_search_length,
-    /*forward_only_in_route*/ false);
+  const auto road_lanes = goal_planner_utils::get_reference_lanelets_for_pullover(
+    upstream_module_output.path, planner_data, backward_search_length, forward_search_length);
 
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, backward_search_length, forward_search_length);
@@ -286,6 +285,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     return {};
   }
 
+  pull_over_path.debug_processed_prev_module_path = processed_prev_module_path;
   return pull_over_path;
 }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp

@@ -23,7 +23,7 @@ void LaneParkingRequest::update(
   const PlannerData & planner_data, const ModuleStatus & current_status,
   const BehaviorModuleOutput & upstream_module_output,
   const std::optional<PullOverPath> & pull_over_path, const PathDecisionState & prev_data,
-  const bool trigger_thread_on_approach)
+  const bool trigger_thread_on_approach, const bool lane_change_status_changed)
 {
   planner_data_ = std::make_shared<PlannerData>(planner_data);
   planner_data_->route_handler = std::make_shared<RouteHandler>(*(planner_data.route_handler));
@@ -32,6 +32,7 @@ void LaneParkingRequest::update(
   pull_over_path_ = pull_over_path;
   prev_data_ = prev_data;
   trigger_thread_on_approach_ = trigger_thread_on_approach;
+  lane_change_status_changed_ = lane_change_status_changed;
 }
 
 void FreespaceParkingRequest::initializeOccupancyGridMap(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

@@ -1076,4 +1076,51 @@ bool has_stopline_except_terminal(const PathWithLaneId & path)
          path.points.size();
 }
 
+std::optional<lanelet::ConstLanelet> find_lane_change_completed_lanelet(
+  const PathWithLaneId & path, const lanelet::LaneletMapConstPtr lanelet_map,
+  const lanelet::routing::RoutingGraphConstPtr routing_graph)
+{
+  std::vector<lanelet::Id> path_lane_ids;
+  for (const auto & point : path.points) {
+    const auto & lane_ids = point.lane_ids;
+    for (const auto & lane_id : lane_ids) {
+      if (std::find(path_lane_ids.begin(), path_lane_ids.end(), lane_id) == path_lane_ids.end()) {
+        path_lane_ids.push_back(lane_id);
+      }
+    }
+  }
+
+  if (path_lane_ids.size() < 2) {
+    return std::nullopt;
+  }
+  for (unsigned i = 0, j = 1; i < path_lane_ids.size() && j < path_lane_ids.size(); i++, j++) {
+    const auto & lane1 = lanelet_map->laneletLayer.get(path_lane_ids.at(i));
+    const auto & lane2 = lanelet_map->laneletLayer.get(path_lane_ids.at(j));
+    const auto & followings = routing_graph->following(lane1);
+    if (std::any_of(followings.begin(), followings.end(), [&](const auto & lane) {
+          return lane.id() == lane2.id();
+        })) {
+      continue;
+    }
+    return lane2;
+  }
+  return std::nullopt;
+}
+
+lanelet::ConstLanelets get_reference_lanelets_for_pullover(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const double backward_length, const double forward_length)
+{
+  const auto & routing_graph = planner_data->route_handler->getRoutingGraphPtr();
+  const auto & lanelet_map = planner_data->route_handler->getLaneletMapPtr();
+  const auto lane_change_complete_lane =
+    find_lane_change_completed_lanelet(path, lanelet_map, routing_graph);
+  if (!lane_change_complete_lane) {
+    return utils::getExtendedCurrentLanesFromPath(
+      path, planner_data, backward_length, forward_length,
+      /*forward_only_in_route*/ false);
+  }
+  return planner_data->route_handler->getLaneletSequence(
+    *lane_change_complete_lane, backward_length, forward_length);
+}
 }  // namespace autoware::behavior_path_planner::goal_planner_utils