uvc_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 using namespace sensor_msgs;
00014 
00015 /* Rotate an 8-bit, 3-channel image by 180 degrees. */
00016 static inline void rotate(unsigned char *dst_chr, unsigned char *src_chr, int pixels) {
00017   struct pixel_t { unsigned char r, g, b; };
00018 
00019   struct pixel_t *src = (pixel_t*) src_chr;
00020   struct pixel_t *dst = &(((pixel_t*) dst_chr)[pixels - 1]);
00021 
00022   for (int i = pixels; i != 0; --i) {
00023     *dst = *src;
00024     src++;
00025     dst--;
00026   }
00027 }
00028 
00029 class UVCStereo {
00030   public:
00031     UVCStereo(int argc, char **argv) : pnode("~"), it(node),
00032       width(640), height(480),
00033       fps(10), skip_frames(0), frames_to_skip(0),
00034       left_device("/dev/video0"), right_device("/dev/video1"),
00035       frame("camera"),
00036       rotate_left(false), rotate_right(false),
00037       left_info_mgr(ros::NodeHandle(node, "left"), "left_camera"),
00038       right_info_mgr(ros::NodeHandle(node, "right"), "right_camera") {
00039 
00040       std::string left_url, right_url;
00041 
00042       pnode.getParam("left/camera_info_url", left_url);
00043       pnode.getParam("right/camera_info_url", right_url);
00044 
00045       left_info_mgr.loadCameraInfo(left_url);
00046       right_info_mgr.loadCameraInfo(right_url);
00047 
00048       pnode.getParam("left/device", left_device);
00049       pnode.getParam("right/device", right_device);
00050 
00051       pnode.getParam("fps", fps);
00052       pnode.getParam("skip_frames", skip_frames);
00053 
00054       pnode.getParam("left/rotate", rotate_left);
00055       pnode.getParam("right/rotate", rotate_right);
00056 
00057       pnode.getParam("width", width);
00058       pnode.getParam("height", height);
00059 
00060       pnode.getParam("frame_id", frame);
00061 
00062       left_pub = it.advertise("left/image_raw", 1);
00063       right_pub = it.advertise("right/image_raw", 1);
00064 
00065       left_info_pub = node.advertise<CameraInfo>("left/camera_info", 1);
00066       right_info_pub = node.advertise<CameraInfo>("right/camera_info", 1);
00067 
00068       cam_left = new uvc_cam::Cam(left_device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00069       cam_left->set_motion_thresholds(100, -1);
00070       cam_right = new uvc_cam::Cam(right_device.c_str(), uvc_cam::Cam::MODE_RGB, width, height, fps);
00071       cam_right->set_motion_thresholds(100, -1);
00072     }
00073 
00074     void sendInfo(ros::Time time) {
00075       CameraInfo info_left = left_info_mgr.getCameraInfo();
00076       CameraInfo info_right = right_info_mgr.getCameraInfo();
00077 
00078       info_left.header.stamp = info_right.header.stamp = time;
00079       info_left.header.frame_id = frame;
00080       info_right.header.frame_id = frame;
00081 
00082       left_info_pub.publish(info_left);
00083       right_info_pub.publish(info_right);
00084     }
00085 
00086     void feedImages() {
00087       unsigned int pair_id = 0;
00088       while (node.ok()) {
00089         unsigned char *frame_left = NULL, *frame_right = NULL;
00090         uint32_t bytes_used_left, bytes_used_right;
00091 
00092         ros::Time capture_time = ros::Time::now();
00093 
00094         int left_idx = cam_left->grab(&frame_left, bytes_used_left);
00095         int right_idx = cam_right->grab(&frame_right, bytes_used_right);
00096 
00097         /* Read in every frame the camera generates, but only send each
00098          * (skip_frames + 1)th frame. This reduces effective frame
00099          * rate, processing time and network usage while keeping the
00100          * images synchronized within the true framerate.
00101          */
00102         if (skip_frames == 0 || frames_to_skip == 0) {
00103           if (frame_left && frame_right) {
00104              Image image_left, image_right;
00105 
00106              image_left.height = height;
00107              image_left.width = width;
00108              image_left.step = 3 * width;
00109              image_left.encoding = image_encodings::RGB8;
00110 
00111              image_right = image_left;
00112 
00113              image_left.header.stamp = capture_time;
00114              image_left.header.seq = pair_id;
00115              image_right.header.stamp = capture_time;
00116              image_right.header.seq = pair_id;
00117 
00118              image_left.header.frame_id = frame;
00119              image_right.header.frame_id = frame;
00120 
00121              image_left.data.resize(image_left.step * image_left.height);
00122              image_right.data.resize(image_right.step * image_right.height);
00123 
00124              if (rotate_left)
00125                rotate(&image_left.data[0], frame_left, width*height);
00126              else
00127                memcpy(&image_left.data[0], frame_left, width*height * 3);
00128 
00129              if (rotate_right)
00130                rotate(&image_right.data[0], frame_right, width*height);
00131              else
00132                memcpy(&image_right.data[0], frame_right, width*height * 3);
00133 
00134              left_pub.publish(image_left);
00135              right_pub.publish(image_right);
00136 
00137              sendInfo(capture_time);
00138 
00139              ++pair_id;
00140           }
00141 
00142           frames_to_skip = skip_frames;
00143         } else {
00144           frames_to_skip--;
00145         }
00146 
00147         if (frame_left) cam_left->release(left_idx);
00148         if (frame_right) cam_right->release(right_idx);
00149       }
00150     }
00151 
00152     ~UVCStereo() {
00153       delete cam_left;
00154       delete cam_right;
00155     }
00156 
00157   private:
00158     ros::NodeHandle node, pnode;
00159     image_transport::ImageTransport it;
00160 
00161     uvc_cam::Cam *cam_left, *cam_right;
00162     int width, height, fps, skip_frames, frames_to_skip;
00163     std::string left_device, right_device, frame;
00164     bool rotate_left, rotate_right;
00165 
00166     camera_info_manager::CameraInfoManager left_info_mgr, right_info_mgr;
00167 
00168     image_transport::Publisher left_pub, right_pub;
00169     ros::Publisher left_info_pub, right_info_pub;
00170 
00171 };
00172 
00173 int main (int argc, char **argv) {
00174   ros::init(argc, argv, "uvc_stereo");
00175 
00176   UVCStereo uvc_node(argc, argv);
00177 
00178   boost::thread image_thread(boost::bind(&UVCStereo::feedImages, &uvc_node));
00179 
00180   ROS_WARN("This package has been replaced. Please use the "
00181       "uvc_camera package's stereo_node or StereoNodelet instead.");
00182 
00183   ros::spin();
00184 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


uvc_stereo
Author(s): Ken Tossell/ktossell@umd.edu
autogenerated on Thu Aug 1 2013 11:04:20