diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index 6b8b757..a07bc23 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -727,6 +727,8 @@ namespace Modelec
             }
         }*/
 
+        RCLCPP_INFO(node_->get_logger(), "Waypoint reached, id: %d", waypoint_queue_.front().id);
+
         if (await_rotate_)
         {
             await_rotate_ = false;
@@ -741,6 +743,8 @@ namespace Modelec
         }
         else
         {
+            RCLCPP_INFO(node_->get_logger(), "Waypoint reached 2");
+
             if (!waypoint_queue_.empty())
             {
                 waypoint_pub_->publish(waypoint_queue_.front().ToMsg());