00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <cmath>
00038 #include "features.h"
00039
00048
00049
00051 namespace
00052 {
00053
00054
00055 static const char *feature_names_[DC1394_FEATURE_NUM] =
00056 {
00057 "brightness",
00058 "exposure",
00059 "sharpness",
00060 "whitebalance",
00061 "hue",
00062 "saturation",
00063 "gamma",
00064 "shutter",
00065 "gain",
00066 "iris",
00067 "focus",
00068 "temperature",
00069 "trigger",
00070 "trigger_delay",
00071 "white_shading",
00072 "frame_rate",
00073 "zoom",
00074 "pan",
00075 "tilt",
00076 "optical_filter",
00077 "capture_size",
00078 "capture_quality"
00079 };
00080
00086 inline const char *featureName(dc1394feature_t feature)
00087 {
00088 if (feature >= DC1394_FEATURE_MIN && feature <= DC1394_FEATURE_MAX)
00089 return feature_names_[feature - DC1394_FEATURE_MIN];
00090 else
00091 return "(unknown)";
00092 }
00093
00094
00095 static const char *mode_names_[DC1394_FEATURE_MODE_NUM] =
00096 {
00097 "Manual",
00098 "Auto",
00099 "OnePush",
00100 };
00101
00107 inline const char *modeName(dc1394feature_mode_t mode)
00108 {
00109 if (mode >= DC1394_FEATURE_MODE_MIN && mode <= DC1394_FEATURE_MODE_MAX)
00110 return mode_names_[mode - DC1394_FEATURE_MODE_MIN];
00111 else
00112 return "(unknown)";
00113 }
00114 }
00115
00117
00119
00124 Features::Features(dc1394camera_t *camera):
00125 camera_(camera)
00126 {}
00127
00137 bool Features::initialize(Config *newconfig)
00138 {
00139
00140 if (DC1394_SUCCESS != dc1394_feature_get_all(camera_, &feature_set_))
00141 {
00142 ROS_ERROR("could not get camera feature information");
00143 return false;
00144 }
00145
00146
00147 configure(DC1394_FEATURE_BRIGHTNESS,
00148 &newconfig->auto_brightness, &newconfig->brightness);
00149 configure(DC1394_FEATURE_EXPOSURE,
00150 &newconfig->auto_exposure, &newconfig->exposure);
00151 configure(DC1394_FEATURE_FOCUS,
00152 &newconfig->auto_focus, &newconfig->focus);
00153 configure(DC1394_FEATURE_GAIN,
00154 &newconfig->auto_gain, &newconfig->gain);
00155 configure(DC1394_FEATURE_GAMMA,
00156 &newconfig->auto_gamma, &newconfig->gamma);
00157 configure(DC1394_FEATURE_HUE,
00158 &newconfig->auto_hue, &newconfig->hue);
00159 configure(DC1394_FEATURE_IRIS,
00160 &newconfig->auto_iris, &newconfig->iris);
00161 configure(DC1394_FEATURE_PAN,
00162 &newconfig->auto_pan, &newconfig->pan);
00163 configure(DC1394_FEATURE_SATURATION,
00164 &newconfig->auto_saturation, &newconfig->saturation);
00165 configure(DC1394_FEATURE_SHARPNESS,
00166 &newconfig->auto_sharpness, &newconfig->sharpness);
00167 configure(DC1394_FEATURE_SHUTTER,
00168 &newconfig->auto_shutter, &newconfig->shutter);
00169 configure(DC1394_FEATURE_WHITE_BALANCE, &newconfig->auto_white_balance,
00170 &newconfig->white_balance_BU, &newconfig->white_balance_RV);
00171 configure(DC1394_FEATURE_ZOOM,
00172 &newconfig->auto_zoom, &newconfig->zoom);
00173
00174
00175 oldconfig_ = *newconfig;
00176 return true;
00177 }
00178
00191 void Features::reconfigure(Config *newconfig)
00192 {
00193 updateIfChanged(DC1394_FEATURE_BRIGHTNESS,
00194 oldconfig_.auto_brightness, &newconfig->auto_brightness,
00195 oldconfig_.brightness, &newconfig->brightness);
00196 updateIfChanged(DC1394_FEATURE_EXPOSURE,
00197 oldconfig_.auto_exposure, &newconfig->auto_exposure,
00198 oldconfig_.exposure, &newconfig->exposure);
00199 updateIfChanged(DC1394_FEATURE_FOCUS,
00200 oldconfig_.auto_focus, &newconfig->auto_focus,
00201 oldconfig_.focus, &newconfig->focus);
00202 updateIfChanged(DC1394_FEATURE_GAIN,
00203 oldconfig_.auto_gain, &newconfig->auto_gain,
00204 oldconfig_.gain, &newconfig->gain);
00205 updateIfChanged(DC1394_FEATURE_GAMMA,
00206 oldconfig_.auto_gamma, &newconfig->auto_gamma,
00207 oldconfig_.gamma, &newconfig->gamma);
00208 updateIfChanged(DC1394_FEATURE_HUE,
00209 oldconfig_.auto_hue, &newconfig->auto_hue,
00210 oldconfig_.hue, &newconfig->hue);
00211 updateIfChanged(DC1394_FEATURE_IRIS,
00212 oldconfig_.auto_iris, &newconfig->auto_iris,
00213 oldconfig_.iris, &newconfig->iris);
00214 updateIfChanged(DC1394_FEATURE_PAN,
00215 oldconfig_.auto_pan, &newconfig->auto_pan,
00216 oldconfig_.pan, &newconfig->pan);
00217 updateIfChanged(DC1394_FEATURE_SATURATION,
00218 oldconfig_.auto_saturation, &newconfig->auto_saturation,
00219 oldconfig_.saturation, &newconfig->saturation);
00220 updateIfChanged(DC1394_FEATURE_SHARPNESS,
00221 oldconfig_.auto_sharpness, &newconfig->auto_sharpness,
00222 oldconfig_.sharpness, &newconfig->sharpness);
00223 updateIfChanged(DC1394_FEATURE_SHUTTER,
00224 oldconfig_.auto_shutter, &newconfig->auto_shutter,
00225 oldconfig_.shutter, &newconfig->shutter);
00226
00227 updateIfChanged(DC1394_FEATURE_WHITE_BALANCE,
00228 oldconfig_.auto_white_balance,
00229 &newconfig->auto_white_balance,
00230 oldconfig_.white_balance_BU, &newconfig->white_balance_BU,
00231 oldconfig_.white_balance_RV, &newconfig->white_balance_RV);
00232 updateIfChanged(DC1394_FEATURE_ZOOM,
00233 oldconfig_.auto_zoom, &newconfig->auto_zoom,
00234 oldconfig_.zoom, &newconfig->zoom);
00235
00236
00237 oldconfig_ = *newconfig;
00238 }
00239
00241
00243
00262 void Features::configure(dc1394feature_t feature, int *control,
00263 double *value, double *value2)
00264 {
00265
00266 dc1394feature_info_t *finfo =
00267 &feature_set_.feature[feature - DC1394_FEATURE_MIN];
00268
00269 if (!finfo->available)
00270 {
00271 *control = camera1394::Camera1394_None;
00272 return;
00273 }
00274
00275 switch (*control)
00276 {
00277 case camera1394::Camera1394_Off:
00278 setPower(finfo, DC1394_OFF);
00279 break;
00280
00281 case camera1394::Camera1394_Query:
00282 getValues(finfo, value, value2);
00283 break;
00284
00285 case camera1394::Camera1394_Auto:
00286 if (!setMode(finfo, DC1394_FEATURE_MODE_AUTO))
00287 {
00288 setPower(finfo, DC1394_OFF);
00289 }
00290 break;
00291
00292 case camera1394::Camera1394_Manual:
00293 if (!setMode(finfo, DC1394_FEATURE_MODE_MANUAL))
00294 {
00295 setPower(finfo, DC1394_OFF);
00296 break;
00297 }
00298
00299
00300 if (finfo->absolute_capable && finfo->abs_control)
00301 {
00302
00303 float fmin, fmax;
00304 if (DC1394_SUCCESS ==
00305 dc1394_feature_get_absolute_boundaries(camera_, feature,
00306 &fmin, &fmax))
00307 {
00308
00309 if (*value < fmin)
00310 *value = (double) fmin;
00311 else if (*value > fmax)
00312 *value = (double) fmax;
00313 }
00314 else
00315 {
00316 ROS_WARN_STREAM("failed to get feature "
00317 << featureName(feature) << " boundaries ");
00318 }
00319
00320
00321 float fval = *value;
00322 if (DC1394_SUCCESS !=
00323 dc1394_feature_set_absolute_value(camera_, feature, fval))
00324 {
00325 ROS_WARN_STREAM("failed to set feature "
00326 << featureName(feature) << " to " << fval);
00327 }
00328 }
00329 else
00330 {
00331
00332 *value = rint(*value);
00333
00334
00335 if (*value < finfo->min)
00336 *value = (double) finfo->min;
00337 else if (*value > finfo->max)
00338 *value = (double) finfo->max;
00339
00340 dc1394error_t rc;
00341 uint32_t ival = (uint32_t) *value;
00342
00343
00344 if (feature == DC1394_FEATURE_WHITE_BALANCE)
00345 {
00346 *value2 = rint(*value2);
00347
00348
00349 if (*value2 < finfo->min)
00350 *value2 = (double) finfo->min;
00351 else if (*value2 > finfo->max)
00352 *value2 = (double) finfo->max;
00353
00354 uint32_t ival2 = (uint32_t) *value2;
00355 rc = dc1394_feature_whitebalance_set_value(camera_, ival, ival2);
00356 }
00357 else
00358 {
00359
00360 rc = dc1394_feature_set_value(camera_, feature, ival);
00361 }
00362 if (rc != DC1394_SUCCESS)
00363 {
00364 ROS_WARN_STREAM("failed to set feature "
00365 << featureName(feature) << " to " << ival);
00366 }
00367 }
00368 break;
00369
00370 case camera1394::Camera1394_OnePush:
00371
00372 setMode(finfo, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
00373
00374
00375 setPower(finfo, DC1394_OFF);
00376 break;
00377
00378 case camera1394::Camera1394_None:
00379
00380 ROS_INFO_STREAM("feature " << featureName(feature)
00381 << " exists, cannot set to None");
00382 break;
00383
00384 default:
00385 ROS_WARN_STREAM("unknown state (" << *control
00386 <<") for feature " << featureName(feature));
00387 }
00388
00389
00390 *control = getState(finfo);
00391 ROS_DEBUG_STREAM("feature " << featureName(feature)
00392 << " now in state " << *control);
00393 }
00394
00402 Features::state_t Features::getState(dc1394feature_info_t *finfo)
00403 {
00404 dc1394feature_t feature = finfo->id;
00405 dc1394error_t rc;
00406
00407 if (!finfo->available)
00408 {
00409
00410 return camera1394::Camera1394_None;
00411 }
00412
00413 if (finfo->on_off_capable)
00414 {
00415
00416 dc1394switch_t pwr;
00417 rc = dc1394_feature_get_power(camera_, feature, &pwr);
00418 if (rc != DC1394_SUCCESS)
00419 {
00420 ROS_WARN_STREAM("failed to get feature " << featureName(feature)
00421 << " Power setting ");
00422 }
00423 else if (pwr == DC1394_OFF)
00424 {
00425
00426 return camera1394::Camera1394_Off;
00427 }
00428 }
00429
00430
00431 dc1394feature_mode_t mode;
00432 rc = dc1394_feature_get_mode(camera_, feature, &mode);
00433 if (rc != DC1394_SUCCESS)
00434 {
00435 ROS_WARN_STREAM("failed to get current mode of feature "
00436 << featureName(feature));
00437
00438 return camera1394::Camera1394_Off;
00439 }
00440
00441 switch (mode)
00442 {
00443 case DC1394_FEATURE_MODE_MANUAL:
00444 return camera1394::Camera1394_Manual;
00445 case DC1394_FEATURE_MODE_AUTO:
00446 return camera1394::Camera1394_Auto;
00447 case DC1394_FEATURE_MODE_ONE_PUSH_AUTO:
00448 return camera1394::Camera1394_OnePush;
00449 default:
00450 return camera1394::Camera1394_Off;
00451 }
00452 }
00453
00463 void Features::getValues(dc1394feature_info_t *finfo,
00464 double *value, double *value2)
00465 {
00466 dc1394feature_t feature = finfo->id;
00467 dc1394error_t rc;
00468
00469 if (!finfo->readout_capable)
00470 {
00471 ROS_INFO_STREAM("feature " << featureName(feature)
00472 << " value not available from device");
00473 return;
00474 }
00475
00476 if (feature == DC1394_FEATURE_WHITE_BALANCE)
00477 {
00478
00479 if (finfo->absolute_capable && finfo->abs_control)
00480 {
00481
00482
00483 rc = DC1394_FUNCTION_NOT_SUPPORTED;
00484 }
00485 else
00486 {
00487
00488 uint32_t bu_val;
00489 uint32_t rv_val;
00490 rc = dc1394_feature_whitebalance_get_value(camera_, &bu_val, &rv_val);
00491 if (DC1394_SUCCESS == rc)
00492 {
00493
00494 *value = bu_val;
00495 *value2 = rv_val;
00496 }
00497 }
00498 if (DC1394_SUCCESS == rc)
00499 {
00500 ROS_DEBUG_STREAM("feature " << featureName(feature)
00501 << " Blue/U: " << *value
00502 << " Red/V: " << *value2);
00503 }
00504 else
00505 {
00506 ROS_WARN_STREAM("failed to get values for feature "
00507 << featureName(feature));
00508 }
00509 }
00510 else
00511 {
00512
00513 if (finfo->absolute_capable && finfo->abs_control)
00514 {
00515
00516 float fval;
00517 rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
00518 if (DC1394_SUCCESS == rc)
00519 {
00520 *value = fval;
00521 }
00522 }
00523 else
00524 {
00525 uint32_t ival;
00526 rc = dc1394_feature_get_value(camera_, feature, &ival);
00527 if (DC1394_SUCCESS == rc)
00528 {
00529 *value = ival;
00530 }
00531 }
00532 if (DC1394_SUCCESS == rc)
00533 {
00534 ROS_DEBUG_STREAM("feature " << featureName(feature)
00535 << " has value " << *value);
00536 }
00537 else
00538 {
00539 ROS_WARN_STREAM("failed to get value of feature "
00540 << featureName(feature));
00541 }
00542 }
00543 }
00544
00553 bool Features::setMode(dc1394feature_info_t *finfo, dc1394feature_mode_t mode)
00554 {
00555 dc1394feature_t feature = finfo->id;
00556 if (hasMode(finfo, mode))
00557 {
00558
00559 setPower(finfo, DC1394_ON);
00560
00561 ROS_DEBUG_STREAM("setting feature " << featureName(feature)
00562 << " mode to " << modeName(mode));
00563 if (DC1394_SUCCESS !=
00564 dc1394_feature_set_mode(camera_, feature, mode))
00565 {
00566 ROS_WARN_STREAM("failed to set feature " << featureName(feature)
00567 << " mode to " << modeName(mode));
00568 return false;
00569 }
00570 }
00571 else
00572 {
00573
00574 ROS_DEBUG_STREAM("no " << modeName(mode)
00575 << " mode for feature " << featureName(feature));
00576 return false;
00577 }
00578 return true;
00579 }
00580
00588 void Features::setPower(dc1394feature_info_t *finfo, dc1394switch_t on_off)
00589 {
00590 dc1394feature_t feature = finfo->id;
00591 if (finfo->on_off_capable)
00592 {
00593 ROS_DEBUG_STREAM("Setting power for feature " << featureName(feature)
00594 << " to " << on_off);
00595 if (DC1394_SUCCESS !=
00596 dc1394_feature_set_power(camera_, feature, on_off))
00597 {
00598 ROS_WARN_STREAM("failed to set feature " << featureName(feature)
00599 << " power to " << on_off);
00600 }
00601 }
00602 else
00603 {
00604
00605
00606 ROS_DEBUG_STREAM("no power control for feature " << featureName(feature));
00607 }
00608 }
00609
00624 void Features::updateIfChanged(dc1394feature_t feature,
00625 int old_control, int *control,
00626 double old_value, double *value)
00627 {
00628 if ((old_control != *control) || (old_value != *value))
00629 {
00630 configure(feature, control, value);
00631 }
00632 }
00633
00651 void Features::updateIfChanged(dc1394feature_t feature,
00652 int old_control, int *control,
00653 double old_value, double *value,
00654 double old_value2, double *value2)
00655 {
00656 if ((old_control != *control)
00657 || (old_value != *value)
00658 || (old_value2 != *value2))
00659 {
00660 configure(feature, control, value, value2);
00661 }
00662 }