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 namespace uvc_camera { 00014 00015 class StereoCamera { 00016 public: 00017 StereoCamera(ros::NodeHandle comm_nh, ros::NodeHandle param_nh); 00018 void onInit(); 00019 void sendInfo(ros::Time time); 00020 void feedImages(); 00021 ~StereoCamera(); 00022 00023 private: 00024 ros::NodeHandle node, pnode; 00025 image_transport::ImageTransport it; 00026 bool ok; 00027 00028 uvc_cam::Cam *cam_left, *cam_right; 00029 int width, height, fps, skip_frames, frames_to_skip; 00030 std::string left_device, right_device, frame; 00031 bool rotate_left, rotate_right; 00032 00033 CameraInfoManager left_info_mgr, right_info_mgr; 00034 00035 image_transport::Publisher left_pub, right_pub; 00036 ros::Publisher left_info_pub, right_info_pub; 00037 00038 boost::thread image_thread; 00039 }; 00040 00041 }; 00042