windows Sun Apr 13 20:58:23 WEDT 2025

README.md

@@ -1 +1 @@
-This contains the priority_executor package that adds an Executor with deadline and chain-aware priority support. The current usage example is out-of-date
+This contains the priority_executor package that adds an Executor with deadline and chain-aware priority support. The `usage_example.cpp` file shows the needed steps to use the executor.

src/priority_executor/CMakeLists.txt

@@ -1,71 +1,55 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
-project(priority_executor)
+project(priority_executor VERSION 0.1.0)
 
-# Default to C99
+# Set C++ standards
-if(NOT CMAKE_C_STANDARD)
+set(CMAKE_CXX_STANDARD 17)
-  set(CMAKE_C_STANDARD 99)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
-endif()
+set(CMAKE_CXX_EXTENSIONS OFF)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
 
+# Compiler options
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
+# Find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rcl REQUIRED)
+find_package(rmw REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(simple_timer REQUIRED)
+find_package(nlohmann_json REQUIRED)
 
-add_library(priority_executor  src/priority_executor.cpp src/priority_memory_strategy.cpp)
+# Library targets
+add_library(priority_executor
+  src/priority_executor.cpp
+  src/priority_memory_strategy.cpp
+  src/performance_monitor.cpp
+  src/default_executor.cpp
+  src/usage_example.cpp
+  src/multithread_priority_executor.cpp
+)
 target_include_directories(priority_executor PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
-# target_link_libraries(priority_executor
-#  simple_timer
-# )
 ament_target_dependencies(priority_executor
   rmw
   rclcpp
   rcl
   simple_timer
+  nlohmann_json
 )
 
-add_library(multithread_priority_executor src/multithread_priority_executor.cpp src/priority_memory_strategy.cpp)
+# Example executable
-target_include_directories(multithread_priority_executor PUBLIC
+add_executable(usage_example
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  src/usage_example.cpp
-  $<INSTALL_INTERFACE:include>
 )
-ament_target_dependencies(multithread_priority_executor
-  rmw
-  rclcpp
-  rcl
-  simple_timer
-)
-
-add_library(default_executor src/default_executor.cpp)
-target_include_directories(default_executor PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-)
-ament_target_dependencies(default_executor
-  rmw
-  rclcpp
-  rcl
-  simple_timer
-)
-
-add_executable(usage_example src/usage_example.cpp)
 target_include_directories(usage_example PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+)
 target_link_libraries(usage_example
   priority_executor
 )
@@ -73,36 +57,37 @@ ament_target_dependencies(usage_example
   rclcpp
   std_msgs
   std_srvs
+  simple_timer
 )
 
+# Testing
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Installation
 install(
   DIRECTORY include/
   DESTINATION include
 )
 
-install(TARGETS usage_example
+install(
-  DESTINATION lib/${PROJECT_NAME}
+  TARGETS priority_executor
-)
+  EXPORT export_${PROJECT_NAME}
-
-install(TARGETS priority_executor multithread_priority_executor default_executor
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
 
-if(BUILD_TESTING)
+install(
-  find_package(ament_lint_auto REQUIRED)
+  TARGETS usage_example
-  # the following line skips the linter which checks for copyrights
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
-  # uncomment the line when a copyright and license is not present in all source files
+)
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
 
+# Export and package configuration
 ament_export_include_directories(include)
-ament_export_libraries(priority_executor)
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_libraries(multithread_priority_executor)
+ament_export_dependencies(rclcpp rcl rmw simple_timer nlohmann_json)
-ament_export_libraries(default_executor)
 ament_package()

src/priority_executor/include/priority_executor/default_executor.hpp

@@ -12,72 +12,74 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RTIS_DEFAULT_EXECUTOR
+#ifndef PRIORITY_EXECUTOR__DEFAULT_EXECUTOR_HPP_
-#define RTIS_DEFAULT_EXECUTOR
+#define PRIORITY_EXECUTOR__DEFAULT_EXECUTOR_HPP_
 
 #include <rmw/rmw.h>
 
+#include <set>
+#include <memory>
 #include <cassert>
 #include <cstdlib>
-#include <memory>
+#include <unordered_map>
-#include <vector>
-#include <set>
 
 #include "rclcpp/executor.hpp"
-#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/utilities.hpp"
-#include "rclcpp/rate.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/detail/mutex_two_priorities.hpp"
-#include "priority_executor/priority_memory_strategy.hpp"
 #include "simple_timer/rt-sched.hpp"
 
-class RTISTimed
+namespace priority_executor {
-{
+
+class RTISTimed {
 public:
     node_time_logger logger_;
 };
 
-class ROSDefaultMultithreadedExecutor : public rclcpp::Executor, public RTISTimed
+class ROSDefaultMultithreadedExecutor : public rclcpp::Executor, public RTISTimed {
-{
 public:
+    RCLCPP_SMART_PTR_DEFINITIONS(ROSDefaultMultithreadedExecutor)
+
     RCLCPP_PUBLIC
     explicit ROSDefaultMultithreadedExecutor(
-      const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions(), int number_of_threads = 2, std::chrono::nanoseconds next_exec_timeout = std::chrono::nanoseconds(-1));
+        rclcpp::ExecutorOptions const &options = rclcpp::ExecutorOptions(),
+        int number_of_threads = 2,
+        std::chrono::nanoseconds next_exec_timeout = std::chrono::nanoseconds(-1));
 
     RCLCPP_PUBLIC size_t get_number_of_threads();
     RCLCPP_PUBLIC void spin() override;
-  bool get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+    bool get_next_executable(rclcpp::AnyExecutable& any_executable,
+                           std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
 protected:
     RCLCPP_PUBLIC void run(size_t thread_number);
 
 private:
+    RCLCPP_DISABLE_COPY(ROSDefaultMultithreadedExecutor)
+
     size_t number_of_threads_;
     std::set<rclcpp::TimerBase::SharedPtr> scheduled_timers_;
+    std::chrono::nanoseconds next_exec_timeout_{std::chrono::nanoseconds(-1)};
+
     static std::unordered_map<ROSDefaultMultithreadedExecutor*,
                              std::shared_ptr<rclcpp::detail::MutexTwoPriorities>>
         wait_mutex_set_;
     static std::mutex shared_wait_mutex_;
-  std::chrono::nanoseconds next_exec_timeout_ = std::chrono::nanoseconds(-1);
 };
 
 /// Single-threaded executor implementation.
 /**
  * This is the default executor created by rclcpp::spin.
  */
-class ROSDefaultExecutor : public rclcpp::Executor, public RTISTimed
+class ROSDefaultExecutor : public rclcpp::Executor, public RTISTimed {
-{
 public:
     RCLCPP_SMART_PTR_DEFINITIONS(ROSDefaultExecutor)
 
     /// Default constructor. See the default constructor for Executor.
     RCLCPP_PUBLIC
     explicit ROSDefaultExecutor(
-      const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions());
+        rclcpp::ExecutorOptions const &options = rclcpp::ExecutorOptions());
 
     /// Default destructor.
     RCLCPP_PUBLIC
@@ -92,16 +94,18 @@ public:
      * \throws std::runtime_error when spin() called while already spinning
      */
     RCLCPP_PUBLIC
-  void
+    void spin() override;
-  spin() override;
+
-  bool get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+    bool get_next_executable(rclcpp::AnyExecutable& any_executable,
+                           std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
     RCLCPP_PUBLIC
-  void
+    void wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
-  wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
 private:
     RCLCPP_DISABLE_COPY(ROSDefaultExecutor)
 };
 
-#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
+}  // namespace priority_executor
+
+#endif  // PRIORITY_EXECUTOR__DEFAULT_EXECUTOR_HPP_

src/priority_executor/include/priority_executor/multithread_priority_executor.hpp

@@ -1,18 +1,37 @@
-#ifndef RTIS_MULTITHREAD_EXECUTOR
+#ifndef PRIORITY_EXECUTOR__MULTITHREAD_PRIORITY_EXECUTOR_HPP_
-#define RTIS_MULTITHREAD_EXECUTOR
+#define PRIORITY_EXECUTOR__MULTITHREAD_PRIORITY_EXECUTOR_HPP_
 
-#include <priority_executor/priority_executor.hpp>
-#include "rclcpp/detail/mutex_two_priorities.hpp"
 #include <set>
+#include <mutex>
+#include <chrono>
+#include <memory>
+#include <thread>
+#include <unordered_map>
 
-namespace timed_executor
+#include "priority_executor/default_executor.hpp"
-{
+#include "priority_executor/priority_executor.hpp"
-  class MultithreadTimedExecutor : public TimedExecutor
+
-  {
+#include "rclcpp/executor.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/visibility_control.hpp"
+
+namespace priority_executor {
+
+class MultithreadTimedExecutor : public TimedExecutor {
 public:
     RCLCPP_PUBLIC
     explicit MultithreadTimedExecutor(
-        const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions(), std::string name = "unnamed executor", int number_of_threads = 2, std::chrono::nanoseconds next_exec_timeout = std::chrono::nanoseconds(-1));
+        rclcpp::ExecutorOptions const &options = rclcpp::ExecutorOptions(),
+        std::string const &name = "unnamed executor",
+        int number_of_threads = 2,
+        std::chrono::nanoseconds next_exec_timeout = std::chrono::nanoseconds(-1));
+
+    RCLCPP_PUBLIC
+    virtual ~MultithreadTimedExecutor() = default;
 
     RCLCPP_PUBLIC size_t get_number_of_threads();
     RCLCPP_PUBLIC void spin() override;
@@ -21,14 +40,17 @@ namespace timed_executor
     RCLCPP_PUBLIC void run(size_t thread_number);
 
 private:
-    size_t number_of_threads_;
+    RCLCPP_DISABLE_COPY(MultithreadTimedExecutor)
     std::set<rclcpp::TimerBase::SharedPtr> scheduled_timers_;
+    size_t number_of_threads_;
+    std::chrono::nanoseconds next_exec_timeout_{std::chrono::nanoseconds(-1)};
+
     static std::unordered_map<MultithreadTimedExecutor*,
                              std::shared_ptr<rclcpp::detail::MutexTwoPriorities>>
         wait_mutex_set_;
     static std::mutex shared_wait_mutex_;
-    std::chrono::nanoseconds next_exec_timeout_ = std::chrono::nanoseconds(-1);
 };
-}
 
-#endif
+}  // namespace priority_executor
+
+#endif  // PRIORITY_EXECUTOR__MULTITHREAD_PRIORITY_EXECUTOR_HPP_

src/priority_executor/include/priority_executor/performance_monitor.hpp

@@ -0,0 +1,237 @@
+#ifndef PERFORMANCE_MONITOR_HPP_
+#define PERFORMANCE_MONITOR_HPP_
+
+#include <atomic>
+#include <chrono>
+#include <deque>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+#include <nlohmann/json.hpp>
+#include "rclcpp/rclcpp.hpp"
+#include "simple_timer/rt-sched.hpp"
+
+namespace priority_executor {
+
+enum class PerformanceEventType {
+    CALLBACK_READY,
+    CALLBACK_START,
+    CALLBACK_END,
+    DEADLINE_MISSED,
+    DEADLINE_MET,
+    CHAIN_START,
+    CHAIN_END
+};
+
+struct PerformanceEvent {
+    uint64_t timestamp;         // High-resolution timestamp (microseconds)
+    PerformanceEventType type;  // Type of event
+    std::string node_name;      // Name of the node
+    std::string callback_name;  // Name of the callback
+    int chain_id{-1};              // Chain identifier
+    bool is_first_in_chain{false};    // First callback in chain
+    bool is_last_in_chain{false};     // Last callback in chain
+    uint64_t deadline{0};         // Deadline (if applicable)
+    uint64_t processing_time{0};  // Time spent executing (if applicable)
+    int executor_id{0};           // Executor identifier
+    std::string additional_data; // JSON string for extensible data
+    
+    // Helper method to extract callback type from additional_data
+    std::string get_callback_type() const {
+        if (additional_data.empty()) return "unknown";
+        
+        try {
+            nlohmann::json json = nlohmann::json::parse(additional_data);
+            if (json.contains("callback_type")) {
+                return json["callback_type"].get<std::string>();
+            }
+        } catch (...) {}
+        
+        return "unknown";
+    }
+    
+    // Helper method to extract thread info from additional_data
+    int get_thread_id() const {
+        if (additional_data.empty()) return -1;
+        
+        try {
+            nlohmann::json json = nlohmann::json::parse(additional_data);
+            if (json.contains("thread_info") && json["thread_info"].is_object() && 
+                json["thread_info"].contains("thread_id")) {
+                return json["thread_info"]["thread_id"].get<int>();
+            }
+            // Direct thread_id if present
+            if (json.contains("thread_id")) {
+                return json["thread_id"].get<int>();
+            }
+        } catch (...) {}
+        
+        return -1;
+    }
+    
+    // Helper method to create a descriptive identifier for this callback
+    std::string get_descriptive_id() const {
+        std::stringstream ss;
+        ss << node_name;
+        
+        if (!callback_name.empty()) {
+            ss << "/" << callback_name;
+        }
+        
+        if (chain_id >= 0) {
+            ss << " (chain:" << chain_id;
+            if (is_first_in_chain) ss << " start";
+            if (is_last_in_chain) ss << " end";
+            ss << ")";
+        }
+        
+        return ss.str();
+    }
+    
+    // Convert additional_data to JSON object
+    nlohmann::json get_additional_data_json() const {
+        try {
+            if (!additional_data.empty()) {
+                return nlohmann::json::parse(additional_data);
+            }
+        } catch (...) {}
+        
+        return nlohmann::json::object();
+    }
+};
+
+class PerformanceMonitor {
+public:
+    static PerformanceMonitor& getInstance();
+    
+    void recordEvent(const PerformanceEvent& event);
+    void setBufferSize(size_t size);
+    void setAutoDumpThreshold(size_t threshold);
+    void setDumpPath(const std::string& path);
+    bool dumpToFile(const std::string& filename);
+    void enable(bool enabled = true);
+    void clear();
+    
+    // Helper method to set monitoring options in one call
+    void setMonitoringOptions(size_t buffer_size, size_t auto_dump_threshold, 
+                              const std::string& dump_path) {
+        setBufferSize(buffer_size);
+        setAutoDumpThreshold(auto_dump_threshold);
+        setDumpPath(dump_path);
+    }
+    
+    // Get all events for a specific callback
+    std::vector<PerformanceEvent> getEventsForCallback(const std::string& node_name, 
+                                                      const std::string& callback_name) const {
+        std::vector<PerformanceEvent> result;
+        std::lock_guard<std::mutex> lock(event_mutex_);
+        
+        for (const auto& event : event_buffer_) {
+            if (event.node_name == node_name && event.callback_name == callback_name) {
+                result.push_back(event);
+            }
+        }
+        
+        return result;
+    }
+    
+    // Get all events for a specific chain
+    std::vector<PerformanceEvent> getEventsForChain(int chain_id) const {
+        std::vector<PerformanceEvent> result;
+        std::lock_guard<std::mutex> lock(event_mutex_);
+        
+        for (const auto& event : event_buffer_) {
+            if (event.chain_id == chain_id) {
+                result.push_back(event);
+            }
+        }
+        
+        return result;
+    }
+    
+    // Get all events by type
+    std::vector<PerformanceEvent> getEventsByType(PerformanceEventType type) const {
+        std::vector<PerformanceEvent> result;
+        std::lock_guard<std::mutex> lock(event_mutex_);
+        
+        for (const auto& event : event_buffer_) {
+            if (event.type == type) {
+                result.push_back(event);
+            }
+        }
+        
+        return result;
+    }
+    
+    // Get a formatted report of callbacks and their execution times
+    std::string getCallbackExecutionReport() const {
+        std::lock_guard<std::mutex> lock(event_mutex_);
+        std::stringstream ss;
+        
+        ss << "Callback Execution Report:\n";
+        ss << "=========================\n\n";
+        
+        // Maps to store total execution time and count per callback
+        std::unordered_map<std::string, uint64_t> total_time;
+        std::unordered_map<std::string, int> call_count;
+        std::unordered_map<std::string, uint64_t> max_time;
+        std::unordered_map<std::string, uint64_t> min_time;
+        
+        // Process all CALLBACK_END events (they contain processing_time)
+        for (const auto& event : event_buffer_) {
+            if (event.type == PerformanceEventType::CALLBACK_END && event.processing_time > 0) {
+                std::string callback_id = event.get_descriptive_id();
+                total_time[callback_id] += event.processing_time;
+                call_count[callback_id]++;
+                max_time[callback_id] = std::max(max_time[callback_id], event.processing_time);
+                if (min_time[callback_id] == 0 || event.processing_time < min_time[callback_id]) {
+                    min_time[callback_id] = event.processing_time;
+                }
+            }
+        }
+        
+        // Print results sorted by callback id
+        std::vector<std::string> callback_ids;
+        for (const auto& entry : call_count) {
+            callback_ids.push_back(entry.first);
+        }
+        
+        std::sort(callback_ids.begin(), callback_ids.end());
+        
+        for (const auto& id : callback_ids) {
+            double avg_time = static_cast<double>(total_time[id]) / call_count[id];
+            ss << id << "\n";
+            ss << "  Calls: " << call_count[id] << "\n";
+            ss << "  Avg time: " << avg_time << " µs\n";
+            ss << "  Min time: " << min_time[id] << " µs\n";
+            ss << "  Max time: " << max_time[id] << " µs\n";
+            ss << "  Total time: " << total_time[id] << " µs\n\n";
+        }
+        
+        return ss.str();
+    }
+
+private:
+    PerformanceMonitor();
+    ~PerformanceMonitor() = default;
+    
+    PerformanceMonitor(const PerformanceMonitor&) = delete;
+    PerformanceMonitor& operator=(const PerformanceMonitor&) = delete;
+    PerformanceMonitor(PerformanceMonitor&&) = delete;
+    PerformanceMonitor& operator=(PerformanceMonitor&&) = delete;
+
+    mutable std::mutex event_mutex_;
+    std::vector<PerformanceEvent> event_buffer_;
+    size_t buffer_size_{10000};
+    size_t auto_dump_threshold_{0};
+    std::string dump_path_{"performance_logs"};
+    std::atomic<bool> enabled_{true};
+    
+    std::string eventsToJson() const;
+    void checkAndAutoDump();
+};
+
+}  // namespace priority_executor
+
+#endif  // PERFORMANCE_MONITOR_HPP_

src/priority_executor/include/priority_executor/priority_executor.hpp

@@ -12,42 +12,39 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RTIS_TIMED_EXECUTOR
+#ifndef PRIORITY_EXECUTOR__PRIORITY_EXECUTOR_HPP_
-#define RTIS_TIMED_EXECUTOR
+#define PRIORITY_EXECUTOR__PRIORITY_EXECUTOR_HPP_
 
 #include <rmw/rmw.h>
 
 #include <cassert>
 #include <cstdlib>
 #include <memory>
-#include <vector>
+#include <string>
-#include <time.h>
 
 #include "rclcpp/executor.hpp"
 #include "rclcpp/macros.hpp"
-#include "rclcpp/memory_strategies.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/utilities.hpp"
-#include "rclcpp/rate.hpp"
 #include "rclcpp/visibility_control.hpp"
+
 #include "priority_executor/priority_memory_strategy.hpp"
-#include <priority_executor/default_executor.hpp>
+#include "priority_executor/default_executor.hpp"
-namespace timed_executor
+#include "priority_executor/performance_monitor.hpp"
-{
+
+namespace priority_executor {
 
 /// Single-threaded executor implementation.
 /**
  * This is the default executor created by rclcpp::spin.
  */
-  class TimedExecutor : public rclcpp::Executor, public RTISTimed
+class TimedExecutor : public rclcpp::Executor, public RTISTimed {
-  {
 public:
     RCLCPP_SMART_PTR_DEFINITIONS(TimedExecutor)
 
     /// Default constructor. See the default constructor for Executor.
     RCLCPP_PUBLIC
     explicit TimedExecutor(
-        const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions(), std::string name = "unnamed executor");
+        rclcpp::ExecutorOptions const &options = rclcpp::ExecutorOptions(),
+        std::string name = "unnamed executor");
 
     /// Default destructor.
     RCLCPP_PUBLIC
@@ -62,28 +59,68 @@ namespace timed_executor
      * \throws std::runtime_error when spin() called while already spinning
      */
     RCLCPP_PUBLIC
-    void
+    void spin() override;
-    spin() override;
-    std::string name;
 
+    std::string name;
     void set_use_priorities(bool use_prio);
-    std::shared_ptr<PriorityMemoryStrategy<>> prio_memory_strategy_ = nullptr;
+    std::shared_ptr<PriorityMemoryStrategy<>> prio_memory_strategy_{nullptr};
+
+    // Performance monitoring control
+    void enableMonitoring(bool enable = true) {
+        PerformanceMonitor::getInstance().enable(enable);
+    }
+
+    void setMonitoringOptions(
+        size_t buffer_size,
+        size_t auto_dump_threshold,
+        const std::string& dump_path) {
+        auto& monitor = PerformanceMonitor::getInstance();
+        monitor.setBufferSize(buffer_size);
+        monitor.setAutoDumpThreshold(auto_dump_threshold);
+        monitor.setDumpPath(dump_path);
+    }
+    
+    // Get a formatted report of all callback execution times
+    std::string getCallbackExecutionReport() const {
+        auto& monitor = PerformanceMonitor::getInstance();
+        return monitor.getCallbackExecutionReport();
+    }
+    
+    // Manually trigger a log dump with a specific filename
+    bool dumpMonitoringData(const std::string& filename) {
+        auto& monitor = PerformanceMonitor::getInstance();
+        return monitor.dumpToFile(filename);
+    }
+    
+    // Get executor ID for identification in logs
+    int getExecutorId() const {
+        return executor_id_;
+    }
+    
+    // Get executor name for identification in logs
+    const std::string& getExecutorName() const {
+        return name;
+    }
 
 protected:
-    bool
+    bool get_next_executable(rclcpp::AnyExecutable& any_executable,
-    get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+                           std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
 
 private:
     RCLCPP_DISABLE_COPY(TimedExecutor)
-    void
-    wait_for_work(std::chrono::nanoseconds timeout);
 
-    bool
+    void wait_for_work(std::chrono::nanoseconds timeout);
-    get_next_ready_executable(rclcpp::AnyExecutable &any_executable);
+    bool get_next_ready_executable(rclcpp::AnyExecutable& any_executable);
+    void recordExecutionEvent(const rclcpp::AnyExecutable& any_exec,
+                            PerformanceEventType event_type,
+                            uint64_t timestamp,
+                            uint64_t processing_time = 0);
 
-    bool use_priorities = true;
+    bool use_priorities{true};
+    static std::atomic<int> next_executor_id_;
+    int executor_id_;
 };
 
-} // namespace timed_executor
+}  // namespace priority_executor
 
-#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_
+#endif  // PRIORITY_EXECUTOR__PRIORITY_EXECUTOR_HPP_

src/priority_executor/include/priority_executor/priority_memory_strategy.hpp

@@ -12,37 +12,34 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef RTIS_PRIORITY_STRATEGY
+#ifndef PRIORITY_EXECUTOR__PRIORITY_MEMORY_STRATEGY_HPP_
-#define RTIS_PRIORITY_STRATEGY
+#define PRIORITY_EXECUTOR__PRIORITY_MEMORY_STRATEGY_HPP_
 
+#include <cstdint>
+#include <deque>
 #include <memory>
-#include <vector>
+#include <semaphore.h>
-#include <queue>
-#include <time.h>
 #include <set>
+#include <time.h>
+#include <vector>
 
 #include "rcl/allocator.h"
 
 #include "rclcpp/allocator/allocator_common.hpp"
 #include "rclcpp/memory_strategy.hpp"
-#include "rclcpp/node.hpp"
-#include "rclcpp/visibility_control.hpp"
-
-#include "rcutils/logging_macros.h"
-
-#include "rmw/types.h"
 
 #include "simple_timer/rt-sched.hpp"
 
 /// Delegate for handling memory allocations while the Executor is executing.
 /**
- * By default, the memory strategy dynamically allocates memory for structures that come in from
+ * By default, the memory strategy dynamically allocates memory for structures
- * the rmw implementation after the executor waits for work, based on the number of entities that
+ * that come in from the rmw implementation after the executor waits for work,
- * come through.
+ * based on the number of entities that come through.
  */
 
-enum ExecutableType
+namespace priority_executor {
-{
+
+enum class ExecutableType {
     SUBSCRIPTION,
     SERVICE,
     CLIENT,
@@ -50,303 +47,139 @@ enum ExecutableType
     WAITABLE
 };
 
-enum ExecutableScheduleType
+std::ostream& operator<<(std::ostream& os, const ExecutableType& obj);
-{
+
+enum class ExecutableScheduleType {
     DEADLINE = 0,
     CHAIN_AWARE_PRIORITY,
     CHAIN_INDEPENDENT_PRIORITY,  // not used here
     DEFAULT,                     // not used here
 };
 
-class PriorityExecutable
+std::ostream& operator<<(std::ostream& os, const ExecutableScheduleType& obj);
-{
+
+class PriorityExecutable {
 public:
+    static size_t num_executables;
+
+    explicit PriorityExecutable(
+        std::shared_ptr<const void> h,
+        int p,
+        ExecutableType t,
+        ExecutableScheduleType sched_type = ExecutableScheduleType::CHAIN_INDEPENDENT_PRIORITY);
+
+    PriorityExecutable();
+
+    void dont_run();
+    void allow_run();
+    void increment_counter();
+    bool operator==(PriorityExecutable const &other) const;
+    friend std::ostream& operator<<(std::ostream& os, PriorityExecutable const &pe);
+
     std::shared_ptr<const void> handle;
     ExecutableType type;
     bool can_be_run = false;
     std::shared_ptr<rclcpp::Waitable> waitable;
     ExecutableScheduleType sched_type;
 
-    int priority;
+    int priority = 0;
     long period = 1000;  // milliseconds
 
     bool is_first_in_chain = false;
     bool is_last_in_chain = false;
-    // chain aware deadlines
+    std::deque<uint64_t>* deadlines = nullptr;  // chain aware deadlines
-    std::deque<uint64_t> *deadlines = nullptr;
     std::shared_ptr<rclcpp::TimerBase> timer_handle;
-    // just used for logging
+    int chain_id = 0;  // just used for logging
-    int chain_id = 0;
+    int counter = 0;   // chain aware priority
-
-    // chain aware priority
-    int counter = 0;
-
-    PriorityExecutable(std::shared_ptr<const void> h, int p, ExecutableType t, ExecutableScheduleType sched_type = CHAIN_INDEPENDENT_PRIORITY);
-
-    void dont_run();
-
-    void allow_run();
-
-    PriorityExecutable();
-
-    void increment_counter();
-
-    bool operator==(const PriorityExecutable &other) const;
-
-    static size_t num_executables;
     int executable_id = 0;
-
     std::string name = "";
 };
 
-class PriorityExecutableComparator
+class PriorityExecutableComparator {
-{
 public:
-    bool operator()(const PriorityExecutable *p1, const PriorityExecutable *p2);
+    bool operator()(PriorityExecutable const *p1, PriorityExecutable const *p2) const;
 };
 
 template <typename Alloc = std::allocator<void>>
-class PriorityMemoryStrategy : public rclcpp::memory_strategy::MemoryStrategy
+class PriorityMemoryStrategy : public rclcpp::memory_strategy::MemoryStrategy {
-{
 public:
     RCLCPP_SMART_PTR_DEFINITIONS(PriorityMemoryStrategy<Alloc>)
 
     using VoidAllocTraits = typename rclcpp::allocator::AllocRebind<void*, Alloc>;
     using VoidAlloc = typename VoidAllocTraits::allocator_type;
 
-    explicit PriorityMemoryStrategy(std::shared_ptr<Alloc> allocator)
+    explicit PriorityMemoryStrategy(std::shared_ptr<Alloc> allocator);
-    {
+    PriorityMemoryStrategy();
-        allocator_ = std::make_shared<VoidAlloc>(*allocator.get());
-        logger_ = create_logger();
-    }
 
-    PriorityMemoryStrategy()
-    {
-        allocator_ = std::make_shared<VoidAlloc>();
-        logger_ = create_logger();
-    }
     node_time_logger logger_;
 
+    // Override methods
     void add_guard_condition(const rcl_guard_condition_t* guard_condition) override;
-
     void remove_guard_condition(const rcl_guard_condition_t* guard_condition) override;
-
     void clear_handles() override;
-
     void remove_null_handles(rcl_wait_set_t* wait_set) override;
-
     bool collect_entities(const WeakNodeList& weak_nodes) override;
-
     void add_waitable_handle(const rclcpp::Waitable::SharedPtr& waitable) override;
-
     bool add_handles_to_wait_set(rcl_wait_set_t* wait_set) override;
 
-    void
+    // Class-specific methods
-    get_next_executable(
+    void get_next_executable(rclcpp::AnyExecutable& any_exec, const WeakNodeList& weak_nodes);
-        rclcpp::AnyExecutable &any_exec,
-        const WeakNodeList &weak_nodes);
-
-    /**
-     * thread-id is used for logging. if -1, then don't log the thread id
-     */
     void post_execute(rclcpp::AnyExecutable any_exec, int thread_id = -1);
 
-    void
+    // Override virtual methods
-    get_next_subscription(
+    void get_next_subscription(rclcpp::AnyExecutable& any_exec, const WeakNodeList& weak_nodes) override;
-        rclcpp::AnyExecutable &any_exec,
+    void get_next_service(rclcpp::AnyExecutable& any_exec, const WeakNodeList& weak_nodes) override;
-        const WeakNodeList &weak_nodes) override;
+    void get_next_client(rclcpp::AnyExecutable& any_exec, const WeakNodeList& weak_nodes) override;
+    void get_next_timer(rclcpp::AnyExecutable& any_exec, const WeakNodeList& weak_nodes) override;
+    void get_next_waitable(rclcpp::AnyExecutable& any_exec, const WeakNodeList& weak_nodes) override;
 
-    void
+    // Priority handling methods
-    get_next_service(
+    std::shared_ptr<PriorityExecutable> get_priority_settings(std::shared_ptr<const void> executable);
-        rclcpp::AnyExecutable &any_exec,
+    rcl_allocator_t get_allocator() override;
-        const WeakNodeList &weak_nodes) override;
 
-    void
+    // Counter methods
-    get_next_client(rclcpp::AnyExecutable &any_exec, const WeakNodeList &weak_nodes) override;
+    size_t number_of_ready_subscriptions() const override;
+    size_t number_of_ready_services() const override;
+    size_t number_of_ready_events() const override;
+    size_t number_of_ready_clients() const override;
+    size_t number_of_guard_conditions() const override;
+    size_t number_of_ready_timers() const override;
+    size_t number_of_waitables() const override;
 
-    void
+    // Executable priority methods
-    get_next_timer(
+    void set_executable_priority(std::shared_ptr<const void> handle, int priority, ExecutableType t);
-        rclcpp::AnyExecutable &any_exec,
+    void set_executable_priority(std::shared_ptr<const void> handle, int priority, ExecutableType t, 
-        const WeakNodeList &weak_nodes) override;
+                               ExecutableScheduleType sc, int chain_index);
-
+    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, 
-    void
+                               int chain_id = 0, std::string name = "");
-    get_next_waitable(rclcpp::AnyExecutable &any_exec, const WeakNodeList &weak_nodes) override;
+    int get_priority(std::shared_ptr<const void> executable);
-
+    void set_first_in_chain(std::shared_ptr<const void> exec_handle);
-    std::shared_ptr<PriorityExecutable> get_priority_settings(std::shared_ptr<const void> executable)
+    void set_last_in_chain(std::shared_ptr<const void> exec_handle);
-    {
+    void assign_deadlines_queue(std::shared_ptr<const void> exec_handle, std::deque<uint64_t>* deadlines);
-        auto search = priority_map.find(executable);
+    std::shared_ptr<std::deque<uint64_t>> get_chain_deadlines(int chain_id);
-        if (search != priority_map.end())
-        {
-            return search->second;
-        }
-        else
-        {
-            return nullptr;
-        }
-    }
-
-    rcl_allocator_t get_allocator() override
-    {
-        return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
-    }
-
-    size_t number_of_ready_subscriptions() const override
-    {
-        size_t number_of_subscriptions = subscription_handles_.size();
-        // std::cout << "ready_raw: " << number_of_subscriptions << std::endl;
-        for (auto waitable : waitable_handles_)
-        {
-            number_of_subscriptions += waitable->get_number_of_ready_subscriptions();
-        }
-        return number_of_subscriptions;
-    }
-
-    size_t number_of_ready_services() const override
-    {
-        size_t number_of_services = service_handles_.size();
-        for (auto waitable : waitable_handles_)
-        {
-            number_of_services += waitable->get_number_of_ready_services();
-        }
-        return number_of_services;
-    }
-
-    size_t number_of_ready_events() const override
-    {
-        size_t number_of_events = 0;
-        for (auto waitable : waitable_handles_)
-        {
-            number_of_events += waitable->get_number_of_ready_events();
-        }
-        return number_of_events;
-    }
-
-    size_t number_of_ready_clients() const override
-    {
-        size_t number_of_clients = client_handles_.size();
-        for (auto waitable : waitable_handles_)
-        {
-            number_of_clients += waitable->get_number_of_ready_clients();
-        }
-        return number_of_clients;
-    }
-
-    size_t number_of_guard_conditions() const override
-    {
-        size_t number_of_guard_conditions = guard_conditions_.size();
-        for (auto waitable : waitable_handles_)
-        {
-            number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
-        }
-        return number_of_guard_conditions;
-    }
-
-    size_t number_of_ready_timers() const override
-    {
-        size_t number_of_timers = timer_handles_.size();
-        for (auto waitable : waitable_handles_)
-        {
-            number_of_timers += waitable->get_number_of_ready_timers();
-        }
-        return number_of_timers;
-    }
-
-    size_t number_of_waitables() const override
-    {
-        return waitable_handles_.size();
-    }
-
-    void set_executable_priority(std::shared_ptr<const void> handle, int priority, ExecutableType t)
-    {
-        // TODO: any sanity checks should go here
-        priority_map[handle] = std::make_shared<PriorityExecutable>(handle, priority, t);
-    }
-    void set_executable_priority(std::shared_ptr<const void> handle, int priority, ExecutableType t, ExecutableScheduleType sc, int chain_index)
-    {
-        // TODO: any sanity checks should go here
-        priority_map[handle] = std::make_shared<PriorityExecutable>(handle, priority, t, sc);
-        priority_map[handle]->chain_id = chain_index;
-    }
-
-    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, int chain_id = 0, std::string name = "")
-    {
-        // TODO: any sanity checks should go here
-        priority_map[handle] = std::make_shared<PriorityExecutable>(handle, period, t, DEADLINE);
-        priority_map[handle]->chain_id = chain_id;
-        priority_map[handle]->name = name;
-    }
-
-    int get_priority(std::shared_ptr<const void> executable)
-    {
-        auto search = priority_map.find(executable);
-        if (search != priority_map.end())
-        {
-            return search->second->priority;
-        }
-        else
-        {
-            return 0;
-        }
-    }
-
-
-    void set_first_in_chain(std::shared_ptr<const void> exec_handle)
-    {
-        get_priority_settings(exec_handle)->is_first_in_chain = true;
-    }
-
-    void set_last_in_chain(std::shared_ptr<const void> exec_handle)
-    {
-        get_priority_settings(exec_handle)->is_last_in_chain = true;
-    }
-
-    void assign_deadlines_queue(std::shared_ptr<const void> exec_handle, std::deque<uint64_t> *deadlines)
-    {
-        get_priority_settings(exec_handle)->deadlines = deadlines;
-    }
 
 private:
-    std::shared_ptr<PriorityExecutable> get_and_reset_priority(std::shared_ptr<const void> executable, ExecutableType t)
+    std::shared_ptr<PriorityExecutable> get_and_reset_priority(std::shared_ptr<const void> executable, 
-    {
+                                                              ExecutableType t);
-        // PriorityExecutable *p = get_priority_settings(executable);
-        std::shared_ptr<PriorityExecutable> p = get_priority_settings(executable);
-        if (p == nullptr)
-        {
-            // priority_map[executable] = PriorityExecutable(executable, 0, t);
-            priority_map[executable] = std::make_shared<PriorityExecutable>(executable, 0, t);
-            p = priority_map[executable];
-        }
-        // p->can_be_run = true;
-        return p;
-    }
 
     template <typename T>
-    using VectorRebind =
+    using VectorRebind = std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
-        std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
+    // Member variables
     VectorRebind<const rcl_guard_condition_t*> guard_conditions_;
-
     VectorRebind<std::shared_ptr<const rcl_subscription_t>> subscription_handles_;
     VectorRebind<std::shared_ptr<const rcl_service_t>> service_handles_;
     VectorRebind<std::shared_ptr<const rcl_client_t>> client_handles_;
     VectorRebind<std::shared_ptr<const rcl_timer_t>> timer_handles_;
     VectorRebind<std::shared_ptr<rclcpp::Waitable>> waitable_handles_;
-
     std::shared_ptr<VoidAlloc> allocator_;
-
-    // TODO: evaluate using node/subscription namespaced strings as keys
-
-    // holds *all* handle->priority mappings
     std::map<std::shared_ptr<const void>, std::shared_ptr<PriorityExecutable>> priority_map;
-
+    std::map<int, std::shared_ptr<std::deque<uint64_t>>> chain_deadlines;
-    // hold *only valid* executable+priorities
-    // std::priority_queue<const PriorityExecutable *, std::vector<const PriorityExecutable *>, PriorityExecutableComparator> all_executables_;
     std::set<const PriorityExecutable*, PriorityExecutableComparator> all_executables_;
 
-    // priority queue doesn't allow iteration. fortunately, std::map is sorted by key, so we can replace the priority queue with a map
-    // the key will be the priority. the value doesn't matter.
-    // std::map<const PriorityExecutable *, int, PriorityExecutableComparator> all_executables_ = std::map<const PriorityExecutable *, int, PriorityExecutableComparator>(PriorityExecutableComparator());
     void add_executable_to_queue(std::shared_ptr<PriorityExecutable> p);
 };
 
-#endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_
+}  // namespace priority_executor
+
+#endif  // PRIORITY_EXECUTOR__PRIORITY_MEMORY_STRATEGY_HPP_

src/priority_executor/package.xml

@@ -2,15 +2,18 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>priority_executor</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
-  <description>TODO: Package description</description>
+  <description>
+    ROS 2 package implementing priority-based executors for real-time task scheduling
+  </description>
   <maintainer email="kurt4wilson@gmail.com">kurt</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>rcl</depend>
+  <depend>rmw</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>simple_timer</depend>

src/priority_executor/src/default_executor.cpp

@@ -13,42 +13,51 @@
 // limitations under the License.
 
 #include "priority_executor/default_executor.hpp"
+
+#include <mutex>
+#include <chrono>
+#include <memory>
+#include <thread>
+#include <sstream>
+#include <functional>
+#include <unordered_map>
+
 #include "rcpputils/scope_exit.hpp"
 #include "rclcpp/any_executable.hpp"
 #include "simple_timer/rt-sched.hpp"
 
-ROSDefaultExecutor::ROSDefaultExecutor(const rclcpp::ExecutorOptions &options)
+namespace priority_executor {
-    : rclcpp::Executor(options)
+
-{
+std::unordered_map<ROSDefaultMultithreadedExecutor*,
+                   std::shared_ptr<rclcpp::detail::MutexTwoPriorities>>
+    ROSDefaultMultithreadedExecutor::wait_mutex_set_;
+std::mutex ROSDefaultMultithreadedExecutor::shared_wait_mutex_;
+
+ROSDefaultExecutor::ROSDefaultExecutor(rclcpp::ExecutorOptions const &options)
+    : rclcpp::Executor(options) {
     logger_ = create_logger();
 }
 
 ROSDefaultExecutor::~ROSDefaultExecutor() {}
 
-void ROSDefaultExecutor::wait_for_work(std::chrono::nanoseconds timeout)
+void ROSDefaultExecutor::wait_for_work(std::chrono::nanoseconds timeout) {
-{
     {
         std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
 
         // Collect the subscriptions and timers to be waited on
         memory_strategy_->clear_handles();
-    bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+        bool const has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
 
         // Clean up any invalid nodes, if they were detected
-    if (has_invalid_weak_nodes)
+        if (has_invalid_weak_nodes) {
-    {
             auto node_it = weak_nodes_.begin();
             auto gc_it = guard_conditions_.begin();
-      while (node_it != weak_nodes_.end())
+            while (node_it != weak_nodes_.end()) {
-      {
+                if (node_it->expired()) {
-        if (node_it->expired())
-        {
                     node_it = weak_nodes_.erase(node_it);
                     memory_strategy_->remove_guard_condition(*gc_it);
                     gc_it = guard_conditions_.erase(gc_it);
-        }
+                } else {
-        else
-        {
                     ++node_it;
                     ++gc_it;
                 }
@@ -56,37 +65,33 @@ void ROSDefaultExecutor::wait_for_work(std::chrono::nanoseconds timeout)
         }
         // clear wait set
         rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
-    if (ret != RCL_RET_OK)
+        if (ret != RCL_RET_OK) {
-    {
             rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear wait set");
         }
 
         // The size of waitables are accounted for in size of the other entities
         ret = rcl_wait_set_resize(
             &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
-        memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+            memory_strategy_->number_of_guard_conditions(),
-        memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
+            memory_strategy_->number_of_ready_timers(),
+            memory_strategy_->number_of_ready_clients(),
+            memory_strategy_->number_of_ready_services(),
             memory_strategy_->number_of_ready_events());
-    if (RCL_RET_OK != ret)
+        if (RCL_RET_OK != ret) {
-    {
             rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set");
         }
 
-    if (!memory_strategy_->add_handles_to_wait_set(&wait_set_))
+        if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) {
-    {
             throw std::runtime_error("Couldn't fill wait set");
         }
     }
-  rcl_ret_t status =
+    rcl_ret_t const status =
         rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
-  if (status == RCL_RET_WAIT_SET_EMPTY)
+    if (status == RCL_RET_WAIT_SET_EMPTY) {
-  {
         RCUTILS_LOG_WARN_NAMED(
             "rclcpp",
             "empty wait set received in rcl_wait(). This should never happen.");
-  }
+    } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
-  else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT)
-  {
         using rclcpp::exceptions::throw_from_rcl_error;
         throw_from_rcl_error(status, "rcl_wait() failed");
     }
@@ -96,8 +101,8 @@ void ROSDefaultExecutor::wait_for_work(std::chrono::nanoseconds timeout)
     memory_strategy_->remove_null_handles(&wait_set_);
 }
 
-bool ROSDefaultExecutor::get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout)
+bool ROSDefaultExecutor::get_next_executable(rclcpp::AnyExecutable& any_executable,
-{
+                                           std::chrono::nanoseconds timeout) {
     bool success = false;
     // Check to see if there are any subscriptions or timers needing service
     // TODO(wjwwood): improve run to run efficiency of this function
@@ -107,76 +112,28 @@ bool ROSDefaultExecutor::get_next_executable(rclcpp::AnyExecutable &any_executab
     return success;
 }
 
-void ROSDefaultExecutor::spin()
+void ROSDefaultExecutor::spin() {
-{
+    if (spinning.exchange(true)) {
-  if (spinning.exchange(true))
-  {
         throw std::runtime_error("spin() called while already spinning");
     }
     RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
-  while (rclcpp::ok(this->context_) && spinning.load())
+    while (rclcpp::ok(this->context_) && spinning.load()) {
-  {
         rclcpp::AnyExecutable any_executable;
-    if (get_next_executable(any_executable))
+        if (get_next_executable(any_executable)) {
-    {
             execute_any_executable(any_executable);
         }
     }
 }
 
-bool ROSDefaultMultithreadedExecutor::get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout)
+size_t ROSDefaultMultithreadedExecutor::get_number_of_threads() {
-{
+    return number_of_threads_;
-  bool success = false;
-  // Check to see if there are any subscriptions or timers needing service
-  // TODO(wjwwood): improve run to run efficiency of this function
-
-  // try to get an executable
-  // record the start time
-  auto start = std::chrono::steady_clock::now();
-  success = get_next_ready_executable(any_executable);
-  // and the end time
-  auto end = std::chrono::steady_clock::now();
-  std::stringstream oss;
-  oss << "{\"operation\":\"get_next_executable\", \"result\":\"" << success << "\", \"duration\":\"" << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() << "\"}";
-  log_entry(logger_, oss.str());
-
-  // If there are none
-  if (!success)
-  {
-    // Wait for subscriptions or timers to work on
-    // queue refresh
-    start = std::chrono::steady_clock::now();
-    wait_for_work(timeout);
-    // and the end time
-    end = std::chrono::steady_clock::now();
-    auto wait_duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
-    oss.str("");
-    oss << "{\"operation\":\"wait_for_work\", \"result\":\"" << success << "\", \"wait_duration\":\"" << wait_duration << "\"}";
-    log_entry(logger_, oss.str());
-    if (!spinning.load())
-    {
-      return false;
 }
-    // Try again
-    start = std::chrono::steady_clock::now();
-    success = get_next_ready_executable(any_executable);
-    // and the end time
-    end = std::chrono::steady_clock::now();
-    oss.str("");
-    oss << "{\"operation\":\"get_next_executable\", \"result\":\"" << success << "\", \"duration\":\"" << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() << "\"}";
-    log_entry(logger_, oss.str());
-  }
-  return success;
-}
-
-std::unordered_map<ROSDefaultMultithreadedExecutor *, std::shared_ptr<rclcpp::detail::MutexTwoPriorities>> ROSDefaultMultithreadedExecutor::wait_mutex_set_;
-std::mutex ROSDefaultMultithreadedExecutor::shared_wait_mutex_;
 
 ROSDefaultMultithreadedExecutor::ROSDefaultMultithreadedExecutor(
-    const rclcpp::ExecutorOptions &options,
+    rclcpp::ExecutorOptions const &options,
-    int number_of_threads, std::chrono::nanoseconds next_exec_timeout)
+    int number_of_threads,
-    : Executor(options)
+    std::chrono::nanoseconds next_exec_timeout)
-{
+    : Executor(options) {
     std::lock_guard<std::mutex> wait_lock(ROSDefaultMultithreadedExecutor::shared_wait_mutex_);
     wait_mutex_set_[this] = std::make_shared<rclcpp::detail::MutexTwoPriorities>();
     number_of_threads_ = number_of_threads;
@@ -184,10 +141,8 @@ ROSDefaultMultithreadedExecutor::ROSDefaultMultithreadedExecutor(
     logger_ = create_logger();
 }
 
-void ROSDefaultMultithreadedExecutor::spin()
+void ROSDefaultMultithreadedExecutor::spin() {
-{
+    if (spinning.exchange(true)) {
-  if (spinning.exchange(true))
-  {
         throw std::runtime_error("spin() called while already spinning");
     }
 
@@ -196,45 +151,39 @@ void ROSDefaultMultithreadedExecutor::spin()
     {
         auto wait_mutex = ROSDefaultMultithreadedExecutor::wait_mutex_set_[this];
         auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
-    std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+        std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable>
-    for (; thread_id < number_of_threads_ - 1; ++thread_id)
+            wait_lock(low_priority_wait_mutex);
-    {
+        for (; thread_id < number_of_threads_ - 1; ++thread_id) {
-      auto func = std::bind(&ROSDefaultMultithreadedExecutor::run, this, thread_id);
+            auto const func = std::bind(&ROSDefaultMultithreadedExecutor::run, this, thread_id);
             threads.emplace_back(func);
         }
     }
     run(thread_id);
-  for (auto &thread : threads)
+    for (auto& thread : threads) {
-  {
         thread.join();
     }
 }
 
-void ROSDefaultMultithreadedExecutor::run(__attribute__((unused)) size_t _thread_number) {
+void ROSDefaultMultithreadedExecutor::run(size_t thread_number) {
-  while (rclcpp::ok(this->context_) && spinning.load())
+    while (rclcpp::ok(this->context_) && spinning.load()) {
-  {
         rclcpp::AnyExecutable any_exec;
         {
             auto wait_mutex = ROSDefaultMultithreadedExecutor::wait_mutex_set_[this];
             auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
-      std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+            std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable>
-      if (!rclcpp::ok(this->context_) || !spinning.load())
+                wait_lock(low_priority_wait_mutex);
-      {
+            if (!rclcpp::ok(this->context_) || !spinning.load()) {
                 return;
             }
-      if (!get_next_executable(any_exec, next_exec_timeout_))
+            if (!get_next_executable(any_exec, next_exec_timeout_)) {
-      {
                 continue;
             }
-      if (any_exec.timer)
+            if (any_exec.timer) {
-      {
                 // Guard against multiple threads getting the same timer.
-        if (scheduled_timers_.count(any_exec.timer) != 0)
+                if (scheduled_timers_.count(any_exec.timer) != 0) {
-        {
                     // Make sure that any_exec's callback group is reset before
                     // the lock is released.
-          if (any_exec.callback_group)
+                    if (any_exec.callback_group) {
-          {
                         any_exec.callback_group->can_be_taken_from().store(true);
                     }
                     continue;
@@ -242,21 +191,16 @@ void ROSDefaultMultithreadedExecutor::run(__attribute__((unused)) size_t _thread
                 scheduled_timers_.insert(any_exec.timer);
             }
         }
-    // if (yield_before_execute_)
-    // {
-    //   std::this_thread::yield();
-    // }
 
         execute_any_executable(any_exec);
 
-    if (any_exec.timer)
+        if (any_exec.timer) {
-    {
             auto wait_mutex = ROSDefaultMultithreadedExecutor::wait_mutex_set_[this];
             auto high_priority_wait_mutex = wait_mutex->get_high_priority_lockable();
-      std::lock_guard<rclcpp::detail::MutexTwoPriorities::HighPriorityLockable> wait_lock(high_priority_wait_mutex);
+            std::lock_guard<rclcpp::detail::MutexTwoPriorities::HighPriorityLockable>
-      auto it = scheduled_timers_.find(any_exec.timer);
+                wait_lock(high_priority_wait_mutex);
-      if (it != scheduled_timers_.end())
+            auto const it = scheduled_timers_.find(any_exec.timer);
-      {
+            if (it != scheduled_timers_.end()) {
                 scheduled_timers_.erase(it);
             }
         }
@@ -265,3 +209,57 @@ void ROSDefaultMultithreadedExecutor::run(__attribute__((unused)) size_t _thread
         any_exec.callback_group.reset();
     }
 }
+
+bool ROSDefaultMultithreadedExecutor::get_next_executable(rclcpp::AnyExecutable& any_executable,
+                                                        std::chrono::nanoseconds timeout) {
+    bool success = false;
+    // Check to see if there are any subscriptions or timers needing service
+    // TODO(wjwwood): improve run to run efficiency of this function
+
+    // try to get an executable
+    // record the start time
+    auto const start = std::chrono::steady_clock::now();
+    success = get_next_ready_executable(any_executable);
+    // and the end time
+    auto const end = std::chrono::steady_clock::now();
+    std::stringstream oss;
+    oss << "{\"operation\":\"get_next_executable\", \"result\":\"" << success
+        << "\", \"duration\":\""
+        << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count()
+        << "\"}";
+    log_entry(logger_, oss.str());
+
+    // If there are none
+    if (!success) {
+        // Wait for subscriptions or timers to work on
+        // queue refresh
+        auto const wait_start = std::chrono::steady_clock::now();
+        wait_for_work(timeout);
+        // and the end time
+        auto const wait_end = std::chrono::steady_clock::now();
+        auto const wait_duration = std::chrono::duration_cast<std::chrono::microseconds>(
+            wait_end - wait_start).count();
+        oss.str("");
+        oss << "{\"operation\":\"wait_for_work\", \"result\":\"" << success
+            << "\", \"wait_duration\":\"" << wait_duration << "\"}";
+        log_entry(logger_, oss.str());
+        if (!spinning.load()) {
+            return false;
+        }
+        // Try again
+        auto const retry_start = std::chrono::steady_clock::now();
+        success = get_next_ready_executable(any_executable);
+        // and the end time
+        auto const retry_end = std::chrono::steady_clock::now();
+        oss.str("");
+        oss << "{\"operation\":\"get_next_executable\", \"result\":\"" << success
+            << "\", \"duration\":\""
+            << std::chrono::duration_cast<std::chrono::microseconds>(
+                   retry_end - retry_start).count()
+            << "\"}";
+        log_entry(logger_, oss.str());
+    }
+    return success;
+}
+
+}  // namespace priority_executor

src/priority_executor/src/multithread_priority_executor.cpp

@@ -1,15 +1,32 @@
 #include "priority_executor/multithread_priority_executor.hpp"
-namespace timed_executor
+#include "priority_executor/performance_monitor.hpp"
-{
+
-    std::unordered_map<MultithreadTimedExecutor *, std::shared_ptr<rclcpp::detail::MutexTwoPriorities>>
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <thread>
+#include <unordered_map>
+#include <nlohmann/json.hpp>
+
+#include "rcpputils/scope_exit.hpp"
+#include "rclcpp/any_executable.hpp"
+#include "simple_timer/rt-sched.hpp"
+
+namespace priority_executor {
+
+std::unordered_map<MultithreadTimedExecutor*,
+                   std::shared_ptr<rclcpp::detail::MutexTwoPriorities>>
     MultithreadTimedExecutor::wait_mutex_set_;
 std::mutex MultithreadTimedExecutor::shared_wait_mutex_;
+
 MultithreadTimedExecutor::MultithreadTimedExecutor(
-        const rclcpp::ExecutorOptions &options,
+    rclcpp::ExecutorOptions const &options,
-        std::string name,
+    std::string const &name,
-        int number_of_threads, std::chrono::nanoseconds next_exec_timeout)
+    int number_of_threads,
-        : TimedExecutor(options, name)
+    std::chrono::nanoseconds next_exec_timeout)
-    {
+    : TimedExecutor(options, name) {
     std::lock_guard<std::mutex> wait_lock(MultithreadTimedExecutor::shared_wait_mutex_);
     wait_mutex_set_[this] = std::make_shared<rclcpp::detail::MutexTwoPriorities>();
     number_of_threads_ = number_of_threads;
@@ -22,10 +39,8 @@ namespace timed_executor
     return number_of_threads_;
 }
 
-    void MultithreadTimedExecutor::spin()
+void MultithreadTimedExecutor::spin() {
-    {
+    if (spinning.exchange(true)) {
-        if (spinning.exchange(true))
-        {
         throw std::runtime_error("spin() called while already spinning");
     }
 
@@ -34,81 +49,239 @@ namespace timed_executor
     {
         auto wait_mutex = MultithreadTimedExecutor::wait_mutex_set_[this];
         auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
-            std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+        std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable>
-            for (; thread_id < number_of_threads_ - 1; ++thread_id)
+            wait_lock(low_priority_wait_mutex);
-            {
+        for (; thread_id < number_of_threads_ - 1; ++thread_id) {
-                auto func = std::bind(&MultithreadTimedExecutor::run, this, thread_id);
+            auto const func = std::bind(&MultithreadTimedExecutor::run, this, thread_id);
             threads.emplace_back(func);
         }
     }
     run(thread_id);
-        for (auto &thread : threads)
+    for (auto& thread : threads) {
-        {
         thread.join();
     }
+    spinning.store(false);
 }
 
-    void MultithreadTimedExecutor::run(size_t thread_number)
+void MultithreadTimedExecutor::run(size_t _thread_number) {
-    {
     // set affinity
     cpu_set_t cpuset;
     CPU_ZERO(&cpuset);
-        CPU_SET(thread_number, &cpuset);
+    CPU_SET(_thread_number, &cpuset);
     int rc = pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);
     if (rc != 0)
     {
-            std::cout << "Error calling pthread_setaffinity_np: " << rc << "\n";
+        RCLCPP_ERROR(rclcpp::get_logger("priority_executor"),
+                     "Error calling pthread_setaffinity_np: %d", rc);
     }
 
-        while (rclcpp::ok(this->context_) && spinning.load())
+    // Create thread context information for logs
-        {
+    nlohmann::json thread_info;
-            rclcpp::AnyExecutable any_executable;
+    thread_info["thread_id"] = _thread_number;
+    thread_info["cpu_affinity"] = _thread_number;
+    std::string thread_info_str = thread_info.dump();
+
+    while (rclcpp::ok(this->context_) && spinning.load()) {
+        rclcpp::AnyExecutable any_exec;
         {
             auto wait_mutex = MultithreadTimedExecutor::wait_mutex_set_[this];
             auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
-                std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
+            std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable>
-                if (!rclcpp::ok(this->context_) || !spinning.load())
+                wait_lock(low_priority_wait_mutex);
-                {
+            if (!rclcpp::ok(this->context_) || !spinning.load()) {
                 return;
             }
-                if (!get_next_executable(any_executable, next_exec_timeout_))
+            if (!get_next_executable(any_exec, next_exec_timeout_)) {
-                {
                 continue;
             }
-                if (any_executable.timer)
+            if (any_exec.timer) {
-                {
+                // Guard against multiple threads getting the same timer.
-                    if (scheduled_timers_.count(any_executable.timer) != 0)
+                if (scheduled_timers_.count(any_exec.timer) != 0) {
-                    {
+                    // Make sure that any_exec's callback group is reset before
-                        if (any_executable.callback_group)
+                    // the lock is released.
-                        {
+                    if (any_exec.callback_group) {
-                            any_executable.callback_group->can_be_taken_from().store(true);
+                        any_exec.callback_group->can_be_taken_from().store(true);
                     }
                     continue;
                 }
-                    scheduled_timers_.insert(any_executable.timer);
+                scheduled_timers_.insert(any_exec.timer);
             }
         }
-            execute_any_executable(any_executable);
+
-            if (any_executable.timer)
+        // Record execution start with thread info
-            {
+        auto start = std::chrono::steady_clock::now();
+        uint64_t start_time = std::chrono::duration_cast<std::chrono::microseconds>(
+            start.time_since_epoch()).count();
+            
+        PerformanceEvent start_event;
+        start_event.timestamp = start_time;
+        start_event.type = PerformanceEventType::CALLBACK_START;
+        start_event.executor_id = getExecutorId();
+        
+        if (any_exec.node_base) {
+            start_event.node_name = any_exec.node_base->get_name();
+            
+            // Enhanced callback identification based on executable type
+            if (any_exec.timer) {
+                // For timer callbacks
+                std::stringstream ss;
+
+                // retrieve the timer period
+                int64_t period;
+                rcl_timer_get_period(any_exec.timer->get_timer_handle().get(), &period);
+
+                // convert period (which is in nanoseconds) to milliseconds
+                std::chrono::nanoseconds period_ns(period);
+                std::chrono::milliseconds period_ms = std::chrono::duration_cast<std::chrono::milliseconds>(period_ns);
+
+                ss << "timer_" << period_ms.count() << "ms";
+                start_event.callback_name = ss.str();
+                
+                // Add memory address as unique identifier
+                std::stringstream addr;
+                addr << "@" << any_exec.timer.get();
+                nlohmann::json additional;
+                additional["callback_type"] = "timer";
+                additional["timer_addr"] = addr.str();
+                additional["thread_info"] = thread_info;
+                start_event.additional_data = additional.dump();
+            } 
+            else if (any_exec.subscription) {
+                // For subscription callbacks
+                start_event.callback_name = any_exec.subscription->get_topic_name();
+                
+                nlohmann::json additional;
+                additional["callback_type"] = "subscription";
+                additional["thread_info"] = thread_info;
+                start_event.additional_data = additional.dump();
+            }
+            else if (any_exec.service) {
+                start_event.callback_name = any_exec.service->get_service_name();
+                
+                nlohmann::json additional;
+                additional["callback_type"] = "service";
+                additional["thread_info"] = thread_info;
+                start_event.additional_data = additional.dump();
+            }
+            else if (any_exec.client) {
+                start_event.callback_name = "client_callback";
+                
+                nlohmann::json additional;
+                additional["callback_type"] = "client";
+                additional["thread_info"] = thread_info;
+                start_event.additional_data = additional.dump();
+            }
+            else if (any_exec.waitable) {
+                start_event.callback_name = "waitable_callback";
+                
+                nlohmann::json additional;
+                additional["callback_type"] = "waitable";
+                additional["thread_info"] = thread_info;
+                start_event.additional_data = additional.dump();
+            }
+            
+            // Add chain information
+            std::shared_ptr<PriorityExecutable> executable = nullptr;
+            if (prio_memory_strategy_) {
+                if (any_exec.timer) {
+                    executable = prio_memory_strategy_->get_priority_settings(
+                        any_exec.timer->get_timer_handle());
+                } else if (any_exec.subscription) {
+                    executable = prio_memory_strategy_->get_priority_settings(
+                        any_exec.subscription->get_subscription_handle());
+                }
+            }
+
+            if (executable) {
+                start_event.chain_id = executable->chain_id;
+                start_event.is_first_in_chain = executable->is_first_in_chain;
+                start_event.is_last_in_chain = executable->is_last_in_chain;
+                
+                if (executable->deadlines && !executable->deadlines->empty()) {
+                    start_event.deadline = executable->deadlines->front();
+                }
+                
+                // Include the priority information
+                try {
+                    nlohmann::json additional = start_event.additional_data.empty() ? 
+                        nlohmann::json() : nlohmann::json::parse(start_event.additional_data);
+                        
+                    additional["priority"] = executable->priority;
+                    
+                    if (executable->chain_id >= 0) {
+                        additional["chain_id"] = executable->chain_id;
+                    }
+                    
+                    if (executable->sched_type != ExecutableScheduleType::DEFAULT) {
+                        additional["schedule_type"] = static_cast<int>(executable->sched_type);
+                    }
+                    
+                    start_event.additional_data = additional.dump();
+                } catch (...) {
+                    // If there was an error parsing the JSON, create new JSON object
+                    nlohmann::json additional;
+                    additional["priority"] = executable->priority;
+                    additional["thread_info"] = thread_info;
+                    start_event.additional_data = additional.dump();
+                }
+            }
+        } else {
+            start_event.additional_data = thread_info_str;
+        }
+        
+        PerformanceMonitor::getInstance().recordEvent(start_event);
+
+        // Execute the callback
+        execute_any_executable(any_exec);
+
+        // Record execution completion
+        auto end = std::chrono::steady_clock::now();
+        uint64_t end_time = std::chrono::duration_cast<std::chrono::microseconds>(
+            end.time_since_epoch()).count();
+        uint64_t processing_time = end_time - start_time;
+
+        // Create identical event structure but for end event
+        PerformanceEvent end_event = start_event;
+        end_event.timestamp = end_time; 
+        end_event.type = PerformanceEventType::CALLBACK_END;
+        end_event.processing_time = processing_time;
+        
+        // Update the additional data to include processing time
+        try {
+            nlohmann::json additional = end_event.additional_data.empty() ? 
+                nlohmann::json() : nlohmann::json::parse(end_event.additional_data);
+            additional["processing_time_us"] = processing_time;
+            end_event.additional_data = additional.dump();
+        } catch (...) {
+            // If JSON parsing failed, create a new object
+            nlohmann::json additional;
+            additional["processing_time_us"] = processing_time;
+            additional["thread_info"] = thread_info;
+            end_event.additional_data = additional.dump();
+        }
+        
+        PerformanceMonitor::getInstance().recordEvent(end_event);
+
+        if (any_exec.timer) {
             auto wait_mutex = MultithreadTimedExecutor::wait_mutex_set_[this];
             auto high_priority_wait_mutex = wait_mutex->get_high_priority_lockable();
-                std::lock_guard<rclcpp::detail::MutexTwoPriorities::HighPriorityLockable> wait_lock(high_priority_wait_mutex);
+            std::lock_guard<rclcpp::detail::MutexTwoPriorities::HighPriorityLockable>
-                auto it = scheduled_timers_.find(any_executable.timer);
+                wait_lock(high_priority_wait_mutex);
-                if (it != scheduled_timers_.end())
+            auto const it = scheduled_timers_.find(any_exec.timer);
-                {
+            if (it != scheduled_timers_.end()) {
                 scheduled_timers_.erase(it);
             }
         }
-            any_executable.callback_group.reset();
+        // Clear the callback_group to prevent the AnyExecutable destructor from
+        // resetting the callback group `can_be_taken_from`
+        any_exec.callback_group.reset();
         if (prio_memory_strategy_ != nullptr)
         {
             auto wait_mutex = MultithreadTimedExecutor::wait_mutex_set_[this];
             auto low_priority_wait_mutex = wait_mutex->get_low_priority_lockable();
             std::lock_guard<rclcpp::detail::MutexTwoPriorities::LowPriorityLockable> wait_lock(low_priority_wait_mutex);
-                // std::shared_ptr<PriorityMemoryStrategy<>> prio_memory_strategy_ = std::dynamic_pointer_cast<PriorityMemoryStrategy>(memory_strategy_);
+            prio_memory_strategy_->post_execute(any_exec, _thread_number);
-                prio_memory_strategy_->post_execute(any_executable, thread_number);
-            }
         }
     }
 }
+}  // namespace priority_executor

src/priority_executor/src/performance_monitor.cpp

@@ -0,0 +1,149 @@
+#include "priority_executor/performance_monitor.hpp"
+#include <filesystem>
+#include <fstream>
+#include <nlohmann/json.hpp>
+#include <sstream>
+#include <iomanip>
+
+namespace priority_executor {
+
+PerformanceMonitor& PerformanceMonitor::getInstance() {
+    static PerformanceMonitor instance;
+    return instance;
+}
+
+PerformanceMonitor::PerformanceMonitor() {
+    event_buffer_.reserve(buffer_size_);
+    std::filesystem::create_directories(dump_path_);
+}
+
+void PerformanceMonitor::recordEvent(const PerformanceEvent& event) {
+    if (!enabled_) return;
+    
+    {
+        std::lock_guard<std::mutex> lock(event_mutex_);
+        event_buffer_.push_back(event);
+        
+        if (auto_dump_threshold_ > 0 && event_buffer_.size() >= auto_dump_threshold_) {
+            checkAndAutoDump();
+        }
+    }
+}
+
+void PerformanceMonitor::setBufferSize(size_t size) {
+    std::lock_guard<std::mutex> lock(event_mutex_);
+    buffer_size_ = size;
+    event_buffer_.reserve(size);
+}
+
+void PerformanceMonitor::setAutoDumpThreshold(size_t threshold) {
+    auto_dump_threshold_ = threshold;
+}
+
+void PerformanceMonitor::setDumpPath(const std::string& path) {
+    dump_path_ = path;
+    std::filesystem::create_directories(dump_path_);
+}
+
+void PerformanceMonitor::enable(bool enabled) {
+    enabled_ = enabled;
+}
+
+void PerformanceMonitor::clear() {
+    std::lock_guard<std::mutex> lock(event_mutex_);
+    event_buffer_.clear();
+}
+
+std::string PerformanceMonitor::eventsToJson() const {
+    nlohmann::json root = nlohmann::json::array();
+    
+    for (const auto& event : event_buffer_) {
+        nlohmann::json event_json;
+        
+        event_json["timestamp"] = event.timestamp;
+        
+        switch (event.type) {
+            case PerformanceEventType::CALLBACK_READY:
+                event_json["type"] = "callback_ready";
+                break;
+            case PerformanceEventType::CALLBACK_START:
+                event_json["type"] = "callback_start";
+                break;
+            case PerformanceEventType::CALLBACK_END:
+                event_json["type"] = "callback_end";
+                break;
+            case PerformanceEventType::DEADLINE_MISSED:
+                event_json["type"] = "deadline_missed";
+                break;
+            case PerformanceEventType::DEADLINE_MET:
+                event_json["type"] = "deadline_met";
+                break;
+            case PerformanceEventType::CHAIN_START:
+                event_json["type"] = "chain_start";
+                break;
+            case PerformanceEventType::CHAIN_END:
+                event_json["type"] = "chain_end";
+                break;
+        }
+        
+        event_json["node_name"] = event.node_name;
+        event_json["callback_name"] = event.callback_name;
+        event_json["chain_id"] = event.chain_id;
+        event_json["is_first_in_chain"] = event.is_first_in_chain;
+        event_json["is_last_in_chain"] = event.is_last_in_chain;
+        
+        if (event.deadline > 0) {
+            event_json["deadline"] = event.deadline;
+        }
+        
+        if (event.processing_time > 0) {
+            event_json["processing_time"] = event.processing_time;
+        }
+        
+        event_json["executor_id"] = event.executor_id;
+        
+        if (!event.additional_data.empty()) {
+            try {
+                event_json["additional_data"] = nlohmann::json::parse(event.additional_data);
+            } catch (...) {
+                event_json["additional_data"] = event.additional_data;
+            }
+        }
+        
+        root.push_back(event_json);
+    }
+    
+    return root.dump(2);  // Indent with 2 spaces
+}
+
+bool PerformanceMonitor::dumpToFile(const std::string& filename) {
+    std::lock_guard<std::mutex> lock(event_mutex_);
+    
+    if (event_buffer_.empty()) {
+        return true;  // Nothing to dump
+    }
+    
+    std::string full_path = dump_path_ + "/" + filename;
+    std::ofstream file(full_path);
+    if (!file.is_open()) {
+        return false;
+    }
+    
+    file << eventsToJson();
+    event_buffer_.clear();
+    return true;
+}
+
+void PerformanceMonitor::checkAndAutoDump() {
+    if (event_buffer_.size() >= auto_dump_threshold_) {
+        auto now = std::chrono::system_clock::now();
+        auto timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(
+            now.time_since_epoch()).count();
+            
+        std::stringstream ss;
+        ss << "performance_log_" << timestamp << ".json";
+        dumpToFile(ss.str());
+    }
+}
+
+}  // namespace priority_executor

src/priority_executor/src/priority_executor.cpp

@@ -19,72 +19,75 @@
 #include "rclcpp/utilities.hpp"
 #include <memory>
 #include <sched.h>
-// for sleep
+#include <unistd.h>  // for sleep
-#include <unistd.h>
+
-namespace timed_executor
+namespace priority_executor
 {
 
-  TimedExecutor::TimedExecutor(const rclcpp::ExecutorOptions &options, std::string name)
+TimedExecutor::TimedExecutor(rclcpp::ExecutorOptions const &options, std::string name)
-      : rclcpp::Executor(options)
+    : rclcpp::Executor(options) {
-  {
+    this->name = std::move(name);
-    this->name = name;
     logger_ = create_logger();
 }
 
-  TimedExecutor::~TimedExecutor() {}
+TimedExecutor::~TimedExecutor() = default;
 
-  void
+void TimedExecutor::spin() {
-  TimedExecutor::spin()
+    if (spinning.exchange(true)) {
-  {
-    if (spinning.exchange(true))
-    {
         throw std::runtime_error("spin() called while already spinning");
     }
     RCLCPP_SCOPE_EXIT(this->spinning.store(false););
-    while (rclcpp::ok(this->context_) && spinning.load())
+
-    {
+    using namespace std::chrono_literals;
+    std::chrono::time_point start = std::chrono::steady_clock::now();
+
+    while (rclcpp::ok(this->context_) && spinning.load() && (std::chrono::steady_clock::now() - start) <= 5s) {
         rclcpp::AnyExecutable any_executable;
         // std::cout<<memory_strategy_->number_of_ready_timers()<<std::endl;
         // std::cout << "spinning " << this->name << std::endl;
         // size_t ready = memory_strategy_->number_of_ready_subscriptions();
         // std::cout << "ready:" << ready << std::endl;
 
-      if (get_next_executable(any_executable, std::chrono::nanoseconds(-1)))
+        if (get_next_executable(any_executable, std::chrono::nanoseconds(-1))) {
-      {
             execute_any_executable(any_executable);
             // make sure memory_strategy_ is an instance of PriorityMemoryStrategy
-        if (prio_memory_strategy_!=nullptr)
+            if (prio_memory_strategy_ != nullptr) {
-        {
                 prio_memory_strategy_->post_execute(any_executable);
             }
         }
     }
-    std::cout << "shutdown" << std::endl;
+    RCLCPP_INFO(rclcpp::get_logger("priority_executor"), "priority executor shutdown");
 }
 
-  bool TimedExecutor::get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout)
+bool TimedExecutor::get_next_executable(
-  {
+    rclcpp::AnyExecutable& any_executable,
+    std::chrono::nanoseconds timeout) {
     bool success = false;
     // Check to see if there are any subscriptions or timers needing service
     // TODO(wjwwood): improve run to run efficiency of this function
     // sched_yield();
     // sleep for 10us
     // usleep(20);
-    auto start = std::chrono::steady_clock::now();
+
+    auto const start = std::chrono::steady_clock::now();
     wait_for_work(timeout);
-    auto end = std::chrono::steady_clock::now();
+    auto const end = std::chrono::steady_clock::now();
-    auto wait_duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+    auto const wait_duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+    
     std::ostringstream oss;
     oss << "{\"operation\": \"wait_for_work\", \"wait_duration\": " << wait_duration.count() << "}";
     log_entry(logger_, oss.str());
 
-    start = std::chrono::steady_clock::now();
+    auto const exec_start = std::chrono::steady_clock::now();
     success = get_next_ready_executable(any_executable);
-    end = std::chrono::steady_clock::now();
+    auto const exec_end = std::chrono::steady_clock::now();
-    auto get_next_duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+    auto const get_next_duration = std::chrono::duration_cast<std::chrono::microseconds>(exec_end - exec_start);
+    
     oss.str("");
-    oss << "{\"operation\": \"get_next_executable\", \"duration\": " << get_next_duration.count() << ", \"result\": " << success << "}";
+    oss << "{\"operation\": \"get_next_executable\", \"duration\": " << get_next_duration.count() 
+        << ", \"result\": " << success << "}";
     log_entry(logger_, oss.str());
+    
     return success;
 }
 

src/priority_executor/src/usage_example.cpp

@@ -1,10 +1,12 @@
-#include "rclcpp/rclcpp.hpp"
-#include "priority_executor/priority_executor.hpp"
-#include "priority_executor/priority_memory_strategy.hpp"
 #include <string>
 #include <fstream>
 #include <unistd.h>
-#include "std_msgs/msg/string.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include "priority_executor/priority_executor.hpp"
+#include "priority_executor/priority_memory_strategy.hpp"
 
 // re-create the classic talker-listener example with two listeners
 class Talker : public rclcpp::Node
@@ -15,7 +17,8 @@ public:
     // Create a publisher on the "chatter" topic with 10 msg queue size.
     pub_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);
     // Create a timer of period 1s that calls our callback member function.
-        timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&Talker::timer_callback, this));
+    timer_ = this->create_wall_timer(std::chrono::seconds(1),
+                                     std::bind(&Talker::timer_callback, this));
   }
   // the timer must be public
   rclcpp::TimerBase::SharedPtr timer_;
@@ -23,15 +26,11 @@ public:
 private:
   void timer_callback()
   {
-        // Create a message and publish it 10 times.
     std_msgs::msg::String msg;
     msg.data = "Hello World!";
-        for (int i = 0; i < 10; ++i)
-        {
     RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str());
     pub_->publish(msg);
   }
-    }
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
 };
 
@@ -40,8 +39,11 @@ class Listener : public rclcpp::Node
 public:
   Listener(std::string name) : Node(name)
   {
-        // Create a subscription on the "chatter" topic with the default callback method.
+    // Create a subscription on the "chatter" topic with the default callback
-        sub_ = this->create_subscription<std_msgs::msg::String>("chatter", 10, std::bind(&Listener::callback, this, std::placeholders::_1));
+    // method.
+    sub_ = this->create_subscription<std_msgs::msg::String>(
+        "chatter", 10,
+        std::bind(&Listener::callback, this, std::placeholders::_1));
   }
   // the publisher must be public
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
@@ -53,27 +55,55 @@ private:
   }
 };
 
-int main(int argc, char **argv)
+int main(int argc, char* argv[]) {
-{
     rclcpp::init(argc, argv);
+
     auto talker = std::make_shared<Talker>();
     auto listener1 = std::make_shared<Listener>("listener1");
     auto listener2 = std::make_shared<Listener>("listener2");
     rclcpp::ExecutorOptions options;
 
-    auto strategy = std::make_shared<PriorityMemoryStrategy<>>();
+    auto strategy = std::make_shared<priority_executor::PriorityMemoryStrategy<>>();
     options.memory_strategy = strategy;
-    auto executor = new timed_executor::TimedExecutor(options);
+    auto executor = new priority_executor::TimedExecutor(options);
-    // replace the above line with the following line to use the default executor
-    // which will intermix the execution of listener1 and listener2
-    // auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(options);
 
-    strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 100, TIMER);
+    // must be set to post_execute can set new deadlines
-    strategy->set_executable_deadline(listener1->sub_->get_subscription_handle(), 100, SUBSCRIPTION);
+    executor->prio_memory_strategy_ = strategy;
-    strategy->set_executable_deadline(listener2->sub_->get_subscription_handle(), 50, SUBSCRIPTION);
+
+
+    // the new funcitons in PriorityMemoryStrategy accept the handle of the
+    // timer/subscription as the first argument
+    strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 1000, 
+                                      priority_executor::ExecutableType::TIMER, 0);
+    // you _must_ set the timer_handle for each chain
+    strategy->get_priority_settings(talker->timer_->get_timer_handle())
+        ->timer_handle = talker->timer_;
+    // you _must_ mark the first executable in the chain
+    strategy->set_first_in_chain(talker->timer_->get_timer_handle());
+    // set the same period and chain_id for each callback in the chain
+    strategy->set_executable_deadline(listener1->sub_->get_subscription_handle(),
+                                      1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
+    strategy->set_executable_deadline(listener2->sub_->get_subscription_handle(),
+                                      1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
+    // you _must_ mark the last executable in the chain (used to keep track of different instances of the same chain)
+    strategy->set_last_in_chain(listener2->sub_->get_subscription_handle());
+    // add all the nodes to the executor
     executor->add_node(talker);
     executor->add_node(listener1);
     executor->add_node(listener2);
 
+    // if the executor behaves unexpectedly, you can print the priority settings to make sure they are correct
+    std::cout << *strategy->get_priority_settings(
+                     talker->timer_->get_timer_handle())
+              << std::endl;
+    std::cout << *strategy->get_priority_settings(
+                     listener1->sub_->get_subscription_handle())
+              << std::endl;
+    std::cout << *strategy->get_priority_settings(
+                     listener2->sub_->get_subscription_handle())
+              << std::endl;
     executor->spin();
+
+    rclcpp::shutdown();
+    return 0;
 }

src/simple_timer/CMakeLists.txt

@@ -1,48 +1,51 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.8)
-project(simple_timer)
+project(simple_timer VERSION 0.1.0)
 
-# Default to C99
+# Set C++ standards
-if(NOT CMAKE_C_STANDARD)
+set(CMAKE_CXX_STANDARD 17)
-  set(CMAKE_C_STANDARD 99)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
-endif()
+set(CMAKE_CXX_EXTENSIONS OFF)
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
 
+# Compiler options
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
+# Find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
+find_package(rclcpp REQUIRED)
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
 
-add_library(simple_timer src/cycle_timer.cpp src/period_timer.cpp)
+# Library targets
+add_library(simple_timer
+  src/cycle_timer.cpp
+  src/period_timer.cpp
+)
 target_include_directories(simple_timer PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
-add_library(rt-sched src/rt-sched.cpp)
+ament_target_dependencies(simple_timer
+  rclcpp
+)
+
+add_library(rt-sched
+  src/rt-sched.cpp
+)
 target_include_directories(rt-sched PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>
 )
+ament_target_dependencies(rt-sched
+  rclcpp
+)
 
+# Testing
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Installation
 install(
   DIRECTORY include/
   DESTINATION include
@@ -50,12 +53,15 @@ install(
 
 install(
   TARGETS simple_timer rt-sched
+  EXPORT export_${PROJECT_NAME}
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
 
+# Export and package configuration
 ament_export_include_directories(include)
-ament_export_libraries(simple_timer)
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_libraries(rt-sched)
+ament_export_dependencies(rclcpp)
 ament_package()

src/simple_timer/include/simple_timer/cycle_timer.hpp

@@ -3,14 +3,15 @@
 
 #include <memory>
 #include "simple_timer/rt-sched.hpp"
-namespace simple_timer
+
-{
+namespace simple_timer {
-    class CycleTimer
+
-    {
+class CycleTimer {
 public:
-        CycleTimer(long start_delay=0);
+    explicit CycleTimer(long const start_delay = 0);
     void tick();
-        const u64 start_delay_time;
+
+    u64 const start_delay_time;
     u64 start_time = 0;
     u64 last_cycle_time = 0;
     unsigned long max_diff = 0;
@@ -18,6 +19,7 @@ namespace simple_timer
     unsigned long last_diff = 0;
     bool recording = false;
 };
+
 } // namespace simple_timer
 
 #endif

src/simple_timer/include/simple_timer/period_timer.hpp

@@ -3,23 +3,24 @@
 
 #include <memory>
 #include "simple_timer/rt-sched.hpp"
-namespace simple_timer
+
-{
+namespace simple_timer {
-    class PeriodTimer
+
-    {
+class PeriodTimer {
 public:
-        PeriodTimer(long start_delay = 0);
+    explicit PeriodTimer(long const start_delay = 0);
     void start();
     void stop();
-        const u64 start_delay_time;
-        u64 start_time = 0;
 
+    u64 const start_delay_time;
+    u64 start_time = 0;
     u64 last_period_time = 0;
     unsigned long max_period = 0;
     unsigned long min_period = 0;
     unsigned long last_period = 0;
     bool recording = false;
 };
+
 } // namespace simple_timer
 
 #endif

src/simple_timer/include/simple_timer/rt-sched.hpp

@@ -47,6 +47,7 @@
 
 typedef unsigned long long u64;
 #define NS_TO_MS 1000000
+
 struct sched_attr {
     uint32_t size;
     uint32_t sched_policy;
@@ -65,28 +66,26 @@ struct sched_attr {
 };
 
 int sched_setattr(pid_t pid,
-		  const struct sched_attr *attr,
+                  sched_attr const* attr,
                   unsigned int flags);
 
 int sched_getattr(pid_t pid,
-		  struct sched_attr *attr,
+                  sched_attr* attr,
                   unsigned int size,
                   unsigned int flags);
 
 u64 get_time_us(void);
 
-typedef struct node_time_logger
+typedef struct node_time_logger {
-{
     std::shared_ptr<std::vector<std::pair<std::string, u64>>> recorded_times;
 } node_time_logger;
 
-void log_entry(node_time_logger logger, std::string text);
+void log_entry(node_time_logger logger, std::string const& text);
 node_time_logger create_logger();
 
-inline u64 get_time_us(void)
+inline u64 get_time_us(void) {
-{
     struct timespec ts;
-	unsigned long long time;
+    u64 time;
 
     clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
     time = ts.tv_sec * 1000000;

src/simple_timer/package.xml

@@ -2,13 +2,19 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>simple_timer</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
-  <description>TODO: Package description</description>
+  <description>
-  <maintainer email="kurt4wilson@gmail.com">nvidia</maintainer>
+    ROS 2 package providing timer functionality for the dynamic executor experiments
-  <license>TODO: License declaration</license>
+  </description>
+  <maintainer email="kurt4wilson@gmail.com">kurt</maintainer>
+  <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <!-- Build Dependencies -->
+  <depend>rclcpp</depend>
+
+  <!-- Test Dependencies -->
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/simple_timer/src/cycle_timer.cpp

@@ -1,43 +1,34 @@
 #include "simple_timer/cycle_timer.hpp"
 
-namespace simple_timer
+namespace simple_timer {
-{
+
-  CycleTimer::CycleTimer(long start_delay) : start_delay_time(start_delay * 1000)
+CycleTimer::CycleTimer(long const start_delay) 
-  {
+    : start_delay_time(start_delay * 1000) {
 }
 
-  void CycleTimer::tick()
+void CycleTimer::tick() {
-  {
+    u64 const current_wall_time = get_time_us();
-    u64 current_wall_time = get_time_us();
     u64 time_diff = 0;
 
-    if (!recording)
+    if (!recording) {
-    {
+        if (start_time == 0) {
-
-      if (start_time == 0)
-      {
             start_time = current_wall_time;
-      }
+        } else if (current_wall_time - start_time > start_delay_time) {
-      else if (current_wall_time - start_time > start_delay_time)
-      {
             recording = true;
             last_cycle_time = current_wall_time;
             start_time = current_wall_time;
         }
-    }
+    } else {
-    else
-    {
         time_diff = current_wall_time - last_cycle_time;
-      if (time_diff < min_diff || min_diff == 0)
+        if (time_diff < min_diff || min_diff == 0) {
-      {
             min_diff = time_diff;
         }
-      if (time_diff > max_diff || max_diff == 0)
+        if (time_diff > max_diff || max_diff == 0) {
-      {
             max_diff = time_diff;
         }
         last_cycle_time = current_wall_time;
         last_diff = time_diff;
     }
 }
+
 }  // namespace simple_timer

src/simple_timer/src/period_timer.cpp

@@ -1,56 +1,43 @@
-
 #include "simple_timer/period_timer.hpp"
 
-namespace simple_timer
+namespace simple_timer {
-{
+
-    PeriodTimer::PeriodTimer(long start_delay) : start_delay_time(start_delay * 1000)
+PeriodTimer::PeriodTimer(long const start_delay)
-    {
+    : start_delay_time(start_delay * 1000) {
 }
 
-    void PeriodTimer::start()
+void PeriodTimer::start() {
-    {
+    u64 const current_wall_time = get_time_us();
-        u64 current_wall_time = get_time_us();
 
-        if (!recording)
+    if (!recording) {
-        {
+        if (start_time == 0) {
-
-            if (start_time == 0)
-            {
             start_time = current_wall_time;
-            }
+        } else if (current_wall_time - start_time > start_delay_time) {
-            else if (current_wall_time - start_time > start_delay_time)
-            {
             recording = true;
             start_time = current_wall_time;
             last_period_time = current_wall_time;
         }
-        }
+    } else {
-        else
-        {
         last_period_time = current_wall_time;
     }
 }
-  void PeriodTimer::stop()
+
-  {
+void PeriodTimer::stop() {
-    u64 current_wall_time = get_time_us();
+    u64 const current_wall_time = get_time_us();
     u64 time_diff = 0;
 
-    if (!recording)
+    if (!recording) {
-    {
         return;
     }
-    else
+
-    {
     time_diff = current_wall_time - last_period_time;
-      if (time_diff < min_period || min_period == 0)
+    if (time_diff < min_period || min_period == 0) {
-      {
         min_period = time_diff;
     }
-      if (time_diff > max_period || max_period == 0)
+    if (time_diff > max_period || max_period == 0) {
-      {
         max_period = time_diff;
     }
     last_period = time_diff;
 }
-  }
+
 }  // namespace simple_timer

src/simple_timer/src/rt-sched.cpp

@@ -13,37 +13,33 @@
 #include <sys/syscall.h>
 #include <time.h>
 #include <iostream>
-
 #include "simple_timer/rt-sched.hpp"
 
-int sched_setattr(pid_t pid,
+int sched_setattr(
-		  const struct sched_attr *attr,
+    pid_t pid,
-		  unsigned int flags)
+    sched_attr const* attr,
-{
+    unsigned int flags) {
     return syscall(__NR_sched_setattr, pid, attr, flags);
 }
 
-int sched_getattr(pid_t pid,
+int sched_getattr(
-		  struct sched_attr *attr,
+    pid_t pid,
+    sched_attr* attr,
     unsigned int size,
-		  unsigned int flags)
+    unsigned int flags) {
-{
     return syscall(__NR_sched_getattr, pid, attr, size, flags);
 }
 
-void log_entry(node_time_logger logger, std::string text)
+void log_entry(node_time_logger logger, std::string const& text) {
-{
+    if (logger.recorded_times != nullptr) {
-    if (logger.recorded_times != nullptr)
+        logger.recorded_times->emplace_back(text, get_time_us() / 1000);
-    {
-        logger.recorded_times->push_back(std::make_pair(text, get_time_us() / 1000));
         // std::cout<<text<<std::endl;
     } else {
         // TODO: report error
     }
 }
 
-node_time_logger create_logger()
+node_time_logger create_logger() {
-{
     node_time_logger logger;
     logger.recorded_times = std::make_shared<std::vector<std::pair<std::string, u64>>>();
     return logger;