Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13615] pkg/trunk
|
From: <vij...@us...> - 2009-04-08 20:51:49
|
Revision: 13615
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13615&view=rev
Author: vijaypradeep
Date: 2009-04-08 20:51:43 +0000 (Wed, 08 Apr 2009)
Log Message:
-----------
Added new constructor to topic_synchronizer. Fixed include path for topic_synchronizer.h
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/util/topic_synchronizer/manifest.xml
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/recognition_lambertian/.build_version
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
Added Paths:
-----------
pkg/trunk/util/topic_synchronizer/CMakeLists.txt
pkg/trunk/util/topic_synchronizer/Makefile
pkg/trunk/util/topic_synchronizer/include/
pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/
pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h
Removed Paths:
-------------
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -39,7 +39,7 @@
#include "robot_msgs/MechanismState.h"
#include "robot_msgs/MocapSnapshot.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "std_msgs/Empty.h"
#include "robot_msgs/PointCloud.h"
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -65,7 +65,7 @@
// transform library
#include <tf/transform_listener.h>
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "CvStereoCamModel.h"
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -59,7 +59,7 @@
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Added: pkg/trunk/util/topic_synchronizer/CMakeLists.txt
===================================================================
--- pkg/trunk/util/topic_synchronizer/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/topic_synchronizer/CMakeLists.txt 2009-04-08 20:51:43 UTC (rev 13615)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(topic_synchronizer)
+# genmsg()
+# gensrv()
Added: pkg/trunk/util/topic_synchronizer/Makefile
===================================================================
--- pkg/trunk/util/topic_synchronizer/Makefile (rev 0)
+++ pkg/trunk/util/topic_synchronizer/Makefile 2009-04-08 20:51:43 UTC (rev 13615)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h (from rev 13608, pkg/trunk/util/topic_synchronizer/topic_synchronizer.h)
===================================================================
--- pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h (rev 0)
+++ pkg/trunk/util/topic_synchronizer/include/topic_synchronizer/topic_synchronizer.h 2009-04-08 20:51:43 UTC (rev 13615)
@@ -0,0 +1,342 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+#ifndef TOPIC_SYNCHRONIZER_HH
+#define TOPIC_SYNCHRONIZER_HH
+
+
+#include <boost/thread.hpp>
+#include "boost/date_time/posix_time/posix_time.hpp"
+
+//#include "rosthread/mutex.h"
+//#include "rosthread/condition.h"
+
+#include "ros/time.h"
+
+ //! A templated class for synchronizing incoming topics
+ /*!
+ * The Topic Synchronizer should be templated by your node, and is
+ * passed a function pointer at construction to be called every time
+ * all of your topics have arrived.
+ */
+template <class N>
+class TopicSynchronizer
+{
+
+ class UnsubscribeList
+ {
+ std::list<std::string> list_;
+ boost::mutex list_mutex_;
+ // ros::thread::mutex list_mutex_;
+
+ public:
+ UnsubscribeList(std::list<std::string>& l) : list_(l) { }
+
+ void doUnsubscribe(ros::Node* n, std::string topic)
+ {
+ list_mutex_.lock();
+ std::list<std::string>::iterator i = list_.begin();
+ while (i != list_.end() && *i != topic)
+ i++;
+
+ if (i != list_.end())
+ {
+ i++;
+
+ while (i != list_.end())
+ {
+ n->unsubscribe(*i);
+ list_.erase(i++);
+ }
+ }
+ list_mutex_.unlock();
+ }
+ };
+
+ class UnsubscribeHelper
+ {
+ UnsubscribeList* ul_;
+ std::string topic_;
+
+ public:
+
+ UnsubscribeHelper(UnsubscribeList* ul, std::string topic) : ul_(ul), topic_(topic) {}
+
+ void doUnsubscribe(ros::Node* node)
+ {
+ ul_->doUnsubscribe(node, topic_);
+ }
+ };
+
+ private:
+
+ //! A pointer to your node for calling callback
+ ros::Node* node_;
+ N* obj_ ;
+
+ //! The callback to be called if successful
+ void (N::*callback_)(ros::Time);
+
+ //! Timeout duration
+ boost::posix_time::time_duration timeout_;
+ // ros::Duration timeout_;
+
+ //! The callback to be called if timed out
+ void (N::*timeout_callback_)(ros::Time);
+
+ //! The condition variable and mutex used for synchronization
+ boost::condition_variable cond_all_;
+ boost::mutex cond_all_mutex_;
+ // ros::thread::condition cond_all_;
+
+ //! The number of expected incoming messages
+ int expected_count_;
+
+ //! The count of messages received so far
+ int count_;
+
+ //! Whether or not the given wait cycle has completed
+ bool done_;
+
+ //! Whether or not the node is ready to process data (all subscriptions are completed)
+ bool ready_;
+
+ //! The timestamp that is currently being waited for
+ ros::Time waiting_time_;
+
+ std::list< UnsubscribeList* > exclusion_lists_;
+ std::list< UnsubscribeHelper** > helper_list_;
+
+ //! The callback invoked by roscpp when any topic comes in
+ /*!
+ * \param p A void* which should point to the timestamp field of the incoming message
+ */
+ void msg_cb(void* p)
+ {
+ //Bail until we are actually ready
+ if (!ready_)
+ return;
+
+ ros::Time* time = (ros::Time*)(p);
+
+
+ boost::unique_lock<boost::mutex> lock(cond_all_mutex_);
+ // cond_all_mutex_.lock();
+
+ // If first to get message, wait for others
+ if (count_ == 0)
+ {
+ wait_for_others(time, lock);
+ return;
+ }
+
+ // If behind, skip
+ if (*time < waiting_time_)
+ {
+ // cond_all_mutex_.unlock();
+ return;
+ }
+
+ // If at time, increment count, possibly signal, and wait
+ if (*time == waiting_time_)
+ {
+ count_++;
+ if (count_ == expected_count_)
+ {
+ cond_all_.notify_all();
+ }
+
+ while (!done_ && *time == waiting_time_)
+ cond_all_.wait(lock);
+
+ // cond_all_mutex_.unlock();
+ return;
+ }
+
+ // If ahead, wakeup others, and ten wait for others
+ if (*time > waiting_time_)
+ {
+ cond_all_.notify_all();
+ wait_for_others(time, lock);
+ }
+ }
+
+ void msg_unsubothers(void* p)
+ {
+ UnsubscribeHelper** uh = (UnsubscribeHelper**)(p);
+
+ if (*uh != 0)
+ {
+ (*uh)->doUnsubscribe(node_);
+ delete (*uh);
+ (*uh) = 0;
+ }
+ }
+
+ //! The function called in a message cb to wait for other messages
+ /*!
+ * \param time The time that is being waited for
+ */
+ void wait_for_others(ros::Time* time, boost::unique_lock<boost::mutex>& lock)
+ {
+ 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(lock, timeout_))
+ {
+ timed_out = true;
+ if (timeout_callback_)
+ (obj_->*timeout_callback_)(*time);
+ }
+
+ if (*time == waiting_time_ && !timed_out)
+ (obj_->*callback_)(*time);
+
+ if (*time == waiting_time_)
+ {
+ done_ = true;
+ count_ = 0;
+ cond_all_.notify_all();
+ }
+ // cond_all_mutex_.unlock();
+ }
+
+ public:
+
+ //! Constructor
+ /*!
+ * The constructor for the TopicSynchronizer. Kept for backwards compatibility. Since you shouldn't
+ * be inheriting from ros::Node, this constructor is probably not the one you want to be using.
+ *
+ * \param node A pointer to your node, which happens to also be the callback object
+ * \param callback A pointer to the callback to invoke when all messages have arrived
+ * \param timeout The duration
+ * \param timeout_callback A callback which is triggered when the timeout expires
+ */
+ TopicSynchronizer(N* obj, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(obj), obj_(obj), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
+ {
+ timeout_ = boost::posix_time::nanosec(timeout.toNSec());
+ }
+
+ //! Constructor
+ /*!
+ * The constructor for the TopicSynchronizer
+ *
+ * \param node A pointer to your node.
+ * \param obj A pointer to the callback object
+ * \param callback A pointer to the callback to invoke when all messages have arrived
+ * \param timeout The duration
+ * \param timeout_callback A callback which is triggered when the timeout expires
+ */
+ TopicSynchronizer(ros::Node* node, N* obj, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), obj_(obj), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
+ {
+ timeout_ = boost::posix_time::nanosec(timeout.toNSec());
+ }
+
+ //! Destructor
+ ~TopicSynchronizer()
+ {
+ for (typename std::list<UnsubscribeList*>::iterator i = exclusion_lists_.begin();
+ i != exclusion_lists_.end();
+ i++)
+ delete (*i);
+
+ for (typename std::list<UnsubscribeHelper**>::iterator i = helper_list_.begin();
+ i != helper_list_.end();
+ i++)
+ {
+ if (**i != 0)
+ delete **i;
+ delete (*i);
+ }
+ }
+
+ //! Subscribe
+ /*!
+ * The synchronized subscribe call. Call this to subscribe for topics you want
+ * to be synchronized.
+ *
+ * \param topic_name The name of the topic to subscribe to
+ * \param msg A reference to the message that will be populated on callbacks
+ * \param queue_size The size of the incoming queue for the message
+ */
+ 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_++;
+ }
+
+ template <class M>
+ void subscribe(std::list<std::string> topic_names, M& msg, int queue_size)
+ {
+ UnsubscribeList* ul = new UnsubscribeList(topic_names);
+ exclusion_lists_.push_back(ul); // so we can delete later
+
+ for (std::list<std::string>::iterator tn_iter = topic_names.begin();
+ tn_iter != topic_names.end();
+ tn_iter++)
+ {
+ UnsubscribeHelper** uh = new UnsubscribeHelper*;
+ *uh = new UnsubscribeHelper(ul, *tn_iter);
+ helper_list_.push_back(uh);
+
+ node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_unsubothers, this, uh, queue_size);
+ node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
+ }
+ expected_count_++;
+ }
+
+ void ready()
+ {
+ ready_ = true;
+ }
+
+ void reset()
+ {
+ expected_count_ = 0;
+ count_ = 0;
+ done_ = false;
+ }
+};
+
+#endif
Modified: pkg/trunk/util/topic_synchronizer/manifest.xml
===================================================================
--- pkg/trunk/util/topic_synchronizer/manifest.xml 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/util/topic_synchronizer/manifest.xml 2009-04-08 20:51:43 UTC (rev 13615)
@@ -6,7 +6,7 @@
<depend package="roscpp" />
<depend package="std_msgs" />
<export>
- <cpp cflags="-I${prefix} -D__STDC_LIMIT_MACROS -DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG `rosboost-cfg --cflags`" lflags="`rosboost-cfg --lflags thread`"/>
+ <cpp cflags="-I${prefix}/include -D__STDC_LIMIT_MACROS -DBOOST_DATE_TIME_POSIX_TIME_STD_CONFIG `rosboost-cfg --cflags`" lflags="`rosboost-cfg --lflags thread`"/>
</export>
<url>http://pr.willowgarage.com/wiki/topic_synchronizer</url>
</package>
Deleted: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2009-04-08 20:51:43 UTC (rev 13615)
@@ -1,325 +0,0 @@
-/*********************************************************************
-* 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.
-*********************************************************************/
-
-/*! \mainpage
- * \htmlinclude manifest.html
- */
-
-#ifndef TOPIC_SYNCHRONIZER_HH
-#define TOPIC_SYNCHRONIZER_HH
-
-
-#include <boost/thread.hpp>
-#include "boost/date_time/posix_time/posix_time.hpp"
-
-//#include "rosthread/mutex.h"
-//#include "rosthread/condition.h"
-
-#include "ros/time.h"
-
- //! A templated class for synchronizing incoming topics
- /*!
- * The Topic Synchronizer should be templated by your node, and is
- * passed a function pointer at construction to be called every time
- * all of your topics have arrived.
- */
-template <class N>
-class TopicSynchronizer
-{
-
- class UnsubscribeList
- {
- std::list<std::string> list_;
- boost::mutex list_mutex_;
- // ros::thread::mutex list_mutex_;
-
- public:
- UnsubscribeList(std::list<std::string>& l) : list_(l) { }
-
- void doUnsubscribe(ros::Node* n, std::string topic)
- {
- list_mutex_.lock();
- std::list<std::string>::iterator i = list_.begin();
- while (i != list_.end() && *i != topic)
- i++;
-
- if (i != list_.end())
- {
- i++;
-
- while (i != list_.end())
- {
- n->unsubscribe(*i);
- list_.erase(i++);
- }
- }
- list_mutex_.unlock();
- }
- };
-
- class UnsubscribeHelper
- {
- UnsubscribeList* ul_;
- std::string topic_;
-
- public:
-
- UnsubscribeHelper(UnsubscribeList* ul, std::string topic) : ul_(ul), topic_(topic) {}
-
- void doUnsubscribe(ros::Node* node)
- {
- ul_->doUnsubscribe(node, topic_);
- }
- };
-
- private:
-
- //! A pointer to your node for calling callback
- N* node_;
-
- //! The callback to be called if successful
- void (N::*callback_)(ros::Time);
-
- //! Timeout duration
- boost::posix_time::time_duration timeout_;
- // ros::Duration timeout_;
-
- //! The callback to be called if timed out
- void (N::*timeout_callback_)(ros::Time);
-
- //! The condition variable and mutex used for synchronization
- boost::condition_variable cond_all_;
- boost::mutex cond_all_mutex_;
- // ros::thread::condition cond_all_;
-
- //! The number of expected incoming messages
- int expected_count_;
-
- //! The count of messages received so far
- int count_;
-
- //! Whether or not the given wait cycle has completed
- bool done_;
-
- //! Whether or not the node is ready to process data (all subscriptions are completed)
- bool ready_;
-
- //! The timestamp that is currently being waited for
- ros::Time waiting_time_;
-
- std::list< UnsubscribeList* > exclusion_lists_;
- std::list< UnsubscribeHelper** > helper_list_;
-
- //! The callback invoked by roscpp when any topic comes in
- /*!
- * \param p A void* which should point to the timestamp field of the incoming message
- */
- void msg_cb(void* p)
- {
- //Bail until we are actually ready
- if (!ready_)
- return;
-
- ros::Time* time = (ros::Time*)(p);
-
-
- boost::unique_lock<boost::mutex> lock(cond_all_mutex_);
- // cond_all_mutex_.lock();
-
- // If first to get message, wait for others
- if (count_ == 0)
- {
- wait_for_others(time, lock);
- return;
- }
-
- // If behind, skip
- if (*time < waiting_time_)
- {
- // cond_all_mutex_.unlock();
- return;
- }
-
- // If at time, increment count, possibly signal, and wait
- if (*time == waiting_time_)
- {
- count_++;
- if (count_ == expected_count_)
- {
- cond_all_.notify_all();
- }
-
- while (!done_ && *time == waiting_time_)
- cond_all_.wait(lock);
-
- // cond_all_mutex_.unlock();
- return;
- }
-
- // If ahead, wakeup others, and ten wait for others
- if (*time > waiting_time_)
- {
- cond_all_.notify_all();
- wait_for_others(time, lock);
- }
- }
-
- void msg_unsubothers(void* p)
- {
- UnsubscribeHelper** uh = (UnsubscribeHelper**)(p);
-
- if (*uh != 0)
- {
- (*uh)->doUnsubscribe(node_);
- delete (*uh);
- (*uh) = 0;
- }
- }
-
- //! The function called in a message cb to wait for other messages
- /*!
- * \param time The time that is being waited for
- */
- void wait_for_others(ros::Time* time, boost::unique_lock<boost::mutex>& lock)
- {
- 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(lock, timeout_))
- {
- timed_out = true;
- if (timeout_callback_)
- (*node_.*timeout_callback_)(*time);
- }
-
- if (*time == waiting_time_ && !timed_out)
- (*node_.*callback_)(*time);
-
- if (*time == waiting_time_)
- {
- done_ = true;
- count_ = 0;
- cond_all_.notify_all();
- }
- // cond_all_mutex_.unlock();
- }
-
- public:
-
- //! Constructor
- /*!
- * The constructor for the TopicSynchronizer
- *
- * \param node A pointer to your node.
- * \param callback A pointer to the callback to invoke when all messages have arrived
- * \param timeout The duration
- * \param timeout_callback A callback which is triggered when the timeout expires
- */
- TopicSynchronizer(N* node, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), callback_(callback), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
- {
- timeout_ = boost::posix_time::nanosec(timeout.toNSec());
- }
-
- //! Destructor
- ~TopicSynchronizer()
- {
- for (typename std::list<UnsubscribeList*>::iterator i = exclusion_lists_.begin();
- i != exclusion_lists_.end();
- i++)
- delete (*i);
-
- for (typename std::list<UnsubscribeHelper**>::iterator i = helper_list_.begin();
- i != helper_list_.end();
- i++)
- {
- if (**i != 0)
- delete **i;
- delete (*i);
- }
- }
-
- //! Subscribe
- /*!
- * The synchronized subscribe call. Call this to subscribe for topics you want
- * to be synchronized.
- *
- * \param topic_name The name of the topic to subscribe to
- * \param msg A reference to the message that will be populated on callbacks
- * \param queue_size The size of the incoming queue for the message
- */
- 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_++;
- }
-
- template <class M>
- void subscribe(std::list<std::string> topic_names, M& msg, int queue_size)
- {
- UnsubscribeList* ul = new UnsubscribeList(topic_names);
- exclusion_lists_.push_back(ul); // so we can delete later
-
- for (std::list<std::string>::iterator tn_iter = topic_names.begin();
- tn_iter != topic_names.end();
- tn_iter++)
- {
- UnsubscribeHelper** uh = new UnsubscribeHelper*;
- *uh = new UnsubscribeHelper(ul, *tn_iter);
- helper_list_.push_back(uh);
-
- node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_unsubothers, this, uh, queue_size);
- node_->subscribe(*tn_iter, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
- }
- expected_count_++;
- }
-
- void ready()
- {
- ready_ = true;
- }
-
- void reset()
- {
- expected_count_ = 0;
- count_ = 0;
- done_ = false;
- }
-};
-
-#endif
Modified: pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
===================================================================
--- pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -65,7 +65,7 @@
#include <point_cloud_mapping/sample_consensus/sac.h>
#include <point_cloud_mapping/sample_consensus/ransac.h>
#include <point_cloud_mapping/sample_consensus/lmeds.h>
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <tf/transform_listener.h>
#include "CvStereoCamModel.h"
Modified: pkg/trunk/vision/people/src/face_detection.cpp
===================================================================
--- pkg/trunk/vision/people/src/face_detection.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/people/src/face_detection.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -50,7 +50,7 @@
#include "image_msgs/CvBridge.h"
#include "image_msgs/ColoredLine.h"
#include "image_msgs/ColoredLines.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include "opencv/cxcore.h"
Modified: pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -49,7 +49,7 @@
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
#include "image_msgs/ColoredLines.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
#include "opencv/cxcore.h"
Modified: pkg/trunk/vision/people/src/track_starter_gui.cpp
===================================================================
--- pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -49,7 +49,7 @@
#include "CvStereoCamModel.h"
#include <robot_msgs/PositionMeasurement.h>
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Modified: pkg/trunk/vision/recognition_lambertian/.build_version
===================================================================
--- pkg/trunk/vision/recognition_lambertian/.build_version 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/recognition_lambertian/.build_version 2009-04-08 20:51:43 UTC (rev 13615)
@@ -1 +1 @@
-:
+https://personalrobots.svn.sourceforge.net/svnroot/personalrobots/pkg/trunk/vision/recognition_lambertian:13607
Modified: pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -65,7 +65,7 @@
// transform library
#include <tf/transform_listener.h>
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include "CvStereoCamModel.h"
Modified: pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
===================================================================
--- pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -73,7 +73,7 @@
// ... //
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <boost/thread.hpp>
//DEFINES
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -48,7 +48,7 @@
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <boost/thread.hpp>
Modified: pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-04-08 20:31:08 UTC (rev 13614)
+++ pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-04-08 20:51:43 UTC (rev 13615)
@@ -61,7 +61,7 @@
#include "color_calib.h"
-#include "topic_synchronizer.h"
+#include "topic_synchronizer/topic_synchronizer.h"
#include <boost/thread.hpp>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
