Report bt action request error (ros-navigation#6289)

* nav2_msgs: add generic action request error codes

Action result definitions used action-specific values for request
failures, and several actions lacked the shared request or timeout
constants. This prevented BtActionNode from reporting consistent
diagnostics.

This commit reserves values 1 and 2 for rejected and unsent goals
across Nav2 actions and defines the missing timeout values.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

* nav2_behavior_tree: report generic action request errors

Action nodes returned failure without consistently writing diagnostic
error codes when a goal was rejected, could not be sent, or timed out.
The implementation and tests also evolved across several corrective
commits.

This commit centralizes request-failure reporting and optional error
code fallback in BtActionNode. It updates action nodes to use the shared
ports and covers rejected goals, send-goal failures, timeouts, and
action-specific error codes.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

* opennav_docking_bt: use generic action request errors

Docking action nodes duplicated the common request-failure handling
and error-port declarations from BtActionNode.

This commit delegates those responsibilities to the generic
BtActionNode implementation.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

* opennav_docking: stop NodeThread before destroying server

The test reset the docking server node while its NodeThread was still
active. The executor could then access node resources during teardown,
causing a shutdown race and preventing the XML result from being
written.

This commit leaves node lifetime management to scope teardown, so the
NodeThread destructor cancels and joins its executor before the server
node is destroyed.

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

---------

Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be>

Author: DylanDeCoeyer-Quimesis

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -18,6 +18,7 @@
 #include <memory>
 #include <string>
 #include <chrono>
+#include <cstdint>
 
 #include "behaviortree_cpp/action_node.h"
 #include "behaviortree_cpp/json_export.h"
@@ -40,6 +41,8 @@ template<class ActionT>
 class BtActionNode : public BT::ActionNodeBase
 {
 public:
+  using ActionResult = typename ActionT::Result;
+
   /**
    * @brief A nav2_behavior_tree::BtActionNode constructor
    * @param xml_tag_name Name for the XML tag for this node
@@ -73,6 +76,28 @@ class BtActionNode : public BT::ActionNodeBase
     goal_ = typename ActionT::Goal();
     result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
 
+    if constexpr (
+      !requires {ActionT::Result::TIMEOUT;} ||
+      !requires {ActionT::Result::GOAL_REJECTED;} ||
+      !requires {ActionT::Result::SEND_GOAL_FAILURE;})
+    {
+      if constexpr (requires {ActionT::Result::UNKNOWN;}) {
+        RCLCPP_WARN(
+          node_->get_logger(),
+          "Action type for \"%s\" does not define one or more of the TIMEOUT, "
+          "GOAL_REJECTED, and SEND_GOAL_FAILURE error codes. UNKNOWN will be "
+          "used for unavailable errors.",
+          xml_tag_name.c_str());
+      } else {
+        RCLCPP_WARN(
+          node_->get_logger(),
+          "Action type for \"%s\" does not define one or more of the TIMEOUT, "
+          "GOAL_REJECTED, and SEND_GOAL_FAILURE error codes. The error_code_id "
+          "output will not be set for unavailable errors.",
+          xml_tag_name.c_str());
+      }
+    }
+
     std::string remapped_action_name;
     if (getInput("server_name", remapped_action_name)) {
       action_name_ = remapped_action_name;
@@ -121,7 +146,9 @@ class BtActionNode : public BT::ActionNodeBase
   {
     BT::PortsList basic = {
       BT::InputPort<std::string>("server_name", "Action server name"),
-      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
+      BT::OutputPort<uint16_t>("error_code_id", "The action error code"),
+      BT::OutputPort<std::string>("error_msg", "The action error message")
     };
     basic.insert(addition.begin(), addition.end());
 
@@ -193,7 +220,40 @@ class BtActionNode : public BT::ActionNodeBase
    */
   virtual void on_timeout()
   {
-    return;
+    if constexpr (requires {ActionT::Result::TIMEOUT;}) {
+      setOutput("error_code_id", ActionResult::TIMEOUT);
+    } else if constexpr (requires {ActionT::Result::UNKNOWN;}) {
+      setOutput("error_code_id", ActionResult::UNKNOWN);
+    }
+    setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+  }
+
+  /**
+   * @brief Function to perform work in a BT Node when the action server rejects a goal
+   * Such as setting the error code ID status for action clients.
+   */
+  virtual void on_goal_rejected()
+  {
+    if constexpr (requires {ActionT::Result::GOAL_REJECTED;}) {
+      setOutput("error_code_id", ActionResult::GOAL_REJECTED);
+    } else if constexpr (requires {ActionT::Result::UNKNOWN;}) {
+      setOutput("error_code_id", ActionResult::UNKNOWN);
+    }
+    setOutput("error_msg", "Goal was rejected by the action server.");
+  }
+
+  /**
+   * @brief Function to perform work when sending a goal to the action server fails
+   * Such as setting the error code ID status for action clients.
+   */
+  virtual void on_send_goal_failure()
+  {
+    if constexpr (requires {ActionT::Result::SEND_GOAL_FAILURE;}) {
+      setOutput("error_code_id", ActionResult::SEND_GOAL_FAILURE);
+    } else if constexpr (requires {ActionT::Result::UNKNOWN;}) {
+      setOutput("error_code_id", ActionResult::UNKNOWN);
+    }
+    setOutput("error_msg", "Failed to send goal to the action server.");
   }
 
   /**
@@ -285,9 +345,11 @@ class BtActionNode : public BT::ActionNodeBase
         }
       }
     } catch (const std::runtime_error & e) {
-      if (e.what() == std::string("send_goal failed") ||
-        e.what() == std::string("Goal was rejected by the action server"))
-      {
+      if (e.what() == std::string("Goal was rejected by the action server")) {
+        on_goal_rejected();
+        return BT::NodeStatus::FAILURE;
+      } else if (e.what() == std::string("send_goal failed")) {
+        on_send_goal_failure();
         // Action related failure that should not fail the tree, but the node
         return BT::NodeStatus::FAILURE;
       } else {

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp

@@ -72,12 +72,6 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
    */
   BT::NodeStatus on_cancelled() override;
 
-  /**
-   * @brief Function to perform work in a BT Node when the action server times out
-   * Such as setting the error code ID status to timed out for action clients.
-   */
-  void on_timeout() override;
-
   /**
    * @brief Function to read parameters and initialize class variables
    */
@@ -93,10 +87,6 @@ class AssistedTeleopAction : public BtActionNode<nav2_msgs::action::AssistedTele
       {
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for running assisted teleop"),
         BT::InputPort<bool>("is_recovery", false, "If true the recovery count will be incremented"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The assisted teleop behavior server error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The assisted teleop behavior server error msg"),
       });
   }
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -73,12 +73,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
    */
   BT::NodeStatus on_cancelled() override;
 
-  /**
-   * @brief Function to perform work in a BT Node when the action server times out
-   * Such as setting the error code ID status to timed out for action clients.
-   */
-  void on_timeout() override;
-
   /**
    * @brief Function to read parameters and initialize class variables
    */
@@ -96,10 +90,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
         BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The back up behavior server error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The back up behavior server error msg"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp

@@ -72,12 +72,6 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
    */
   BT::NodeStatus on_cancelled() override;
 
-  /**
-   * @brief Function to perform work in a BT Node when the action server times out
-   * Such as setting the error code ID status to timed out for action clients.
-   */
-  void on_timeout() override;
-
   /**
    * @brief Function to perform some user-defined operation after a timeout
    * waiting for a result that hasn't been received yet
@@ -114,10 +108,6 @@ class ComputeAndTrackRouteAction : public BtActionNode<nav2_msgs::action::Comput
         BT::OutputPort<builtin_interfaces::msg::Duration>(
           "execution_duration",
           "Time taken to compute and track route"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The compute route error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The compute route error msg"),
         BT::OutputPort<uint16_t>(
           "last_node_id",
           "ID of the previous node"),

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp

@@ -110,10 +110,6 @@ class ComputePathThroughPosesAction
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathThroughPoses node"),
         BT::OutputPort<int>(
           "last_reached_index", "Index of the last reachable pose from requested list of poses"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The compute path through poses error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The compute path through poses error msg"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

@@ -74,12 +74,6 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
    */
   BT::NodeStatus on_cancelled() override;
 
-  /**
-   * @brief Function to perform work in a BT Node when the action server times out
-   * Such as setting the error code ID status to timed out for action clients.
-   */
-  void on_timeout() override;
-
   /**
    * \brief Override required by the a BT action. Cancel the action and set the path output
    */
@@ -111,10 +105,6 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
           "planner_id", "",
           "Mapped name to the planner plugin type to use"),
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The compute path to pose error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The compute path to pose error msg"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp

@@ -72,12 +72,6 @@ class ComputeRouteAction : public BtActionNode<nav2_msgs::action::ComputeRoute>
    */
   BT::NodeStatus on_cancelled() override;
 
-  /**
-   * @brief Function to perform work in a BT Node when the action server times out
-   * Such as setting the error code ID status to timed out for action clients.
-   */
-  void on_timeout() override;
-
   /**
    * \brief Override required by the a BT action. Cancel the action and set the path output
    */
@@ -114,10 +108,6 @@ class ComputeRouteAction : public BtActionNode<nav2_msgs::action::ComputeRoute>
           "planning_time",
           "Time taken to compute route"),
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputeRoute node"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The compute route error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The compute route error msg"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -69,10 +69,6 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
         BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
-        BT::OutputPort<Action::Result::_error_code_type>(
-          "error_code_id", "The drive on heading behavior server error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The drive on heading behavior server error msg"),
       });
   }
 
@@ -95,12 +91,6 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
    * @brief Function to perform some user-defined operation upon cancellation of the action
    */
   BT::NodeStatus on_cancelled() override;
-
-  /**
-   * @brief Function to perform work in a BT Node when the action server times out
-   * Such as setting the error code ID status to timed out for action clients.
-   */
-  void on_timeout() override;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_object_action.hpp

@@ -90,10 +90,6 @@ class FollowObjectAction
 
         BT::OutputPort<ActionResult::_total_elapsed_time_type>(
           "total_elapsed_time", "Total elapsed time"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "Error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "Error message"),
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -111,10 +111,6 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<std::string>("path_handler_id", ""),
         BT::OutputPort<nav2_msgs::msg::TrackingFeedback>("tracking_feedback",
           "Tracking feedback from controller server"),
-        BT::OutputPort<ActionResult::_error_code_type>(
-          "error_code_id", "The follow path error code"),
-        BT::OutputPort<std::string>(
-          "error_msg", "The follow path error msg"),
       });
   }
 };