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)
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())
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 
87  stats.arrival_time_list.push_back(received_time);
88 
89  if (dropped)
90  {
91  stats.dropped_msgs++;
92  }
93 
94  // try to extract header, if the message has one. this fails sometimes,
95  // therefore the try-catch
96  if (hasHeader_)
97  {
98  try
99  {
100  std_msgs::Header header;
102  ros::serialization::deserialize(stream, header);
103  if (!header.stamp.isZero())
104  {
105  stats.age_list.push_back(received_time - header.stamp);
106  }
107  }
109  {
110  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()));
111  hasHeader_ = false;
112  }
113  }
114 
115  // should publish new statistics?
116  double pub_period = 1.0 / pub_frequency_;
117  if (stats.last_publish + ros::Duration(pub_period) < received_time)
118  {
119  ros::Time window_start = stats.last_publish;
120  stats.last_publish = received_time;
121 
122  // fill the message with the aggregated data
123  rosgraph_msgs::TopicStatistics msg;
124  msg.topic = topic;
125  msg.node_pub = callerid;
126  msg.node_sub = ros::this_node::getName();
127  msg.window_start = window_start;
128  msg.window_stop = received_time;
129  msg.delivered_msgs = stats.arrival_time_list.size();
130  msg.dropped_msgs = stats.dropped_msgs;
131  msg.traffic = bytes_sent - stats.stat_bytes_last;
132 
133  // not all message types have this
134  if (stats.age_list.size() > 0)
135  {
136  double stamp_age_sum = 0.0;
137  msg.stamp_age_max = ros::Duration(0);
138 
139  for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
140  {
141  ros::Duration age = *it;
142  stamp_age_sum += age.toSec();
143 
144  if (age > msg.stamp_age_max)
145  {
146  msg.stamp_age_max = age;
147  }
148  }
149 
150  msg.stamp_age_mean = ros::Duration(stamp_age_sum / stats.age_list.size());
151 
152  double stamp_age_variance = 0.0;
153  for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
154  {
155  ros::Duration t = msg.stamp_age_mean - *it;
156  stamp_age_variance += t.toSec() * t.toSec();
157  }
158  double stamp_age_stddev = sqrt(stamp_age_variance / stats.age_list.size());
159  try
160  {
161  msg.stamp_age_stddev = ros::Duration(stamp_age_stddev);
162  }
163  catch(std::runtime_error& e)
164  {
165  msg.stamp_age_stddev = ros::Duration(0);
166  ROS_WARN_STREAM("Error updating stamp_age_stddev for topic [" << topic << "]"
167  << " from node [" << callerid << "],"
168  << " likely due to the time between the mean stamp age and this message being exceptionally large."
169  << " Exception was: " << e.what());
170  ROS_DEBUG_STREAM("Mean stamp age was: " << msg.stamp_age_mean << " - std_dev of: " << stamp_age_stddev);
171  }
172 
173  }
174  else
175  {
176  // in that case, set to NaN
177  msg.stamp_age_mean = ros::Duration(0);
178  msg.stamp_age_stddev = ros::Duration(0);
179  msg.stamp_age_max = ros::Duration(0);
180  }
181 
182  // first, calculate the mean period between messages in this connection
183  // we need at least two messages in the window for this.
184  if (stats.arrival_time_list.size() > 1)
185  {
186  msg.period_mean = ros::Duration(0);
187  msg.period_max = ros::Duration(0);
188 
189  ros::Time prev;
190  for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++) {
191 
192  if (it == stats.arrival_time_list.begin()) {
193  prev = *it;
194  continue;
195  }
196 
197  ros::Duration period = *it - prev;
198  msg.period_mean += period;
199  if (period > msg.period_max)
200  msg.period_max = period;
201  prev = *it;
202  }
203  msg.period_mean *= 1.0 / (stats.arrival_time_list.size() - 1);
204 
205  // then, calc the stddev
206  double period_variance = 0.0;
207  for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++)
208  {
209  if (it == stats.arrival_time_list.begin())
210  {
211  prev = *it;
212  continue;
213  }
214  ros::Duration period = *it - prev;
215  ros::Duration t = msg.period_mean - period;
216  period_variance += t.toSec() * t.toSec();
217  prev = *it;
218  }
219  double period_stddev = sqrt(period_variance / (stats.arrival_time_list.size() - 1));
220  msg.period_stddev = ros::Duration(period_stddev);
221  }
222  else
223  {
224  // in that case, set to NaN
225  msg.period_mean = ros::Duration(0);
226  msg.period_stddev = ros::Duration(0);
227  msg.period_max = ros::Duration(0);
228  }
229 
230  if (!pub_.getTopic().length())
231  {
232  ros::NodeHandle n("~");
233  // creating the publisher in the constructor results in a deadlock. so do it here.
234  pub_ = n.advertise<rosgraph_msgs::TopicStatistics>("/statistics", 1);
235  }
236 
237  pub_.publish(msg);
238 
239  // dynamic window resizing
240  if (stats.arrival_time_list.size() > static_cast<size_t>(max_elements) && pub_period / 2.0 >= min_window)
241  {
242  pub_frequency_ *= 2;
243  }
244  if (stats.arrival_time_list.size() < static_cast<size_t>(min_elements) && pub_period * 2.0 <= max_window)
245  {
246  pub_frequency_ /= 2;
247  }
248 
249  // clear the window
250  stats.age_list.clear();
251  stats.arrival_time_list.clear();
252  stats.dropped_msgs = 0;
253  stats.stat_bytes_last = bytes_sent;
254 
255  }
256  // store the stats for this connection
257  map_[callerid] = stats;
258 }
259 
260 
261 } // namespace ros
bool param(const std::string &param_name, T &param_val, const T &default_val)
Assign value from parameter server, with default.
Definition: param.h:608
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)
Definition: statistics.cpp:53
void init(const SubscriptionCallbackHelperPtr &helper)
Definition: statistics.cpp:44
std::list< ros::Time > arrival_time_list
Definition: statistics.h:95
ROSCPP_DECL const std::string & getName()
Returns the name of the current node.
Definition: this_node.cpp:81
std::map< std::string, struct StatData > map_
Definition: statistics.h:107
void publish(const boost::shared_ptr< M > &message) const
Publish a message on the topic associated with this Publisher.
Definition: publisher.h:63
roscpp&#39;s interface for creating subscribers, publishers, etc.
Definition: node_handle.h:87
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Advertise a topic, simple version.
Definition: node_handle.h:249
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
ros::Publisher pub_
Definition: statistics.h:89
std::string getTopic() const
Returns the topic that this Publisher will publish on.
Definition: publisher.cpp:112
std::list< ros::Duration > age_list
Definition: statistics.h:97
boost::shared_array< uint8_t > buf
static Time now()
const std::string header
void deserialize(Stream &stream, T &t)
#define ROS_DEBUG(...)


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Wed Oct 16 2019 03:27:44