Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6564] pkg/trunk
|
From: <jl...@us...> - 2008-11-12 01:29:41
|
Revision: 6564
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6564&view=rev
Author: jleibs
Date: 2008-11-12 01:29:38 +0000 (Wed, 12 Nov 2008)
Log Message:
-----------
TopicSynchronizer allows for optional timeout specification and timeout callback. Also callback
takes a ros::time argument to allow for intelligent determination of which messages are actually
synchronized.
Modified Paths:
--------------
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
pkg/trunk/vision/stereo_view/stereo_view.cpp
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-12 01:26:40 UTC (rev 6563)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-12 01:29:38 UTC (rev 6564)
@@ -77,6 +77,8 @@
} else if (rosimg.depth == "uint16") {
cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_16U, 1);
cvSetData(img_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
+ } else {
+ return false;
}
return true;
Modified: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2008-11-12 01:26:40 UTC (rev 6563)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2008-11-12 01:29:38 UTC (rev 6564)
@@ -40,6 +40,7 @@
#define TOPIC_SYNCHRONIZER_HH
#include "rosthread/condition.h"
+#include "ros/time.h"
//! A templated class for synchronizing incoming topics
/*!
@@ -55,8 +56,14 @@
//! A pointer to your node for calling callback
N* node_;
- //! The callback to be called
- void (N::*callback_)();
+ //! The callback to be called if successful
+ void (N::*callback_)(ros::Time);
+
+ //! Timeout duration
+ ros::Duration timeout_;
+
+ //! The callback to be called if timed out
+ void (N::*timeout_callback_)(ros::Time);
//! The condition variable used for synchronization
ros::thread::condition cond_all_;
@@ -122,6 +129,9 @@
}
//! 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)
{
count_ = 1;
@@ -131,14 +141,15 @@
bool timed_out = false;
while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
- if (!cond_all_.timed_wait(1))
+ if (!cond_all_.timed_wait(timeout_))
{
- printf(" Timed out waiting for other images...\n");
timed_out = true;
+ if (timeout_callback_)
+ (*node_.*timeout_callback_)(*time);
}
if (*time == waiting_time_ && !timed_out)
- (*node_.*callback_)();
+ (*node_.*callback_)(*time);
if (*time == waiting_time_)
{
@@ -155,10 +166,12 @@
/*!
* 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 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)()) : node_(node), callback_(callback), expected_count_(0), count_(0), done_(false)
+ 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_(timeout), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
{ }
//! Subscribe
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-12 01:26:40 UTC (rev 6563)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-12 01:29:38 UTC (rev 6564)
@@ -61,7 +61,7 @@
TopicSynchronizer<StereoView> sync;
- StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all)
+ StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout)
{
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
@@ -72,23 +72,39 @@
sync.subscribe("dcam/disparity", dimage, 1);
}
- void image_cb_all()
+ void image_cb_all(ros::Time t)
{
- lbridge.fromImage(limage);
- rbridge.fromImage(rimage);
- dbridge.fromImage(dimage);
+ if (lbridge.fromImage(limage))
+ cvShowImage("left", lbridge.toIpl());
- // Disparity has to be scaled to be be nicely displayable
- IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+ if (rbridge.fromImage(rimage))
+ cvShowImage("right", rbridge.toIpl());
- cvShowImage("left", lbridge.toIpl());
- cvShowImage("right", rbridge.toIpl());
- cvShowImage("disparity", disp);
+ if (dbridge.fromImage(dimage))
+ {
+ // Disparity has to be scaled to be be nicely displayable
+ IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
+ cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+ cvShowImage("disparity", disp);
+ cvReleaseImage(&disp);
+ }
cvWaitKey(5);
+ }
- cvReleaseImage(&disp);
+ void image_cb_timeout(ros::Time t)
+ {
+ if (limage.header.stamp != t)
+ printf("Timed out waiting for left image\n");
+
+ if (rimage.header.stamp != t)
+ printf("Timed out waiting for right image\n");
+
+ if (dimage.header.stamp != t)
+ printf("Timed out waiting for disparity image\n");
+
+ //Proceed to show images anyways
+ image_cb_all(t);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
