From d0a432ea827c7ed92afbaa074253155f837519f6 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <maxime.clement@tier4.jp>
Date: Fri, 16 Feb 2024 12:07:23 +0900
Subject: [PATCH] fix(behavior_path_planner): catch exception when calculating
curvature
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
.../drivable_area_expansion.cpp | 17 ++++++++++++-----
1 file changed, 12 insertions(+), 5 deletions(-)
diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
index da7f0fbb327d7..42c2a375d77cf 100644
--- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
@@ -383,11 +383,18 @@ void expand_drivable_area(
stop_watch.tic("curvatures_expansion");
// Only add curvatures for the new points. Curvatures of reused path points are not updated.
- const auto new_curvatures =
- calculate_smoothed_curvatures(path_poses, params.curvature_average_window);
- const auto first_new_point_idx = curvatures.size();
- curvatures.insert(
- curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end());
+ try {
+ if (path_poses.size() > curvatures.size()) {
+ const auto new_curvatures =
+ calculate_smoothed_curvatures(path_poses, params.curvature_average_window);
+ const auto first_new_point_idx = curvatures.size();
+ curvatures.insert(
+ curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end());
+ }
+ } catch (const std::exception & e) {
+ std::cerr << "[drivable_area_expansion] could not calculate path curvatures\n";
+ curvatures.resize(path_poses.size(), 0.0);
+ }
auto expansion =
calculate_expansion(path_poses, path.left_bound, path.right_bound, curvatures, params);
const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion");