From 83751b420fe28f2e623522276bba2bbbeca71ceb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 10 Jun 2022 00:23:34 +0900 Subject: [PATCH 1/2] fix(obstacle_avoidance_planner): empty points handling (#1070) Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index ad5d933952105..c2e14e894908e 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1114,6 +1114,10 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea( std::vector & traj_points, const CVMaps & cv_maps) { + if (traj_points.empty()) { + return; + } + stop_watch_.tic(__func__); const auto & map_info = cv_maps.map_info; From 640deb3b0dcfc3d78dc8134fd2482899f7bed3b5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 10 Jun 2022 16:42:28 +0900 Subject: [PATCH 2/2] fix(obstacle_avoidance_planner): add error handling of empty reference point (#1072) Signed-off-by: Takayuki Murooka --- planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 8d68e5f19f7b6..82247c557d98d 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -377,6 +377,9 @@ std::vector MPTOptimizer::getReferencePoints( traj_param_.num_sampling_points * mpt_param_.delta_arc_length_for_mpt_points + tmp_ref_points_margin; ref_points = points_utils::clipForwardPoints(ref_points, 0, ref_length_with_margin); + if (ref_points.empty()) { + return std::vector{}; + } // set bounds information calcBounds(ref_points, enable_avoidance, maps, prev_trajs, debug_data_ptr);