diff --git a/cm_topic_hardware_component/CMakeLists.txt b/cm_topic_hardware_component/CMakeLists.txt new file mode 100644 index 0000000..bf95e10 --- /dev/null +++ b/cm_topic_hardware_component/CMakeLists.txt @@ -0,0 +1,77 @@ +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 $ + $) +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 + 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() 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/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 new file mode 100644 index 0000000..21971f4 --- /dev/null +++ b/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 +#include + +// ROS +#include +#include +#include + +#include +#include + +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::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 cm_topic_hardware_component diff --git a/cm_topic_hardware_component/package.xml b/cm_topic_hardware_component/package.xml new file mode 100644 index 0000000..96751c4 --- /dev/null +++ b/cm_topic_hardware_component/package.xml @@ -0,0 +1,34 @@ + + + cm_topic_hardware_component + 0.2.0 + ros2_control hardware interface using pal_statistics messages + + 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 + + ament_cmake_gmock + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp b/cm_topic_hardware_component/src/cm_topic_hardware_component.cpp new file mode 100644 index 0000000..d722581 --- /dev/null +++ b/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 + +#include + +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( + "~/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*/) +{ + 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) 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 new file mode 100644 index 0000000..d3ab28b --- /dev/null +++ b/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 +#include +#include + +#include "gmock/gmock.h" +#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"( + + + cm_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; + auto node = std::make_shared("test_topic_based_system"); + auto executor = std::make_shared(); + +// 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(); +} diff --git a/doc/index.rst b/doc/index.rst index ef44419..c0d66f1 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,8 +1,77 @@ :github_url: https://github.com/ros-controls/topic_based_hardware_interfaces/blob/{REPOS_FILE_BRANCH}/doc/index.rst 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 :parser: myst_parser.sphinx_ + +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``, +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. +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`` + +ros2_control section in URDF +---------------------------- + +.. code:: xml + + + + cm_topic_hardware_component/CMTopicSystem + + + + + 0.2 + + + + + + + 0.3 + + + + + + + 0.1 + + + +