Add ROS 2 Jazzy compatibility for hardware_interface and PidROS APIs

Jazzy (hardware_interface v4) uses the old on_init(HardwareInfo&) API
and 3-arg PidROS constructor, but with new method names
(initialize_from_ros_parameters, get_gains, compute_command). The
existing code assumed v4+ meant HardwareComponentInterfaceParams and
4-arg PidROS, which only exist in Rolling/Kilted (v5+).

Author: software-pathon-robotics

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp

@@ -57,6 +57,10 @@
 #include "transmission_interface/transmission_loader.hpp"
 
 #define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3)
+// Jazzy has hardware_interface v4 but still uses old on_init(HardwareInfo&) API and 3-arg PidROS.
+// HardwareComponentInterfaceParams and 4-arg PidROS only exist in Rolling/Kilted (v5+).
+#define ROS_DISTRO_JAZZY (HARDWARE_INTERFACE_VERSION_MAJOR == 4)
+#define USE_OLD_ON_INIT_API (ROS_DISTRO_HUMBLE || ROS_DISTRO_JAZZY)
 
 // defining these for Humble, because they are defined elsewhere in future versions, and we use them in this file
 #if ROS_DISTRO_HUMBLE
@@ -85,9 +89,9 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
   ~MujocoSystemInterface() override;
 
   hardware_interface::CallbackReturn
-// Jazzy introduces a new HarwareComponentInterfaceParams object which doesn't exist in humble. This adds
-// compatibility by switching to the old interface, which behaves similarly
-#if ROS_DISTRO_HUMBLE
+// HardwareComponentInterfaceParams only exists in Rolling/Kilted (v5+).
+// Humble and Jazzy both use on_init(HardwareInfo&).
+#if USE_OLD_ON_INIT_API
   on_init(const hardware_interface::HardwareInfo& info) override;
 #else
   on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;

mujoco_ros2_control/src/mujoco_system_interface.cpp

@@ -678,9 +678,8 @@ MujocoSystemInterface::~MujocoSystemInterface()
 }
 
 hardware_interface::CallbackReturn
-// after humble switches from HardwareInfo to HardwareComponentInterfaceParams. This keeps it backwards compatible
-// between the two distros
-#if ROS_DISTRO_HUMBLE
+// Humble and Jazzy use on_init(HardwareInfo&); Rolling/Kilted use HardwareComponentInterfaceParams.
+#if USE_OLD_ON_INIT_API
 MujocoSystemInterface::on_init(const hardware_interface::HardwareInfo& params)
 #else
 MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterfaceParams& params)
@@ -1696,12 +1695,18 @@ bool MujocoSystemInterface::register_mujoco_actuators()
 
     // Initialize PID controllers for actuators that have them configured
     const auto initialize_position_pids = [&]() -> bool {
-// after humble has an additional argument in the PidROS constructor, and uses a different function to initialize from parameters
+// Humble: 3-arg PidROS ctor + old method names; Jazzy: 3-arg ctor + new method names;
+// Rolling/Kilted: 4-arg ctor + new method names
 #if ROS_DISTRO_HUMBLE
       actuator_data.pos_pid = std::make_shared<control_toolbox::PidROS>(
           get_node(), "pid_gains.position." + actuator_data.joint_name, false);
       actuator_data.pos_pid->initPid();
       const auto gains = actuator_data.pos_pid->getGains();
+#elif ROS_DISTRO_JAZZY
+      actuator_data.pos_pid = std::make_shared<control_toolbox::PidROS>(
+          get_node(), "pid_gains.position." + actuator_data.joint_name, false);
+      actuator_data.pos_pid->initialize_from_ros_parameters();
+      const auto gains = actuator_data.pos_pid->get_gains();
 #else
       actuator_data.pos_pid = std::make_shared<control_toolbox::PidROS>(
           get_node(), "pid_gains.position." + actuator_data.joint_name, "", false);
@@ -1712,12 +1717,18 @@ bool MujocoSystemInterface::register_mujoco_actuators()
     };
 
     const auto initialize_velocity_pids = [&]() -> bool {
-// after humble has an additional argument in the PidROS constructor, and uses a different function to initialize from parameters
+// Humble: 3-arg PidROS ctor + old method names; Jazzy: 3-arg ctor + new method names;
+// Rolling/Kilted: 4-arg ctor + new method names
 #if ROS_DISTRO_HUMBLE
       actuator_data.vel_pid = std::make_shared<control_toolbox::PidROS>(
           get_node(), "pid_gains.velocity." + actuator_data.joint_name, false);
       actuator_data.vel_pid->initPid();
       const auto gains = actuator_data.pos_pid->getGains();
+#elif ROS_DISTRO_JAZZY
+      actuator_data.vel_pid = std::make_shared<control_toolbox::PidROS>(
+          get_node(), "pid_gains.velocity." + actuator_data.joint_name, false);
+      actuator_data.vel_pid->initialize_from_ros_parameters();
+      const auto gains = actuator_data.vel_pid->get_gains();
 #else
       actuator_data.vel_pid = std::make_shared<control_toolbox::PidROS>(
           get_node(), "pid_gains.velocity." + actuator_data.joint_name, "", false);

mujoco_ros2_control/test/test_headless_init.cpp

@@ -27,6 +27,8 @@
 #include <rclcpp/rclcpp.hpp>
 
 #define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3)
+#define ROS_DISTRO_JAZZY (HARDWARE_INTERFACE_VERSION_MAJOR == 4)
+#define USE_OLD_ON_INIT_API (ROS_DISTRO_HUMBLE || ROS_DISTRO_JAZZY)
 
 class HeadlessInitTest : public ::testing::Test
 {
@@ -122,7 +124,7 @@ class HeadlessInitTest : public ::testing::Test
 TEST_F(HeadlessInitTest, HeadlessInitialization)
 {
   // Test that MujocoSystemInterface can be initialized in headless mode
-#if ROS_DISTRO_HUMBLE
+#if USE_OLD_ON_INIT_API
   auto result = interface_->on_init(hardware_info_);
 #else
   hardware_interface::HardwareComponentInterfaceParams params;