fix(behavior_path_planner): move findEgoIndex & findEgoSegmentIndex t…

…o planner data (#2912)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -213,24 +213,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
    */
   Path convertToPath(
     const std::shared_ptr<PathWithLaneId> & path_candidate_ptr, const bool is_ready);
-
-  template <class T>
-  size_t findEgoIndex(const std::vector<T> & points) const
-  {
-    const auto & p = planner_data_;
-    return motion_utils::findFirstNearestIndexWithSoftConstraints(
-      points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold,
-      p->parameters.ego_nearest_yaw_threshold);
-  }
-
-  template <class T>
-  size_t findEgoSegmentIndex(const std::vector<T> & points) const
-  {
-    const auto & p = planner_data_;
-    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold,
-      p->parameters.ego_nearest_yaw_threshold);
-  }
 };
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -32,6 +32,7 @@
 
 #include <memory>
 #include <string>
+#include <vector>
 
 namespace behavior_path_planner
 {
@@ -74,6 +75,22 @@ struct PlannerData
   std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
   BehaviorPathPlannerParameters parameters{};
   drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
+
+  template <class T>
+  size_t findEgoIndex(const std::vector<T> & points) const
+  {
+    return motion_utils::findFirstNearestIndexWithSoftConstraints(
+      points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold,
+      parameters.ego_nearest_yaw_threshold);
+  }
+
+  template <class T>
+  size_t findEgoSegmentIndex(const std::vector<T> & points) const
+  {
+    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold,
+      parameters.ego_nearest_yaw_threshold);
+  }
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -339,7 +339,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   void postProcess(PathShifter & path_shifter) const
   {
-    const size_t nearest_idx = findEgoIndex(path_shifter.getReferencePath().points);
+    const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter.getReferencePath().points);
     path_shifter.removeBehindShiftLineAndSetBaseOffset(nearest_idx);
   }
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -306,24 +306,6 @@ class SceneModuleInterface
     return uuid;
   }
 
-  template <class T>
-  size_t findEgoIndex(const std::vector<T> & points) const
-  {
-    const auto & p = planner_data_;
-    return motion_utils::findFirstNearestIndexWithSoftConstraints(
-      points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold,
-      p->parameters.ego_nearest_yaw_threshold);
-  }
-
-  template <class T>
-  size_t findEgoSegmentIndex(const std::vector<T> & points) const
-  {
-    const auto & p = planner_data_;
-    return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      points, p->self_odometry->pose.pose, p->parameters.ego_nearest_dist_threshold,
-      p->parameters.ego_nearest_yaw_threshold);
-  }
-
 public:
   BT::NodeStatus current_state_;
 };

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -798,7 +798,7 @@ void BehaviorPathPlannerNode::run()
   // publish drivable bounds
   publish_bounds(*path);
 
-  const size_t target_idx = findEgoIndex(path->points);
+  const size_t target_idx = planner_data->findEgoIndex(path->points);
   util::clipPathLength(*path, target_idx, planner_data_->parameters);
 
   if (!path->points.empty()) {

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2182,7 +2182,7 @@ bool AvoidanceModule::isSafePath(
   auto path_with_current_velocity = shifted_path.path;
   path_with_current_velocity = util::resamplePathWithSpline(path_with_current_velocity, 0.5);
 
-  const size_t ego_idx = findEgoIndex(path_with_current_velocity.points);
+  const size_t ego_idx = planner_data_->findEgoIndex(path_with_current_velocity.points);
   util::clipPathLength(path_with_current_velocity, ego_idx, forward_check_distance, 0.0);
 
   constexpr double MIN_EGO_VEL_IN_PREDICTION = 1.38;  // 5km/h
@@ -2964,7 +2964,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   output.path = std::make_shared<PathWithLaneId>(avoidance_path.path);
 
-  const size_t ego_idx = findEgoIndex(output.path->points);
+  const size_t ego_idx = planner_data_->findEgoIndex(output.path->points);
   util::clipPathLength(*output.path, ego_idx, planner_data_->parameters);
 
   // Drivable area generation.
@@ -3009,7 +3009,7 @@ CandidateOutput AvoidanceModule::planCandidate() const
       "");
   }
 
-  const size_t ego_idx = findEgoIndex(shifted_path.path.points);
+  const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
   util::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters);
 
   output.path_candidate = shifted_path.path;

planning/behavior_path_planner/src/scene_module/lane_following/lane_following_module.cpp

@@ -133,7 +133,7 @@ PathWithLaneId LaneFollowingModule::getReferencePath() const
     p.forward_path_length, p);
 
   // clip backward length
-  const size_t current_seg_idx = findEgoSegmentIndex(reference_path.points);
+  const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(reference_path.points);
   util::clipPathLength(
     reference_path, current_seg_idx, p.forward_path_length, p.backward_path_length);
   const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler);

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

@@ -485,7 +485,7 @@ BehaviorModuleOutput PullOverModule::plan()
 
     // generate drivable area for each partial path
     for (auto & path : status_.pull_over_path.partial_paths) {
-      const size_t ego_idx = findEgoIndex(path.points);
+      const size_t ego_idx = planner_data_->findEgoIndex(path.points);
       util::clipPathLength(path, ego_idx, planner_data_->parameters);
       const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes);
       const auto expanded_lanes = util::expandLanelets(
@@ -661,7 +661,7 @@ PathWithLaneId PullOverModule::generateStopPath()
                     : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose);
 
   // if stop pose is closer than min_stop_distance, stop as soon as possible
-  const size_t ego_idx = findEgoIndex(reference_path.points);
+  const size_t ego_idx = planner_data_->findEgoIndex(reference_path.points);
   const size_t stop_idx = findFirstNearestSegmentIndexWithSoftConstraints(
     reference_path.points, stop_pose, common_parameters.ego_nearest_dist_threshold,
     common_parameters.ego_nearest_yaw_threshold);
@@ -722,7 +722,7 @@ PathWithLaneId PullOverModule::generateEmergencyStopPath()
   }
 
   // set deceleration velocity
-  const size_t ego_idx = findEgoIndex(stop_path.points);
+  const size_t ego_idx = planner_data_->findEgoIndex(stop_path.points);
   for (auto & point : stop_path.points) {
     auto & p = point.point;
     const size_t target_idx = findFirstNearestSegmentIndexWithSoftConstraints(

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -194,7 +194,7 @@ void SideShiftModule::updateData()
   current_lanelets_ = route_handler->getLaneletSequence(
     current_lane, reference_pose, p.backward_path_length, p.forward_path_length);
 
-  const size_t nearest_idx = findEgoIndex(path_shifter_.getReferencePath().points);
+  const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
   path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx);
 }
 
@@ -337,7 +337,7 @@ ShiftLine SideShiftModule::calcShiftLine() const
     return dist_to_end;
   }();
 
-  const size_t nearest_idx = findEgoIndex(reference_path_->points);
+  const size_t nearest_idx = planner_data_->findEgoIndex(reference_path_->points);
   ShiftLine shift_line;
   shift_line.end_shift_length = requested_lateral_offset_;
   shift_line.start_idx = util::getIdxByArclength(*reference_path_, nearest_idx, dist_to_start);