Revert "Revert "fix(behavior_path): only apply spline interpolation f…

…or its output, not for turn_signal processing (#909)"" This reverts commit e6dd540.
tier4 · Oct 13, 2023 · 2f2ded4 · 2f2ded4
1 parent 046c467
commit 2f2ded4
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 5 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -50,6 +50,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -136,7 +137,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief extract path from behavior tree output
    */
-  PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out);
+  std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> getPath(
+    const BehaviorModuleOutput & bt_out);
 
   /**
    * @brief extract path candidate from behavior tree output

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -478,7 +478,10 @@ void BehaviorPathPlannerNode::run()
   const auto output = bt_manager_->run(planner_data_);
 
   // path handling
-  const auto path = getPath(output);
+  const auto paths = getPath(output);
+  const auto path = paths.first;
+  const auto original_path = paths.second;
+
   const auto path_candidate = getPathCandidate(output);
   planner_data_->prev_output_path = path;
 
@@ -504,7 +507,7 @@ void BehaviorPathPlannerNode::run()
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
+        *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
         output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
       hazard_signal.command = HazardLightsCommand::DISABLE;
     }
@@ -527,7 +530,9 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
-PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output)
+// output: spline interpolated path, original path
+std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> BehaviorPathPlannerNode::getPath(
+  const BehaviorModuleOutput & bt_output)
 {
   // TODO(Horibe) do some error handling when path is not available.
 
@@ -539,7 +544,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleO
 
   const auto resampled_path =
     util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval);
-  return std::make_shared<PathWithLaneId>(resampled_path);
+  return std::make_pair(std::make_shared<PathWithLaneId>(resampled_path), path);
 }
 
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(