00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <camera_info_manager/camera_info_manager.h>
00023 #include <tuw_uvc/uvc_defaults.h>
00024 #include <tuw_uvc/uvc_ros.h>
00025
00026 #include <boost/interprocess/sync/scoped_lock.hpp>
00027 #include "luvcview/v4l2uvc.h"
00028 #include <cv_bridge/cv_bridge.h>
00029
00030 typedef boost::interprocess::scoped_lock<boost::interprocess::interprocess_mutex> Lock;
00031
00032 extern "C" {
00033 unsigned int Pyuv422torgb24(unsigned char *input_ptr, unsigned char *output_ptr, unsigned int image_width, unsigned int image_height);
00034 unsigned int Pyuv422tobgr24(unsigned char *input_ptr, unsigned char *output_ptr, unsigned int image_width, unsigned int image_height);
00035 unsigned int Pyuv422togray8(unsigned char *input_ptr, unsigned char *output_ptr, unsigned int image_width, unsigned int image_height);
00036 }
00037
00038
00039 V4RCamNode::~V4RCamNode()
00040 {
00041 show_camera_image_ = false;
00042 boost::this_thread::sleep(boost::posix_time::milliseconds(100));
00043 }
00044
00045 V4RCamNode::V4RCamNode(ros::NodeHandle &n)
00046 : n_(n), n_param_("~"), imageTransport_(n_param_), generate_dynamic_reconfigure_(false), show_camera_image_(false), camera_freeze_(false), queueRosParamToV4LCommit_(0), showCameraImageThreadActive_(false)
00047 {
00048
00049
00050 cameraPublisher_ = imageTransport_.advertiseCamera("image_raw", 1);
00051 cameraThumbnailPublisher_ = imageTransport_.advertise("image_thumbnail", 1);
00052 set_camera_info_srv_ = n_param_.advertiseService("set_camera_info", &V4RCamNode::setCameraInfo, this);
00053
00054 readInitParams();
00055 initCamera();
00056 detectControlEnties();
00057 commitRosParamsToV4L(true);
00058 for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00059 ROS_INFO_STREAM(controlEntries_[i]->getQueryCtrlInfo());
00060 }
00061 }
00062
00063 void V4RCamNode::commitRosParamsToV4L(bool force)
00064 {
00065 if((!queueRosParamToV4LCommit_) && (!force)) return;
00066 bool tmp;
00067 n_param_.getParam("show_camera_image", tmp);
00068 if(show_camera_image_ != tmp) {
00069 show_camera_image_ = tmp;
00070 if(show_camera_image_) {
00071 showCameraImageThread_ = boost::thread(&V4RCamNode::showCameraImage, this);
00072 ROS_INFO("start show camera image thread");
00073 }
00074 }
00075 for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00076 const std::string &name = controlEntries_[i]->varName;
00077 int default_value = controlEntries_[i]->queryctrl->default_value;
00078 int &target_value = controlEntries_[i]->targetValue;
00079 if(controlEntries_[i]->queryctrl->type == V4L2_CTRL_TYPE_BOOLEAN) {
00080 n_param_.param<bool>(name, tmp, (bool) default_value);
00081 target_value = tmp;
00082 } else {
00083 n_param_.param<int>(name, target_value, default_value);
00084 }
00085 v4lset(controlEntries_[i]);
00086 if(controlEntries_[i]->hasErrorMsg()) ROS_ERROR_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullErrorMsg());
00087 if(controlEntries_[i]->hasInfoMsg()) ROS_INFO_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullInfoMsg());
00088 }
00089
00090 queueRosParamToV4LCommit_ = false;
00091 }
00092 void V4RCamNode::commitV4LToRosParams()
00093 {
00094 for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00095 v4lget(controlEntries_[i]);
00096 if(controlEntries_[i]->queryctrl->type == V4L2_CTRL_TYPE_BOOLEAN) {
00097 n_param_.setParam(controlEntries_[i]->varName, (bool) controlEntries_[i]->currentValue);
00098 std::cout << controlEntries_[i]->varName << ": bool " << controlEntries_[i]->currentValue << std::endl;
00099 } else {
00100 n_param_.setParam(controlEntries_[i]->varName, (int) controlEntries_[i]->currentValue);
00101 std::cout << controlEntries_[i]->varName << ": int " << controlEntries_[i]->currentValue << std::endl;
00102 }
00103 if(controlEntries_[i]->hasErrorMsg()) ROS_ERROR_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullErrorMsg());
00104 if(controlEntries_[i]->hasInfoMsg()) ROS_INFO_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullInfoMsg());
00105 }
00106 }
00107 void V4RCamNode::readInitParams()
00108 {
00109 double tmp;
00110 ROS_INFO("V4RCamNode::readParams()");
00111 n_param_.param<bool>("show_camera_image", show_camera_image_, DEFAULT_SHOW_CAMERA_IMAGE);
00112 ROS_INFO("show_camera_image: %s", ((show_camera_image_) ? "true" : "false"));
00113 n_param_.param<bool>("camera_freeze", camera_freeze_, DEFAULT_CAMERA_FREEZE);
00114 ROS_INFO("camera_freeze: %s", ((camera_freeze_) ? "true" : "false"));
00115
00116 n_param_.param<std::string>("video_device", videoDevice_, DEFAULT_VIDEODEVICE);
00117 ROS_INFO("video_device: %s", videoDevice_.c_str());
00118 n_param_.param<std::string>("avi_filename", aviFilename_, DEFAULT_AVIFILENAME);
00119 ROS_INFO("avi_filename: %s", aviFilename_.c_str());
00120 n_param_.param<int>("convert_image_", convert_image_, DEFAULT_CONVERT_IMAGE);
00121 ROS_INFO("convert_image: %i", convert_image_);
00122 n_param_.param<int>("ratioThumbnail", ratioThumbnail_, DEFAULT_RATIO_THUMBNAIL);
00123 ROS_INFO("ratioThumbnail: %i", ratioThumbnail_);
00124 n_param_.param<int>("width", width_, DEFAULT_WIDTH);
00125 ROS_INFO("width: %i", width_);
00126 n_param_.param<int>("height", height_, DEFAULT_HEIGHT);
00127 ROS_INFO("height: %i", height_);
00128 n_param_.param<double>("fps", tmp, DEFAULT_FPS);
00129 fps_ = tmp;
00130 ROS_INFO("fps: %4.3f", fps_);
00131
00132 std::string camera_info_url;
00133 if(n_param_.getParam("camera_info_url", camera_info_url)) {
00134 camera_info_manager::CameraInfoManager cinfo(n_param_);
00135 if(cinfo.validateURL(camera_info_url)) {
00136 cinfo.loadCameraInfo(camera_info_url);
00137 cameraInfo_ = cinfo.getCameraInfo();
00138 } else {
00139 ROS_FATAL("camera_info_url not valid.");
00140 n_param_.shutdown();
00141 return;
00142 }
00143 } else {
00144 XmlRpc::XmlRpcValue double_list;
00145 n_param_.getParam("K", double_list);
00146 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 9)) {
00147 for(int i = 0; i < 9; i++) {
00148 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00149 cameraInfo_.K[i] = double_list[i];
00150 }
00151 }
00152
00153 n_param_.getParam("D", double_list);
00154 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 5)) {
00155 for(int i = 0; i < 5; i++) {
00156 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00157 cameraInfo_.D[i] = double_list[i];
00158 }
00159 }
00160
00161 n_param_.getParam("R", double_list);
00162 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 9)) {
00163 for(int i = 0; i < 9; i++) {
00164 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00165 cameraInfo_.R[i] = double_list[i];
00166 }
00167 }
00168
00169 n_param_.getParam("P", double_list);
00170 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 12)) {
00171 for(int i = 0; i < 12; i++) {
00172 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00173 cameraInfo_.P[i] = double_list[i];
00174 }
00175 }
00176 }
00177 ROS_INFO("tf_camera_id: %s", cameraInfo_.header.frame_id.c_str());
00178 n_param_.param<std::string>("frame_id", cameraInfo_.header.frame_id, DEFAULT_FRAME_ID);
00179 ROS_INFO("frame_id: %s", cameraInfo_.header.frame_id.c_str());
00180 cameraInfo_.header.seq = 0;
00181 }
00182
00183 void V4RCamNode::publishCamera()
00184 {
00185 bool freeze = camera_freeze_ && (cameraImage_.header.seq > 100);
00186 cameraInfo_.header.stamp.sec = timeLastFrame_.tv_sec;
00187 cameraInfo_.header.stamp.nsec = timeLastFrame_.tv_usec * 1000;
00188 cameraImage_.header = cameraInfo_.header;
00189 if( !freeze) {
00190 cameraImage_.height = cameraInfo_.height = pVideoIn_->height;
00191 cameraImage_.width = cameraInfo_.width = pVideoIn_->width;
00192 cameraImage_.is_bigendian = true;
00193 cameraImage_.step = cameraInfo_.width * 2;
00194 switch(convert_image_) {
00195 case CONVERT_RAW:
00196 cameraImage_.encoding = "yuvu";
00197 cameraImage_.data.resize(pVideoIn_->framesizeIn);
00198 memcpy(&cameraImage_.data[0], pVideoIn_->framebuffer, cameraImage_.data.size());
00199 break;
00200 case CONVERT_YUV422toRGB:
00201 cameraImage_.encoding = "rgb8";
00202 cameraImage_.data.resize(width_ * height_ * 3);
00203 cameraImage_.step = cameraInfo_.width * 3;
00204 Pyuv422torgb24(pVideoIn_->framebuffer, &cameraImage_.data[0], width_, height_);
00205 break;
00206 case CONVERT_YUV422toBGR:
00207 cameraImage_.encoding = "bgr8";
00208 cameraImage_.data.resize(width_ * height_ * 3);
00209 cameraImage_.step = cameraInfo_.width * 3;
00210 Pyuv422tobgr24(pVideoIn_->framebuffer, &cameraImage_.data[0], width_, height_);
00211 break;
00212 case CONVERT_YUV422toGray:
00213 cameraImage_.encoding = "mono8";
00214 cameraImage_.data.resize(width_ * height_);
00215 cameraImage_.step = cameraInfo_.width * 1;
00216 Pyuv422togray8(pVideoIn_->framebuffer, &cameraImage_.data[0], width_, height_);
00217 break;
00218 default:
00219 cameraImage_.encoding = "yuv422";
00220 cameraImage_.data.resize(width_ * height_ * 2);
00221 memcpy(&cameraImage_.data[0], pVideoIn_->framebuffer, cameraImage_.data.size());
00222 }
00223 }
00224 cameraPublisher_.publish(cameraImage_, cameraInfo_);
00225
00226 if(cameraThumbnailPublisher_.getNumSubscribers() > 0) {
00227 cameraThumbnail_.header = cameraInfo_.header;
00228 if( !freeze) {
00229 cameraThumbnail_.height = cameraInfo_.height = pVideoIn_->height / ratioThumbnail_;
00230 cameraThumbnail_.width = cameraInfo_.width = pVideoIn_->width / ratioThumbnail_;
00231 cameraThumbnail_.is_bigendian = true;
00232 cameraThumbnail_.step = cameraInfo_.width;
00233 cameraThumbnail_.encoding = "mono8";
00234 cameraThumbnail_.data.resize(cameraThumbnail_.width * cameraThumbnail_.height);
00235 unsigned char *des = (unsigned char *) &cameraThumbnail_.data[0];
00236 for(unsigned int h = 0; h < cameraThumbnail_.height; h++) {
00237 unsigned char *src = (unsigned char *) pVideoIn_->framebuffer + h*ratioThumbnail_ * pVideoIn_->width*2;
00238 for(unsigned int w = 0; w < cameraThumbnail_.width; w++) {
00239 *des++ = src[w * 2*ratioThumbnail_];
00240 }
00241 }
00242 cameraThumbnailPublisher_.publish(cameraThumbnail_);
00243 }
00244 }
00245
00246 commitRosParamsToV4L();
00247 cameraInfo_.header.seq ++;
00248 if(show_camera_image_ && showCameraImageThreadActive_ == false){
00249 showCameraImageThread_ = boost::thread(&V4RCamNode::showCameraImage, this);
00250 }
00251 }
00252
00253
00254 void V4RCamNode::showCameraImage()
00255 {
00256 std::string window_name = ros::NodeHandle ( "~" ).getNamespace();
00257 timeval lastShownFrame = timeLastFrame_;
00258 double lastDuration = durationLastFrame_;
00259 cv::Mat img;
00260 int key = -1;
00261 showCameraImageThreadActive_ = true;
00262 do {
00263 if((lastShownFrame.tv_sec != timeLastFrame_.tv_sec) || (lastShownFrame.tv_usec != timeLastFrame_.tv_usec)) {
00264 img.create(height_, width_, CV_8UC3);
00265 lastShownFrame = timeLastFrame_;
00266 std::stringstream ss;
00267 ss << 1000.0 / ((durationLastFrame_ + lastDuration) / 2.0);
00268 lastDuration = durationLastFrame_;
00269 Lock myLock(mutexImage_);
00270 if(!camera_freeze_){
00271 Pyuv422tobgr24(pVideoIn_->framebuffer, img.data, width_, height_);
00272 cv::putText(img, ss.str(), cv::Point(10, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar::all(0), 1, CV_AA);
00273 }
00274 cv::imshow(window_name, img);
00275 }
00276 key = cv::waitKey(50);
00277 } while((key == -1) && show_camera_image_ && showCameraImageThreadActive_);
00278
00279 cv::putText(img, "OFFLINE", cv::Point(img.cols/4, img.rows/4), cv::FONT_HERSHEY_PLAIN, 5, cv::Scalar::all(0), 3, CV_AA);
00280 cv::imshow(window_name, img);
00281 cv::waitKey(50);
00282 cv::destroyWindow(window_name);
00283 cv::waitKey(50);
00284 showCameraImageThreadActive_ = false;
00285 if(key != -1) {
00286 pVideoIn_->signalquit = 0;
00287 }
00288 }
00289
00290 void V4RCamNode::callbackSphere (const tuw_uvc::SphereConstPtr& msg){
00291
00292 ROS_INFO("action %s, pitch = %4.2f, yaw = %4.2f", msg->action.c_str(), msg->pitch, msg->yaw);
00293
00294 }
00295
00296
00297 bool V4RCamNode::setCameraInfo(sensor_msgs::SetCameraInfoRequest &req, sensor_msgs::SetCameraInfoResponse &rsp){
00298 ROS_INFO("New camera info received");
00299 sensor_msgs::CameraInfo &info = req.camera_info;
00300
00301 if (info.width != cameraInfo_.width || info.height != cameraInfo_.height)
00302 {
00303 rsp.success = false;
00304 rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
00305 "setting, camera running at resolution %ix%i.")
00306 % info.width % info.height % cameraInfo_.width % cameraInfo_.height).str();
00307 ROS_ERROR("%s", rsp.status_message.c_str());
00308 return true;
00309 }
00310
00311 cameraInfo_ = info;
00312
00313 return true;
00314 }