Fix/behavior path nearest ego3

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -120,8 +120,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   PathWithLaneId modifyPathForSmoothGoalConnection(
     const PathWithLaneId & path) const;  // (TODO) move to util
 
-  void clipPathLength(PathWithLaneId & path) const;  // (TODO) move to util
-
   /**
    * @brief Execute behavior tree and publish planned data.
    */
@@ -148,6 +146,24 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // debug
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_drivable_area_lanelets_publisher_;
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
+
+  template <class T>
+  size_t findEgoIndex(const std::vector<T> & points) const
+  {
+    const auto & p = planner_data_;
+    return motion_utils::findFirstNearestIndexWithSoftConstraints(
+      points, p->self_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_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/parameters.hpp

@@ -41,6 +41,9 @@ struct BehaviorPathPlannerParameters
 
   double path_interval;
 
+  double ego_nearest_dist_threshold;
+  double ego_nearest_yaw_threshold;
+
   // vehicle info
   vehicle_info_util::VehicleInfo vehicle_info;
   double wheel_base;

planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp

@@ -41,17 +41,18 @@ std::vector<double> calcPathArcLengthArray(
   const PathWithLaneId & path, size_t start = 0, size_t end = std::numeric_limits<size_t>::max(),
   double offset = 0.0);
 
-double calcPathArcLength(
-  const PathWithLaneId & path, size_t start = 0, size_t end = std::numeric_limits<size_t>::max());
-
 PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval);
 
 Path toPath(const PathWithLaneId & input);
 
-size_t getIdxByArclength(const PathWithLaneId & path, const Pose & origin, const double signed_arc);
+size_t getIdxByArclength(
+  const PathWithLaneId & path, const size_t target_idx, const double signed_arc);
+
+void clipPathLength(
+  PathWithLaneId & path, const size_t target_idx, const double forward, const double backward);
 
 void clipPathLength(
-  PathWithLaneId & path, const Pose base_pose, const double forward, const double backward);
+  PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params);
 
 std::pair<TurnIndicatorsCommand, double> getPathTurnSignal(
   const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path,

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

@@ -260,8 +260,6 @@ class AvoidanceModule : public SceneModuleInterface
   PathWithLaneId calcCenterLinePath(
     const std::shared_ptr<const PlannerData> & planner_data, const PoseStamped & pose) const;
 
-  void clipPathLength(PathWithLaneId & path) const;
-
   // TODO(Horibe): think later.
   // for unique ID
   mutable uint64_t original_unique_id = 0;  // TODO(Horibe) remove mutable

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -70,6 +70,7 @@ bool isLaneChangePathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
+  const size_t current_seg_idx, 
   const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters,
   const LaneChangeParameters & lane_change_parameters, const bool use_buffer = true,
   const double acceleration = 0.0);

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

@@ -65,7 +65,7 @@ bool isPullOverPathSafe(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const Twist & current_twist, const double vehicle_width,
+  const size_t current_seg_idx, const Twist & current_twist, const double vehicle_width,
   const behavior_path_planner::PullOverParameters & ros_parameters, const bool use_buffer = true,
   const double acceleration = 0.0);
 bool hasEnoughDistance(

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

@@ -36,6 +36,7 @@
 #include <random>
 #include <string>
 #include <utility>
+#include <vector>
 
 namespace behavior_path_planner
 {
@@ -281,6 +282,24 @@ 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_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_pose->pose, p->parameters.ego_nearest_dist_threshold,
+      p->parameters.ego_nearest_yaw_threshold);
+  }
+
 public:
   BT::NodeStatus current_state_;
 };

planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp

@@ -96,7 +96,7 @@ class PathShifter
    * @details The previous offset information is stored in the base_offset_.
    *          This should be called after generate().
    */
-  void removeBehindShiftPointAndSetBaseOffset(const Point & base);
+  void removeBehindShiftPointAndSetBaseOffset(const Pose & pose, const size_t nearest_idx);
 
   ////////////////////////////////////////
   // Utility Functions
@@ -169,13 +169,6 @@ class PathShifter
    */
   std::vector<double> calcLateralJerk();
 
-  /**
-   * @brief  Calculate shift point from path arclength for start and end point.
-   */
-  static bool calcShiftPointFromArcLength(
-    const PathWithLaneId & path, const Point & origin, double dist_to_start, double dist_to_end,
-    double shift_length, ShiftPoint * shift_point);
-
 private:
   // The reference path along which the shift will be performed.
   PathWithLaneId reference_path_;

planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp

@@ -35,8 +35,9 @@ class TurnSignalDecider
 {
 public:
   TurnIndicatorsCommand getTurnSignal(
-    const PathWithLaneId & path, const Pose & current_pose, const RouteHandler & route_handler,
-    const TurnIndicatorsCommand & turn_signal_plan, const double plan_distance) const;
+    const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
+    const RouteHandler & route_handler, const TurnIndicatorsCommand & turn_signal_plan,
+    const double plan_distance) const;
 
   void setParameters(const double base_link2front, const double intersection_search_distance)
   {
@@ -46,7 +47,7 @@ class TurnSignalDecider
 
 private:
   std::pair<TurnIndicatorsCommand, double> getIntersectionTurnSignal(
-    const PathWithLaneId & path, const Pose & current_pose,
+    const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
     const RouteHandler & route_handler) const;
 
   rclcpp::Logger logger_{

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -97,22 +97,38 @@ void getProjectedDistancePointFromPolygons(
 
 Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id);
 
-std::vector<Point> convertToPointArray(const PathWithLaneId & path);
+std::vector<Pose> convertToPoseArray(const PathWithLaneId & path);
 
 std::vector<Point> convertToGeometryPointArray(const PathWithLaneId & path);
 
 PoseArray convertToGeometryPoseArray(const PathWithLaneId & path);
 
 PredictedPath convertToPredictedPath(
-  const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose,
-  const double duration, const double resolution, const double acceleration,
-  double min_speed = 1.0);
+  const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose,
+  const double nearest_seg_idx, const double duration, const double resolution,
+  const double acceleration,
+  const double min_speed = 1.0);
 
+template <class T>
 FrenetCoordinate3d convertToFrenetCoordinate3d(
-  const std::vector<Point> & linestring, const Point & search_point_geom);
+  const std::vector<T> & pose_array, const Point & search_point_geom, const size_t seg_idx)
+{
+  FrenetCoordinate3d frenet_coordinate;
 
-FrenetCoordinate3d convertToFrenetCoordinate3d(
-  const PathWithLaneId & path, const Point & search_point_geom);
+  const double longitudinal_length =
+    motion_utils::calcLongitudinalOffsetToSegment(pose_array, seg_idx, search_point_geom);
+  frenet_coordinate.length =
+    motion_utils::calcSignedArcLength(pose_array, 0, seg_idx) + longitudinal_length;
+  frenet_coordinate.distance = motion_utils::calcLateralOffset(pose_array, search_point_geom);
+
+  return frenet_coordinate;
+}
+
+inline FrenetCoordinate3d convertToFrenetCoordinate3d(
+  const PathWithLaneId & path, const Point & search_point_geom, const size_t seg_idx)
+{
+  return convertToFrenetCoordinate3d(path.points, search_point_geom, seg_idx);
+}
 
 std::vector<uint64_t> getIds(const lanelet::ConstLanelets & lanelets);
 
@@ -140,7 +156,42 @@ double getArcLengthToTargetLanelet(
 
 Pose lerpByPose(const Pose & p1, const Pose & p2, const double t);
 
-Point lerpByLength(const std::vector<Point> & array, const double length);
+inline Point lerpByPoint(const Point & p1, const Point & p2, const double t)
+{
+  tf2::Vector3 v1, v2;
+  v1.setValue(p1.x, p1.y, p1.z);
+  v2.setValue(p2.x, p2.y, p2.z);
+
+  const auto lerped_point = v1.lerp(v2, t);
+
+  Point point;
+  point.x = lerped_point.x();
+  point.y = lerped_point.y();
+  point.z = lerped_point.z();
+  return point;
+}
+
+template <class T>
+Point lerpByLength(const std::vector<T> & point_array, const double length)
+{
+  Point lerped_point;
+  if (point_array.empty()) {
+    return lerped_point;
+  }
+  Point prev_geom_pt = tier4_autoware_utils::getPoint(point_array.front());
+  double accumulated_length = 0;
+  for (const auto & pt : point_array) {
+    const auto & geom_pt = tier4_autoware_utils::getPoint(pt);
+    const double distance = tier4_autoware_utils::calcDistance3d(prev_geom_pt, geom_pt);
+    if (accumulated_length + distance > length) {
+      return lerpByPoint(prev_geom_pt, geom_pt, (length - accumulated_length) / distance);
+    }
+    accumulated_length += distance;
+    prev_geom_pt = geom_pt;
+  }
+
+  return tier4_autoware_utils::getPoint(point_array.back());
+}
 
 bool lerpByTimeStamp(const PredictedPath & path, const double t, Pose * lerped_pt);
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -203,6 +203,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   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);
+  p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
+  p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
   p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold", 3.0);
   p.longitudinal_distance_min_threshold =
@@ -564,7 +566,9 @@ void BehaviorPathPlannerNode::run()
   } else {
     clipped_path = modifyPathForSmoothGoalConnection(*path);
   }
-  clipPathLength(clipped_path);
+  const size_t target_idx = findEgoIndex(clipped_path.points);
+  util::clipPathLength(clipped_path, target_idx, planner_data_->parameters);
+
   if (!clipped_path.points.empty()) {
     path_publisher_->publish(clipped_path);
   } else {
@@ -582,8 +586,9 @@ void BehaviorPathPlannerNode::run()
       turn_signal.command = TurnIndicatorsCommand::DISABLE;
       hazard_signal.command = output.turn_signal_info.hazard_signal.command;
     } else {
+      const size_t ego_seg_idx = findEgoSegmentIndex(path->points);
       turn_signal = turn_signal_decider_.getTurnSignal(
-        *path, planner_data->self_pose->pose, *(planner_data->route_handler),
+        *path, planner_data->self_pose->pose, ego_seg_idx, *(planner_data->route_handler),
         output.turn_signal_info.turn_signal, output.turn_signal_info.signal_distance);
       hazard_signal.command = HazardLightsCommand::DISABLE;
     }
@@ -705,15 +710,6 @@ void BehaviorPathPlannerNode::onRoute(const HADMapRoute::ConstSharedPtr msg)
   }
 }
 
-void BehaviorPathPlannerNode::clipPathLength(PathWithLaneId & path) const
-{
-  const auto ego_pose = planner_data_->self_pose->pose;
-  const double forward = planner_data_->parameters.forward_path_length;
-  const double backward = planner_data_->parameters.backward_path_length;
-
-  util::clipPathLength(path, ego_pose, forward, backward);
-}
-
 PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
   const PathWithLaneId & path) const
 {