From 012e5926b64601883523e77cf08a9ba876a4cfaa Mon Sep 17 00:00:00 2001 From: marqrazz Date: Fri, 19 Apr 2024 10:52:49 -0600 Subject: [PATCH 1/2] Action/Server timeout option through ports --- .../behaviortree_ros2/bt_action_node.hpp | 39 ++++++++++++++++-- .../behaviortree_ros2/bt_service_node.hpp | 41 ++++++++++++++++--- 2 files changed, 71 insertions(+), 9 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index e125afc..e6db949 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -105,7 +105,9 @@ class RosActionNode : public BT::ActionNodeBase static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { - InputPort("action_name", "__default__placeholder__", "Action server name") + InputPort("action_name", "__default__placeholder__", "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; @@ -170,8 +172,8 @@ class RosActionNode : public BT::ActionNodeBase std::shared_ptr node_; 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_; + std::chrono::milliseconds server_timeout_; + std::chrono::milliseconds wait_for_server_timeout_; private: @@ -203,13 +205,42 @@ template inline 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(node_->get_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(node_->get_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 67a6750..1e9c550 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -94,7 +94,9 @@ class RosServiceNode : public BT::ActionNodeBase static PortsList providedBasicPorts(PortsList addition) { PortsList basic = { - InputPort("service_name", "__default__placeholder__", "Service name") + InputPort("service_name", "__default__placeholder__", "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; @@ -133,7 +135,7 @@ class RosServiceNode : public BT::ActionNodeBase * It must return either SUCCESS or FAILURE. */ virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/) - { + { return NodeStatus::FAILURE; } @@ -142,8 +144,8 @@ class RosServiceNode : public BT::ActionNodeBase std::shared_ptr node_; 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_; + std::chrono::milliseconds service_timeout_; + std::chrono::milliseconds wait_for_service_timeout_; private: @@ -174,8 +176,37 @@ template inline 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(node_->get_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(node_->get_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; From 293bef6d3e1a5ae634c18ef81c59ef6240781014 Mon Sep 17 00:00:00 2001 From: marqrazz Date: Sun, 5 May 2024 08:30:05 -0600 Subject: [PATCH 2/2] fix ci --- .../behaviortree_ros2/bt_action_node.hpp | 36 ++++++++++++------- .../behaviortree_ros2/bt_service_node.hpp | 36 ++++++++++++------- 2 files changed, 46 insertions(+), 26 deletions(-) diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp index a79d1a9..991378f 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp @@ -108,11 +108,13 @@ class RosActionNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { - InputPort("action_name", "__default__placeholder__", "Action server name"), - InputPort("server_timeout", "Action server goal timeout (mSec)"), - InputPort("wait_for_server_timeout", "Action server discovery timeout (mSec)") - }; + PortsList basic = { InputPort("action_name", "__default__placeholder__", + "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; } @@ -265,12 +267,16 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, { int timeout = 0; getInput("server_timeout", timeout); - if(timeout > 0) { + if(timeout > 0) + { server_timeout_ = std::chrono::milliseconds(timeout); } - else { - RCLCPP_WARN(node_->get_logger(), "%s: Port `server_timeout` is not greater than zero. " - "Defaulting to %d mSec.", name().c_str(), static_cast(server_timeout_.count())); + 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 @@ -279,12 +285,16 @@ inline RosActionNode::RosActionNode(const std::string& instance_name, { int timeout = 0; getInput("wait_for_server_timeout", timeout); - if(timeout > 0) { + if(timeout > 0) + { wait_for_server_timeout_ = std::chrono::milliseconds(timeout); } - else { - RCLCPP_WARN(node_->get_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())); + 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())); } } diff --git a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp index 3e49833..05e9d8d 100644 --- a/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp +++ b/behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp @@ -96,11 +96,13 @@ class RosServiceNode : public BT::ActionNodeBase */ static PortsList providedBasicPorts(PortsList addition) { - PortsList basic = { - InputPort("service_name", "__default__placeholder__", "Service name"), - InputPort("server_timeout", "Service server goal timeout (mSec)"), - InputPort("wait_for_server_timeout", "Service server discovery timeout (mSec)") - }; + PortsList basic = { InputPort("service_name", "__default__placeholder__", + "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; } @@ -236,12 +238,16 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, { int timeout = 0; getInput("server_timeout", timeout); - if(timeout > 0) { + if(timeout > 0) + { service_timeout_ = std::chrono::milliseconds(timeout); } - else { - RCLCPP_WARN(node_->get_logger(), "%s: Port `server_timeout` is not greater than zero. " - "Defaulting to %d mSec.", name().c_str(), static_cast(service_timeout_.count())); + 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 @@ -250,12 +256,16 @@ inline RosServiceNode::RosServiceNode(const std::string& instance_name, { int timeout = 0; getInput("wait_for_server_timeout", timeout); - if(timeout > 0) { + if(timeout > 0) + { wait_for_service_timeout_ = std::chrono::milliseconds(timeout); } - else { - RCLCPP_WARN(node_->get_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())); + 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())); } }