Merge branch 'master' into allow-prepare-comand-mode-switch-only-for-…

…existing-and-available-interfaces

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -115,7 +115,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions().enable_logger_service(true));
 
   /// Custom configure method to read additional parameters for controller-nodes

controller_interface/src/controller_interface_base.cpp

@@ -26,10 +26,11 @@ namespace controller_interface
 {
 return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-  const std::string & namespace_, const rclcpp::NodeOptions & node_options)
+  const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    controller_name, namespace_, node_options, false);  // disable LifecycleNode service interfaces
+    controller_name, node_namespace, node_options,
+    false);  // disable LifecycleNode service interfaces
   urdf_ = urdf;
 
   try

controller_interface/test/test_controller_with_options.hpp

@@ -41,13 +41,13 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
 
   controller_interface::return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)
         .automatically_declare_parameters_from_overrides(true)) override
   {
-    ControllerInterface::init(controller_name, urdf, cm_update_rate, namespace_, node_options);
+    ControllerInterface::init(controller_name, urdf, cm_update_rate, node_namespace, node_options);
 
     switch (on_init())
     {

controller_manager/controller_manager/controller_manager_services.py

@@ -47,68 +47,89 @@ 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
+        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
+        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
+        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,
+        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,
+        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
+        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,
+        service_timeout,
     )
 
 
-def set_hardware_component_state(node, controller_manager_name, component_name, lifecyle_state):
+def set_hardware_component_state(
+    node, controller_manager_name, component_name, lifecyle_state, service_timeout=10.0
+):
     request = SetHardwareComponentState.Request()
     request.name = component_name
     request.target_state = lifecyle_state
@@ -117,6 +138,7 @@ def set_hardware_component_state(node, controller_manager_name, component_name,
         f"{controller_manager_name}/set_hardware_component_state",
         SetHardwareComponentState,
         request,
+        service_timeout,
     )
 
 
@@ -143,9 +165,13 @@ def switch_controllers(
     )
 
 
-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
+        node,
+        f"{controller_manager_name}/unload_controller",
+        UnloadController,
+        request,
+        service_timeout,
     )

controller_manager/include/controller_manager/controller_manager.hpp

@@ -71,14 +71,14 @@ class ControllerManager : public rclcpp::Node
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
   CONTROLLER_MANAGER_PUBLIC
   ControllerManager(
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & options = get_cm_node_options());
 
   CONTROLLER_MANAGER_PUBLIC

controller_manager/src/controller_manager.cpp

@@ -255,8 +255,8 @@ rclcpp::NodeOptions get_cm_node_options()
 
 ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
-  const std::string & namespace_, const rclcpp::NodeOptions & options)
-: rclcpp::Node(manager_node_name, namespace_, options),
+  const std::string & node_namespace, const rclcpp::NodeOptions & options)
+: rclcpp::Node(manager_node_name, node_namespace, options),
   resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
     update_rate_, this->get_node_clock_interface())),
   diagnostics_updater_(this),
@@ -286,19 +286,19 @@ ControllerManager::ControllerManager(
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
     init_resource_manager(robot_description_);
+    init_services();
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-  init_services();
 }
 
 ControllerManager::ControllerManager(
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
-  const std::string & namespace_, const rclcpp::NodeOptions & options)
-: rclcpp::Node(manager_node_name, namespace_, options),
+  const std::string & node_namespace, const rclcpp::NodeOptions & options)
+: rclcpp::Node(manager_node_name, node_namespace, options),
   resource_manager_(std::move(resource_manager)),
   diagnostics_updater_(this),
   executor_(executor),
@@ -313,12 +313,15 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
+  if (resource_manager_->is_urdf_already_loaded())
+  {
+    init_services();
+  }
   subscribe_to_robot_description_topic();
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-  init_services();
 }
 
 void ControllerManager::subscribe_to_robot_description_topic()
@@ -352,6 +355,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       return;
     }
     init_resource_manager(robot_description_);
+    init_services();
   }
   catch (std::runtime_error & e)
   {

controller_manager/test/test_controller_failed_init/test_controller_failed_init.cpp

@@ -33,7 +33,7 @@ TestControllerFailedInit::on_init()
 
 controller_interface::return_type TestControllerFailedInit::init(
   const std::string & /* controller_name */, const std::string & /* urdf */,
-  unsigned int /*cm_update_rate*/, const std::string & /*namespace_*/,
+  unsigned int /*cm_update_rate*/, const std::string & /*node_namespace*/,
   const rclcpp::NodeOptions & /*node_options*/)
 {
   return controller_interface::return_type::ERROR;

controller_manager/test/test_controller_failed_init/test_controller_failed_init.hpp

@@ -41,7 +41,7 @@ class TestControllerFailedInit : public controller_interface::ControllerInterfac
   CONTROLLER_INTERFACE_PUBLIC
   controller_interface::return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
-    const std::string & namespace_ = "",
+    const std::string & node_namespace = "",
     const rclcpp::NodeOptions & node_options =
       rclcpp::NodeOptions()
         .allow_undeclared_parameters(true)

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -54,9 +54,9 @@ class TestableControllerManager : public controller_manager::ControllerManager
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "")
+    const std::string & node_namespace = "")
   : controller_manager::ControllerManager(
-      std::move(resource_manager), executor, manager_node_name, namespace_)
+      std::move(resource_manager), executor, manager_node_name, node_namespace)
   {
   }
 };

controller_manager/test/test_controller_manager_urdf_passing.cpp

@@ -40,9 +40,9 @@ class TestableControllerManager : public controller_manager::ControllerManager
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "")
+    const std::string & node_namespace = "")
   : controller_manager::ControllerManager(
-      std::move(resource_manager), executor, manager_node_name, namespace_)
+      std::move(resource_manager), executor, manager_node_name, node_namespace)
   {
   }
 };

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -94,9 +94,9 @@ class TestableControllerManager : public controller_manager::ControllerManager
     std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
     std::shared_ptr<rclcpp::Executor> executor,
     const std::string & manager_node_name = "controller_manager",
-    const std::string & namespace_ = "")
+    const std::string & node_namespace = "")
   : controller_manager::ControllerManager(
-      std::move(resource_manager), executor, manager_node_name, namespace_)
+      std::move(resource_manager), executor, manager_node_name, node_namespace)
   {
   }
 };

controller_manager/test/test_hardware_management_srvs.cpp

@@ -84,7 +84,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    cm_->init_resource_manager(robot_description);
+    auto msg = std_msgs::msg::String();
+    msg.data = robot_description_;
+    cm_->robot_description_callback(msg);
 
     SetUpSrvsCMExecutor();
   }
@@ -383,7 +385,9 @@ class TestControllerManagerHWManagementSrvsWithoutParams
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    cm_->init_resource_manager(robot_description);
+    auto msg = std_msgs::msg::String();
+    msg.data = robot_description_;
+    cm_->robot_description_callback(msg);
 
     SetUpSrvsCMExecutor();
   }
@@ -440,7 +444,9 @@ class TestControllerManagerHWManagementSrvsOldParameters
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    cm_->init_resource_manager(robot_description);
+    auto msg = std_msgs::msg::String();
+    msg.data = robot_description_;
+    cm_->robot_description_callback(msg);
 
     SetUpSrvsCMExecutor();
   }

hardware_interface/include/hardware_interface/component_parser.hpp

@@ -33,8 +33,5 @@ namespace hardware_interface
 HARDWARE_INTERFACE_PUBLIC
 std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string & urdf);
 
-HARDWARE_INTERFACE_PUBLIC
-bool parse_bool(const std::string & bool_string);
-
 }  // namespace hardware_interface
 #endif  // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_

hardware_interface/include/hardware_interface/lexical_casts.hpp

@@ -0,0 +1,52 @@
+// Copyright 2023 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.
+
+#ifndef HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_
+#define HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+namespace hardware_interface
+{
+
+/** \brief Helper function to convert a std::string to double in a locale-independent way.
+ \throws std::invalid_argument if not a valid number
+ * 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;
+}
+
+bool parse_bool(const std::string & bool_string)
+{
+  return bool_string == "true" || bool_string == "True";
+}
+
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__LEXICAL_CASTS_HPP_