nm33_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <rospack/rospack.h>
00003 #include <sensor_msgs/fill_image.h>
00004 #include <sensor_msgs/SetCameraInfo.h>
00005 #include <image_transport/image_transport.h>
00006 #include <camera_calibration_parsers/parse.h>
00007 
00008 #include <opencv/cv.h>
00009 #include <cv_bridge/cv_bridge.h>
00010 
00011 #include <dynamic_reconfigure/server.h>
00012 #include <opt_camera/OptNM33CameraConfig.h>
00013 #include "opt_nm33_uvc/opt_nm33_camera.h"
00014 
00015 class OptCamNode
00016 {
00017 private:
00018   // camera
00019   OptNM3xCamera *camera_;
00020   std::string serial_id_;
00021 
00022   // ros
00023   ros::NodeHandle &node_;
00024   ros::Time next_time_;
00025   int count_;
00026 
00027   // Publications
00028   sensor_msgs::Image img_, img_omni_, img_wide_, img_middle_, img_narrow_, img_panorama_;
00029   image_transport::CameraPublisher img_pub_, img_omni_pub_, img_wide_pub_, img_middle_pub_, img_narrow_pub_,img_panorama_pub_;
00030   sensor_msgs::CameraInfo info_, info_omni_, info_wide_, info_middle_, info_narrow_, info_panorama_;
00031   ros::ServiceServer info_srv_, info_omni_srv_, info_wide_srv_, info_middle_srv_, info_narrow_srv_, info_panorama_srv_;
00032 
00033   // Dynamic reconfigure
00034   typedef dynamic_reconfigure::Server<opt_camera::OptNM33CameraConfig> ReconfigureServer;
00035   ReconfigureServer reconfigure_server_;
00036   opt_camera::OptNM33CameraConfig config_;
00037 
00038 public:
00039   OptCamNode(ros::NodeHandle &node) : node_(node)
00040   {
00041     // camera device
00042     ros::NodeHandle nh("~");
00043     int camera_index = 0;
00044 
00045     nh.getParam("camera_index", camera_index);
00046     camera_ = new OptNM3xCamera(camera_index);
00047     if ((serial_id_ = camera_->getSerialID()) == "") {
00048       nh.getParam("serial_id", serial_id_);
00049     }
00050     camera_->setSmallHemisphere(1);
00051     camera_->setLocationAbsolute(0, 0, 0, 0,   0);
00052     camera_->setLocationAbsolute(1, 0, 0, 0,  40);
00053     camera_->setLocationAbsolute(2, 0, 0, 0, 120);
00054 
00055     // image header
00056     img_.header.frame_id = std::string("head_camera");
00057     img_omni_.header.frame_id = std::string("head_camera");
00058     img_wide_.header.frame_id = std::string("head_camera");
00059     img_middle_.header.frame_id = std::string("head_camera");
00060     img_narrow_.header.frame_id = std::string("head_camera");
00061     img_panorama_.header.frame_id = std::string("head_camera");
00062     nh.getParam("frame_id", img_.header.frame_id);
00063     nh.getParam("omni_frame_id", img_omni_.header.frame_id);
00064     nh.getParam("wide_frame_id", img_wide_.header.frame_id);
00065     nh.getParam("middle_frame_id", img_middle_.header.frame_id);
00066     nh.getParam("narrow_frame_id", img_narrow_.header.frame_id);
00067     nh.getParam("panorama_frame_id", img_panorama_.header.frame_id);
00068 
00069     // info
00070     info_.header.frame_id = img_.header.frame_id;
00071     info_omni_.header.frame_id = img_omni_.header.frame_id;
00072     info_wide_.header.frame_id = img_wide_.header.frame_id;
00073     info_middle_.header.frame_id = img_middle_.header.frame_id;
00074     info_narrow_.header.frame_id = img_narrow_.header.frame_id;
00075     info_panorama_.header.frame_id = img_panorama_.header.frame_id;
00076 
00077     // advertise
00078     img_pub_ = image_transport::ImageTransport(node).advertiseCamera("image_raw" , 1);
00079     img_omni_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"omni")).advertiseCamera("image_raw" , 1);
00080     img_wide_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"wide")).advertiseCamera("image_raw" , 1);
00081     img_middle_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"middle")).advertiseCamera("image_raw" , 1);
00082     img_narrow_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"narrow")).advertiseCamera("image_raw" , 1);
00083     img_panorama_pub_ = image_transport::ImageTransport(ros::NodeHandle(node,"panorama")).advertiseCamera("image_raw" , 1);
00084 
00085     info_srv_ = node.advertiseService("set_camera_info", &OptCamNode::set_camera_info, this);
00086     info_omni_srv_ = ros::NodeHandle(node,"omni").advertiseService("set_camera_info", &OptCamNode::set_camera_info_omni, this);
00087     info_wide_srv_ = ros::NodeHandle(node,"wide").advertiseService("set_camera_info", &OptCamNode::set_camera_info_wide, this);
00088     info_middle_srv_ = ros::NodeHandle(node,"middle").advertiseService("set_camera_info", &OptCamNode::set_camera_info_middle, this);
00089     info_narrow_srv_ = ros::NodeHandle(node,"narrow").advertiseService("set_camera_info", &OptCamNode::set_camera_info_narrow, this);
00090     info_panorama_srv_ = ros::NodeHandle(node,"panorama").advertiseService("set_camera_info", &OptCamNode::set_camera_info_panorama, this);
00091 
00092     // get camera info
00093     get_camera_info_method("",info_);
00094     get_camera_info_method("-omni",info_omni_);
00095     get_camera_info_method("-wide",info_wide_);
00096     get_camera_info_method("-middle",info_middle_);
00097     get_camera_info_method("-narrow",info_narrow_);
00098     get_camera_info_method("-panorama",info_panorama_);
00099 
00100     // Set up dynamic reconfiguration
00101     ReconfigureServer::CallbackType f = boost::bind(&OptCamNode::config_cb, this, _1, _2);
00102     reconfigure_server_.setCallback(f);
00103 
00104     next_time_ = ros::Time::now();
00105     count_ = 0;
00106   }
00107 
00108   ~OptCamNode()
00109   {
00110   }
00111 
00112   bool take_and_send_image()
00113   {
00114     IplImage *frame = NULL;
00115     ros::Time now;
00116 
00117     if ( ( frame = camera_->queryFrame() ) == NULL ) {
00118       ROS_ERROR_STREAM("[" << serial_id_ << "] cvQueryFrame");
00119       return false;
00120     }
00121     now = ros::Time::now();
00122     img_.header.stamp = now;
00123     img_omni_.header.stamp = now;
00124     img_wide_.header.stamp = now;
00125     img_middle_.header.stamp = now;
00126     img_narrow_.header.stamp = now;
00127     img_panorama_.header.stamp = now;
00128     info_.header.stamp = now;
00129     info_omni_.header.stamp = now;
00130     info_wide_.header.stamp = now;
00131     info_middle_.header.stamp = now;
00132     info_narrow_.header.stamp = now;
00133     info_panorama_.header.stamp = now;
00134 
00135     try {
00136       fillImage(img_, "bgr8", frame->height, frame->width, frame->width * frame->nChannels, frame->imageData);
00137       img_pub_.publish(img_, info_);
00138       {
00139         IplImage *tmp_img_ = camera_->queryOmniFrame();
00140         fillImage(img_omni_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
00141         img_omni_pub_.publish(img_omni_, info_omni_);
00142       }
00143       {
00144         IplImage *tmp_img_ = camera_->queryWideFrame();
00145         fillImage(img_wide_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
00146         img_wide_pub_.publish(img_wide_, info_wide_);
00147       }
00148       {
00149         IplImage *tmp_img_ = camera_->queryMiddleFrame();
00150         fillImage(img_middle_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
00151         img_middle_pub_.publish(img_middle_, info_middle_);
00152       }
00153       {
00154         IplImage *tmp_img_ = camera_->queryNarrowFrame();
00155         fillImage(img_narrow_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
00156         img_narrow_pub_.publish(img_narrow_, info_narrow_);
00157       }
00158       if(config_.mode == 2) { // mode = panorama(2)
00159         IplImage* tmp_img_ = cvCreateImage(cvSize(frame->width*2,frame->height/2),frame->depth,frame->nChannels);
00160         // lower -> left, upper -> right
00161         cvSetImageROI(frame,cvRect(0,frame->height/2,frame->width,frame->height/2));
00162         cvSetImageROI(tmp_img_,cvRect(0,0,frame->width,frame->height/2));
00163         cvCopy(frame,tmp_img_);
00164         cvSetImageROI(frame,cvRect(0,0,frame->width,frame->height/2));
00165         cvSetImageROI(tmp_img_,cvRect(frame->width,0,frame->width,frame->height/2));
00166         cvCopy(frame,tmp_img_);
00167         fillImage(img_panorama_, "bgr8", tmp_img_->height, tmp_img_->width, tmp_img_->width * tmp_img_->nChannels, tmp_img_->imageData);
00168         img_panorama_pub_.publish(img_panorama_, info_panorama_);
00169         cvResetImageROI(frame);
00170         cvReleaseImage(&tmp_img_);
00171       }
00172     }
00173     catch (cv_bridge::Exception& e)
00174       {
00175         ROS_ERROR("Could not convert from opencv to message.");
00176       }
00177     return true;
00178   }
00179 
00180   void config_cb(opt_camera::OptNM33CameraConfig &config, uint32_t level)
00181   {
00182 #define SET_CAMERA(methodname, paramname)         \
00183     if (config_.paramname != config.paramname ) { \
00184       camera_->methodname(config.paramname);      \
00185     }
00186 
00187     SET_CAMERA(setMode, mode);
00188     if ( config.autoexposure ) {
00189       SET_CAMERA(setAutoExposure, autoexposure);
00190     } else {
00191       SET_CAMERA(setExposure, exposure);
00192       SET_CAMERA(setIris, iris);
00193     }
00194     if ( config.mode != 4 && config.mode != 0 ) {
00195       SET_CAMERA(setPanAbsolute,  pan);
00196       SET_CAMERA(setTiltAbsolute, tilt);
00197       SET_CAMERA(setRollAbsolute, roll);
00198       SET_CAMERA(setZoomAbsolute, zoom);
00199     }
00200     if ( config.autoexposure ) {
00201       SET_CAMERA(setBrightness, brightness);
00202     }
00203     SET_CAMERA(setSharpness, sharpness);
00204     if ( ! config.autowhitebalance ) {
00205       SET_CAMERA(setWhitebalance, whitebalance);
00206     } else {
00207       SET_CAMERA(setAutoWhitebalance, autowhitebalance);
00208     }
00209     config.firmwareversion = camera_->getFirmwareVersion();
00210     SET_CAMERA(setFlipScreen, flipscreen);
00211     SET_CAMERA(setSmallHemisphere, smallhemisphere);
00212     SET_CAMERA(setMedianFilter, medianfilter); // ???
00213     SET_CAMERA(setJpegQuality, jpegquality);
00214     config.serialid = serial_id_;
00215     SET_CAMERA(setInfoDisplay, infodisplay);
00216     if ( config.capturefps != config_.capturefps) {
00217       SET_CAMERA(setCaptureFPS, capturefps);
00218     }
00219     //config.actualfps = camera_->getActualFPS();
00220     if ( config.lenstype != config_.lenstype) {
00221       SET_CAMERA(setLensType, lenstype);
00222     }
00223     if ( config.mode == 4 ) {
00224       // right top
00225       //camera_->setPanAbsolute (1,config.camera1_pan);
00226       //camera_->setTiltAbsolute(1,config.camera1_tilt);
00227       //camera_->setRollAbsolute(1,config.camera1_roll);
00228       //camera_->setZoomAbsolute(1,config.camera1_zoom);
00229       if ( ( config.camera1_pan  != config_.camera1_pan ) ||
00230            ( config.camera1_tilt != config_.camera1_tilt ) ||
00231            ( config.camera1_roll != config_.camera1_roll ) ||
00232            ( config.camera1_zoom != config_.camera1_zoom ) ) {
00233         camera_->setLocationAbsolute(1,config.camera1_pan, config.camera1_tilt, config.camera1_roll, config.camera1_zoom);
00234       }
00235 
00236       // left bottom
00237       //camera_->setPanAbsolute (2,config.camera2_pan);
00238       //camera_->setTiltAbsolute(2,config.camera2_tilt);
00239       //camera_->setRollAbsolute(2,config.camera2_roll);
00240       //camera_->setZoomAbsolute(2,config.camera2_zoom);w
00241       if ( ( config.camera2_pan  != config_.camera2_pan ) ||
00242            ( config.camera2_tilt != config_.camera2_tilt ) ||
00243            ( config.camera2_roll != config_.camera2_roll ) ||
00244            ( config.camera2_zoom != config_.camera2_zoom ) ) {
00245         camera_->setLocationAbsolute(2,config.camera2_pan, config.camera2_tilt, config.camera2_roll, config.camera2_zoom);
00246       }
00247 
00248       // right bottom
00249       if ( ( config.camera3_pan  != config_.camera3_pan ) ||
00250            ( config.camera3_tilt != config_.camera3_tilt ) ||
00251            ( config.camera3_roll != config_.camera3_roll ) ||
00252            ( config.camera3_zoom != config_.camera3_zoom ) ) {
00253         camera_->setLocationAbsolute(0,config.camera3_pan, config.camera3_tilt, config.camera3_roll, config.camera3_zoom);
00254       }
00255       //SET_CAMERA(setPanAbsolute,  camera3_pan);
00256       //SET_CAMERA(setTiltAbsolute, camera3_tilt);
00257       //SET_CAMERA(setRollAbsolute, camera3_roll);
00258       //SET_CAMERA(setZoomAbsolute, camera3_zoom);
00259 #if 0
00260       if ( config.camera3_pan != config_.camera3_pan ) {
00261         camera_->setPanAbsolute (0,config.camera3_pan);
00262       }
00263       if ( config.camera3_tilt != config_.camera3_tilt ) {
00264         camera_->setTiltAbsolute(0,config.camera3_tilt);
00265       }
00266       if ( config.camera3_roll != config_.camera3_roll ) {
00267         camera_->setRollAbsolute(0,config.camera3_roll);
00268       }
00269       if ( config.camera3_zoom != config_.camera3_zoom ) {
00270         camera_->setZoomAbsolute(0,config.camera3_zoom);
00271       }
00272 #endif
00273     }
00274     config_ = config;
00275   }
00276 
00277   bool get_camera_info_method(std::string view_name, sensor_msgs::CameraInfo &info) {
00278     std::string cam_name = serial_id_+view_name;
00279     std::string ini_name = cam_name+".ini";
00280     try {
00281 #ifdef ROSPACK_EXPORT
00282       rospack::ROSPack rp;
00283       rospack::Package *p = rp.get_pkg("opt_camera");
00284       if (p!=NULL) ini_name = p->path + "/cfg/" + ini_name;
00285 #else
00286       rospack::Rospack rp;
00287       std::vector<std::string> search_path;
00288       rp.getSearchPathFromEnv(search_path);
00289       rp.crawl(search_path, 1);
00290       std::string path;
00291       if (rp.find("opt_camera",path)==true) ini_name = path + "/cfg" + ini_name;
00292 #endif
00293     } catch (std::runtime_error &e) {
00294     }
00295     if (!camera_calibration_parsers::readCalibration(ini_name, cam_name, info)) {
00296       return false;
00297     }
00298     if ( info.P[0] == 0.0 ) {
00299       ROS_ERROR_STREAM( "Loading wrong camera_info from " << ini_name );
00300       return false;
00301     }
00302     ROS_ERROR_STREAM( "Loading camera_info from " << ini_name << info.P[0] );
00303     return true;
00304   }
00305 
00306 #ifdef ROSPACK_EXPORT
00307 #define set_camera_info_method(set_camera_info_method, view_name)       \
00308   bool set_camera_info_method(sensor_msgs::SetCameraInfo::Request& req, \
00309                               sensor_msgs::SetCameraInfo::Response& rsp) { \
00310     ROS_INFO("New camera info received");                               \
00311     sensor_msgs::CameraInfo &info = req.camera_info;                    \
00312                                                                         \
00313     std::string cam_name = serial_id_+view_name;            \
00314     std::string ini_name = cam_name+".ini";                             \
00315     rospack::ROSPack rp;                                                \
00316     try {                                                               \
00317       rospack::Package *p = rp.get_pkg("opt_camera");                   \
00318       if (p!=NULL) ini_name = p->path + "/cfg/" + ini_name;             \
00319     } catch (std::runtime_error &e) {                                   \
00320     }                                                                   \
00321     if (!camera_calibration_parsers::writeCalibration(ini_name, cam_name.c_str(), info)) { \
00322       rsp.status_message = "Error writing camera_info to " + cam_name + ".ini"; \
00323       rsp.success = false;                                              \
00324     }                                                                   \
00325                                                                         \
00326     rsp.success = true;                                                 \
00327     return true;                                                        \
00328   }
00329 #else
00330 #define set_camera_info_method(set_camera_info_method, view_name)       \
00331   bool set_camera_info_method(sensor_msgs::SetCameraInfo::Request& req, \
00332                               sensor_msgs::SetCameraInfo::Response& rsp) { \
00333     ROS_INFO("New camera info received");                               \
00334     sensor_msgs::CameraInfo &info = req.camera_info;                    \
00335                                                                         \
00336     std::string cam_name = serial_id_+view_name;            \
00337     std::string ini_name = cam_name+".ini";                             \
00338     rospack::Rospack rp;                                                \
00339     try {                                                               \
00340       std::vector<std::string> search_path;                             \
00341       rp.getSearchPathFromEnv(search_path);                             \
00342       rp.crawl(search_path, 1);                                         \
00343       std::string path;                                                 \
00344       if(rp.find("opt_camera", path)==true) ini_name = path + "/cfg/" + ini_name; \
00345     } catch (std::runtime_error &e) {                                   \
00346     }                                                                   \
00347     if (!camera_calibration_parsers::writeCalibration(ini_name, cam_name.c_str(), info)) { \
00348       rsp.status_message = "Error writing camera_info to " + cam_name + ".ini"; \
00349       rsp.success = false;                                              \
00350     }                                                                   \
00351                                                                         \
00352     rsp.success = true;                                                 \
00353     return true;                                                        \
00354   }
00355 #endif
00356 
00357   set_camera_info_method(set_camera_info,"")
00358   set_camera_info_method(set_camera_info_omni,"-omni")
00359   set_camera_info_method(set_camera_info_wide,"-wide")
00360   set_camera_info_method(set_camera_info_middle,"-middle")
00361   set_camera_info_method(set_camera_info_narrow,"-narrow")
00362   set_camera_info_method(set_camera_info_panorama,"-panorama")
00363 
00364   bool spin()
00365   {
00366     while (node_.ok())
00367       {
00368         if (take_and_send_image())
00369           {
00370             count_++;
00371             ros::Time now_time = ros::Time::now();
00372             if (now_time > next_time_) {
00373               std::cout << count_ << " frames/sec at " << now_time << std::endl;
00374               count_ = 0;
00375               next_time_ = next_time_ + ros::Duration(1,0);
00376             }
00377           } else {
00378           ROS_ERROR_STREAM("[" << node_.getNamespace() << "] couldn't take image.");
00379           usleep(1000000);
00380         }
00381         ros::spinOnce();
00382       }
00383     return true;
00384   }
00385 
00386 };
00387 
00388 
00389 int main (int argc, char **argv)
00390 {
00391   ros::init(argc, argv, "nm33_cam");
00392   ros::NodeHandle nh("camera");
00393 
00394   OptCamNode camera(nh);
00395 
00396   camera.spin();
00397 
00398   return 0;
00399 }


opt_camera
Author(s): Kei Okada
autogenerated on Sun Jan 25 2015 12:37:47