subscriber_test.cpp
Go to the documentation of this file.
00001 #include <boost/bind.hpp>
00002 #include <ros/ros.h>
00003 #include <swri_roscpp/subscriber.h>
00004 #include <boost/bind.hpp>
00005 
00006 #include <diagnostic_updater/diagnostic_updater.h>
00007 
00008 #include <std_msgs/Float32.h>
00009 #include <nav_msgs/Odometry.h>
00010 
00011 namespace du = diagnostic_updater;
00012 
00013 // Alias type for easier access to DiagnosticStatus enumerations.
00014 typedef diagnostic_msgs::DiagnosticStatus DS;
00015 
00016 class SubscriberTest
00017 {
00018   ros::NodeHandle nh_;
00019   ros::WallTimer init_timer_;
00020   ros::Timer diag_timer_;
00021 
00022   du::Updater diagnostic_updater_;
00023   
00024   swri::Subscriber sub_;
00025   
00026  public:
00027   SubscriberTest()
00028   {
00029     // Setup a one-shot timer to initialize the node after a brief
00030     // delay so that /rosout is always fully initialized.
00031     ROS_INFO("Starting initialization timer...");
00032     init_timer_ = nh_.createWallTimer(ros::WallDuration(1.0),
00033                                       &SubscriberTest::initialize,
00034                                       this,
00035                                       true);
00036   }
00037 
00038   void initialize(const ros::WallTimerEvent &ignored)
00039   {
00040     // Example to subscribe to a class method.
00041     sub_ = swri::Subscriber(nh_, "odom", 100, &SubscriberTest::handleMessage, this);
00042 
00043     // Example to subscribe using a boost function.  I am unable to
00044     // figure out how to get the compiler to infer the type, so I have
00045     // to declare the function first and then pass that.
00046     boost::function<void(const boost::shared_ptr< nav_msgs::Odometry const> &)> callback = boost::bind(&SubscriberTest::handleMessage, this, _1);
00047     sub_ = swri::Subscriber(nh_, "odom", 100, callback);
00048 
00049     sub_.setTimeout(ros::Duration(1.0));
00050 
00051     diagnostic_updater_.setHardwareID("none");
00052     diagnostic_updater_.add(
00053       "swri::Subscriber test (manual diagnostics)", this,
00054       &SubscriberTest::manualDiagnostics);
00055 
00056     diagnostic_updater_.add(
00057       "swri::Subscriber test (auto diagnostics)", this,
00058       &SubscriberTest::autoDiagnostics);
00059     
00060     diag_timer_ = nh_.createTimer(ros::Duration(1.0),
00061                                   &SubscriberTest::handleDiagnosticsTimer,
00062                                   this);
00063   }
00064 
00065   void handleMessage(const nav_msgs::OdometryConstPtr &msg)
00066   {
00067     ROS_INFO_THROTTLE(1.0, "Receiving messages.");
00068   }
00069 
00070   void handleDiagnosticsTimer(const ros::TimerEvent &ignored)
00071   {
00072     diagnostic_updater_.update();
00073   }
00074 
00075   void manualDiagnostics(du::DiagnosticStatusWrapper& status) // NOLINT
00076   {
00077     status.summary(DS::OK, "No errors reported.");
00078 
00079     // The swri::Subscriber provides methods to access all of the
00080     // statistics and status information.  These are suitable for
00081     // making control decisions or report diagnostics.  In this
00082     // example, we access the data directly to update a diagnostic
00083     // status on our own.  This is useful if you want to apply custom
00084     // summary semantics or change how the key/values are included.
00085     
00086     if (sub_.messageCount() == 0) {
00087       status.mergeSummary(DS::WARN, "No messages received.");
00088     } else if (sub_.inTimeout()) {
00089       status.mergeSummary(DS::ERROR, "Topic has timed out.");
00090     } else if (sub_.timeoutCount() > 0) {
00091       status.mergeSummary(DS::WARN, "Timeouts have occurred.");
00092     }
00093 
00094     if (sub_.mappedTopic() == sub_.unmappedTopic()) {
00095       status.addf("Topic Name", "%s", sub_.mappedTopic().c_str());
00096     } else {
00097       status.addf("Topic Name", "%s -> %s",
00098                   sub_.unmappedTopic().c_str(),
00099                   sub_.mappedTopic().c_str());
00100     }
00101     status.addf("Number of publishers", "%d", sub_.numPublishers());
00102     
00103     status.addf("Mean Latency [us]", "%f", sub_.meanLatencyMicroseconds());
00104     status.addf("Min Latency [us]", "%f", sub_.minLatencyMicroseconds());
00105     status.addf("Max Latency [us]", "%f", sub_.maxLatencyMicroseconds());
00106     
00107     status.addf("Mean Frequency [Hz]", "%f", sub_.meanFrequencyHz());
00108     status.addf("Mean Period [ms]", "%f", sub_.meanPeriodMilliseconds());
00109     status.addf("Min Period [ms]", "%f", sub_.minPeriodMilliseconds());
00110     status.addf("Max Period [ms]", "%f", sub_.maxPeriodMilliseconds());
00111 
00112     if (sub_.timeoutEnabled()) {
00113       status.addf("Timed Out Count", "%d [> %f ms]",
00114                   sub_.timeoutCount(),
00115                   sub_.timeoutMilliseconds());
00116     } else {
00117       status.add("Timed Out Count", "N/A");
00118     }
00119   }
00120 
00121   void autoDiagnostics(du::DiagnosticStatusWrapper& status) // NOLINT
00122   {
00123     status.summary(DS::OK, "No errors reported.");
00124 
00125     // In this example, we use swri::Subscriber's appendDiagnostics
00126     // method to include diagnostics using a common format and common
00127     // summary semantics.  If we didn't care about specific bits of
00128     // information, we can leave them out by removing specific flags.
00129     sub_.appendDiagnostics(status, "Odometry",
00130                            swri::Subscriber::DIAG_CONNECTION |
00131                            swri::Subscriber::DIAG_MSG_COUNT |
00132                            swri::Subscriber::DIAG_TIMEOUT |
00133                            swri::Subscriber::DIAG_LATENCY |
00134                            swri::Subscriber::DIAG_RATE);
00135   }
00136 };  // class SubscriberTest
00137 
00138 int main(int argc, char **argv)
00139 {
00140   ros::init(argc, argv, "subscriber_test");
00141 
00142   SubscriberTest node;
00143   ros::spin();
00144   
00145   return 0;  
00146 }


swri_roscpp
Author(s):
autogenerated on Thu Jun 6 2019 20:34:47