00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
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");
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");
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
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
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
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
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
00476
00477 }