features.cpp
Go to the documentation of this file.
00001 /* $Id$ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (c) 2010 Jack O'Quin
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *********************************************************************/
00036 
00037 #include <cmath>
00038 #include "features.h"
00039 
00048 
00049 // static data and functions:
00051 namespace
00052 {
00053   // driver feature parameter names, corresponding to DC1394 modes
00054   // (not all currently supported by the driver)
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   // driver mode parameter names, corresponding to DC1394 modes
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 // public methods:
00119 
00124 Features::Features(dc1394camera_t *camera):
00125   camera_(camera)
00126 {}
00127 
00137 bool Features::initialize(Config *newconfig)
00138 {
00139   // query all features for this device
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   // validate and set configured value of each supported feature
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   // save configured values
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   // White balance has two component parameters: Blue/U and Red/V.
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   // save modified values
00237   oldconfig_ = *newconfig;
00238 }
00239 
00241 // private methods:
00243 
00262 void Features::configure(dc1394feature_t feature, int *control,
00263                          double *value, double *value2)
00264 {
00265   // device-relevant information for this feature
00266   dc1394feature_info_t *finfo =
00267     &feature_set_.feature[feature - DC1394_FEATURE_MIN];
00268 
00269   if (!finfo->available)                // feature not 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       // TODO: break this into some internal methods
00300       if (finfo->absolute_capable && finfo->abs_control)
00301         {
00302           // supports reading and setting float value
00303           float fmin, fmax;
00304           if (DC1394_SUCCESS ==
00305               dc1394_feature_get_absolute_boundaries(camera_, feature,
00306                                                      &fmin, &fmax))
00307             {
00308               // clamp *value between minimum and maximum
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           // @todo handle absolute White Balance values
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 // no float representation
00330         {
00331           // round requested value to nearest integer
00332           *value = rint(*value);
00333 
00334           // clamp *value between minimum and maximum
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           // handle White Balance, which has two components
00344           if (feature == DC1394_FEATURE_WHITE_BALANCE)
00345             {
00346               *value2 = rint(*value2);
00347 
00348               // clamp *value2 between same minimum and maximum
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               // other features only have one component
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       // Try to set OnePush mode
00372       setMode(finfo, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
00373 
00374       // Now turn the control off, so camera does not continue adjusting
00375       setPower(finfo, DC1394_OFF);
00376       break;
00377 
00378     case camera1394::Camera1394_None:
00379       // Invalid user input, because this feature actually does exist.
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   // return actual state reported by the device
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       // not available: nothing more to do
00410       return camera1394::Camera1394_None;
00411     }
00412 
00413   if (finfo->on_off_capable)
00414     {
00415       // get On/Off state
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           // Off overrides mode settings
00426           return camera1394::Camera1394_Off;
00427         }
00428     }
00429 
00430   // not off, so get mode
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       // treat unavailable mode as Off
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       // handle White Balance separately, it has two components
00479       if (finfo->absolute_capable && finfo->abs_control)
00480         {
00481           // supports reading and setting float value
00482           // @todo get absolute White Balance values
00483           rc = DC1394_FUNCTION_NOT_SUPPORTED;
00484         }
00485       else
00486         {
00487           // get integer White Balance values
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               // convert to double
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       // other features only have one component
00513       if (finfo->absolute_capable && finfo->abs_control)
00514         {
00515           // supports reading and setting float value
00516           float fval;
00517           rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
00518           if (DC1394_SUCCESS == rc)
00519             {
00520               *value = fval;                // convert to double
00521             }
00522         }
00523       else // no float representation
00524         {
00525           uint32_t ival;
00526           rc = dc1394_feature_get_value(camera_, feature, &ival);
00527           if (DC1394_SUCCESS == rc)
00528             {
00529               *value = ival;                // convert to double
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       // first, make sure the feature is powered on
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       // device does not support this mode for this feature
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       // This device does not support turning this feature on or off.
00605       // That's OK.
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 }


camera1394
Author(s): Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy
autogenerated on Wed Aug 26 2015 10:56:59