diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..186f778 --- /dev/null +++ b/.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/ diff --git a/CMakeLists.txt b/CMakeLists.txt index ccc49b5..4210f2a 100644 --- a/CMakeLists.txt +++ b/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 $ ) -# 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() diff --git a/README.md b/README.md index 67e04ed..8c7f2ba 100644 --- a/README.md +++ b/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 +#include + +// 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(); +``` diff --git a/include/profiling/StatsPublisher.h b/include/profiling/StatsPublisher.h index a22650b..dbdc800 100644 --- a/include/profiling/StatsPublisher.h +++ b/include/profiling/StatsPublisher.h @@ -4,7 +4,8 @@ #include #include #include -#include +#include +#include #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::SharedPtr pub_stats_; std::map timers_; std::stack stack_; - - + rclcpp::Node* node_; }; diff --git a/include/tue/profiling/ros/profile_publisher.h b/include/tue/profiling/ros/profile_publisher.h index d2a2198..ecb6a26 100644 --- a/include/tue/profiling/ros/profile_publisher.h +++ b/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 +#include +#include +#include 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::SharedPtr pub_stats_; + + rclcpp::Node* node_; }; diff --git a/package.xml b/package.xml index b94a384..b443922 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ rosidl_default_runtime python3-numpy + rclpy ament_lint_common diff --git a/scripts/liveprofile b/scripts/liveprofile index 4e4f2b1..23fdbd0 100755 --- a/scripts/liveprofile +++ b/scripts/liveprofile @@ -1,14 +1,14 @@ -#!/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 @@ -16,21 +16,24 @@ class LiveProfile: 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() diff --git a/src/profiling/StatsPublisher.cpp b/src/profiling/StatsPublisher.cpp index 2c981af..805b506 100644 --- a/src/profiling/StatsPublisher.cpp +++ b/src/profiling/StatsPublisher.cpp @@ -1,11 +1,12 @@ #include "profiling/StatsPublisher.h" -#include -#include +#include +#include +#include // ---------------------------------------------------------------------------------------------------- -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("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("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::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); } diff --git a/src/ros/profile_publisher.cpp b/src/ros/profile_publisher.cpp index 446bd3c..8929b88 100644 --- a/src/ros/profile_publisher.cpp +++ b/src/ros/profile_publisher.cpp @@ -1,64 +1,77 @@ #include "tue/profiling/ros/profile_publisher.h" #include "tue/profiling/profiler.h" -#include -#include +#include #include +#include namespace tue { // ---------------------------------------------------------------------------------------------------- -ProfilePublisher::ProfilePublisher() : profiler_(nullptr) +ProfilePublisher::ProfilePublisher() : profiler_(nullptr), node_(nullptr) { } // ---------------------------------------------------------------------------------------------------- -ProfilePublisher::ProfilePublisher(const Profiler& profiler) : profiler_(&profiler) +ProfilePublisher::ProfilePublisher(const Profiler& profiler, rclcpp::Node* node) : profiler_(&profiler), node_(node) { - initialize(); + initialize(node); } // ---------------------------------------------------------------------------------------------------- -ProfilePublisher::ProfilePublisher(const Profiler* profiler) : profiler_(profiler) +ProfilePublisher::ProfilePublisher(const Profiler* profiler, rclcpp::Node* node) : profiler_(profiler), node_(node) { - initialize(); + initialize(node); } // ---------------------------------------------------------------------------------------------------- -void ProfilePublisher::initialize(const Profiler& profiler) +void ProfilePublisher::initialize(const Profiler& profiler, rclcpp::Node* node) { profiler_ = &profiler; - initialize(); + node_ = node; + initialize(node); } // ---------------------------------------------------------------------------------------------------- -void ProfilePublisher::initialize(const Profiler* profiler) +void ProfilePublisher::initialize(const Profiler* profiler, rclcpp::Node* node) { profiler_ = profiler; - initialize(); + node_ = node; + initialize(node); } // ---------------------------------------------------------------------------------------------------- -void ProfilePublisher::initialize() +void ProfilePublisher::initialize(rclcpp::Node* node) { - assert((void("Profiler is a nullptr"), profiler_)); - ros::NodeHandle nh("~"); - pub_stats_ = nh.advertise("profile/" + profiler_->getName(), 1); + if (!profiler_) + { + std::cerr << "[tue::Profiler] ProfilePublisher: Profiler is a nullptr" << std::endl; + return; + } + + if (!node) + { + std::cerr << "[tue::Profiler] ProfilePublisher: Node is a nullptr" << std::endl; + return; + } + + node_ = node; + pub_stats_ = node_->create_publisher("profile/" + profiler_->getName(), 1); } // ---------------------------------------------------------------------------------------------------- void ProfilePublisher::publish() const { - if (pub_stats_.getTopic() == "") + if (!pub_stats_) { std::cout << "[tue::Profiler] ProfilePublisher not initialized." << std::endl; return; @@ -67,10 +80,10 @@ void ProfilePublisher::publish() const std::stringstream s; s << *profiler_; - std_msgs::String msg; + std_msgs::msg::String msg; msg.data = s.str(); - pub_stats_.publish(msg); + pub_stats_->publish(msg); } }