$search
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 }