Merge branch 'humble' into init_srvs_after_resource_manager_init_humble

controller_manager/controller_manager/controller_manager_services.py

@@ -37,51 +37,84 @@ def service_caller(node, service_name, service_type, request, service_timeout=10
         raise RuntimeError(f'Exception while calling service: {future.exception()}')
 
 
-def configure_controller(node, controller_manager_name, controller_name):
+def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
     request = ConfigureController.Request()
     request.name = controller_name
-    return service_caller(node, f'{controller_manager_name}/configure_controller',
-                          ConfigureController, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/configure_controller",
+        ConfigureController,
+        request,
+        service_timeout,
+    )
 
 
-def list_controllers(node, controller_manager_name):
+def list_controllers(node, controller_manager_name, service_timeout=10.0):
     request = ListControllers.Request()
-    return service_caller(node, f'{controller_manager_name}/list_controllers',
-                          ListControllers, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/list_controllers",
+        ListControllers,
+        request,
+        service_timeout,
+    )
 
 
-def list_controller_types(node, controller_manager_name):
+def list_controller_types(node, controller_manager_name, service_timeout=10.0):
     request = ListControllerTypes.Request()
-    return service_caller(node,
-                          f'{controller_manager_name}/list_controller_types',
-                          ListControllerTypes, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/list_controller_types",
+        ListControllerTypes,
+        request,
+        service_timeout,
+    )
 
 
-def list_hardware_components(node, controller_manager_name):
+def list_hardware_components(node, controller_manager_name, service_timeout=10.0):
     request = ListHardwareComponents.Request()
-    return service_caller(node, f'{controller_manager_name}/list_hardware_components',
-                          ListHardwareComponents, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/list_hardware_components",
+        ListHardwareComponents,
+        request,
+        service_timeout,
+    )
 
 
-def list_hardware_interfaces(node, controller_manager_name):
+def list_hardware_interfaces(node, controller_manager_name, service_timeout=10.0):
     request = ListHardwareInterfaces.Request()
-    return service_caller(node, f'{controller_manager_name}/list_hardware_interfaces',
-                          ListHardwareInterfaces, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/list_hardware_interfaces",
+        ListHardwareInterfaces,
+        request,
+        service_timeout,
+    )
 
 
-def load_controller(node, controller_manager_name, controller_name):
+def load_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
     request = LoadController.Request()
     request.name = controller_name
-    return service_caller(node, f'{controller_manager_name}/load_controller',
-                          LoadController, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/load_controller",
+        LoadController,
+        request,
+        service_timeout,
+    )
 
 
-def reload_controller_libraries(node, controller_manager_name, force_kill):
+def reload_controller_libraries(node, controller_manager_name, force_kill, service_timeout=10.0):
     request = ReloadControllerLibraries.Request()
     request.force_kill = force_kill
-    return service_caller(node,
-                          f'{controller_manager_name}/reload_controller_libraries',
-                          ReloadControllerLibraries, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/reload_controller_libraries",
+        ReloadControllerLibraries,
+        request,
+        service_timeout,
+    )
 
 
 def switch_controllers(node, controller_manager_name, deactivate_controllers,
@@ -99,8 +132,13 @@ def switch_controllers(node, controller_manager_name, deactivate_controllers,
                           SwitchController, request)
 
 
-def unload_controller(node, controller_manager_name, controller_name):
+def unload_controller(node, controller_manager_name, controller_name, service_timeout=10.0):
     request = UnloadController.Request()
     request.name = controller_name
-    return service_caller(node, f'{controller_manager_name}/unload_controller',
-                          UnloadController, request)
+    return service_caller(
+        node,
+        f"{controller_manager_name}/unload_controller",
+        UnloadController,
+        request,
+        service_timeout,
+    )

hardware_interface/CMakeLists.txt

@@ -29,6 +29,7 @@ add_library(
   src/resource_manager.cpp
   src/sensor.cpp
   src/system.cpp
+  src/lexical_casts.cpp
 )
 target_include_directories(
   ${PROJECT_NAME}

hardware_interface/include/hardware_interface/lexical_casts.hpp

@@ -28,24 +28,9 @@ namespace hardware_interface
  * from
  https://github.com/ros-planning/srdfdom/blob/ad17b8d25812f752c397a6011cec64aeff090c46/src/model.cpp#L53
 */
-double stod(const std::string & s)
-{
-  // convert from string using no locale
-  std::istringstream stream(s);
-  stream.imbue(std::locale::classic());
-  double result;
-  stream >> result;
-  if (stream.fail() || !stream.eof())
-  {
-    throw std::invalid_argument("Failed converting string to real number");
-  }
-  return result;
-}
+double stod(const std::string & s);
 
-bool parse_bool(const std::string & bool_string)
-{
-  return bool_string == "true" || bool_string == "True";
-}
+bool parse_bool(const std::string & bool_string);
 
 }  // namespace hardware_interface
 

hardware_interface/src/lexical_casts.cpp

@@ -0,0 +1,37 @@
+// Copyright 2024 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "hardware_interface/lexical_casts.hpp"
+
+namespace hardware_interface
+{
+double stod(const std::string & s)
+{
+  // convert from string using no locale
+  std::istringstream stream(s);
+  stream.imbue(std::locale::classic());
+  double result;
+  stream >> result;
+  if (stream.fail() || !stream.eof())
+  {
+    throw std::invalid_argument("Failed converting string to real number");
+  }
+  return result;
+}
+
+bool parse_bool(const std::string & bool_string)
+{
+  return bool_string == "true" || bool_string == "True";
+}
+}  // namespace hardware_interface

rqt_controller_manager/rqt_controller_manager/controller_manager.py

@@ -168,17 +168,22 @@ def _list_controllers(self):
         @rtype [str]
         """
         # Add loaded controllers first
-        controllers = list_controllers(self._node, self._cm_name).controller
-
-        # Append potential controller configs found in the node's parameters
-        for name in _get_parameter_controller_names(self._node, self._cm_name):
-            add_ctrl = all(name != ctrl.name for ctrl in controllers)
-            if add_ctrl:
-                type_str = _get_controller_type(self._node, self._cm_name, name)
-                uninit_ctrl = ControllerState(name=name,
-                                              type=type_str)
-                controllers.append(uninit_ctrl)
-        return controllers
+        try:
+            controllers = list_controllers(
+                self._node, self._cm_name, 2.0 / self._cm_update_freq
+            ).controller
+
+            # Append potential controller configs found in the node's parameters
+            for name in _get_parameter_controller_names(self._node, self._cm_name):
+                add_ctrl = all(name != ctrl.name for ctrl in controllers)
+                if add_ctrl:
+                    type_str = _get_controller_type(self._node, self._cm_name, name)
+                    uninit_ctrl = ControllerState(name=name, type=type_str)
+                    controllers.append(uninit_ctrl)
+            return controllers
+        except RuntimeError as e:
+            print(e)
+            return []
 
     def _show_controllers(self):
         table_view = self._widget.table_view