camera.cpp
Go to the documentation of this file.
00001 #include <boost/thread.hpp>
00002 
00003 #include <ros/ros.h>
00004 #include <ros/time.h>
00005 
00006 #include "uvc_cam/uvc_cam.h"
00007 #include "sensor_msgs/Image.h"
00008 #include "sensor_msgs/image_encodings.h"
00009 #include "sensor_msgs/CameraInfo.h"
00010 #include "camera_info_manager/camera_info_manager.h"
00011 #include "image_transport/image_transport.h"
00012 
00013 #include "uvc_camera/camera.h"
00014 
00015 using namespace sensor_msgs;
00016 
00017 namespace uvc_camera {
00018 
00019 Camera::Camera(ros::NodeHandle _comm_nh, ros::NodeHandle _param_nh) :
00020       node(_comm_nh), pnode(_param_nh), it(_comm_nh),
00021       info_mgr(_comm_nh, "camera"), cam(0) {
00022 
00023       /* default config values */
00024       width = 640;
00025       height = 480;
00026       fps = 10;
00027       skip_frames = 0;
00028       frames_to_skip = 0;
00029       device = "/dev/video0";
00030       frame = "camera";
00031       rotate = false;
00032 
00033       /* set up information manager */
00034       std::string url;
00035 
00036       pnode.getParam("camera_info_url", url);
00037 
00038       info_mgr.loadCameraInfo(url);
00039 
00040       /* pull other configuration */
00041       pnode.getParam("device", device);
00042 
00043       pnode.getParam("fps", fps);
00044       pnode.getParam("skip_frames", skip_frames);
00045 
00046       pnode.getParam("width", width);
00047       pnode.getParam("height", height);
00048 
00049       pnode.getParam("frame_id", frame);
00050 
00051       /* advertise image streams and info streams */
00052       pub = it.advertise("image_raw", 1);
00053 
00054       info_pub = node.advertise<CameraInfo>("camera_info", 1);
00055 
00056       /* initialize the cameras */
00057       cam = new uvc_cam::Cam(device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00058       cam->set_motion_thresholds(100, -1);
00059 
00060       /* and turn on the streamer */
00061       ok = true;
00062       image_thread = boost::thread(boost::bind(&Camera::feedImages, this));
00063     }
00064 
00065     void Camera::sendInfo(ImagePtr &image, ros::Time time) {
00066       CameraInfoPtr info(new CameraInfo(info_mgr.getCameraInfo()));
00067 
00068       /* Throw out any CamInfo that's not calibrated to this camera mode */
00069       if (info->K[0] != 0.0 &&
00070            (image->width != info->width
00071               || image->height != info->height)) {
00072         info.reset(new CameraInfo());
00073       }
00074 
00075       /* If we don't have a calibration, set the image dimensions */
00076       if (info->K[0] == 0.0) {
00077         info->width = image->width;
00078         info->height = image->height;
00079       }
00080 
00081       info->header.stamp = time;
00082       info->header.frame_id = frame;
00083 
00084       info_pub.publish(info);
00085     }
00086 
00087     void Camera::feedImages() {
00088       unsigned int pair_id = 0;
00089       while (ok) {
00090         unsigned char *img_frame = NULL;
00091         uint32_t bytes_used;
00092 
00093         ros::Time capture_time = ros::Time::now();
00094 
00095         int idx = cam->grab(&img_frame, bytes_used);
00096 
00097         /* Read in every frame the camera generates, but only send each
00098          * (skip_frames + 1)th frame. It's set up this way just because
00099          * this is based on Stereo...
00100          */
00101         if (skip_frames == 0 || frames_to_skip == 0) {
00102           if (img_frame) {
00103              ImagePtr image(new Image);
00104 
00105              image->height = height;
00106              image->width = width;
00107              image->step = 3 * width;
00108              image->encoding = image_encodings::RGB8;
00109 
00110              image->header.stamp = capture_time;
00111              image->header.seq = pair_id;
00112 
00113              image->header.frame_id = frame;
00114 
00115              image->data.resize(image->step * image->height);
00116 
00117              memcpy(&image->data[0], img_frame, width*height * 3);
00118 
00119              pub.publish(image);
00120 
00121              sendInfo(image, capture_time);
00122 
00123              ++pair_id;
00124           }
00125 
00126           frames_to_skip = skip_frames;
00127         } else {
00128           frames_to_skip--;
00129         }
00130 
00131         if (img_frame) cam->release(idx);
00132       }
00133     }
00134 
00135     Camera::~Camera() {
00136       ok = false;
00137       image_thread.join();
00138       if (cam) delete cam;
00139     }
00140 
00141 
00142 };
00143 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


uvc_camera_yuv
Author(s): Jochen Sprickerhof/jochen@sprickerhof.de
autogenerated on Thu Apr 25 2013 16:19:21