revert: #929,#909,#782

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml

@@ -14,4 +14,3 @@
     drivable_area_margin: 6.0
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
-    path_interval: 2.0

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -17,6 +17,4 @@
     refine_goal_search_radius_range: 7.5
     intersection_search_distance: 30.0
 
-    path_interval: 2.0
-
     visualize_drivable_area_for_shared_linestrings_lanelet: true

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -50,7 +50,6 @@
 
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 namespace behavior_path_planner
@@ -137,8 +136,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   /**
    * @brief extract path from behavior tree output
    */
-  std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> getPath(
-    const BehaviorModuleOutput & bt_out);
+  PathWithLaneId::SharedPtr getPath(const BehaviorModuleOutput & bt_out);
 
   /**
    * @brief extract path candidate from behavior tree output

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -37,8 +37,6 @@ struct BehaviorPathPlannerParameters
   double turn_light_on_threshold_dis_long;
   double turn_light_on_threshold_time;
 
-  double path_interval;
-
   // vehicle info
   double wheel_base;
   double front_overhang;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -165,7 +165,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.turn_light_on_threshold_dis_lat = declare_parameter("turn_light_on_threshold_dis_lat", 0.3);
   p.turn_light_on_threshold_dis_long = declare_parameter("turn_light_on_threshold_dis_long", 10.0);
   p.turn_light_on_threshold_time = declare_parameter("turn_light_on_threshold_time", 3.0);
-  p.path_interval = declare_parameter<double>("path_interval");
   p.visualize_drivable_area_for_shared_linestrings_lanelet =
     declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true);
 
@@ -478,10 +477,7 @@ void BehaviorPathPlannerNode::run()
   const auto output = bt_manager_->run(planner_data_);
 
   // path handling
-  const auto paths = getPath(output);
-  const auto path = paths.first;
-  const auto original_path = paths.second;
-
+  const auto path = getPath(output);
   const auto path_candidate = getPathCandidate(output);
   planner_data_->prev_output_path = path;
 
@@ -507,7 +503,7 @@ void BehaviorPathPlannerNode::run()
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *original_path, planner_data_->self_pose->pose, *(planner_data_->route_handler),
+        *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;
     }
@@ -530,9 +526,7 @@ void BehaviorPathPlannerNode::run()
   RCLCPP_DEBUG(get_logger(), "----- behavior path planner end -----\n\n");
 }
 
-// output: spline interpolated path, original path
-std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> BehaviorPathPlannerNode::getPath(
-  const BehaviorModuleOutput & bt_output)
+PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(const BehaviorModuleOutput & bt_output)
 {
   // TODO(Horibe) do some error handling when path is not available.
 
@@ -541,10 +535,7 @@ std::pair<PathWithLaneId::SharedPtr, PathWithLaneId::SharedPtr> BehaviorPathPlan
   path->header.stamp = this->now();
   RCLCPP_DEBUG(
     get_logger(), "BehaviorTreeManager: output is %s.", bt_output.path ? "FOUND" : "NOT FOUND");
-
-  const auto resampled_path =
-    util::resamplePathWithSpline(*path, planner_data_->parameters.path_interval);
-  return std::make_pair(std::make_shared<PathWithLaneId>(resampled_path), path);
+  return path;
 }
 
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(

planning/behavior_path_planner/src/path_utilities.cpp

@@ -90,18 +90,7 @@ double calcPathArcLength(const PathWithLaneId & path, size_t start, size_t end)
 PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval)
 {
   const auto base_points = calcPathArcLengthArray(path);
-  auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval);
-
-  constexpr double epsilon = 0.01;
-  for (const auto & b : base_points) {
-    const auto is_almost_same_value = std::find_if(
-      sampling_points.begin(), sampling_points.end(),
-      [&](const auto v) { return std::abs(v - b) < epsilon; });
-    if (is_almost_same_value == sampling_points.end()) {
-      sampling_points.push_back(b);
-    }
-  }
-  std::sort(sampling_points.begin(), sampling_points.end());
+  const auto sampling_points = tier4_autoware_utils::arange(0.0, base_points.back(), interval);
 
   if (base_points.empty() || sampling_points.empty()) {
     return path;
@@ -133,25 +122,22 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv
 
   // For LaneIds, Type, Twist
   //
-  // ------|----|----|----|----|----|----|----|--------> resampled
-  //      [0]  [1]  [2]  [3]  [4]  [5]  [6]  [7]
+  // ------|----|----|----|----|----|----|-------> resampled
+  //      [0]  [1]  [2]  [3]  [4]  [5]  [6]
   //
-  // ------|---------------|----------|-------|---> base
-  //      [0]             [1]        [2]     [3]
+  // --------|---------------|----------|---------> base
+  //        [0]             [1]        [2]
   //
-  // resampled[0] = base[0]
-  // resampled[1~3] = base[1]
-  // resampled[4~5] = base[2]
-  // resampled[6~7] = base[3]
+  // resampled[0~3] = base[0]
+  // resampled[4~5] = base[1]
+  // resampled[6] = base[2]
   //
   size_t base_idx{0};
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
-    while (base_idx < base_points.size() - 1 &&
-           ((sampling_points.at(i) > base_points.at(base_idx)) ||
-            (sampling_points.at(i) - base_points.at(base_idx)) > 1e-3)) {
+    while (base_idx < base_points.size() - 1 && sampling_points.at(i) > base_points.at(base_idx)) {
       ++base_idx;
     }
-    size_t ref_idx = std::max(static_cast<int>(base_idx), 0);
+    size_t ref_idx = std::max(static_cast<int>(base_idx) - 1, 0);
     if (i == resampled_path.points.size() - 1) {
       ref_idx = base_points.size() - 1;  // for last point
     }

planning/behavior_velocity_planner/src/node.cpp

@@ -420,10 +420,10 @@ void BehaviorVelocityPlannerNode::onTrigger(
   const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path));
 
   // interpolation
-  // const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_);
+  const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_);
 
   // check stop point
-  auto output_path_msg = filterStopPathPoint(filtered_path);
+  auto output_path_msg = filterStopPathPoint(interpolated_path_msg);
   output_path_msg.header.frame_id = "map";
   output_path_msg.header.stamp = this->now();