ximea_driver.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002 
00003 Copyright 2015  Abdelhamid El-Bably (University of Waterloo)
00004                       [ahelbably@uwaterloo.ca]
00005                 Arun Das (University of Waterloo)
00006                       [adas@uwaterloo.ca]
00007                 Michael Tribou (University of Waterloo)
00008                       [mjtribou@uwaterloo.ca]
00009 
00010 All rights reserved.
00011 
00012 ********************************************************************************/
00013 
00014 #include <ximea_camera/ximea_driver.h>
00015 #include <stdexcept>
00016 #include <sstream>
00017 #include <string>
00018 
00019 ximea_driver::ximea_driver(int serial_no, std::string cam_name)
00020 {
00021   serial_no_ = serial_no;
00022   cam_name_ = cam_name;
00023   assignDefaultValues();
00024 }
00025 
00026 void ximea_driver::assignDefaultValues()
00027 {
00028   cams_on_bus_ = 4;
00029   bandwidth_safety_margin_ = 30;
00030   binning_enabled_ = false;
00031   downsample_factor_ = false;
00032   auto_exposure_ = 0;
00033   auto_exposure_limit_ = 500000;
00034   auto_gain_limit_ = 2;
00035   auto_exposure_priority_ = 0.8;
00036 
00037   exposure_time_ = 1000;
00038   image_data_format_ = "XI_MONO8";
00039   rect_left_ = 0;
00040   rect_top_ = 0;
00041   rect_width_ = 1280;
00042   rect_height_ = 1024;
00043   xiH_ = NULL;
00044   image_.size = sizeof(XI_IMG);
00045   image_.bp = NULL;
00046   image_.bp_size = 0;
00047   acquisition_active_ = false;
00048   image_capture_timeout_ = 1000;
00049   frame_id_ = "";
00050 }
00051 
00052 ximea_driver::ximea_driver(std::string file_name)
00053 {
00054   assignDefaultValues();
00055   readParamsFromFile(file_name);
00056   ROS_INFO_STREAM("ximea_driver: reading paramter values from file: " << file_name);
00057 }
00058 
00059 void ximea_driver::errorHandling(XI_RETURN ret, std::string message)
00060 {
00061   if (ret != XI_OK)
00062   {
00063     std::stringstream errMsg;
00064     std::cout << "Error after "  << message << std::endl;
00065     closeDevice();
00066   }
00067 }
00068 
00069 void ximea_driver::applyParameters()
00070 {
00071   setImageDataFormat(image_data_format_);
00072   if (0 == auto_exposure_) {
00073       setExposure(exposure_time_);
00074   } else {
00075       setAutoExposure(auto_exposure_);
00076       setAutoExposureLimit(auto_exposure_limit_);
00077       setAutoGainLimit(auto_gain_limit_);
00078       setAutoExposurePriority(auto_exposure_priority_);
00079   }
00080   setROI(rect_left_, rect_top_, rect_width_, rect_height_);
00081 }
00082 
00083 void ximea_driver::openDevice()
00084 {
00085   XI_RETURN stat;
00086   if (serial_no_ == 0)
00087   {
00088     stat = xiOpenDevice(0, &xiH_);
00089     errorHandling(stat, "Open Device");
00090   }
00091   else
00092   {
00093     std::stringstream conv;
00094     conv << serial_no_;
00095     stat = xiOpenDeviceBy(XI_OPEN_BY_SN, conv.str().c_str(), &xiH_);
00096     errorHandling(stat, "Open Device");
00097   }
00098   applyParameters();
00099 }
00100 
00101 void ximea_driver::closeDevice()
00102 {
00103   if (xiH_)
00104   {
00105     xiCloseDevice(xiH_);
00106     xiH_ = NULL;
00107   }
00108 }
00109 
00110 void ximea_driver::stopAcquisition()
00111 {
00112   XI_RETURN stat;
00113   if (!hasValidHandle())
00114   {
00115     return;
00116   }
00117   stat = xiStopAcquisition(xiH_);
00118   errorHandling(stat, "Starting Acquisition");  // die if we cannot stop acquisition.
00119   acquisition_active_ = false;
00120 }
00121 
00122 void ximea_driver::startAcquisition()
00123 {
00124   if (!hasValidHandle())
00125   {
00126     return;
00127   }
00128   XI_RETURN stat = xiStartAcquisition(xiH_);
00129   errorHandling(stat, "Starting Acquisition");
00130   acquisition_active_ = true;
00131 }
00132 
00133 void ximea_driver::acquireImage()
00134 {
00135   if (!hasValidHandle())
00136   {
00137     return;
00138   }
00139   XI_RETURN stat = xiGetImage(xiH_, image_capture_timeout_, &image_);
00140   if (stat != 0)
00141   {
00142     std::cout << "Error on" << cam_name_ << " with error " <<  stat << std::endl;
00143   }
00144 }
00145 
00146 void ximea_driver::setImageDataFormat(std::string image_format)
00147 {
00148   XI_RETURN stat;
00149   int image_data_format;
00150 
00151   if (!hasValidHandle())
00152   {
00153     return;
00154   }
00155   if (image_format == std::string("XI_MONO16"))
00156   {
00157     image_data_format = XI_MONO16;
00158   }
00159 
00160   else if (image_format == std::string("XI_RGB24"))
00161   {
00162     image_data_format = XI_RGB24;
00163   }
00164 
00165   else if (image_format == std::string("XI_RGB32"))
00166   {
00167     image_data_format = XI_RGB32;
00168   }
00169 
00170   else if (image_format == std::string("XI_RGB_PLANAR"))
00171   {
00172     image_data_format = XI_MONO8;
00173     std::cout << "This is unsupported in ROS default to XI_MONO8" << std::endl;
00174   }
00175 
00176   else if (image_format == std::string("XI_RAW8"))
00177   {
00178     image_data_format = XI_RAW8;
00179   }
00180 
00181   else if (image_format == std::string("XI_RAW16"))
00182   {
00183     image_data_format = XI_RAW16;
00184   }
00185 
00186   else
00187   {
00188     image_data_format = XI_MONO8;
00189   }
00190 
00191   stat = xiSetParamInt(xiH_, XI_PRM_IMAGE_DATA_FORMAT, image_data_format);
00192   errorHandling(stat, "image_format");  // if we cannot set the format then there is something wrong we should probably quit then
00193   image_data_format_ = image_data_format;
00194 }
00195 
00196 void ximea_driver::setROI(int l, int t, int w, int h)
00197 {
00198   XI_RETURN stat;
00199 
00200   if (!hasValidHandle())
00201   {
00202     return;
00203   }
00204 
00205   if (l < 0 || l > 1280) rect_left_ = 0;
00206   else rect_left_ = l;
00207   if (t < 0 || t > 1024) rect_top_ = 0;
00208   else rect_top_ = t;
00209   if (w < 0 || w > 1280) rect_width_ = 1280;
00210   else rect_width_ = w;
00211   if (h < 0 || h > 1024) rect_height_ = 1024;
00212   else rect_height_ = h;
00213   if (l + w > 1280)
00214   {
00215     rect_left_ =  0;
00216     rect_width_ = 1280;
00217   }
00218   if (h + t > 1024)
00219   {
00220     rect_top_ =  0;
00221     rect_height_ = 1024;
00222   }
00223 
00224   std::cout << rect_height_ << " " << rect_width_ << " " << rect_left_ << " " << rect_top_ << std::endl;
00225 
00226   int tmp;
00227   stat = xiSetParamInt(xiH_, XI_PRM_WIDTH, rect_width_);
00228   errorHandling(stat, "xiSetParamInt (aoi width)");
00229   xiGetParamInt(xiH_, XI_PRM_WIDTH XI_PRM_INFO_INCREMENT, &tmp);
00230   std::cout << "width increment " << tmp << std::endl;
00231 
00232   stat = xiSetParamInt(xiH_, XI_PRM_HEIGHT, rect_height_);
00233   errorHandling(stat, "xiSetParamInt (aoi height)");
00234   xiGetParamInt(xiH_, XI_PRM_HEIGHT XI_PRM_INFO_INCREMENT, &tmp);
00235   std::cout << "height increment " << tmp << std::endl;
00236 
00237   stat = xiSetParamInt(xiH_, XI_PRM_OFFSET_X, rect_left_);
00238   errorHandling(stat, "xiSetParamInt (aoi left)");
00239   xiGetParamInt(xiH_, XI_PRM_OFFSET_X XI_PRM_INFO_INCREMENT, &tmp);
00240   std::cout << "left increment " << tmp << std::endl;
00241 
00242   stat = xiSetParamInt(xiH_, XI_PRM_OFFSET_Y, rect_top_);
00243   errorHandling(stat, "xiSetParamInt (aoi top)");
00244   xiGetParamInt(xiH_, XI_PRM_OFFSET_Y XI_PRM_INFO_INCREMENT, &tmp);
00245   std::cout << "top increment " << tmp << std::endl;
00246 }
00247 
00248 void ximea_driver::setExposure(int time)
00249 {
00250   XI_RETURN stat = xiSetParamInt(xiH_, XI_PRM_EXPOSURE, time);
00251   errorHandling(stat, "xiOSetParamInt (Exposure Time)");
00252   if (!stat)
00253   {
00254     exposure_time_ = time;
00255   }
00256 }
00257 
00258 void ximea_driver::setAutoExposure(int auto_exposure)
00259 {
00260   XI_RETURN stat = xiSetParamInt(xiH_, XI_PRM_AEAG, auto_exposure);
00261   errorHandling(stat, "xiOSetParamInt (AutoExposure Time)");
00262   if (!stat)
00263   {
00264     // auto_exposureme_ = time;
00265   }
00266 }
00267 
00268 void ximea_driver::setAutoExposureLimit(int ae_limit)
00269 {
00270   XI_RETURN stat = xiSetParamFloat(xiH_, XI_PRM_AE_MAX_LIMIT, ae_limit);
00271   errorHandling(stat, "xiOSetParamInt (AutoExposure Limit Time)");
00272   if (!stat)
00273   {
00274     // auto_exposureme_ = time;
00275   }
00276 }
00277 
00278 void ximea_driver::setAutoGainLimit(int ag_limit)
00279 {
00280   XI_RETURN stat = xiSetParamInt(xiH_, XI_PRM_AG_MAX_LIMIT, ag_limit);
00281   errorHandling(stat, "xiOSetParamInt (AutoExposure Limit GAIN)");
00282   if (!stat)
00283   {
00284     // auto_exposureme_ = time;
00285   }
00286 }
00287 
00288 
00289 void ximea_driver::setAutoExposurePriority(float exp_priority)
00290 {
00291   XI_RETURN stat = xiSetParamFloat(xiH_, XI_PRM_EXP_PRIORITY, exp_priority);
00292   errorHandling(stat, "xiOSetParamInt (AutoExposure Priority)");
00293   if (!stat)
00294   {
00295     // auto_exposureme_ = time;
00296   }
00297 }
00298 
00299 
00300 int ximea_driver::readParamsFromFile(std::string file_name)
00301 {
00302   std::ifstream fin(file_name.c_str());
00303   if (fin.fail())
00304   {
00305     ROS_ERROR_STREAM("could not open file " << file_name.c_str() << std::endl);
00306     exit(-1);
00307   }
00308 
00309 
00310   YAML::Node doc = YAML::LoadFile(file_name);
00311   std::string tmpS;
00312   int tmpI1, tmpI2, tmpI3, tmpI4;
00313   bool tmpB;
00314 
00315 
00316   try
00317   {
00318     serial_no_ = doc["serial_no"].as<int>();
00319   }
00320   catch (std::runtime_error) {}
00321   try
00322   {
00323     cam_name_ =  doc["cam_name"].as<std::string>();
00324   }
00325   catch (std::runtime_error) {}
00326   try
00327   {
00328     frame_id_ =  doc["frame_id"].as<std::string>();
00329   }
00330   catch (std::runtime_error) {}
00331 
00332   try
00333   {
00334     yaml_url_ = doc["yaml_url"].as<std::string>();
00335   }
00336   catch (std::runtime_error) {}
00337 
00338   try
00339   {
00340     cams_on_bus_ = doc["cams_on_bus"].as<int>();
00341   }
00342   catch (std::runtime_error) {}
00343 
00344   try
00345   {
00346     bandwidth_safety_margin_ = doc["bandwidth_safety_margin"].as<int>();
00347   }
00348   catch (std::runtime_error) {}
00349 
00350   try
00351   {
00352     frame_rate_ = doc["frame_rate"].as<int>();
00353   }
00354   catch (std::runtime_error) {}
00355 
00356   try
00357   {
00358     exposure_time_ = doc["exposure_time"].as<int>();
00359   }
00360   catch (std::runtime_error) {}
00361 
00362   try
00363   {
00364     auto_exposure_ = doc["auto_exposure"].as<int>();
00365   }
00366   catch (std::runtime_error) {}
00367 
00368   try
00369   {
00370     auto_exposure_limit_ = doc["auto_exposure_limit"].as<int>();
00371   }
00372   catch (std::runtime_error) {}
00373 
00374   try
00375   {
00376     auto_gain_limit_ = doc["auto_gain_limit"].as<int>();
00377   }
00378   catch (std::runtime_error) {}
00379 
00380   try
00381   {
00382     auto_exposure_priority_ = doc["auto_exposure_priority"].as<float>();
00383   }
00384   catch (std::runtime_error) {}
00385 
00386   try
00387   {
00388     binning_enabled_ = doc["binning_enabled"].as<bool>();
00389   }
00390   catch (std::runtime_error) {}
00391 
00392   try
00393   {
00394     downsample_factor_ = doc["downsample_factor_"].as<int>();
00395   }
00396   catch (std::runtime_error) {}
00397 
00398   try
00399   {
00400     rect_left_ = doc["rect_left"].as<int>();
00401   }
00402   catch (std::runtime_error) {}
00403   try
00404   {
00405     rect_top_ = doc["rect_top"].as<int>();
00406   }
00407   catch (std::runtime_error) {}
00408   try
00409   {
00410     rect_width_ = doc["rect_width"].as<int>();
00411   }
00412   catch (std::runtime_error) {}
00413 
00414   try
00415   {
00416     rect_height_ = doc["rect_height"].as<int>();
00417   }
00418   catch (std::runtime_error) {}
00419   setROI(rect_left_, rect_top_, rect_width_, rect_height_);
00420   try
00421   {
00422     image_data_format_ = doc["image_data_format"].as<std::string>();
00423   }
00424   catch (std::runtime_error) {}
00425   setImageDataFormat(image_data_format_);
00426 }
00427 void ximea_driver::enableTrigger(unsigned char trigger_mode)
00428 {
00429   if (!xiH_) return;
00430   trigger_mode_ = trigger_mode;
00431   if (trigger_mode_ > 3 || trigger_mode_ < 0)
00432   {
00433     trigger_mode_ = 0;
00434   }
00435   XI_RETURN stat;
00436   switch (trigger_mode_)
00437   {
00438   case (0):
00439     break;
00440   case (1):
00441     stat = xiSetParamInt(xiH_, XI_PRM_TRG_SOURCE, XI_TRG_SOFTWARE);
00442     errorHandling(stat, "Could not enable software trigger");
00443     break;
00444   case (2):
00445     break;
00446   default:
00447     break;
00448   }
00449 }
00450 
00451 void ximea_driver::triggerDevice()
00452 {
00453   if (!xiH_) return;
00454   XI_RETURN stat;
00455   switch (trigger_mode_)
00456   {
00457   case (0):
00458     break;
00459   case (1):
00460     stat = xiSetParamInt(xiH_, XI_PRM_TRG_SOFTWARE, 1);
00461     errorHandling(stat, "Error During triggering");
00462     std::cout << "triggering " << cam_name_ <<  std::endl;
00463     break;
00464   case (2):
00465     break;
00466   default:
00467     break;
00468   }
00469 }
00470 
00471 void ximea_driver::limitBandwidth(int mbps)
00472 {
00473   if (!xiH_) return;
00474   XI_RETURN stat;
00475   // stat = xiSetParamInt(xiH_, XI_PRM_LIMIT_BANDWIDTH , mbps);
00476   // errorHandling(stat, "could not limit bandwidth");
00477 }


ximea_camera
Author(s): Abdelhamid El-Bably, Arun Das, Michael Tribou
autogenerated on Thu Jun 6 2019 21:17:12