statistics.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013-2014, Dariush Forouher
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  * * Redistributions of source code must retain the above copyright notice,
7  * this list of conditions and the following disclaimer.
8  * * Redistributions in binary form must reproduce the above copyright
9  * notice, this list of conditions and the following disclaimer in the
10  * documentation and/or other materials provided with the distribution.
11  * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
12  * contributors may be used to endorse or promote products derived from
13  * this software without specific prior written permission.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25  * POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 #include "ros/statistics.h"
29 #include "ros/node_handle.h"
30 #include <rosgraph_msgs/TopicStatistics.h>
31 #include "ros/this_node.h"
32 #include "ros/message_traits.h"
33 #include "std_msgs/Header.h"
34 #include "ros/param.h"
35 
36 namespace ros
37 {
38 
40 : pub_frequency_(1.0)
41 {
42 }
43 
45  hasHeader_ = helper->hasHeader();
46  param::param("/enable_statistics", enable_statistics, false);
47  param::param("/statistics_window_min_elements", min_elements, 10);
48  param::param("/statistics_window_max_elements", max_elements, 100);
49  param::param("/statistics_window_min_size", min_window, 4);
50  param::param("/statistics_window_max_size", max_window, 64);
51 }
52 
54  const std::string& topic, const std::string& callerid, const SerializedMessage& m, const uint64_t& bytes_sent,
55  const ros::Time& received_time, bool dropped, int connection_id)
56 {
57  (void)connection_header;
58  struct StatData stats;
59 
60  if (!enable_statistics)
61  {
62  return;
63  }
64 
65  // ignore /clock for safety and /statistics to reduce noise
66  if (topic == "/statistics" || topic == "/clock")
67  {
68  return;
69  }
70 
71  // callerid identifies the connection
72  std::map<std::string, struct StatData>::iterator stats_it = map_.find(callerid);
73  if (stats_it == map_.end() || previous_connection_id != connection_id)
74  {
75  // this is the first time, we received something on this connection
76  stats.stat_bytes_last = 0;
77  stats.dropped_msgs = 0;
78  stats.last_seq = 0;
79  stats.last_publish = ros::Time::now();
80  map_[callerid] = stats;
81  }
82  else
83  {
84  stats = map_[callerid];
85  }
86  previous_connection_id = connection_id;
87 
88  stats.arrival_time_list.push_back(received_time);
89 
90  if (dropped)
91  {
92  stats.dropped_msgs++;
93  }
94 
95  // try to extract header, if the message has one. this fails sometimes,
96  // therefore the try-catch
97  if (hasHeader_)
98  {
99  try
100  {
101  std_msgs::Header header;
104  if (!header.stamp.isZero())
105  {
106  stats.age_list.push_back(received_time - header.stamp);
107  }
108  }
110  {
111  ROS_DEBUG("Error during header extraction for statistics (topic=%s, message_length=%li)", topic.c_str(), m.num_bytes - (m.message_start - m.buf.get()));
112  hasHeader_ = false;
113  }
114  }
115 
116  // should publish new statistics?
117  double pub_period = 1.0 / pub_frequency_;
118  if (stats.last_publish + ros::Duration(pub_period) < received_time)
119  {
120  ros::Time window_start = stats.last_publish;
121  stats.last_publish = received_time;
122 
123  // fill the message with the aggregated data
124  rosgraph_msgs::TopicStatistics msg;
125  msg.topic = topic;
126  msg.node_pub = callerid;
127  msg.node_sub = ros::this_node::getName();
128  msg.window_start = window_start;
129  msg.window_stop = received_time;
130  msg.delivered_msgs = stats.arrival_time_list.size();
131  msg.dropped_msgs = stats.dropped_msgs;
132  msg.traffic = bytes_sent - stats.stat_bytes_last;
133 
134  // not all message types have this
135  if (stats.age_list.size() > 0)
136  {
137  double stamp_age_sum = 0.0;
138  msg.stamp_age_max = ros::Duration(0);
139 
140  for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
141  {
142  ros::Duration age = *it;
143  stamp_age_sum += age.toSec();
144 
145  if (age > msg.stamp_age_max)
146  {
147  msg.stamp_age_max = age;
148  }
149  }
150 
151  msg.stamp_age_mean = ros::Duration(stamp_age_sum / stats.age_list.size());
152 
153  double stamp_age_variance = 0.0;
154  for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
155  {
156  ros::Duration t = msg.stamp_age_mean - *it;
157  stamp_age_variance += t.toSec() * t.toSec();
158  }
159  double stamp_age_stddev = sqrt(stamp_age_variance / stats.age_list.size());
160  try
161  {
162  msg.stamp_age_stddev = ros::Duration(stamp_age_stddev);
163  }
164  catch(std::runtime_error& e)
165  {
166  msg.stamp_age_stddev = ros::Duration(0);
167  ROS_WARN_STREAM("Error updating stamp_age_stddev for topic [" << topic << "]"
168  << " from node [" << callerid << "],"
169  << " likely due to the time between the mean stamp age and this message being exceptionally large."
170  << " Exception was: " << e.what());
171  ROS_DEBUG_STREAM("Mean stamp age was: " << msg.stamp_age_mean << " - std_dev of: " << stamp_age_stddev);
172  }
173 
174  }
175  else
176  {
177  // in that case, set to NaN
178  msg.stamp_age_mean = ros::Duration(0);
179  msg.stamp_age_stddev = ros::Duration(0);
180  msg.stamp_age_max = ros::Duration(0);
181  }
182 
183  // first, calculate the mean period between messages in this connection
184  // we need at least two messages in the window for this.
185  if (stats.arrival_time_list.size() > 1)
186  {
187  msg.period_mean = ros::Duration(0);
188  msg.period_max = ros::Duration(0);
189 
190  ros::Time prev;
191  for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++) {
192 
193  if (it == stats.arrival_time_list.begin()) {
194  prev = *it;
195  continue;
196  }
197 
198  ros::Duration period = *it - prev;
199  msg.period_mean += period;
200  if (period > msg.period_max)
201  msg.period_max = period;
202  prev = *it;
203  }
204  msg.period_mean *= 1.0 / (stats.arrival_time_list.size() - 1);
205 
206  // then, calc the stddev
207  double period_variance = 0.0;
208  for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++)
209  {
210  if (it == stats.arrival_time_list.begin())
211  {
212  prev = *it;
213  continue;
214  }
215  ros::Duration period = *it - prev;
216  ros::Duration t = msg.period_mean - period;
217  period_variance += t.toSec() * t.toSec();
218  prev = *it;
219  }
220  double period_stddev = sqrt(period_variance / (stats.arrival_time_list.size() - 1));
221  msg.period_stddev = ros::Duration(period_stddev);
222  }
223  else
224  {
225  // in that case, set to NaN
226  msg.period_mean = ros::Duration(0);
227  msg.period_stddev = ros::Duration(0);
228  msg.period_max = ros::Duration(0);
229  }
230 
231  if (!pub_.getTopic().length())
232  {
233  ros::NodeHandle n("~");
234  // creating the publisher in the constructor results in a deadlock. so do it here.
235  pub_ = n.advertise<rosgraph_msgs::TopicStatistics>("/statistics", 1);
236  }
237 
238  pub_.publish(msg);
239 
240  // dynamic window resizing
241  if (stats.arrival_time_list.size() > static_cast<size_t>(max_elements) && pub_period / 2.0 >= min_window)
242  {
243  pub_frequency_ *= 2;
244  }
245  if (stats.arrival_time_list.size() < static_cast<size_t>(min_elements) && pub_period * 2.0 <= max_window)
246  {
247  pub_frequency_ /= 2;
248  }
249 
250  // clear the window
251  stats.age_list.clear();
252  stats.arrival_time_list.clear();
253  stats.dropped_msgs = 0;
254  stats.stat_bytes_last = bytes_sent;
255 
256  }
257  // store the stats for this connection
258  map_[callerid] = stats;
259 }
260 
261 
262 } // namespace ros
ros::StatisticsLogger::min_window
int min_window
Definition: statistics.h:72
this_node.h
ros::SerializedMessage
ros::StatisticsLogger::init
void init(const SubscriptionCallbackHelperPtr &helper)
Definition: statistics.cpp:44
ros::SerializedMessage::message_start
uint8_t * message_start
ros::serialization::deserialize
void deserialize(Stream &stream, boost::array< T, N > &t)
boost::shared_ptr< SubscriptionCallbackHelper >
ros::param::param
bool param(const std::string &param_name, T &param_val, const T &default_val)
Assign value from parameter server, with default.
Definition: param.h:619
ros::StatisticsLogger::StatData::last_seq
uint64_t last_seq
Definition: statistics.h:102
ros
ros::StatisticsLogger::pub_frequency_
double pub_frequency_
Definition: statistics.h:87
ros::StatisticsLogger::StatisticsLogger
StatisticsLogger()
Definition: statistics.cpp:39
ros::StatisticsLogger::StatData::age_list
std::list< ros::Duration > age_list
Definition: statistics.h:98
ros::StatisticsLogger::previous_connection_id
int previous_connection_id
Definition: statistics.h:73
statistics.h
ros::serialization::IStream
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
Publish a message on the topic associated with this Publisher.
Definition: publisher.h:65
ros::StatisticsLogger::hasHeader_
bool hasHeader_
Definition: statistics.h:84
ros::StatisticsLogger::callback
void callback(const boost::shared_ptr< M_string > &connection_header, const std::string &topic, const std::string &callerid, const SerializedMessage &m, const uint64_t &bytes_sent, const ros::Time &received_time, bool dropped, int connection_id)
Definition: statistics.cpp:53
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
ros::StatisticsLogger::max_window
int max_window
Definition: statistics.h:71
message_traits.h
ROS_DEBUG
#define ROS_DEBUG(...)
ros::StatisticsLogger::pub_
ros::Publisher pub_
Definition: statistics.h:90
param.h
ros::StatisticsLogger::map_
std::map< std::string, struct StatData > map_
Definition: statistics.h:108
ros::StatisticsLogger::StatData::last_publish
ros::Time last_publish
Definition: statistics.h:94
ros::NodeHandle
roscpp's interface for creating subscribers, publishers, etc.
Definition: node_handle.h:87
ros::StatisticsLogger::StatData::stat_bytes_last
uint64_t stat_bytes_last
Definition: statistics.h:104
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Advertise a topic, simple version.
Definition: node_handle.h:249
ros::SerializedMessage::num_bytes
size_t num_bytes
ros::StatisticsLogger::enable_statistics
bool enable_statistics
Definition: statistics.h:81
ros::StatisticsLogger::max_elements
int max_elements
Definition: statistics.h:78
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
Returns the name of the current node.
Definition: this_node.cpp:74
ros::Publisher::getTopic
std::string getTopic() const
Returns the topic that this Publisher will publish on.
Definition: publisher.cpp:126
ros::serialization::StreamOverrunException
ros::Time
ros::StatisticsLogger::StatData
Definition: statistics.h:92
ros::StatisticsLogger::StatData::arrival_time_list
std::list< ros::Time > arrival_time_list
Definition: statistics.h:96
DurationBase< Duration >::toSec
double toSec() const
header
const std::string header
ros::Duration
ros::StatisticsLogger::StatData::dropped_msgs
uint64_t dropped_msgs
Definition: statistics.h:100
node_handle.h
ros::SerializedMessage::buf
boost::shared_array< uint8_t > buf
ros::StatisticsLogger::min_elements
int min_elements
Definition: statistics.h:79
ros::Time::now
static Time now()


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim, Dirk Thomas , Jacob Perron
autogenerated on Thu Nov 23 2023 04:01:44