pal_statistics.cpp
Go to the documentation of this file.
1 /*
2  @file
3 
4  @author victor
5 
6  @copyright (c) 2018 PAL Robotics SL. All Rights Reserved
7 */
8 
11 namespace pal_statistics
12 {
13 
14 StatisticsRegistry::StatisticsRegistry(const std::string &topic)
15 {
16  pub_ = nh_.advertise<pal_statistics_msgs::Statistics>(topic + "/full", 10000);
17  pub_names_ = nh_.advertise<pal_statistics_msgs::StatisticsNames>(topic + "/names", 10000, true);
18  pub_values_ = nh_.advertise<pal_statistics_msgs::StatisticsValues>(topic + "/values", 10000);
22  is_data_ready_ = false;
23 
24  customRegister(*this, "topic_stats." + topic + ".publish_async_attempts", &publish_async_attempts_, &internal_stats_raii_);
25  customRegister(*this, "topic_stats." + topic + ".publish_async_failures", &publish_async_failures_, &internal_stats_raii_);
26  customRegister(*this, "topic_stats." + topic + ".publish_buffer_full_errors", &registration_list_.overwritten_data_count_, &internal_stats_raii_);
27  customRegister(*this, "topic_stats." + topic + ".last_async_pub_duration", &last_async_pub_duration_, &internal_stats_raii_);
28 }
29 
31 {
32  is_data_ready_ = true; //To let the thread exit nicely
33 
35  {
36  publisher_thread_->interrupt();
37  publisher_thread_->join();
38  }
40  ROS_INFO_STREAM("publish_async_failures_ " << publish_async_failures_);
41 }
42 
44  const boost::function<double()> &funct,
45  RegistrationsRAII *bookkeeping, bool enabled)
46 {
47  return registerInternal(name, VariableHolder(funct), bookkeeping, enabled);
48 }
49 
51 {
52  if (bookkeeping)
53  {
54  bookkeeping->remove(id);
55  }
56 
57  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
59 }
60 
61 void StatisticsRegistry::unregisterVariable(const std::string &name, RegistrationsRAII *bookkeeping)
62 {
63  if (bookkeeping)
64  {
65  bookkeeping->remove(name);
66  }
67 
68  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
70 }
71 
72 
74 {
75  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
76  handlePendingDisables(data_lock);
78 
79  boost::unique_lock<boost::mutex> pub_lock(pub_mutex_);
80  bool minor_changes = updateMsg(names_msg_, values_msg_, true);
81  data_lock.unlock(); //msg_ is covered by pub_mutex_
82  doPublish(!minor_changes);
83 }
84 
86 {
87  double begin = ros::SteadyTime::now().toSec();
89  if (data_mutex_.try_lock())
90  {
91  if (!publisher_thread_.get())
92  {
93  ROS_WARN_STREAM_ONCE("Called publishAsync but publisher thread has not been started, THIS IS NOT RT safe. You should start it yourself.");
95  }
96 
97  {
98  boost::unique_lock<boost::mutex> data_lock(data_mutex_, boost::adopt_lock);
99  handlePendingDisables(data_lock);
100 
102  }
103  is_data_ready_ = true;
104 
106  return true;
107  }
109  // Commented for RT safety
110  // ROS_DEBUG("Missed publishRT opportunity because lock could not be acquired.");
111  return false;
112 }
113 
115 {
116  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
118 }
120 {
121  publisher_thread_.reset(new boost::thread(&StatisticsRegistry::publisherThreadCycle, this));
122 }
123 
124 IdType StatisticsRegistry::registerInternal(const std::string &name, VariableHolder &&variable,
125  RegistrationsRAII *bookkeeping, bool enabled)
126 {
127  IdType id;
128  {
129  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
130  id = registration_list_.registerVariable(name, std::move(variable), enabled);
131  enabled_ids_.set_capacity(registration_list_.size());
132  }
133 
134  if (bookkeeping)
135  bookkeeping->add(Registration(name, id, weak_from_this()));
136  return id;
137 }
138 
139 bool StatisticsRegistry::setEnabledmpl(const IdType &id, bool enabled)
140 {
141  EnabledId aux;
142  aux.enabled = enabled;
143  aux.id = id;
144 
145  return enabled_ids_.bounded_push(aux);
146 }
147 
148 void StatisticsRegistry::handlePendingDisables(const boost::unique_lock<boost::mutex> &data_lock)
149 {
150  if (!data_lock.owns_lock() || data_lock.mutex() != &data_mutex_)
151  {
152  throw ros::Exception("Called handlePendingDisables without proper lock");
153  }
154 
155  EnabledId elem;
156  while (enabled_ids_.pop(elem))
157  {
159  }
160 }
161 
162 void StatisticsRegistry::doPublish(bool publish_names_msg)
163 {
164  if (pub_.getNumSubscribers() > 0)
165  {
168  }
169 
170  // We don't check subscribers here, because this topic is latched and we
171  // always want the latest version published
172  if (publish_names_msg) //only publish strings if changed
173  {
175  }
176  if (pub_values_.getNumSubscribers() > 0 ) //only publish strings if changed
177  {
179  }
180 }
181 
182 pal_statistics_msgs::Statistics StatisticsRegistry::createMsg()
183 {
184  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
185  handlePendingDisables(data_lock);
187  GeneratedStatistics gen_sts;
188  pal_statistics_msgs::StatisticsNames names;
189  pal_statistics_msgs::StatisticsValues values;
190 
191  updateMsg(names, values, false);
192  gen_sts.update(names, values);
193  return gen_sts.msg_;
194 }
195 
197 {
198  return setEnabledmpl(id, true);
199 }
200 
202 {
203  return setEnabledmpl(id, false);
204 }
205 
206 bool StatisticsRegistry::updateMsg(pal_statistics_msgs::StatisticsNames &names,
207  pal_statistics_msgs::StatisticsValues &values,
208  bool smart_fill)
209 {
210  if (smart_fill)
211  return registration_list_.smartFillMsg(names, values);
212  else
213  {
214  registration_list_.fillMsg(names, values);
215  return false;
216  }
217 }
218 
220 {
221  //wait until the variable is set
222  while (!publisher_thread_.get())
223  ros::WallDuration(5e-4).sleep();
224 
225 
226  while (ros::ok() && !publisher_thread_->interruption_requested())
227  {
228  while (!is_data_ready_ && !publisher_thread_->interruption_requested())
229  ros::WallDuration(5e-4).sleep();
230 
231  boost::unique_lock<boost::mutex> data_lock(data_mutex_);
232 
234  {
235  bool minor_changes = updateMsg(names_msg_, values_msg_, true);
236 
237  boost::unique_lock<boost::mutex> pub_lock(pub_mutex_);
238  data_lock.unlock();
239  doPublish(!minor_changes);
240  pub_lock.unlock();
241  data_lock.lock();
242  }
243  is_data_ready_ = false;
244  }
245 }
246 
248  const pal_statistics_msgs::StatisticsNames &names,
249  const pal_statistics_msgs::StatisticsValues &values)
250 {
251  msg_.header = values.header;
252  if (last_names_version_ == names.names_version && !msg_.statistics.empty())
253  {
254  // only need to update the values
255  for (size_t i = 0; i < values.values.size(); ++i)
256  {
257  msg_.statistics[i].value = values.values[i];
258  }
259  }
260  else
261  {
262  msg_.statistics.clear();
263  for (size_t i = 0; i < names.names.size(); ++i)
264  {
265  pal_statistics_msgs::Statistic s;
266  s.name = names.names[i];
267  s.value = values.values[i];
268  msg_.statistics.push_back(s);
269  }
270  last_names_version_ = names.names_version;
271  }
272 }
273 }
bool publishAsync()
publishAsync Capture data and flag it to be published by the publisher thread. Real-Time safe...
IdType registerInternal(const std::string &name, VariableHolder &&variable, RegistrationsRAII *bookkeeping, bool enabled)
int registerVariable(const std::string &name, VariableHolder &&holder, bool enabled=true)
StatisticsRegistry(const std::string &topic)
pal_statistics_msgs::StatisticsValues values_msg_
XmlRpcServer s
bool remove(const std::string &name)
IdType customRegister(StatisticsRegistry &registry, const std::string &name, const T *variable, RegistrationsRAII *bookkeeping=NULL, bool enabled=true)
Default implementation that accepts anything variable that can be casted to a double.
void doPublish(bool publish_names_msg=true)
doPublish publishes the subscribed topics, requires mutex
bool updateMsg(pal_statistics_msgs::StatisticsNames &names, pal_statistics_msgs::StatisticsValues &values, bool smart_fill=false)
updateMsg update names and values, optionally using smartfill to minimize copying ...
GeneratedStatistics generated_statistics_
void setEnabled(const IdType &id, bool enabled)
void startPublishThread()
startPublishThread creates and starts the publisherThread. The user still has to call publishAsync ea...
void publish()
publish Reads the values of all registered variables and publishes them to the topic associated to th...
bool smartFillMsg(pal_statistics_msgs::StatisticsNames &names, pal_statistics_msgs::StatisticsValues &values)
smartFillMsg Attempts to minimize the amount of string copies
void publish(const boost::shared_ptr< M > &message) const
void update(const pal_statistics_msgs::StatisticsNames &names, const pal_statistics_msgs::StatisticsValues &values)
bool setEnabledmpl(const IdType &id, bool enabled)
void unregisterVariable(const IdType &id)
static SteadyTime now()
The Registration class is a handle to a registered variable, when out of scope unregisters the variab...
void fillMsg(pal_statistics_msgs::StatisticsNames &names, pal_statistics_msgs::StatisticsValues &value)
fills message with the last captured values.
boost::shared_ptr< boost::thread > publisher_thread_
ROSCPP_DECL bool ok()
pal_statistics_msgs::Statistics msg_
This message is generated using an updated StatiticsNames and StatisticsValues.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep() const
pal_statistics_msgs::StatisticsNames names_msg_
LockFreeQueue< EnabledId > enabled_ids_
disabled_ids_ this is used to keep track of enabled/disabled variables in a lock free way ...
The RegistrationsRAII class holds handles to registered variables and when it is destroyed, unregisters them automatically.
#define ROS_INFO_STREAM(args)
#define ROS_WARN_STREAM_ONCE(args)
void add(Registration &&registration)
uint32_t getNumSubscribers() const
pal_statistics_msgs::Statistics createMsg()
createMsg creates a Statistics message from the registered variables, useful for debugging ...
void unregisterVariable(const std::string &name, RegistrationsRAII *bookkeeping=NULL)
void handlePendingDisables(const boost::unique_lock< boost::mutex > &data_lock)
handlePendingDisables Empties by handling the queue of disabled/enabled ids.
IdType registerFunction(const std::string &name, const boost::function< double()> &funct, RegistrationsRAII *bookkeeping=NULL, bool enabled=true)
registerFunction Adds a function that returns double with the specified name


pal_statistics
Author(s):
autogenerated on Mon Feb 28 2022 23:04:05