featuresstereo.cpp
Go to the documentation of this file.
00001 /* $Id: features.cpp 35412 2011-01-23 17:49:15Z joq $ */
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 "featuresstereo.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_SATURATION,
00162             &newconfig->auto_saturation, &newconfig->saturation);
00163   configure(DC1394_FEATURE_SHARPNESS,
00164             &newconfig->auto_sharpness, &newconfig->sharpness);
00165   configure(DC1394_FEATURE_SHUTTER,
00166             &newconfig->auto_shutter, &newconfig->shutter);
00167   configure(DC1394_FEATURE_PAN,
00168             &newconfig->auto_pan, &newconfig->pan);
00169   configure(DC1394_FEATURE_FRAME_RATE,
00170             &newconfig->auto_frame_rate_feature, &newconfig->frame_rate_feature);
00171   configure(DC1394_FEATURE_WHITE_BALANCE, &newconfig->auto_white_balance,
00172             &newconfig->white_balance_BU, &newconfig->white_balance_RV);
00173   configure(DC1394_FEATURE_ZOOM,
00174             &newconfig->auto_zoom, &newconfig->zoom);
00175 
00176   // save configured values
00177   oldconfig_ = *newconfig;
00178   return true;
00179 }
00180 
00193 void Features::reconfigure(Config *newconfig)
00194 {
00195   updateIfChanged(DC1394_FEATURE_BRIGHTNESS,
00196                   oldconfig_.auto_brightness, &newconfig->auto_brightness,
00197                   oldconfig_.brightness, &newconfig->brightness);
00198   updateIfChanged(DC1394_FEATURE_EXPOSURE,
00199                   oldconfig_.auto_exposure, &newconfig->auto_exposure,
00200                   oldconfig_.exposure, &newconfig->exposure);
00201   updateIfChanged(DC1394_FEATURE_FOCUS,
00202                   oldconfig_.auto_focus, &newconfig->auto_focus,
00203                   oldconfig_.focus, &newconfig->focus);
00204   updateIfChanged(DC1394_FEATURE_GAIN,
00205                   oldconfig_.auto_gain, &newconfig->auto_gain,
00206                   oldconfig_.gain, &newconfig->gain);
00207   updateIfChanged(DC1394_FEATURE_GAMMA,
00208                   oldconfig_.auto_gamma, &newconfig->auto_gamma,
00209                   oldconfig_.gamma, &newconfig->gamma);
00210   updateIfChanged(DC1394_FEATURE_HUE,
00211                   oldconfig_.auto_hue, &newconfig->auto_hue,
00212                   oldconfig_.hue, &newconfig->hue);
00213   updateIfChanged(DC1394_FEATURE_IRIS,
00214                   oldconfig_.auto_iris, &newconfig->auto_iris,
00215                   oldconfig_.iris, &newconfig->iris);
00216   updateIfChanged(DC1394_FEATURE_SATURATION,
00217                   oldconfig_.auto_saturation, &newconfig->auto_saturation,
00218                   oldconfig_.saturation, &newconfig->saturation);
00219   updateIfChanged(DC1394_FEATURE_SHARPNESS,
00220                   oldconfig_.auto_sharpness, &newconfig->auto_sharpness,
00221                   oldconfig_.sharpness, &newconfig->sharpness);
00222   updateIfChanged(DC1394_FEATURE_SHUTTER,
00223                   oldconfig_.auto_shutter, &newconfig->auto_shutter,
00224                   oldconfig_.shutter, &newconfig->shutter);
00225   updateIfChanged(DC1394_FEATURE_PAN,
00226                   oldconfig_.auto_pan, &newconfig->auto_pan,
00227                   oldconfig_.pan, &newconfig->pan);
00228   updateIfChanged(DC1394_FEATURE_FRAME_RATE,
00229                   oldconfig_.auto_frame_rate_feature, &newconfig->auto_frame_rate_feature,
00230                   oldconfig_.frame_rate_feature, &newconfig->frame_rate_feature);
00231 
00232   // White balance has two component parameters: Blue/U and Red/V.
00233   updateIfChanged(DC1394_FEATURE_WHITE_BALANCE,
00234                   oldconfig_.auto_white_balance,
00235                   &newconfig->auto_white_balance,
00236                   oldconfig_.white_balance_BU, &newconfig->white_balance_BU,
00237                   oldconfig_.white_balance_RV, &newconfig->white_balance_RV);
00238   updateIfChanged(DC1394_FEATURE_ZOOM,
00239                   oldconfig_.auto_zoom, &newconfig->auto_zoom,
00240                   oldconfig_.zoom, &newconfig->zoom);
00241 
00242   // save modified values
00243   oldconfig_ = *newconfig;
00244 }
00245 
00247 // private methods:
00249 
00268 void Features::configure(dc1394feature_t feature, int *control,
00269                          double *value, double *value2)
00270 {
00271   // device-relevant information for this feature
00272   dc1394feature_info_t *finfo =
00273     &feature_set_.feature[feature - DC1394_FEATURE_MIN];
00274 
00275   if (!finfo->available)                // feature not available?
00276     {
00277       *control = camera1394stereo::Camera1394Stereo_None;
00278       return;
00279     }
00280 
00281   switch (*control)
00282     {
00283     case camera1394stereo::Camera1394Stereo_Off:
00284       setOff(finfo);
00285       break;
00286 
00287     case camera1394stereo::Camera1394Stereo_Query:
00288       getValues(finfo, value, value2);
00289       break;
00290 
00291     case camera1394stereo::Camera1394Stereo_Auto:
00292       if (!setMode(finfo, DC1394_FEATURE_MODE_AUTO))
00293         {
00294           setOff(finfo);
00295         }
00296       break;
00297 
00298     case camera1394stereo::Camera1394Stereo_Manual:
00299       if (!setMode(finfo, DC1394_FEATURE_MODE_MANUAL))
00300         {
00301           setOff(finfo);
00302           break;
00303         }
00304 
00305       // TODO: break this into some internal methods
00306       if (finfo->absolute_capable && finfo->abs_control)
00307         {
00308           // supports reading and setting float value
00309           float fmin, fmax;
00310           if (DC1394_SUCCESS ==
00311               dc1394_feature_get_absolute_boundaries(camera_, feature,
00312                                                      &fmin, &fmax))
00313             {
00314               // clamp *value between minimum and maximum
00315               if (*value < fmin)
00316                 *value = (double) fmin;
00317               else if (*value > fmax)
00318                 *value = (double) fmax;
00319             }
00320           else
00321             {
00322               ROS_WARN_STREAM("failed to get feature "
00323                               << featureName(feature) << " boundaries ");
00324             }
00325 
00326           // @todo handle absolute White Balance values
00327           float fval = *value;
00328           if (DC1394_SUCCESS !=
00329               dc1394_feature_set_absolute_value(camera_, feature, fval))
00330             {
00331               ROS_WARN_STREAM("failed to set feature "
00332                               << featureName(feature) << " to " << fval);
00333             }
00334         }
00335       else // no float representation
00336         {
00337           // round requested value to nearest integer
00338           *value = rint(*value);
00339 
00340           // clamp *value between minimum and maximum
00341           if (*value < finfo->min)
00342             *value = (double) finfo->min;
00343           else if (*value > finfo->max)
00344             *value = (double) finfo->max;
00345 
00346           dc1394error_t rc;
00347           uint32_t ival = (uint32_t) *value;
00348 
00349           // handle White Balance, which has two components
00350           if (feature == DC1394_FEATURE_WHITE_BALANCE)
00351             {
00352               *value2 = rint(*value2);
00353 
00354               // clamp *value2 between same minimum and maximum
00355               if (*value2 < finfo->min)
00356                 *value2 = (double) finfo->min;
00357               else if (*value2 > finfo->max)
00358                 *value2 = (double) finfo->max;
00359 
00360               uint32_t ival2 = (uint32_t) *value2;
00361               rc = dc1394_feature_whitebalance_set_value(camera_, ival, ival2);
00362             }
00363           else
00364             {
00365               // other features only have one component
00366               rc = dc1394_feature_set_value(camera_, feature, ival);
00367             }
00368           if (rc != DC1394_SUCCESS)
00369             {
00370               ROS_WARN_STREAM("failed to set feature "
00371                               << featureName(feature) << " to " << ival);
00372             }
00373         }
00374       break;
00375 
00376     case camera1394stereo::Camera1394Stereo_OnePush:
00377       // Try to set OnePush mode
00378       setMode(finfo, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
00379 
00380       // Now turn the control off, so camera does not continue adjusting
00381       setOff(finfo);
00382       break;
00383 
00384     case camera1394stereo::Camera1394Stereo_None:
00385       // Invalid user input, because this feature actually does exist.
00386       ROS_INFO_STREAM("feature " << featureName(feature)
00387                       << " exists, cannot set to None");
00388       break;
00389 
00390     default:
00391       ROS_WARN_STREAM("unknown state (" << *control
00392                       <<") for feature " << featureName(feature));
00393     }
00394 
00395   // return actual state reported by the device
00396   *control = getState(finfo);
00397   ROS_DEBUG_STREAM("feature " << featureName(feature)
00398                    << " now in state " << *control);
00399 }
00400 
00408 Features::state_t Features::getState(dc1394feature_info_t *finfo)
00409 {
00410   dc1394feature_t feature = finfo->id;
00411   dc1394error_t rc;
00412 
00413   if (!finfo->available)
00414     {
00415       // not available: nothing more to do
00416       return camera1394stereo::Camera1394Stereo_None;
00417     }
00418 
00419   if (finfo->on_off_capable)
00420     {
00421       // get On/Off state
00422       dc1394switch_t pwr;
00423       rc = dc1394_feature_get_power(camera_, feature, &pwr);
00424       if (rc != DC1394_SUCCESS)
00425         {
00426           ROS_WARN_STREAM("failed to get feature " << featureName(feature)
00427                           << " Power setting ");
00428         }
00429       else if (pwr == DC1394_OFF)
00430         {
00431           // Off overrides mode settings
00432           return camera1394stereo::Camera1394Stereo_Off;
00433         }
00434     }
00435 
00436   // not off, so get mode
00437   dc1394feature_mode_t mode;
00438   rc = dc1394_feature_get_mode(camera_, feature, &mode);
00439   if (rc != DC1394_SUCCESS)
00440     {
00441       ROS_WARN_STREAM("failed to get current mode of feature "
00442                       << featureName(feature));
00443       // treat unavailable mode as Off
00444       return camera1394stereo::Camera1394Stereo_Off;
00445     }
00446 
00447   switch (mode)
00448     {
00449     case DC1394_FEATURE_MODE_MANUAL:
00450       return camera1394stereo::Camera1394Stereo_Manual;
00451     case DC1394_FEATURE_MODE_AUTO:
00452       return camera1394stereo::Camera1394Stereo_Auto;
00453     case DC1394_FEATURE_MODE_ONE_PUSH_AUTO:
00454       return camera1394stereo::Camera1394Stereo_OnePush;
00455     default:
00456       return camera1394stereo::Camera1394Stereo_Off;
00457     }
00458 }
00459 
00469 void Features::getValues(dc1394feature_info_t *finfo,
00470                            double *value, double *value2)
00471 {
00472   dc1394feature_t feature = finfo->id;
00473   dc1394error_t rc;
00474 
00475   if (!finfo->readout_capable)
00476     {
00477       ROS_INFO_STREAM("feature " << featureName(feature)
00478                       << " value not available from device");
00479       return;
00480     }
00481 
00482   if (feature == DC1394_FEATURE_WHITE_BALANCE)
00483     {
00484       // handle White Balance separately, it has two components
00485       if (finfo->absolute_capable && finfo->abs_control)
00486         {
00487           // supports reading and setting float value
00488           // @todo get absolute White Balance values
00489           rc = DC1394_FUNCTION_NOT_SUPPORTED;
00490         }
00491       else
00492         {
00493           // get integer White Balance values
00494           uint32_t bu_val;
00495           uint32_t rv_val;
00496           rc = dc1394_feature_whitebalance_get_value(camera_, &bu_val, &rv_val);
00497           if (DC1394_SUCCESS == rc)
00498             {
00499               // convert to double
00500               *value = bu_val;
00501               *value2 = rv_val;
00502             }
00503         }
00504       if (DC1394_SUCCESS == rc)
00505         {
00506           ROS_DEBUG_STREAM("feature " << featureName(feature)
00507                            << " Blue/U: " << *value
00508                            << " Red/V: " << *value2);
00509         }
00510       else
00511         {
00512           ROS_WARN_STREAM("failed to get values for feature "
00513                           << featureName(feature));
00514         }
00515     }
00516   else
00517     {
00518       // other features only have one component
00519       if (finfo->absolute_capable && finfo->abs_control)
00520         {
00521           // supports reading and setting float value
00522           float fval;
00523           rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
00524           if (DC1394_SUCCESS == rc)
00525             {
00526               *value = fval;                // convert to double
00527             }
00528         }
00529       else // no float representation
00530         {
00531           uint32_t ival;
00532           rc = dc1394_feature_get_value(camera_, feature, &ival);
00533           if (DC1394_SUCCESS == rc)
00534             {
00535               *value = ival;                // convert to double
00536             }
00537         }
00538       if (DC1394_SUCCESS == rc)
00539         {
00540           ROS_DEBUG_STREAM("feature " << featureName(feature)
00541                            << " has value " << *value);
00542         }
00543       else
00544         {
00545           ROS_WARN_STREAM("failed to get value of feature "
00546                           << featureName(feature));
00547         }
00548     }
00549 }
00550 
00559 bool Features::setMode(dc1394feature_info_t *finfo, dc1394feature_mode_t mode)
00560 {
00561   dc1394feature_t feature = finfo->id;
00562   if (hasMode(finfo, mode))
00563     {
00564       ROS_DEBUG_STREAM("setting feature " << featureName(feature)
00565                        << " mode to " << modeName(mode));
00566       if (DC1394_SUCCESS !=
00567           dc1394_feature_set_mode(camera_, feature, mode))
00568         {
00569           ROS_WARN_STREAM("failed to set feature " << featureName(feature)
00570                           << " mode to " << modeName(mode));
00571           return false;
00572         }
00573     }
00574   else
00575     {
00576       // device does not support this mode for this feature
00577       ROS_DEBUG_STREAM("no " << modeName(mode)
00578                        << " mode for feature " << featureName(feature));
00579       return false;
00580     }
00581   return true;
00582 }
00583 
00590 void Features::setOff(dc1394feature_info_t *finfo)
00591 {
00592   dc1394feature_t feature = finfo->id;
00593   if (finfo->on_off_capable)
00594     {
00595       ROS_DEBUG_STREAM("setting feature " << featureName(feature) << " Off");
00596       if (DC1394_SUCCESS !=
00597           dc1394_feature_set_power(camera_, feature, DC1394_OFF))
00598         {
00599           ROS_WARN_STREAM("failed to set feature " << featureName(feature)
00600                           << " Off ");
00601         }
00602     }
00603   else
00604     {
00605       // This device does not support turning this feature off.
00606       // Inform the user, but pretend it worked.
00607       ROS_DEBUG_STREAM("no Off mode for feature " << featureName(feature));
00608     }
00609 }
00610 
00625 void Features::updateIfChanged(dc1394feature_t feature,
00626                                int old_control, int *control,
00627                                double old_value, double *value)
00628 {
00629   if ((old_control != *control) || (old_value != *value))
00630     {
00631       configure(feature, control, value);
00632     }
00633 }
00634 
00652 void Features::updateIfChanged(dc1394feature_t feature,
00653                                int old_control, int *control,
00654                                double old_value, double *value,
00655                                double old_value2, double *value2)
00656 {
00657   if ((old_control != *control)
00658       || (old_value != *value)
00659       || (old_value2 != *value2))
00660     {
00661       configure(feature, control, value, value2);
00662     }
00663 }


camera1394stereo
Author(s): Joan Pau Beltran
autogenerated on Thu Aug 27 2015 12:39:08