chore(motion_utils): Enrich error message (#5665)

* chore(motion_utils): colorize error message Signed-off-by: Takamasa Horibe <[email protected]> * enrich error log Signed-off-by: Takamasa Horibe <[email protected]> * update Signed-off-by: Takamasa Horibe <[email protected]> * update Signed-off-by: Takamasa Horibe <[email protected]> * update Signed-off-by: Takamasa Horibe <[email protected]> --------- Signed-off-by: Takamasa Horibe <[email protected]>
autowarefoundation · Dec 20, 2023 · 4ca5500 · 4ca5500
1 parent c9b9fca
commit 4ca5500
Show file tree

Hide file tree

Showing 3 changed files with 102 additions and 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
@@ -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 <motion_utils/constants.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
@@ -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 <class T>
 bool validate_size(const T & points)
 {
@@ -58,23 +62,28 @@ bool validate_arguments(const T & input_points, const std::vector<double> & 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;
   }