diff --git a/src/tiago_nav_module.cpp b/src/tiago_nav_module.cpp
index aec421c64e9105a568dc34748efb0379cc175528..8ba689ee5470e9fdaa8db3587d6c59947bde309b 100644
--- a/src/tiago_nav_module.cpp
+++ b/src/tiago_nav_module.cpp
@@ -235,10 +235,13 @@ void CTiagoNavModule::state_machine(void)
                             this->new_goal=false;
                             this->state=TIAGO_NAV_MODULE_TRANSFORM;
                           }
-                          if(this->cancel_pending)
+                          else
                           {
-                            this->cancel_pending=false;
-                            this->cancel_action();
+                            if(this->cancel_pending)
+                            {
+                              this->cancel_pending=false;
+                              this->cancel_action();
+                            }
                           }
                           break;