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


opt_camera
Author(s): Kei Okada
autogenerated on Wed Jul 10 2019 03:24:10