diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp
index 2e23fec..b289876 100755
--- a/waypoint_nav/src/waypoint_nav.cpp
+++ b/waypoint_nav/src/waypoint_nav.cpp
@@ -522,6 +522,7 @@
                 wp_num_pub_.publish(wp_num_);
 
             } catch(const SwitchRunningStatus &e) {
+                move_base_action_.cancelAllGoals();
                 ROS_INFO_STREAM("Running status switched");
             }
             sleep();