Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6514] pkg/trunk
|
From: <jl...@us...> - 2008-11-11 07:09:54
|
Revision: 6514
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6514&view=rev
Author: jleibs
Date: 2008-11-11 07:09:49 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Fixed timestamps in dcam_node and added example of subscribing to multiple nodes in synchronized fashion.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
Added Paths:
-----------
pkg/trunk/vision/stereo_view/
pkg/trunk/vision/stereo_view/CMakeLists.txt
pkg/trunk/vision/stereo_view/Makefile
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/stereo_view/stereo_view.cpp
Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-11 06:09:22 UTC (rev 6513)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-11 07:09:49 UTC (rev 6514)
@@ -251,6 +251,8 @@
stcam->stIm->imHeight, stcam->stIm->imWidth, 1,
"mono", "uint16",
stcam->stIm->imDisp );
+
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish("~disparity", img_);
stereo_info_.has_disparity = true;
@@ -258,7 +260,7 @@
stereo_info_.has_disparity = false;
}
- stereo_info_.header.stamp = ros::Time(stcam->stIm->imLeft->im_time * 1000);
+ stereo_info_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
stereo_info_.height = stcam->stIm->imHeight;
stereo_info_.width = stcam->stIm->imWidth;
@@ -288,16 +290,14 @@
void publishImages(std::string base_name, cam::ImageData* img_data)
{
-
- img_.header.stamp = ros::Time(img_data->im_time * 1000);
- cam_info_.header.stamp = ros::Time(img_data->im_time * 1000);
-
if (img_data->imRawType != COLOR_CODING_NONE)
{
fillImage(img_, "image_raw",
img_data->imHeight, img_data->imWidth, 1,
"mono", "byte",
img_data->imRaw );
+
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_raw"), img_);
cam_info_.has_image = true;
} else {
@@ -310,6 +310,7 @@
img_data->imHeight, img_data->imWidth, 1,
"mono", "byte",
img_data->im );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image"), img_);
cam_info_.has_image = true;
} else {
@@ -322,6 +323,8 @@
img_data->imHeight, img_data->imWidth, 4,
"rgba", "byte",
img_data->imColor );
+
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_color"), img_);
cam_info_.has_image_color = true;
} else {
@@ -334,6 +337,7 @@
img_data->imHeight, img_data->imWidth, 1,
"mono", "byte",
img_data->imRect );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect"), img_);
cam_info_.has_image_rect = true;
} else {
@@ -346,12 +350,14 @@
img_data->imHeight, img_data->imWidth, 4,
"rgba", "byte",
img_data->imRectColor );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect_color"), img_);
cam_info_.has_image_rect_color = true;
} else {
cam_info_.has_image_rect_color = false;
}
+ cam_info_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
cam_info_.height = img_data->imHeight;
cam_info_.width = img_data->imWidth;
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-11 06:09:22 UTC (rev 6513)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-11 07:09:49 UTC (rev 6514)
@@ -75,7 +75,7 @@
cvInitImageHeader(img_, cvSize(rosimg.byte_data.layout.dim[1].size, rosimg.byte_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
cvSetData(img_, &(rosimg.byte_data.data[0]), rosimg.byte_data.layout.dim[1].stride);
} else if (rosimg.depth == "uint16") {
- cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
+ 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));
}
Added: pkg/trunk/vision/stereo_view/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/stereo_view/CMakeLists.txt (rev 0)
+++ pkg/trunk/vision/stereo_view/CMakeLists.txt 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+
+rospack(stereo_view)
+
+rospack_add_executable(stereo_view stereo_view.cpp)
Added: pkg/trunk/vision/stereo_view/Makefile
===================================================================
--- pkg/trunk/vision/stereo_view/Makefile (rev 0)
+++ pkg/trunk/vision/stereo_view/Makefile 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/vision/stereo_view/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_view/manifest.xml (rev 0)
+++ pkg/trunk/vision/stereo_view/manifest.xml 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1,13 @@
+<package>
+ <description>
+ An opencv based viewer for dcam stereo images
+ </description>
+ <author>Jeremy Leibs</author>
+ <license>BSD</license>
+ <depend package="opencv_latest"/>
+ <depend package="std_msgs"/>
+ <depend package="std_srvs"/>
+ <depend package="image_msgs"/>
+ <depend package="roscpp"/>
+ <depend package="rosthread"/>
+</package>
Added: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp (rev 0)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1,159 @@
+#include <vector>
+
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+#include "ros/node.h"
+#include "image_msgs/StereoInfo.h"
+#include "image_msgs/Image.h"
+#include "image_msgs/CvBridge.h"
+
+#include "rosthread/condition.h"
+
+using namespace std;
+
+class StereoView : public ros::node
+{
+public:
+
+ image_msgs::Image limage;
+ image_msgs::Image rimage;
+ image_msgs::Image dimage;
+
+ image_msgs::CvBridge lbridge;
+ image_msgs::CvBridge rbridge;
+ image_msgs::CvBridge dbridge;
+
+ std::string ltopic;
+ std::string rtopic;
+ std::string dtopic;
+
+ ros::Time image_time;
+
+ ros::thread::condition cond_all;
+
+ int count;
+ bool done;
+
+ StereoView() : ros::node("cv_view"), count(0), done(false)
+ {
+ 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);
+
+ cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
+ subscribe(rtopic, rimage, &StereoView::image_cb, &rimage, 1);
+
+ cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
+ subscribe(dtopic, dimage, &StereoView::image_cb, &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);
+ rbridge.fromImage(rimage);
+ dbridge.fromImage(dimage);
+
+ IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
+
+ // Disparity has to be scaled to be be nicely displayable
+ cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+
+ cvShowImage("left", lbridge.toIpl());
+ cvShowImage("right", rbridge.toIpl());
+ cvShowImage("disparity", disp);
+
+ cvWaitKey(5);
+
+ cvReleaseImage(&disp);
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ StereoView view;
+ view.spin();
+ ros::fini();
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
