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

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 3884
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3884&view=rev
Author:   jleibs
Date:     2008-09-03 01:08:17 +0000 (Wed, 03 Sep 2008)

Log Message:
-----------
Slightly refactoring self test to make usage more straightforward.

Modified Paths:
--------------
    pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
    pkg/trunk/drivers/imu/imu_node/imu_node.cc
    pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
    pkg/trunk/hardware_test/self_test/include/self_test/self_test.h

Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================

--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp	2008-09-03 00:47:02 UTC (rev 3883)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp	2008-09-03 01:08:17 UTC (rev 3884)
@@ -30,8 +30,6 @@
 #include <stdio.h>
 #include <iostream>
 
-#include <pthread.h>
-
 #include "ros/node.h"
 #include "std_msgs/Image.h"
 #include "std_srvs/PolledImage.h"
@@ -64,8 +62,6 @@
     param("~host", axis_host, string("192.168.0.90"));
     printf("axis_cam host set to [%s]\n", axis_host.c_str());
 
-    self_test_.lock();
-
     self_test_.addTest(&Axis_cam_node::checkImage);
     self_test_.addTest(&Axis_cam_node::checkMac);
 
@@ -73,8 +69,6 @@
 
     next_time = ros::Time::now();
     count_ = 0;
-    
-    self_test_.unlock();
   }
 
   virtual ~Axis_cam_node()
@@ -94,15 +88,12 @@
 
   bool take_and_send_image()
   {
-    self_test_.lock();
     uint8_t *jpeg;
     uint32_t jpeg_size;
 
     if (cam->get_jpeg(&jpeg, &jpeg_size))
     {
       log(ros::ERROR, "woah! AxisCam::get_jpeg returned an error");
-      self_test_.unlock();
-      sched_yield();
       return false;
     }
 
@@ -115,10 +106,6 @@
 
     publish("image", image);
 
-    self_test_.unlock();
-
-    sched_yield();
-
     return true;
   }
 
@@ -143,6 +130,7 @@
         param("~host", axis_host, string("192.168.0.90"));
         cam->set_host(axis_host);
       }
+      self_test_.checkTest();
     }
     return true;
   }

Modified: pkg/trunk/drivers/imu/imu_node/imu_node.cc
===================================================================
--- pkg/trunk/drivers/imu/imu_node/imu_node.cc	2008-09-03 00:47:02 UTC (rev 3883)
+++ pkg/trunk/drivers/imu/imu_node/imu_node.cc	2008-09-03 01:08:17 UTC (rev 3884)
@@ -74,14 +74,12 @@
 
 #include "3dmgx2.h"
 
-#include <ros/node.h>
-#include <std_msgs/ImuData.h>
-#include <std_msgs/EulerAngles.h>
+#include "ros/node.h"
 #include "ros/time.h"
-
 #include "self_test/self_test.h"
 
-#include <pthread.h>
+#include "std_msgs/ImuData.h"
+#include "std_msgs/EulerAngles.h"
 
 using namespace std;
 
@@ -148,8 +146,6 @@
   {
     stop();
 
-    self_test_.lock();
-
     try
     {
       imu.open_port(port.c_str());
@@ -170,20 +166,16 @@
 
     } catch (MS_3DMGX2::exception& e) {
       printf("Exception thrown while starting imu.\n %s\n", e.what());
-      self_test_.unlock();
       return -1;
     }
 
     next_time = ros::Time::now();
 
-    self_test_.unlock();
     return(0);
   }
   
   int stop()
   {
-    self_test_.lock();
-
     if(running)
     {
       try
@@ -195,15 +187,11 @@
       running = false;
     }
 
-    self_test_.unlock();
     return(0);
   }
 
   int publish_datum()
   {
-
-    self_test_.lock();
-
     try
     {
       uint64_t time;
@@ -247,14 +235,12 @@
 
       default:
         printf("Unhandled message type!\n");
-        self_test_.unlock();
         return -1;
 
       }
         
     } catch (MS_3DMGX2::exception& e) {
       printf("Exception thrown while trying to get the reading.\n%s\n", e.what());
-      self_test_.unlock();
       return -1;
     }
 
@@ -266,8 +252,6 @@
       next_time = next_time + ros::Duration(1,0);
     }
 
-    self_test_.unlock();
-    sched_yield();
     return(0);
   }
 
@@ -281,9 +265,11 @@
         while(ok()) {
           if(publish_datum() < 0)
             break;
+          self_test_.checkTest();
         }
       } else {
         usleep(1000000);
+        self_test_.checkTest();
       }
     }
 

Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc	2008-09-03 00:47:02 UTC (rev 3883)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc	2008-09-03 01:08:17 UTC (rev 3884)
@@ -99,7 +99,6 @@
 #include <iostream>
 #include <sstream>
 #include <iomanip>
-#include <pthread.h>
 
 #include "ros/node.h"
 #include "ros/time.h"
@@ -188,7 +187,6 @@
   {
     stop();
 
-    self_test_.lock();
     try
     {
       urg.open(port.c_str());
@@ -205,7 +203,6 @@
 
       if (status != 0) {
         printf("Failed to request scans from URG.  Status: %d.\n", status);
-        self_test_.unlock();
         return -1;
       }
 
@@ -213,19 +210,16 @@
 
     } catch (URG::exception& e) {
       printf("Exception thrown while starting urg.\n%s\n", e.what());
-      self_test_.unlock();
       return -1;
     }
 
     next_time = ros::Time::now();
 
-    self_test_.unlock();
     return(0);
   }
 
   int stop()
   {
-    self_test_.lock();
     if(running)
     {
       try
@@ -237,13 +231,11 @@
       running = false;
     }
 
-    self_test_.unlock();
     return 0;
   }
 
   int publish_scan()
   {
-    self_test_.lock();
     try
     {
       int status = urg.service_scan(&scan);
@@ -255,12 +247,10 @@
       }
     } catch (URG::corrupted_data_exception &e) {
       printf("CORRUPTED DATA\n");
-      self_test_.unlock();
       return 0;
     } catch (URG::exception& e) {
       printf("Exception thrown while trying to get scan.\n%s\n", e.what());
       running = false; //If we're here, we are no longer running
-      self_test_.unlock();
       return -1;
     }
     
@@ -292,8 +282,6 @@
       next_time = next_time + ros::Duration(1,0);
     }
 
-    self_test_.unlock();
-    sched_yield();
     return(0);
   }
 
@@ -308,9 +296,11 @@
         while(ok()) {
           if(publish_scan() < 0)
             break;
+          self_test_.checkTest();
         }
       } else {
         usleep(1000000);
+        self_test_.checkTest();
       }
     }
 

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	2008-09-03 00:47:02 UTC (rev 3883)
+++ pkg/trunk/hardware_test/self_test/include/self_test/self_test.h	2008-09-03 01:08:17 UTC (rev 3884)
@@ -40,18 +40,21 @@
 #include <string>
 
 #include "ros/node.h"
-#include "rosthread/mutex.h"
+#include "rosthread/condition.h"
 
 #include "robot_msgs/DiagnosticStatus.h"
 #include "robot_srvs/SelfTest.h"
 
+
+
 template <class T>
 class SelfTest
 {
 private:
 
   T* node_;
-  ros::thread::mutex testing_mutex;
+  ros::thread::condition testing_condition;
+  ros::thread::condition done_testing_condition;
 
   std::string id_;
 
@@ -60,11 +63,21 @@
   void (T::*pretest_)();
   void (T::*posttest_)();
 
+  int count;
+
+  bool waiting;
+  bool ready;
+  bool done;
+
 public:
 
   SelfTest(T* node) : node_(node), pretest_(NULL), posttest_(NULL)
   {
     node_->advertise_service("~self_test", &SelfTest::doTest, this);
+    count = 0;
+    waiting = false;
+    ready   = false;
+    done    = false;
   }
 
   void addTest(void (T::*f)(robot_msgs::DiagnosticStatus&))
@@ -82,14 +95,24 @@
     posttest_ = f;
   }
 
-  void lock()
+  void checkTest()
   {
-    testing_mutex.lock();
-  }
+    testing_condition.lock();
+    if (waiting)
+    {
+      ready = true;
+      testing_condition.signal();
+      testing_condition.unlock();
 
-  void unlock()
-  {
-    testing_mutex.unlock();
+      done_testing_condition.lock();
+      done = false;
+      while (!done)
+        done_testing_condition.wait();
+      done_testing_condition.unlock();
+    } else {
+      testing_condition.unlock();
+    }
+    sched_yield();
   }
 
   void setID(std::string id)
@@ -100,8 +123,28 @@
   bool doTest(robot_srvs::SelfTest::request &req,
                 robot_srvs::SelfTest::response &res)
   {
-    lock();
+    testing_condition.lock();
 
+    waiting = true;
+    ready   = false;
+
+    while (!ready)
+    {
+      if (!testing_condition.timed_wait(5))
+      {
+        printf("Timed out waiting to run self test.\n");
+        waiting = false;
+        testing_condition.unlock();
+        return false;
+      }
+    }
+
+    testing_condition.unlock();
+
+    bool retval = false;
+
+    printf("Begining test.\n");
+
     if (node_->ok())
     {
 
@@ -139,6 +182,9 @@
         status_vec.push_back(status);
       }
 
+      waiting = false;
+      testing_condition.unlock();
+
       //One of the test calls should use setID
       res.id = id_;
 
@@ -160,13 +206,17 @@
 
       printf("Self test completed\n");
 
-      unlock();
+      retval = true;
 
-      return true;
+    }
 
-    } else {
-      return false;
-    }
+    done_testing_condition.lock();
+    done = true;
+    done_testing_condition.signal();
+    done_testing_condition.unlock();
+
+    return retval;
+
   }
 };
 


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


