feat(behavior_path_planner): keep original points in resampling of pull over (#2478)

* feat(behavior_path_planner): keep original points in resampling

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

* Update planning/behavior_path_planner/src/path_utilities.cpp

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

* change motion_utils::overlap_threshold to 0.2 and use it

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

* Revert "change motion_utils::overlap_threshold to 0.2 and use it"

This reverts commit edfb2d5.

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

* keep input points only in pull over

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

* Add comment of temporary processe

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

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

Author: kosuke55

Diff for: planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

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

Diff for: planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp

@@ -41,7 +41,8 @@ std::vector<double> calcPathArcLengthArray(
   const PathWithLaneId & path, const size_t start = 0,
   const size_t end = std::numeric_limits<size_t>::max(), const double offset = 0.0);
 
-PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval);
+PathWithLaneId resamplePathWithSpline(
+  const PathWithLaneId & path, const double interval, const bool keep_input_points = false);
 
 Path toPath(const PathWithLaneId & input);
 

Diff for: planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -817,8 +817,9 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
     connected_path = modifyPathForSmoothGoalConnection(*path);
   }
 
-  const auto resampled_path =
-    util::resamplePathWithSpline(connected_path, planner_data_->parameters.path_interval);
+  const auto resampled_path = util::resamplePathWithSpline(
+    connected_path, planner_data_->parameters.path_interval,
+    keepInputPoints(module_status_ptr_vec));
   return std::make_shared<PathWithLaneId>(resampled_path);
 }
 
@@ -837,6 +838,22 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection(
   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
+{
+  const auto target_module = "PullOver";
+
+  for (auto & status : statuses) {
+    if (status->is_waiting_approval || status->status == BT::NodeStatus::RUNNING) {
+      if (target_module == status->module_name) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
 void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(mutex_pd_);

Diff for: planning/behavior_path_planner/src/path_utilities.cpp

@@ -55,7 +55,8 @@ std::vector<double> calcPathArcLengthArray(
 /**
  * @brief resamplePathWithSpline
  */
-PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval)
+PathWithLaneId resamplePathWithSpline(
+  const PathWithLaneId & path, const double interval, const bool keep_input_points)
 {
   if (path.points.size() < 2) {
     return path;
@@ -66,7 +67,7 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv
     transformed_path.at(i) = path.points.at(i).point;
   }
 
-  constexpr double epsilon = 0.01;
+  constexpr double epsilon = 0.2;
   const auto has_almost_same_value = [&](const auto & vec, const auto x) {
     if (vec.empty()) return false;
     const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; };
@@ -79,6 +80,9 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv
   for (size_t i = 0; i < path.points.size(); ++i) {
     const double s = motion_utils::calcSignedArcLength(transformed_path, 0, i);
     for (const auto & lane_id : path.points.at(i).lane_ids) {
+      if (keep_input_points && !has_almost_same_value(s_in, s)) {
+        s_in.push_back(s);
+      }
       if (
         std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) !=
         unique_lane_ids.end()) {