Merge branch 'master' into fix_multiple_cleanup_on_unconfigured_compo…

…nents

.github/workflows/reusable-ros-tooling-source-build.yml

@@ -36,6 +36,7 @@ jobs:
         with:
           target-ros2-distro: ${{ inputs.ros_distro }}
           # build all packages listed in the meta package
+          ref: ${{ inputs.ref }} # otherwise the default branch is used for scheduled workflows
           package-name:
             controller_interface
             controller_manager

.github/workflows/reviewer_lottery.yml

@@ -6,6 +6,7 @@ on:
 jobs:
   test:
     runs-on: ubuntu-latest
+    if: github.actor != 'dependabot[bot]' && github.actor != 'mergify[bot]'
     steps:
     - uses: actions/checkout@v4
     - uses: uesteibar/reviewer-lottery@v3

controller_manager/controller_manager/launch_utils.py

@@ -32,12 +32,12 @@ def generate_load_controller_launch_description(
     Examples # noqa: D416
     --------
       # Assuming the controller type and controller parameters are known to the controller_manager
-      generate_load_controller_launch_description('joint_state_controller')
+      generate_load_controller_launch_description('joint_state_broadcaster')
 
       # Passing controller type and controller parameter file to load
       generate_load_controller_launch_description(
-        'joint_state_controller',
-        controller_type='joint_state_controller/JointStateController',
+        'joint_state_broadcaster',
+        controller_type='joint_state_broadcaster/JointStateBroadcaster',
         controller_params_file=os.path.join(get_package_share_directory('my_pkg'),
                                             'config', 'controller_params.yaml')
         )

controller_manager/src/controller_manager.cpp

@@ -948,7 +948,7 @@ controller_interface::return_type ControllerManager::switch_controller(
     {
       RCLCPP_WARN(
         get_logger(),
-        "Controller with name '%s' is not inactive so its following"
+        "Controller with name '%s' is not inactive so its following "
         "controllers do not have to be checked, because it cannot be activated.",
         controller_it->info.name.c_str());
       status = controller_interface::return_type::ERROR;

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_

hardware_interface/src/component_parser.cpp

@@ -23,6 +23,7 @@
 
 #include "hardware_interface/component_parser.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 
 namespace
 {
@@ -128,26 +129,23 @@ double get_parameter_value_or(
 {
   while (params_it)
   {
-    // Fill the map with parameters
-    const auto tag_name = params_it->Name();
-    if (strcmp(tag_name, parameter_name) == 0)
+    try
     {
-      const auto tag_text = params_it->GetText();
-      if (tag_text)
+      // Fill the map with parameters
+      const auto tag_name = params_it->Name();
+      if (strcmp(tag_name, parameter_name) == 0)
       {
-        // Parse and return double value if there is no parsing error
-        double result_value;
-        const auto parse_result =
-          std::from_chars(tag_text, tag_text + std::strlen(tag_text), result_value);
-        if (parse_result.ec == std::errc())
+        const auto tag_text = params_it->GetText();
+        if (tag_text)
         {
-          return result_value;
+          return hardware_interface::stod(tag_text);
         }
-
-        // Parsing failed - exit loop and return default value
-        break;
       }
     }
+    catch (const std::exception & e)
+    {
+      return default_value;
+    }
 
     params_it = params_it->NextSiblingElement();
   }
@@ -616,9 +614,4 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
   return hardware_info;
 }
 
-bool parse_bool(const std::string & bool_string)
-{
-  return bool_string == "true" || bool_string == "True";
-}
-
 }  // namespace hardware_interface

hardware_interface/src/mock_components/generic_system.cpp

@@ -26,22 +26,12 @@
 #include <vector>
 
 #include "hardware_interface/component_parser.hpp"
+#include "hardware_interface/lexical_casts.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rcutils/logging_macros.h"
 
 namespace mock_components
 {
-double parse_double(const std::string & text)
-{
-  double result_value;
-  const auto parse_result = std::from_chars(text.data(), text.data() + text.size(), result_value);
-  if (parse_result.ec == std::errc())
-  {
-    return result_value;
-  }
-
-  return 0.0;
-}
 
 CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & info)
 {
@@ -123,7 +113,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
   it = info_.hardware_parameters.find("position_state_following_offset");
   if (it != info_.hardware_parameters.end())
   {
-    position_state_following_offset_ = parse_double(it->second);
+    position_state_following_offset_ = hardware_interface::stod(it->second);
     it = info_.hardware_parameters.find("custom_interface_with_following_offset");
     if (it != info_.hardware_parameters.end())
     {
@@ -169,7 +159,7 @@ CallbackReturn GenericSystem::on_init(const hardware_interface::HardwareInfo & i
       auto param_it = joint.parameters.find("multiplier");
       if (param_it != joint.parameters.end())
       {
-        mimic_joint.multiplier = parse_double(joint.parameters.at("multiplier"));
+        mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier"));
       }
       mimic_joints_.push_back(mimic_joint);
     }
@@ -696,7 +686,7 @@ void GenericSystem::initialize_storage_vectors(
         // Check the initial_value param is used
         if (!interface.initial_value.empty())
         {
-          states[index][i] = parse_double(interface.initial_value);
+          states[index][i] = hardware_interface::stod(interface.initial_value);
         }
       }
     }

hardware_interface/test/test_components/test_actuator.cpp

@@ -120,5 +120,15 @@ class TestActuator : public ActuatorInterface
   double max_velocity_command_ = 0.0;
 };
 
+class TestUnitilizableActuator : public TestActuator
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    ActuatorInterface::on_init(info);
+    return CallbackReturn::ERROR;
+  }
+};
+
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(TestActuator, hardware_interface::ActuatorInterface)
+PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuator, hardware_interface::ActuatorInterface)

hardware_interface/test/test_components/test_components.xml

@@ -17,4 +17,22 @@
       Test System
     </description>
   </class>
+
+  <class name="test_unitilizable_actuator" type="TestUnitilizableActuator" base_class_type="hardware_interface::ActuatorInterface">
+    <description>
+      Test Unitilizable Actuator
+    </description>
+  </class>
+
+  <class name="test_unitilizable_sensor" type="TestUnitilizableSensor" base_class_type="hardware_interface::SensorInterface">
+    <description>
+      Test Unitilizable Sensor
+    </description>
+  </class>
+
+  <class name="test_unitilizable_system" type="TestUnitilizableSystem" base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Test Unitilizable System
+    </description>
+  </class>
 </library>

hardware_interface/test/test_components/test_sensor.cpp

@@ -55,5 +55,15 @@ class TestSensor : public SensorInterface
   double velocity_state_ = 0.0;
 };
 
+class TestUnitilizableSensor : public TestSensor
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    SensorInterface::on_init(info);
+    return CallbackReturn::ERROR;
+  }
+};
+
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(TestSensor, hardware_interface::SensorInterface)
+PLUGINLIB_EXPORT_CLASS(TestUnitilizableSensor, hardware_interface::SensorInterface)

hardware_interface/test/test_components/test_system.cpp

@@ -123,5 +123,15 @@ class TestSystem : public SystemInterface
   double configuration_command_ = 0.0;
 };
 
+class TestUnitilizableSystem : public TestSystem
+{
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+  {
+    SystemInterface::on_init(info);
+    return CallbackReturn::ERROR;
+  }
+};
+
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(TestSystem, hardware_interface::SystemInterface)
+PLUGINLIB_EXPORT_CLASS(TestUnitilizableSystem, hardware_interface::SystemInterface)

hardware_interface/test/test_resource_manager.cpp

@@ -69,6 +69,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager
   FRIEND_TEST(ResourceManagerTest, post_initialization_add_components);
   FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces);
   FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle);
+  FRIEND_TEST(ResourceManagerTest, test_unitilizable_hardware_no_validation);
 
   TestableResourceManager() : hardware_interface::ResourceManager() {}
 
@@ -154,6 +155,45 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf)
   ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf));
 }
 
+TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation)
+{
+  // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a
+  // runtime exception is thrown
+  TestableResourceManager rm;
+  ASSERT_THROW(
+    rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true),
+    std::runtime_error);
+}
+
+TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation)
+{
+  // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces,
+  // the interface should not show up
+  TestableResourceManager rm;
+  EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false));
+
+  // test actuator
+  EXPECT_FALSE(rm.state_interface_exists("joint1/position"));
+  EXPECT_FALSE(rm.state_interface_exists("joint1/velocity"));
+  EXPECT_FALSE(rm.command_interface_exists("joint1/position"));
+  EXPECT_FALSE(rm.command_interface_exists("joint1/max_velocity"));
+
+  // test sensor
+  EXPECT_FALSE(rm.state_interface_exists("sensor1/velocity"));
+
+  // test system
+  EXPECT_FALSE(rm.state_interface_exists("joint2/position"));
+  EXPECT_FALSE(rm.state_interface_exists("joint2/velocity"));
+  EXPECT_FALSE(rm.state_interface_exists("joint2/acceleration"));
+  EXPECT_FALSE(rm.command_interface_exists("joint2/velocity"));
+  EXPECT_FALSE(rm.command_interface_exists("joint2/max_acceleration"));
+  EXPECT_FALSE(rm.state_interface_exists("joint3/position"));
+  EXPECT_FALSE(rm.state_interface_exists("joint3/velocity"));
+  EXPECT_FALSE(rm.state_interface_exists("joint3/acceleration"));
+  EXPECT_FALSE(rm.command_interface_exists("joint3/velocity"));
+  EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration"));
+}
+
 TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation)
 {
   // we validate the results manually

ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp

@@ -176,6 +176,55 @@ const auto hardware_resources =
   </ros2_control>
 )";
 
+const auto unitilizable_hardware_resources =
+  R"(
+  <ros2_control name="TestUnitilizableActuatorHardware" type="actuator">
+    <hardware>
+      <plugin>test_unitilizable_actuator</plugin>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <command_interface name="max_velocity" />
+    </joint>
+  </ros2_control>
+  <ros2_control name="TestUnitilizableSensorHardware" type="sensor">
+    <hardware>
+      <plugin>test_unitilizable_sensor</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <sensor name="sensor1">
+      <state_interface name="velocity"/>
+    </sensor>
+  </ros2_control>
+  <ros2_control name="TestUnitilizableSystemHardware" type="system">
+    <hardware>
+      <plugin>test_unitilizable_system</plugin>
+      <param name="example_param_write_for_sec">2</param>
+      <param name="example_param_read_for_sec">2</param>
+    </hardware>
+    <joint name="joint2">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+      <command_interface name="max_acceleration" />
+    </joint>
+    <joint name="joint3">
+      <command_interface name="velocity"/>
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="acceleration"/>
+    </joint>
+    <joint name="configuration">
+      <command_interface name="max_tcp_jerk"/>
+      <state_interface name="max_tcp_jerk"/>
+    </joint>
+  </ros2_control>
+)";
+
 const auto hardware_resources_missing_state_keys =
   R"(
   <ros2_control name="TestActuatorHardware" type="actuator">
@@ -406,6 +455,8 @@ const auto diffbot_urdf =
 
 const auto minimal_robot_urdf =
   std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
+const auto minimal_unitilizable_robot_urdf =
+  std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail);
 
 const auto minimal_robot_missing_state_keys_urdf =
   std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) +