fix: obstacle stop planner died when path size is 1 (#976)

Signed-off-by: tomoya.kimura <[email protected]>
tier4 · May 26, 2022 · 36a77c9 · 36a77c9
1 parent b76b6e1
commit 36a77c9
Showing 1 changed file with 8 additions and 6 deletions.
diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
@@ -781,12 +781,14 @@ void ObstacleStopPlannerNode::insertVelocity(
     }
   }
 
-  for (size_t i = 0; i < output.size() - 2; ++i) {
-    const auto & p_base = output.at(i).pose;
-    const auto & p_target = output.at(i + 1).pose;
-    const auto & p_next = output.at(i + 2).pose;
-    if (!checkValidIndex(p_base, p_next, p_target)) {
-      RCLCPP_ERROR(get_logger(), "detect bad index");
+  if (output.size() >= 2) {
+    for (size_t i = 0; i < output.size() - 2; ++i) {
+      const auto & p_base = output.at(i).pose;
+      const auto & p_target = output.at(i + 1).pose;
+      const auto & p_next = output.at(i + 2).pose;
+      if (!checkValidIndex(p_base, p_next, p_target)) {
+        RCLCPP_ERROR(get_logger(), "detect bad index");
+      }
     }
   }