stereo_sender.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Morgan Quigley, Clemens Eppner, Tully Foote
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // 
00031 // Modified Apr 6, 2010 by Adam Leeper - changed to use "image_transport"
00032 // A stereo version of the "sender" file
00033 //
00034 
00035 #include <cstdio>
00036 #include <ros/ros.h>
00037 #include <ros/time.h>
00038 #include "uvc_cam/uvc_cam.h"
00039 #include "sensor_msgs/Image.h"
00040 #include "sensor_msgs/image_encodings.h"
00041 
00042 #include <image_transport/image_transport.h>
00043 #include <opencv/cvwimage.h>
00044 #include <opencv/highgui.h>
00045 #include <cv_bridge/CvBridge.h>
00046 
00047 int main(int argc, char **argv)
00048 {
00049   ros::init(argc, argv, "uvc_cam");
00050   ros::NodeHandle n;
00051   ros::NodeHandle n_private("~");
00052 
00053   std::string deviceL, deviceR;
00054   std::string stereoName, imageName;
00055   n_private.param<std::string>("deviceL", deviceL, "/dev/video0");
00056   n_private.param<std::string>("deviceR", deviceR, "/dev/video1");
00057   n_private.param<std::string>("stereoName", stereoName,  "/my_stereo");
00058   n_private.param<std::string>("imageName", imageName,    "image_raw");
00059   int width, height, fps, modetect_lum, modetect_count;
00060   n_private.param("width", width, 640);
00061   n_private.param("height", height, 480);
00062   n_private.param("fps", fps, 30);
00063   n_private.param("motion_threshold_luminance", modetect_lum, 100); 
00064   n_private.param("motion_threshold_count", modetect_count, -1); 
00065 
00066   std::string left = stereoName + "/left/" + imageName;
00067   std::string right = stereoName + "/right/" + imageName;
00068   image_transport::ImageTransport itL(n);
00069   image_transport::Publisher pubL = itL.advertise(left.c_str(), 1);
00070   image_transport::ImageTransport itR(n);
00071   image_transport::Publisher pubR = itR.advertise(right.c_str(), 1);
00072 
00073   //ros::Publisher pub = n.advertise<sensor_msgs::Image>(out_topic.c_str(), 1);
00074   ROS_INFO("opening uvc_cam at %dx%d, %d fps", width, height, fps);
00075   uvc_cam::Cam camL(deviceL.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00076   uvc_cam::Cam camR(deviceR.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00077   camL.set_motion_thresholds(modetect_lum, modetect_count);
00078   camR.set_motion_thresholds(modetect_lum, modetect_count);
00079   
00080   IplImage *imageIplL = cvCreateImageHeader(cvSize(640,480), 8, 3);
00081   IplImage *imageIplR = cvCreateImageHeader(cvSize(640,480), 8, 3);
00082 
00083   ros::Time t_prev(ros::Time::now());
00084   int count = 0, skip_count = 0;
00085   while (n.ok())
00086   {
00087     unsigned char *frameL, *frameR = NULL;
00088     uint32_t bytes_usedL, bytes_usedR;
00089     int buf_idxL = camL.grab(&frameL, bytes_usedL);
00090     int buf_idxR = camR.grab(&frameR, bytes_usedR);
00091     if (count++ % fps == 0)
00092     {
00093       ros::Time t(ros::Time::now());
00094       ros::Duration d(t - t_prev);
00095       ROS_INFO("%.1f fps skip %d", (double)fps / d.toSec(), skip_count);
00096       t_prev = t;
00097     }
00098     if (frameL != NULL && frameR != NULL)
00099     {
00100       imageIplL->imageData = (char *)frameL;
00101       imageIplR->imageData = (char *)frameR;
00102       sensor_msgs::Image::Ptr imageL = sensor_msgs::CvBridge::cvToImgMsg( imageIplL, "bgr8");
00103       sensor_msgs::Image::Ptr imageR = sensor_msgs::CvBridge::cvToImgMsg( imageIplR, "bgr8");
00104 
00105       //sensor_msgs::Image image; 
00106       
00107       imageL->header.stamp = imageR->header.stamp = ros::Time::now();
00108       imageL->encoding = imageR->encoding = sensor_msgs::image_encodings::RGB8;
00109       imageL->height = imageR->height = height;
00110       imageL->width = imageR->width = width;
00111       imageL->step = imageR->step = 3 * width;
00112 
00113       //image->set_data_size( image.step * image.height );
00114       
00115       /*
00116       uint8_t* bgr = &(image.data[0]);
00117       for (uint32_t y = 0; y < height; y++)
00118         for (uint32_t x = 0; x < width; x++)
00119         {
00120           // hack... flip bgr to rgb
00121           uint8_t *p = frame + y * width * 3 + x * 3;
00122           uint8_t *q = bgr   + y * width * 3 + x * 3;
00123           q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
00124         }
00125       */
00126       //memcpy(&image.data[0], frame, width * height * 3);
00127       pubL.publish(imageL);
00128       pubR.publish(imageR);
00129       camL.release(buf_idxL);
00130       camR.release(buf_idxR);
00131     }
00132     else
00133       skip_count++;
00134   }
00135   return 0;
00136 }
00137 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


uvc_cam2
Author(s): Morgan Quigley, extended and maintained by Juergen Sturm
autogenerated on Wed Dec 26 2012 15:36:01