fix(lane_departure_checker): use common nearest ego search (autowaref…

…oundation#1611)

* fix(lane_departure_checker): use common nearest ego search

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix typo

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -58,7 +58,9 @@ struct Param
   double max_lateral_deviation;
   double max_longitudinal_deviation;
   double max_yaw_deviation_deg;
-  double delta_yaw_threshold_for_closest_point;
+  // nearest search to ego
+  double ego_nearest_dist_threshold;
+  double ego_nearest_yaw_threshold;
 };
 
 struct Input
@@ -111,7 +113,7 @@ class LaneDepartureChecker
 
   static PoseDeviation calcTrajectoryDeviation(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose,
-    const double yaw_threshold);
+    const double dist_threshold, const double yaw_threshold);
 
   //! This function assumes the input trajectory is sampled dense enough
   static TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double interval);

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -57,15 +57,6 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2
   return false;
 }
 
-size_t findNearestIndexWithSoftYawConstraints(
-  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double yaw_threshold)
-{
-  const auto nearest_idx_optional = motion_utils::findNearestIndex(
-    trajectory.points, pose, std::numeric_limits<double>::max(), yaw_threshold);
-  return nearest_idx_optional ? *nearest_idx_optional
-                              : motion_utils::findNearestIndex(trajectory.points, pose.position);
-}
-
 LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprints)
 {
   MultiPoint2d combined;
@@ -109,8 +100,8 @@ Output LaneDepartureChecker::update(const Input & input)
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 
   output.trajectory_deviation = calcTrajectoryDeviation(
-    *input.reference_trajectory, input.current_pose->pose,
-    param_.delta_yaw_threshold_for_closest_point);
+    *input.reference_trajectory, input.current_pose->pose, param_.ego_nearest_dist_threshold,
+    param_.ego_nearest_yaw_threshold);
   output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true);
 
   {
@@ -153,9 +144,11 @@ bool LaneDepartureChecker::checkPathWillLeaveLane(
 }
 
 PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
-  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double yaw_threshold)
+  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double dist_threshold,
+  const double yaw_threshold)
 {
-  const auto nearest_idx = findNearestIndexWithSoftYawConstraints(trajectory, pose, yaw_threshold);
+  const auto nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    trajectory.points, pose, dist_threshold, yaw_threshold);
   return tier4_autoware_utils::calcPoseDeviation(trajectory.points.at(nearest_idx).pose, pose);
 }
 

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -142,8 +142,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   param_.max_lateral_deviation = declare_parameter("max_lateral_deviation", 1.0);
   param_.max_longitudinal_deviation = declare_parameter("max_longitudinal_deviation", 1.0);
   param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg", 30.0);
-  param_.delta_yaw_threshold_for_closest_point =
-    declare_parameter("delta_yaw_threshold_for_closest_point", 90.0);
+  param_.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
+  param_.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
   // Parameter Callback
   set_param_res_ =