Go to the documentation of this file.
30 #include <rosgraph_msgs/TopicStatistics.h>
33 #include "std_msgs/Header.h"
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)
57 (void)connection_header;
66 if (topic ==
"/statistics" || topic ==
"/clock")
72 std::map<std::string, struct StatData>::iterator stats_it =
map_.find(callerid);
80 map_[callerid] = stats;
84 stats =
map_[callerid];
104 if (!
header.stamp.isZero())
124 rosgraph_msgs::TopicStatistics msg;
126 msg.node_pub = callerid;
128 msg.window_start = window_start;
129 msg.window_stop = received_time;
137 double stamp_age_sum = 0.0;
140 for(std::list<ros::Duration>::iterator it = stats.
age_list.begin(); it != stats.
age_list.end(); it++)
143 stamp_age_sum += age.
toSec();
145 if (age > msg.stamp_age_max)
147 msg.stamp_age_max = age;
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++)
159 double stamp_age_stddev = sqrt(stamp_age_variance / stats.
age_list.size());
164 catch(std::runtime_error& e)
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);
199 msg.period_mean += period;
200 if (period > msg.period_max)
201 msg.period_max = period;
207 double period_variance = 0.0;
220 double period_stddev = sqrt(period_variance / (stats.
arrival_time_list.size() - 1));
235 pub_ = n.
advertise<rosgraph_msgs::TopicStatistics>(
"/statistics", 1);
258 map_[callerid] = stats;
void init(const SubscriptionCallbackHelperPtr &helper)
void deserialize(Stream &stream, boost::array< T, N > &t)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val)
Assign value from parameter server, with default.
std::list< ros::Duration > age_list
int previous_connection_id
#define ROS_DEBUG_STREAM(args)
void publish(const boost::shared_ptr< M > &message) const
Publish a message on the topic associated with this Publisher.
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)
#define ROS_WARN_STREAM(args)
std::map< std::string, struct StatData > map_
roscpp's interface for creating subscribers, publishers, etc.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Advertise a topic, simple version.
const ROSCPP_DECL std::string & getName()
Returns the name of the current node.
std::string getTopic() const
Returns the topic that this Publisher will publish on.
std::list< ros::Time > arrival_time_list
boost::shared_array< uint8_t > buf
roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim, Dirk Thomas
, Jacob Perron
autogenerated on Sat Sep 14 2024 02:59:35