BehaviorTree · MarqRazz · Apr 19, 2024 · May 2, 2024 · May 5, 2024
diff --git 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<std::string>("action_name", "__default__placeholder__",
-                                               "Action server name") };
+                                               "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;
   }
@@ -217,8 +222,8 @@ class RosActionNode : public BT::ActionNodeBase
   std::shared_ptr<ActionClientInstance> 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<T>::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<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(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;

diff --git 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<std::string>("service_name", "__default__placeholder__",
-                                               "Service name") };
+                                               "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;
   }
@@ -187,8 +192,8 @@ class RosServiceNode : public BT::ActionNodeBase
   std::weak_ptr<rclcpp::Node> 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<ServiceClientInstance> srv_instance_;
@@ -227,8 +232,45 @@ inline RosServiceNode<T>::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<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(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;