5 #include <swri_profiler_msgs/ProfileIndex.h> 6 #include <swri_profiler_msgs/ProfileIndexArray.h> 7 #include <swri_profiler_msgs/ProfileData.h> 8 #include <swri_profiler_msgs/ProfileDataArray.h> 10 namespace spm = swri_profiler_msgs;
48 if (profiler_initialized_) {
52 ROS_INFO(
"Initializing swri_profiler...");
54 profiler_index_pub_ = nh.
advertise<spm::ProfileIndexArray>(
"/profiler/index", 1,
true);
55 profiler_data_pub_ = nh.
advertise<spm::ProfileDataArray>(
"/profiler/data", 100,
false);
57 profiler_initialized_ =
true;
63 ROS_ERROR(
"Attempt to initialize thread local storage again.");
68 tls_->stack_depth = 0;
72 snprintf(buffer,
sizeof(buffer),
"%p/",
tls_.get());
73 tls_->thread_prefix = std::string(buffer);
80 ROS_DEBUG(
"swri_profiler thread started.");
89 ROS_DEBUG(
"swri_profiler thread stopped.");
94 static bool first_run =
true;
98 std::unordered_map<std::string, ClosedInfo> new_closed_blocks;
99 std::unordered_map<std::string, OpenInfo> threaded_open_blocks;
106 threaded_open_blocks[pair.first].t0 = pair.second.t0;
107 pair.second.last_report_time = now;
112 for (
auto &pair : all_closed_blocks_) {
118 bool update_index =
false;
121 for (
auto const &pair : new_closed_blocks) {
122 const auto &label = pair.first;
123 const auto &new_info = pair.second;
125 auto &all_info = all_closed_blocks_[label];
127 if (all_info.key == 0) {
129 all_info.key = all_closed_blocks_.size();
132 all_info.abs_call_count += new_info.count;
135 all_info.rel_max_duration = std::max(all_info.rel_max_duration,
141 std::unordered_map<std::string, spm::ProfileData> combined_open_blocks;
142 for (
auto const &pair : threaded_open_blocks) {
143 const auto &threaded_label = pair.first;
144 const auto &threaded_info = pair.second;
146 size_t slash_index = threaded_label.find(
'/');
147 if (slash_index == std::string::npos) {
148 ROS_ERROR(
"Missing expected slash in label: %s", threaded_label.c_str());
154 const auto label = threaded_label.substr(slash_index+1);
155 auto &new_info = combined_open_blocks[label];
157 if (new_info.key == 0) {
158 auto &all_info = all_closed_blocks_[label];
159 if (all_info.key == 0) {
161 all_info.key = all_closed_blocks_.size();
163 new_info.key = all_info.key;
166 new_info.abs_call_count++;
167 new_info.abs_total_duration += duration;
169 new_info.rel_total_duration += duration;
171 new_info.rel_total_duration += std::min(
174 new_info.rel_max_duration = std::max(new_info.rel_max_duration, duration);
178 spm::ProfileIndexArray index;
181 index.data.resize(all_closed_blocks_.size());
183 for (
auto const &pair : all_closed_blocks_) {
184 size_t i = pair.second.key - 1;
185 index.data[i].key = pair.second.key;
186 index.data[i].label = pair.first;
188 profiler_index_pub_.
publish(index);
192 spm::ProfileDataArray
msg;
195 msg.rostime_stamp = ros_now;
197 msg.data.resize(all_closed_blocks_.size());
198 for (
auto &pair : all_closed_blocks_) {
199 auto const &item = pair.second;
200 size_t i = item.key - 1;
202 msg.data[i].key = item.key;
203 msg.data[i].abs_call_count = item.abs_call_count;
204 msg.data[i].abs_total_duration = item.abs_total_duration;
205 msg.data[i].rel_total_duration = item.rel_total_duration;
206 msg.data[i].rel_max_duration = item.rel_max_duration;
209 for (
auto &pair : combined_open_blocks) {
210 auto const &item = pair.second;
211 size_t i = item.key - 1;
212 msg.data[i].abs_call_count += item.abs_call_count;
213 msg.data[i].abs_total_duration += item.abs_total_duration;
214 msg.data[i].rel_total_duration += item.rel_total_duration;
215 msg.data[i].rel_max_duration = std::max(
216 msg.data[i].rel_max_duration,
217 item.rel_max_duration);
220 profiler_data_pub_.
publish(msg);
static ros::Publisher profiler_data_pub_
void publish(const boost::shared_ptr< M > &message) const
static void initializeTLS()
static ros::Time timeFromWall(const ros::WallTime &src)
static std::unordered_map< std::string, OpenInfo > open_blocks_
static ros::Publisher profiler_index_pub_
static ros::Duration durationFromWall(const ros::WallDuration &src)
ROSCPP_DECL const std::string & getName()
static void profilerMain()
static boost::thread_specific_ptr< TLS > tls_
static void initializeProfiler()
static boost::thread profiler_thread_
static bool profiler_initialized_
static std::unordered_map< std::string, spm::ProfileData > all_closed_blocks_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static std::unordered_map< std::string, ClosedInfo > closed_blocks_
static void collectAndPublish()