stereo.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/stereocamera.h"
00014 
00015 using namespace sensor_msgs;
00016 
00017 
00018 struct control_mod {
00019   uint32_t id;
00020   int32_t val;
00021   std::string name;
00022 
00023   control_mod(uint32_t id, int32_t val, const std::string& name) {
00024     this->id = id;
00025     this->val = val;
00026     this->name = name;
00027   }
00028 };
00029 typedef struct control_mod control_mod_t;
00030 
00031 
00032 /* Rotate an 8-bit, 3-channel image by 180 degrees. */
00033 static inline void rotate(unsigned char *dst_chr, unsigned char *src_chr, int pixels) {
00034   struct pixel_t {
00035     unsigned char r, g, b;
00036   };
00037 
00038   struct pixel_t *src = (pixel_t *) src_chr;
00039   struct pixel_t *dst = &(((pixel_t *) dst_chr)[pixels - 1]);
00040 
00041   for (int i = pixels; i != 0; --i) {
00042     *dst = *src;
00043     src++;
00044     dst--;
00045   }
00046 }
00047 
00048 namespace uvc_camera {
00049 
00050 StereoCamera::StereoCamera(ros::NodeHandle comm_nh, ros::NodeHandle param_nh) :
00051   node(comm_nh), pnode(param_nh), it(comm_nh),
00052   left_info_mgr(ros::NodeHandle(comm_nh, "left"), "left_camera"),
00053   right_info_mgr(ros::NodeHandle(comm_nh, "right"), "right_camera") {
00054 
00055   /* default config values */
00056   width = 640;
00057   height = 480;
00058   fps = 10;
00059   skip_frames = 0;
00060   frames_to_skip = 0;
00061   left_device = "/dev/video0";
00062   right_device = "/dev/video1";
00063   frame = "camera";
00064   rotate_left = false;
00065   rotate_right = false;
00066 
00067   /* set up information managers */
00068   std::string left_url, right_url;
00069 
00070   pnode.getParam("left/camera_info_url", left_url);
00071   pnode.getParam("right/camera_info_url", right_url);
00072 
00073   left_info_mgr.loadCameraInfo(left_url);
00074   right_info_mgr.loadCameraInfo(right_url);
00075 
00076   /* pull other configuration */
00077   pnode.getParam("left/device", left_device);
00078   pnode.getParam("right/device", right_device);
00079 
00080   pnode.getParam("fps", fps);
00081   pnode.getParam("skip_frames", skip_frames);
00082 
00083   pnode.getParam("left/rotate", rotate_left);
00084   pnode.getParam("right/rotate", rotate_right);
00085 
00086   pnode.getParam("width", width);
00087   pnode.getParam("height", height);
00088 
00089   pnode.getParam("frame_id", frame);
00090 
00091   /* advertise image streams and info streams */
00092   left_pub = it.advertise("left/image_raw", 1);
00093   right_pub = it.advertise("right/image_raw", 1);
00094 
00095   left_info_pub = node.advertise<CameraInfo>("left/camera_info", 1);
00096   right_info_pub = node.advertise<CameraInfo>("right/camera_info", 1);
00097 
00098   /* initialize the cameras */
00099   cam_left =
00100       new uvc_cam::Cam(left_device.c_str(), uvc_cam::Cam::MODE_RGB,
00101                        width, height, fps);
00102   cam_left->set_motion_thresholds(100, -1);
00103   cam_right =
00104       new uvc_cam::Cam(right_device.c_str(), uvc_cam::Cam::MODE_RGB,
00105                        width, height, fps);
00106   cam_right->set_motion_thresholds(100, -1);
00107 
00108 
00109   bool auto_focus;
00110   if (pnode.getParam("auto_focus", auto_focus)) {
00111     cam_left->set_v4l2_control(V4L2_CID_FOCUS_AUTO, auto_focus, "auto_focus");
00112     cam_right->set_v4l2_control(V4L2_CID_FOCUS_AUTO, auto_focus, "auto_focus");
00113   }
00114 
00115   int focus_absolute;
00116   if (pnode.getParam("focus_absolute", focus_absolute)) {
00117     cam_left->set_v4l2_control(V4L2_CID_FOCUS_ABSOLUTE, focus_absolute, "focus_absolute");
00118     cam_right->set_v4l2_control(V4L2_CID_FOCUS_ABSOLUTE, focus_absolute, "focus_absolute");
00119   }
00120 
00121   bool auto_exposure;
00122   if (pnode.getParam("auto_exposure", auto_exposure)) {
00123     int val;
00124     if (auto_exposure) {
00125       val = V4L2_EXPOSURE_AUTO;
00126     } else {
00127       val = V4L2_EXPOSURE_MANUAL;
00128     }
00129     cam_left->set_v4l2_control(V4L2_CID_EXPOSURE_AUTO, val, "auto_exposure");
00130     cam_right->set_v4l2_control(V4L2_CID_EXPOSURE_AUTO, val, "auto_exposure");
00131   }
00132 
00133   int exposure_absolute;
00134   if (pnode.getParam("exposure_absolute", exposure_absolute)) {
00135     cam_left->set_v4l2_control(V4L2_CID_EXPOSURE_ABSOLUTE, exposure_absolute, "exposure_absolute");
00136     cam_right->set_v4l2_control(V4L2_CID_EXPOSURE_ABSOLUTE, exposure_absolute, "exposure_absolute");
00137   }
00138 
00139   int brightness;
00140   if (pnode.getParam("brightness", brightness)) {
00141     cam_left->set_v4l2_control(V4L2_CID_BRIGHTNESS, brightness, "brightness");
00142     cam_right->set_v4l2_control(V4L2_CID_BRIGHTNESS, brightness, "brightness");
00143   }
00144 
00145   int power_line_frequency;
00146   if (pnode.getParam("power_line_frequency", power_line_frequency)) {
00147     int val;
00148     if (power_line_frequency == 0) {
00149       val = V4L2_CID_POWER_LINE_FREQUENCY_DISABLED;
00150     } else if (power_line_frequency == 50) {
00151       val = V4L2_CID_POWER_LINE_FREQUENCY_50HZ;
00152     } else if (power_line_frequency == 60) {
00153       val = V4L2_CID_POWER_LINE_FREQUENCY_60HZ;
00154     } else {
00155       printf("power_line_frequency=%d not supported. Using auto.\n", power_line_frequency);
00156       val = V4L2_CID_POWER_LINE_FREQUENCY_AUTO;
00157     }
00158     cam_left->set_v4l2_control(V4L2_CID_POWER_LINE_FREQUENCY, val, "power_line_frequency");
00159     cam_right->set_v4l2_control(V4L2_CID_POWER_LINE_FREQUENCY, val, "power_line_frequency");
00160   }
00161 
00162   // TODO:
00163   // - add params for
00164   //   contrast
00165   //   saturation
00166   //   hue
00167   //   white balance temperature, auto and manual
00168   //   gamma
00169   //   sharpness
00170   //   backlight compensation
00171   //   exposure auto priority
00172   //   zoom
00173   // - add generic parameter list:
00174   //   [(id0, val0, name0), (id1, val1, name1), ...]
00175 
00176 
00177   /* and turn on the streamer */
00178   ok = true;
00179   image_thread = boost::thread(boost::bind(&StereoCamera::feedImages, this));
00180 }
00181 
00182 void StereoCamera::sendInfo(ros::Time time) {
00183   CameraInfoPtr info_left(new CameraInfo(left_info_mgr.getCameraInfo()));
00184   CameraInfoPtr info_right(new CameraInfo(right_info_mgr.getCameraInfo()));
00185 
00186   info_left->header.stamp = time;
00187   info_right->header.stamp = time;
00188   info_left->header.frame_id = frame;
00189   info_right->header.frame_id = frame;
00190 
00191   left_info_pub.publish(info_left);
00192   right_info_pub.publish(info_right);
00193 }
00194 
00195 
00196 void StereoCamera::feedImages() {
00197   unsigned int pair_id = 0;
00198   while (ok) {
00199     unsigned char *frame_left = NULL, *frame_right = NULL;
00200     uint32_t bytes_used_left, bytes_used_right;
00201 
00202     ros::Time capture_time = ros::Time::now();
00203 
00204     int left_idx = cam_left->grab(&frame_left, bytes_used_left);
00205     int right_idx = cam_right->grab(&frame_right, bytes_used_right);
00206 
00207     /* Read in every frame the camera generates, but only send each
00208      * (skip_frames + 1)th frame. This reduces effective frame
00209      * rate, processing time and network usage while keeping the
00210      * images synchronized within the true framerate.
00211      */
00212     if (skip_frames == 0 || frames_to_skip == 0) {
00213       if (frame_left && frame_right) {
00214         ImagePtr image_left(new Image);
00215         ImagePtr image_right(new Image);
00216 
00217         image_left->height = height;
00218         image_left->width = width;
00219         image_left->step = 3 * width;
00220         image_left->encoding = image_encodings::RGB8;
00221         image_left->header.stamp = capture_time;
00222         image_left->header.seq = pair_id;
00223 
00224         image_right->height = height;
00225         image_right->width = width;
00226         image_right->step = 3 * width;
00227         image_right->encoding = image_encodings::RGB8;
00228         image_right->header.stamp = capture_time;
00229         image_right->header.seq = pair_id;
00230 
00231         image_left->header.frame_id = frame;
00232         image_right->header.frame_id = frame;
00233 
00234         image_left->data.resize(image_left->step * image_left->height);
00235         image_right->data.resize(image_right->step * image_right->height);
00236 
00237         if (rotate_left)
00238           rotate(&image_left->data[0], frame_left, width * height);
00239         else
00240           memcpy(&image_left->data[0], frame_left, width * height * 3);
00241 
00242         if (rotate_right)
00243           rotate(&image_right->data[0], frame_right, width * height);
00244         else
00245           memcpy(&image_right->data[0], frame_right, width * height * 3);
00246 
00247         left_pub.publish(image_left);
00248         right_pub.publish(image_right);
00249 
00250         sendInfo(capture_time);
00251 
00252         ++pair_id;
00253       }
00254 
00255       frames_to_skip = skip_frames;
00256     } else {
00257       frames_to_skip--;
00258     }
00259 
00260     if (frame_left)
00261       cam_left->release(left_idx);
00262     if (frame_right)
00263       cam_right->release(right_idx);
00264   }
00265 }
00266 
00267 StereoCamera::~StereoCamera() {
00268   ok = false;
00269   image_thread.join();
00270   if (cam_left)
00271     delete cam_left;
00272   if (cam_right)
00273     delete cam_right;
00274 }
00275 
00276 };


uvc_camera
Author(s): Ken Tossell
autogenerated on Fri Aug 28 2015 10:16:42