diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 1a2128b..c4e74a0 100644
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -194,7 +194,8 @@
     */
     bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response)
     {
-        if (has_activate_) {
+        if ((has_activate_) || (current_waypoint_ != waypoints_.poses.begin())) {
+            ROS_WARN("Waypoint navigation is already started");
             response.success = false;
             return false;
         }