Added goal accepted check before flagging navigator as running (#2949)

ros-navigation · May 13, 2022 · 29e4c42 · 29e4c42
1 parent ceda5a9
commit 29e4c42
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp
@@ -255,9 +255,13 @@ class Navigator
       return false;
     }
 
-    plugin_muxer_->startNavigating(getName());
+    bool goal_accepted = goalReceived(goal);
 
-    return goalReceived(goal);
+    if (goal_accepted) {
+      plugin_muxer_->startNavigating(getName());
+    }
+
+    return goal_accepted;
   }
 
   /**