00001 /*************************************************************************** 00002 * Copyright (C) 2012 by Markus Bader * 00003 * markus.bader@tuwien.ac.at * 00004 * * 00005 * This program is free software; you can redistribute it and/or modify * 00006 * it under the terms of the GNU General Public License as published by * 00007 * the Free Software Foundation; either version 2 of the License, or * 00008 * (at your option) any later version. * 00009 * * 00010 * This program is distributed in the hope that it will be useful, * 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 00013 * GNU General Public License for more details. * 00014 * * 00015 * You should have received a copy of the GNU General Public License * 00016 * along with this program; if not, write to the * 00017 * Free Software Foundation, Inc., * 00018 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 00019 ***************************************************************************/ 00020 00021 00022 #ifndef V4R_CAM_NODE_H 00023 #define V4R_CAM_NODE_H 00024 00025 #include <ros/ros.h> 00026 #include <tuw_uvc/uvc.h> 00027 #include <tuw_uvc/Sphere.h> 00028 #include <opencv2/opencv.hpp> 00029 #include <image_transport/image_transport.h> 00030 #include <image_transport/camera_publisher.h> 00031 #include <sensor_msgs/SetCameraInfo.h> 00032 00034 class V4RCamNode : public V4RCam { 00035 public: 00036 static const int CONVERT_RAW = 0; 00037 static const int CONVERT_YUV422toRGB = 1; 00038 static const int CONVERT_YUV422toBGR = 2; 00039 static const int CONVERT_YUV422toGray = 3; 00040 00041 V4RCamNode ( ros::NodeHandle &n ); 00042 ~V4RCamNode(); 00043 void publishCamera(); 00044 void showCameraImage(); 00045 protected: 00046 ros::NodeHandle n_; 00047 ros::NodeHandle n_param_; 00048 image_transport::ImageTransport imageTransport_; 00049 image_transport::CameraPublisher cameraPublisher_; 00050 image_transport::Publisher cameraThumbnailPublisher_; 00051 sensor_msgs::CameraInfo cameraInfo_; 00052 sensor_msgs::Image cameraImage_; 00053 sensor_msgs::Image cameraThumbnail_; 00054 ros::ServiceServer set_camera_info_srv_; 00055 ros::Subscriber subSphere_; 00056 void callbackSphere (const tuw_uvc::SphereConstPtr& msg); 00057 bool setCameraInfo(sensor_msgs::SetCameraInfoRequest &req, sensor_msgs::SetCameraInfoResponse &rsp); 00058 bool generate_dynamic_reconfigure_; 00059 bool show_camera_image_; 00060 bool camera_freeze_; 00061 bool queueRosParamToV4LCommit_; 00062 bool showCameraImageThreadActive_; 00063 boost::thread showCameraImageThread_; 00064 protected: 00065 void readInitParams(); 00066 void commitRosParamsToV4L(bool force = false); 00067 void commitV4LToRosParams(); 00068 void loopCamera(); 00069 int convert_image_; 00070 int ratioThumbnail_; 00071 00076 void readCameraControls(); 00081 void writeCameraControls(); 00082 00087 void readControlEntryInfo ( ControlEntry *entry ); 00092 void updateControlEntry ( ControlEntry *entry ); 00096 void updateDynamicReconfigureFile ( const char* filename ) const; 00097 }; 00098 00099 #endif // V4R_CAM_NODE_H