diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 2fb31641b0..2f7ef176aa 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -4,9 +4,11 @@ project(hardware_interface) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra) endif() +option(RECORD_INTERFACES "record command interfaces with data_tamer" OFF) set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs + data_tamer lifecycle_msgs pluginlib rclcpp_lifecycle @@ -46,6 +48,10 @@ add_library( SHARED src/mock_components/generic_system.cpp ) +if(RECORD_INTERFACES) + target_compile_definitions(mock_components PRIVATE RECORD_INTERFACES) +endif() + target_include_directories( mock_components PUBLIC @@ -75,6 +81,9 @@ add_library( src/mock_components/generic_system.cpp src/mock_components/fake_generic_system.cpp ) +if(RECORD_INTERFACES) + target_compile_definitions(fake_components PRIVATE RECORD_INTERFACES) +endif() target_include_directories( fake_components PUBLIC diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index c244ee1254..8eabf374ff 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -26,6 +26,14 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#ifdef RECORD_INTERFACES +#include "data_tamer/data_tamer.hpp" +#include "data_tamer/sinks/mcap_sink.hpp" + +static auto mcap_sink = std::make_shared("/tmp/ros2_control_node.mcap"); +static auto channel = DataTamer::LogChannel::create("command_interfaces"); +#endif + using hardware_interface::return_type; namespace mock_components @@ -45,6 +53,9 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override { +#ifdef RECORD_INTERFACES + channel->takeSnapshot(); +#endif return return_type::OK; } diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 5d1ca1a913..5f6537e4e9 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -10,6 +10,7 @@ ament_cmake control_msgs + data_tamer lifecycle_msgs pluginlib rclcpp_lifecycle diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp index 8b48b2efdf..71a1c38f04 100644 --- a/hardware_interface/src/mock_components/generic_system.cpp +++ b/hardware_interface/src/mock_components/generic_system.cpp @@ -297,7 +297,9 @@ std::vector GenericSystem::export_state_inte std::vector GenericSystem::export_command_interfaces() { std::vector command_interfaces; - +#ifdef RECORD_INTERFACES + channel->addDataSink(mcap_sink); +#endif // Joints' state interfaces for (auto i = 0u; i < info_.joints.size(); i++) { @@ -317,7 +319,30 @@ std::vector GenericSystem::export_command_ "Interface is not found in the standard nor other list. " "This should never happen!"); } +#ifdef RECORD_INTERFACES + else + { + auto it = std::find(other_interfaces_.begin(), other_interfaces_.end(), interface.name); + if (it != other_interfaces_.end()) + { + auto j = std::distance(other_interfaces_.begin(), it); + channel->registerValue(joint.name + "/" + interface.name, &other_commands_[j][i]); + } + } +#endif + } +#ifdef RECORD_INTERFACES + else + { + auto it = + std::find(standard_interfaces_.begin(), standard_interfaces_.end(), interface.name); + if (it != standard_interfaces_.end()) + { + auto j = std::distance(standard_interfaces_.begin(), it); + channel->registerValue(joint.name + "/" + interface.name, &joint_commands_[j][i]); + } } +#endif } }