Add controller manager introspection topic system

cm_topic_hardware_component/CMakeLists.txt

@@ -0,0 +1,78 @@
+cmake_minimum_required(VERSION 3.16)
+project(cm_topic_hardware_component CXX)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+if(WIN32)
+  # set the same behavior for windows as it is on linux
+  export_windows_symbols()
+endif()
+
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  angles
+  rclcpp
+  hardware_interface
+  pal_statistics_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+###########
+## Build ##
+###########
+
+# Declare a C++ library
+add_library(
+  ${PROJECT_NAME}
+  src/cm_topic_hardware_component.cpp
+)
+target_link_libraries(${PROJECT_NAME} PUBLIC
+                      angles::angles
+                      rclcpp::rclcpp
+                      hardware_interface::hardware_interface
+                      ${pal_statistics_msgs_TARGETS})
+target_include_directories(${PROJECT_NAME}
+  PUBLIC  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)
+
+#############
+## Install ##
+#############
+
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}Targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  # GTests
+  # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/14 is fixed
+  ament_add_gmock(cm_topic_hardware_component_test test/cm_topic_hardware_component_test.cpp)
+  target_link_libraries(cm_topic_hardware_component_test
+                        ${PROJECT_NAME}
+                        rclcpp::rclcpp
+                        hardware_interface::hardware_interface
+                        ${pal_statistics_msgs_TARGETS}
+                        ros2_control_test_assets::ros2_control_test_assets)
+endif()
+
+pluginlib_export_plugin_description_file(hardware_interface cm_topic_hardware_component_plugin_description.xml)
+ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

cm_topic_hardware_component/README.md

@@ -0,0 +1,3 @@
+# cm_topic_hardware_component
+
+See doc/index.rst

cm_topic_hardware_component/cm_topic_hardware_component_plugin_description.xml

@@ -0,0 +1,11 @@
+<library path="cm_topic_hardware_component">
+  <class
+    name="cm_topic_hardware_component/CMTopicSystem"
+    type="cm_topic_hardware_component::CMTopicSystem"
+    base_class_type="hardware_interface::SystemInterface"
+  >
+    <description>
+      ros2_control hardware interface for CM topic based control.
+    </description>
+  </class>
+</library>

cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp

@@ -0,0 +1,51 @@
+// Copyright 2025 AIT Austrian Institute of Technology GmbH
+//
+// 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.
+
+/* Author: Christoph Froehlich */
+
+#pragma once
+
+// C++
+#include <memory>
+#include <string>
+
+// ROS
+#include <hardware_interface/system_interface.hpp>
+#include <hardware_interface/types/hardware_component_interface_params.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include <pal_statistics_msgs/msg/statistics_names.hpp>
+#include <pal_statistics_msgs/msg/statistics_values.hpp>
+
+namespace cm_topic_hardware_component
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class CMTopicSystem : public hardware_interface::SystemInterface
+{
+public:
+  CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;
+
+  hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  hardware_interface::return_type write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) override;
+
+private:
+  rclcpp::Subscription<pal_statistics_msgs::msg::StatisticsNames>::SharedPtr pal_names_subscriber_;
+  rclcpp::Subscription<pal_statistics_msgs::msg::StatisticsValues>::SharedPtr pal_values_subscriber_;
+  pal_statistics_msgs::msg::StatisticsValues latest_pal_values_;
+  std::unordered_map<uint32_t, std::vector<std::string>> pal_statistics_names_per_topic_;
+};
+
+}  // namespace cm_topic_hardware_component

cm_topic_hardware_component/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>cm_topic_hardware_component</name>
+  <version>0.2.0</version>
+  <description>ros2_control hardware interface using pal_statistics messages</description>
+
+  <maintainer email="[email protected]">Marq Rasmussen</maintainer>
+  <maintainer email="[email protected]">Jafar Uruc</maintainer>
+  <maintainer email="[email protected]">Bence Magyar</maintainer>
+
+  <author email="[email protected]">Christoph Froehlich</author>
+
+  <license>Apache License 2.0</license>
+
+  <url type="website">https://github.com/ros-controls/topic_based_hardware_interfaces</url>
+  <url type="bugtracker">
+    https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces/issues</url>
+  <url type="repository">https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ros2_control_cmake</build_depend>
+
+  <depend>rclcpp</depend>
+  <depend>hardware_interface</depend>
+  <depend>pal_statistics_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>hardware_interface</test_depend>
+  <test_depend>ros2_control_test_assets</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

cm_topic_hardware_component/src/cm_topic_hardware_component.cpp

@@ -0,0 +1,91 @@
+// Copyright 2025 AIT Austrian Institute of Technology GmbH
+//
+// 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.
+
+/* Author: Christoph Froehlich */
+
+#include <string>
+
+#include <cm_topic_hardware_component/cm_topic_hardware_component.hpp>
+
+namespace cm_topic_hardware_component
+{
+
+CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponentInterfaceParams& params)
+{
+  if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS)
+  {
+    return CallbackReturn::ERROR;
+  }
+  // TODO(christophfroehlich): should we use RT box here?
+  pal_names_subscriber_ = get_node()->create_subscription<pal_statistics_msgs::msg::StatisticsNames>(
+      "~/names", rclcpp::SensorDataQoS(), [this](const pal_statistics_msgs::msg::StatisticsNames::SharedPtr pal_names) {
+        pal_statistics_names_per_topic_[pal_names->names_version] = std::move(pal_names->names);
+      });
+  pal_values_subscriber_ = get_node()->create_subscription<pal_statistics_msgs::msg::StatisticsValues>(
+      "~/values", rclcpp::SensorDataQoS(),
+      [this](const pal_statistics_msgs::msg::StatisticsValues::SharedPtr pal_values) {
+        latest_pal_values_ = *pal_values;
+      });
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+  if (latest_pal_values_.names_version == 0 || pal_statistics_names_per_topic_.empty())
+  {
+    // no data received yet
+    return hardware_interface::return_type::OK;
+  }
+
+  auto it = pal_statistics_names_per_topic_.find(latest_pal_values_.names_version);
+  auto end_it = pal_statistics_names_per_topic_.end();
+  if (it != end_it)
+  {
+    const auto& names = it->second;
+    const size_t N = std::min(names.size(), latest_pal_values_.values.size());
+    for (size_t i = 0; i < N; i++)
+    {
+      // If name begins with "state_interface.", extract the remainder
+      const std::string prefix = "state_interface.";
+      if (names[i].rfind(prefix, 0) == 0)
+      {  // starts with prefix
+        const auto& name = names[i].substr(prefix.length());
+        if (joint_state_interfaces_.find(name) != joint_state_interfaces_.end() ||
+            sensor_state_interfaces_.find(name) != sensor_state_interfaces_.end() ||
+            gpio_state_interfaces_.find(name) != gpio_state_interfaces_.end() ||
+            unlisted_state_interfaces_.find(name) != unlisted_state_interfaces_.end())
+        {
+          // TODO(christophfroehlich): does not support other values than double now
+          set_state(name, latest_pal_values_.values.at(i));
+        }
+      }
+    }
+  }
+  else
+  {
+    RCLCPP_WARN(get_node()->get_logger(), "No matching statistics names found");
+  }
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type CMTopicSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+  // nothing to do here
+  return hardware_interface::return_type::OK;
+}
+}  // end namespace cm_topic_hardware_component
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(cm_topic_hardware_component::CMTopicSystem, hardware_interface::SystemInterface)

cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp

@@ -0,0 +1,96 @@
+// Copyright 2025 AIT Austrian Institute of Technology GmbH
+//
+// 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.
+
+/* Author: Christoph Froehlich */
+
+#include <cmath>
+#include <string>
+#include <vector>
+
+#include "gmock/gmock.h"
+#if __has_include(<hardware_interface/hardware_interface/version.h>)
+#include <hardware_interface/hardware_interface/version.h>
+#else
+#include <hardware_interface/version.h>
+#endif
+#include <hardware_interface/resource_manager.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/utilities.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <ros2_control_test_assets/descriptions.hpp>
+
+TEST(TestTopicBasedSystem, load_topic_based_system_2dof)
+{
+  const std::string hardware_system_2dof_topic_based =
+      R"(
+   <ros2_control name="hardware_component_name" type="system">
+    <hardware>
+      <plugin>cm_topic_hardware_component/CMTopicSystem</plugin>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.2</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint2">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.3</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="joint3">
+      <command_interface name="position"/>
+      <state_interface name="position">
+        <param name="initial_value">0.1</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+    </joint>
+  </ros2_control>
+)";
+  auto urdf =
+      ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail;
+  auto node = std::make_shared<rclcpp::Node>("test_topic_based_system");
+  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+// The API of the ResourceManager has changed in hardware_interface 4.13.0
+#if HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0)
+  hardware_interface::ResourceManagerParams params;
+  params.robot_description = urdf;
+  params.clock = node->get_clock();
+  params.logger = node->get_logger();
+  params.executor = executor;
+  try
+  {
+    hardware_interface::ResourceManager rm(params, true);
+  }
+  catch (const std::exception& e)
+  {
+    std::cerr << "Exception caught: " << e.what() << std::endl;
+    FAIL() << "Exception thrown during ResourceManager construction: " << e.what();
+  }
+#else
+  ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false));
+#endif
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleMock(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  return RUN_ALL_TESTS();
+}