[Personalrobots-commit] SF.net SVN: personalrobots:[16583] pkg/trunk

[<bla...@us...>]

Revision: 16583
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16583&view=rev
Author:   blaisegassend
Date:     2009-06-02 15:35:05 +0000 (Tue, 02 Jun 2009)

Log Message:
-----------
Added DiagnosedPublisher and HeaderlessDiagnosedPublisher to automatically
publish diagnostics upon publication, and integrated them with the
forearm_camera.
Started writing an outling of the driver_base classes.

Modified Paths:
--------------
    pkg/trunk/drivers/cam/camera_trigger_test/src/CMakeLists.txt
    pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
    pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml
    pkg/trunk/drivers/cam/camera_trigger_test/testmode0.launch
    pkg/trunk/drivers/cam/camera_trigger_test/testmode1.launch
    pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
    pkg/trunk/drivers/generic/driver_base/include/driver_base/config_manager.h
    pkg/trunk/drivers/generic/driver_base/include/driver_base/driver_base.h
    pkg/trunk/drivers/imu/imu_node/imu_node.cc
    pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.h
    pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.h
    pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/update_functions.h
    pkg/trunk/hardware_test/diagnostic_updater/test/diagnostic_updater_test.cpp
    pkg/trunk/hardware_test/self_test/include/self_test/self_test.h

Added Paths:
-----------
    pkg/trunk/drivers/generic/driver_base/include/driver_base/device.h
    pkg/trunk/drivers/generic/driver_base/include/driver_base/driver.h
    pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/publisher.h

Modified: pkg/trunk/drivers/cam/camera_trigger_test/src/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/src/CMakeLists.txt	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/cam/camera_trigger_test/src/CMakeLists.txt	2009-06-02 15:35:05 UTC (rev 16583)
@@ -1 +1,2 @@
 rospack_add_executable(trigger_test trigger_test.cpp)
+rospack_add_executable(timestamp_test timestamp_test.cpp)

Modified: pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp	2009-06-02 15:35:05 UTC (rev 16583)
@@ -61,10 +61,6 @@
 public:
   TriggerTest(ros::Node &node) : node_(node), outfile_(NULL)
   {
-    // Subscribe to image stream.
-
-    node_.subscribe("image", img_msg_, &TriggerTest::image_cb, this, 1);
-    
     // Read operating mode.
     
     node_.param("~mode", mode_, 0);
@@ -127,6 +123,10 @@
     ignore_count_ = 5;
     node_.param("~repetitions", num_repetitions_, 10);
     reps_ = 0;
+    
+    // Subscribe to image stream.
+
+    node_.subscribe("image", img_msg_, &TriggerTest::image_cb, this, 1);
   }
 
   ~TriggerTest()

Modified: pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml	2009-06-02 15:35:05 UTC (rev 16583)
@@ -1,10 +1,10 @@
 <controllers>
 <controller name="cam_controller" type="TriggerControllerNode"> 
   <actuator name="fl_caster_l_wheel_motor" />
-  <!--waveform rep_rate="10" active_low="0" phase="0" duty_cycle=".5" running="1" pulsed="1" /-->
+  <waveform rep_rate="24" active_low="0" phase="0" duty_cycle=".5" running="1" pulsed="1" />
 </controller>    
 <controller name="led_controller" type="TriggerControllerNode">
   <actuator name="bl_caster_l_wheel_motor" />
-  <!--waveform rep_rate="10" active_low="0" phase="0" duty_cycle=".5" running="1" pulsed="1" /-->
+  <waveform rep_rate="10" active_low="1" phase="0" duty_cycle=".5" running="0" pulsed="1" />
 </controller>    
 </controllers>

Modified: pkg/trunk/drivers/cam/camera_trigger_test/testmode0.launch
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/testmode0.launch	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/cam/camera_trigger_test/testmode0.launch	2009-06-02 15:35:05 UTC (rev 16583)
@@ -6,7 +6,7 @@
 
   <group ns="forearm">
     <param name="ext_trigger" type="bool" value="True"/>
-    <param name="if_name" type="str" value="eth1"/>
+    <param name="if_name" type="str" value="eth2"/>
 		<param name="ip_address" type="str" value="169.254.0.1"/>
     <!--param name="serial_number" type="int" value="2"/-->
     <param name="video_mode" type="str" value="640x480x15"/>

Modified: pkg/trunk/drivers/cam/camera_trigger_test/testmode1.launch
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/testmode1.launch	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/cam/camera_trigger_test/testmode1.launch	2009-06-02 15:35:05 UTC (rev 16583)
@@ -6,7 +6,7 @@
 
   <group ns="forearm">
     <param name="ext_trigger" type="bool" value="True"/>
-    <param name="if_name" type="str" value="eth1"/>
+    <param name="if_name" type="str" value="eth2"/>
 		<param name="ip_address" type="str" value="169.254.0.1"/>
     <!--param name="serial_number" type="int" value="2"/-->
     <param name="video_mode" type="str" value="640x480x15"/>

Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp	2009-06-02 15:35:05 UTC (rev 16583)
@@ -44,6 +44,7 @@
 #include <image_msgs/FillImage.h>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/update_functions.h>
+#include <diagnostic_updater/publisher.h>
 #include <self_test/self_test.h>
 #include <robot_mechanism_controllers/SetWaveform.h>
 #include <robot_mechanism_controllers/trigger_controller.h>
@@ -202,7 +203,9 @@
   
   bool open_;
 
-  ros::Publisher cam_pub_;
+  diagnostic_updater::Updater diagnostic_;
+  
+  diagnostic_updater::DiagnosedPublisher<image_msgs::Image> cam_pub_;
   ros::Publisher cam_info_pub_;
   ros::ServiceClient trig_service_;
   
@@ -219,9 +222,8 @@
   controller::trigger_configuration trig_req_;
   robot_mechanism_controllers::SetWaveform::Response trig_rsp_;
 
-  diagnostic_updater::Updater diagnostic_;
-  diagnostic_updater::FrequencyStatus freq_diag_;
-  diagnostic_updater::TimeStampStatus timestamp_diag_;
+//  diagnostic_updater::FrequencyStatus freq_diag_;
+//  diagnostic_updater::TimeStampStatus timestamp_diag_;
 
   SelfTest<ForearmNode> self_test_;
   bool was_running_pretest_;
@@ -240,7 +242,10 @@
   ForearmNode(ros::NodeHandle &nh)
     : node_handle_(nh), camera_(NULL), started_video_(false),
       diagnostic_(ros::NodeHandle()), 
-      freq_diag_(desired_freq_, desired_freq_, 0.05),
+      cam_pub_(node_handle_.advertise<image_msgs::Image>("~image_raw", 1), 
+          diagnostic_,
+          diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05), 
+          diagnostic_updater::TimeStampStatusParam()),
       self_test_(this),
       useFrame_(boost::bind(&ForearmNode::publishImage, this, _1, _2, _3, _4))
   {
@@ -252,8 +257,8 @@
     diagnostic_thread_ = NULL;
 
     // Setup diagnostics
-    diagnostic_.add(timestamp_diag_);
-    diagnostic_.add("Frequency Status", this, &ForearmNode::freqStatus );
+    //diagnostic_.add(timestamp_diag_);
+    //diagnostic_.add("Frequency Status", this, &ForearmNode::freqStatus );
     diagnostic_.add("Link Status", this, &ForearmNode::linkStatus );
 
     // Setup self test
@@ -280,7 +285,7 @@
 
     if (node_handle_.ok())
     {
-      freq_diag_.clear(); // Avoids having an error until the window fills up.
+      cam_pub_.clear_window(); // Avoids having an error until the window fills up.
       diagnostic_thread_ = new boost::thread( boost::bind(&ForearmNode::diagnosticsLoop, this) );
     }
   }
@@ -294,7 +299,7 @@
     else {
       ROS_FATAL("IP address not specified");
       exit_status_ = 1;
-      node_handle_.shutdown();
+      //node_handle_.shutdown();
       return;
     }
 
@@ -527,22 +532,6 @@
 
         trig_service_ = node_handle_.serviceClient<robot_mechanism_controllers::SetWaveform>(trig_controller_cmd_);
         // Retry a few times in case the service is just coming up.
-        bool ret_val = false;
-        for (int retry_count = 0; retry_count < 5 && ! ret_val; retry_count++)
-        {
-          ret_val = trig_service_.call(trig_req_, trig_rsp_);
-          if (ret_val)
-            break;
-          sleep(1);
-        }
-
-        if (!ret_val)
-        {
-          ROS_FATAL("Unable to set trigger controller.");
-          exit_status_ = 1;
-          node_handle_.shutdown();
-          return;
-        }
       }
       else
       {
@@ -554,7 +543,7 @@
     if ( pr2ImagerModeSelect( camera_, video_mode_ ) != 0) {
       ROS_FATAL("Mode select error");
       exit_status_ = 1;
-      node_handle_.shutdown();
+      //node_handle_.shutdown();
       return;
     }
 
@@ -580,7 +569,7 @@
     
     // Receive frames through callback
     // TODO: start this in separate thread?
-    cam_pub_ = node_handle_.advertise<image_msgs::Image>("~image_raw", 1);
+    //cam_pub_ = node_handle_.advertise<image_msgs::Image>("~image_raw", 1);
     if (calibrated_)
       cam_info_pub_ = node_handle_.advertise<image_msgs::CamInfo>("~cam_info", 1);
     
@@ -623,18 +612,6 @@
       }
     }
     
-    // Stop Triggering
-    if (!trig_controller_cmd_.empty())
-    {   
-      ROS_DEBUG("Stopping triggering.");
-      trig_req_.running = 0;
-      ros::Node shutdown_node("forearm_node", ros::Node::ANONYMOUS_NAME | ros::Node::DONT_ADD_ROSOUT_APPENDER); // Need this because the node has been shutdown already
-      if (!trig_service_.call(trig_req_, trig_rsp_))
-      { // This probably means that the trigger controller was turned off,
-        // so we don't really care.
-        ROS_DEBUG("Was not able to stop triggering.");
-      }
-    }
   }
   
 private:
@@ -648,12 +625,38 @@
       exit_status_ = 1;
       return;
     }
+    if (!trig_controller_cmd_.empty())
+    {
+      trig_req_.running = 1;
+      if (!trig_service_.call(trig_req_, trig_rsp_))
+      {
+        ROS_ERROR("Unable to set trigger controller.");
+        exit_status_ = 1;
+        //node_handle_.shutdown();
+        goto stop_video;
+      }
+    }
     frameTimeFilter_.reset_filter();
     ROS_INFO("Camera running.");
     
     // Receive video
     pr2VidReceive(camera_->ifName, port, height_, width_, &ForearmNode::frameHandler, this);
     
+    // Stop Triggering
+    if (!trig_controller_cmd_.empty())
+    {   
+      ROS_DEBUG("Stopping triggering.");
+      trig_req_.running = 0;
+      /// @todo need to turn on a node in the case where the node is
+      //already down.
+      //ros::Node shutdown_node("forearm_node", ros::Node::ANONYMOUS_NAME | ros::Node::DONT_ADD_ROSOUT_APPENDER); // Need this because the node has been shutdown already
+      if (!trig_service_.call(trig_req_, trig_rsp_))
+      { // This probably means that the trigger controller was turned off,
+        // so we don't really care.
+        ROS_DEBUG("Was not able to stop triggering.");
+      }
+    }
+stop_video:
     // Stop video
     if (started_video_) // Exited unexpectedly.
     {
@@ -670,14 +673,11 @@
     ROS_DEBUG("Image thread exiting.");
   }
 
-  void freqStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
+/*  void freqStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
     freq_diag_(status);
 
-    status.addv("Free-running frequency", imager_freq_);
-    status.adds("External trigger controller", trig_controller_);
-    status.adds("Trigger mode", ext_trigger_ ? "external" : "internal");
-  }
+  }*/
 
   void linkStatus(diagnostic_updater::DiagnosticStatusWrapper& stat)
   {
@@ -685,10 +685,14 @@
     {
       stat.summary(2, "Next frame is past due.");
     }
-    else
+    else if (started_video_)
     {
       stat.summary(0, "Frames are streaming.");
     }
+    else
+    {
+      stat.summary(1, "Frames are not streaming.");
+    }
     
     stat.addv("Missing image line frames", missed_line_count_);
     stat.addv("Missing EOF frames", missed_eof_count_);
@@ -701,6 +705,9 @@
     stat.adds("Image mode", mode_name_);
     stat.addsf("Latest frame time", "%f", last_image_time_);
     stat.adds("Latest frame #", last_frame_number_);
+    stat.addv("Free-running frequency", imager_freq_);
+    stat.adds("External trigger controller", trig_controller_);
+    stat.adds("Trigger mode", ext_trigger_ ? "external" : "internal");
   }
 
   int publishImage(size_t width, size_t height, uint8_t *frameData, ros::Time t)
@@ -708,13 +715,13 @@
     fillImage(image_, "image", height, width, 1, "bayer_bggr", "uint8", frameData);
     
     image_.header.stamp = t;
-    timestamp_diag_.tick(t);
+    //timestamp_diag_.tick(t);
     cam_pub_.publish(image_);
     if (calibrated_) {
       cam_info_.header.stamp = t;
       cam_info_pub_.publish(cam_info_);
     }
-    freq_diag_.tick();
+    //freq_diag_.tick();
 
     return 0;
   }
@@ -950,7 +957,7 @@
     stop();
   }
 
-  void interruptionTest(robot_msgs::DiagnosticStatus& status)
+  void interruptionTest(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
     status.name = "Interruption Test";
 
@@ -966,7 +973,7 @@
     }
   }
 
-  void connectTest(robot_msgs::DiagnosticStatus& status)
+  void connectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
     status.name = "Connection Test";
 
@@ -987,7 +994,7 @@
     self_test_.setID(str(boost::format("FCAM%i")%serial_number_));
   }
 
-  void startTest(robot_msgs::DiagnosticStatus& status)
+  void startTest(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
     status.name = "Start Test";
 
@@ -997,35 +1004,17 @@
     status.message = "Started successfully.";
   }
 
-  void streamingTest(robot_msgs::DiagnosticStatus& status)
+  void streamingTest(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
-    freq_diag_.clear();
+    cam_pub_.clear_window();
     sleep(5);
 
-    diagnostic_updater::DiagnosticStatusWrapper dsfreq;
-    freq_diag_(dsfreq);
+    cam_pub_.run(status);
 
-    diagnostic_updater::DiagnosticStatusWrapper dstime;
-    timestamp_diag_(dstime);
-
-    if (dsfreq.level > 0)
-    { 
-      status = dsfreq;
-    }
-    else if (dstime.level > 0 && dstime.level > dsfreq.level)
-    { 
-      status = dstime;
-    }
-    else
-    {
-      status.level = 0;
-      status.message = "Started successfully.";
-    }
-    
     status.name = "Streaming Test";
   }
 
-  void disconnectTest(robot_msgs::DiagnosticStatus& status)
+  void disconnectTest(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
     status.name = "Disconnect Test";
 
@@ -1076,9 +1065,9 @@
   {
     public:
       VideoModeTestFrameHandler(diagnostic_updater::DiagnosticStatusWrapper &status) : status_(status)
-    {}
+      {}
 
-      int operator()(size_t width, size_t height, uint8_t *data, ros::Time stamp)
+      int run(size_t width, size_t height, uint8_t *data, ros::Time stamp)
       {
         status_.adds("Got a frame", "Pass");
 
@@ -1123,7 +1112,7 @@
 
     mode_name_ = mode;
     VideoModeTestFrameHandler callback(status);
-    useFrame_ = boost::bind<int>(boost::ref(callback), _1, _2, _3, _4);
+    useFrame_ = boost::bind(&VideoModeTestFrameHandler::run, boost::ref(callback), _1, _2, _3, _4);
 
     status.name = mode + " Pattern Test";
     status.summary(0, "Passed"); // If nobody else fills this, then the test passed.
@@ -1155,7 +1144,7 @@
     mode_name_ = oldmode;
   }
 
-  void resumeTest(robot_msgs::DiagnosticStatus& status)
+  void resumeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
   {
     status.name = "Resume Test";
 

Modified: pkg/trunk/drivers/generic/driver_base/include/driver_base/config_manager.h
===================================================================
--- pkg/trunk/drivers/generic/driver_base/include/driver_base/config_manager.h	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/generic/driver_base/include/driver_base/config_manager.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -1,3 +1,5 @@
+#error Do not use this file. It is scheduled for deletion.
+
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
@@ -32,6 +34,8 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
+// Author: Blaise Gassend
+
 #ifndef __CONFIG_MANAGER_H__
 #define __CONFIG_MANAGER_H__
 

Added: pkg/trunk/drivers/generic/driver_base/include/driver_base/device.h
===================================================================
--- pkg/trunk/drivers/generic/driver_base/include/driver_base/device.h	                        (rev 0)
+++ pkg/trunk/drivers/generic/driver_base/include/driver_base/device.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -0,0 +1,58 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Author: Blaise Gassend
+#ifndef __DRIVER_BASE__DEVICE_H__
+#define __DRIVER_BASE__DEVICE_H__
+
+namespace driver_base
+{
+
+class Device
+{
+public:  
+  void open() = 0;
+  void close() = 0;
+  void start() = 0;
+  void stop() = 0;
+  void poll() = 0;
+  void started() = 0;
+};
+
+/// @fixme derived classes for nodes that only poll or only stream.
+
+};
+
+#endif
+

Added: pkg/trunk/drivers/generic/driver_base/include/driver_base/driver.h
===================================================================
--- pkg/trunk/drivers/generic/driver_base/include/driver_base/driver.h	                        (rev 0)
+++ pkg/trunk/drivers/generic/driver_base/include/driver_base/driver.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -0,0 +1,78 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Author: Blaise Gassend
+#ifndef __DRIVER_BASE__DRIVER_H__
+#define __DRIVER_BASE__DRIVER_H__
+
+#include <diagnostic_updater/diagnostic_updater.h>
+#include <self_test/self_test.h>
+
+namespace driver_base
+{
+
+
+class Driver
+{
+public:
+  void Driver()
+  {
+    prepare_diagnostics();
+    prepare_self_tests();
+    read_config();
+  }
+  
+  void spin()
+  {
+    while (node_handle_.ok())
+    {
+      if (parameters_.autostart && !started())
+      {
+        start();
+      }
+
+      /// Will need some locking here or in diagnostic_updater?
+      diagnostic_.update();
+      self_test_.checkTest();
+    }
+  }
+
+private:
+  diagnostic_updater::Updater diagnostic_;
+  self_test::Dispatcher self_test_;
+};
+
+};
+
+#endif

Modified: pkg/trunk/drivers/generic/driver_base/include/driver_base/driver_base.h
===================================================================
--- pkg/trunk/drivers/generic/driver_base/include/driver_base/driver_base.h	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/generic/driver_base/include/driver_base/driver_base.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -1,3 +1,40 @@
+#error Do not use this file. It is scheduled for deletion.
+
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Author: Blaise Gassend
 #ifndef __DRIVER_BASE_H__
 #define __DRIVER_BASE_H__
 

Modified: pkg/trunk/drivers/imu/imu_node/imu_node.cc
===================================================================
--- pkg/trunk/drivers/imu/imu_node/imu_node.cc	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/drivers/imu/imu_node/imu_node.cc	2009-06-02 15:35:05 UTC (rev 16583)
@@ -143,7 +143,8 @@
 
   ImuNode(ros::NodeHandle h) : self_test_(this), diagnostic_(h), 
   node_handle_(h), calibrated_(false), calibrate_request_(false), error_count_(0), 
-  desired_freq_(100), freq_diag_(desired_freq_, desired_freq_, 0.05)
+  desired_freq_(100), 
+  freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05))
   {
     imu_data_pub_ = node_handle_.advertise<robot_msgs::PoseWithRatesStamped>("imu_data", 100);
 

Modified: pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.h
===================================================================
--- pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.h	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -60,6 +60,36 @@
     message = s;
   }
 
+  void mergeSummary(unsigned char lvl, const std::string s)
+  {
+      if (lvl > level)
+      {
+        if (lvl == 0)
+          message = "";
+  
+        level = lvl;
+      }
+  
+      if ((lvl > 0) == (level > 0))
+      {
+        if (!message.empty())
+          message += " ";
+        message += s;
+      }
+  }
+
+  void mergeSummaryf(unsigned char lvl, const char *format, ...)
+  {
+    va_list va;
+    char buff[1000]; // @todo This could be done more elegantly.
+    va_start(va, format);
+    if (vsnprintf(buff, 1000, format, va) >= 1000)
+      ROS_DEBUG("Really long string in DiagnosticStatusWrapper::addsf, it was truncated.");
+    std::string value = std::string(buff);
+    mergeSummary(lvl, value);
+    va_end(va);
+  }
+
   void summaryf(unsigned char lvl, const char *format, ...)
   {
     va_list va;

Modified: pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.h
===================================================================
--- pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.h	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -97,7 +97,7 @@
   {
     return name_;
   }
-  virtual void operator() (diagnostic_updater::DiagnosticStatusWrapper &stat) = 0;
+  virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat) = 0;
   virtual ~DiagnosticTask()
   {}
 
@@ -113,7 +113,7 @@
     DiagnosticTask(name), fn_(fn)
   {}
   
-  virtual void operator ()(DiagnosticStatusWrapper &stat)
+  virtual void run(DiagnosticStatusWrapper &stat)
   {
     fn_(stat);
   }
@@ -127,6 +127,63 @@
 typedef GenericFunctionDiagnosticTask<DiagnosticStatusWrapper> FunctionDiagnosticTask;
 
 /**
+ * A ComposableDiagnosticTask is a DiagnosticTask that is designed to be
+ * composed with other ComposableDiagnosticTask into a single status
+ * message using a CombinationDiagnosticTask.
+ */
+
+class ComposableDiagnosticTask : public DiagnosticTask
+{
+public:
+  ComposableDiagnosticTask(const std::string name) : DiagnosticTask(name)
+  {}
+  
+  void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
+  {
+    split_run(stat, stat);
+  }
+
+  virtual void split_run(diagnostic_updater::DiagnosticStatusWrapper &summary, 
+      diagnostic_updater::DiagnosticStatusWrapper &details) = 0;
+};
+
+/**
+ * The CombinationDiagnosticTask allows multiple ComposableDiagnosticTask instances
+ * to be combined into a single DiagnosticStatus. The output of the
+ * combination has the max of the status levels, and a concatenation of the
+ * non-zero-level messages.
+ */
+
+class CombinationDiagnosticTask : public DiagnosticTask
+{
+public:
+  CombinationDiagnosticTask(const std::string name) : DiagnosticTask(name)
+  {}
+
+  virtual void run(DiagnosticStatusWrapper &stat)
+  {
+    DiagnosticStatusWrapper summary;
+    stat.summary(0, "");
+    
+    for (std::vector<ComposableDiagnosticTask *>::iterator i = tasks_.begin();
+        i != tasks_.end(); i++)
+    {
+      (*i)->split_run(summary, stat);
+    
+      stat.mergeSummary(summary.level, summary.message);
+    }
+  }
+  
+  void addTask(ComposableDiagnosticTask *t)
+  {
+    tasks_.push_back(t);
+  }
+
+private:
+  std::vector<ComposableDiagnosticTask *> tasks_;
+};
+
+/**
  *
  * The @b DiagnosticTaskVector class is abstract base class that manages a
  * collection of diagnostic updaters. It contains the common functionality
@@ -144,7 +201,7 @@
       name_(name), fn_(f)
     {}
 
-    void operator() (diagnostic_updater::DiagnosticStatusWrapper &stat) const
+    void run(diagnostic_updater::DiagnosticStatusWrapper &stat) const
     {
       stat.name = name_;
       fn_(stat);
@@ -172,7 +229,7 @@
    * \brief Add a DiagnosticTask to the DiagnosticTaskVector
    *
    * \param task The DiagnosticTask to be added. It must remain valid at
-   * least until the last time its operator() is called. It need not be
+   * least until the last time its diagnostic method is called. It need not be
    * valid at the time the DiagnosticTaskVector is destructed.
    */
 
@@ -184,7 +241,7 @@
 
   void add(DiagnosticTask &task)
   {
-    TaskFunction f = boost::bind<void>(boost::ref(task), _1);
+    TaskFunction f = boost::bind(&DiagnosticTask::run, &task, _1);
     add(task.getName(), f);
   }
   
@@ -200,6 +257,7 @@
   {}
   std::vector<DiagnosticTaskInternal> tasks_;
   
+protected:
   void addInternal(DiagnosticTaskInternal &task)
   {
     boost::mutex::scoped_lock lock(lock_);
@@ -245,7 +303,7 @@
         status.level = 2;
         status.message = "No message was set";
 
-        (*iter)(status);
+        iter->run(status);
 
         status_vec.push_back(status);
       }

Added: pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/publisher.h
===================================================================
--- pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/publisher.h	                        (rev 0)
+++ pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/publisher.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -0,0 +1,128 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Author: Blaise Gassend
+#ifndef __DIAGNOSTIC_UPDATER__DRIVER_H__
+#define __DIAGNOSTIC_UPDATER__DRIVER_H__
+
+#include <ros/publisher.h>
+
+namespace diagnostic_updater
+{
+                                   
+class HeaderlessDiagnosedPublisher : public CombinationDiagnosticTask
+{
+public:
+  HeaderlessDiagnosedPublisher(const ros::Publisher &pub,
+      diagnostic_updater::Updater &diag,
+      const diagnostic_updater::FrequencyStatusParam &freq) :
+    CombinationDiagnosticTask(const_cast<ros::Publisher &>(pub).getTopic() + " topic status"), /// @todo Get rid of this const_cast as soon as the publisher API is fixed.
+    publisher_(pub),
+    freq_(freq)
+  {
+    addTask(&freq_);
+    diag.add(*this);
+  }
+
+  virtual ~HeaderlessDiagnosedPublisher()
+  {}
+  
+  virtual void publish(const ros::MessageConstPtr& message)
+  {
+    freq_.tick();
+    publisher_.publish(message);
+  }
+ 
+  virtual void publish(const ros::Message& message)
+  {
+    freq_.tick();
+    publisher_.publish(message);
+  }
+
+  /*void set(Publisher &pub)
+  {
+    publisher_ = pub;
+  }*/
+
+  virtual void clear_window()
+  {
+    freq_.clear();
+  }
+  
+  ros::Publisher publisher() const
+  {
+    return publisher_;
+  }
+
+private:
+  const ros::Publisher publisher_;
+  diagnostic_updater::FrequencyStatus freq_;
+};
+
+template<class T>
+class DiagnosedPublisher : public HeaderlessDiagnosedPublisher
+{
+public:
+  DiagnosedPublisher(const ros::Publisher &pub,
+      diagnostic_updater::Updater &diag, 
+      const diagnostic_updater::FrequencyStatusParam &freq, 
+      const diagnostic_updater::TimeStampStatusParam &stamp) : 
+    HeaderlessDiagnosedPublisher(pub, diag, freq),
+    stamp_(stamp)
+  {
+    addTask(&stamp_);
+  }
+  
+  virtual ~DiagnosedPublisher()
+  {}
+  
+  virtual void publish(const boost::shared_ptr<T>& message)
+  {
+    stamp_.tick(message->header.stamp);
+    HeaderlessDiagnosedPublisher::publish(message);
+  }
+ 
+  virtual void publish(const T& message)
+  {
+    stamp_.tick(message.header.stamp);
+    HeaderlessDiagnosedPublisher::publish(message);
+  }
+
+private:
+  TimeStampStatus stamp_;
+};
+
+};
+
+#endif


Property changes on: pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/publisher.h
___________________________________________________________________
Added: svn:mergeinfo
   + 

Modified: pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/update_functions.h
===================================================================
--- pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/update_functions.h	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/update_functions.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -1,32 +1,86 @@
-#ifndef __FREQUENCY_STATUS_DIAGNOSTIC_H__
-#define __FREQUENCY_STATUS_DIAGNOSTIC_H__
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
 
+/// Author: Blaise Gassend
+
+#ifndef __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__
+#define __DIAGNOSTIC_STATUS__UPDATE_FUNCTIONS_H__
+
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <math.h>
 
 namespace diagnostic_updater
 {
-  
-class FrequencyStatus : public DiagnosticTask
+
+struct FrequencyStatusParam
 {
+  FrequencyStatusParam(double *min_freq, double *max_freq, double tolerance = 0.1, int window_size = 5) :
+    min_freq_(min_freq), max_freq_(max_freq), tolerance_(tolerance), window_size_(window_size)
+  {}
+
+  double *min_freq_;
+  double *max_freq_;
+  double tolerance_;
+  int window_size_;
+};
+
+class FrequencyStatus : public ComposableDiagnosticTask
+{
+private:                                         
+  const FrequencyStatusParam params_;
+
+  int count_;
+  std::vector <ros::Time> times_;
+  std::vector <int> seq_nums_;
+  int hist_indx_;
+  boost::mutex lock_;
+
 public:
-  FrequencyStatus(double &min_freq, double& max_freq, double tolerance = 0.1, int window_size = 5) : 
-    DiagnosticTask("Frequency Status"), min_freq_(min_freq), max_freq_(max_freq),
-    times_(window_size), seq_nums_(window_size)
+  FrequencyStatus(const FrequencyStatusParam &params) : 
+    ComposableDiagnosticTask("Frequency Status"), params_(params), 
+    times_(params_.window_size_), seq_nums_(params_.window_size_)
   {
-    tolerance_ = tolerance;
-    window_size_ = window_size;
-
     clear();
   }
-
+  
   void clear()
   {
     boost::mutex::scoped_lock lock(lock_);
     ros::Time curtime = ros::Time::now();
     count_ = 0;
 
-    for (int i = 0; i < window_size_; i++)
+    for (int i = 0; i < params_.window_size_; i++)
     {
       times_[i] = curtime;
       seq_nums_[i] = count_;
@@ -42,7 +96,8 @@
     count_++;
   }
 
-  void operator()(diagnostic_updater::DiagnosticStatusWrapper &stat)
+  void split_run(diagnostic_updater::DiagnosticStatusWrapper &summary, 
+      diagnostic_updater::DiagnosticStatusWrapper &details)
   {
     boost::mutex::scoped_lock lock(lock_);
     ros::Time curtime = ros::Time::now();
@@ -52,120 +107,162 @@
     double freq = events / window;
     seq_nums_[hist_indx_] = curseq;
     times_[hist_indx_] = curtime;
-    hist_indx_ = (hist_indx_ + 1) % window_size_;
+    hist_indx_ = (hist_indx_ + 1) % params_.window_size_;
 
     if (events == 0)
     {
-      stat.summary(2, "No events recorded.");
+      summary.summary(2, "No events recorded.");
     }
-    else if (freq < min_freq_ * (1 - tolerance_))
+    else if (freq < *params_.min_freq_ * (1 - params_.tolerance_))
     {
-      stat.summary(2, "Frequency too low.");
+      summary.summary(2, "Frequency too low.");
     }
-    else if (freq > max_freq_ * (1 + tolerance_))
+    else if (freq > *params_.max_freq_ * (1 + params_.tolerance_))
     {
-      stat.summary(2, "Frequency too high.");
+      summary.summary(2, "Frequency too high.");
     }
     else
     {
-      stat.summary(0, "Desired frequency met");
+      summary.summary(0, "Desired frequency met");
     }
 
-    stat.addv("Events in window", events);
-    stat.addv("Events since startup", count_);
-    stat.addv("Duration of window (s)", window);
-    stat.addv("Actual frequency (Hz)", freq);
-    if (min_freq_ == max_freq_)
-      stat.addv("Target frequency (Hz)", min_freq_);
-    if (min_freq_ > 0)
-      stat.addv("Minimum acceptable frequency (Hz)", 
-          min_freq_ * (1 - tolerance_));
-    if (finite(max_freq_))
-      stat.addv("Maximum acceptable frequency (Hz)", 
-          max_freq_ * (1 + tolerance_));
+    details.addv("Events in window", events);
+    details.addv("Events since startup", count_);
+    details.addv("Duration of window (s)", window);
+    details.addv("Actual frequency (Hz)", freq);
+    if (*params_.min_freq_ == *params_.max_freq_)
+      details.addv("Target frequency (Hz)", *params_.min_freq_);
+    if (*params_.min_freq_ > 0)
+      details.addv("Minimum acceptable frequency (Hz)", 
+          *params_.min_freq_ * (1 - params_.tolerance_));
+    if (finite(*params_.max_freq_))
+      details.addv("Maximum acceptable frequency (Hz)", 
+          *params_.max_freq_ * (1 + params_.tolerance_));
   }
+};
 
-private:
-  double &min_freq_;
-  double &max_freq_;
+struct TimeStampStatusParam
+{
+  TimeStampStatusParam(const double min_acceptable = -1, const double max_acceptable = 5) :
+    max_acceptable_(max_acceptable), min_acceptable_(min_acceptable)
+  {}
+  
+  double max_acceptable_;
+  double min_acceptable_;
 
-  int count_;
-  double tolerance_;
-  int window_size_;
-  std::vector <ros::Time> times_;
-  std::vector <int> seq_nums_;
-  int hist_indx_;
-  boost::mutex lock_;
 };
+  
+static TimeStampStatusParam DefaultTimeStampStatusParam = TimeStampStatusParam();
 
-class TimeStampStatus : public DiagnosticTask
+class TimeStampStatus : public ComposableDiagnosticTask
 {
+private:
+  void init()
+  {
+    early_count_ = 0;
+    late_count_ = 0;
+    zero_count_ = 0;
+    zero_seen_ = false;
+    max_delta_ = 0;
+    min_delta_ = 0;
+    deltas_valid_ = false;
+  }
+
 public:
-  TimeStampStatus(double min_acceptable = -1, double max_acceptable = 5) : 
-    DiagnosticTask("Timestamp Status"), 
-    early_count_(0), late_count_(0), max_delta_(0), 
-    min_delta_(0), deltas_valid_(false), max_acceptable_(max_acceptable),
-    min_acceptable_(min_acceptable)
-  {}
+  TimeStampStatus(const TimeStampStatusParam &params) : 
+    ComposableDiagnosticTask("Timestamp Status"), 
+    params_(params)
+  {
+    init();
+  }
+  
+  TimeStampStatus() : 
+    ComposableDiagnosticTask("Timestamp Status") 
+  {
+    init();
+  }
 
   void tick(double stamp)
   {
-    
     boost::mutex::scoped_lock lock(lock_);
-    double delta = ros::Time::now().toSec() - stamp;
 
-    if (!deltas_valid_ || delta > max_delta_)
-      max_delta_ = delta;
+    if (stamp == 0)
+    {
+      zero_seen_ = true;
+    }
+    else
+    {
+      double delta = ros::Time::now().toSec() - stamp;
 
-    if (!deltas_valid_ || delta < min_delta_)
-      min_delta_ = delta;
+      if (!deltas_valid_ || delta > max_delta_)
+        max_delta_ = delta;
 
-    deltas_valid_ = true;
+      if (!deltas_valid_ || delta < min_delta_)
+        min_delta_ = delta;
+
+      deltas_valid_ = true;
+    }
   }
 
-  void tick(ros::Time t)
+  void tick(const ros::Time t)
   {
     tick(t.toSec());
   }
 
-  void operator()(diagnostic_updater::DiagnosticStatusWrapper &stat)
+  void split_run(diagnostic_updater::DiagnosticStatusWrapper &summary, 
+      diagnostic_updater::DiagnosticStatusWrapper &details)
   {
     boost::mutex::scoped_lock lock(lock_);
+
+    summary.summary(0, "Timestamps are reasonable.");
     if (!deltas_valid_)
-      stat.summary(1, "No data since last update.");
-    else if (min_delta_ < min_acceptable_)
     {
-      stat.summary(2, "Timestamps too far in future seen.");
-      early_count_++;
+      summary.summary(1, "No data since last update.");
     }
-    else if (max_delta_ > max_acceptable_)
+    else 
     {
-      stat.summary(2, "Timestamps too far in past seen.");
-      late_count_++;
+      if (min_delta_ < params_.min_acceptable_)
+      {
+        summary.summary(2, "Timestamps too far in future seen.");
+        early_count_++;
+      }
+      
+      if (max_delta_ > params_.max_acceptable_)
+      {
+        summary.summary(2, "Timestamps too far in past seen.");
+        late_count_++;
+      }
+
+      if (zero_seen_)
+      {
+        summary.summary(2, "Zero timestamp seen.");
+        zero_count_++;
+      }
     }
-    else
-      stat.summary(0, "Timestamps are reasonable.");
 
-    stat.addv("Earliest timestamp delay:", min_delta_);
-    stat.addv("Latest timestamp delay:", max_delta_);
-    stat.addv("Earliest acceptable timestamp delay:", min_acceptable_);
-    stat.addv("Latest acceptable timestamp delay:", max_acceptable_);
-    stat.adds("Late diagnostic update count:", late_count_); 
-    stat.adds("Early diagnostic update count:", early_count_); 
+    details.addv("Earliest timestamp delay:", min_delta_);
+    details.addv("Latest timestamp delay:", max_delta_);
+    details.addv("Earliest acceptable timestamp delay:", params_.min_acceptable_);
+    details.addv("Latest acceptable timestamp delay:", params_.max_acceptable_);
+    details.adds("Late diagnostic update count:", late_count_); 
+    details.adds("Early diagnostic update count:", early_count_); 
+    details.adds("Zero seen diagnostic update count:", zero_count_); 
 
     deltas_valid_ = false;
     min_delta_ = 0;
     max_delta_ = 0;
+    zero_seen_ = false;
   }
+
 private:
-
+  TimeStampStatusParam params_;
   int early_count_;
   int late_count_;
+  int zero_count_;
+  bool zero_seen_;
   double max_delta_;
   double min_delta_;
   bool deltas_valid_;
-  double max_acceptable_;
-  double min_acceptable_;
   boost::mutex lock_;
 };
 

Modified: pkg/trunk/hardware_test/diagnostic_updater/test/diagnostic_updater_test.cpp
===================================================================
--- pkg/trunk/hardware_test/diagnostic_updater/test/diagnostic_updater_test.cpp	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/hardware_test/diagnostic_updater/test/diagnostic_updater_test.cpp	2009-06-02 15:35:05 UTC (rev 16583)
@@ -59,8 +59,7 @@
     classFunction() : DiagnosticTask("classFunction")
     {}
 
-  private:
-    void operator() (DiagnosticStatusWrapper &s) 
+    void run(DiagnosticStatusWrapper &s) 
     {
       s.summary(0, "Test is running");
       s.addv("Value", 5);
@@ -114,23 +113,23 @@
   double minFreq = 10;
   double maxFreq = 20;
   
-  FrequencyStatus fs(minFreq, maxFreq, 0.5, 2);
+  FrequencyStatus fs(FrequencyStatusParam(&minFreq, &maxFreq, 0.5, 2));
   
   DiagnosticStatusWrapper stat[5];
   fs.tick();
   usleep(20000);
-  fs(stat[0]); // Should be too fast, 20 ms for 1 tick, lower limit should be 33ms.
+  fs.run(stat[0]); // Should be too fast, 20 ms for 1 tick, lower limit should be 33ms.
   usleep(50000);
   fs.tick();
-  fs(stat[1]); // Should be good, 70 ms for 2 ticks, lower limit should be 66 ms.
+  fs.run(stat[1]); // Should be good, 70 ms for 2 ticks, lower limit should be 66 ms.
   usleep(300000);
   fs.tick();
-  fs(stat[2]); // Should be good, 350 ms for 2 ticks, upper limit should be 400 ms.
+  fs.run(stat[2]); // Should be good, 350 ms for 2 ticks, upper limit should be 400 ms.
   usleep(150000);
   fs.tick();
-  fs(stat[3]); // Should be too slow, 450 ms for 2 ticks, upper limit should be 400 ms.
+  fs.run(stat[3]); // Should be too slow, 450 ms for 2 ticks, upper limit should be 400 ms.
   fs.clear();
-  fs(stat[4]); // Should be good, just cleared it.
+  fs.run(stat[4]); // Should be good, just cleared it.
 
   EXPECT_EQ(2, stat[0].level) << "max frequency exceeded but not reported";
   EXPECT_EQ(0, stat[1].level) << "within max frequency but reported error";
@@ -143,18 +142,18 @@
 
 TEST(DiagnosticUpdater, testTimeStampStatus)
 {
-  TimeStampStatus ts;
+  TimeStampStatus ts(DefaultTimeStampStatusParam);
 
   DiagnosticStatusWrapper stat[5];
-  ts(stat[0]);
+  ts.run(stat[0]);
   ts.tick(ros::Time::now().toSec() + 2);
-  ts(stat[1]);
+  ts.run(stat[1]);
   ts.tick(ros::Time::now());
-  ts(stat[2]);
+  ts.run(stat[2]);
   ts.tick(ros::Time::now().toSec() - 4);
-  ts(stat[3]);
+  ts.run(stat[3]);
   ts.tick(ros::Time::now().toSec() - 6);
-  ts(stat[4]);
+  ts.run(stat[4]);
  
   EXPECT_EQ(1, stat[0].level) << "no data should return a warning";
   EXPECT_EQ(2, stat[1].level) << "too far future not reported";

Modified: pkg/trunk/hardware_test/self_test/include/self_test/self_test.h
===================================================================
--- pkg/trunk/hardware_test/self_test/include/self_test/self_test.h	2009-06-02 15:32:13 UTC (rev 16582)
+++ pkg/trunk/hardware_test/self_test/include/self_test/self_test.h	2009-06-02 15:35:05 UTC (rev 16583)
@@ -109,7 +109,7 @@
   void add(const std::string name, void (S::*f)(diagnostic_updater::DiagnosticStatusWrapper&))
   {
     DiagnosticTaskInternal int_task(name, boost::bind(f, owner_, _1));
-    add(int_task);
+    addInternal(int_task);
   }
   
   void checkTest()
@@ -194,7 +194,7 @@
 
         try {
 
-          (*iter)(status);
+          iter->run(status);
 
         } catch (std::exception& e)
         {
@@ -259,7 +259,7 @@
 
   void addTest(void (T::*f)(diagnostic_updater::DiagnosticStatusWrapper&))
   {
-    self_test::Dispatcher<T>::add(f);
+    self_test::Dispatcher<T>::add("", f);
   }
 
   void addTest(void (T::*f)(robot_msgs::DiagnosticStatus&))


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.