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>{};
}