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...");
34  this,
35  true);
36  }
37 
38  void initialize(const ros::WallTimerEvent &ignored)
39  {
41  sub_ = swri::Subscriber(nh_, "odom", &msg_);
42 
45  "swri::Subscriber test (manual diagnostics)", this,
47 
50  "swri::Subscriber test (auto diagnostics)", this,
52 
55  "swri::Subscriber test (value diagnostics)", this,
57 
60  this);
61  }
62 
64  {
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(),
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 }
swri::Subscriber::maxPeriodMilliseconds
double maxPeriodMilliseconds() const
Definition: subscriber.h:406
swri::Subscriber
Definition: subscriber.h:61
swri::Subscriber::meanPeriodMilliseconds
double meanPeriodMilliseconds() const
Definition: subscriber.h:394
diagnostic_updater::DiagnosticStatusWrapper::add
void add(const std::string &key, const bool &b)
swri::Subscriber::meanFrequencyHz
double meanFrequencyHz() const
Definition: subscriber.h:376
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
StoringSubscriberTest::init_timer_
ros::WallTimer init_timer_
Definition: storing_subscriber_test.cpp:17
ros.h
swri::Subscriber::mappedTopic
const std::string & mappedTopic() const
Definition: subscriber.h:288
ros::WallTimer
diagnostic_updater::Updater
subscriber.h
StoringSubscriberTest
Definition: storing_subscriber_test.cpp:14
StoringSubscriberTest::msg_
std_msgs::Float32ConstPtr msg_
Definition: storing_subscriber_test.cpp:22
StoringSubscriberTest::sub_
swri::Subscriber sub_
Definition: storing_subscriber_test.cpp:23
diagnostic_updater.h
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(const diagnostic_msgs::DiagnosticStatus &src)
StoringSubscriberTest::diagnostic_updater_
du::Updater diagnostic_updater_
Definition: storing_subscriber_test.cpp:20
StoringSubscriberTest::StoringSubscriberTest
StoringSubscriberTest()
Definition: storing_subscriber_test.cpp:27
swri::Subscriber::timeoutCount
int timeoutCount()
Definition: subscriber.h:471
diagnostic_updater
StoringSubscriberTest::manualDiagnostics
void manualDiagnostics(du::DiagnosticStatusWrapper &status)
Definition: storing_subscriber_test.cpp:68
swri::Subscriber::appendDiagnostics
void appendDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status, const std::string &name, const int flags)
Definition: subscriber.h:477
swri::Subscriber::timeoutMilliseconds
double timeoutMilliseconds() const
Definition: subscriber.h:459
swri::Subscriber::unmappedTopic
const std::string & unmappedTopic() const
Definition: subscriber.h:282
swri::Subscriber::maxLatencyMicroseconds
double maxLatencyMicroseconds() const
Definition: subscriber.h:364
StoringSubscriberTest::autoDiagnostics
void autoDiagnostics(du::DiagnosticStatusWrapper &status)
Definition: storing_subscriber_test.cpp:114
StoringSubscriberTest::nh_
ros::NodeHandle nh_
Definition: storing_subscriber_test.cpp:16
swri::Subscriber::DIAG_CONNECTION
@ DIAG_CONNECTION
Definition: subscriber.h:182
DS
diagnostic_msgs::DiagnosticStatus DS
Definition: storing_subscriber_test.cpp:12
StoringSubscriberTest::valueDiagnostics
void valueDiagnostics(du::DiagnosticStatusWrapper &status)
Definition: storing_subscriber_test.cpp:130
swri::Subscriber::DIAG_MSG_COUNT
@ DIAG_MSG_COUNT
Definition: subscriber.h:183
StoringSubscriberTest::handleDiagnosticsTimer
void handleDiagnosticsTimer(const ros::TimerEvent &ignored)
Definition: storing_subscriber_test.cpp:63
ros::TimerEvent
swri::Subscriber::messageCount
int messageCount() const
Definition: subscriber.h:306
diagnostic_updater::DiagnosticStatusWrapper::mergeSummary
void mergeSummary(const diagnostic_msgs::DiagnosticStatus &src)
diagnostic_updater::Updater::setHardwareID
void setHardwareID(const std::string &hwid)
StoringSubscriberTest::initialize
void initialize(const ros::WallTimerEvent &ignored)
Definition: storing_subscriber_test.cpp:38
swri::Subscriber::inTimeout
bool inTimeout()
Definition: subscriber.h:465
diagnostic_updater::Updater::update
void update()
ros::WallTimerEvent
swri::Subscriber::DIAG_RATE
@ DIAG_RATE
Definition: subscriber.h:186
swri::Subscriber::numPublishers
int numPublishers() const
Definition: subscriber.h:294
swri::Subscriber::meanLatencyMicroseconds
double meanLatencyMicroseconds() const
Definition: subscriber.h:352
swri::Subscriber::DIAG_LATENCY
@ DIAG_LATENCY
Definition: subscriber.h:185
swri::Subscriber::setTimeout
void setTimeout(const ros::Duration &time_out)
Definition: subscriber.h:412
diagnostic_updater::DiagnosticStatusWrapper::addf
void addf(const std::string &key, const char *format,...)
diagnostic_updater::DiagnosticStatusWrapper
ros::spin
ROSCPP_DECL void spin()
swri::Subscriber::minPeriodMilliseconds
double minPeriodMilliseconds() const
Definition: subscriber.h:400
ros::WallDuration
swri::Subscriber::timeoutEnabled
bool timeoutEnabled() const
Definition: subscriber.h:453
main
int main(int argc, char **argv)
Definition: storing_subscriber_test.cpp:142
ROS_INFO
#define ROS_INFO(...)
diagnostic_updater::DiagnosticTaskVector::add
void add(const std::string &name, TaskFunction f)
swri::Subscriber::minLatencyMicroseconds
double minLatencyMicroseconds() const
Definition: subscriber.h:358
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
ros::Duration
ros::NodeHandle::createWallTimer
WallTimer createWallTimer(WallDuration period, const WallTimerCallback &callback, bool oneshot=false, bool autostart=true) const
ros::Timer
swri::Subscriber::DIAG_TIMEOUT
@ DIAG_TIMEOUT
Definition: subscriber.h:184
ros::NodeHandle
StoringSubscriberTest::diag_timer_
ros::Timer diag_timer_
Definition: storing_subscriber_test.cpp:18


swri_roscpp
Author(s): P. J. Reed
autogenerated on Fri Aug 2 2024 08:39:15