ROS Parameter for service call timeout for ros_control controllers (moveit#3419)

Author: ashwinsnambiar

moveit_plugins/moveit_ros_control_interface/README.md

@@ -23,9 +23,11 @@ In your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_contro
 ```
 
 Make sure to set the `ros_control_namespace` parameter to the namespace (without the /controller_manager/ part) of the ros_control-based node you like to interface.
+Also, the timeout in seconds for the service call to the controller can be changed from the default value, by setting the `controller_service_call_timeout` parameter.
 If you are using `moveit_setup_assistant`, you can add it to `ROBOT_moveit_config/config/moveit_controllers.yaml`, e.g.:
 ```
 ros_control_namespace: /ROS_CONTROL_NODE
+controller_service_call_timeout: 5.0
 controller_list:
   - name: /ROS_CONTROL_NODE/position_trajectory_controller
     action_ns: follow_joint_trajectory

moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp

@@ -57,11 +57,11 @@
 #include <moveit/utils/logger.hpp>
 
 static const rclcpp::Duration CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1.0);
-// TODO: Create a ROS parameter allowing to customize this default timeout
-static constexpr double SERVICE_CALL_TIMEOUT = 3.0;
 
 namespace moveit_ros_control_interface
 {
+static constexpr double DEFAULT_SERVICE_CALL_TIMEOUT = 3.0;
+
 namespace
 {
 rclcpp::Logger getLogger()
@@ -133,6 +133,7 @@ MOVEIT_CLASS_FORWARD(Ros2ControlManager);  // Defines Ros2ControlManagerPtr, Con
 class Ros2ControlManager : public moveit_controller_manager::MoveItControllerManager
 {
   std::string ns_;
+  std::chrono::duration<double> service_call_timeout_;
   pluginlib::ClassLoader<ControllerHandleAllocator> loader_;
   typedef std::map<std::string, controller_manager_msgs::msg::ControllerState> ControllersMap;
 
@@ -192,11 +193,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
 
     auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();
     auto result_future = list_controllers_service_->async_send_request(request);
-    if (result_future.wait_for(std::chrono::duration<double>(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout)
+    if (result_future.wait_for(service_call_timeout_) == std::future_status::timeout)
     {
       RCLCPP_WARN_STREAM(getLogger(), "Failed to read controllers from "
                                           << list_controllers_service_->get_service_name() << " within "
-                                          << SERVICE_CALL_TIMEOUT << " seconds");
+                                          << service_call_timeout_.count() << " seconds");
       return;
     }
 
@@ -318,6 +319,20 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
       }
     }
 
+    double timeout_seconds;
+    if (!node_->has_parameter("controller_service_call_timeout"))
+    {
+      timeout_seconds =
+          node_->declare_parameter<double>("controller_service_call_timeout", DEFAULT_SERVICE_CALL_TIMEOUT);
+    }
+    else
+    {
+      node_->get_parameter("controller_service_call_timeout", timeout_seconds);
+    }
+    service_call_timeout_ = std::chrono::duration<double>(timeout_seconds);
+
+    RCLCPP_INFO_STREAM(getLogger(), "Using service call timeout " << service_call_timeout_.count() << " seconds");
+
     list_controllers_service_ = node_->create_client<controller_manager_msgs::srv::ListControllers>(
         getAbsName("controller_manager/list_controllers"));
     switch_controller_service_ = node_->create_client<controller_manager_msgs::srv::SwitchController>(
@@ -552,11 +567,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
     if (!request->activate_controllers.empty() || !request->deactivate_controllers.empty())
     {  // something to switch?
       auto result_future = switch_controller_service_->async_send_request(request);
-      if (result_future.wait_for(std::chrono::duration<double>(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout)
+      if (result_future.wait_for(service_call_timeout_) == std::future_status::timeout)
       {
         RCLCPP_ERROR_STREAM(getLogger(), "Couldn't switch controllers at "
                                              << switch_controller_service_->get_service_name() << " within "
-                                             << SERVICE_CALL_TIMEOUT << " seconds");
+                                             << service_call_timeout_.count() << " seconds");
         return false;
       }
       discover(true);