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 index 5fbba8bf064..58c1f422b5a 100644 --- 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 index f3f7e0794c0..f4a740abc84 100644 --- 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 */ 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 index c56da81de5d..6d258552a7f 100644 --- 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 */ 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 index 55a30e74c3b..2a2ac3948ca 100644 --- 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 */ 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 index fa175002a15..71c6597683f 100644 --- 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 */ 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 index f4e65a7bf82..866dc130901 100644 --- 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 */ 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 index 2c95dda33a7..3f293f588bf 100644 --- 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 index 5e29107c9e4..d31ff9aecb1 100644 --- 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 index cc6db6fbe8b..eb2edc19b76 100644 --- 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/*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 index 3b778903df5..72a62258599 100644 --- 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 index 41772aca79b..0d80527c913 100644 --- 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 index cc16125c74b..6613d3e7b91 100644 --- 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 index e3a0fc02765..b612c16f86b 100644 --- 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 index 875c1b71c44..e89a23e282a 100644 --- 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/*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 index be6272317ab..3b362d55300 100644 --- 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 index 4bfac9e8829..775314babc4 100644 --- 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 index ebd72cbf137..cb420e0508e 100644 --- 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 index c41d2a97744..f5b1cb07fa9 100644 --- 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 index 4a357fd1b94..8d22313e04b 100644 --- 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 index 0a07e12e7f7..8f4a4afeff2 100644 --- 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 index 8674e477791..d0e78e3611f 100644 --- 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 index be3e58b8c8c..35cea456b4f 100644 --- a/nav2_docking/opennav_docking_bt/src/dock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/dock_robot.cpp @@ -67,6 +67,12 @@ BT::NodeStatus DockRobotAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void DockRobotAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void DockRobotAction::halt() { BtActionNode::halt(); diff --git a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp index 0669bad92bd..400615e6f0d 100644 --- a/nav2_docking/opennav_docking_bt/src/undock_robot.cpp +++ b/nav2_docking/opennav_docking_bt/src/undock_robot.cpp @@ -58,6 +58,12 @@ BT::NodeStatus UndockRobotAction::on_cancelled() return BT::NodeStatus::SUCCESS; } +void UndockRobotAction::on_timeout() +{ + setOutput("error_code_id", ActionResult::TIMEOUT); + setOutput("error_msg", "Behavior Tree action client timed out waiting."); +} + void UndockRobotAction::halt() { BtActionNode::halt(); diff --git a/nav2_msgs/action/DockRobot.action b/nav2_msgs/action/DockRobot.action index e3e2fbf78cb..ae70c33413a 100644 --- a/nav2_msgs/action/DockRobot.action +++ b/nav2_msgs/action/DockRobot.action @@ -21,6 +21,7 @@ uint16 FAILED_TO_STAGE=903 uint16 FAILED_TO_DETECT_DOCK=904 uint16 FAILED_TO_CONTROL=905 uint16 FAILED_TO_CHARGE=906 +uint16 TIMEOUT=907 uint16 UNKNOWN=999 bool success True # docking success status diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 8bbaa361e73..1d7a1951578 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -11,6 +11,7 @@ uint16 NONE=0 uint16 UNKNOWN=9100 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9101 uint16 TF_ERROR=9102 +uint16 TIMEOUT=9103 uint16 error_code string error_msg diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 6da35cee510..14272c5663a 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -10,6 +10,7 @@ uint16 NONE=0 uint16 UNKNOWN=9000 uint16 FAILED_TO_LOAD_BEHAVIOR_TREE=9001 uint16 TF_ERROR=9002 +uint16 TIMEOUT=9003 uint16 error_code string error_msg diff --git a/nav2_msgs/action/UndockRobot.action b/nav2_msgs/action/UndockRobot.action index 61747830607..5bb882a2e41 100644 --- a/nav2_msgs/action/UndockRobot.action +++ b/nav2_msgs/action/UndockRobot.action @@ -16,6 +16,7 @@ float32 max_undocking_time 30.0 # Maximum time to undock uint16 NONE=0 uint16 DOCK_NOT_VALID=902 uint16 FAILED_TO_CONTROL=905 +uint16 TIMEOUT=907 uint16 UNKNOWN=999 bool success True # docking success status diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index 1496e3371ed..c6b20d4f93d 100644 --- a/nav2_msgs/action/Wait.action +++ b/nav2_msgs/action/Wait.action @@ -8,6 +8,7 @@ string error_msg uint16 NONE=0 uint16 UNKNOWN=740 +uint16 TIMEOUT=741 --- #feedback definition builtin_interfaces/Duration time_left