diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index be892de..991378f 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -109,7 +109,12 @@ class RosActionNode : public BT::ActionNodeBase static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { InputPort("action_name", "__default__placeholder__", - "Action server name") }; + "Action server name"), + InputPort("server_timeout", "Action server goal timeout " + "(mSec)"), + InputPort("wait_for_server_timeout", "Action server " + "discovery timeout " + "(mSec)") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -217,8 +222,8 @@ class RosActionNode : public BT::ActionNodeBase std::shared_ptr client_instance_; std::string action_name_; bool action_name_may_change_ = false; - const std::chrono::milliseconds server_timeout_; - const std::chrono::milliseconds wait_for_server_timeout_; + std::chrono::milliseconds server_timeout_; + std::chrono::milliseconds wait_for_server_timeout_; std::string action_client_key_; private: @@ -256,13 +261,50 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, , server_timeout_(params.server_timeout) , wait_for_server_timeout_(params.wait_for_server_timeout) { + // update server_timeout_ if set throuh port and greater than 0 + auto portIt = config().input_ports.find("server_timeout"); + if(portIt != config().input_ports.end()) + { + int timeout = 0; + getInput("server_timeout", timeout); + if(timeout > 0) + { + server_timeout_ = std::chrono::milliseconds(timeout); + } + else + { + RCLCPP_WARN(logger(), + "%s: Port `server_timeout` is not greater than zero. " + "Defaulting to %d mSec.", + name().c_str(), static_cast(server_timeout_.count())); + } + } + // update wait_for_server_timeout_ if set throuh port and greater than 0 + portIt = config().input_ports.find("wait_for_server_timeout"); + if(portIt != config().input_ports.end()) + { + int timeout = 0; + getInput("wait_for_server_timeout", timeout); + if(timeout > 0) + { + wait_for_server_timeout_ = std::chrono::milliseconds(timeout); + } + else + { + RCLCPP_WARN(logger(), + "%s: Port `wait_for_server_timeout` is not greater than zero. " + "Defaulting to %d mSec.", + name().c_str(), static_cast(wait_for_server_timeout_.count())); + } + } + // Three cases: // - we use the default action_name in RosNodeParams when port is empty // - we use the action_name in the port and it is a static string. // - we use the action_name in the port and it is blackboard entry. // check port remapping - auto portIt = config().input_ports.find("action_name"); + portIt = config().input_ports.find("action_name"); if(portIt != config().input_ports.end()) { const std::string& bb_action_name = portIt->second; diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 93e14ea..05e9d8d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -97,7 +97,12 @@ class RosServiceNode : public BT::ActionNodeBase static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { InputPort("service_name", "__default__placeholder__", - "Service name") }; + "Service name"), + InputPort("server_timeout", "Service server goal timeout " + "(mSec)"), + InputPort("wait_for_server_timeout", "Service server " + "discovery timeout " + "(mSec)") }; basic.insert(addition.begin(), addition.end()); return basic; } @@ -187,8 +192,8 @@ class RosServiceNode : public BT::ActionNodeBase std::weak_ptr node_; std::string service_name_; bool service_name_may_change_ = false; - const std::chrono::milliseconds service_timeout_; - const std::chrono::milliseconds wait_for_service_timeout_; + std::chrono::milliseconds service_timeout_; + std::chrono::milliseconds wait_for_service_timeout_; private: std::shared_ptr srv_instance_; @@ -227,8 +232,45 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, , service_timeout_(params.server_timeout) , wait_for_service_timeout_(params.wait_for_server_timeout) { + // update service_timeout_ if set throuh port and greater than 0 + auto portIt = config().input_ports.find("server_timeout"); + if(portIt != config().input_ports.end()) + { + int timeout = 0; + getInput("server_timeout", timeout); + if(timeout > 0) + { + service_timeout_ = std::chrono::milliseconds(timeout); + } + else + { + RCLCPP_WARN(logger(), + "%s: Port `server_timeout` is not greater than zero. " + "Defaulting to %d mSec.", + name().c_str(), static_cast(service_timeout_.count())); + } + } + // update wait_for_service_timeout_ if set throuh port and greater than 0 + portIt = config().input_ports.find("wait_for_server_timeout"); + if(portIt != config().input_ports.end()) + { + int timeout = 0; + getInput("wait_for_server_timeout", timeout); + if(timeout > 0) + { + wait_for_service_timeout_ = std::chrono::milliseconds(timeout); + } + else + { + RCLCPP_WARN(logger(), + "%s: Port `wait_for_server_timeout` is not greater than zero. " + "Defaulting to %d mSec.", + name().c_str(), static_cast(wait_for_service_timeout_.count())); + } + } + // check port remapping - auto portIt = config().input_ports.find("service_name"); + portIt = config().input_ports.find("service_name"); if(portIt != config().input_ports.end()) { const std::string& bb_service_name = portIt->second;