format7.cpp
Go to the documentation of this file.
00001 /* $Id$ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (c) 2010 Ken Tossell, 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 
00043 #include <stdint.h>
00044 #include "yuv.h"
00045 #include <sensor_msgs/image_encodings.h>
00046 #include "format7.h"
00047 #include "modes.h"
00048 
00049 
00058 bool Format7::start(dc1394camera_t *camera,
00059                     dc1394video_mode_t mode,
00060                     Config &newconfig)
00061 {
00062   active_ = false;
00063 
00064   // copy Format7 parameters for updateCameraInfo()
00065   binning_x_ = newconfig.binning_x;
00066   binning_y_ = newconfig.binning_y;
00067   roi_.x_offset = newconfig.x_offset;
00068   roi_.y_offset = newconfig.y_offset;
00069   roi_.width = newconfig.roi_width;
00070   roi_.height = newconfig.roi_height;
00071 
00072   uint32_t packet_size = newconfig.format7_packet_size;
00073 
00074   // Built-in libdc1394 Bayer decoding (now deprecated) is not
00075   // supported at all in Format7 modes.
00076   if (newconfig.bayer_method != "")
00077     {
00078       ROS_WARN_STREAM("Bayer method [" << newconfig.bayer_method
00079                       << "] not supported for Format7 modes."
00080                       << "  Using image_proc instead.");
00081       newconfig.bayer_method = "";
00082     }
00083 
00084   const char* pattern = newconfig.bayer_pattern.c_str();
00085   dc1394color_filter_t bayer_pattern = findBayerPattern(pattern);
00086   if ((bayer_pattern >= DC1394_COLOR_FILTER_MIN)
00087       && (bayer_pattern <= DC1394_COLOR_FILTER_MAX))
00088     {
00089       BayerPattern_ = bayer_pattern;
00090     }
00091   else
00092     {
00093       ROS_WARN_STREAM("Bayer pattern [" << newconfig.bayer_pattern << " (" 
00094                       << bayer_pattern << ")] is invalid.");
00095     }
00096 
00097   // scan all Format7 modes to determine the full-sensor image size,
00098   // from which we will calculate the binning values
00099   uint32_t sensor_width = 0, sensor_height = 0;
00100 
00101   for (int scan_mode = DC1394_VIDEO_MODE_FORMAT7_MIN;
00102        scan_mode <= DC1394_VIDEO_MODE_FORMAT7_MAX;
00103        ++scan_mode)
00104     {
00105       uint32_t scan_width, scan_height;
00106 
00107       // TODO: only scan modes supported by this device
00108       if (dc1394_format7_get_max_image_size(camera,
00109                                             (dc1394video_mode_t) scan_mode,
00110                                             &scan_width, &scan_height)
00111           != DC1394_SUCCESS)
00112         continue;
00113 
00114       if (scan_width > sensor_width)
00115         sensor_width = scan_width;
00116 
00117       if (scan_height > sensor_height)
00118         sensor_height = scan_height;
00119     }
00120 
00121   if (DC1394_SUCCESS != dc1394_format7_get_max_image_size(camera, mode,
00122                                                       &maxWidth_,
00123                                                       &maxHeight_))
00124     {
00125       ROS_ERROR("Could not get max image size");
00126       return false;
00127     }
00128 
00129   if (newconfig.binning_x == 0 || newconfig.binning_y == 0)
00130     {
00131       binning_x_ = sensor_width / maxWidth_;
00132       binning_y_ = sensor_height / maxHeight_;
00133     }
00134   else
00135     {
00136       binning_x_ = newconfig.binning_x;
00137       binning_y_ = newconfig.binning_y;
00138     }
00139 
00140   maxWidth_ *= binning_x_;
00141   maxHeight_ *= binning_y_;
00142 
00143   if ((roi_.x_offset | roi_.y_offset | roi_.width | roi_.height) == 0)
00144     {
00145       roi_.width = maxWidth_;
00146       roi_.height = maxHeight_;
00147     }
00148 
00149   uint32_t unit_w, unit_h, unit_x, unit_y;
00150 
00151   if (DC1394_SUCCESS != dc1394_format7_get_unit_size(camera, mode,
00152                                                      &unit_w, &unit_h))
00153     {
00154       ROS_ERROR("Could not get ROI size units");
00155       return false;
00156     }
00157 
00158   unit_w *= binning_x_;
00159   unit_h *= binning_y_;
00160 
00161   if (DC1394_SUCCESS != dc1394_format7_get_unit_position(camera, mode,
00162                                                          &unit_x, &unit_y))
00163     {
00164       ROS_ERROR("Could not get ROI position units");
00165       return false;
00166     }
00167 
00168   // some devices return zeros for the position units
00169   if (unit_x == 0) unit_x = 1;
00170   if (unit_y == 0) unit_y = 1;
00171 
00172   unit_x *= binning_x_;
00173   unit_y *= binning_y_;
00174 
00175   ROS_INFO_STREAM("Format7 unit size: ("
00176                   << unit_w << "x" << unit_h
00177                   << "), position: ("
00178                   << unit_x << "x" << unit_y
00179                   << ")");
00180   ROS_INFO_STREAM("Format7 region size: ("
00181                   << roi_.width << "x" << roi_.height
00182                   << "), offset: ("
00183                   << roi_.x_offset << ", " << roi_.y_offset
00184                   << ")");
00185 
00186   /* Reset ROI position to (0,0). If it was previously (x,y) and
00187    * the requested ROI size (w,h) results in (x,y) + (w,h) >
00188    * (max_w,max_h), we'll be unable to set up some valid ROIs
00189    */
00190   dc1394_format7_set_image_position(camera, mode, 0, 0);
00191 
00192   if ((roi_.width % unit_w) || (roi_.height % unit_h))
00193     {
00195       ROS_ERROR("Requested image size invalid; (w,h) must be"
00196                 " a multiple of (%d, %d)", unit_w, unit_h);
00197       return false;
00198     }
00199 
00200   uint32_t binned_width = roi_.width / binning_x_;
00201   uint32_t binned_height = roi_.height / binning_y_;
00202 
00203   uint32_t binned_x_offset = roi_.x_offset / binning_x_;
00204   uint32_t binned_y_offset = roi_.y_offset / binning_y_;
00205 
00206   if (DC1394_SUCCESS != dc1394_format7_set_image_size(camera, mode,
00207                                                       binned_width,
00208                                                       binned_height))
00209     {
00210       ROS_ERROR("Could not set size of ROI");
00211       return false;
00212     }
00213 
00214   if ((roi_.x_offset % unit_x) || (roi_.y_offset % unit_y))
00215     {
00216       ROS_ERROR("Requested image position invalid; (x,y) must"
00217                 " be a multiple of (%d, %d)", unit_x, unit_y);
00218       return false;
00219     }
00220 
00221   if (DC1394_SUCCESS != dc1394_format7_set_image_position(camera, mode,
00222                                                           binned_x_offset,
00223                                                           binned_y_offset))
00224     {
00225       ROS_ERROR("Could not set position of ROI");
00226       return false;
00227     }
00228 
00229   // Try to set requested color coding. Use current camera value if
00230   // requested coding is not supported by the camera.
00231   coding_ = Modes::getColorCoding(camera, mode,
00232                                   newconfig.format7_color_coding);
00233 
00234   if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera, mode,
00235                                                         coding_))
00236     {
00237       ROS_ERROR("Could not set color coding");
00238       return false;
00239     }
00240       
00241   uint32_t rec_packet_size;
00242 
00243   if (DC1394_SUCCESS
00244       != dc1394_format7_get_recommended_packet_size(camera, mode,
00245                                                     &rec_packet_size))
00246     {
00247       ROS_ERROR("Could not get default packet size");
00248       return false;
00249     }
00250 
00251   if (0 == packet_size)
00252     packet_size = rec_packet_size;
00253 
00254   uint32_t unit_bytes, max_bytes;
00255 
00256   if (DC1394_SUCCESS
00257       != dc1394_format7_get_packet_parameters(camera, mode,
00258                                               &unit_bytes, &max_bytes))
00259     {
00260       ROS_ERROR("Could not determine maximum and increment for packet size");
00261       return false;
00262     }
00263 
00264   if (packet_size % unit_bytes
00265       || (max_bytes > 0 && packet_size > max_bytes))
00266     {
00267       ROS_ERROR("Invalid packet size: %d. Must be a "
00268                 "multiple of %d, at most %d [%d]",
00269                 packet_size, unit_bytes, max_bytes, rec_packet_size);
00270       return false;
00271     }
00272 
00273   if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera, mode,
00274                                                        packet_size))
00275     {
00276       ROS_ERROR("Could not set packet size");
00277       return false;
00278     }
00279 
00280   if (coding_ == DC1394_COLOR_CODING_RAW8
00281       || coding_ == DC1394_COLOR_CODING_RAW16)
00282     {
00283       dc1394color_filter_t
00284         color_filter = (dc1394color_filter_t) DC1394_COLOR_FILTER_NUM;
00285       if (DC1394_SUCCESS != dc1394_format7_get_color_filter(camera, mode,
00286                                                             &color_filter))
00287         {
00288           ROS_ERROR("Could not determine color pattern");
00289           return false;
00290         }
00291       if (BayerPattern_ == (dc1394color_filter_t) DC1394_COLOR_FILTER_NUM)
00292         {
00293           BayerPattern_ = color_filter;
00294         }
00295       else if ( BayerPattern_ != color_filter )
00296         {
00297           ROS_WARN_STREAM("Bayer Pattern was set to "
00298                           << BayerPattern_ << "but get_color_filter returned "
00299                           << color_filter << ". Using " << BayerPattern_);
00300         }
00301     }
00302 
00303   active_ = true;                       // Format7 mode is active
00304 
00305   return true;
00306 }
00307 
00309 void Format7::stop(void)
00310 {
00311   active_ = false;
00312 }
00313 
00314 extern std::string bayer_string(dc1394color_filter_t pattern,
00315                                 unsigned int bits);
00316 
00318 void Format7::unpackData(sensor_msgs::Image &image, uint8_t *capture_buffer)
00319 {
00320   int image_size;  
00321   switch (coding_)
00322     {
00323     case DC1394_COLOR_CODING_MONO8:
00324       image.step = image.width;
00325       image_size = image.height * image.step;
00326       image.encoding = sensor_msgs::image_encodings::MONO8;
00327       image.is_bigendian = false;
00328       image.data.resize(image_size);
00329       memcpy(&image.data[0], capture_buffer, image_size);
00330       break;
00331     case DC1394_COLOR_CODING_YUV411:
00332       image.step = image.width * 3;
00333       image_size = image.height * image.step;
00334       image.encoding = sensor_msgs::image_encodings::RGB8;
00335       image.data.resize(image_size);
00336       yuv::uyyvyy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00337                       reinterpret_cast<unsigned char *> (&image.data[0]),
00338                       image.width * image.height);
00339       break;
00340     case DC1394_COLOR_CODING_YUV422:
00341       image.step = image.width * 3;
00342       image_size = image.height * image.step;
00343       image.encoding = sensor_msgs::image_encodings::RGB8;
00344       image.data.resize(image_size);
00345       yuv::uyvy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00346                     reinterpret_cast<unsigned char *> (&image.data[0]),
00347                     image.width * image.height);
00348       break;
00349     case DC1394_COLOR_CODING_YUV444:
00350       image.step=image.width * 3;
00351       image_size = image.height * image.step;
00352       image.encoding = sensor_msgs::image_encodings::RGB8;
00353       image.data.resize(image_size);
00354       yuv::uyv2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00355                    reinterpret_cast<unsigned char *> (&image.data[0]),
00356                    image.width * image.height);
00357       break;
00358     case DC1394_COLOR_CODING_RGB8:
00359       image.step=image.width*3;
00360       image_size = image.height*image.step;
00361       image.encoding = sensor_msgs::image_encodings::RGB8;
00362       image.data.resize(image_size);
00363       memcpy(&image.data[0], capture_buffer, image_size);
00364       break;
00365     case DC1394_COLOR_CODING_MONO16:
00366       image.step=image.width*2;
00367       image_size = image.height*image.step;
00368       image.encoding = sensor_msgs::image_encodings::MONO16;
00369       image.is_bigendian = true;
00370       image.data.resize(image_size);
00371       memcpy(&image.data[0], capture_buffer, image_size);
00372       break;
00373     case DC1394_COLOR_CODING_RGB16:
00374       image.step = image.width * 6;
00375       image_size = image.height * image.step;
00376       image.encoding = sensor_msgs::image_encodings::TYPE_16UC3;
00377       image.is_bigendian = true;
00378       image.data.resize(image_size);
00379       memcpy(&image.data[0], capture_buffer, image_size);
00380       break;
00381     case DC1394_COLOR_CODING_MONO16S:
00382       image.step = image.width * 2;
00383       image_size = image.height * image.step;
00384       image.encoding = sensor_msgs::image_encodings::TYPE_16SC1;
00385       image.is_bigendian = true;
00386       image.data.resize(image_size);
00387       memcpy(&image.data[0], capture_buffer, image_size);
00388       break;
00389     case DC1394_COLOR_CODING_RGB16S:
00390       image.step = image.width * 6;
00391       image_size = image.height * image.step;
00392       image.encoding = sensor_msgs::image_encodings::TYPE_16SC3;
00393       image.is_bigendian = true;
00394       image.data.resize(image_size);
00395       memcpy(&image.data[0], capture_buffer, image_size);
00396       break;
00397     case DC1394_COLOR_CODING_RAW8:
00398       image.step = image.width;
00399       image_size = image.height * image.step;
00400       image.encoding = bayer_string(BayerPattern_, 8);
00401       image.data.resize(image_size);
00402       memcpy(&image.data[0], capture_buffer, image_size);
00403       break;
00404     case DC1394_COLOR_CODING_RAW16:
00405       image.step = image.width * 2;
00406       image_size = image.height * image.step;
00407       image.encoding = bayer_string(BayerPattern_, 16);
00408       image.is_bigendian = true;
00409       image.data.resize(image_size);
00410       memcpy(&image.data[0], capture_buffer, image_size);
00411       break;
00412     default:
00413       ROS_ERROR_STREAM("Driver bug: unknown Format7 color coding:"
00414                        << coding_);
00415       ROS_BREAK();
00416     }
00417 }
00418 
00429 bool Format7::checkCameraInfo(const sensor_msgs::CameraInfo &cinfo)
00430 {
00431   // see if the (full) image size matches the calibration
00432   if (cinfo.width == maxWidth_ && cinfo.height == maxHeight_)
00433     {
00434       return true;
00435     }
00436   // or if the ROI size matches the calibration
00437   else if (cinfo.width == roi_.width && cinfo.height == roi_.height)
00438     {
00439       return true;
00440     }
00441   else
00442     {
00443       ROS_WARN_STREAM_THROTTLE(30, "Calibrated image size ("
00444                                << cinfo.width << "x" << cinfo.height
00445                                << ") matches neither full Format7 size ("
00446                                << maxWidth_ << "x" << maxHeight_ << ")"
00447                                << ") nor ROI size ("
00448                                << roi_.width << "x" << roi_.height << ")");
00449       return false;
00450     }
00451 }
00452 
00463 void Format7::setOperationalParameters(sensor_msgs::CameraInfo &cinfo)
00464 {
00465   // copy the operational data determined during start()
00466   cinfo.binning_x = binning_x_;
00467   cinfo.binning_y = binning_y_;
00468   cinfo.roi = roi_;
00469 
00470   // set do_rectify depending on current calibration parameters
00471   cinfo.roi.do_rectify = false;
00472 
00473   if (cinfo.K[0] == 0.0)
00474     return;                             // uncalibrated
00475 
00476   bool roiMatchesCalibration = (cinfo.width == roi_.width
00477                                 && cinfo.height == roi_.height);
00478 
00479   if (cinfo.width == maxWidth_ && cinfo.height == maxHeight_)
00480     {
00481       // calibration matches full image size
00482       if (!roiMatchesCalibration)
00483         {
00484           // ROI is not full image: tell image_pipeline to rectify
00485           cinfo.roi.do_rectify = true;
00486         }
00487     }
00488   else
00489     {
00490       // calibration differs from full image
00491       if (!roiMatchesCalibration)
00492         {
00493           // calibrated size is neither full image nor current ROI:
00494           //   tell image_pipeline to rectify the data.
00495           cinfo.roi.do_rectify = true;
00496         }
00497     }
00498 }
00499 
00506 dc1394color_filter_t Format7::findBayerPattern(const char* bayer)
00507 {
00508   // determine Bayer color encoding pattern
00509   // (default is different from any color filter provided by DC1394)
00510   dc1394color_filter_t
00511     pattern = (dc1394color_filter_t) DC1394_COLOR_FILTER_NUM;
00512 
00513   if (0 == strcmp(bayer, "bggr"))
00514     {
00515       pattern = DC1394_COLOR_FILTER_BGGR;
00516     }
00517   else if (0 == strcmp(bayer, "grbg"))
00518     {
00519       pattern = DC1394_COLOR_FILTER_GRBG;
00520     }
00521   else if (0 == strcmp(bayer, "rggb"))
00522     {
00523       pattern = DC1394_COLOR_FILTER_RGGB;
00524     }
00525   else if (0 == strcmp(bayer, "gbrg"))
00526     {
00527       pattern = DC1394_COLOR_FILTER_GBRG;
00528     }
00529   else if (0 != strcmp(bayer, ""))
00530     {
00531       ROS_ERROR("unknown bayer pattern [%s]", bayer);
00532     }
00533   return pattern;
00534 }
00535 


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