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


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