fix(start_planner): publish stop path when goal is behind from ego

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -149,6 +149,12 @@ class StartPlannerModule : public SceneModuleInterface
   bool isStopped();
   bool hasFinishedCurrentPath();
 
+  // check if the goal is located behind the ego in the same route segment.
+  bool IsGoalBehindOfEgoInSameRouteSegment() const;
+
+  // generate BehaviorPathOutput with stopping path.
+  BehaviorModuleOutput generateStopOutput() const;
+
   void setDebugData() const;
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -153,6 +153,12 @@ ModuleStatus StartPlannerModule::updateState()
 
 BehaviorModuleOutput StartPlannerModule::plan()
 {
+  if (IsGoalBehindOfEgoInSameRouteSegment()) {
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
+      return generateStopOutput();
+  }
+
   if (isWaitingApproval()) {
     clearWaitingApproval();
     resetPathCandidate();
@@ -296,6 +302,12 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
   updatePullOutStatus();
   waitApproval();
 
+  if (IsGoalBehindOfEgoInSameRouteSegment()) {
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now");
+      return generateStopOutput();
+  }
+
   BehaviorModuleOutput output;
   if (!status_.is_safe) {
     RCLCPP_WARN_THROTTLE(
@@ -841,6 +853,46 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
   return turn_signal;
 }
 
+bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const
+{
+  const auto & rh = planner_data_->route_handler;
+
+  // Check if the goal and ego are in the same route segment. If not, this is out of scope of this
+  // function. Return false.
+  lanelet::ConstLanelet ego_lanelet;
+  rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet);
+  const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet);
+
+  if (!is_ego_in_goal_route_section) {
+    return false;
+  }
+
+  // If the goal and ego are in the same route segment, check the goal and ego pose relation.
+  // Return true when the goal is located behind of ego.
+  const auto ego_lane_path = rh->getCenterLinePath(
+    lanelet::ConstLanelets{ego_lanelet}, 0.0, std::numeric_limits<double>::max());
+  const auto dist_ego_to_goal = motion_utils::calcSignedArcLength(
+    ego_lane_path.points, getEgoPosition(), rh->getGoalPose().position);
+
+  const bool is_goal_behind_of_ego = (dist_ego_to_goal < 0.0);
+  return is_goal_behind_of_ego;
+}
+
+// NOTE: this must be called after updatePullOutStatus(). This must be fixed.
+BehaviorModuleOutput StartPlannerModule::generateStopOutput() const
+{
+  BehaviorModuleOutput output;
+  output.path = std::make_shared<PathWithLaneId>(generateStopPath());
+
+  DrivableAreaInfo current_drivable_area_info;
+  current_drivable_area_info.drivable_lanes = status_.lanes;
+  output.drivable_area_info = utils::combineDrivableAreaInfo(
+    current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
+
+  output.reference_path = getPreviousModuleOutput().reference_path;
+  return output;
+}
+
 void StartPlannerModule::setDebugData() const
 {
   using marker_utils::createPathMarkerArray;