1 #ifndef SWRI_PROFILER_PROFILER_H_ 2 #define SWRI_PROFILER_PROFILER_H_ 6 #include <unordered_map> 23 while (locked_.test_and_set(std::memory_order_acquire)) { ; }
28 locked_.clear(std::memory_order_release);
87 static boost::thread_specific_ptr<TLS>
tls_;
93 static void initializeProfiler();
94 static void initializeTLS();
95 static void profilerMain();
96 static void collectAndPublish();
100 if (!tls_.get()) { initializeTLS(); }
103 ROS_ERROR(
"Profiler error: Profiled section has empty name. " 104 "Current stack is '%s'.",
105 tls_->stack_str.c_str());
109 if (tls_->stack_depth >= 100) {
110 ROS_ERROR(
"Profiler error: reached max stack size (%zu) while " 111 "opening '%s'. Current stack is '%s'.",
114 tls_->stack_str.c_str());
119 tls_->stack_str = tls_->stack_str +
"/" + name;
121 std::string open_index = tls_->thread_prefix + tls_->stack_str;
124 OpenInfo &info = open_blocks_[open_index];
134 std::string open_index = tls_->thread_prefix + tls_->stack_str;
138 auto const open_it = open_blocks_.find(open_index);
139 if (open_it == open_blocks_.end()) {
140 ROS_ERROR(
"Missing entry for '%s' in open_index. Profiler is probably corrupted.",
147 if (open_it->second.last_report_time > open_it->second.t0) {
148 rel_duration = tf - open_it->second.last_report_time;
150 rel_duration = tf - open_it->second.t0;
152 open_blocks_.erase(open_it);
154 ClosedInfo &info = closed_blocks_[tls_->stack_str];
156 if (info.
count == 1) {
167 const size_t len = name.size()+1;
168 tls_->stack_str.erase(tls_->stack_str.size()-len, len);
187 if (!name_.empty()) {
195 #define SWRI_PROFILER_CONCAT_DIRECT(s1,s2) s1##s2 196 #define SWRI_PROFILER_CONCAT(s1, s2) SWRI_PROFILER_CONCAT_DIRECT(s1,s2) 198 #define SWRI_PROFILER_IMP(block_var, name) \ 199 swri_profiler::Profiler block_var(name); \ 201 #ifndef DISABLE_SWRI_PROFILER 202 #define SWRI_PROFILE(name) SWRI_PROFILER_IMP( \ 203 SWRI_PROFILER_CONCAT(prof_block_, __LINE__), \ 205 #else // ndef DISABLE_SWRI_PROFILER 206 #define SWRI_PROFILE(name) 207 #endif // def DISABLE_SWRI_PROFILER 209 #endif // SWRI_PROFILER_PROFILER_H_
static bool open(const std::string &name, const ros::WallTime &t0)
static std::unordered_map< std::string, OpenInfo > open_blocks_
static boost::thread_specific_ptr< TLS > tls_
ros::WallDuration max_duration
Profiler(const std::string &name)
ros::WallDuration rel_duration
static std::unordered_map< std::string, ClosedInfo > closed_blocks_
ros::WallDuration total_duration
std::string thread_prefix
ros::WallTime last_report_time
static void close(const std::string &name, const ros::WallTime &tf)
SpinLockGuard(SpinLock &lock)