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_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_WHITE_BALANCE, &newconfig->auto_white_balance,
00168             &newconfig->white_balance_BU, &newconfig->white_balance_RV);
00169   configure(DC1394_FEATURE_ZOOM,
00170             &newconfig->auto_zoom, &newconfig->zoom);
00171 
00172   // save configured values
00173   oldconfig_ = *newconfig;
00174   return true;
00175 }
00176 
00189 void Features::reconfigure(Config *newconfig)
00190 {
00191   updateIfChanged(DC1394_FEATURE_BRIGHTNESS,
00192                   oldconfig_.auto_brightness, &newconfig->auto_brightness,
00193                   oldconfig_.brightness, &newconfig->brightness);
00194   updateIfChanged(DC1394_FEATURE_EXPOSURE,
00195                   oldconfig_.auto_exposure, &newconfig->auto_exposure,
00196                   oldconfig_.exposure, &newconfig->exposure);
00197   updateIfChanged(DC1394_FEATURE_FOCUS,
00198                   oldconfig_.auto_focus, &newconfig->auto_focus,
00199                   oldconfig_.focus, &newconfig->focus);
00200   updateIfChanged(DC1394_FEATURE_GAIN,
00201                   oldconfig_.auto_gain, &newconfig->auto_gain,
00202                   oldconfig_.gain, &newconfig->gain);
00203   updateIfChanged(DC1394_FEATURE_GAMMA,
00204                   oldconfig_.auto_gamma, &newconfig->auto_gamma,
00205                   oldconfig_.gamma, &newconfig->gamma);
00206   updateIfChanged(DC1394_FEATURE_HUE,
00207                   oldconfig_.auto_hue, &newconfig->auto_hue,
00208                   oldconfig_.hue, &newconfig->hue);
00209   updateIfChanged(DC1394_FEATURE_IRIS,
00210                   oldconfig_.auto_iris, &newconfig->auto_iris,
00211                   oldconfig_.iris, &newconfig->iris);
00212   updateIfChanged(DC1394_FEATURE_SATURATION,
00213                   oldconfig_.auto_saturation, &newconfig->auto_saturation,
00214                   oldconfig_.saturation, &newconfig->saturation);
00215   updateIfChanged(DC1394_FEATURE_SHARPNESS,
00216                   oldconfig_.auto_sharpness, &newconfig->auto_sharpness,
00217                   oldconfig_.sharpness, &newconfig->sharpness);
00218   updateIfChanged(DC1394_FEATURE_SHUTTER,
00219                   oldconfig_.auto_shutter, &newconfig->auto_shutter,
00220                   oldconfig_.shutter, &newconfig->shutter);
00221   // White balance has two component parameters: Blue/U and Red/V.
00222   updateIfChanged(DC1394_FEATURE_WHITE_BALANCE,
00223                   oldconfig_.auto_white_balance,
00224                   &newconfig->auto_white_balance,
00225                   oldconfig_.white_balance_BU, &newconfig->white_balance_BU,
00226                   oldconfig_.white_balance_RV, &newconfig->white_balance_RV);
00227   updateIfChanged(DC1394_FEATURE_ZOOM,
00228                   oldconfig_.auto_zoom, &newconfig->auto_zoom,
00229                   oldconfig_.zoom, &newconfig->zoom);
00230 
00231   // save modified values
00232   oldconfig_ = *newconfig;
00233 }
00234 
00236 // private methods:
00238 
00257 void Features::configure(dc1394feature_t feature, int *control,
00258                          double *value, double *value2)
00259 {
00260   // device-relevant information for this feature
00261   dc1394feature_info_t *finfo =
00262     &feature_set_.feature[feature - DC1394_FEATURE_MIN];
00263 
00264   if (!finfo->available)                // feature not available?
00265     {
00266       *control = camera1394::Camera1394_None;
00267       return;
00268     }
00269 
00270   switch (*control)
00271     {
00272     case camera1394::Camera1394_Off:
00273       setOff(finfo);
00274       break;
00275 
00276     case camera1394::Camera1394_Query:
00277       getValues(finfo, value, value2);
00278       break;
00279 
00280     case camera1394::Camera1394_Auto:
00281       if (!setMode(finfo, DC1394_FEATURE_MODE_AUTO))
00282         {
00283           setOff(finfo);
00284         }
00285       break;
00286 
00287     case camera1394::Camera1394_Manual:
00288       if (!setMode(finfo, DC1394_FEATURE_MODE_MANUAL))
00289         {
00290           setOff(finfo);
00291           break;
00292         }
00293 
00294       // TODO: break this into some internal methods
00295       if (finfo->absolute_capable && finfo->abs_control)
00296         {
00297           // supports reading and setting float value
00298           float fmin, fmax;
00299           if (DC1394_SUCCESS ==
00300               dc1394_feature_get_absolute_boundaries(camera_, feature,
00301                                                      &fmin, &fmax))
00302             {
00303               // clamp *value between minimum and maximum
00304               if (*value < fmin)
00305                 *value = (double) fmin;
00306               else if (*value > fmax)
00307                 *value = (double) fmax;
00308             }
00309           else
00310             {
00311               ROS_WARN_STREAM("failed to get feature "
00312                               << featureName(feature) << " boundaries ");
00313             }
00314 
00315           // @todo handle absolute White Balance values
00316           float fval = *value;
00317           if (DC1394_SUCCESS !=
00318               dc1394_feature_set_absolute_value(camera_, feature, fval))
00319             {
00320               ROS_WARN_STREAM("failed to set feature "
00321                               << featureName(feature) << " to " << fval);
00322             }
00323         }
00324       else // no float representation
00325         {
00326           // round requested value to nearest integer
00327           *value = rint(*value);
00328 
00329           // clamp *value between minimum and maximum
00330           if (*value < finfo->min)
00331             *value = (double) finfo->min;
00332           else if (*value > finfo->max)
00333             *value = (double) finfo->max;
00334 
00335           dc1394error_t rc;
00336           uint32_t ival = (uint32_t) *value;
00337 
00338           // handle White Balance, which has two components
00339           if (feature == DC1394_FEATURE_WHITE_BALANCE)
00340             {
00341               *value2 = rint(*value2);
00342 
00343               // clamp *value2 between same minimum and maximum
00344               if (*value2 < finfo->min)
00345                 *value2 = (double) finfo->min;
00346               else if (*value2 > finfo->max)
00347                 *value2 = (double) finfo->max;
00348 
00349               uint32_t ival2 = (uint32_t) *value2;
00350               rc = dc1394_feature_whitebalance_set_value(camera_, ival, ival2);
00351             }
00352           else
00353             {
00354               // other features only have one component
00355               rc = dc1394_feature_set_value(camera_, feature, ival);
00356             }
00357           if (rc != DC1394_SUCCESS)
00358             {
00359               ROS_WARN_STREAM("failed to set feature "
00360                               << featureName(feature) << " to " << ival);
00361             }
00362         }
00363       break;
00364 
00365     case camera1394::Camera1394_OnePush:
00366       // Try to set OnePush mode
00367       setMode(finfo, DC1394_FEATURE_MODE_ONE_PUSH_AUTO);
00368 
00369       // Now turn the control off, so camera does not continue adjusting
00370       setOff(finfo);
00371       break;
00372 
00373     case camera1394::Camera1394_None:
00374       // Invalid user input, because this feature actually does exist.
00375       ROS_INFO_STREAM("feature " << featureName(feature)
00376                       << " exists, cannot set to None");
00377       break;
00378 
00379     default:
00380       ROS_WARN_STREAM("unknown state (" << *control
00381                       <<") for feature " << featureName(feature));
00382     }
00383 
00384   // return actual state reported by the device
00385   *control = getState(finfo);
00386   ROS_DEBUG_STREAM("feature " << featureName(feature)
00387                    << " now in state " << *control);
00388 }
00389 
00397 Features::state_t Features::getState(dc1394feature_info_t *finfo)
00398 {
00399   dc1394feature_t feature = finfo->id;
00400   dc1394error_t rc;
00401 
00402   if (!finfo->available)
00403     {
00404       // not available: nothing more to do
00405       return camera1394::Camera1394_None;
00406     }
00407 
00408   if (finfo->on_off_capable)
00409     {
00410       // get On/Off state
00411       dc1394switch_t pwr;
00412       rc = dc1394_feature_get_power(camera_, feature, &pwr);
00413       if (rc != DC1394_SUCCESS)
00414         {
00415           ROS_WARN_STREAM("failed to get feature " << featureName(feature)
00416                           << " Power setting ");
00417         }
00418       else if (pwr == DC1394_OFF)
00419         {
00420           // Off overrides mode settings
00421           return camera1394::Camera1394_Off;
00422         }
00423     }
00424 
00425   // not off, so get mode
00426   dc1394feature_mode_t mode;
00427   rc = dc1394_feature_get_mode(camera_, feature, &mode);
00428   if (rc != DC1394_SUCCESS)
00429     {
00430       ROS_WARN_STREAM("failed to get current mode of feature "
00431                       << featureName(feature));
00432       // treat unavailable mode as Off
00433       return camera1394::Camera1394_Off;
00434     }
00435 
00436   switch (mode)
00437     {
00438     case DC1394_FEATURE_MODE_MANUAL:
00439       return camera1394::Camera1394_Manual;
00440     case DC1394_FEATURE_MODE_AUTO:
00441       return camera1394::Camera1394_Auto;
00442     case DC1394_FEATURE_MODE_ONE_PUSH_AUTO:
00443       return camera1394::Camera1394_OnePush;
00444     default:
00445       return camera1394::Camera1394_Off;
00446     }
00447 }
00448 
00458 void Features::getValues(dc1394feature_info_t *finfo,
00459                            double *value, double *value2)
00460 {
00461   dc1394feature_t feature = finfo->id;
00462   dc1394error_t rc;
00463 
00464   if (!finfo->readout_capable)
00465     {
00466       ROS_INFO_STREAM("feature " << featureName(feature)
00467                       << " value not available from device");
00468       return;
00469     }
00470 
00471   if (feature == DC1394_FEATURE_WHITE_BALANCE)
00472     {
00473       // handle White Balance separately, it has two components
00474       if (finfo->absolute_capable && finfo->abs_control)
00475         {
00476           // supports reading and setting float value
00477           // @todo get absolute White Balance values
00478           rc = DC1394_FUNCTION_NOT_SUPPORTED;
00479         }
00480       else
00481         {
00482           // get integer White Balance values
00483           uint32_t bu_val;
00484           uint32_t rv_val;
00485           rc = dc1394_feature_whitebalance_get_value(camera_, &bu_val, &rv_val);
00486           if (DC1394_SUCCESS == rc)
00487             {
00488               // convert to double
00489               *value = bu_val;
00490               *value2 = rv_val;
00491             }
00492         }
00493       if (DC1394_SUCCESS == rc)
00494         {
00495           ROS_DEBUG_STREAM("feature " << featureName(feature)
00496                            << " Blue/U: " << *value
00497                            << " Red/V: " << *value2);
00498         }
00499       else
00500         {
00501           ROS_WARN_STREAM("failed to get values for feature "
00502                           << featureName(feature));
00503         }
00504     }
00505   else
00506     {
00507       // other features only have one component
00508       if (finfo->absolute_capable && finfo->abs_control)
00509         {
00510           // supports reading and setting float value
00511           float fval;
00512           rc = dc1394_feature_get_absolute_value(camera_, feature, &fval);
00513           if (DC1394_SUCCESS == rc)
00514             {
00515               *value = fval;                // convert to double
00516             }
00517         }
00518       else // no float representation
00519         {
00520           uint32_t ival;
00521           rc = dc1394_feature_get_value(camera_, feature, &ival);
00522           if (DC1394_SUCCESS == rc)
00523             {
00524               *value = ival;                // convert to double
00525             }
00526         }
00527       if (DC1394_SUCCESS == rc)
00528         {
00529           ROS_DEBUG_STREAM("feature " << featureName(feature)
00530                            << " has value " << *value);
00531         }
00532       else
00533         {
00534           ROS_WARN_STREAM("failed to get value of feature "
00535                           << featureName(feature));
00536         }
00537     }
00538 }
00539 
00548 bool Features::setMode(dc1394feature_info_t *finfo, dc1394feature_mode_t mode)
00549 {
00550   dc1394feature_t feature = finfo->id;
00551   if (hasMode(finfo, mode))
00552     {
00553       ROS_DEBUG_STREAM("setting feature " << featureName(feature)
00554                        << " mode to " << modeName(mode));
00555       if (DC1394_SUCCESS !=
00556           dc1394_feature_set_mode(camera_, feature, mode))
00557         {
00558           ROS_WARN_STREAM("failed to set feature " << featureName(feature)
00559                           << " mode to " << modeName(mode));
00560           return false;
00561         }
00562     }
00563   else
00564     {
00565       // device does not support this mode for this feature
00566       ROS_DEBUG_STREAM("no " << modeName(mode)
00567                        << " mode for feature " << featureName(feature));
00568       return false;
00569     }
00570   return true;
00571 }
00572 
00579 void Features::setOff(dc1394feature_info_t *finfo)
00580 {
00581   dc1394feature_t feature = finfo->id;
00582   if (finfo->on_off_capable)
00583     {
00584       ROS_DEBUG_STREAM("setting feature " << featureName(feature) << " Off");
00585       if (DC1394_SUCCESS !=
00586           dc1394_feature_set_power(camera_, feature, DC1394_OFF))
00587         {
00588           ROS_WARN_STREAM("failed to set feature " << featureName(feature)
00589                           << " Off ");
00590         }
00591     }
00592   else
00593     {
00594       // This device does not support turning this feature off.
00595       // Inform the user, but pretend it worked.
00596       ROS_DEBUG_STREAM("no Off mode for feature " << featureName(feature));
00597     }
00598 }
00599 
00614 void Features::updateIfChanged(dc1394feature_t feature,
00615                                int old_control, int *control,
00616                                double old_value, double *value)
00617 {
00618   if ((old_control != *control) || (old_value != *value))
00619     {
00620       configure(feature, control, value);
00621     }
00622 }
00623 
00641 void Features::updateIfChanged(dc1394feature_t feature,
00642                                int old_control, int *control,
00643                                double old_value, double *value,
00644                                double old_value2, double *value2)
00645 {
00646   if ((old_control != *control)
00647       || (old_value != *value)
00648       || (old_value2 != *value2))
00649     {
00650       configure(feature, control, value, value2);
00651     }
00652 }


camera1394
Author(s): Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy
autogenerated on Sat Dec 28 2013 16:50:11