When new_goal and cancel_pending arrive at the same time, apply first new_goal

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;