From 88f1dfd6220ffefba47c1d26a6bd6d8ff2788d2d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 27 Sep 2022 11:48:34 +0900 Subject: [PATCH] fix(behavior_path_planner): fix bug of turn signal with overlap lane (#1886) Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../motion_utils/trajectory/trajectory.hpp | 35 ++++++++++- .../test/src/trajectory/test_trajectory.cpp | 61 +++++++++++++++++++ .../behavior_path_planner/utilities.hpp | 3 +- 3 files changed, 95 insertions(+), 4 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 705542e769d81..5cc81aa04b750 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -347,7 +347,8 @@ boost::optional findNearestSegmentIndex( */ template double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) + const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false) { const auto overlap_removed_points = removeOverlapPoints(points, 0); @@ -371,8 +372,6 @@ double calcLateralOffset( return std::nan(""); } - const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); @@ -383,6 +382,36 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } +template +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) +{ + const auto overlap_removed_points = removeOverlapPoints(points, 0); + + if (throw_exception) { + validateNonEmpty(overlap_removed_points); + } else { + try { + validateNonEmpty(overlap_removed_points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return std::nan(""); + } + } + + if (overlap_removed_points.size() == 1) { + const std::runtime_error e("Same points are given."); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return std::nan(""); + } + + const size_t seg_idx = findNearestSegmentIndex(overlap_removed_points, p_target); + return calcLateralOffset(points, p_target, seg_idx, throw_exception); +} + /** * @brief calcSignedArcLength from index to index */ diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 12628934bc2bb..9bdcf1208540a 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -564,6 +564,67 @@ TEST(trajectory, calcLateralOffset) EXPECT_NEAR(calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0)), -3.0, epsilon); } +TEST(trajectory, calcLateralOffset_without_segment_idx) +{ + using motion_utils::calcLateralOffset; + + const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; + + // Empty + EXPECT_THROW( + calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}, throw_exception), + std::invalid_argument); + + // Trajectory size is 1 + { + const auto one_point_traj = generateTestTrajectory(1, 1.0); + EXPECT_THROW( + calcLateralOffset( + one_point_traj.points, geometry_msgs::msg::Point{}, static_cast(0), + throw_exception), + std::runtime_error); + } + + // Same close points in trajectory + { + const auto invalid_traj = generateTestTrajectory(10, 0.0); + const auto p = createPoint(3.0, 0.0, 0.0); + EXPECT_THROW( + calcLateralOffset(invalid_traj.points, p, static_cast(2), throw_exception), + std::runtime_error); + EXPECT_THROW( + calcLateralOffset(invalid_traj.points, p, static_cast(3), throw_exception), + std::runtime_error); + } + + // Point on trajectory + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(3.1, 0.0, 0.0), static_cast(3)), 0.0, + epsilon); + + // Point before start point + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(-3.9, 3.0, 0.0), static_cast(0)), 3.0, + epsilon); + + // Point after start point + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(13.3, -10.0, 0.0), static_cast(8)), -10.0, + epsilon); + + // Random cases + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(4.3, 7.0, 0.0), static_cast(4)), 7.0, + epsilon); + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast(0)), -3.0, + epsilon); + EXPECT_NEAR( + calcLateralOffset(traj.points, createPoint(1.0, -3.0, 0.0), static_cast(1)), -3.0, + epsilon); +} + TEST(trajectory, calcLateralOffset_CurveTrajectory) { using motion_utils::calcLateralOffset; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index b399fa5197ba0..f44a0de79a056 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -119,7 +119,8 @@ FrenetCoordinate3d convertToFrenetCoordinate3d( 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); + frenet_coordinate.distance = + motion_utils::calcLateralOffset(pose_array, search_point_geom, seg_idx); return frenet_coordinate; }