diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index 1057e4a..b8b24de 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -590,6 +590,15 @@ namespace Modelec
             std_msgs::msg::Bool start_odo_msg;
             start_odo_msg.data = false;
             start_odo_pub_->publish(start_odo_msg);
+
+            waypoints_.clear();
+
+            Waypoint w(*msg, -1, false);
+
+            waypoints_.emplace_back(w);
+
+            // SendWaypoint();
+            SendGoTo();
         }
     }