senderIT.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 // Modified Apr 6, 2010 by Adam Leeper - changed to use "image_transport"
00031 
00032 #include <cstdio>
00033 #include <ros/ros.h>
00034 #include <ros/time.h>
00035 #include "uvc_cam/uvc_cam.h"
00036 #include "sensor_msgs/Image.h"
00037 #include "sensor_msgs/image_encodings.h"
00038 
00039 #include <image_transport/image_transport.h>
00040 #include <opencv/cvwimage.h>
00041 #include <opencv/highgui.h>
00042 #include <cv_bridge/CvBridge.h>
00043 
00044 int main(int argc, char **argv)
00045 {
00046   ros::init(argc, argv, "uvc_cam");
00047   ros::NodeHandle n;
00048   ros::NodeHandle n_private("~");
00049 
00050   std::string device;
00051   std::string out_topic;
00052   n_private.param<std::string>("device", device, "/dev/video0");
00053   n_private.param<std::string>("topic", out_topic, "/camera/image");
00054   int width, height, fps, modetect_lum, modetect_count;
00055   n_private.param("width", width, 640);
00056   n_private.param("height", height, 480);
00057   n_private.param("fps", fps, 30);
00058   n_private.param("motion_threshold_luminance", modetect_lum, 100); 
00059   n_private.param("motion_threshold_count", modetect_count, -1); 
00060 
00061   image_transport::ImageTransport it(n);
00062   image_transport::Publisher pub = it.advertise(out_topic.c_str(), 1);
00063 
00064   //ros::Publisher pub = n.advertise<sensor_msgs::Image>(out_topic.c_str(), 1);
00065   ROS_INFO("opening uvc_cam at %dx%d, %d fps", width, height, fps);
00066   uvc_cam::Cam cam(device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00067   cam.set_motion_thresholds(modetect_lum, modetect_count);
00068   IplImage *imageIpl = cvCreateImageHeader(cvSize(640,480), 8, 3);
00069 
00070   ros::Time t_prev(ros::Time::now());
00071   int count = 0, skip_count = 0;
00072   while (n.ok())
00073   {
00074     unsigned char *frame = NULL;
00075     uint32_t bytes_used;
00076     int buf_idx = cam.grab(&frame, bytes_used);
00077     if (count++ % fps == 0)
00078     {
00079       ros::Time t(ros::Time::now());
00080       ros::Duration d(t - t_prev);
00081       ROS_INFO("%.1f fps skip %d", (double)fps / d.toSec(), skip_count);
00082       t_prev = t;
00083     }
00084     if (frame)
00085     {
00086       //cv::WImageBuffer3_b image( frame );
00087       //cv::Mat data(height, width, CV_8UC1, frame, 3 * width);
00088       imageIpl->imageData = (char *)frame;
00089       sensor_msgs::Image::Ptr image = sensor_msgs::CvBridge::cvToImgMsg( imageIpl, "bgr8");
00090 
00091       //sensor_msgs::Image image; 
00092       
00093       image->header.stamp = ros::Time::now();
00094       image->encoding = sensor_msgs::image_encodings::RGB8;
00095       image->height = height;
00096       image->width = width;
00097       image->step = 3 * width;
00098 
00099       //image->set_data_size( image.step * image.height );
00100       
00101       /*
00102       uint8_t* bgr = &(image.data[0]);
00103       for (uint32_t y = 0; y < height; y++)
00104         for (uint32_t x = 0; x < width; x++)
00105         {
00106           // hack... flip bgr to rgb
00107           uint8_t *p = frame + y * width * 3 + x * 3;
00108           uint8_t *q = bgr   + y * width * 3 + x * 3;
00109           q[0] = p[2]; q[1] = p[1]; q[2] = p[0];
00110         }
00111       */
00112       //memcpy(&image.data[0], frame, width * height * 3);
00113       pub.publish(image);
00114       cam.release(buf_idx);
00115     }
00116     else
00117       skip_count++;
00118   }
00119   return 0;
00120 }
00121 
 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