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

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

(cherry picked from commit 64e934f)

Co-authored-by: Ashwin Sajith Nambiar <[email protected]>

Author: mergify[bot]

moveit_plugins/moveit_ros_control_interface/README.md

@@ -22,10 +22,12 @@ In your MoveIt launch file (e.g. `ROBOT_moveit_config/launch/ROBOT_moveit_contro
 <param name="moveit_controller_manager" value="moveit_ros_control_interface::Ros2ControlManager" />
 ```
 
-And 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.
-If you are using the `moveit_setup_assistent` you can add it to `ROBOT_moveit_config/config/moveit_controllers.yaml`, e.g.:
+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

@@ -56,11 +56,11 @@
 
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.plugins.ros_control_interface");
 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;
+
 /**
  * \brief Get joint name from resource name reported by ros2_control, since claimed_interfaces return by ros2_control
  * will have the interface name as suffix joint_name/INTERFACE_TYPE
@@ -124,6 +124,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;
 
@@ -181,10 +182,10 @@ 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(LOGGER, "Failed to read controllers from " << list_controllers_service_->get_service_name()
-                                                                    << " within " << SERVICE_CALL_TIMEOUT
+                                                                    << " within " << service_call_timeout_.count()
                                                                     << " seconds");
       return;
     }
@@ -307,6 +308,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(LOGGER, "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>(
@@ -523,10 +538,10 @@ 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(LOGGER, "Couldn't switch controllers at " << switch_controller_service_->get_service_name()
-                                                                      << " within " << SERVICE_CALL_TIMEOUT
+                                                                      << " within " << service_call_timeout_.count()
                                                                       << " seconds");
         return false;
       }