feat(behavior_path_planner): add goal planner (autowarefoundation#3454)

* feat(behavior_path_planner): add goal planner

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

* fix test

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

---------

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

Author: kosuke55

planning/behavior_path_planner/CMakeLists.txt

@@ -33,6 +33,7 @@ set(common_src
   src/utils/pull_over/geometric_pull_over.cpp
   src/utils/pull_over/freespace_pull_over.cpp
   src/utils/pull_over/goal_searcher.cpp
+  src/utils/pull_over/default_fixed_goal_planner.cpp
   src/utils/pull_out/util.cpp
   src/utils/pull_out/shift_pull_out.cpp
   src/utils/pull_out/geometric_pull_out.cpp

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -208,12 +208,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
     const std::shared_ptr<PlannerManager> & planner_manager);
 #endif
 
-  /**
-   * @brief skip smooth goal connection
-   */
-  bool skipSmoothGoalConnection(
-    const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const;
-
   bool keepInputPoints(const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const;
 
   /**

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
 #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp"
 #include "behavior_path_planner/utils/pull_over/freespace_pull_over.hpp"
 #include "behavior_path_planner/utils/pull_over/geometric_pull_over.hpp"
 #include "behavior_path_planner/utils/pull_over/goal_searcher.hpp"
@@ -127,6 +128,7 @@ class PullOverModule : public SceneModuleInterface
 
   std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
   std::unique_ptr<PullOverPlannerBase> freespace_planner_;
+  std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;
   std::shared_ptr<GoalSearcherBase> goal_searcher_;
   PullOverPath shift_parking_path_;
   vehicle_info_util::VehicleInfo vehicle_info_;
@@ -157,7 +159,7 @@ class PullOverModule : public SceneModuleInterface
   double approximate_pull_over_distance_{20.0};
 
   bool left_side_parking_{true};
-  bool enable_goal_search_{false};
+  bool allow_goal_modification_{false};
 
   bool incrementPathIndex();
   PathWithLaneId getCurrentPath() const;
@@ -195,6 +197,9 @@ class PullOverModule : public SceneModuleInterface
   bool returnToLaneParking();
   bool isStuck();
 
+  BehaviorModuleOutput planWithGoalModification();
+  BehaviorModuleOutput planWaitingApprovalWithGoalModification();
+
   // timer for generating pull over path candidates
   void onTimer();
   rclcpp::TimerBase::SharedPtr lane_parking_timer_;

planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp

@@ -0,0 +1,42 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__DEFAULT_FIXED_GOAL_PLANNER_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__DEFAULT_FIXED_GOAL_PLANNER_HPP_
+
+#include "behavior_path_planner/utils/pull_over/fixed_goal_planner_base.hpp"
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+class DefaultFixedGoalPlanner : public FixedGoalPlannerBase
+{
+public:
+  DefaultFixedGoalPlanner() = default;
+  BehaviorModuleOutput plan(const std::shared_ptr<const PlannerData> & planner_data) const override;
+
+protected:
+  boost::optional<BehaviorModuleOutput> getReferencePath(
+    const std::shared_ptr<const PlannerData> & planner_data) const;
+  PathWithLaneId modifyPathForSmoothGoalConnection(
+    const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data) const;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__DEFAULT_FIXED_GOAL_PLANNER_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/pull_over/fixed_goal_planner_base.hpp

@@ -0,0 +1,45 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FIXED_GOAL_PLANNER_BASE_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FIXED_GOAL_PLANNER_BASE_HPP_
+
+#include "behavior_path_planner/data_manager.hpp"
+#include "behavior_path_planner/parameters.hpp"
+
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <memory>
+#include <vector>
+
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using geometry_msgs::msg::Pose;
+using tier4_autoware_utils::LinearRing2d;
+
+namespace behavior_path_planner
+{
+
+class FixedGoalPlannerBase
+{
+public:
+  FixedGoalPlannerBase() = default;
+  virtual ~FixedGoalPlannerBase() = default;
+
+  virtual BehaviorModuleOutput plan(
+    const std::shared_ptr<const PlannerData> & planner_data) const = 0;
+};
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__PULL_OVER__FIXED_GOAL_PLANNER_BASE_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1346,39 +1346,12 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
 #else
   const auto module_status_ptr_vec = planner_manager->getSceneModuleStatus();
 #endif
-  if (skipSmoothGoalConnection(module_status_ptr_vec)) {
-    connected_path = *path;
-  } else {
-    connected_path = modifyPathForSmoothGoalConnection(*path, planner_data);
-  }
 
   const auto resampled_path = utils::resamplePathWithSpline(
-    connected_path, planner_data->parameters.output_path_interval,
-    keepInputPoints(module_status_ptr_vec));
+    *path, planner_data->parameters.output_path_interval, keepInputPoints(module_status_ptr_vec));
   return std::make_shared<PathWithLaneId>(resampled_path);
 }
 
-bool BehaviorPathPlannerNode::skipSmoothGoalConnection(
-  const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const
-{
-#ifdef USE_OLD_ARCHITECTURE
-  const auto target_module = "PullOver";
-#else
-  const auto target_module = "pull_over";
-#endif
-
-  const auto target_status = ModuleStatus::RUNNING;
-
-  for (auto & status : statuses) {
-    if (status->is_waiting_approval || status->status == target_status) {
-      if (target_module == status->module_name) {
-        return true;
-      }
-    }
-  }
-  return false;
-}
-
 // This is a temporary process until motion planning can take the terminal pose into account
 bool BehaviorPathPlannerNode::keepInputPoints(
   const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -72,6 +72,9 @@ PullOverModule::PullOverModule(
 
   left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE;
 
+  // planner when goal modification is not allowed
+  fixed_goal_planner_ = std::make_unique<DefaultFixedGoalPlanner>();
+
   // set enabled planner
   if (parameters_->enable_shift_parking) {
     pull_over_planners_.push_back(std::make_shared<ShiftPullOver>(
@@ -289,7 +292,7 @@ void PullOverModule::processOnEntry()
   // todo: remove `checkOriginalGoalIsInShoulder()`
   // the function here is temporary condition for backward compatibility.
   // if the goal is in shoulder, allow goal_modification
-  enable_goal_search_ =
+  allow_goal_modification_ =
     route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder();
 
   // initialize when receiving new route
@@ -301,7 +304,7 @@ void PullOverModule::processOnEntry()
     // calculate goal candidates
     const Pose goal_pose = route_handler->getGoalPose();
     refined_goal_pose_ = calcRefinedGoal(goal_pose);
-    if (enable_goal_search_) {
+    if (allow_goal_modification_) {
       goal_searcher_->setPlannerData(planner_data_);
       goal_candidates_ = goal_searcher_->search(refined_goal_pose_);
     } else {
@@ -336,15 +339,24 @@ bool PullOverModule::isExecutionRequested() const
   lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &current_lane);
   const double self_to_goal_arc_length =
     utils::getSignedDistance(current_pose, goal_pose, current_lanes);
-  if (self_to_goal_arc_length > calcModuleRequestLength()) {
+  const bool allow_goal_modification =
+    route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder();
+  const double request_length =
+    allow_goal_modification ? calcModuleRequestLength() : parameters_->minimum_request_length;
+  if (self_to_goal_arc_length > request_length) {
     return false;
   }
 
-  // check if target lane is shoulder lane and goal_pose is in the lane
-  const bool goal_is_in_shoulder = checkOriginalGoalIsInShoulder();
-  // if allow_goal_modification is set false and goal is in road lane, do not execute pull over
-  if (!route_handler->isAllowedGoalModification() && !goal_is_in_shoulder) {
-    return false;
+  // if goal modification is not allowed and goal_pose in current_lanes,
+  // plan path to the original fixed goal
+  if (!allow_goal_modification) {
+    if (std::any_of(
+          current_lanes.begin(), current_lanes.end(),
+          [&](const lanelet::ConstLanelet & current_lane) {
+            return lanelet::utils::isInLanelet(goal_pose, current_lane);
+          })) {
+      return true;
+    }
   }
 
   // if (A) or (B) is met execute pull over
@@ -490,6 +502,15 @@ bool PullOverModule::returnToLaneParking()
 }
 
 BehaviorModuleOutput PullOverModule::plan()
+{
+  if (allow_goal_modification_) {
+    return planWithGoalModification();
+  } else {
+    return fixed_goal_planner_->plan(planner_data_);
+  }
+}
+
+BehaviorModuleOutput PullOverModule::planWithGoalModification()
 {
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const double current_vel = planner_data_->self_odometry->twist.twist.linear.x;
@@ -686,7 +707,7 @@ BehaviorModuleOutput PullOverModule::plan()
 
   // Publish the modified goal only when it is updated
   if (
-    enable_goal_search_ && status_.is_safe && modified_goal_pose_ &&
+    allow_goal_modification_ && status_.is_safe && modified_goal_pose_ &&
     (!prev_goal_id_ || *prev_goal_id_ != modified_goal_pose_->id)) {
     PoseWithUuidStamped modified_goal{};
     modified_goal.uuid = route_handler->getRouteUuid();
@@ -731,6 +752,15 @@ CandidateOutput PullOverModule::planCandidate() const
 }
 
 BehaviorModuleOutput PullOverModule::planWaitingApproval()
+{
+  if (allow_goal_modification_) {
+    return planWaitingApprovalWithGoalModification();
+  } else {
+    return fixed_goal_planner_->plan(planner_data_);
+  }
+}
+
+BehaviorModuleOutput PullOverModule::planWaitingApprovalWithGoalModification()
 {
   updateOccupancyGrid();
   BehaviorModuleOutput out;
@@ -1215,7 +1245,7 @@ bool PullOverModule::isCrossingPossible(
       return true;
     }
 
-    // Travesing is not allowed between road lanes
+    // Traversing is not allowed between road lanes
     if (!is_shoulder_lane) {
       continue;
     }
@@ -1273,7 +1303,7 @@ void PullOverModule::setDebugData()
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
 
-  if (enable_goal_search_) {
+  if (allow_goal_modification_) {
     // Visualize pull over areas
     const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999)  // yellow
                                                 : createMarkerColor(0.0, 1.0, 0.0, 0.999);  // green

planning/behavior_path_planner/src/utils/pull_over/default_fixed_goal_planner.cpp

@@ -0,0 +1,84 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "behavior_path_planner/utils/pull_over/default_fixed_goal_planner.hpp"
+
+#include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/pull_over/util.hpp"
+
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace behavior_path_planner
+{
+
+BehaviorModuleOutput DefaultFixedGoalPlanner::plan(
+  const std::shared_ptr<const PlannerData> & planner_data) const
+{
+  auto output = getReferencePath(planner_data);
+  if (!output) {
+    return {};
+  }
+  const PathWithLaneId smoothed_path =
+    modifyPathForSmoothGoalConnection(*(output->path), planner_data);
+
+  output->path = std::make_shared<PathWithLaneId>(smoothed_path);
+  output->reference_path = std::make_shared<PathWithLaneId>(smoothed_path);
+  return *output;
+}
+
+boost::optional<BehaviorModuleOutput> DefaultFixedGoalPlanner::getReferencePath(
+  const std::shared_ptr<const PlannerData> & planner_data) const
+{
+  const auto & route_handler = planner_data->route_handler;
+  const auto current_pose = planner_data->self_odometry->pose.pose;
+  const double dist_threshold = planner_data->parameters.ego_nearest_dist_threshold;
+  const double yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold;
+
+  lanelet::ConstLanelet current_lane{};
+  if (!route_handler->getClosestLaneletWithConstrainsWithinRoute(
+        current_pose, &current_lane, dist_threshold, yaw_threshold)) {
+    return {};  // TODO(Horibe)
+  }
+
+  return utils::getReferencePath(current_lane, planner_data);
+}
+
+PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection(
+  const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data) const
+{
+  const auto goal = planner_data->route_handler->getGoalPose();
+  const auto goal_lane_id = planner_data->route_handler->getGoalLaneId();
+
+  Pose refined_goal{};
+  {
+    lanelet::ConstLanelet goal_lanelet;
+    if (planner_data->route_handler->getGoalLanelet(&goal_lanelet)) {
+      refined_goal = utils::refineGoal(goal, goal_lanelet);
+    } else {
+      refined_goal = goal;
+    }
+  }
+
+  auto refined_path = utils::refinePathForGoal(
+    planner_data->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal,
+    goal_lane_id);
+
+  return refined_path;
+}
+
+}  // namespace behavior_path_planner