Provide bt result

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

@@ -40,7 +40,8 @@ class BtActionServer
   typedef std::function<bool (typename ActionT::Goal::ConstSharedPtr)> OnGoalReceivedCallback;
   typedef std::function<void ()> OnLoopCallback;
   typedef std::function<void (typename ActionT::Goal::ConstSharedPtr)> OnPreemptCallback;
-  typedef std::function<void (typename ActionT::Result::SharedPtr)> OnCompletionCallback;
+  typedef std::function<void (typename ActionT::Result::SharedPtr,
+      nav2_behavior_tree::BtStatus)> OnCompletionCallback;
 
   /**
    * @brief A constructor for nav2_behavior_tree::BtActionServer class

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp

@@ -216,7 +216,7 @@ void BtActionServer<ActionT>::executeCallback()
   // Give server an opportunity to populate the result message or simple give
   // an indication that the action is complete.
   auto result = std::make_shared<typename ActionT::Result>();
-  on_completion_callback_(result);
+  on_completion_callback_(result, rc);
 
   switch (rc) {
     case nav2_behavior_tree::BtStatus::SUCCEEDED:

nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp

@@ -164,7 +164,7 @@ class Navigator
       std::bind(&Navigator::onGoalReceived, this, std::placeholders::_1),
       std::bind(&Navigator::onLoop, this),
       std::bind(&Navigator::onPreempt, this, std::placeholders::_1),
-      std::bind(&Navigator::onCompletion, this, std::placeholders::_1));
+      std::bind(&Navigator::onCompletion, this, std::placeholders::_1, std::placeholders::_2));
 
     bool ok = true;
     if (!bt_action_server_->on_configure()) {
@@ -180,7 +180,7 @@ class Navigator
   }
 
   /**
-   * @brief Actiation of the navigator's backend BT and actions
+   * @brief Activation of the navigator's backend BT and actions
    * @return bool If successful
    */
   bool on_activate()
@@ -195,7 +195,7 @@ class Navigator
   }
 
   /**
-   * @brief Dectiation of the navigator's backend BT and actions
+   * @brief Deactivation of the navigator's backend BT and actions
    * @return bool If successful
    */
   bool on_deactivate()
@@ -265,12 +265,14 @@ class Navigator
   }
 
   /**
-   * @brief An intermediate compution function to mux navigators
+   * @brief An intermediate completion function to mux navigators
    */
-  void onCompletion(typename ActionT::Result::SharedPtr result)
+  void onCompletion(
+    typename ActionT::Result::SharedPtr result,
+    const nav2_behavior_tree::BtStatus final_bt_status)
   {
     plugin_muxer_->stopNavigating(getName());
-    goalCompleted(result);
+    goalCompleted(result, final_bt_status);
   }
 
   /**
@@ -292,10 +294,12 @@ class Navigator
   virtual void onPreempt(typename ActionT::Goal::ConstSharedPtr goal) = 0;
 
   /**
-   * @brief A callback that is called when a the action is completed, can fill in
+   * @brief A callback that is called when a the action is completed; Can fill in
    * action result message or indicate that this action is done.
    */
-  virtual void goalCompleted(typename ActionT::Result::SharedPtr result) = 0;
+  virtual void goalCompleted(
+    typename ActionT::Result::SharedPtr result,
+    const nav2_behavior_tree::BtStatus final_bt_status) = 0;
 
   /**
    * @param Method to configure resources.
@@ -313,12 +317,12 @@ class Navigator
   virtual bool cleanup() {return true;}
 
   /**
-   * @brief Method to active and any threads involved in execution.
+   * @brief Method to activate any threads involved in execution.
    */
   virtual bool activate() {return true;}
 
   /**
-   * @brief Method to deactive and any threads involved in execution.
+   * @brief Method to deactivate and any threads involved in execution.
    */
   virtual bool deactivate() {return true;}
 

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

@@ -95,8 +95,12 @@ class NavigateThroughPosesNavigator
    * @brief A callback that is called when a the action is completed, can fill in
    * action result message or indicate that this action is done.
    * @param result Action template result message to populate
+   * @param final_bt_status Resulting status of the behavior tree execution that may be
+   * referenced while populating the result.
    */
-  void goalCompleted(typename ActionT::Result::SharedPtr result) override;
+  void goalCompleted(
+    typename ActionT::Result::SharedPtr result,
+    const nav2_behavior_tree::BtStatus final_bt_status) override;
 
   /**
    * @brief Goal pose initialization on the blackboard

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

@@ -106,8 +106,12 @@ class NavigateToPoseNavigator
    * @brief A callback that is called when a the action is completed, can fill in
    * action result message or indicate that this action is done.
    * @param result Action template result message to populate
+   * @param final_bt_status Resulting status of the behavior tree execution that may be
+   * referenced while populating the result.
    */
-  void goalCompleted(typename ActionT::Result::SharedPtr result) override;
+  void goalCompleted(
+    typename ActionT::Result::SharedPtr result,
+    const nav2_behavior_tree::BtStatus final_bt_status) override;
 
   /**
    * @brief Goal pose initialization on the blackboard

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -87,7 +87,9 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 }
 
 void
-NavigateThroughPosesNavigator::goalCompleted(typename ActionT::Result::SharedPtr /*result*/)
+NavigateThroughPosesNavigator::goalCompleted(
+  typename ActionT::Result::SharedPtr /*result*/,
+  const nav2_behavior_tree::BtStatus /*final_bt_status*/)
 {
 }
 

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -101,7 +101,9 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
 }
 
 void
-NavigateToPoseNavigator::goalCompleted(typename ActionT::Result::SharedPtr /*result*/)
+NavigateToPoseNavigator::goalCompleted(
+  typename ActionT::Result::SharedPtr /*result*/,
+  const nav2_behavior_tree::BtStatus /*final_bt_status*/)
 {
 }