From 265397312e3727820efd44e77afd5891279cff4f Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Sat, 9 Jul 2022 12:17:07 +0900 Subject: [PATCH] fix(tier4_autoware_utils): exception handling in tier4 autoware utils (#1268) * fix(tier4_autoware_utils): fix exception handling (WIP) Signed-off-by: Fumiya Watanabe * fix(tier4_autoware_utils): fix for overlapping points and test Signed-off-by: Fumiya Watanabe * fix(tier4_autoware_utils): formatting Signed-off-by: Fumiya Watanabe * fix(trajectory_follower): fix for overlapping points and test Signed-off-by: Fumiya Watanabe * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../trajectory/path_with_lane_id.hpp | 53 +++- .../trajectory/trajectory.hpp | 238 +++++++++++++++--- .../src/trajectory/test_path_with_lane_id.cpp | 32 +-- .../test/src/trajectory/test_trajectory.cpp | 74 +++--- .../src/longitudinal_controller_utils.cpp | 4 + .../test_longitudinal_controller_utils.cpp | 4 +- 6 files changed, 300 insertions(+), 105 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp index 7289c1f573ac1..bd2174f011022 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp @@ -38,12 +38,22 @@ namespace tier4_autoware_utils template <> inline boost::optional calcLongitudinalOffsetPoint( const std::vector & points, - const size_t src_idx, const double offset) + const size_t src_idx, const double offset, const bool throw_exception) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } if (points.size() - 1 < src_idx) { - throw std::out_of_range("Invalid source index"); + const auto e = std::out_of_range("Invalid source index"); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return {}; } if (points.size() == 1) { @@ -92,7 +102,12 @@ inline boost::optional calcLongitudinalOffsetPoint( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } if (offset < 0.0) { auto reverse_points = points; @@ -117,12 +132,22 @@ inline boost::optional calcLongitudinalOffsetPoint( template <> inline boost::optional calcLongitudinalOffsetPose( const std::vector & points, - const size_t src_idx, const double offset) + const size_t src_idx, const double offset, const bool throw_exception) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } if (points.size() - 1 < src_idx) { - throw std::out_of_range("Invalid source index"); + const auto e = std::out_of_range("Invalid source index"); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return {}; } if (points.size() == 1) { @@ -184,7 +209,12 @@ inline boost::optional calcLongitudinalOffsetPose( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); const double signed_length_src_offset = @@ -206,7 +236,12 @@ inline boost::optional insertTargetPoint( std::vector & points, const double overlap_threshold) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } // invalid segment index if (seg_idx + 1 >= points.size()) { diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp index e7c3c9a5234d4..37bf335762ef2 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp @@ -26,6 +26,37 @@ #include #include +namespace +{ +template +std::vector removeOverlapPoints(const T & points, const size_t & idx) +{ + std::vector dst; + + for (const auto & pt : points) { + dst.push_back(tier4_autoware_utils::getPoint(pt)); + } + + if (points.empty()) { + return dst; + } + + constexpr double eps = 1.0E-08; + size_t i = idx; + while (i != dst.size() - 1) { + const auto p = tier4_autoware_utils::getPoint(dst.at(i)); + const auto p_next = tier4_autoware_utils::getPoint(dst.at(i + 1)); + const Eigen::Vector3d v{p_next.x - p.x, p_next.y - p.y, 0.0}; + if (v.norm() < eps) { + dst.erase(dst.begin() + i + 1); + } else { + ++i; + } + } + return dst; +} +} // namespace + namespace tier4_autoware_utils { template @@ -61,7 +92,12 @@ template boost::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) { - validateNonEmpty(points_with_twist); + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } constexpr double epsilon = 1e-3; for (size_t i = src_idx; i < dst_idx; ++i) { @@ -103,7 +139,12 @@ boost::optional findNearestIndex( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const double max_squared_dist = max_dist * max_dist; @@ -143,20 +184,46 @@ boost::optional findNearestIndex( */ template double calcLongitudinalOffsetToSegment( - const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target) + const T & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false) { - validateNonEmpty(points); + if (seg_idx >= points.size() - 1) { + const std::out_of_range e("Segment index is invalid."); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return std::nan(""); + } - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); + const auto overlap_removed_points = removeOverlapPoints(points, seg_idx); - const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; - const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 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 (segment_vec.norm() == 0.0) { - throw std::runtime_error("Same points are given."); + if (seg_idx >= 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 auto p_front = getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = getPoint(overlap_removed_points.at(seg_idx + 1)); + + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + return segment_vec.dot(target_vec) / segment_vec.norm(); } @@ -235,22 +302,39 @@ boost::optional findNearestSegmentIndex( * @return length (unsigned) */ template -double calcLateralOffset(const T & points, const geometry_msgs::msg::Point & p_target) +double calcLateralOffset( + const T & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false) { - validateNonEmpty(points); + const auto overlap_removed_points = removeOverlapPoints(points, 0); - const size_t seg_idx = findNearestSegmentIndex(points, p_target); + 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(""); + } + } - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); + 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); + + const auto p_front = getPoint(overlap_removed_points.at(seg_idx)); + const auto p_back = getPoint(overlap_removed_points.at(seg_idx + 1)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; - if (segment_vec.norm() == 0.0) { - throw std::runtime_error("Same points are given."); - } - const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); return cross_vec(2) / segment_vec.norm(); } @@ -261,7 +345,12 @@ double calcLateralOffset(const T & points, const geometry_msgs::msg::Point & p_t template double calcSignedArcLength(const T & points, const size_t src_idx, const size_t dst_idx) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } if (src_idx > dst_idx) { return -calcSignedArcLength(points, dst_idx, src_idx); @@ -281,7 +370,12 @@ template double calcSignedArcLength( const T & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); @@ -301,7 +395,12 @@ boost::optional calcSignedArcLength( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); if (!src_seg_idx) { @@ -322,7 +421,12 @@ template double calcSignedArcLength( const T & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } return -calcSignedArcLength(points, dst_point, src_idx); } @@ -335,7 +439,12 @@ double calcSignedArcLength( const T & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); const size_t dst_seg_idx = findNearestSegmentIndex(points, dst_point); @@ -359,7 +468,12 @@ boost::optional calcSignedArcLength( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const auto src_seg_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); if (!src_seg_idx) { @@ -383,7 +497,12 @@ boost::optional calcSignedArcLength( template double calcArcLength(const T & points) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return 0.0; + } return calcSignedArcLength(points, 0, points.size() - 1); } @@ -395,7 +514,12 @@ template boost::optional calcDistanceToForwardStopPoint( const T & points_with_twist, const size_t src_idx = 0) { - validateNonEmpty(points_with_twist); + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const auto closest_stop_idx = searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); @@ -415,7 +539,12 @@ boost::optional calcDistanceToForwardStopPoint( const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) { - validateNonEmpty(points_with_twist); + try { + validateNonEmpty(points_with_twist); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); @@ -450,12 +579,22 @@ boost::optional calcDistanceToForwardStopPoint( */ template inline boost::optional calcLongitudinalOffsetPoint( - const T & points, const size_t src_idx, const double offset) + const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } if (points.size() - 1 < src_idx) { - throw std::out_of_range("Invalid source index"); + const auto e = std::out_of_range("Invalid source index"); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return {}; } if (points.size() == 1) { @@ -503,7 +642,12 @@ template inline boost::optional calcLongitudinalOffsetPoint( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } if (offset < 0.0) { auto reverse_points = points; @@ -527,12 +671,22 @@ inline boost::optional calcLongitudinalOffsetPoint( */ template inline boost::optional calcLongitudinalOffsetPose( - const T & points, const size_t src_idx, const double offset) + const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } if (points.size() - 1 < src_idx) { - throw std::out_of_range("Invalid source index"); + const auto e = std::out_of_range("Invalid source index"); + if (throw_exception) { + throw e; + } + std::cerr << e.what() << std::endl; + return {}; } if (points.size() == 1) { @@ -593,7 +747,12 @@ template inline boost::optional calcLongitudinalOffsetPose( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } const size_t src_seg_idx = findNearestSegmentIndex(points, src_point); const double signed_length_src_offset = @@ -614,7 +773,12 @@ inline boost::optional insertTargetPoint( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { - validateNonEmpty(points); + try { + validateNonEmpty(points); + } catch (const std::exception & e) { + std::cerr << e.what() << std::endl; + return {}; + } // invalid segment index if (seg_idx + 1 >= points.size()) { diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp index 56798aec0aa8e..419e816f328ad 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -72,12 +72,11 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex_PathWithLaneId) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {})); // Out of range - EXPECT_THROW( - calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); - EXPECT_THROW(calcLongitudinalOffsetPoint(traj.points, -1, 1.0), std::out_of_range); + EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0)); + EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, -1, 1.0)); // Found Pose(forward) for (size_t i = 0; i < traj.points.size(); ++i) { @@ -150,7 +149,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint_PathWithLaneId) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPoint(PathWithLaneId{}.points, {}, {})); // Found Pose(forward) for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { @@ -211,9 +210,8 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint_PathWithLaneId) // Out of range(Trajectory size is 1) { const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_THROW( - calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {}), - std::out_of_range); + EXPECT_FALSE( + calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {})); } } @@ -228,12 +226,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_PathWithLaneId) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {})); // Out of range - EXPECT_THROW( - calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); - EXPECT_THROW(calcLongitudinalOffsetPose(traj.points, -1, 1.0), std::out_of_range); + EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0)); + EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, -1, 1.0)); // Found Pose(forward) for (size_t i = 0; i < traj.points.size(); ++i) { @@ -314,7 +311,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_PathWithLaneId) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPose(PathWithLaneId{}.points, {}, {})); // Found Pose(forward) for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { @@ -383,9 +380,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_PathWithLaneId) // Out of range(Trajectory size is 1) { const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_THROW( - calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {}), - std::out_of_range); + EXPECT_FALSE( + calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {})); } } @@ -604,9 +600,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId) { auto empty_traj = generateTestTrajectory(0, 1.0); const size_t segment_idx = 0; - EXPECT_THROW( - insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points), - std::invalid_argument); + EXPECT_FALSE(insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); } } diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp index 1344f621a0c4f..0415c38fd566c 100644 --- a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp @@ -168,7 +168,7 @@ TEST(trajectory, searchZeroVelocityIndex) using tier4_autoware_utils::searchZeroVelocityIndex; // Empty - EXPECT_THROW(searchZeroVelocityIndex(Trajectory{}.points), std::invalid_argument); + EXPECT_FALSE(searchZeroVelocityIndex(Trajectory{}.points)); // No zero velocity point { @@ -290,8 +290,7 @@ TEST(trajectory, findNearestIndex_Pose_NoThreshold) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW( - findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {}), std::invalid_argument); + EXPECT_FALSE(findNearestIndex(Trajectory{}.points, geometry_msgs::msg::Pose{}, {})); // Start point EXPECT_EQ(*findNearestIndex(traj.points, createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), 0U); @@ -404,26 +403,30 @@ TEST(trajectory, calcLongitudinalOffsetToSegment_StraightTrajectory) using tier4_autoware_utils::calcLongitudinalOffsetToSegment; const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; // Empty EXPECT_THROW( - calcLongitudinalOffsetToSegment(Trajectory{}.points, {}, geometry_msgs::msg::Point{}), + calcLongitudinalOffsetToSegment( + Trajectory{}.points, {}, geometry_msgs::msg::Point{}, throw_exception), std::invalid_argument); // Out of range EXPECT_THROW( - calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}), + calcLongitudinalOffsetToSegment(traj.points, -1, geometry_msgs::msg::Point{}, throw_exception), std::out_of_range); EXPECT_THROW( calcLongitudinalOffsetToSegment( - traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}), + traj.points, traj.points.size() - 1, geometry_msgs::msg::Point{}, throw_exception), std::out_of_range); // 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(calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p), std::runtime_error); + EXPECT_THROW( + calcLongitudinalOffsetToSegment(invalid_traj.points, 3, p, throw_exception), + std::runtime_error); } // Same point @@ -462,23 +465,26 @@ TEST(trajectory, calcLateralOffset) using tier4_autoware_utils::calcLateralOffset; const auto traj = generateTestTrajectory(10, 1.0); + const bool throw_exception = true; // Empty EXPECT_THROW( - calcLateralOffset(Trajectory{}.points, geometry_msgs::msg::Point{}), std::invalid_argument); + 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{}), std::out_of_range); + calcLateralOffset(one_point_traj.points, geometry_msgs::msg::Point{}, 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), std::runtime_error); + EXPECT_THROW(calcLateralOffset(invalid_traj.points, p, throw_exception), std::runtime_error); } // Point on trajectory @@ -513,7 +519,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToIndex) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Out of range EXPECT_THROW(calcSignedArcLength(traj.points, -1, 1), std::out_of_range); @@ -536,7 +542,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToIndex) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point EXPECT_NEAR(calcSignedArcLength(traj.points, createPoint(3.0, 0.0, 0.0), 3), 0, epsilon); @@ -607,7 +613,7 @@ TEST(trajectory, calcSignedArcLengthFromIndexToPoint) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point EXPECT_NEAR(calcSignedArcLength(traj.points, 3, createPoint(3.0, 0.0, 0.0)), 0, epsilon); @@ -636,7 +642,7 @@ TEST(trajectory, calcSignedArcLengthFromPointToPoint) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcSignedArcLength(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcSignedArcLength(Trajectory{}.points, {}, {}), 0.0); // Same point { @@ -699,7 +705,7 @@ TEST(trajectory, calcArcLength) const auto traj = generateTestTrajectory(10, 1.0); // Empty - EXPECT_THROW(calcArcLength(Trajectory{}.points), std::invalid_argument); + EXPECT_DOUBLE_EQ(calcArcLength(Trajectory{}.points), 0.0); // Whole Length EXPECT_NEAR(calcArcLength(traj.points), 9.0, epsilon); @@ -753,7 +759,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Empty { - EXPECT_THROW(calcDistanceToForwardStopPoint(Trajectory{}.points), std::invalid_argument); + EXPECT_FALSE(calcDistanceToForwardStopPoint(Trajectory{}.points)); } // No Stop Point @@ -811,9 +817,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) // Empty { - EXPECT_THROW( - calcDistanceToForwardStopPoint(Trajectory{}.points, geometry_msgs::msg::Pose{}), - std::invalid_argument); + EXPECT_FALSE(calcDistanceToForwardStopPoint(Trajectory{}.points, geometry_msgs::msg::Pose{})); } // No Stop Point @@ -995,12 +999,11 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {})); // Out of range - EXPECT_THROW( - calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); - EXPECT_THROW(calcLongitudinalOffsetPoint(traj.points, -1, 1.0), std::out_of_range); + EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, traj.points.size() + 1, 1.0)); + EXPECT_FALSE(calcLongitudinalOffsetPoint(traj.points, -1, 1.0)); // Found Pose(forward) for (size_t i = 0; i < traj.points.size(); ++i) { @@ -1073,7 +1076,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPoint(Trajectory{}.points, {}, {})); // Found Pose(forward) for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { @@ -1134,9 +1137,8 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) // Out of range(Trajectory size is 1) { const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_THROW( - calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {}), - std::out_of_range); + EXPECT_FALSE( + calcLongitudinalOffsetPoint(one_point_traj.points, geometry_msgs::msg::Point{}, {})); } } @@ -1151,12 +1153,11 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {})); // Out of range - EXPECT_THROW( - calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0), std::out_of_range); - EXPECT_THROW(calcLongitudinalOffsetPose(traj.points, -1, 1.0), std::out_of_range); + EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, traj.points.size() + 1, 1.0)); + EXPECT_FALSE(calcLongitudinalOffsetPose(traj.points, -1, 1.0)); // Found Pose(forward) for (size_t i = 0; i < traj.points.size(); ++i) { @@ -1323,7 +1324,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) const auto total_length = calcArcLength(traj.points); // Empty - EXPECT_THROW(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {}), std::invalid_argument); + EXPECT_FALSE(calcLongitudinalOffsetPose(Trajectory{}.points, {}, {})); // Found Pose(forward) for (double x_start = 0.0; x_start < total_length + epsilon; x_start += 0.1) { @@ -1392,9 +1393,8 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) // Out of range(Trajectory size is 1) { const auto one_point_traj = generateTestTrajectory(1, 1.0); - EXPECT_THROW( - calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {}), - std::out_of_range); + EXPECT_FALSE( + calcLongitudinalOffsetPose(one_point_traj.points, geometry_msgs::msg::Point{}, {})); } } @@ -1687,9 +1687,7 @@ TEST(trajectory, insertTargetPoint) { auto empty_traj = generateTestTrajectory(0, 1.0); const size_t segment_idx = 0; - EXPECT_THROW( - insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points), - std::invalid_argument); + EXPECT_FALSE(insertTargetPoint(segment_idx, geometry_msgs::msg::Point{}, empty_traj.points)); } } diff --git a/control/trajectory_follower/src/longitudinal_controller_utils.cpp b/control/trajectory_follower/src/longitudinal_controller_utils.cpp index db9370ba8dabf..a60ac9c2da2a2 100644 --- a/control/trajectory_follower/src/longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/src/longitudinal_controller_utils.cpp @@ -77,6 +77,10 @@ float64_t calcStopDistance( const float64_t signed_length_src_offset = tier4_autoware_utils::calcLongitudinalOffsetToSegment( traj.points, *seg_idx, current_pose.position); + if (std::isnan(signed_length_src_offset)) { + return 0.0; + } + // If no zero velocity point, return the length between current_pose to the end of trajectory. if (!stop_idx_opt) { float64_t signed_length_on_traj = diff --git a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp index 354c483982a8f..6bd8403b2b514 100644 --- a/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp +++ b/control/trajectory_follower/test/test_longitudinal_controller_utils.cpp @@ -68,8 +68,8 @@ TEST(TestLongitudinalControllerUtils, calcStopDistance) point.pose.position.y = 0.0; point.longitudinal_velocity_mps = 0.0; traj.points.push_back(point); - EXPECT_THROW( - longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), std::out_of_range); + EXPECT_DOUBLE_EQ( + longitudinal_utils::calcStopDistance(current_pose, traj, max_dist, max_yaw), 0.0); traj.points.clear(); // non stopping trajectory: stop distance = trajectory length point.pose.position.x = 0.0;