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

[<jl...@us...>]

Revision: 6515
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6515&view=rev
Author:   jleibs
Date:     2008-11-11 07:51:46 +0000 (Tue, 11 Nov 2008)

Log Message:
-----------
Refactoring stereo_view to use the new topic_synchronizer

Modified Paths:
--------------
    pkg/trunk/vision/stereo_view/manifest.xml
    pkg/trunk/vision/stereo_view/stereo_view.cpp

Added Paths:
-----------
    pkg/trunk/util/topic_synchronizer/
    pkg/trunk/util/topic_synchronizer/manifest.xml
    pkg/trunk/util/topic_synchronizer/topic_synchronizer.h

Added: pkg/trunk/util/topic_synchronizer/manifest.xml
===================================================================
--- pkg/trunk/util/topic_synchronizer/manifest.xml	                        (rev 0)
+++ pkg/trunk/util/topic_synchronizer/manifest.xml	2008-11-11 07:51:46 UTC (rev 6515)
@@ -0,0 +1,11 @@
+<package>
+  <description>A simple class to synchronize incoming topics by timestamp.</description>
+  <author>Jeremy Leibs</author>
+  <license>BSD</license>
+  <depend package="roscpp" />
+  <depend package="std_msgs" />
+  <depend package="rosthread" />
+  <export>
+    <cpp cflags="-I${prefix}"/>
+  </export>
+</package>

Added: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h	                        (rev 0)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h	2008-11-11 07:51:46 UTC (rev 6515)
@@ -0,0 +1,145 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, 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.
+*********************************************************************/
+
+
+#ifndef TOPIC_SYNCHRONIZER_HH
+#define TOPIC_SYNCHRONIZER_HH
+
+#include "rosthread/condition.h"
+
+template <class N>
+class TopicSynchronizer
+{
+  private:
+
+  N* node_;
+  void (N::*callback_)();
+  
+  ros::thread::condition cond_all_;
+
+  int expected_count_;
+
+  int count_;
+
+  bool done_;
+
+  ros::Time waiting_time_;
+
+  void msg_cb(void* p)
+  {
+    ros::Time* time = (ros::Time*)(p);
+
+    cond_all_.lock();
+
+    // If first to time, wait
+    if (count_ == 0)
+    {
+      wait_for_others(time);
+      return;
+    }
+
+    // If behind, skip
+    if (*time < waiting_time_)
+    {
+      cond_all_.unlock();
+      return;
+    }
+
+    // If at time, increment and signal or wait
+    if (*time == waiting_time_)
+    {
+      count_++;
+      if (count_ == expected_count_)
+      {
+        cond_all_.broadcast();
+      }
+      else
+      {
+        while (!done_ && *time == waiting_time_)
+          cond_all_.wait();
+      }
+      
+      cond_all_.unlock();
+      return;
+    }
+
+    // If ahead, wake up others and then wait
+    if (*time > waiting_time_)
+    {
+      cond_all_.broadcast();
+      wait_for_others(time);
+    }
+  }
+
+  void wait_for_others(ros::Time* time)
+  {
+    count_ = 1;
+    done_ = false;
+
+    waiting_time_ = *time;
+    bool timed_out = false;
+
+    while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
+      if (!cond_all_.timed_wait(1))
+      {
+        printf(" Timed out waiting for other images...\n");
+        timed_out = true;
+      }
+
+    if (*time == waiting_time_ && !timed_out)
+      (*node_.*callback_)();
+
+    if (*time == waiting_time_)
+    {
+      done_ = true;
+      count_ = 0;
+      cond_all_.broadcast();
+    }
+    cond_all_.unlock();
+  }
+
+  public:
+
+  TopicSynchronizer(N* node, void (N::*callback)()) : node_(node), callback_(callback), expected_count_(0), count_(0), done_(false)
+  { }
+
+  template <class M>
+  void subscribe(std::string topic_name, M& msg, int queue_size)
+  {
+    node_->subscribe(topic_name, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
+    expected_count_++;
+  }
+};
+
+#endif

Modified: pkg/trunk/vision/stereo_view/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_view/manifest.xml	2008-11-11 07:09:49 UTC (rev 6514)
+++ pkg/trunk/vision/stereo_view/manifest.xml	2008-11-11 07:51:46 UTC (rev 6515)
@@ -9,5 +9,5 @@
   <depend package="std_srvs"/>
   <depend package="image_msgs"/>
   <depend package="roscpp"/>
-  <depend package="rosthread"/>
+  <depend package="topic_synchronizer"/>
 </package>

Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp	2008-11-11 07:09:49 UTC (rev 6514)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp	2008-11-11 07:51:46 UTC (rev 6515)
@@ -1,3 +1,37 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, 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.
+*********************************************************************/
+
 #include <vector>
 
 #include "opencv/cxcore.h"
@@ -9,7 +43,7 @@
 #include "image_msgs/Image.h"
 #include "image_msgs/CvBridge.h"
 
-#include "rosthread/condition.h"
+#include "topic_synchronizer.h"
 
 using namespace std;
 
@@ -29,104 +63,24 @@
   std::string rtopic;
   std::string dtopic;
 
-  ros::Time image_time;
+  TopicSynchronizer<StereoView> sync;
 
-  ros::thread::condition cond_all;
-
-  int count;
-  bool done;
-
-  StereoView() : ros::node("cv_view"), count(0), done(false)
+  StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all)
   { 
     ltopic = map_name("dcam") + std::string("/left/image_rect");
     rtopic = map_name("dcam") + std::string("/right/image_rect");
     dtopic = map_name("dcam") + std::string("/disparity");
 
     cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
-    subscribe(ltopic, limage, &StereoView::image_cb, &limage, 1);
+    sync.subscribe(ltopic, limage, 1);
 
     cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
-    subscribe(rtopic, rimage, &StereoView::image_cb, &rimage, 1);
+    sync.subscribe(rtopic, rimage, 1);
 
     cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
-    subscribe(dtopic, dimage, &StereoView::image_cb, &dimage, 1);
+    sync.subscribe(dtopic, dimage, 1);
   }
 
-  void image_cb(void* p)
-  {
-    image_msgs::Image* img = (image_msgs::Image*)(p);
-
-    cond_all.lock();
-
-    // If first to time, wait
-    if (count == 0)
-    {
-      wait_for_others(img);
-      return;
-    }
-
-    // If behind, skip
-    if (img->header.stamp < image_time)
-    {
-      cond_all.unlock();
-      return;
-    }
-
-    // If at time, increment and signal or wait
-    if (img->header.stamp == image_time)
-    {
-      count++;
-      if (count == 3)
-      {
-        cond_all.broadcast();
-      }
-      else
-      {
-        while (!done && img->header.stamp == image_time)
-          cond_all.wait();
-      }
-      
-      cond_all.unlock();
-      return;
-    }
-
-    // If ahead, wake up others and then wait
-    if (img->header.stamp > image_time)
-    {
-      printf(" %s is already past with time: %d\n", img->label.c_str(), img->header.stamp.nsec);
-      cond_all.broadcast();
-      wait_for_others(img);
-    }
-  }
-
-  void wait_for_others(image_msgs::Image* img)
-  {
-    count = 1;
-    done = false;
-    image_time = img->header.stamp;
-    bool timed_out = false;
-
-    while (count < 3 && img->header.stamp == image_time && !timed_out)
-      if (!cond_all.timed_wait(1))
-      {
-        printf(" Timed out waiting for other images...\n");
-        timed_out = true;
-      }
-    
-    if (img->header.stamp == image_time && !timed_out)
-      image_cb_all();
-    else
-      printf(" Got interrupted from time %d\n", img->header.stamp.nsec);
-
-    if (img->header.stamp == image_time)
-    {
-      done = true;
-      count = 0;
-      cond_all.broadcast();
-    }
-    cond_all.unlock();
-  }
-
   void image_cb_all()
   {
     lbridge.fromImage(limage);


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