statistics.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2013-2014, Dariush Forouher
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *   * Redistributions of source code must retain the above copyright notice,
00007  *     this list of conditions and the following disclaimer.
00008  *   * Redistributions in binary form must reproduce the above copyright
00009  *     notice, this list of conditions and the following disclaimer in the
00010  *     documentation and/or other materials provided with the distribution.
00011  *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
00012  *     contributors may be used to endorse or promote products derived from
00013  *     this software without specific prior written permission.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 #include "ros/statistics.h"
00029 #include "ros/node_handle.h"
00030 #include <rosgraph_msgs/TopicStatistics.h>
00031 #include "ros/this_node.h"
00032 #include "ros/message_traits.h"
00033 #include "std_msgs/Header.h"
00034 #include "ros/param.h"
00035 
00036 namespace ros
00037 {
00038 
00039 StatisticsLogger::StatisticsLogger()
00040 : pub_frequency_(1.0)
00041 {
00042 }
00043 
00044 void StatisticsLogger::init(const SubscriptionCallbackHelperPtr& helper) {
00045   hasHeader_ = helper->hasHeader();
00046   param::param("/enable_statistics", enable_statistics, false);
00047   param::param("/statistics_window_min_elements", min_elements, 10);
00048   param::param("/statistics_window_max_elements", max_elements, 100);
00049   param::param("/statistics_window_min_size", min_window, 4);
00050   param::param("/statistics_window_max_size", max_window, 64);
00051 }
00052 
00053 void StatisticsLogger::callback(const boost::shared_ptr<M_string>& connection_header,
00054                                 const std::string& topic, const std::string& callerid, const SerializedMessage& m, const uint64_t& bytes_sent,
00055                                 const ros::Time& received_time, bool dropped)
00056 {
00057   (void)connection_header;
00058   struct StatData stats;
00059 
00060   if (!enable_statistics)
00061   {
00062     return;
00063   }
00064 
00065   // ignore /clock for safety and /statistics to reduce noise
00066   if (topic == "/statistics" || topic == "/clock")
00067   {
00068     return;
00069   }
00070 
00071   // callerid identifies the connection
00072   std::map<std::string, struct StatData>::iterator stats_it = map_.find(callerid);
00073   if (stats_it == map_.end())
00074   {
00075     // this is the first time, we received something on this connection
00076     stats.stat_bytes_last = 0;
00077     stats.dropped_msgs = 0;
00078     stats.last_seq = 0;
00079     stats.last_publish = ros::Time::now();
00080     map_[callerid] = stats;
00081   }
00082   else
00083   {
00084     stats = map_[callerid];
00085   }
00086 
00087   stats.arrival_time_list.push_back(received_time);
00088 
00089   if (dropped)
00090   {
00091     stats.dropped_msgs++;
00092   }
00093 
00094   // try to extract header, if the message has one. this fails sometimes,
00095   // therefore the try-catch
00096   if (hasHeader_)
00097   {
00098     try
00099     {
00100       std_msgs::Header header;
00101       ros::serialization::IStream stream(m.message_start, m.num_bytes - (m.message_start - m.buf.get()));
00102       ros::serialization::deserialize(stream, header);
00103       stats.age_list.push_back(received_time - header.stamp);
00104     }
00105     catch (ros::serialization::StreamOverrunException& e)
00106     {
00107       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()));
00108       hasHeader_ = false;
00109     }
00110   }
00111 
00112   // should publish new statistics?
00113   if (stats.last_publish + ros::Duration(pub_frequency_) < received_time)
00114   {
00115     ros::Time window_start = stats.last_publish;
00116     stats.last_publish = received_time;
00117 
00118     // fill the message with the aggregated data
00119     rosgraph_msgs::TopicStatistics msg;
00120     msg.topic = topic;
00121     msg.node_pub = callerid;
00122     msg.node_sub = ros::this_node::getName();
00123     msg.window_start = window_start;
00124     msg.window_stop = received_time;
00125     msg.delivered_msgs = stats.arrival_time_list.size();
00126     msg.dropped_msgs = stats.dropped_msgs;
00127     msg.traffic = bytes_sent - stats.stat_bytes_last;
00128 
00129     // not all message types have this
00130     if (stats.age_list.size() > 0)
00131     {
00132       msg.stamp_age_mean = ros::Duration(0);
00133       msg.stamp_age_max = ros::Duration(0);
00134 
00135       for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
00136       {
00137         ros::Duration age = *it;
00138         msg.stamp_age_mean += age;
00139 
00140         if (age > msg.stamp_age_max)
00141         {
00142             msg.stamp_age_max = age;
00143         }
00144       }
00145 
00146       msg.stamp_age_mean *= 1.0 / stats.age_list.size();
00147 
00148       double stamp_age_variance = 0.0;
00149       for(std::list<ros::Duration>::iterator it = stats.age_list.begin(); it != stats.age_list.end(); it++)
00150       {
00151         ros::Duration t = msg.stamp_age_mean - *it;
00152         stamp_age_variance += t.toSec() * t.toSec();
00153       }
00154       double stamp_age_stddev = sqrt(stamp_age_variance / stats.age_list.size());
00155       try
00156       {
00157         msg.stamp_age_stddev = ros::Duration(stamp_age_stddev);
00158       }
00159       catch(std::runtime_error& e)
00160       {
00161         msg.stamp_age_stddev = ros::Duration(0);
00162         ROS_WARN_STREAM("Error updating stamp_age_stddev for topic [" << topic << "]"
00163           << " from node [" << callerid << "],"
00164           << " likely due to the time between the mean stamp age and this message being exceptionally large."
00165           << " Exception was: " << e.what());
00166         ROS_DEBUG_STREAM("Mean stamp age was: " << msg.stamp_age_mean << " - std_dev of: " << stamp_age_stddev);
00167       }
00168 
00169     }
00170     else
00171     {
00172       // in that case, set to NaN
00173       msg.stamp_age_mean = ros::Duration(0);
00174       msg.stamp_age_stddev = ros::Duration(0);
00175       msg.stamp_age_max = ros::Duration(0);
00176     }
00177 
00178     // first, calculate the mean period between messages in this connection
00179     // we need at least two messages in the window for this.
00180     if (stats.arrival_time_list.size() > 1)
00181     {
00182       msg.period_mean = ros::Duration(0);
00183       msg.period_max = ros::Duration(0);
00184 
00185       ros::Time prev;
00186       for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++) {
00187 
00188         if (it == stats.arrival_time_list.begin()) {
00189           prev = *it;
00190           continue;
00191         }
00192 
00193         ros::Duration period = *it - prev;
00194         msg.period_mean += period;
00195         if (period > msg.period_max)
00196             msg.period_max = period;
00197         prev = *it;
00198       }
00199       msg.period_mean *= 1.0 / (stats.arrival_time_list.size() - 1);
00200 
00201       // then, calc the stddev
00202       msg.period_stddev = ros::Duration(0);
00203       for(std::list<ros::Time>::iterator it = stats.arrival_time_list.begin(); it != stats.arrival_time_list.end(); it++)
00204       {
00205         if (it == stats.arrival_time_list.begin())
00206         {
00207           prev = *it;
00208           continue;
00209         }
00210         ros::Duration period = *it - prev;
00211         ros::Duration t = msg.period_mean - period;
00212         msg.period_stddev += ros::Duration(t.toSec() * t.toSec());
00213         prev = *it;
00214       }
00215       msg.period_stddev = ros::Duration(sqrt(msg.period_stddev.toSec() / (stats.arrival_time_list.size() - 1)));
00216 
00217     }
00218     else
00219     {
00220       // in that case, set to NaN
00221       msg.period_mean = ros::Duration(0);
00222       msg.period_stddev = ros::Duration(0);
00223       msg.period_max = ros::Duration(0);
00224     }
00225 
00226     if (!pub_.getTopic().length())
00227     {
00228       ros::NodeHandle n("~");
00229       // creating the publisher in the constructor results in a deadlock. so do it here.
00230       pub_ = n.advertise<rosgraph_msgs::TopicStatistics>("/statistics", 1);
00231     }
00232 
00233     pub_.publish(msg);
00234 
00235     // dynamic window resizing
00236     if (stats.arrival_time_list.size() > static_cast<size_t>(max_elements) && pub_frequency_ * 2 <= max_window)
00237     {
00238       pub_frequency_ *= 2;
00239     }
00240     if (stats.arrival_time_list.size() < static_cast<size_t>(min_elements) && pub_frequency_ / 2 >= min_window)
00241     {
00242       pub_frequency_ /= 2;
00243     }
00244 
00245     // clear the window
00246     stats.age_list.clear();
00247     stats.arrival_time_list.clear();
00248     stats.dropped_msgs = 0;
00249     stats.stat_bytes_last = bytes_sent;
00250 
00251   }
00252   // store the stats for this connection
00253   map_[callerid] = stats;
00254 }
00255 
00256 
00257 } // namespace ros


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Tue Mar 7 2017 03:44:47