Complete ROS2 migration: ProfilePublisher, StatsPublisher, and liveprofile

.gitignore

@@ -0,0 +1,49 @@
+# ROS2 build artifacts
+build/
+install/
+log/
+
+# CodeQL
+_codeql_detected_source_root
+
+# Compiled Object files
+*.o
+*.obj
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Static libraries
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# IDE
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# Python
+__pycache__/
+*.py[cod]
+*$py.class
+*.egg-info/
+
+# CMake
+CMakeFiles/
+CMakeCache.txt
+cmake_install.cmake
+Makefile
+
+# Doxygen
+doc/
+html/
+latex/

CMakeLists.txt

@@ -14,13 +14,13 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
-# 1. Dit maakt intern al een target "code_profiler" aan!
+# 1. This internally creates a target "code_profiler"
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Statistics.msg"
   DEPENDENCIES std_msgs
 )
 
-# 2. Geef de library een unieke naam (Projectnaam + _lib)
+# 2. Give the library a unique name (Project name + _lib)
 add_library(${PROJECT_NAME}_lib SHARED
   src/loop_timer.cpp
   src/profiler.cpp
@@ -32,7 +32,7 @@ add_library(${PROJECT_NAME}_lib SHARED
   src/timer.cpp
 )
 
-# Koppel de typesupport van je eigen berichten
+# Link the typesupport of custom messages
 rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
 target_link_libraries(${PROJECT_NAME}_lib
   PRIVATE
@@ -46,7 +46,7 @@ target_include_directories(${PROJECT_NAME}_lib PUBLIC
   $<INSTALL_INTERFACE:include>
 )
 
-# 3. Executables (Zorg dat ze ALLEMAAL zijn gedefinieerd voor de install stap)
+# 3. Executables (Make sure they are all defined before the install step)
 add_executable(profiler_test src/main.cpp)
 target_link_libraries(profiler_test ${PROJECT_NAME}_lib)
 
@@ -56,7 +56,7 @@ target_link_libraries(test_loop_timer ${PROJECT_NAME}_lib)
 add_executable(test_profiler test/test_profiler.cpp)
 target_link_libraries(test_profiler ${PROJECT_NAME}_lib)
 
-# 4. Installatie
+# 4. Installation
 install(TARGETS ${PROJECT_NAME}_lib
   EXPORT export_${PROJECT_NAME}
   LIBRARY DESTINATION lib
@@ -66,6 +66,11 @@ install(TARGETS ${PROJECT_NAME}_lib
 
 install(DIRECTORY include/ DESTINATION include)
 
+install(PROGRAMS
+  scripts/liveprofile
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(TARGETS
     profiler_test
     test_loop_timer
@@ -75,6 +80,12 @@ install(TARGETS
 
 # 5. Export
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(rclcpp std_msgs)
+ament_export_dependencies(rclcpp std_msgs rosidl_default_runtime)
+
+# 6. Testing and Linting
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
 
 ament_package()

README.md

@@ -1,3 +1,33 @@
 # code_profiler
 
 [![CI](https://github.com/tue-robotics/code_profiler/actions/workflows/main.yml/badge.svg)](https://github.com/tue-robotics/code_profiler/actions/workflows/main.yml)
+
+Various classes to profile code in different ways, including some additional loop statistics.
+
+## ROS2 Compatibility
+
+This package has been fully migrated to ROS2. It provides profiling utilities that can be used standalone or integrated with ROS2 nodes.
+
+## Key Components
+
+- **Profiler**: Core profiling class for timing code sections
+- **LoopTimer**: Timer with Kahan summation for accurate loop timing
+- **ProfilePublisher**: ROS2 publisher for profiling data (requires `rclcpp::Node*`)
+- **StatsPublisher**: ROS2 publisher for statistics messages (requires `rclcpp::Node*`)
+- **liveprofile**: Python script for live visualization of profiling data
+
+## Usage with ROS2
+
+The `ProfilePublisher` and `StatsPublisher` classes are designed to be usable by other libraries. They accept an `rclcpp::Node*` pointer during initialization:
+
+```cpp
+#include <tue/profiling/profiler.h>
+#include <tue/profiling/ros/profile_publisher.h>
+
+// In your node class
+tue::Profiler profiler("my_profiler");
+tue::ProfilePublisher publisher(profiler, this);  // 'this' is your rclcpp::Node*
+
+// Later, publish profiling data
+publisher.publish();
+```

include/profiling/StatsPublisher.h

@@ -4,7 +4,8 @@
 #include <string>
 #include <map>
 #include <stack>
-#include <ros/publisher.h>
+#include <rclcpp/rclcpp.hpp>
+#include <code_profiler/msg/statistics.hpp>
 
 #include "profiling/Timer.h"
 
@@ -28,7 +29,7 @@ class StatsPublisher
 
     virtual ~StatsPublisher();
 
-    void initialize();
+    void initialize(rclcpp::Node* node);
 
     void startTimer(const std::string& label);
 
@@ -38,14 +39,13 @@ class StatsPublisher
 
 protected:
 
-    ros::Publisher pub_stats_;
+    rclcpp::Publisher<code_profiler::msg::Statistics>::SharedPtr pub_stats_;
 
     std::map<std::string, Timer> timers_;
 
     std::stack<ScopeStat> stack_;
 
-
-
+    rclcpp::Node* node_;
 
 };
 

include/tue/profiling/ros/profile_publisher.h

@@ -1,7 +1,9 @@
 #ifndef TUE_PROFILING_ROS_PROFILE_PUBLISHER_H_
 #define TUE_PROFILING_ROS_PROFILE_PUBLISHER_H_
 
-#include <ros/publisher.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <memory>
 
 namespace tue
 {
@@ -14,25 +16,27 @@ class ProfilePublisher {
 
     ProfilePublisher();
 
-    ProfilePublisher(const Profiler& profiler);
+    ProfilePublisher(const Profiler& profiler, rclcpp::Node* node);
 
-    ProfilePublisher(const Profiler* profiler);
+    ProfilePublisher(const Profiler* profiler, rclcpp::Node* node);
 
     virtual ~ProfilePublisher() = default;
 
-    void initialize(const Profiler& profiler);
+    void initialize(const Profiler& profiler, rclcpp::Node* node);
 
-    void initialize(const Profiler* profiler);
+    void initialize(const Profiler* profiler, rclcpp::Node* node);
 
     void publish() const;
 
 protected:
 
-    void initialize();
+    void initialize(rclcpp::Node* node);
 
     const Profiler* profiler_;
 
-    ros::Publisher pub_stats_;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_stats_;
+
+    rclcpp::Node* node_;
 
 };
 

package.xml

@@ -21,6 +21,7 @@
 
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>rclpy</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
 

scripts/liveprofile

@@ -1,36 +1,39 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import sys
-import rospy
+import rclpy
+from rclpy.node import Node
 from code_profiler.msg import Statistics
 
 import numpy
 
-import sys
 
-class LiveProfile:
+class LiveProfile(Node):
 
     agg_length = 1  # seconds
 
     agg = {}
     start_agg = None
 
     def __init__(self):
-        rospy.init_node('liveprofile', anonymous=True)
-
-        self.start_agg = rospy.get_rostime()
-
-        args = rospy.myargv()
+        super().__init__('liveprofile')
+
+        args = sys.argv
         if len(args) < 2:
-            print(f'usage: {args[0]} topicname')
+            self.get_logger().error(f'usage: {args[0]} topicname')
             sys.exit(1)
 
-        rospy.Subscriber(args[1], Statistics, self.callback)
+        self.start_agg = self.get_clock().now()
+        self.subscription = self.create_subscription(
+            Statistics,
+            args[1],
+            self.callback,
+            10)
 
     def callback(self, data):
-        now = rospy.get_rostime()
-        diff = now - self.start_agg
-        if diff.to_sec() > self.agg_length:
+        now = self.get_clock().now()
+        diff = (now - self.start_agg).nanoseconds / 1e9  # Convert to seconds
+        if diff > self.agg_length:
             self.report(self.agg)
             self.start_agg = now
             self.agg = {}
@@ -41,12 +44,15 @@ class LiveProfile:
                 self.agg[l].append(t)
 
     def report(self, data):
-        for k, v in sorted(data.iteritems()):
+        for k, v in sorted(data.items()):
             mean = "{0:.3f} ms".format(numpy.mean(v, axis=0)*1000)
             std  = "{0:.3f} ms".format(numpy.std(v, axis=0)*1000)
-            print('{k.ljust(55)}\t{mean=}\t{std=}\t({len(v)} samples)')
+            print(f'{k:<55}\tmean={mean}\tstd={std}\t({len(v)} samples)')
 
 
 if __name__ == '__main__':
+    rclpy.init()
     profiler = LiveProfile()
-    rospy.spin()
+    rclpy.spin(profiler)
+    profiler.destroy_node()
+    rclpy.shutdown()

src/profiling/StatsPublisher.cpp

@@ -1,11 +1,12 @@
 #include "profiling/StatsPublisher.h"
 
-#include <ros/ros.h>
-#include <code_profiler/Statistics.h>
+#include <rclcpp/rclcpp.hpp>
+#include <code_profiler/msg/statistics.hpp>
+#include <iostream>
 
 // ----------------------------------------------------------------------------------------------------
 
-StatsPublisher::StatsPublisher()
+StatsPublisher::StatsPublisher() : node_(nullptr)
 {
 }
 
@@ -17,10 +18,16 @@ StatsPublisher::~StatsPublisher()
 
 // ----------------------------------------------------------------------------------------------------
 
-void StatsPublisher::initialize()
+void StatsPublisher::initialize(rclcpp::Node* node)
 {
-    ros::NodeHandle nh("~");
-    pub_stats_ = nh.advertise<code_profiler::Statistics>("profiler_stats", 1);
+    if (!node)
+    {
+        std::cerr << "code_profiler: StatsPublisher::initialize() - Node is a nullptr." << std::endl;
+        return;
+    }
+
+    node_ = node;
+    pub_stats_ = node_->create_publisher<code_profiler::msg::Statistics>("profiler_stats", 1);
 }
 
 // ----------------------------------------------------------------------------------------------------
@@ -60,18 +67,18 @@ void StatsPublisher::stopTimer(const std::string& label)
 
 void StatsPublisher::publish() const
 {
-    if (pub_stats_.getTopic() == "")
+    if (!pub_stats_)
     {
-        std::cout << "code_profiler: StatsPublisher not initialzed." << std::endl;
+        std::cout << "code_profiler: StatsPublisher not initialized." << std::endl;
         return;
     }
 
-    code_profiler::Statistics msg;
+    code_profiler::msg::Statistics msg;
     for(std::map<std::string, Timer>::const_iterator it = timers_.begin(); it != timers_.end(); ++it)
     {
         msg.labels.push_back(it->first);
         msg.time_secs.push_back(it->second.getElapsedTimeInSec());
     }
 
-    pub_stats_.publish(msg);
+    pub_stats_->publish(msg);
 }