Parametrizing service execution timeout (ros2#340)

* Parametrizing service execution timeout

Signed-off-by: Marco Bassa <marco@robotcloud.eu>
Signed-off-by: Tommy Persson <tommy.persson@liu.se>

include/ros1_bridge/factory.hpp

@@ -294,7 +294,8 @@ class ServiceFactory : public ServiceFactoryInterface
 
   bool forward_1_to_2(
     rclcpp::ClientBase::SharedPtr cli, rclcpp::Logger logger,
-    const ROS1Request & request1, ROS1Response & response1)
+    const ROS1Request & request1, ROS1Response & response1,
+    int service_execution_timeout)
   {
     auto client = std::dynamic_pointer_cast<rclcpp::Client<ROS2_T>>(cli);
     if (!client) {
@@ -311,28 +312,31 @@ class ServiceFactory : public ServiceFactoryInterface
       }
       RCLCPP_WARN(logger, "Waiting for ROS 2 service %s...", cli->get_service_name());
     }
-    auto timeout = std::chrono::seconds(5);
+    auto timeout = std::chrono::seconds(service_execution_timeout);
     auto future = client->async_send_request(request2);
     auto status = future.wait_for(timeout);
     if (status == std::future_status::ready) {
       auto response2 = future.get();
       translate_2_to_1(*response2, response1);
     } else {
-      RCLCPP_ERROR(logger, "Failed to get response from ROS 2 service %s", cli->get_service_name());
+      RCLCPP_ERROR(
+        logger, "Failed to get response from ROS 2 service %s within %d seconds",
+        cli->get_service_name(), service_execution_timeout);
       return false;
     }
     return true;
   }
 
   ServiceBridge1to2 service_bridge_1_to_2(
-    ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name)
+    ros::NodeHandle & ros1_node, rclcpp::Node::SharedPtr ros2_node, const std::string & name,
+    int service_execution_timeout)
   {
     ServiceBridge1to2 bridge;
     bridge.client = ros2_node->create_client<ROS2_T>(name);
     auto m = &ServiceFactory<ROS1_T, ROS2_T>::forward_1_to_2;
     auto f = std::bind(
       m, this, bridge.client, ros2_node->get_logger(), std::placeholders::_1,
-      std::placeholders::_2);
+      std::placeholders::_2, service_execution_timeout);
     bridge.server = ros1_node.advertiseService<ROS1Request, ROS1Response>(name, f);
     return bridge;
   }

include/ros1_bridge/factory_interface.hpp

@@ -123,7 +123,7 @@ class ServiceFactoryInterface
 {
 public:
   virtual ServiceBridge1to2 service_bridge_1_to_2(
-    ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0;
+    ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &, int) = 0;
 
   virtual ServiceBridge2to1 service_bridge_2_to_1(
     ros::NodeHandle &, rclcpp::Node::SharedPtr, const std::string &) = 0;

src/dynamic_bridge.cpp

@@ -327,6 +327,10 @@ void update_bridge(
     }
   }
 
+  int service_execution_timeout{5};
+  ros1_node.getParamCached(
+    "ros1_bridge/dynamic_bridge/service_execution_timeout", service_execution_timeout);
+
   // create bridges for ros2 services
   for (auto & service : ros2_services) {
     auto & name = service.first;
@@ -339,7 +343,8 @@ void update_bridge(
         "ros2", details.at("package"), details.at("name"));
       if (factory) {
         try {
-          service_bridges_1_to_2[name] = factory->service_bridge_1_to_2(ros1_node, ros2_node, name);
+          service_bridges_1_to_2[name] = factory->service_bridge_1_to_2(
+            ros1_node, ros2_node, name, service_execution_timeout);
           printf("Created 1 to 2 bridge for service %s\n", name.data());
         } catch (std::runtime_error & e) {
           fprintf(stderr, "Failed to created a bridge: %s\n", e.what());

src/parameter_bridge.cpp

@@ -58,6 +58,8 @@ int main(int argc, char * argv[])
   // type: the type of the service to bridge (e.g. 'pkgname/srv/SrvName')
   const char * services_1_to_2_parameter_name = "services_1_to_2";
   const char * services_2_to_1_parameter_name = "services_2_to_1";
+  const char * service_execution_timeout_parameter_name =
+    "ros1_bridge/parameter_bridge/service_execution_timeout";
   if (argc > 1) {
     topics_parameter_name = argv[1];
   }
@@ -110,6 +112,9 @@ int main(int argc, char * argv[])
     ros1_node.getParam(services_1_to_2_parameter_name, services_1_to_2) &&
     services_1_to_2.getType() == XmlRpc::XmlRpcValue::TypeArray)
   {
+    int service_execution_timeout{5};
+    ros1_node.getParamCached(
+      service_execution_timeout_parameter_name, service_execution_timeout);
     for (size_t i = 0; i < static_cast<size_t>(services_1_to_2.size()); ++i) {
       std::string service_name = static_cast<std::string>(services_1_to_2[i]["service"]);
       std::string type_name = static_cast<std::string>(services_1_to_2[i]["type"]);
@@ -143,7 +148,7 @@ int main(int argc, char * argv[])
         try {
           service_bridges_1_to_2.push_back(
             factory->service_bridge_1_to_2(
-              ros1_node, ros2_node, service_name));
+              ros1_node, ros2_node, service_name, service_execution_timeout));
           printf("Created 1 to 2 bridge for service %s\n", service_name.c_str());
         } catch (std::runtime_error & e) {
           fprintf(