aggregator.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
39 #ifndef DIAGNOSTIC_AGGREGATOR_H
40 #define DIAGNOSTIC_AGGREGATOR_H
41 
42 #include <ros/ros.h>
43 #include <string>
44 #include <map>
45 #include <vector>
46 #include <set>
47 #include <boost/shared_ptr.hpp>
48 #include <boost/thread/mutex.hpp>
49 #include <bondcpp/bond.h>
50 #include <diagnostic_msgs/DiagnosticArray.h>
51 #include <diagnostic_msgs/DiagnosticStatus.h>
52 #include <diagnostic_msgs/KeyValue.h>
53 #include <diagnostic_msgs/AddDiagnostics.h>
54 #include "XmlRpcValue.h"
59 
60 
62 
106 {
107 public:
111  Aggregator();
112 
113  ~Aggregator();
114 
118  void publishData();
119 
123  bool ok() const { return n_.ok(); }
124 
128  double getPubRate() const { return pub_rate_; }
129 
130 private:
136  boost::mutex mutex_;
137  double pub_rate_;
138 
142  void diagCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr& diag_msg);
143 
150  bool addDiagnostics(diagnostic_msgs::AddDiagnostics::Request &req,
151  diagnostic_msgs::AddDiagnostics::Response &res);
152 
154 
156 
157  std::vector<boost::shared_ptr<bond::Bond> > bonds_;
159  /*
160  *!\brief called when a bond between the aggregator and a node is broken
161  *
162  * Modifies the contents of added_analyzers_ and analyzer_group, removing the
163  * diagnostics that had been brought up by that bond.
164  *!\param bond_id The bond id (namespace) from which the analyzer was created
165  *!\param analyzer Shared pointer to the analyzer group that was added
166  */
167  void bondBroken(std::string bond_id,
168  boost::shared_ptr<Analyzer> analyzer);
169 
170  /*
171  *!\brief called when a bond is formed between the aggregator and a node.
172  * Actually adds the analyzergroup to the main analyzer group. Before this
173  * function is called, the added diagnostics will not be analyzed by the
174  * aggregator.
175  *!\param group Shared pointer to the analyzer group that is to be added,
176  * which was created in the addDiagnostics function
177  */
179 
182  std::set<std::string> ros_warnings_;
184  /*
185  *!\brief Checks timestamp of message, and warns if timestamp is 0 (not set)
186  */
187  void checkTimestamp(const diagnostic_msgs::DiagnosticArray::ConstPtr& diag_msg);
188 
189 };
190 
191 /*
192  *!\brief Functor for checking whether a bond has the same ID as the given string
193  */
195 {
196  BondIDMatch(const std::string s) : s(s) {}
197  bool operator()(const boost::shared_ptr<bond::Bond>& b){ return s == b->getId(); }
198  const std::string s;
199 };
200 
201 }
202 
203 #endif // DIAGNOSTIC_AGGREGATOR_H
void diagCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr &diag_msg)
Callback for incoming "/diagnostics".
Definition: aggregator.cpp:95
::std::string string
Definition: gtest.h:1979
XmlRpcServer s
std::string base_path_
Prepended to all status names of aggregator.
Definition: aggregator.h:180
bool operator()(const boost::shared_ptr< bond::Bond > &b)
Definition: aggregator.h:197
void bondFormed(boost::shared_ptr< Analyzer > group)
Definition: aggregator.cpp:144
BondIDMatch(const std::string s)
Definition: aggregator.h:196
Aggregator()
Constructor initializes with main prefix (ex: &#39;/Robot&#39;)
Definition: aggregator.cpp:42
void checkTimestamp(const diagnostic_msgs::DiagnosticArray::ConstPtr &diag_msg)
Definition: aggregator.cpp:74
void bondBroken(std::string bond_id, boost::shared_ptr< Analyzer > analyzer)
Definition: aggregator.cpp:125
std::set< std::string > ros_warnings_
Records all ROS warnings. No warnings are repeated.
Definition: aggregator.h:182
bool addDiagnostics(diagnostic_msgs::AddDiagnostics::Request &req, diagnostic_msgs::AddDiagnostics::Response &res)
Service request callback for addition of diagnostics. Creates a bond between the calling node and the...
Definition: aggregator.cpp:151
double getPubRate() const
Publish rate defaults to 1Hz, but can be set with ~pub_rate param.
Definition: aggregator.h:128
std::vector< boost::shared_ptr< bond::Bond > > bonds_
Contains all bonds for additional diagnostics.
Definition: aggregator.h:157
Allows analyzers to be grouped together, or used as sub-analyzers.
bool ok() const
True if the NodeHandle reports OK.
Definition: aggregator.h:123
Aggregator processes /diagnostics, republishes on /diagnostics_agg.
Definition: aggregator.h:105
void publishData()
Processes, publishes data. Should be called at pub_rate.
Definition: aggregator.cpp:202
bool ok() const


diagnostic_aggregator
Author(s): Kevin Watts, Brice Rebsamen
autogenerated on Mon Feb 28 2022 22:16:34