feat(lane_departure_checker): add angle limitation to lane_departure_…

…chacker (#1034)

* feat(lane_departure_checker): add angle limitation to lane_departure_chacker

Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com>

* ci(pre-commit): autofix

Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com>

* change angle unit from deg to rad

Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com>

* fix: stop_line visualization (#1029)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
Signed-off-by: kyoichi sugahara <81.s.kyo.19@gmail.com>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>

control/lane_departure_checker/config/lane_departure_checker.param.yaml

@@ -12,3 +12,4 @@
     max_lateral_deviation: 2.0
     max_longitudinal_deviation: 2.0
     max_yaw_deviation_deg: 60.0
+    delta_yaw_threshold_for_closest_point: 1.570 #M_PI/2.0, delta yaw thres for closest point

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -18,6 +18,7 @@
 #include <rosidl_runtime_cpp/message_initialization.hpp>
 #include <tier4_autoware_utils/geometry/boost_geometry.hpp>
 #include <tier4_autoware_utils/geometry/pose_deviation.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
@@ -55,6 +56,7 @@ struct Param
   double max_lateral_deviation;
   double max_longitudinal_deviation;
   double max_yaw_deviation_deg;
+  double delta_yaw_threshold_for_closest_point;
 };
 
 struct Input
@@ -98,7 +100,8 @@ class LaneDepartureChecker
   std::shared_ptr<vehicle_info_util::VehicleInfo> vehicle_info_ptr_;
 
   static PoseDeviation calcTrajectoryDeviation(
-    const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose);
+    const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose,
+    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

@@ -56,22 +56,14 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2
   return false;
 }
 
-size_t findNearestIndex(const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose)
+size_t findNearestIndexWithSoftYawConstraints(
+  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double yaw_threshold)
 {
-  std::vector<double> distances;
-  distances.reserve(trajectory.points.size());
-  std::transform(
-    trajectory.points.cbegin(), trajectory.points.cend(), std::back_inserter(distances),
-    [&](const TrajectoryPoint & p) {
-      const auto p1 = tier4_autoware_utils::fromMsg(p.pose.position).to_2d();
-      const auto p2 = tier4_autoware_utils::fromMsg(pose.position).to_2d();
-      return boost::geometry::distance(p1, p2);
-    });
-
-  const auto min_itr = std::min_element(distances.cbegin(), distances.cend());
-  const auto min_idx = static_cast<size_t>(std::distance(distances.cbegin(), min_itr));
-
-  return min_idx;
+  const auto nearest_idx_optional = tier4_autoware_utils::findNearestIndex(
+    trajectory.points, pose, std::numeric_limits<double>::max(), yaw_threshold);
+  return nearest_idx_optional
+           ? *nearest_idx_optional
+           : tier4_autoware_utils::findNearestIndex(trajectory.points, pose.position);
 }
 
 LinearRing2d createHullFromFootprints(const std::vector<LinearRing2d> & footprints)
@@ -116,8 +108,9 @@ 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);
+  output.trajectory_deviation = calcTrajectoryDeviation(
+    *input.reference_trajectory, input.current_pose->pose,
+    param_.delta_yaw_threshold_for_closest_point);
   output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true);
 
   {
@@ -152,9 +145,9 @@ Output LaneDepartureChecker::update(const Input & input)
 }
 
 PoseDeviation LaneDepartureChecker::calcTrajectoryDeviation(
-  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose)
+  const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double yaw_threshold)
 {
-  const auto nearest_idx = findNearestIndex(trajectory, pose);
+  const auto nearest_idx = findNearestIndexWithSoftYawConstraints(trajectory, pose, 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

@@ -134,6 +134,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);
 
   // Parameter Callback
   set_param_res_ =