Merge branch 'ros-controls:master' into issue-759

Author: bailaC

Diff for: .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

Diff for: .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

Diff for: 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')
         )

Diff for: controller_manager/doc/userdoc.rst

@@ -11,10 +11,9 @@ Determinism
 -----------
 
 For best performance when controlling hardware you want the controller manager to have as little jitter as possible in the main control loop.
-The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control.
-The two easiest kernel options are the `Real-time Ubuntu 22.04 LTS Beta <https://ubuntu.com/blog/real-time-ubuntu-released>`_ or `linux-image-rt-amd64 <https://packages.debian.org/bullseye/linux-image-rt-amd64>`_ on Debian Bullseye.
 
-If you have a realtime kernel installed, the main thread of Controller Manager attempts to configure ``SCHED_FIFO`` with a priority of ``50``.
+Independent of the kernel installed, the main thread of Controller Manager attempts to
+configure ``SCHED_FIFO`` with a priority of ``50``.
 By default, the user does not have permission to set such a high priority.
 To give the user such permissions, add a group named realtime and add the user controlling your robot to this group:
 
@@ -36,6 +35,16 @@ Afterwards, add the following limits to the realtime group in ``/etc/security/li
 
 The limits will be applied after you log out and in again.
 
+The normal linux kernel is optimized for computational throughput and therefore is not well suited for hardware control.
+Alternatives to the standard kernel include
+
+- `Real-time Ubuntu 22.04 LTS Beta <https://ubuntu.com/blog/real-time-ubuntu-released>`_ on Ubuntu 22.04
+- `linux-image-rt-amd64 <https://packages.debian.org/bullseye/linux-image-rt-amd64>`_ on Debian Bullseye
+- lowlatency kernel (``sudo apt install linux-lowlatency``) on any ubuntu
+
+Though installing a realtime-kernel will definitely get the best results when it comes to low
+jitter, using a lowlatency kernel can improve things a lot with being really easy to install.
+
 Parameters
 -----------
 

Diff for: controller_manager/src/controller_manager.cpp

@@ -1007,7 +1007,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;

Diff for: controller_manager/src/ros2_control_node.cpp

@@ -48,16 +48,14 @@ int main(int argc, char ** argv)
   std::thread cm_thread(
     [cm]()
     {
-      if (realtime_tools::has_realtime_kernel())
+      if (!realtime_tools::configure_sched_fifo(kSchedPriority))
       {
-        if (!realtime_tools::configure_sched_fifo(kSchedPriority))
-        {
-          RCLCPP_WARN(cm->get_logger(), "Could not enable FIFO RT scheduling policy");
-        }
-      }
-      else
-      {
-        RCLCPP_INFO(cm->get_logger(), "RT kernel is recommended for better performance");
+        RCLCPP_WARN(
+          cm->get_logger(),
+          "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT "
+          "scheduling. See "
+          "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
+          "for details.");
       }
 
       // for calculating sleep time

Diff for: 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_

Diff for: 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_

Diff for: 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

Diff for: 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);
         }
       }
     }

Diff for: 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)

Diff for: 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>

Diff for: 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)

Diff for: 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)

Diff for: 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