subscriber_test.cpp
Go to the documentation of this file.
1 #include <boost/bind.hpp>
2 #include <ros/ros.h>
4 #include <boost/bind.hpp>
5 
7 
8 #include <std_msgs/Float32.h>
9 #include <nav_msgs/Odometry.h>
10 
11 namespace du = diagnostic_updater;
12 
13 // Alias type for easier access to DiagnosticStatus enumerations.
14 typedef diagnostic_msgs::DiagnosticStatus DS;
15 
17 {
21 
23 
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  // Example to subscribe to a class method.
41  sub_ = swri::Subscriber(nh_, "odom", 100, &SubscriberTest::handleMessage, this);
42 
43  // Example to subscribe using a boost function. I am unable to
44  // figure out how to get the compiler to infer the type, so I have
45  // to declare the function first and then pass that.
46  boost::function<void(const boost::shared_ptr< nav_msgs::Odometry const> &)> callback = boost::bind(&SubscriberTest::handleMessage, this, _1);
47  sub_ = swri::Subscriber(nh_, "odom", 100, callback);
48 
49  sub_.setTimeout(ros::Duration(1.0));
50 
51  diagnostic_updater_.setHardwareID("none");
52  diagnostic_updater_.add(
53  "swri::Subscriber test (manual diagnostics)", this,
55 
56  diagnostic_updater_.add(
57  "swri::Subscriber test (auto diagnostics)", this,
59 
60  diag_timer_ = nh_.createTimer(ros::Duration(1.0),
62  this);
63  }
64 
65  void handleMessage(const nav_msgs::OdometryConstPtr &msg)
66  {
67  ROS_INFO_THROTTLE(1.0, "Receiving messages.");
68  }
69 
71  {
72  diagnostic_updater_.update();
73  }
74 
76  {
77  status.summary(DS::OK, "No errors reported.");
78 
79  // The swri::Subscriber provides methods to access all of the
80  // statistics and status information. These are suitable for
81  // making control decisions or report diagnostics. In this
82  // example, we access the data directly to update a diagnostic
83  // status on our own. This is useful if you want to apply custom
84  // summary semantics or change how the key/values are included.
85 
86  if (sub_.messageCount() == 0) {
87  status.mergeSummary(DS::WARN, "No messages received.");
88  } else if (sub_.inTimeout()) {
89  status.mergeSummary(DS::ERROR, "Topic has timed out.");
90  } else if (sub_.timeoutCount() > 0) {
91  status.mergeSummary(DS::WARN, "Timeouts have occurred.");
92  }
93 
94  if (sub_.mappedTopic() == sub_.unmappedTopic()) {
95  status.addf("Topic Name", "%s", sub_.mappedTopic().c_str());
96  } else {
97  status.addf("Topic Name", "%s -> %s",
98  sub_.unmappedTopic().c_str(),
99  sub_.mappedTopic().c_str());
100  }
101  status.addf("Number of publishers", "%d", sub_.numPublishers());
102 
103  status.addf("Mean Latency [us]", "%f", sub_.meanLatencyMicroseconds());
104  status.addf("Min Latency [us]", "%f", sub_.minLatencyMicroseconds());
105  status.addf("Max Latency [us]", "%f", sub_.maxLatencyMicroseconds());
106 
107  status.addf("Mean Frequency [Hz]", "%f", sub_.meanFrequencyHz());
108  status.addf("Mean Period [ms]", "%f", sub_.meanPeriodMilliseconds());
109  status.addf("Min Period [ms]", "%f", sub_.minPeriodMilliseconds());
110  status.addf("Max Period [ms]", "%f", sub_.maxPeriodMilliseconds());
111 
112  if (sub_.timeoutEnabled()) {
113  status.addf("Timed Out Count", "%d [> %f ms]",
114  sub_.timeoutCount(),
115  sub_.timeoutMilliseconds());
116  } else {
117  status.add("Timed Out Count", "N/A");
118  }
119  }
120 
122  {
123  status.summary(DS::OK, "No errors reported.");
124 
125  // In this example, we use swri::Subscriber's appendDiagnostics
126  // method to include diagnostics using a common format and common
127  // summary semantics. If we didn't care about specific bits of
128  // information, we can leave them out by removing specific flags.
129  sub_.appendDiagnostics(status, "Odometry",
135  }
136 }; // class SubscriberTest
137 
138 int main(int argc, char **argv)
139 {
140  ros::init(argc, argv, "subscriber_test");
141 
142  SubscriberTest node;
143  ros::spin();
144 
145  return 0;
146 }
const std::string & mappedTopic() const
Definition: subscriber.h:288
ros::NodeHandle nh_
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)
ros::WallTimer init_timer_
void add(const std::string &name, TaskFunction f)
du::Updater diagnostic_updater_
void appendDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status, const std::string &name, const int flags)
Definition: subscriber.h:477
double maxLatencyMicroseconds() const
Definition: subscriber.h:364
swri::Subscriber sub_
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
#define ROS_INFO_THROTTLE(period,...)
diagnostic_msgs::DiagnosticStatus DS
#define ROS_INFO(...)
double meanLatencyMicroseconds() const
Definition: subscriber.h:352
int numPublishers() const
Definition: subscriber.h:294
int main(int argc, char **argv)
ROSCPP_DECL void spin()
void setTimeout(const ros::Duration &time_out)
Definition: subscriber.h:412
void initialize(const ros::WallTimerEvent &ignored)
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
void autoDiagnostics(du::DiagnosticStatusWrapper &status)
bool timeoutEnabled() const
Definition: subscriber.h:453
void handleDiagnosticsTimer(const ros::TimerEvent &ignored)
ros::Timer diag_timer_
int messageCount() const
Definition: subscriber.h:306
void add(const std::string &key, const T &val)
double meanPeriodMilliseconds() const
Definition: subscriber.h:394
void handleMessage(const nav_msgs::OdometryConstPtr &msg)
double meanFrequencyHz() const
Definition: subscriber.h:376


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