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 <v4r_uvc/v4r_uvc_defaults.h>
00024 #include <v4r_uvc/v4r_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 n_param_.setParam("show_camera_image", camera_freeze_);
00050 cameraPublisher_ = imageTransport_.advertiseCamera("image_raw", 1);
00051 cameraThumbnailPublisher_ = imageTransport_.advertise("image_thumbnail", 1);
00052
00053 readInitParams();
00054 initCamera();
00055 detectControlEnties();
00056 commitRosParamsToV4L(true);
00057 for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00058 ROS_INFO_STREAM(controlEntries_[i]->getQueryCtrlInfo());
00059 }
00060 }
00061
00062 void V4RCamNode::commitRosParamsToV4L(bool force)
00063 {
00064 if((!queueRosParamToV4LCommit_) && (!force)) return;
00065 bool tmp;
00066 n_param_.getParam("show_camera_image", tmp);
00067 if(show_camera_image_ != tmp) {
00068 show_camera_image_ = tmp;
00069 if(show_camera_image_) {
00070 showCameraImageThread_ = boost::thread(&V4RCamNode::showCameraImage, this);
00071 ROS_INFO("start show camera image thread");
00072 }
00073 }
00074 for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00075 const std::string &name = controlEntries_[i]->varName;
00076 int default_value = controlEntries_[i]->queryctrl->default_value;
00077 int &target_value = controlEntries_[i]->targetValue;
00078 if(controlEntries_[i]->queryctrl->type == V4L2_CTRL_TYPE_BOOLEAN) {
00079 n_param_.param<bool>(name, tmp, (bool) default_value);
00080 target_value = tmp;
00081 } else {
00082 n_param_.param<int>(name, target_value, default_value);
00083 }
00084 v4lset(controlEntries_[i]);
00085 if(controlEntries_[i]->hasErrorMsg()) ROS_ERROR_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullErrorMsg());
00086 if(controlEntries_[i]->hasInfoMsg()) ROS_INFO_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullInfoMsg());
00087 }
00088
00089 queueRosParamToV4LCommit_ = false;
00090 }
00091 void V4RCamNode::commitV4LToRosParams()
00092 {
00093 for(unsigned int i = 0; i < controlEntries_.size(); i++) {
00094 v4lget(controlEntries_[i]);
00095 if(controlEntries_[i]->queryctrl->type == V4L2_CTRL_TYPE_BOOLEAN) {
00096 n_param_.setParam(controlEntries_[i]->varName, (bool) controlEntries_[i]->currentValue);
00097 std::cout << controlEntries_[i]->varName << ": bool " << controlEntries_[i]->currentValue << std::endl;
00098 } else {
00099 n_param_.setParam(controlEntries_[i]->varName, (int) controlEntries_[i]->currentValue);
00100 std::cout << controlEntries_[i]->varName << ": int " << controlEntries_[i]->currentValue << std::endl;
00101 }
00102 if(controlEntries_[i]->hasErrorMsg()) ROS_ERROR_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullErrorMsg());
00103 if(controlEntries_[i]->hasInfoMsg()) ROS_INFO_STREAM(controlEntries_[i]->varName << ": " << controlEntries_[i]->pullInfoMsg());
00104 }
00105 }
00106 void V4RCamNode::readInitParams()
00107 {
00108 double tmp;
00109 ROS_INFO("V4RCamNode::readParams()");
00110 n_param_.param<bool>("show_camera_image", show_camera_image_, DEFAULT_SHOW_CAMERA_IMAGE);
00111 ROS_INFO("show_camera_image: %s", ((show_camera_image_) ? "true" : "false"));
00112 n_param_.param<bool>("camera_freeze", camera_freeze_, DEFAULT_CAMERA_FREEZE);
00113 ROS_INFO("camera_freeze: %s", ((camera_freeze_) ? "true" : "false"));
00114
00115 n_param_.param<std::string>("video_device", videoDevice_, DEFAULT_VIDEODEVICE);
00116 ROS_INFO("video_device: %s", videoDevice_.c_str());
00117 n_param_.param<std::string>("avi_filename", aviFilename_, DEFAULT_AVIFILENAME);
00118 ROS_INFO("avi_filename: %s", aviFilename_.c_str());
00119 n_param_.param<int>("convert_image_", convert_image_, DEFAULT_CONVERT_IMAGE);
00120 ROS_INFO("convert_image: %i", convert_image_);
00121 n_param_.param<int>("ratioThumbnail", ratioThumbnail_, DEFAULT_RATIO_THUMBNAIL);
00122 ROS_INFO("ratioThumbnail: %i", ratioThumbnail_);
00123 n_param_.param<int>("width", width_, DEFAULT_WIDTH);
00124 ROS_INFO("width: %i", width_);
00125 n_param_.param<int>("height", height_, DEFAULT_HEIGHT);
00126 ROS_INFO("height: %i", height_);
00127 n_param_.param<double>("fps", tmp, DEFAULT_FPS);
00128 fps_ = tmp;
00129 ROS_INFO("fps: %4.3f", fps_);
00130
00131 std::string camera_info_url;
00132 if(n_param_.getParam("camera_info_url", camera_info_url)) {
00133 camera_info_manager::CameraInfoManager cinfo(n_param_);
00134 if(cinfo.validateURL(camera_info_url)) {
00135 cinfo.loadCameraInfo(camera_info_url);
00136 cameraInfo_ = cinfo.getCameraInfo();
00137 } else {
00138 ROS_FATAL("camera_info_url not valid.");
00139 n_param_.shutdown();
00140 return;
00141 }
00142 } else {
00143 XmlRpc::XmlRpcValue double_list;
00144 n_param_.getParam("K", double_list);
00145 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 9)) {
00146 for(int i = 0; i < 9; i++) {
00147 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00148 cameraInfo_.K[i] = double_list[i];
00149 }
00150 }
00151
00152 n_param_.getParam("D", double_list);
00153 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 5)) {
00154 for(int i = 0; i < 5; i++) {
00155 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00156 cameraInfo_.D[i] = double_list[i];
00157 }
00158 }
00159
00160 n_param_.getParam("R", double_list);
00161 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 9)) {
00162 for(int i = 0; i < 9; i++) {
00163 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00164 cameraInfo_.R[i] = double_list[i];
00165 }
00166 }
00167
00168 n_param_.getParam("P", double_list);
00169 if((double_list.getType() == XmlRpc::XmlRpcValue::TypeArray) && (double_list.size() == 12)) {
00170 for(int i = 0; i < 12; i++) {
00171 ROS_ASSERT(double_list[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00172 cameraInfo_.P[i] = double_list[i];
00173 }
00174 }
00175 }
00176 ROS_INFO("tf_camera_id: %s", cameraInfo_.header.frame_id.c_str());
00177 n_param_.param<std::string>("frame_id", cameraInfo_.header.frame_id, DEFAULT_FRAME_ID);
00178 ROS_INFO("frame_id: %s", cameraInfo_.header.frame_id.c_str());
00179 cameraInfo_.header.seq = 0;
00180 }
00181
00182 void V4RCamNode::publishCamera()
00183 {
00184 bool freeze = camera_freeze_ && (cameraImage_.header.seq > 100);
00185 cameraInfo_.header.stamp.sec = timeLastFrame_.tv_sec;
00186 cameraInfo_.header.stamp.nsec = timeLastFrame_.tv_usec * 1000;
00187 cameraImage_.header = cameraInfo_.header;
00188 if( !freeze) {
00189 cameraImage_.height = cameraInfo_.height = pVideoIn_->height;
00190 cameraImage_.width = cameraInfo_.width = pVideoIn_->width;
00191 cameraImage_.is_bigendian = true;
00192 cameraImage_.step = cameraInfo_.width * 2;
00193 switch(convert_image_) {
00194 case CONVERT_RAW:
00195 cameraImage_.encoding = "yuvu";
00196 cameraImage_.data.resize(pVideoIn_->framesizeIn);
00197 memcpy(&cameraImage_.data[0], pVideoIn_->framebuffer, cameraImage_.data.size());
00198 break;
00199 case CONVERT_YUV422toRGB:
00200 cameraImage_.encoding = "rgb8";
00201 cameraImage_.data.resize(width_ * height_ * 3);
00202 cameraImage_.step = cameraInfo_.width * 3;
00203 Pyuv422torgb24(pVideoIn_->framebuffer, &cameraImage_.data[0], width_, height_);
00204 break;
00205 case CONVERT_YUV422toBGR:
00206 cameraImage_.encoding = "bgr8";
00207 cameraImage_.data.resize(width_ * height_ * 3);
00208 cameraImage_.step = cameraInfo_.width * 3;
00209 Pyuv422tobgr24(pVideoIn_->framebuffer, &cameraImage_.data[0], width_, height_);
00210 break;
00211 case CONVERT_YUV422toGray:
00212 cameraImage_.encoding = "mono8";
00213 cameraImage_.data.resize(width_ * height_);
00214 cameraImage_.step = cameraInfo_.width * 1;
00215 Pyuv422togray8(pVideoIn_->framebuffer, &cameraImage_.data[0], width_, height_);
00216 break;
00217 default:
00218 cameraImage_.encoding = "yuv422";
00219 cameraImage_.data.resize(width_ * height_ * 2);
00220 memcpy(&cameraImage_.data[0], pVideoIn_->framebuffer, cameraImage_.data.size());
00221 }
00222 }
00223 cameraPublisher_.publish(cameraImage_, cameraInfo_);
00224
00225 if(cameraThumbnailPublisher_.getNumSubscribers() > 0) {
00226 cameraThumbnail_.header = cameraInfo_.header;
00227 if( !freeze) {
00228 cameraThumbnail_.height = cameraInfo_.height = pVideoIn_->height / ratioThumbnail_;
00229 cameraThumbnail_.width = cameraInfo_.width = pVideoIn_->width / ratioThumbnail_;
00230 cameraThumbnail_.is_bigendian = true;
00231 cameraThumbnail_.step = cameraInfo_.width;
00232 cameraThumbnail_.encoding = "mono8";
00233 cameraThumbnail_.data.resize(cameraThumbnail_.width * cameraThumbnail_.height);
00234 unsigned char *des = (unsigned char *) &cameraThumbnail_.data[0];
00235 for(unsigned int h = 0; h < cameraThumbnail_.height; h++) {
00236 unsigned char *src = (unsigned char *) pVideoIn_->framebuffer + h*ratioThumbnail_ * pVideoIn_->width*2;
00237 for(unsigned int w = 0; w < cameraThumbnail_.width; w++) {
00238 *des++ = src[w * 2*ratioThumbnail_];
00239 }
00240 }
00241 cameraThumbnailPublisher_.publish(cameraThumbnail_);
00242 }
00243 }
00244
00245 commitRosParamsToV4L();
00246 cameraInfo_.header.seq ++;
00247 if(show_camera_image_ && showCameraImageThreadActive_ == false){
00248 showCameraImageThread_ = boost::thread(&V4RCamNode::showCameraImage, this);
00249 }
00250 }
00251
00252
00253 void V4RCamNode::showCameraImage()
00254 {
00255 timeval lastShownFrame = timeLastFrame_;
00256 double lastDuration = durationLastFrame_;
00257 cv::Mat img;
00258 int key = -1;
00259 showCameraImageThreadActive_ = true;
00260 do {
00261 if((lastShownFrame.tv_sec != timeLastFrame_.tv_sec) || (lastShownFrame.tv_usec != timeLastFrame_.tv_usec)) {
00262 img.create(height_, width_, CV_8UC3);
00263 lastShownFrame = timeLastFrame_;
00264 std::stringstream ss;
00265 ss << 1000.0 / ((durationLastFrame_ + lastDuration) / 2.0);
00266 lastDuration = durationLastFrame_;
00267 Lock myLock(mutexImage_);
00268 if(!camera_freeze_){
00269 Pyuv422tobgr24(pVideoIn_->framebuffer, img.data, width_, height_);
00270 cv::putText(img, ss.str(), cv::Point(10, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar::all(0), 1, CV_AA);
00271 }
00272 cv::imshow("Camera", img);
00273 }
00274 key = cv::waitKey(50);
00275 } while((key == -1) && show_camera_image_ && showCameraImageThreadActive_);
00276
00277 cv::putText(img, "OFFLINE", cv::Point(img.cols/4, img.rows/4), cv::FONT_HERSHEY_PLAIN, 5, cv::Scalar::all(0), 3, CV_AA);
00278 cv::imshow("Camera", img);
00279 cv::waitKey(50);
00280 cv::destroyWindow("Camera");
00281 showCameraImageThreadActive_ = false;
00282 if(key != -1) {
00283 pVideoIn_->signalquit = 0;
00284 }
00285 }
00286
00287 void V4RCamNode::callbackSphere (const v4r_uvc::SphereConstPtr& msg){
00288
00289 ROS_INFO("action %s, pitch = %4.2f, yaw = %4.2f", msg->action.c_str(), msg->pitch, msg->yaw);
00290
00291 }