action timeout in BT client edits error code and string (#5184)

SteveMacenski · web-flow · commit cb394620d91a · 2025-05-20T18:02:29.000-07:00
Signed-off-by: Steve Macenski &lt;stevenmacenski@gmail.com&gt;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -187,6 +187,15 @@ class BtActionNode : public BT::ActionNodeBase
     return BT::NodeStatus::SUCCESS;
   }
 
+  /**
+   * @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.
+   */
+  virtual void on_timeout()
+  {
+    return;
+  }
+
   /**
    * @brief The main override required by a BT action
    * @return BT::NodeStatus Status of tick execution
@@ -231,6 +240,7 @@ class BtActionNode : public BT::ActionNodeBase
             "Timed out while waiting for action server to acknowledge goal request for %s",
             action_name_.c_str());
           future_goal_handle_.reset();
+          on_timeout();
           return BT::NodeStatus::FAILURE;
         }
       }
@@ -261,6 +271,7 @@ class BtActionNode : public BT::ActionNodeBase
               "Timed out while waiting for action server to acknowledge goal request for %s",
               action_name_.c_str());
             future_goal_handle_.reset();
+            on_timeout();
             return BT::NodeStatus::FAILURE;
           }
         }
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/assisted_teleop_action.hpp
@@ -66,6 +66,12 @@ 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
    */
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -66,6 +66,12 @@ 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
    */
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp
@@ -64,6 +64,12 @@ 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
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp
@@ -69,6 +69,12 @@ class ComputePathThroughPosesAction
    */
   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
    */
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
@@ -66,6 +66,12 @@ 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
    */
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_route_action.hpp
@@ -64,6 +64,12 @@ 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
    */
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp
@@ -88,6 +88,12 @@ 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
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp
@@ -66,6 +66,12 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
    */
   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
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp
@@ -67,6 +67,12 @@ class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::Naviga
    */
   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 Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp
@@ -67,6 +67,12 @@ class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPo
    */
   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 Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smooth_path_action.hpp
@@ -65,6 +65,12 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
    */
   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 Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
@@ -50,6 +50,12 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
    */
   void on_tick() 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
    */
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_action.hpp
@@ -50,6 +50,12 @@ class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
    */
   void on_tick() 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
    */
diff --git a/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp b/nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp
@@ -69,6 +69,12 @@ BT::NodeStatus AssistedTeleopAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void AssistedTeleopAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp
@@ -78,6 +78,12 @@ BT::NodeStatus BackUpAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void BackUpAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp
@@ -75,6 +75,12 @@ BT::NodeStatus ComputeAndTrackRouteAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void ComputeAndTrackRouteAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 void ComputeAndTrackRouteAction::on_wait_for_result(
   std::shared_ptr<const Action::Feedback>/*feedback*/)
 {
diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp
@@ -66,6 +66,12 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void ComputePathThroughPosesAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 void ComputePathThroughPosesAction::halt()
 {
   nav_msgs::msg::Path empty_path;
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -82,6 +82,12 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void ComputePathToPoseAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 void ComputePathToPoseAction::halt()
 {
   nav_msgs::msg::Path empty_path;
diff --git a/nav2_behavior_tree/plugins/action/compute_route_action.cpp b/nav2_behavior_tree/plugins/action/compute_route_action.cpp
@@ -87,6 +87,12 @@ BT::NodeStatus ComputeRouteAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void ComputeRouteAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 void ComputeRouteAction::halt()
 {
   resetPorts();
diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
@@ -76,6 +76,12 @@ BT::NodeStatus DriveOnHeadingAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void DriveOnHeadingAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp
@@ -58,6 +58,12 @@ BT::NodeStatus FollowPathAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void FollowPathAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::CONTROLLER_TIMED_OUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 void FollowPathAction::on_wait_for_result(
   std::shared_ptr<const Action::Feedback>/*feedback*/)
 {
diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp
@@ -61,6 +61,11 @@ BT::NodeStatus NavigateThroughPosesAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void NavigateThroughPosesAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
 
 }  // namespace nav2_behavior_tree
 
diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp
@@ -60,6 +60,12 @@ BT::NodeStatus NavigateToPoseAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void NavigateToPoseAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/smooth_path_action.cpp b/nav2_behavior_tree/plugins/action/smooth_path_action.cpp
@@ -65,6 +65,12 @@ BT::NodeStatus SmoothPathAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void SmoothPathAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp
@@ -68,6 +68,12 @@ BT::NodeStatus SpinAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void SpinAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp
@@ -73,6 +73,12 @@ BT::NodeStatus WaitAction::on_cancelled()
   return BT::NodeStatus::SUCCESS;
 }
 
+void WaitAction::on_timeout()
+{
+  setOutput("error_code_id", ActionResult::TIMEOUT);
+  setOutput("error_msg", "Behavior Tree action client timed out waiting.");
+}
+
 }  // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/dock_robot.hpp
@@ -68,6 +68,12 @@ class DockRobotAction
    */
   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
    */
diff --git a/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp b/nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp
@@ -68,6 +68,12 @@ class UndockRobotAction
    */
   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
    */
diff --git a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp
diff --git a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp
diff --git a/nav2_msgs/action/DockRobot.action b/nav2_msgs/action/DockRobot.action
diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action
diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action
diff --git a/nav2_msgs/action/UndockRobot.action b/nav2_msgs/action/UndockRobot.action
diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action

Original file line number	Diff line number	Diff line change
`@@ -187,6 +187,15 @@ class BtActionNode : public BT::ActionNodeBase`
`187`	`187`	`return BT::NodeStatus::SUCCESS;`
`188`	`188`	`}`
`189`	`189`
	`190`	`+ /**`
	`191`	`+ * @brief Function to perform work in a BT Node when the action server times out`
	`192`	`+ * Such as setting the error code ID status to timed out for action clients.`
	`193`	`+ */`
	`194`	`+ virtual void on_timeout()`
	`195`	`+ {`
	`196`	`+ return;`
	`197`	`+ }`
	`198`	`+`
`190`	`199`	`/**`
`191`	`200`	`* @brief The main override required by a BT action`
`192`	`201`	`* @return BT::NodeStatus Status of tick execution`
`@@ -231,6 +240,7 @@ class BtActionNode : public BT::ActionNodeBase`
`231`	`240`	`"Timed out while waiting for action server to acknowledge goal request for %s",`
`232`	`241`	`action_name_.c_str());`
`233`	`242`	`future_goal_handle_.reset();`
	`243`	`+ on_timeout();`
`234`	`244`	`return BT::NodeStatus::FAILURE;`
`235`	`245`	`}`
`236`	`246`	`}`
`@@ -261,6 +271,7 @@ class BtActionNode : public BT::ActionNodeBase`
`261`	`271`	`"Timed out while waiting for action server to acknowledge goal request for %s",`
`262`	`272`	`action_name_.c_str());`
`263`	`273`	`future_goal_handle_.reset();`
	`274`	`+ on_timeout();`
`264`	`275`	`return BT::NodeStatus::FAILURE;`
`265`	`276`	`}`
`266`	`277`	`}`