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


swri_roscpp
Author(s):
autogenerated on Tue Oct 3 2017 03:19:27