From 0de1722e6998d5c9cae0fcb7a665b27c9a99a7d7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 20:18:27 +0000 Subject: [PATCH 01/15] Initial version for rosbag replay component --- .../CHANGELOG.rst | 16 ++ .../CMakeLists.txt | 90 +++++++++++ .../README.md | 8 + ..._hardware_component_plugin_description.xml | 11 ++ ...oller_manager_topic_hardware_component.hpp | 52 +++++++ .../package.xml | 43 ++++++ ...oller_manager_topic_hardware_component.cpp | 94 ++++++++++++ .../test/control.launch.py | 54 +++++++ .../test/joint_state_topic_based_robot.py | 116 +++++++++++++++ ...nt_state_topic_hardware_interface_test.cpp | 83 +++++++++++ .../test/ros2_control.test.py | 76 ++++++++++ .../test/ros2_controllers.yaml | 25 ++++ .../test/rrr.urdf.xacro | 140 ++++++++++++++++++ .../test/test_topic_based_robot.py | 35 +++++ 14 files changed, 843 insertions(+) create mode 100644 controller_manager_topic_hardware_component/CHANGELOG.rst create mode 100644 controller_manager_topic_hardware_component/CMakeLists.txt create mode 100644 controller_manager_topic_hardware_component/README.md create mode 100644 controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml create mode 100644 controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp create mode 100644 controller_manager_topic_hardware_component/package.xml create mode 100644 controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp create mode 100644 controller_manager_topic_hardware_component/test/control.launch.py create mode 100644 controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py create mode 100644 controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp create mode 100644 controller_manager_topic_hardware_component/test/ros2_control.test.py create mode 100644 controller_manager_topic_hardware_component/test/ros2_controllers.yaml create mode 100644 controller_manager_topic_hardware_component/test/rrr.urdf.xacro create mode 100755 controller_manager_topic_hardware_component/test/test_topic_based_robot.py diff --git a/controller_manager_topic_hardware_component/CHANGELOG.rst b/controller_manager_topic_hardware_component/CHANGELOG.rst new file mode 100644 index 0000000..289873a --- /dev/null +++ b/controller_manager_topic_hardware_component/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package joint_state_topic_hardware_interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.2.0 (2025-08-20) +------------------ +* Deactivate all tests (`#24 `_) +* Fix headings (`#13 `_) +* Switched to Default Node from CM Executor and updated to new params API (`#6 `_) +* Add common repository features from ros-controls (`#8 `_) +* Switch to on_init(HardwareComponentInterfaceParams) (`#11 `_) +* Add Bence Magyar as maintainer (`#4 `_) +* Relicense to Apache 2.0 (`#3 `_) +* Fix CI (`#1 `_) +* rename to joint_state_topic_hardware_interface +* Contributors: Bence Magyar, Christoph Fröhlich, Marq Rasmussen, Soham Patil diff --git a/controller_manager_topic_hardware_component/CMakeLists.txt b/controller_manager_topic_hardware_component/CMakeLists.txt new file mode 100644 index 0000000..4917e9d --- /dev/null +++ b/controller_manager_topic_hardware_component/CMakeLists.txt @@ -0,0 +1,90 @@ +cmake_minimum_required(VERSION 3.16) +project(controller_manager_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/controller_manager_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 $ + $) +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_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + find_package(ros_testing REQUIRED) + + # GTests + # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/14 is fixed + # ament_add_gtest(controller_manager_topic_hardware_component_test test/controller_manager_topic_hardware_component_test.cpp) + # target_link_libraries(controller_manager_topic_hardware_component_test + # ${PROJECT_NAME} + # angles::angles + # rclcpp::rclcpp + # hardware_interface::hardware_interface + # ${sensor_msgs_TARGETS} + # ros2_control_test_assets::ros2_control_test_assets) + + # Integration tests + # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/21 is fixed + # add_ros_test( + # test/ros2_control.test.py + # TIMEOUT + # 120 + # ARGS + # test_file:=${CMAKE_CURRENT_SOURCE_DIR}/test/test_topic_based_robot.py) +endif() + +pluginlib_export_plugin_description_file(hardware_interface controller_manager_topic_hardware_component_plugin_description.xml) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/controller_manager_topic_hardware_component/README.md b/controller_manager_topic_hardware_component/README.md new file mode 100644 index 0000000..2e50450 --- /dev/null +++ b/controller_manager_topic_hardware_component/README.md @@ -0,0 +1,8 @@ +# controller_manager_topic_hardware_component + +``` +ros2 bag play \ + --topics /controller_manager/introspection_data/names /controller_manager/introspection_data/values \ + --remap /controller_manager/introspection_data/names:=/epsiloncranebag/names\ + /controller_manager/introspection_data/values:=/epsiloncranebag/values +``` \ No newline at end of file diff --git a/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml b/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml new file mode 100644 index 0000000..76e5006 --- /dev/null +++ b/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml @@ -0,0 +1,11 @@ + + + + ros2_control hardware interface for CM topic based control. + + + diff --git a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp b/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp new file mode 100644 index 0000000..9a3abef --- /dev/null +++ b/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp @@ -0,0 +1,52 @@ +// Copyright 2025 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. + +/* Author: Jafar Abdi */ + +#pragma once + +// C++ +#include +#include + +// ROS +#include +#include +#include +#include + +#include +#include + +namespace controller_manager_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::SharedPtr pal_names_subscriber_; + rclcpp::Subscription::SharedPtr pal_values_subscriber_; + pal_statistics_msgs::msg::StatisticsValues latest_pal_values_; + std::unordered_map> pal_statistics_names_per_topic_; +}; + +} // namespace controller_manager_topic_hardware_component diff --git a/controller_manager_topic_hardware_component/package.xml b/controller_manager_topic_hardware_component/package.xml new file mode 100644 index 0000000..d16966d --- /dev/null +++ b/controller_manager_topic_hardware_component/package.xml @@ -0,0 +1,43 @@ + + + controller_manager_topic_hardware_component + 0.2.0 + + ros2_control hardware interface for JointState topic based control + + + Marq Rasmussen + Jafar Uruc + Bence Magyar + + Christoph Froehlich + + Apache License 2.0 + + https://github.com/ros-controls/topic_based_hardware_interfaces + https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces/issues + https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces + + ament_cmake + ros2_control_cmake + + rclcpp + hardware_interface + pal_statistics_msgs + + controller_manager + joint_state_broadcaster + joint_trajectory_controller + launch + launch_ros + launch_testing + rclpy + robot_state_publisher + ros2_control_test_assets + ros_testing + xacro + + + ament_cmake + + diff --git a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp new file mode 100644 index 0000000..33d7623 --- /dev/null +++ b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp @@ -0,0 +1,94 @@ +// Copyright 2025 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. + +#include +#include +#include +#include +#include +#include +#include + +#include \ + + +namespace controller_manager_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( + "~/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( + "~/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*/) +{ + 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 controller_manager_topic_hardware_component + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + controller_manager_topic_hardware_component::CMTopicSystem, + hardware_interface::SystemInterface) diff --git a/controller_manager_topic_hardware_component/test/control.launch.py b/controller_manager_topic_hardware_component/test/control.launch.py new file mode 100644 index 0000000..4dd127c --- /dev/null +++ b/controller_manager_topic_hardware_component/test/control.launch.py @@ -0,0 +1,54 @@ +# Copyright 2025 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. + +import os +from pathlib import Path + +import xacro +from launch import LaunchDescription +from launch_ros.actions import Node + +SCRIPT_PATH = Path(os.path.realpath(__file__)).parent + + +def generate_launch_description(): + ros2_controllers_file = Path(SCRIPT_PATH / "ros2_controllers.yaml") + robot_description = { + "robot_description": xacro.process_file(SCRIPT_PATH / "rrr.urdf.xacro").toxml(), + } + controllers = ["joint_state_broadcaster", "joint_trajectory_controller"] + return LaunchDescription( + [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[robot_description], + ), + Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_file], + output="screen", + ), + ] + + [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller], + ) + for controller in controllers + ], + ) diff --git a/controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py b/controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py new file mode 100644 index 0000000..af417c1 --- /dev/null +++ b/controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py @@ -0,0 +1,116 @@ +# Copyright 2025 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. + + +from collections import OrderedDict + +import numpy as np +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile, qos_profile_sensor_data, qos_profile_system_default +from sensor_msgs.msg import JointState + + +class JointStateTopicBasedRobot(Node): + """JointState Topic Based Robot to test joint_state_topic_hardware_interface""" + + def __init__(self, joint_names: list[str]) -> None: + super().__init__("topic_based_robot") + self.joint_names = joint_names.copy() + self.last_joint_command = [] + self.current_joint_state = [] + self.callback_group = ReentrantCallbackGroup() + # Publisher for the robot internal joint state (Ground truth) + self.actual_joint_state_publisher = self.create_publisher( + JointState, + "topic_based_joint_states", + qos_profile=qos_profile_sensor_data, + callback_group=self.callback_group, + ) + # Subscriber for the desired joint state from the controller + self.desired_joint_state_subscriber = self.create_subscription( + JointState, + "topic_based_joint_commands", + self.command_callback, + QoSProfile(depth=1), + callback_group=self.callback_group, + ) + # Reported joint state from ros2_control + self.current_joint_state_subscriber = self.create_subscription( + JointState, + "joint_states", + self.joint_states_callback, + qos_profile_system_default, + callback_group=self.callback_group, + ) + + def filter_joint_state_msg(self, msg: JointState): + joint_states = [] + for joint_name in self.joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + msg = f"Joint name '{joint_name}' not in input keys {msg.name}" + raise ValueError(msg) from None + joint_states.append(msg.position[index]) + return joint_states + + def command_callback(self, msg: JointState): + self.last_joint_command = self.filter_joint_state_msg(msg) + + def joint_states_callback(self, msg: JointState): + self.current_joint_state = self.filter_joint_state_msg(msg) + + def get_current_joint_command(self) -> OrderedDict[str, float]: + """Get the last joint command sent to the robot.""" + self.last_joint_command = [] + while len(self.last_joint_command) == 0: + self.get_logger().warning( + f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.last_joint_command + + def get_current_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_joint_state = [] + while len(self.current_joint_state) == 0: + self.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.current_joint_state + + def set_joint_positions( + self, + joint_positions: list[float] | np.ndarray, + ) -> None: + """Set the joint positions of the robot.""" + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.joint_names), + position=joint_positions, + ), + ) + while not np.allclose( + self.get_current_joint_state(), + joint_positions, + atol=1e-3, + ): + rclpy.spin_once(self, timeout_sec=0.0) diff --git a/controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp b/controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp new file mode 100644 index 0000000..e711b61 --- /dev/null +++ b/controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp @@ -0,0 +1,83 @@ +// Copyright 2025 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. + +#include +#include +#include +#include + +#include +#if __has_include() +#include +#else +#include +#endif +#include +#include +#include +#include +#include + +TEST(TestTopicBasedSystem, load_topic_based_system_2dof) +{ + const std::string hardware_system_2dof_topic_based = + R"( + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_custom_joint_states + + + + + + + + + + + + + + +)"; + auto urdf = + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; + auto node = std::make_shared("test_topic_based_system"); + +// The API of the ResourceManager has changed in hardware_interface 5.3.0 +#if HARDWARE_INTERFACE_VERSION_GTE(5, 3, 0) + hardware_interface::ResourceManagerParams params; + params.robot_description = urdf; + params.clock = node->get_clock(); + params.logger = node->get_logger(); + params.executor = std::make_shared(); + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(params, true)); +// The API of the ResourceManager has changed in hardware_interface 4.13.0 +#elif HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, node->get_node_clock_interface(), + node->get_node_logging_interface(), false)); +#else + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false)); +#endif +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/controller_manager_topic_hardware_component/test/ros2_control.test.py b/controller_manager_topic_hardware_component/test/ros2_control.test.py new file mode 100644 index 0000000..ce78780 --- /dev/null +++ b/controller_manager_topic_hardware_component/test/ros2_control.test.py @@ -0,0 +1,76 @@ +# Copyright 2025 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. + + +import os +import unittest +from pathlib import Path + +import launch_testing +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + TimerAction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node + +SCRIPT_PATH = Path(os.path.realpath(__file__)).parent + + +def generate_test_description(): + test_node = Node( + executable=LaunchConfiguration("test_file"), + ) + + return LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + str(SCRIPT_PATH), + "control.launch.py", + ], + ), + ], + ), + ), + DeclareLaunchArgument("test_file"), + TimerAction(period=2.0, actions=[test_node]), + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ], + ), { + "test_node": test_node, + } + + +class TestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_run_complete(self, test_node): + self.proc_info.assertWaitForShutdown(test_node, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_pass(self, proc_info, test_node): + launch_testing.asserts.assertExitCodes(proc_info, process=test_node) diff --git a/controller_manager_topic_hardware_component/test/ros2_controllers.yaml b/controller_manager_topic_hardware_component/test/ros2_controllers.yaml new file mode 100644 index 0000000..acdf09c --- /dev/null +++ b/controller_manager_topic_hardware_component/test/ros2_controllers.yaml @@ -0,0 +1,25 @@ +controller_manager: + ros__parameters: + update_rate: 50 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_trajectory_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 diff --git a/controller_manager_topic_hardware_component/test/rrr.urdf.xacro b/controller_manager_topic_hardware_component/test/rrr.urdf.xacro new file mode 100644 index 0000000..3288c87 --- /dev/null +++ b/controller_manager_topic_hardware_component/test/rrr.urdf.xacro @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + joint_state_topic_hardware_interface/JointStateTopicSystem + /topic_based_joint_commands + /topic_based_joint_states + + + + + 0.2 + + + + + + + 0.3 + + + + + + + 0.1 + + + + + diff --git a/controller_manager_topic_hardware_component/test/test_topic_based_robot.py b/controller_manager_topic_hardware_component/test/test_topic_based_robot.py new file mode 100755 index 0000000..4329730 --- /dev/null +++ b/controller_manager_topic_hardware_component/test/test_topic_based_robot.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 + +# Copyright 2025 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. + + +import rclpy +from joint_state_topic_based_robot import JointStateTopicBasedRobot + +rclpy.init() + +robot = JointStateTopicBasedRobot(["joint_1", "joint_2", "joint_3"]) +# By default the joint_states should have the initial_value from rrr.urdf.xacro +current_joint_state = robot.get_current_joint_state() +urdf_initial_values = [0.2, 0.3, 0.1] +assert current_joint_state == urdf_initial_values, ( + f"{current_joint_state=} != {urdf_initial_values=}" +) + +# Test setting the robot joint states +joint_state = [0.1, 0.2, 0.3] +robot.set_joint_positions(joint_state) +current_joint_state = robot.get_current_joint_state() +assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" From 18d39e0cff177bc3589bbba563fab14b297d50c6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 20:45:56 +0000 Subject: [PATCH 02/15] Cleanup --- .../controller_manager_topic_hardware_component.hpp | 12 ++++++++---- .../controller_manager_topic_hardware_component.cpp | 6 ------ 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp b/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp index 9a3abef..7ec2eae 100644 --- a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp +++ b/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp @@ -23,7 +23,6 @@ // ROS #include #include -#include #include #include @@ -36,11 +35,16 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class CMTopicSystem : public hardware_interface::SystemInterface { public: - CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; + 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 read( + const rclcpp::Time & time, + const rclcpp::Duration & period) override; - hardware_interface::return_type write(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::SharedPtr pal_names_subscriber_; diff --git a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp index 33d7623..d72c7e3 100644 --- a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp +++ b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp @@ -12,13 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include -#include #include -#include #include \ From eccba8a1250c7d0181d409e10ee0f1161d8382a6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Sep 2025 20:46:03 +0000 Subject: [PATCH 03/15] Pre-commit --- ...oller_manager_topic_hardware_component.hpp | 11 +--- ...oller_manager_topic_hardware_component.cpp | 65 +++++++++---------- 2 files changed, 33 insertions(+), 43 deletions(-) diff --git a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp b/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp index 7ec2eae..7073aa7 100644 --- a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp +++ b/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp @@ -35,16 +35,11 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface class CMTopicSystem : public hardware_interface::SystemInterface { public: - CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams & params) - override; + 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 read(const rclcpp::Time& time, const rclcpp::Duration& period) override; - hardware_interface::return_type write( - 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::SharedPtr pal_names_subscriber_; diff --git a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp index d72c7e3..b7561ef 100644 --- a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp +++ b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp @@ -14,68 +14,65 @@ #include -#include \ - +#include namespace controller_manager_topic_hardware_component { -CallbackReturn CMTopicSystem::on_init( - const hardware_interface::HardwareComponentInterfaceParams & params) +CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponentInterfaceParams& params) { - if (hardware_interface::SystemInterface::on_init(params) != CallbackReturn::SUCCESS) { + 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( - "~/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( - "~/values", rclcpp::SensorDataQoS(), - [this](const pal_statistics_msgs::msg::StatisticsValues::SharedPtr pal_values) { - latest_pal_values_ = *pal_values; - }); + pal_names_subscriber_ = get_node()->create_subscription( + "~/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( + "~/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*/) +hardware_interface::return_type CMTopicSystem::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { 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; + 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++) { + 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 (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()) + 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 { + } + 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*/) +hardware_interface::return_type CMTopicSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // nothing to do here return hardware_interface::return_type::OK; @@ -83,6 +80,4 @@ hardware_interface::return_type CMTopicSystem::write( } // end namespace controller_manager_topic_hardware_component #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - controller_manager_topic_hardware_component::CMTopicSystem, - hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(controller_manager_topic_hardware_component::CMTopicSystem, hardware_interface::SystemInterface) From 2cf05783832323fd04ecc28190bf8fc904f938f5 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 06:44:22 +0000 Subject: [PATCH 04/15] Add some documentation --- .../README.md | 7 +-- doc/index.rst | 60 ++++++++++++++++++- 2 files changed, 60 insertions(+), 7 deletions(-) diff --git a/controller_manager_topic_hardware_component/README.md b/controller_manager_topic_hardware_component/README.md index 2e50450..5049333 100644 --- a/controller_manager_topic_hardware_component/README.md +++ b/controller_manager_topic_hardware_component/README.md @@ -1,8 +1,3 @@ # controller_manager_topic_hardware_component -``` -ros2 bag play \ - --topics /controller_manager/introspection_data/names /controller_manager/introspection_data/values \ - --remap /controller_manager/introspection_data/names:=/epsiloncranebag/names\ - /controller_manager/introspection_data/values:=/epsiloncranebag/values -``` \ No newline at end of file +See doc/index.rst \ No newline at end of file diff --git a/doc/index.rst b/doc/index.rst index ef44419..9f80bbc 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,8 +1,66 @@ :github_url: https://github.com/ros-controls/topic_based_hardware_interfaces/blob/{REPOS_FILE_BRANCH}/doc/index.rst -topic_based_hardware_interfaces +topic_based_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The Joint State Topic Based System implements a ros2_control ``hardware_interface::SystemInterface`` supporting command and state interfaces through the ROS topic communication layer. .. include:: ../joint_state_topic_hardware_interface/README.md :parser: myst_parser.sphinx_ + +controller_manager_topic_hardware_component +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The CM Topic System implements a ros2_control ``hardware_interface::SystemInterface`` subscribing to +topics of type ``pal_statistics_msgs::msg::StatisticsNames`` and ``pal_statistics_msgs::msg::StatisticsValues``, +and sets its state interface to the received values (if present). + +Per default, the ros2_control controller manager publishes these topics to ``/controller_manager/introspection_data/names`` +and ``/controller_manager/introspection_data/values``. + +This component serves as a possibility to replay ROS bags and inject the states from a hardware component into the ros2_control stack. +For example, use ``ros2bag`` CLI to extract these two topics via + +.. code:: bash + + ros2 bag play \ + --topics /controller_manager/introspection_data/names /controller_manager/introspection_data/values \ + --remap /controller_manager/introspection_data/names:=//names\ + /controller_manager/introspection_data/values:=//values + +Note that with this setup the current time of your OS is used and not published from the ROS bag. + +ROS subscribers +---------------------------- +* //names: ``pal_statistics_msgs::msg::StatisticsNames`` +* //values: ``pal_statistics_msgs::msg::StatisticsValues + +ros2_control section in URDF +---------------------------- + +.. code:: xml + + + + controller_manager_topic_hardware_component/CMTopicSystem + + + + + 0.2 + + + + + + + 0.3 + + + + + + + 0.1 + + + + \ No newline at end of file From 6098d91201c756b03aea0ab2a8fc3f75a5bbd0be Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 07:16:51 +0000 Subject: [PATCH 05/15] Activate gmock test --- .../CMakeLists.txt | 17 +++---- ...manager_topic_hardware_component_test.cpp} | 49 ++++++++++++------- 2 files changed, 39 insertions(+), 27 deletions(-) rename controller_manager_topic_hardware_component/test/{joint_state_topic_hardware_interface_test.cpp => controller_manager_topic_hardware_component_test.cpp} (62%) diff --git a/controller_manager_topic_hardware_component/CMakeLists.txt b/controller_manager_topic_hardware_component/CMakeLists.txt index 4917e9d..12c2842 100644 --- a/controller_manager_topic_hardware_component/CMakeLists.txt +++ b/controller_manager_topic_hardware_component/CMakeLists.txt @@ -58,21 +58,20 @@ install( ############# if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(ros2_control_test_assets REQUIRED) find_package(ros_testing REQUIRED) # GTests # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/14 is fixed - # ament_add_gtest(controller_manager_topic_hardware_component_test test/controller_manager_topic_hardware_component_test.cpp) - # target_link_libraries(controller_manager_topic_hardware_component_test - # ${PROJECT_NAME} - # angles::angles - # rclcpp::rclcpp - # hardware_interface::hardware_interface - # ${sensor_msgs_TARGETS} - # ros2_control_test_assets::ros2_control_test_assets) + ament_add_gmock(controller_manager_topic_hardware_component_test test/controller_manager_topic_hardware_component_test.cpp) + target_link_libraries(controller_manager_topic_hardware_component_test + ${PROJECT_NAME} + rclcpp::rclcpp + hardware_interface::hardware_interface + ${pal_statistics_msgs_TARGETS} + ros2_control_test_assets::ros2_control_test_assets) # Integration tests # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/21 is fixed diff --git a/controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp similarity index 62% rename from controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp rename to controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp index e711b61..22ed475 100644 --- a/controller_manager_topic_hardware_component/test/joint_state_topic_hardware_interface_test.cpp +++ b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp @@ -14,10 +14,9 @@ #include #include -#include #include -#include +#include "gmock/gmock.h" #if __has_include() #include #else @@ -32,29 +31,37 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) { const std::string hardware_system_2dof_topic_based = - R"( - + R"( + - joint_state_topic_hardware_interface/JointStateTopicSystem - /topic_based_joint_commands - /topic_based_custom_joint_states + controller_manager_topic_hardware_component/CMTopicSystem - - + + 0.2 + - - + + 0.3 + + + + + + + 0.1 + )"; auto urdf = - ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + + ros2_control_test_assets::urdf_tail; auto node = std::make_shared("test_topic_based_system"); // The API of the ResourceManager has changed in hardware_interface 5.3.0 @@ -64,19 +71,25 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) params.clock = node->get_clock(); params.logger = node->get_logger(); params.executor = std::make_shared(); - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(params, true)); -// The API of the ResourceManager has changed in hardware_interface 4.13.0 + 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(); + } #elif HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, node->get_node_clock_interface(), - node->get_node_logging_interface(), false)); + ASSERT_NO_THROW( + hardware_interface::ResourceManager rm( + urdf, node->get_node_clock_interface(), + node->get_node_logging_interface(), false)); #else ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false)); #endif } -int main(int argc, char** argv) +int main(int argc, char ** argv) { - testing::InitGoogleTest(&argc, argv); + testing::InitGoogleMock(&argc, argv); rclcpp::init(argc, argv); return RUN_ALL_TESTS(); From 7807816776733dae1a16ebf5100fed44d9326d4d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 07:19:50 +0000 Subject: [PATCH 06/15] Cleanup --- .../CHANGELOG.rst | 16 -- .../CMakeLists.txt | 11 -- .../package.xml | 23 +-- .../test/control.launch.py | 54 ------- .../test/joint_state_topic_based_robot.py | 116 --------------- .../test/ros2_control.test.py | 76 ---------- .../test/ros2_controllers.yaml | 25 ---- .../test/rrr.urdf.xacro | 140 ------------------ .../test/test_topic_based_robot.py | 35 ----- 9 files changed, 7 insertions(+), 489 deletions(-) delete mode 100644 controller_manager_topic_hardware_component/CHANGELOG.rst delete mode 100644 controller_manager_topic_hardware_component/test/control.launch.py delete mode 100644 controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py delete mode 100644 controller_manager_topic_hardware_component/test/ros2_control.test.py delete mode 100644 controller_manager_topic_hardware_component/test/ros2_controllers.yaml delete mode 100644 controller_manager_topic_hardware_component/test/rrr.urdf.xacro delete mode 100755 controller_manager_topic_hardware_component/test/test_topic_based_robot.py diff --git a/controller_manager_topic_hardware_component/CHANGELOG.rst b/controller_manager_topic_hardware_component/CHANGELOG.rst deleted file mode 100644 index 289873a..0000000 --- a/controller_manager_topic_hardware_component/CHANGELOG.rst +++ /dev/null @@ -1,16 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package joint_state_topic_hardware_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.2.0 (2025-08-20) ------------------- -* Deactivate all tests (`#24 `_) -* Fix headings (`#13 `_) -* Switched to Default Node from CM Executor and updated to new params API (`#6 `_) -* Add common repository features from ros-controls (`#8 `_) -* Switch to on_init(HardwareComponentInterfaceParams) (`#11 `_) -* Add Bence Magyar as maintainer (`#4 `_) -* Relicense to Apache 2.0 (`#3 `_) -* Fix CI (`#1 `_) -* rename to joint_state_topic_hardware_interface -* Contributors: Bence Magyar, Christoph Fröhlich, Marq Rasmussen, Soham Patil diff --git a/controller_manager_topic_hardware_component/CMakeLists.txt b/controller_manager_topic_hardware_component/CMakeLists.txt index 12c2842..d71810c 100644 --- a/controller_manager_topic_hardware_component/CMakeLists.txt +++ b/controller_manager_topic_hardware_component/CMakeLists.txt @@ -59,9 +59,7 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_cmake_pytest REQUIRED) find_package(ros2_control_test_assets REQUIRED) - find_package(ros_testing REQUIRED) # GTests # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/14 is fixed @@ -72,15 +70,6 @@ if(BUILD_TESTING) hardware_interface::hardware_interface ${pal_statistics_msgs_TARGETS} ros2_control_test_assets::ros2_control_test_assets) - - # Integration tests - # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/21 is fixed - # add_ros_test( - # test/ros2_control.test.py - # TIMEOUT - # 120 - # ARGS - # test_file:=${CMAKE_CURRENT_SOURCE_DIR}/test/test_topic_based_robot.py) endif() pluginlib_export_plugin_description_file(hardware_interface controller_manager_topic_hardware_component_plugin_description.xml) diff --git a/controller_manager_topic_hardware_component/package.xml b/controller_manager_topic_hardware_component/package.xml index d16966d..7fe3e70 100644 --- a/controller_manager_topic_hardware_component/package.xml +++ b/controller_manager_topic_hardware_component/package.xml @@ -1,10 +1,8 @@ - + controller_manager_topic_hardware_component 0.2.0 - - ros2_control hardware interface for JointState topic based control - + ros2_control hardware interface for JointState topic based control Marq Rasmussen Jafar Uruc @@ -15,7 +13,8 @@ Apache License 2.0 https://github.com/ros-controls/topic_based_hardware_interfaces - https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces/issues + + https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces/issues https://github.com/ros-controlsros-controls/topic_based_hardware_interfaces ament_cmake @@ -25,19 +24,11 @@ hardware_interface pal_statistics_msgs - controller_manager - joint_state_broadcaster - joint_trajectory_controller - launch - launch_ros - launch_testing - rclpy - robot_state_publisher + ament_cmake_gmock + hardware_interface ros2_control_test_assets - ros_testing - xacro ament_cmake - + \ No newline at end of file diff --git a/controller_manager_topic_hardware_component/test/control.launch.py b/controller_manager_topic_hardware_component/test/control.launch.py deleted file mode 100644 index 4dd127c..0000000 --- a/controller_manager_topic_hardware_component/test/control.launch.py +++ /dev/null @@ -1,54 +0,0 @@ -# Copyright 2025 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. - -import os -from pathlib import Path - -import xacro -from launch import LaunchDescription -from launch_ros.actions import Node - -SCRIPT_PATH = Path(os.path.realpath(__file__)).parent - - -def generate_launch_description(): - ros2_controllers_file = Path(SCRIPT_PATH / "ros2_controllers.yaml") - robot_description = { - "robot_description": xacro.process_file(SCRIPT_PATH / "rrr.urdf.xacro").toxml(), - } - controllers = ["joint_state_broadcaster", "joint_trajectory_controller"] - return LaunchDescription( - [ - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - parameters=[robot_description], - ), - Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_file], - output="screen", - ), - ] - + [ - Node( - package="controller_manager", - executable="spawner", - arguments=[controller], - ) - for controller in controllers - ], - ) diff --git a/controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py b/controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py deleted file mode 100644 index af417c1..0000000 --- a/controller_manager_topic_hardware_component/test/joint_state_topic_based_robot.py +++ /dev/null @@ -1,116 +0,0 @@ -# Copyright 2025 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. - - -from collections import OrderedDict - -import numpy as np -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from rclpy.qos import QoSProfile, qos_profile_sensor_data, qos_profile_system_default -from sensor_msgs.msg import JointState - - -class JointStateTopicBasedRobot(Node): - """JointState Topic Based Robot to test joint_state_topic_hardware_interface""" - - def __init__(self, joint_names: list[str]) -> None: - super().__init__("topic_based_robot") - self.joint_names = joint_names.copy() - self.last_joint_command = [] - self.current_joint_state = [] - self.callback_group = ReentrantCallbackGroup() - # Publisher for the robot internal joint state (Ground truth) - self.actual_joint_state_publisher = self.create_publisher( - JointState, - "topic_based_joint_states", - qos_profile=qos_profile_sensor_data, - callback_group=self.callback_group, - ) - # Subscriber for the desired joint state from the controller - self.desired_joint_state_subscriber = self.create_subscription( - JointState, - "topic_based_joint_commands", - self.command_callback, - QoSProfile(depth=1), - callback_group=self.callback_group, - ) - # Reported joint state from ros2_control - self.current_joint_state_subscriber = self.create_subscription( - JointState, - "joint_states", - self.joint_states_callback, - qos_profile_system_default, - callback_group=self.callback_group, - ) - - def filter_joint_state_msg(self, msg: JointState): - joint_states = [] - for joint_name in self.joint_names: - try: - index = msg.name.index(joint_name) - except ValueError: - msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - raise ValueError(msg) from None - joint_states.append(msg.position[index]) - return joint_states - - def command_callback(self, msg: JointState): - self.last_joint_command = self.filter_joint_state_msg(msg) - - def joint_states_callback(self, msg: JointState): - self.current_joint_state = self.filter_joint_state_msg(msg) - - def get_current_joint_command(self) -> OrderedDict[str, float]: - """Get the last joint command sent to the robot.""" - self.last_joint_command = [] - while len(self.last_joint_command) == 0: - self.get_logger().warning( - f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", - throttle_duration_sec=2.0, - skip_first=True, - ) - rclpy.spin_once(self, timeout_sec=0.0) - return self.last_joint_command - - def get_current_joint_state(self) -> OrderedDict[str, float]: - """Get the current joint state reported by ros2_control on joint_states topic.""" - self.current_joint_state = [] - while len(self.current_joint_state) == 0: - self.get_logger().warning( - f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", - throttle_duration_sec=2.0, - skip_first=True, - ) - rclpy.spin_once(self, timeout_sec=0.0) - return self.current_joint_state - - def set_joint_positions( - self, - joint_positions: list[float] | np.ndarray, - ) -> None: - """Set the joint positions of the robot.""" - self.actual_joint_state_publisher.publish( - JointState( - name=list(self.joint_names), - position=joint_positions, - ), - ) - while not np.allclose( - self.get_current_joint_state(), - joint_positions, - atol=1e-3, - ): - rclpy.spin_once(self, timeout_sec=0.0) diff --git a/controller_manager_topic_hardware_component/test/ros2_control.test.py b/controller_manager_topic_hardware_component/test/ros2_control.test.py deleted file mode 100644 index ce78780..0000000 --- a/controller_manager_topic_hardware_component/test/ros2_control.test.py +++ /dev/null @@ -1,76 +0,0 @@ -# Copyright 2025 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. - - -import os -import unittest -from pathlib import Path - -import launch_testing -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - TimerAction, -) -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node - -SCRIPT_PATH = Path(os.path.realpath(__file__)).parent - - -def generate_test_description(): - test_node = Node( - executable=LaunchConfiguration("test_file"), - ) - - return LaunchDescription( - [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - str(SCRIPT_PATH), - "control.launch.py", - ], - ), - ], - ), - ), - DeclareLaunchArgument("test_file"), - TimerAction(period=2.0, actions=[test_node]), - launch_testing.util.KeepAliveProc(), - launch_testing.actions.ReadyToTest(), - ], - ), { - "test_node": test_node, - } - - -class TestWaitForCompletion(unittest.TestCase): - # Waits for test to complete, then waits a bit to make sure result files are generated - def test_run_complete(self, test_node): - self.proc_info.assertWaitForShutdown(test_node, timeout=4000.0) - - -@launch_testing.post_shutdown_test() -class TestProcessPostShutdown(unittest.TestCase): - # Checks if the test has been completed with acceptable exit codes (successful codes) - def test_pass(self, proc_info, test_node): - launch_testing.asserts.assertExitCodes(proc_info, process=test_node) diff --git a/controller_manager_topic_hardware_component/test/ros2_controllers.yaml b/controller_manager_topic_hardware_component/test/ros2_controllers.yaml deleted file mode 100644 index acdf09c..0000000 --- a/controller_manager_topic_hardware_component/test/ros2_controllers.yaml +++ /dev/null @@ -1,25 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 50 - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - -joint_trajectory_controller: - ros__parameters: - joints: - - joint_1 - - joint_2 - - joint_3 - command_interfaces: - - position - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.0 - goal_time: 0.0 diff --git a/controller_manager_topic_hardware_component/test/rrr.urdf.xacro b/controller_manager_topic_hardware_component/test/rrr.urdf.xacro deleted file mode 100644 index 3288c87..0000000 --- a/controller_manager_topic_hardware_component/test/rrr.urdf.xacro +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - joint_state_topic_hardware_interface/JointStateTopicSystem - /topic_based_joint_commands - /topic_based_joint_states - - - - - 0.2 - - - - - - - 0.3 - - - - - - - 0.1 - - - - - diff --git a/controller_manager_topic_hardware_component/test/test_topic_based_robot.py b/controller_manager_topic_hardware_component/test/test_topic_based_robot.py deleted file mode 100755 index 4329730..0000000 --- a/controller_manager_topic_hardware_component/test/test_topic_based_robot.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2025 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. - - -import rclpy -from joint_state_topic_based_robot import JointStateTopicBasedRobot - -rclpy.init() - -robot = JointStateTopicBasedRobot(["joint_1", "joint_2", "joint_3"]) -# By default the joint_states should have the initial_value from rrr.urdf.xacro -current_joint_state = robot.get_current_joint_state() -urdf_initial_values = [0.2, 0.3, 0.1] -assert current_joint_state == urdf_initial_values, ( - f"{current_joint_state=} != {urdf_initial_values=}" -) - -# Test setting the robot joint states -joint_state = [0.1, 0.2, 0.3] -robot.set_joint_positions(joint_state) -current_joint_state = robot.get_current_joint_state() -assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" From 81eeb5f510a31dd68f854ac2bfcf3e74f54bdf2e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 08:34:46 +0000 Subject: [PATCH 07/15] Early abort if no message is received yet --- .../src/controller_manager_topic_hardware_component.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp index b7561ef..cb6fb4b 100644 --- a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp +++ b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp @@ -41,6 +41,11 @@ CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponen 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) From b96d0574ac1b3a5bff3e6d64ba3f366db679ce52 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 08:35:50 +0000 Subject: [PATCH 08/15] Format code --- .../README.md | 2 +- .../package.xml | 2 +- ...oller_manager_topic_hardware_component.cpp | 3 ++- ..._manager_topic_hardware_component_test.cpp | 20 +++++++++---------- doc/index.rst | 4 ++-- 5 files changed, 16 insertions(+), 15 deletions(-) diff --git a/controller_manager_topic_hardware_component/README.md b/controller_manager_topic_hardware_component/README.md index 5049333..78921ad 100644 --- a/controller_manager_topic_hardware_component/README.md +++ b/controller_manager_topic_hardware_component/README.md @@ -1,3 +1,3 @@ # controller_manager_topic_hardware_component -See doc/index.rst \ No newline at end of file +See doc/index.rst diff --git a/controller_manager_topic_hardware_component/package.xml b/controller_manager_topic_hardware_component/package.xml index 7fe3e70..2f92514 100644 --- a/controller_manager_topic_hardware_component/package.xml +++ b/controller_manager_topic_hardware_component/package.xml @@ -31,4 +31,4 @@ ament_cmake - \ No newline at end of file + diff --git a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp index cb6fb4b..597b42d 100644 --- a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp +++ b/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp @@ -41,7 +41,8 @@ CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponen 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()) { + if (latest_pal_values_.names_version == 0 || pal_statistics_names_per_topic_.empty()) + { // no data received yet return hardware_interface::return_type::OK; } diff --git a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp index 22ed475..247d24a 100644 --- a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp +++ b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp @@ -31,7 +31,7 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) { const std::string hardware_system_2dof_topic_based = - R"( + R"( controller_manager_topic_hardware_component/CMTopicSystem @@ -60,8 +60,7 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) )"; auto urdf = - ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + - ros2_control_test_assets::urdf_tail; + ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; auto node = std::make_shared("test_topic_based_system"); // The API of the ResourceManager has changed in hardware_interface 5.3.0 @@ -71,23 +70,24 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) params.clock = node->get_clock(); params.logger = node->get_logger(); params.executor = std::make_shared(); - try { + try + { hardware_interface::ResourceManager rm(params, true); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { std::cerr << "Exception caught: " << e.what() << std::endl; FAIL() << "Exception thrown during ResourceManager construction: " << e.what(); } #elif HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) - ASSERT_NO_THROW( - hardware_interface::ResourceManager rm( - urdf, node->get_node_clock_interface(), - node->get_node_logging_interface(), false)); + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, node->get_node_clock_interface(), + node->get_node_logging_interface(), false)); #else ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false)); #endif } -int main(int argc, char ** argv) +int main(int argc, char** argv) { testing::InitGoogleMock(&argc, argv); rclcpp::init(argc, argv); diff --git a/doc/index.rst b/doc/index.rst index 9f80bbc..d1b9924 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -13,7 +13,7 @@ The CM Topic System implements a ros2_control ``hardware_interface::SystemInterf topics of type ``pal_statistics_msgs::msg::StatisticsNames`` and ``pal_statistics_msgs::msg::StatisticsValues``, and sets its state interface to the received values (if present). -Per default, the ros2_control controller manager publishes these topics to ``/controller_manager/introspection_data/names`` +Per default, the ros2_control controller manager publishes these topics to ``/controller_manager/introspection_data/names`` and ``/controller_manager/introspection_data/values``. This component serves as a possibility to replay ROS bags and inject the states from a hardware component into the ros2_control stack. @@ -63,4 +63,4 @@ ros2_control section in URDF - \ No newline at end of file + From 9c5acb5246f036d462f650749d1ccc79dc707890 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 08:45:21 +0000 Subject: [PATCH 09/15] Update clock topic in docs --- doc/index.rst | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index d1b9924..61ae806 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,7 +1,7 @@ :github_url: https://github.com/ros-controls/topic_based_hardware_interfaces/blob/{REPOS_FILE_BRANCH}/doc/index.rst -topic_based_hardware_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +topic_based_hardware_interfaces +############################### The Joint State Topic Based System implements a ros2_control ``hardware_interface::SystemInterface`` supporting command and state interfaces through the ROS topic communication layer. .. include:: ../joint_state_topic_hardware_interface/README.md @@ -27,11 +27,22 @@ For example, use ``ros2bag`` CLI to extract these two topics via /controller_manager/introspection_data/values:=//values Note that with this setup the current time of your OS is used and not published from the ROS bag. +If you want to control the speed of playback, run + +* your ros2_control_node with ``--ros-args -p --use_sim_time:=true`` +* and the ``--rate`` and ``--clock`` options, for example + + .. code:: bash + + ros2 bag play --rate 2.0 --clock 100 \ + --topics /controller_manager/introspection_data/names /controller_manager/introspection_data/values \ + --remap /controller_manager/introspection_data/names:=//names\ + /controller_manager/introspection_data/values:=//values ROS subscribers ---------------------------- * //names: ``pal_statistics_msgs::msg::StatisticsNames`` -* //values: ``pal_statistics_msgs::msg::StatisticsValues +* //values: ``pal_statistics_msgs::msg::StatisticsValues`` ros2_control section in URDF ---------------------------- From d80c16a2c044c0c487ad14163ac4dc2e3c35763e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 09:00:04 +0000 Subject: [PATCH 10/15] Update package description --- controller_manager_topic_hardware_component/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager_topic_hardware_component/package.xml b/controller_manager_topic_hardware_component/package.xml index 2f92514..daed13d 100644 --- a/controller_manager_topic_hardware_component/package.xml +++ b/controller_manager_topic_hardware_component/package.xml @@ -2,7 +2,7 @@ controller_manager_topic_hardware_component 0.2.0 - ros2_control hardware interface for JointState topic based control + ros2_control hardware interface using pal_statistics messages Marq Rasmussen Jafar Uruc From 324639b6d10454cd12a23598e526c93f5d4da29e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 09:03:59 +0000 Subject: [PATCH 11/15] Fix test for jazzy --- .../controller_manager_topic_hardware_component_test.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp index 247d24a..64f0027 100644 --- a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp +++ b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp @@ -62,14 +62,15 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail; auto node = std::make_shared("test_topic_based_system"); + auto executor = std::make_shared(); // The API of the ResourceManager has changed in hardware_interface 5.3.0 -#if HARDWARE_INTERFACE_VERSION_GTE(5, 3, 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 = std::make_shared(); + params.executor = executor; try { hardware_interface::ResourceManager rm(params, true); @@ -79,9 +80,6 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) std::cerr << "Exception caught: " << e.what() << std::endl; FAIL() << "Exception thrown during ResourceManager construction: " << e.what(); } -#elif HARDWARE_INTERFACE_VERSION_GTE(4, 13, 0) - ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, node->get_node_clock_interface(), - node->get_node_logging_interface(), false)); #else ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf, true, false)); #endif From ab30d663f8d262a9db9ee41219459cc1cefe3155 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 09:17:50 +0000 Subject: [PATCH 12/15] Fix comment --- .../test/controller_manager_topic_hardware_component_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp index 64f0027..d12054d 100644 --- a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp +++ b/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp @@ -64,7 +64,7 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) auto node = std::make_shared("test_topic_based_system"); auto executor = std::make_shared(); -// The API of the ResourceManager has changed in hardware_interface 5.3.0 +// 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; From aedcd9c3d8516c9c5978e80f8aa38462e0692493 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 09:20:16 +0000 Subject: [PATCH 13/15] Rename package --- .../CMakeLists.txt | 10 +++++----- cm_topic_hardware_component/README.md | 3 +++ ...cm_topic_hardware_component_plugin_description.xml | 11 +++++++++++ .../cm_topic_hardware_component.hpp | 4 ++-- .../package.xml | 2 +- .../src/cm_topic_hardware_component.cpp | 8 ++++---- .../test/cm_topic_hardware_component_test.cpp | 2 +- controller_manager_topic_hardware_component/README.md | 3 --- ...er_topic_hardware_component_plugin_description.xml | 11 ----------- doc/index.rst | 4 ++-- 10 files changed, 29 insertions(+), 29 deletions(-) rename {controller_manager_topic_hardware_component => cm_topic_hardware_component}/CMakeLists.txt (81%) create mode 100644 cm_topic_hardware_component/README.md create mode 100644 cm_topic_hardware_component/cm_topic_hardware_component_plugin_description.xml rename controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp => cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp (94%) rename {controller_manager_topic_hardware_component => cm_topic_hardware_component}/package.xml (95%) rename controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp => cm_topic_hardware_component/src/cm_topic_hardware_component.cpp (90%) rename controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp => cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp (97%) delete mode 100644 controller_manager_topic_hardware_component/README.md delete mode 100644 controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml diff --git a/controller_manager_topic_hardware_component/CMakeLists.txt b/cm_topic_hardware_component/CMakeLists.txt similarity index 81% rename from controller_manager_topic_hardware_component/CMakeLists.txt rename to cm_topic_hardware_component/CMakeLists.txt index d71810c..77b471c 100644 --- a/controller_manager_topic_hardware_component/CMakeLists.txt +++ b/cm_topic_hardware_component/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(controller_manager_topic_hardware_component CXX) +project(cm_topic_hardware_component CXX) find_package(ros2_control_cmake REQUIRED) set_compiler_options() @@ -29,7 +29,7 @@ endforeach() # Declare a C++ library add_library( ${PROJECT_NAME} - src/controller_manager_topic_hardware_component.cpp + src/cm_topic_hardware_component.cpp ) target_link_libraries(${PROJECT_NAME} PUBLIC angles::angles @@ -63,8 +63,8 @@ if(BUILD_TESTING) # GTests # TODO(anyone): reactivate if https://github.com/ros-controls/topic_based_hardware_interfaces/issues/14 is fixed - ament_add_gmock(controller_manager_topic_hardware_component_test test/controller_manager_topic_hardware_component_test.cpp) - target_link_libraries(controller_manager_topic_hardware_component_test + 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 @@ -72,7 +72,7 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets) endif() -pluginlib_export_plugin_description_file(hardware_interface controller_manager_topic_hardware_component_plugin_description.xml) +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() diff --git a/cm_topic_hardware_component/README.md b/cm_topic_hardware_component/README.md new file mode 100644 index 0000000..4b4b753 --- /dev/null +++ b/cm_topic_hardware_component/README.md @@ -0,0 +1,3 @@ +# cm_topic_hardware_component + +See doc/index.rst diff --git a/cm_topic_hardware_component/cm_topic_hardware_component_plugin_description.xml b/cm_topic_hardware_component/cm_topic_hardware_component_plugin_description.xml new file mode 100644 index 0000000..56b2658 --- /dev/null +++ b/cm_topic_hardware_component/cm_topic_hardware_component_plugin_description.xml @@ -0,0 +1,11 @@ + + + + ros2_control hardware interface for CM topic based control. + + + diff --git a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp b/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp similarity index 94% rename from controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp rename to cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp index 7073aa7..6364f53 100644 --- a/controller_manager_topic_hardware_component/include/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component.hpp +++ b/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp @@ -28,7 +28,7 @@ #include #include -namespace controller_manager_topic_hardware_component +namespace cm_topic_hardware_component { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -48,4 +48,4 @@ class CMTopicSystem : public hardware_interface::SystemInterface std::unordered_map> pal_statistics_names_per_topic_; }; -} // namespace controller_manager_topic_hardware_component +} // namespace cm_topic_hardware_component diff --git a/controller_manager_topic_hardware_component/package.xml b/cm_topic_hardware_component/package.xml similarity index 95% rename from controller_manager_topic_hardware_component/package.xml rename to cm_topic_hardware_component/package.xml index daed13d..96751c4 100644 --- a/controller_manager_topic_hardware_component/package.xml +++ b/cm_topic_hardware_component/package.xml @@ -1,6 +1,6 @@ - controller_manager_topic_hardware_component + cm_topic_hardware_component 0.2.0 ros2_control hardware interface using pal_statistics messages diff --git a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp similarity index 90% rename from controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp rename to cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 597b42d..1174946 100644 --- a/controller_manager_topic_hardware_component/src/controller_manager_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -14,9 +14,9 @@ #include -#include +#include -namespace controller_manager_topic_hardware_component +namespace cm_topic_hardware_component { CallbackReturn CMTopicSystem::on_init(const hardware_interface::HardwareComponentInterfaceParams& params) @@ -83,7 +83,7 @@ hardware_interface::return_type CMTopicSystem::write(const rclcpp::Time& /*time* // nothing to do here return hardware_interface::return_type::OK; } -} // end namespace controller_manager_topic_hardware_component +} // end namespace cm_topic_hardware_component #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(controller_manager_topic_hardware_component::CMTopicSystem, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(cm_topic_hardware_component::CMTopicSystem, hardware_interface::SystemInterface) diff --git a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp b/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp similarity index 97% rename from controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp rename to cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp index d12054d..70b5523 100644 --- a/controller_manager_topic_hardware_component/test/controller_manager_topic_hardware_component_test.cpp +++ b/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp @@ -34,7 +34,7 @@ TEST(TestTopicBasedSystem, load_topic_based_system_2dof) R"( - controller_manager_topic_hardware_component/CMTopicSystem + cm_topic_hardware_component/CMTopicSystem diff --git a/controller_manager_topic_hardware_component/README.md b/controller_manager_topic_hardware_component/README.md deleted file mode 100644 index 78921ad..0000000 --- a/controller_manager_topic_hardware_component/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# controller_manager_topic_hardware_component - -See doc/index.rst diff --git a/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml b/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml deleted file mode 100644 index 76e5006..0000000 --- a/controller_manager_topic_hardware_component/controller_manager_topic_hardware_component_plugin_description.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - ros2_control hardware interface for CM topic based control. - - - diff --git a/doc/index.rst b/doc/index.rst index 61ae806..c0d66f1 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -7,7 +7,7 @@ The Joint State Topic Based System implements a ros2_control ``hardware_interfac .. include:: ../joint_state_topic_hardware_interface/README.md :parser: myst_parser.sphinx_ -controller_manager_topic_hardware_component +cm_topic_hardware_component ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The CM Topic System implements a ros2_control ``hardware_interface::SystemInterface`` subscribing to topics of type ``pal_statistics_msgs::msg::StatisticsNames`` and ``pal_statistics_msgs::msg::StatisticsValues``, @@ -51,7 +51,7 @@ ros2_control section in URDF - controller_manager_topic_hardware_component/CMTopicSystem + cm_topic_hardware_component/CMTopicSystem From ba671683d232caeb45f0228a6ff366aa3e34c1fd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Sep 2025 09:23:33 +0000 Subject: [PATCH 14/15] Update copyright claim --- .../cm_topic_hardware_component.hpp | 4 ++-- .../src/cm_topic_hardware_component.cpp | 4 +++- .../test/cm_topic_hardware_component_test.cpp | 4 +++- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp b/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp index 6364f53..21971f4 100644 --- a/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp +++ b/cm_topic_hardware_component/include/cm_topic_hardware_component/cm_topic_hardware_component.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 ros2_control Development Team +// 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -/* Author: Jafar Abdi */ +/* Author: Christoph Froehlich */ #pragma once diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp index 1174946..d722581 100644 --- a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp +++ b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 ros2_control Development Team +// 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. @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +/* Author: Christoph Froehlich */ + #include #include diff --git a/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp b/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp index 70b5523..d3ab28b 100644 --- a/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp +++ b/cm_topic_hardware_component/test/cm_topic_hardware_component_test.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 ros2_control Development Team +// 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. @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +/* Author: Christoph Froehlich */ + #include #include #include From 32a835121557f13fdea4c7ab3df116fd52a68a9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 3 Oct 2025 18:05:28 +0200 Subject: [PATCH 15/15] Remove old comment Co-authored-by: Marq Rasmussen --- cm_topic_hardware_component/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/cm_topic_hardware_component/CMakeLists.txt b/cm_topic_hardware_component/CMakeLists.txt index 77b471c..bf95e10 100644 --- a/cm_topic_hardware_component/CMakeLists.txt +++ b/cm_topic_hardware_component/CMakeLists.txt @@ -62,7 +62,6 @@ if(BUILD_TESTING) 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}