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