From b5bd9beca89ae2324f7f601a8d108a63c80c3d99 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Thu, 9 Feb 2023 10:55:55 +0200
Subject: [PATCH] Fix bad vector indexing

---
 src/axidraw_controller/src/cpp/axidraw_controller.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/axidraw_controller/src/cpp/axidraw_controller.cpp b/src/axidraw_controller/src/cpp/axidraw_controller.cpp
index 4fab443..f084815 100644
--- a/src/axidraw_controller/src/cpp/axidraw_controller.cpp
+++ b/src/axidraw_controller/src/cpp/axidraw_controller.cpp
@@ -158,10 +158,9 @@ class AxidrawController : public RobotController
       else
       {
         this->pendown_pub->publish(std_msgs::msg::Empty());
-        while (i + 1 < points.size() && points[i + 1].z <= 0)
+        while (i + count + 1 < points.size() && points[i + count + 1].z <= 0)
         {
           count++;
-          i++;
         }
       }
 
@@ -184,6 +183,7 @@ class AxidrawController : public RobotController
         RCLCPP_INFO(this->get_logger(), log.c_str());
         this->path_pub->publish(msg);
       }
+      i += count;
 
       // Update status
       feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " complete";