add data_tamer logging for command interfaces

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
ros-controls · Dec 28, 2023 · 0f40bf4 · 0f40bf4
1 parent 81a9fe7
commit 0f40bf4
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 1 deletion.
diff --git 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
@@ -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<DataTamer::MCAPSink>("/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
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>control_msgs</depend>
+  <depend>data_tamer</depend>
   <depend>lifecycle_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp_lifecycle</depend>

diff --git a/hardware_interface/src/mock_components/generic_system.cpp b/hardware_interface/src/mock_components/generic_system.cpp
@@ -297,7 +297,9 @@ std::vector<hardware_interface::StateInterface> GenericSystem::export_state_inte
 std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_interfaces()
 {
   std::vector<hardware_interface::CommandInterface> 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<hardware_interface::CommandInterface> 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
     }
   }