storing_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 
22  std_msgs::Float32ConstPtr msg_;
24 
25 
26  public:
28  {
29  // Setup a one-shot timer to initialize the node after a brief
30  // delay so that /rosout is always fully initialized.
31  ROS_INFO("Starting initialization timer...");
32  init_timer_ = nh_.createWallTimer(ros::WallDuration(1.0),
34  this,
35  true);
36  }
37 
38  void initialize(const ros::WallTimerEvent &ignored)
39  {
40  sub_.setTimeout(ros::Duration(1.0));
41  sub_ = swri::Subscriber(nh_, "odom", &msg_);
42 
43  diagnostic_updater_.setHardwareID("none");
44  diagnostic_updater_.add(
45  "swri::Subscriber test (manual diagnostics)", this,
47 
48  diagnostic_updater_.setHardwareID("none");
49  diagnostic_updater_.add(
50  "swri::Subscriber test (auto diagnostics)", this,
52 
53  diagnostic_updater_.setHardwareID("none");
54  diagnostic_updater_.add(
55  "swri::Subscriber test (value diagnostics)", this,
57 
58  diag_timer_ = nh_.createTimer(ros::Duration(1.0),
60  this);
61  }
62 
64  {
65  diagnostic_updater_.update();
66  }
67 
69  {
70  status.summary(DS::OK, "No errors reported.");
71 
72  // The swri::Subscriber provides methods to access all of the
73  // statistics and status information. These are suitable for
74  // making control decisions or report diagnostics. In this
75  // example, we access the data directly to update a diagnostic
76  // status on our own. This is useful if you want to apply custom
77  // summary semantics or change how the key/values are included.
78 
79  if (sub_.messageCount() == 0) {
80  status.mergeSummary(DS::WARN, "No messages received.");
81  } else if (sub_.inTimeout()) {
82  status.mergeSummary(DS::ERROR, "Topic has timed out.");
83  } else if (sub_.timeoutCount() > 0) {
84  status.mergeSummary(DS::WARN, "Timeouts have occurred.");
85  }
86 
87  if (sub_.mappedTopic() == sub_.unmappedTopic()) {
88  status.addf("Topic Name", "%s", sub_.mappedTopic().c_str());
89  } else {
90  status.addf("Topic Name", "%s -> %s",
91  sub_.unmappedTopic().c_str(),
92  sub_.mappedTopic().c_str());
93  }
94  status.addf("Number of publishers", "%d", sub_.numPublishers());
95 
96  status.addf("Mean Latency [us]", "%f", sub_.meanLatencyMicroseconds());
97  status.addf("Min Latency [us]", "%f", sub_.minLatencyMicroseconds());
98  status.addf("Max Latency [us]", "%f", sub_.maxLatencyMicroseconds());
99 
100  status.addf("Mean Frequency [Hz]", "%f", sub_.meanFrequencyHz());
101  status.addf("Mean Period [ms]", "%f", sub_.meanPeriodMilliseconds());
102  status.addf("Min Period [ms]", "%f", sub_.minPeriodMilliseconds());
103  status.addf("Max Period [ms]", "%f", sub_.maxPeriodMilliseconds());
104 
105  if (sub_.timeoutEnabled()) {
106  status.addf("Timed Out Count", "%d [> %f ms]",
107  sub_.timeoutCount(),
108  sub_.timeoutMilliseconds());
109  } else {
110  status.add("Timed Out Count", "N/A");
111  }
112  }
113 
115  {
116  status.summary(DS::OK, "No errors reported.");
117 
118  // In this example, we use swri::Subscriber's appendDiagnostics
119  // method to include diagnostics using a common format and common
120  // summary semantics. If we didn't care about specific bits of
121  // information, we can leave them out by removing specific flags.
122  sub_.appendDiagnostics(status, "Odometry",
128  }
129 
131  {
132  if (!msg_) {
133  status.summary(DS::WARN, "No message has been received.");
134  status.add("Float value", "N/A");
135  } else {
136  status.summary(DS::OK, "No errors reported.");
137  status.addf("Float value", "%f", msg_->data);
138  }
139  }
140 }; // class StoringSubscriberTest
141 
142 int main(int argc, char **argv)
143 {
144  ros::init(argc, argv, "storing_subscriber_test");
145 
147  ros::spin();
148 
149  return 0;
150 }
const std::string & mappedTopic() const
Definition: subscriber.h:288
void valueDiagnostics(du::DiagnosticStatusWrapper &status)
void handleDiagnosticsTimer(const ros::TimerEvent &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)
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
int main(int argc, char **argv)
const std::string & unmappedTopic() const
Definition: subscriber.h:282
void addf(const std::string &key, const char *format,...)
void manualDiagnostics(du::DiagnosticStatusWrapper &status)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
void autoDiagnostics(du::DiagnosticStatusWrapper &status)
#define ROS_INFO(...)
diagnostic_msgs::DiagnosticStatus DS
double meanLatencyMicroseconds() const
Definition: subscriber.h:352
int numPublishers() const
Definition: subscriber.h:294
void initialize(const ros::WallTimerEvent &ignored)
ROSCPP_DECL void spin()
void setTimeout(const ros::Duration &time_out)
Definition: subscriber.h:412
std_msgs::Float32ConstPtr msg_
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 messageCount() const
Definition: subscriber.h:306
void add(const std::string &key, const T &val)
double meanPeriodMilliseconds() const
Definition: subscriber.h:394
double meanFrequencyHz() const
Definition: subscriber.h:376


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