action timeout in BT client edits error code and string

nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp

@@ -187,6 +187,15 @@
     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 @@
             "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 @@
               "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;
           }
         }

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
    */

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
    */

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

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
    */

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
    */

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
    */

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

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

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

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

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

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
    */

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
    */

nav2_behavior_tree/plugins/action/assisted_teleop_action.cpp

@@ -69,6 +69,12 @@
   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"

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -78,6 +78,12 @@
   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"

nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp

@@ -75,6 +75,12 @@
   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*/)
 {

nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp

@@ -66,6 +66,12 @@
   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;

nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

@@ -82,6 +82,12 @@
   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;

nav2_behavior_tree/plugins/action/compute_route_action.cpp

@@ -87,6 +87,12 @@
   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();

nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp

@@ -76,6 +76,12 @@
   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"

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -58,6 +58,12 @@
   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*/)
 {

nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp

@@ -61,6 +61,11 @@
   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
 

nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp

@@ -60,6 +60,12 @@
   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"

nav2_behavior_tree/plugins/action/smooth_path_action.cpp

@@ -65,6 +65,12 @@
   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"

nav2_behavior_tree/plugins/action/spin_action.cpp

@@ -68,6 +68,12 @@
   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"

nav2_behavior_tree/plugins/action/wait_action.cpp

@@ -73,6 +73,12 @@
   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"

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
    */

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
    */