Action/Server timeout option through ports

Author: MarqRazz

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<std::string>("action_name", "__default__placeholder__", "Action server name")
+      InputPort<std::string>("action_name", "__default__placeholder__", "Action server name"),
+      InputPort<int>("server_timeout", "Action server goal timeout (mSec)"),
+      InputPort<int>("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<rclcpp::Node> 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<class T> 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<int>(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<int>(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;

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<std::string>("service_name", "__default__placeholder__", "Service name")
+      InputPort<std::string>("service_name", "__default__placeholder__", "Service name"),
+      InputPort<int>("server_timeout", "Service server goal timeout (mSec)"),
+      InputPort<int>("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<rclcpp::Node> 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<class T> 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<int>(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<int>(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;