From 4ca5500868b10d283f88c21ba92b8eb4ca9d173e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 20 Dec 2023 11:25:56 +0900 Subject: [PATCH] chore(motion_utils): Enrich error message (#5665) * chore(motion_utils): colorize error message Signed-off-by: Takamasa Horibe * enrich error log Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../motion_utils/resample/resample_utils.hpp | 29 +++-- .../motion_utils/trajectory/trajectory.hpp | 114 ++++++++++++------ .../test/src/trajectory/test_trajectory.cpp | 14 +-- 3 files changed, 102 insertions(+), 55 deletions(-) diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 87a0d54af6619..8bb5f13e3fb78 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#include "tier4_autoware_utils/system/backtrace.hpp" + #include #include #include @@ -25,6 +27,8 @@ namespace resample_utils { constexpr double close_s_threshold = 1e-6; +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; + template bool validate_size(const T & points) { @@ -58,23 +62,28 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { - std::cerr << "The number of resampling intervals is less than 2" << std::endl; + log_error( + "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - std::cerr << "resampling interval is longer than input points" << std::endl; + log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } @@ -86,20 +95,24 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - std::cerr << "resampling interval is less than " << motion_utils::overlap_threshold - << std::endl; + log_error( + "[resample_utils] invalid argument: resampling interval is less than " + + std::to_string(motion_utils::overlap_threshold)); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f2e0c1c0c184c..d81f75815ecc6 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -30,10 +30,12 @@ #include #include #include +#include #include #include namespace motion_utils { +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; /** * @brief validate if points container is empty or not @@ -44,7 +46,7 @@ void validateNonEmpty(const T & points) { if (points.empty()) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Points is empty."); + throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); } } @@ -82,7 +84,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Sharp angle."); + throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -214,7 +216,7 @@ std::optional searchZeroVelocityIndex( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -246,7 +248,7 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -336,7 +338,7 @@ std::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -401,12 +403,17 @@ double calcLongitudinalOffsetToSegment( const bool throw_exception = false) { if (seg_idx >= points.size() - 1) { - const auto error_message{"Segment index is invalid."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -418,18 +425,22 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return std::nan(""); } } if (seg_idx >= overlap_removed_points.size() - 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -575,18 +586,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -637,18 +654,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -684,7 +707,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -727,7 +750,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -779,7 +802,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -822,7 +845,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -861,7 +884,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -901,7 +924,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -1007,7 +1030,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1041,17 +1064,22 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } if (points.size() - 1 < src_idx) { - const auto error_message{"Invalid source index"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return {}; } @@ -1118,7 +1146,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } @@ -1166,21 +1194,25 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } if (points.size() - 1 < src_idx) { - const auto error_message{"Invalid source index"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error(error_message); return {}; } if (points.size() == 1) { + log_error("Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1265,7 +1297,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1311,7 +1343,7 @@ std::optional insertTargetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1326,7 +1358,7 @@ std::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -2192,7 +2224,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate stop distance" + std::string(e.what())); return {}; } @@ -2331,9 +2363,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - RCLCPP_ERROR( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), - ". Return original points since cropped_points size is less than 2."); + log_error("Return original points since cropped_points size is less than 2."); return points; } @@ -2378,18 +2408,22 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } } if (overlap_removed_points.size() <= 1) { - const auto error_message{"points size is less than 2"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return 0 since no_throw option is enabled. The maintainer must check the code."); return 0.0; } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 22567b569d0ad..6664e6154bdfd 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1883,7 +1883,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1896,7 +1896,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1909,7 +1909,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2310,7 +2310,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4317,7 +4317,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4330,7 +4330,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4343,7 +4343,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); }