Merge branch 'master' into add/hw_components/update_rate

controller_manager/src/controller_manager.cpp

@@ -467,13 +467,21 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
       controller = chainable_loader_->createSharedInstance(controller_type);
     }
   }
-  catch (const pluginlib::CreateClassException & e)
+  catch (const std::exception & e)
   {
     RCLCPP_ERROR(
-      get_logger(), "Error happened during creation of controller '%s' with type '%s':\n%s",
+      get_logger(), "Caught exception while loading the controller '%s' of plugin type '%s':\n%s",
       controller_name.c_str(), controller_type.c_str(), e.what());
     return nullptr;
   }
+  catch (...)
+  {
+    RCLCPP_ERROR(
+      get_logger(),
+      "Caught unknown exception while loading the controller '%s' of plugin type '%s'",
+      controller_name.c_str(), controller_type.c_str());
+    throw;
+  }
 
   ControllerSpec controller_spec;
   controller_spec.c = controller;

controller_manager/src/ros2_control_node.cpp

@@ -50,7 +50,6 @@ int main(int argc, char ** argv)
       // A simple way to reject non ros args
       continue;
     }
-    RCLCPP_INFO(rclcpp::get_logger("CM args"), "Adding argument: %s", argv[i]);
     node_arguments.push_back(argv[i]);
   }
   cm_node_options.arguments(node_arguments);

doc/release_notes.rst

@@ -141,6 +141,7 @@ hardware_interface
       </gpio>
     </ros2_control>
 * With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
+* With (`#1421 <https://github.com/ros-controls/ros2_control/pull/1421>`_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file.
 
 joint_limits
 ************

hardware_interface/doc/hardware_interface_types_userdoc.rst

@@ -9,6 +9,38 @@ The ``ros2_control`` framework provides a set of hardware interface types that c
 a hardware component for a specific robot or device.
 The following sections describe the different hardware interface types and their usage.
 
+Overview
+*****************************
+Hardware in ros2_control is described as URDF and internally parsed and encapsulated as ``HardwareInfo``.
+The definition can be found in the `ros2_control repository <https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/hardware_info.hpp>`_.
+You can check the structs defined there to see what attributes are available for each of the xml tags.
+A generic example which shows the structure is provided below. More specific examples can be found in the Example part below.
+
+.. code:: xml
+
+    <ros2_control name="Name_of_the_hardware" type="system">
+      <hardware>
+        <plugin>library_name/ClassName</plugin>
+        <!-- added to hardware_parameters -->
+        <param name="example_param">value</param>
+      </hardware>
+      <joint name="name_of_the_component">
+        <command_interface name="interface_name">
+          <!-- All of them are optional. `data_type` and `size` are used for GPIOs. Size is length of an array. -->
+          <param name="min">-1</param>
+          <param name="max">1</param>
+          <param name="initial_value">0.0</param>
+          <param name="data_type"></param>
+          <param name="size">5</param>
+           <!-- Optional. Added to the key/value storage parameters -->
+          <param name="own_param_1">some_value</param>
+          <param name="own_param_2">other_value</param>
+        </command_interface>
+        <!-- Short form to define StateInterface. Can be extended like CommandInterface. -->
+        <state_interface name="position"/>
+      </joint>
+    </ros2_control>
+
 Joints
 *****************************
 ``<joint>``-tag groups the interfaces associated with the joints of physical robots and actuators.

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -46,6 +46,10 @@ struct InterfaceInfo
   int size;
   /// (Optional) enable or disable the limits for the command interfaces
   bool enable_limits;
+  /// (Optional) Key-value pairs of command/stateInterface parameters. This is
+  /// useful for drivers that operate on protocols like modbus, where each
+  /// interface needs own address(register), datatype, etc.
+  std::unordered_map<std::string, std::string> parameters;
 };
 
 /// @brief This structure stores information about a joint that is mimicking another joint

hardware_interface/include/hardware_interface/lifecycle_helpers.hpp

@@ -0,0 +1,30 @@
+// Copyright 2024 PAL Robotics S.L.
+//
+// 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__LIFECYCLE_HELPERS_HPP_
+#define HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_
+
+#include <lifecycle_msgs/msg/state.hpp>
+
+namespace hardware_interface
+{
+constexpr bool lifecycleStateThatRequiresNoAction(const lifecycle_msgs::msg::State::_id_type state)
+{
+  return state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN ||
+         state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
+         state == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED;
+}
+}  // namespace hardware_interface
+
+#endif  // HARDWARE_INTERFACE__LIFECYCLE_HELPERS_HPP_

hardware_interface/src/actuator.cpp

@@ -22,6 +22,7 @@
 #include "hardware_interface/actuator_interface.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/lifecycle_helpers.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -246,9 +247,7 @@ const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_c
 
 return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     last_read_cycle_time_ = time;
     return return_type::OK;
@@ -270,9 +269,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p
 
 return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     last_write_cycle_time_ = time;
     return return_type::OK;

hardware_interface/src/component_parser.cpp

@@ -350,6 +350,13 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml(
   interface.data_type = "double";
   interface.size = 1;
 
+  // Parse parameters
+  const auto * params_it = interfaces_it->FirstChildElement(kParamTag);
+  if (params_it)
+  {
+    interface.parameters = parse_parameters_from_xml(params_it);
+  }
+
   return interface;
 }
 

hardware_interface/src/sensor.cpp

@@ -21,6 +21,7 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/lifecycle_helpers.hpp"
 #include "hardware_interface/sensor_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -220,9 +221,7 @@ const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle
 
 return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     last_read_cycle_time_ = time;
     return return_type::OK;

hardware_interface/src/system.cpp

@@ -21,6 +21,7 @@
 
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/lifecycle_helpers.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
 #include "hardware_interface/types/lifecycle_state_names.hpp"
@@ -242,9 +243,7 @@ const rclcpp::Time & System::get_last_write_time() const { return last_write_cyc
 
 return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     last_read_cycle_time_ = time;
     return return_type::OK;
@@ -266,9 +265,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per
 
 return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period)
 {
-  if (
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
-    impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
+  if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id()))
   {
     last_write_cycle_time_ = time;
     return return_type::OK;

hardware_interface/test/test_component_parser.cpp

@@ -297,6 +297,108 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens
   }
 }
 
+TEST_F(
+  TestComponentParser,
+  successfully_parse_valid_urdf_system_multi_interface_custom_interface_parameters)
+{
+  std::string urdf_to_test =
+    std::string(ros2_control_test_assets::urdf_head) +
+    ros2_control_test_assets::
+      valid_urdf_ros2_control_system_multi_interface_and_custom_interface_parameters +
+    ros2_control_test_assets::urdf_tail;
+  const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
+  ASSERT_THAT(control_hardware, SizeIs(1));
+  const auto hardware_info = control_hardware.front();
+
+  EXPECT_EQ(hardware_info.name, "RRBotSystemMultiInterface");
+  EXPECT_EQ(hardware_info.type, "system");
+  EXPECT_EQ(
+    hardware_info.hardware_plugin_name,
+    "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware");
+  ASSERT_THAT(hardware_info.hardware_parameters, SizeIs(2));
+  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), "2");
+  EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2");
+
+  ASSERT_THAT(hardware_info.joints, SizeIs(2));
+
+  EXPECT_EQ(hardware_info.joints[0].name, "joint1");
+  EXPECT_EQ(hardware_info.joints[0].type, "joint");
+  EXPECT_EQ(hardware_info.joints[0].parameters.size(), 3);
+  EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_ip"), "1.1.1.1");
+  EXPECT_EQ(hardware_info.joints[0].parameters.at("modbus_server_port"), "1234");
+  EXPECT_EQ(hardware_info.joints[0].parameters.at("use_persistent_connection"), "true");
+  ASSERT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(3));
+  ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(3));
+  // CommandInterfaces of joints
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].initial_value, "1.2");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.size(), 5);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register"), "1");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].parameters.at("register_size"), "2");
+
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].initial_value, "3.4");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].min, "-1.5");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].max, "1.5");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.size(), 5);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register"), "2");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].parameters.at("register_size"), "4");
+
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].min, "-0.5");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].max, "0.5");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].initial_value, "");
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1);
+  EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].parameters.size(), 2);
+
+  // StateInterfaces of joints
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.size(), 2);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register"), "3");
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].parameters.at("register_size"), "2");
+
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.size(), 2);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register"), "4");
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[1].parameters.at("register_size"), "4");
+
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].name, HW_IF_EFFORT);
+  EXPECT_EQ(hardware_info.joints[0].state_interfaces[2].parameters.size(), 0);
+
+  // Second Joint
+  EXPECT_EQ(hardware_info.joints[1].name, "joint2");
+  EXPECT_EQ(hardware_info.joints[1].type, "joint");
+  EXPECT_EQ(hardware_info.joints[1].parameters.size(), 2);
+  EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_ip"), "192.168.178.123");
+  EXPECT_EQ(hardware_info.joints[1].parameters.at("modbus_server_port"), "4321");
+  ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1));
+  ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3));
+  // CommandInterfaces
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].initial_value, "");
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].min, "-1");
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1");
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.size(), 4);
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("register"), "20");
+  EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].parameters.at("data_type"), "int32_t");
+  // StateInterfaces of joints
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.size(), 2);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("register"), "21");
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].parameters.at("data_type"), "int32_t");
+
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].name, HW_IF_VELOCITY);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[1].parameters.size(), 0);
+
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.size(), 2);
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("register"), "21");
+  EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].parameters.at("data_type"), "int32_t");
+}
+
 TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor)
 {
   std::string urdf_to_test =