Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[6514] pkg/trunk (Page 15)

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

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.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[6514] pkg/trunk (Page 15)

personalrobots-commit