windows Sun Apr 13 20:58:23 WEDT 2025

This commit is contained in:
Niklas Halle 2025-04-13 20:58:23 +02:00
parent f53b7047f1
commit d7ecdf0108
6 changed files with 605 additions and 38 deletions

View file

@ -19,11 +19,16 @@ find_package(rmw REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(simple_timer REQUIRED)
find_package(nlohmann_json REQUIRED)
# 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>
@ -34,35 +39,7 @@ ament_target_dependencies(priority_executor
rclcpp
rcl
simple_timer
)
add_library(multithread_priority_executor
src/multithread_priority_executor.cpp
src/priority_memory_strategy.cpp
)
target_include_directories(multithread_priority_executor PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<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
nlohmann_json
)
# Example executable
@ -75,8 +52,6 @@ target_include_directories(usage_example PUBLIC
)
target_link_libraries(usage_example
priority_executor
multithread_priority_executor
default_executor
)
ament_target_dependencies(usage_example
rclcpp
@ -98,7 +73,7 @@ install(
)
install(
TARGETS priority_executor multithread_priority_executor default_executor
TARGETS priority_executor
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
@ -114,5 +89,5 @@ install(
# Export and package configuration
ament_export_include_directories(include)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(rclcpp rcl rmw simple_timer)
ament_export_dependencies(rclcpp rcl rmw simple_timer nlohmann_json)
ament_package()

View file

@ -37,13 +37,13 @@ public:
RCLCPP_PUBLIC void spin() override;
protected:
RCLCPP_PUBLIC void run(size_t _thread_number);
RCLCPP_PUBLIC void run(size_t thread_number);
private:
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);
node_time_logger logger_;
std::chrono::nanoseconds next_exec_timeout_{std::chrono::nanoseconds(-1)};
static std::unordered_map<MultithreadTimedExecutor*,
std::shared_ptr<rclcpp::detail::MutexTwoPriorities>>

View file

@ -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_

View file

@ -20,6 +20,7 @@
#include <cassert>
#include <cstdlib>
#include <memory>
#include <string>
#include "rclcpp/executor.hpp"
#include "rclcpp/macros.hpp"
@ -27,6 +28,7 @@
#include "priority_executor/priority_memory_strategy.hpp"
#include "priority_executor/default_executor.hpp"
#include "priority_executor/performance_monitor.hpp"
namespace priority_executor {
@ -63,6 +65,43 @@ public:
void set_use_priorities(bool use_prio);
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 get_next_executable(rclcpp::AnyExecutable& any_executable,
std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
@ -72,8 +111,14 @@ private:
void wait_for_work(std::chrono::nanoseconds timeout);
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};
static std::atomic<int> next_executor_id_;
int executor_id_;
};
} // namespace priority_executor

View file

@ -1,4 +1,5 @@
#include "priority_executor/multithread_priority_executor.hpp"
#include "priority_executor/performance_monitor.hpp"
#include <chrono>
#include <functional>
@ -7,6 +8,7 @@
#include <sstream>
#include <thread>
#include <unordered_map>
#include <nlohmann/json.hpp>
#include "rcpputils/scope_exit.hpp"
#include "rclcpp/any_executable.hpp"
@ -58,6 +60,7 @@ void MultithreadTimedExecutor::spin() {
for (auto& thread : threads) {
thread.join();
}
spinning.store(false);
}
void MultithreadTimedExecutor::run(size_t _thread_number) {
@ -68,9 +71,16 @@ void MultithreadTimedExecutor::run(size_t _thread_number) {
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);
}
// Create thread context information for logs
nlohmann::json thread_info;
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;
{
@ -98,8 +108,160 @@ void MultithreadTimedExecutor::run(size_t _thread_number) {
}
}
// 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();
@ -118,7 +280,6 @@ void MultithreadTimedExecutor::run(size_t _thread_number) {
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);
}
}

View file

@ -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