diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index d9c0a9b..e12f62a 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -171,6 +171,7 @@ class RosActionNode : public BT::ActionNodeBase std::string prev_action_name_; bool action_name_may_change_ = false; const std::chrono::milliseconds server_timeout_; + const std::chrono::milliseconds wait_for_server_timeout_; private: @@ -199,7 +200,8 @@ template inline const RosNodeParams ¶ms): BT::ActionNodeBase(instance_name, conf), node_(params.nh), - server_timeout_(params.server_timeout) + server_timeout_(params.server_timeout), + wait_for_server_timeout_(params.wait_for_server_timeout) { // Three cases: // - we use the default action_name in RosNodeParams when port is empty @@ -260,7 +262,7 @@ template inline prev_action_name_ = action_name; - bool found = action_client_->wait_for_action_server(server_timeout_); + bool found = action_client_->wait_for_action_server(wait_for_server_timeout_); if(!found) { RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.", diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 7b97214..66477e3 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -143,6 +143,7 @@ class RosServiceNode : public BT::ActionNodeBase std::string prev_service_name_; bool service_name_may_change_ = false; const std::chrono::milliseconds service_timeout_; + const std::chrono::milliseconds wait_for_service_timeout_; private: @@ -170,7 +171,8 @@ template inline const RosNodeParams& params): BT::ActionNodeBase(instance_name, conf), node_(params.nh), - service_timeout_(params.server_timeout) + service_timeout_(params.server_timeout), + wait_for_service_timeout_(params.wait_for_server_timeout) { // check port remapping auto portIt = config().input_ports.find("service_name"); @@ -225,7 +227,7 @@ template inline service_client_ = node_->create_client(service_name, rmw_qos_profile_services_default, callback_group_); prev_service_name_ = service_name; - bool found = service_client_->wait_for_service(service_timeout_); + bool found = service_client_->wait_for_service(wait_for_service_timeout_); if(!found) { RCLCPP_ERROR(node_->get_logger(), "%s: Service with name '%s' is not reachable.", diff --git a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp index 28dd7c7..cfe49cd 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/ros_node_params.hpp @@ -34,8 +34,9 @@ struct RosNodeParams // - RosTopicSubNode: name of the topic to subscribe to std::string default_port_value; - // parameter used only by service client and action clients + // parameters used only by service client and action clients std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000); + std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500); }; }