From 2e3726245c9dcb0c81d96eaa74e4af5901f36b31 Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Thu, 2 May 2024 15:51:23 +0900
Subject: [PATCH 1/9] fix(obstacle_avoidance_planner): add empty check

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 90e9daccb0a89..036cbaeae060c 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -339,7 +339,10 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) {
     new_s.push_back(i);
   }
-
+  if (new_s.empty()) {
+    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+  }
+  
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 
   // spline interpolation

From d5907caa5e9c030123f49ed0461f5bcb388ae12d Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 2 May 2024 06:59:57 +0000
Subject: [PATCH 2/9] ci(pre-commit): autofix

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 036cbaeae060c..dcf16e1af0492 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -342,7 +342,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   if (new_s.empty()) {
     return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
-  
+
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 
   // spline interpolation

From 84559a4bcc4473a922764b6c2f638f595bfb225b Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Wed, 8 May 2024 13:06:28 +0900
Subject: [PATCH 3/9] add invalid_argument

---
 .../obstacle_avoidance_planner/src/utils.cpp  | 38 ++++++++++---------
 1 file changed, 21 insertions(+), 17 deletions(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index dcf16e1af0492..877bd983e3a14 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -346,27 +346,31 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 
   // spline interpolation
-  const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
-  const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
-  const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
+  try {
+    const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
+    const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
+    const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
+
+    for (size_t i = 0; i < interpolated_x.size(); i++) {
+      if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
+        return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+      }
+    }
 
-  for (size_t i = 0; i < interpolated_x.size(); i++) {
-    if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
-      return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
+    std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
+    for (size_t i = 0; i < interpolated_x.size(); i++) {
+      autoware_auto_planning_msgs::msg::TrajectoryPoint point;
+      point.pose.position.x = interpolated_x[i];
+      point.pose.position.y = interpolated_y[i];
+      point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
+        tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
+      interpolated_points.push_back(point);
     }
-  }
 
-  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
-  for (size_t i = 0; i < interpolated_x.size(); i++) {
-    autoware_auto_planning_msgs::msg::TrajectoryPoint point;
-    point.pose.position.x = interpolated_x[i];
-    point.pose.position.y = interpolated_y[i];
-    point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
-      tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
-    interpolated_points.push_back(point);
+    return interpolated_points;
+  } catch (const std::invalid_argument & e) {
+    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
-
-  return interpolated_points;
 }
 
 std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getInterpolatedTrajectoryPoints(

From 153a1159a7a078a3d84d79a61f30e70505b2e58b Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Thu, 9 May 2024 09:38:21 +0900
Subject: [PATCH 4/9] delete empty check

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 877bd983e3a14..f2aac12045c62 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -339,9 +339,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   for (double i = 0.0; i < base_s.back() - 1e-6; i += resolution) {
     new_s.push_back(i);
   }
-  if (new_s.empty()) {
-    return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
-  }
 
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 

From f0041d3374dfcb743ef347fecfe95a0178fa6378 Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Thu, 9 May 2024 14:16:24 +0900
Subject: [PATCH 5/9] return code moved to end

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index f2aac12045c62..2bf8fbc6d65d5 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -343,6 +343,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
   const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
 
   // spline interpolation
+  std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
   try {
     const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
     const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
@@ -354,7 +355,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
       }
     }
 
-    std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
     for (size_t i = 0; i < interpolated_x.size(); i++) {
       autoware_auto_planning_msgs::msg::TrajectoryPoint point;
       point.pose.position.x = interpolated_x[i];
@@ -364,10 +364,10 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
       interpolated_points.push_back(point);
     }
 
-    return interpolated_points;
   } catch (const std::invalid_argument & e) {
     return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
+  return interpolated_points;
 }
 
 std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> getInterpolatedTrajectoryPoints(

From bacba67529223b5794aaeadf1d60206c7abbcec1 Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Thu, 9 May 2024 14:39:55 +0900
Subject: [PATCH 6/9] add warning log

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 2bf8fbc6d65d5..54025a1f9ec09 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -344,6 +344,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
 
   // spline interpolation
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
+  auto clock = rclcpp::Clock{RCL_ROS_TIME};
   try {
     const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
     const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
@@ -365,6 +366,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
     }
 
   } catch (const std::invalid_argument & e) {
+    RCLCPP_WARN_THROTTLE(rclcpp::get_logger("util"), clock, 1000, "%s", e.what());
     return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
   return interpolated_points;

From 52e02e91b7575fe7c86200a1dec3aa9a9849004a Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Thu, 9 May 2024 15:26:51 +0900
Subject: [PATCH 7/9] update rclcpp_debug

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 54025a1f9ec09..20109d0c3acaf 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -344,7 +344,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
 
   // spline interpolation
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
-  auto clock = rclcpp::Clock{RCL_ROS_TIME};
   try {
     const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
     const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
@@ -366,7 +365,7 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
     }
 
   } catch (const std::invalid_argument & e) {
-    RCLCPP_WARN_THROTTLE(rclcpp::get_logger("util"), clock, 1000, "%s", e.what());
+    RCLCPP_DEBUG(rclcpp::get_logger("util"), clock, 1000, "%s", e.what());
     return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
   return interpolated_points;

From 57fe51e38b95a1a2ed4b4899f86b888e354c8eab Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Mon, 13 May 2024 15:42:43 +0900
Subject: [PATCH 8/9] delete debug log

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 20109d0c3acaf..2bf8fbc6d65d5 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -365,7 +365,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
     }
 
   } catch (const std::invalid_argument & e) {
-    RCLCPP_DEBUG(rclcpp::get_logger("util"), clock, 1000, "%s", e.what());
     return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }
   return interpolated_points;

From a81148742987c7795dde8f07e299ef162dc7909d Mon Sep 17 00:00:00 2001
From: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp>
Date: Mon, 13 May 2024 17:28:56 +0900
Subject: [PATCH 9/9] Delete unnecessary blank lines

---
 planning/obstacle_avoidance_planner/src/utils.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp
index 2bf8fbc6d65d5..57eef51c89549 100644
--- a/planning/obstacle_avoidance_planner/src/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils.cpp
@@ -363,7 +363,6 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
         tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
       interpolated_points.push_back(point);
     }
-
   } catch (const std::invalid_argument & e) {
     return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
   }