modes.cpp
Go to the documentation of this file.
00001 /* $Id: modes.cpp 35615 2011-01-30 13:07:49Z 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 <ros/ros.h>
00038 #include "modes.h"
00039 
00047 
00048 // static data and functions:
00050 namespace Modes
00051 {
00052   // driver parameter names, corresponding to DC1394 video modes
00053   static const std::string video_mode_names_[DC1394_VIDEO_MODE_NUM] =
00054     {
00055       "160x120_yuv444",
00056       "320x240_yuv422",
00057       "640x480_yuv411",
00058       "640x480_yuv422",
00059       "640x480_rgb8",
00060       "640x480_mono8",
00061       "640x480_mono16",
00062       "800x600_yuv422",
00063       "800x600_rgb8",
00064       "800x600_mono8",
00065       "1024x768_yuv422",
00066       "1024x768_rgb8",
00067       "1024x768_mono8",
00068       "800x600_mono16",
00069       "1024x768_mono16",
00070       "1280x960_yuv422",
00071       "1280x960_rgb8",
00072       "1280x960_mono8",
00073       "1600x1200_yuv422",
00074       "1600x1200_rgb8",
00075       "1600x1200_mono8",
00076       "1280x960_mono16",
00077       "1600x1200_mono16",
00078       "exif",
00079       "format7_mode0",
00080       "format7_mode1",
00081       "format7_mode2",
00082       "format7_mode3",
00083       "format7_mode4",
00084       "format7_mode5",
00085       "format7_mode6",
00086       "format7_mode7"
00087     };
00088 
00094   inline const std::string videoModeName(dc1394video_mode_t mode)
00095   {
00096     if (mode >= DC1394_VIDEO_MODE_MIN
00097         && mode <= DC1394_VIDEO_MODE_MAX)
00098       return video_mode_names_[mode - DC1394_VIDEO_MODE_MIN];
00099     else
00100       return "";
00101   }
00102 
00104   static const std::string color_coding_names_[DC1394_COLOR_CODING_NUM] =
00105     {
00106       "mono8",
00107       "yuv411",
00108       "yuv422",
00109       "yuv444",
00110       "rgb8",
00111       "mono16",
00112       "rgb16",
00113       "mono16s",
00114       "rgb16s",
00115       "raw8",
00116       "raw16",
00117     };
00118 
00124   inline const std::string colorCodingName(dc1394color_coding_t mode)
00125   {
00126     if (mode >= DC1394_COLOR_CODING_MIN
00127         && mode <= DC1394_COLOR_CODING_MAX)
00128       return color_coding_names_[mode - DC1394_COLOR_CODING_MIN];
00129     else
00130       return "";
00131   }
00132 
00134   // public functions:
00136 
00148   dc1394color_coding_t getColorCoding(dc1394camera_t *camera,
00149                                       dc1394video_mode_t video_mode,
00150                                       std::string &color_coding)
00151   {
00152     for (int ccode = DC1394_COLOR_CODING_MIN;
00153          ccode <= DC1394_COLOR_CODING_MAX;
00154          ++ccode)
00155       {
00156         if (color_coding_names_[ccode-DC1394_COLOR_CODING_MIN] == color_coding)
00157           {
00158             // found the requested mode
00159             dc1394color_codings_t ccs;
00160             dc1394error_t err =
00161               dc1394_format7_get_color_codings(camera, video_mode, &ccs);
00162             if (err != DC1394_SUCCESS)
00163               {
00164                 ROS_FATAL("unable to get supported color codings");
00165                 // TODO raise exception
00166                 return (dc1394color_coding_t) 0;
00167               }
00168 
00169             // see if requested mode is available
00170             for (uint32_t i = 0; i < ccs.num; ++i)
00171               {
00172                 if (ccs.codings[i] == ccode)
00173                   return (dc1394color_coding_t) ccode; // yes: success
00174               }
00175 
00176             // requested mode not available, revert to current mode of camera
00177             ROS_ERROR_STREAM("Color coding " << color_coding
00178                              << " not supported by this camera");
00179             dc1394color_coding_t current_mode;
00180             err = dc1394_format7_get_color_coding(camera, video_mode,
00181                                                   &current_mode);
00182             if (err != DC1394_SUCCESS)
00183               {
00184                 ROS_FATAL("unable to get current color coding");
00185                 // TODO raise exception
00186                 return (dc1394color_coding_t) 0;
00187               }
00188 
00189             // TODO list available modes
00190 
00191             // change color_coding parameter to show current mode of camera
00192             color_coding = colorCodingName(current_mode);
00193             return current_mode;
00194           }
00195       }
00196 
00197     // Requested color coding does not match any known string, set to
00198     // "mono8" and update parameter.
00199     ROS_FATAL_STREAM("Unknown color_coding: " << color_coding);
00200     color_coding = colorCodingName(DC1394_COLOR_CODING_MONO8);
00201     return (dc1394color_coding_t) DC1394_COLOR_CODING_MONO8;
00202   }
00203 
00216   dc1394framerate_t getFrameRate(dc1394camera_t *camera,
00217                                  dc1394video_mode_t video_mode,
00218                                  double &frame_rate)
00219   {
00220     // list frame rates supported for this video mode
00221     dc1394framerates_t avail_rates;
00222     dc1394error_t err =
00223       dc1394_video_get_supported_framerates(camera, video_mode, &avail_rates);
00224     if (err != DC1394_SUCCESS)
00225       {
00226         ROS_FATAL("getFrameRate() cannot be used for Format7 modes");
00227         return (dc1394framerate_t) DC1394_FRAMERATE_NUM; // failure
00228       }
00229 
00230     int result = DC1394_FRAMERATE_240;
00231     double rate = 240.0;
00232 
00233     // round frame rate down to next-lower defined value
00234     while (result >= DC1394_FRAMERATE_MIN)
00235       {
00236         for (uint32_t i = 0; i < avail_rates.num; ++i)
00237           {
00238             if (avail_rates.framerates[i] == result
00239                 && rate <= frame_rate)
00240               {
00241                 // update configured rate to match selected value
00242                 frame_rate = rate;
00243                 return (dc1394framerate_t) result;
00244               }
00245           }
00246 
00247         // continue with next-lower possible value
00248         --result;
00249         rate = rate / 2.0;
00250       }
00251 
00252     // no valid frame rate discovered
00253     ROS_ERROR("requested frame_rate (%.3f) not available", frame_rate);
00254     return (dc1394framerate_t) DC1394_FRAMERATE_NUM; // failure
00255   }
00256 
00265   dc1394video_mode_t getVideoMode(dc1394camera_t *camera,
00266                                   std::string &video_mode)
00267   {
00268     for (int vm = DC1394_VIDEO_MODE_MIN;
00269          vm <= DC1394_VIDEO_MODE_MAX;
00270          ++vm)
00271       {
00272         if (video_mode_names_[vm-DC1394_VIDEO_MODE_MIN] == video_mode)
00273           {
00274             // found the requested mode
00275             dc1394video_modes_t vmodes;
00276             dc1394error_t err =
00277               dc1394_video_get_supported_modes(camera, &vmodes);
00278             if (err != DC1394_SUCCESS)
00279               {
00280                 ROS_FATAL("unable to get supported video modes");
00281                 // TODO raise exception
00282                 return (dc1394video_mode_t) 0;
00283               }
00284 
00285             // see if requested mode is available
00286             for (uint32_t i = 0; i < vmodes.num; ++i)
00287               {
00288                 if (vmodes.modes[i] == vm)
00289                   return (dc1394video_mode_t) vm; // yes: success
00290               }
00291 
00292             // requested mode not available, revert to current mode of camera
00293             ROS_ERROR_STREAM("Video mode " << video_mode
00294                              << " not supported by this camera");
00295             dc1394video_mode_t current_mode;
00296             err = dc1394_video_get_mode(camera, &current_mode);
00297             if (err != DC1394_SUCCESS)
00298               {
00299                 ROS_FATAL("unable to get current video mode");
00300                 // TODO raise exception
00301                 return (dc1394video_mode_t) 0;
00302               }
00303 
00304             // TODO list available modes
00305 
00306             // change video_mode parameter to show current mode of camera
00307             video_mode = videoModeName(current_mode);
00308             return current_mode;
00309           }
00310       }
00311 
00312     // request video mode does not match any known string
00313     ROS_FATAL_STREAM("Unknown video_mode:" << video_mode);
00314     ROS_BREAK();
00315     // TODO raise exception
00316     //CAM_EXCEPT(camera1394::Exception, "Unsupported video_mode");
00317     return (dc1394video_mode_t) 0;
00318   }
00319 
00320 
00332   bool setFrameRate(dc1394camera_t *camera,
00333                     dc1394video_mode_t video_mode,
00334                     double &frame_rate)
00335   {
00336     dc1394framerate_t rate = getFrameRate(camera, video_mode, frame_rate);
00337     if (DC1394_FRAMERATE_NUM == rate)
00338       {
00339         ROS_WARN("No valid frame rate");
00340         return false;                   // failure
00341       }
00342     if (DC1394_SUCCESS != dc1394_video_set_framerate(camera, rate))
00343       {
00344         ROS_WARN("Failed to set frame rate");
00345         return false;                   // failure
00346       }
00347     return true;
00348   }
00349 
00360   bool setIsoSpeed(dc1394camera_t *camera, int &iso_speed)
00361   {
00362     // Enable IEEE1394b mode if the camera and bus support it
00363     bool bmode = camera->bmode_capable;
00364     if (bmode
00365         && (DC1394_SUCCESS !=
00366             dc1394_video_set_operation_mode(camera,
00367                                             DC1394_OPERATION_MODE_1394B)))
00368       {
00369         bmode = false;
00370         ROS_WARN("failed to set IEEE1394b mode");
00371       }
00372 
00373     // start with highest speed supported
00374     dc1394speed_t request = DC1394_ISO_SPEED_3200;
00375     int rate = 3200;
00376     if (!bmode)
00377       {
00378         // not IEEE1394b capable: so 400Mb/s is the limit
00379         request = DC1394_ISO_SPEED_400;
00380         rate = 400;
00381       }
00382 
00383     // round requested speed down to next-lower defined value
00384     while (rate > iso_speed)
00385       {
00386         if (request <= DC1394_ISO_SPEED_MIN)
00387           {
00388             // get current ISO speed of the device
00389             dc1394speed_t curSpeed;
00390             if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera, &curSpeed)
00391                 && curSpeed <= DC1394_ISO_SPEED_MAX)
00392               {
00393                 // Translate curSpeed back to an int for the parameter
00394                 // update, works as long as any new higher speeds keep
00395                 // doubling.
00396                 request = curSpeed;
00397                 rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
00398               }
00399             else
00400               {
00401                 ROS_WARN("Unable to get ISO speed; assuming 400Mb/s");
00402                 rate = 400;
00403                 request = DC1394_ISO_SPEED_400;
00404               }
00405             break;
00406           }
00407 
00408         // continue with next-lower possible value
00409         request = (dc1394speed_t) ((int) request - 1);
00410         rate = rate / 2;
00411       }
00412 
00413     // update configured rate to match selected value
00414     iso_speed = rate;
00415 
00416     // set the requested speed
00417     if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera, request))
00418       {
00419         ROS_WARN("Failed to set iso speed");
00420         return false;
00421       }
00422     
00423     return true;
00424   }
00425 
00426 } // namespace Modes


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