latched_subscriber_test.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 
5 
6 #include <std_msgs/Float32.h>
7 #include <nav_msgs/Odometry.h>
8 
9 namespace du = diagnostic_updater;
10 
11 // Alias type for easier access to DiagnosticStatus enumerations.
12 typedef diagnostic_msgs::DiagnosticStatus DS;
13 
15 {
19 
21 
23 
24  public:
26  {
27  // Setup a one-shot timer to initialize the node after a brief
28  // delay so that /rosout is always fully initialized.
29  ROS_INFO("Starting initialization timer...");
30  init_timer_ = nh_.createWallTimer(ros::WallDuration(1.0),
32  this,
33  true);
34  }
35 
36  void initialize(const ros::WallTimerEvent &ignored)
37  {
38  sub_.setTimeout(ros::Duration(1.0));
39  sub_.initialize(nh_, "odom");
40 
41  diagnostic_updater_.setHardwareID("none");
42  diagnostic_updater_.add(
43  "swri::Subscriber test (manual diagnostics)", this,
45 
46  diagnostic_updater_.setHardwareID("none");
47  diagnostic_updater_.add(
48  "swri::Subscriber test (auto diagnostics)", this,
50 
51  diagnostic_updater_.setHardwareID("none");
52  diagnostic_updater_.add(
53  "swri::Subscriber test (value diagnostics)", this,
55 
56  diag_timer_ = nh_.createTimer(ros::Duration(1.0),
58  this);
59  }
60 
62  {
63  diagnostic_updater_.update();
64  }
65 
67  {
68  status.summary(DS::OK, "No errors reported.");
69 
70  // The swri::Subscriber provides methods to access all of the
71  // statistics and status information. These are suitable for
72  // making control decisions or report diagnostics. In this
73  // example, we access the data directly to update a diagnostic
74  // status on our own. This is useful if you want to apply custom
75  // summary semantics or change how the key/values are included.
76 
77  if (sub_.messageCount() == 0) {
78  status.mergeSummary(DS::WARN, "No messages received.");
79  } else if (sub_.inTimeout()) {
80  status.mergeSummary(DS::ERROR, "Topic has timed out.");
81  } else if (sub_.timeoutCount() > 0) {
82  status.mergeSummary(DS::WARN, "Timeouts have occurred.");
83  }
84 
85  if (sub_.mappedTopic() == sub_.unmappedTopic()) {
86  status.addf("Topic Name", "%s", sub_.mappedTopic().c_str());
87  } else {
88  status.addf("Topic Name", "%s -> %s",
89  sub_.unmappedTopic().c_str(),
90  sub_.mappedTopic().c_str());
91  }
92  status.addf("Number of publishers", "%d", sub_.numPublishers());
93 
94  status.addf("Mean Latency [us]", "%f", sub_.meanLatencyMicroseconds());
95  status.addf("Min Latency [us]", "%f", sub_.minLatencyMicroseconds());
96  status.addf("Max Latency [us]", "%f", sub_.maxLatencyMicroseconds());
97 
98  status.addf("Mean Frequency [Hz]", "%f", sub_.meanFrequencyHz());
99  status.addf("Mean Period [ms]", "%f", sub_.meanPeriodMilliseconds());
100  status.addf("Min Period [ms]", "%f", sub_.minPeriodMilliseconds());
101  status.addf("Max Period [ms]", "%f", sub_.maxPeriodMilliseconds());
102 
103  if (sub_.timeoutEnabled()) {
104  status.addf("Timed Out Count", "%d [> %f ms]",
105  sub_.timeoutCount(),
106  sub_.timeoutMilliseconds());
107  } else {
108  status.add("Timed Out Count", "N/A");
109  }
110  }
111 
113  {
114  status.summary(DS::OK, "No errors reported.");
115 
116  // In this example, we use swri::Subscriber's appendDiagnostics
117  // method to include diagnostics using a common format and common
118  // summary semantics. If we didn't care about specific bits of
119  // information, we can leave them out by removing specific flags.
120  sub_.appendDiagnostics(status, "Odometry",
126  }
127 
129  {
130  if (!sub_.messageCount()) {
131  status.summary(DS::WARN, "No message has been received.");
132  status.add("Float value", "N/A");
133  } else {
134  status.summary(DS::OK, "No errors reported.");
135  if (sub_.data()) {
136  status.addf("Float value (data())", "%f", sub_.data()->data);
137  } else {
138  ROS_ERROR("no data?!");
139  }
140 
141  status.addf("Float value (->)", "%f", sub_->data);
142  }
143  }
144 }; // class LatchedSubscriberTest
145 
146 int main(int argc, char **argv)
147 {
148  ros::init(argc, argv, "latched_subscriber_test");
149 
151  ros::spin();
152 
153  return 0;
154 }
const std::string & mappedTopic() const
Definition: subscriber.h:288
void initialize(const ros::WallTimerEvent &ignored)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void summary(unsigned char lvl, const std::string s)
void handleDiagnosticsTimer(const ros::TimerEvent &ignored)
double maxPeriodMilliseconds() const
Definition: subscriber.h:406
void setHardwareID(const std::string &hwid)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void add(const std::string &name, TaskFunction f)
void appendDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status, const std::string &name, const int flags)
Definition: subscriber.h:477
double maxLatencyMicroseconds() const
Definition: subscriber.h:364
void autoDiagnostics(du::DiagnosticStatusWrapper &status)
const std::string & unmappedTopic() const
Definition: subscriber.h:282
void addf(const std::string &key, const char *format,...)
void initialize(ros::NodeHandle &nh, const std::string &topic, const ros::TransportHints &transport_hints=ros::TransportHints())
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
#define ROS_INFO(...)
double meanLatencyMicroseconds() const
Definition: subscriber.h:352
int numPublishers() const
Definition: subscriber.h:294
ROSCPP_DECL void spin()
void setTimeout(const ros::Duration &time_out)
Definition: subscriber.h:412
void mergeSummary(unsigned char lvl, const std::string s)
double timeoutMilliseconds() const
Definition: subscriber.h:459
double minPeriodMilliseconds() const
Definition: subscriber.h:400
double minLatencyMicroseconds() const
Definition: subscriber.h:358
bool timeoutEnabled() const
Definition: subscriber.h:453
int main(int argc, char **argv)
int messageCount() const
Definition: subscriber.h:306
void add(const std::string &key, const T &val)
const boost::shared_ptr< M const > & data() const
double meanPeriodMilliseconds() const
Definition: subscriber.h:394
swri::LatchedSubscriber< std_msgs::Float32 > sub_
#define ROS_ERROR(...)
void manualDiagnostics(du::DiagnosticStatusWrapper &status)
diagnostic_msgs::DiagnosticStatus DS
double meanFrequencyHz() const
Definition: subscriber.h:376
void valueDiagnostics(du::DiagnosticStatusWrapper &status)


swri_roscpp
Author(s):
autogenerated on Sat Jan 21 2023 03:13:16