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


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Fri Jun 8 2018 02:54:34