From ac96ce5973284b9bba8d130c26da93b71e804369 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 3 Jun 2022 22:39:24 +0900 Subject: [PATCH] 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 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 --- .../config/lane_departure_checker.param.yaml | 1 + .../lane_departure_checker.hpp | 5 ++- .../lane_departure_checker.cpp | 31 +++++++------------ .../lane_departure_checker_node.cpp | 2 ++ 4 files changed, 19 insertions(+), 20 deletions(-) diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index f4925fdf17ec4..092a6765aa948 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/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 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 0ee843bce22b7..3c16b4db06dec 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -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_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); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 867de951accab..ad3fca6f8504e 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/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 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(std::distance(distances.cbegin(), min_itr)); - - return min_idx; + const auto nearest_idx_optional = tier4_autoware_utils::findNearestIndex( + trajectory.points, pose, std::numeric_limits::max(), yaw_threshold); + return nearest_idx_optional + ? *nearest_idx_optional + : tier4_autoware_utils::findNearestIndex(trajectory.points, pose.position); } LinearRing2d createHullFromFootprints(const std::vector & footprints) @@ -116,8 +108,9 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch 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); } diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 3509a5c83294f..315458aaf04a8 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/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_ =