dev_camera1394stereo.cpp
Go to the documentation of this file.
00001 
00002 // Joan Pau Beltran
00003 //  Modificat per donar suport a les cameres Bumblebee2 de Pointgrey.
00004 //
00005 // Copyright (C) 2009, 2010 Patrick Beeson, Jack O'Quin, Ken Tossell
00006 //  ROS port of the Player 1394 camera driver.
00007 //
00008 // Copyright (C) 2004 Nate Koenig, Andrew Howard
00009 //  Player driver for IEEE 1394 digital camera capture
00010 //
00011 // Copyright (C) 2000-2003 Damien Douxchamps, Dan Dennedy
00012 //  Bayer filtering from libdc1394
00013 //
00014 // NOTE: On 4 Jan. 2011, this file was re-licensed under the GNU LGPL
00015 // with permission of the original GPL authors: Nate Koenig, Andrew
00016 // Howard, Damien Douxchamps and Dan Dennedy.
00017 //
00018 // This program is free software; you can redistribute it and/or
00019 // modify it under the terms of the GNU Lesser General Public License
00020 // as published by the Free Software Foundation; either version 2 of
00021 // the License, or (at your option) any later version.
00022 // 
00023 // This program is distributed in the hope that it will be useful, but
00024 // WITHOUT ANY WARRANTY; without even the implied warranty of
00025 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00026 // Lesser General Public License for more details.
00027 // 
00028 // You should have received a copy of the GNU Lesser General Public
00029 // License along with this program; if not, write to the Free Software
00030 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
00031 // 02111-1307, USA.
00032 //
00034 
00035 // $Id: dev_camera1394.cpp 35607 2011-01-30 01:28:30Z joq $
00036 
00051 #include <stdint.h>
00052 
00053 #include "yuv.h"
00054 #include <sensor_msgs/image_encodings.h>
00055 #include "dev_camera1394stereo.h"
00056 #include "featuresstereo.h"
00057 #include "modes.h"
00058 
00059 #define NUM_DMA_BUFFERS 4
00060 
00061 // @todo eliminate these macros
00063 #define CAM_EXCEPT(except, msg)                                 \
00064   {                                                             \
00065     char buf[100];                                              \
00066     snprintf(buf, 100, "[Camera1394Stereo::%s]: " msg, __FUNCTION__); \
00067     throw except(buf);                                          \
00068   }
00069 
00071 #define CAM_EXCEPT_ARGS(except, msg, ...)                               \
00072   {                                                                     \
00073     char buf[100];                                                      \
00074     snprintf(buf, 100, "[Camera1394Stereo::%s]: " msg, __FUNCTION__, __VA_ARGS__); \
00075     throw except(buf);                                                  \
00076   }
00077 
00078 
00079 using namespace camera1394stereo;
00080 
00082 // Constructor
00083 Camera1394Stereo::Camera1394Stereo():
00084   camera_(NULL)
00085 {}
00086 
00087 Camera1394Stereo::~Camera1394Stereo() 
00088 {
00089   SafeCleanup();
00090 }
00091 
00092 void Camera1394Stereo::findBayerPattern(const char* bayer)
00093 {
00094   // determine Bayer color encoding pattern
00095   // (default is different from any color filter provided by DC1394)
00096   BayerPattern_ = (dc1394color_filter_t) DC1394_COLOR_FILTER_NUM;
00097   if (0 == strcmp(bayer, "bggr"))
00098     {
00099       BayerPattern_ = DC1394_COLOR_FILTER_BGGR;
00100     }
00101   else if (0 == strcmp(bayer, "grbg"))
00102     {
00103       BayerPattern_ = DC1394_COLOR_FILTER_GRBG;
00104     }
00105   else if (0 == strcmp(bayer, "rggb"))
00106     {
00107       BayerPattern_ = DC1394_COLOR_FILTER_RGGB;
00108     }
00109   else if (0 == strcmp(bayer, "gbrg"))
00110     {
00111       BayerPattern_ = DC1394_COLOR_FILTER_GBRG;
00112     }
00113   else if (0 != strcmp(bayer, ""))
00114     {
00115       ROS_ERROR("unknown bayer pattern [%s]", bayer);
00116     }
00117 }
00118 
00119 bool Camera1394Stereo::findBayerMethod(const char* method)
00120 {
00121   // Do Bayer conversion in the driver node?
00122   bool DoBayer = false;                 // return value
00123   if (0 != strcmp(method, "")
00124       && BayerPattern_ != DC1394_COLOR_FILTER_NUM)
00125     {
00126       DoBayer = true;                   // decoding in driver
00127       // add method name to message:
00128       ROS_WARN("[%s] Bayer decoding in the driver is DEPRECATED;"
00129                " image_proc decoding preferred.", method);
00130 
00131       // Set decoding method
00132       if (!strcmp(method, "DownSample"))
00133         BayerMethod_ = DC1394_BAYER_METHOD_DOWNSAMPLE;
00134       else if (!strcmp(method, "Simple"))
00135         BayerMethod_ = DC1394_BAYER_METHOD_SIMPLE;
00136       else if (!strcmp(method, "Bilinear"))
00137         BayerMethod_ = DC1394_BAYER_METHOD_BILINEAR;
00138       else if (!strcmp(method, "HQ"))
00139         BayerMethod_ = DC1394_BAYER_METHOD_HQLINEAR;
00140       else if (!strcmp(method, "VNG"))
00141         BayerMethod_ = DC1394_BAYER_METHOD_VNG;
00142       else if (!strcmp(method, "AHD"))
00143         BayerMethod_ = DC1394_BAYER_METHOD_AHD;
00144       else
00145         {
00146           ROS_ERROR("Unknown Bayer method [%s]. Using ROS image_proc instead.",
00147                     method);
00148           DoBayer = false;
00149         }
00150     }
00151   return DoBayer;
00152 }
00153 
00154 bool Camera1394Stereo::findStereoMethod(const char* method)
00155 {
00156   bool doStereo = true;
00157   if (0 == strcmp(method, "Interlaced"))
00158     {
00159       stereoMethod_ = DC1394_STEREO_METHOD_INTERLACED;
00160     }
00161   else if (0 == strcmp(method, "Field"))
00162     {
00163       stereoMethod_ = DC1394_STEREO_METHOD_FIELD;
00164     }
00165   else
00166     {
00167       doStereo = false;
00168     }
00169     
00170   return doStereo;
00171 }
00172 
00183 int Camera1394Stereo::open(camera1394stereo::Camera1394StereoConfig &newconfig)
00184 {
00186   // First, look for the camera
00188 
00189   const char *guid = newconfig.guid.c_str();  // C-style GUID for libdc1394
00190   int err;
00191   dc1394_t *d;
00192   dc1394camera_list_t *list;
00193 
00194   // TODO: make error exit paths clean up resources properly
00195   d = dc1394_new ();
00196   if (d == NULL)
00197     {
00198       CAM_EXCEPT(camera1394stereo::Exception,
00199                  "Could not initialize dc1394_context.\n"
00200                  "Make sure /dev/raw1394 exists, you have access permission,\n"
00201                  "and libraw1394 development package is installed.");
00202     }
00203 
00204   err = dc1394_camera_enumerate(d, &list);
00205   if (err != DC1394_SUCCESS)
00206     {
00207       CAM_EXCEPT(camera1394stereo::Exception, "Could not get camera list");
00208       return -1;
00209     }
00210   
00211   if (list->num == 0)
00212     {
00213       CAM_EXCEPT(camera1394stereo::Exception, "No cameras found");
00214       return -1;
00215     }
00216   
00217   char* temp=(char*)malloc(1024*sizeof(char));
00218   for (unsigned i=0; i < list->num; i++)
00219     {
00220       // Create a camera
00221       camera_ = dc1394_camera_new (d, list->ids[i].guid);
00222       if (!camera_)
00223         ROS_WARN_STREAM("Failed to initialize camera with GUID "
00224                         << std::hex << list->ids[i].guid);
00225       else
00226         ROS_INFO_STREAM("Found camera with GUID "
00227                         << std::hex << list->ids[i].guid);
00228 
00229       uint32_t value[3];
00230       
00231       value[0]= camera_->guid & 0xffffffff;
00232       value[1]= (camera_->guid >>32) & 0x000000ff;
00233       value[2]= (camera_->guid >>40) & 0xfffff;
00234       
00235       sprintf(temp,"%06x%02x%08x", value[2], value[1], value[0]);
00236 
00237       if (strcmp(guid,"")==0)
00238         {
00239           ROS_INFO_STREAM("No guid specified, using first camera found, GUID: "
00240                           << std::hex << camera_->guid);
00241           device_id_ = std::string(temp);
00242           break;
00243         }
00244 
00245       ROS_DEBUG("Comparing %s to %s",guid,temp);
00246       if (strcmp(temp,guid)==0)
00247         {
00248           device_id_=guid;
00249           break;
00250         }
00251       SafeCleanup();
00252     }
00253   free (temp);
00254   dc1394_camera_free_list (list);
00255   
00256   if (!camera_)
00257     {
00258       if (strcmp(guid,"")==0)
00259         { 
00260           CAM_EXCEPT(camera1394stereo::Exception, "Could not find camera");
00261         }
00262       else
00263         {
00264           CAM_EXCEPT_ARGS(camera1394stereo::Exception,
00265                           "Could not find camera with guid %s", guid);
00266         }
00267       return -1;
00268     }
00269 
00270   ROS_INFO_STREAM("camera model: " << camera_->vendor
00271                   << " " << camera_->model);
00272 
00274   // initialize camera
00276 
00277   // resetting some cameras is not a good idea
00278   if (newconfig.reset_on_open
00279       && DC1394_SUCCESS != dc1394_camera_reset(camera_))
00280     {
00281       // reset failed: log a warning, but continue
00282       ROS_WARN("Unable to reset camera (continuing).");
00283     }
00284 
00285   // first, set parameters that are common between Format7 and other modes
00286   if (false == Modes::setIsoSpeed(camera_, newconfig.iso_speed))
00287     {
00288       SafeCleanup();
00289       CAM_EXCEPT(camera1394stereo::Exception,
00290                  "Unable to set ISO speed; is the camera plugged in?");
00291       return -1;
00292     }
00293 
00294   // set video mode
00295   videoMode_ = Modes::getVideoMode(camera_, newconfig.video_mode);
00296   if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode_))
00297     {
00298       SafeCleanup();
00299       CAM_EXCEPT(camera1394stereo::Exception, "Failed to set video mode");
00300       return -1;
00301     }
00302 
00304   // special handling for Format7 modes
00306 
00307   findBayerPattern(newconfig.bayer_pattern.c_str());
00308   DoBayerConversion_ = findBayerMethod(newconfig.bayer_method.c_str());
00309   DoStereoExtract_ = findStereoMethod(newconfig.stereo_method.c_str());
00310 
00311   if (dc1394_is_video_mode_scalable(videoMode_) == DC1394_TRUE)
00312     {
00313       // set Format7 parameters
00314       if (!format7_.start(camera_, videoMode_, newconfig))
00315         {
00316           SafeCleanup();
00317           CAM_EXCEPT(camera1394stereo::Exception, "Format7 start failed");
00318           return -1;
00319         }
00320     }
00321   else
00322     {
00323       // Set frame rate and Bayer method (only valid for non-Format7 modes)
00324       if (!Modes::setFrameRate(camera_, videoMode_, newconfig.frame_rate))
00325         {
00326           SafeCleanup();
00327           CAM_EXCEPT(camera1394stereo::Exception, "Failed to set frame rate");
00328           return -1;
00329         }
00330     }
00331 
00333   // get current color coding
00335   if (DC1394_SUCCESS !=
00336       dc1394_get_color_coding_from_video_mode(camera_, videoMode_, &colorCoding_) )
00337     {
00338       SafeCleanup();
00339       CAM_EXCEPT(camera1394stereo::Exception, "Format7 start failed");
00340       return -1;
00341     }
00342 
00344   // start the device streaming data
00346 
00347   // Set camera to use DMA, improves performance.
00348   if (DC1394_SUCCESS != dc1394_capture_setup(camera_, NUM_DMA_BUFFERS,
00349                                              DC1394_CAPTURE_FLAGS_DEFAULT))
00350     {
00351       SafeCleanup();
00352       CAM_EXCEPT(camera1394stereo::Exception, "Failed to open device!");
00353       return -1;
00354     }
00355   
00356   // Start transmitting camera data
00357   if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
00358     {
00359       SafeCleanup();
00360       CAM_EXCEPT(camera1394stereo::Exception, "Failed to start device!");
00361       return -1;
00362     }
00363 
00365   // initialize feature settings
00367 
00368   // TODO: pass newconfig here and eliminate initialize() method
00369   features_.reset(new Features(camera_));
00370  
00371   return 0;
00372 }
00373 
00374 
00376 void Camera1394Stereo::SafeCleanup()
00377 {
00378   if (camera_)
00379     {
00380       format7_.stop();
00381       dc1394_capture_stop(camera_);
00382       dc1394_camera_free(camera_);
00383       camera_ = NULL;
00384     }
00385 }
00386 
00387 
00389 int Camera1394Stereo::close()
00390 {
00391   if (camera_)
00392     {
00393       if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF)
00394           || DC1394_SUCCESS != dc1394_capture_stop(camera_))
00395         ROS_WARN("unable to stop camera");
00396     }
00397 
00398   // Free resources
00399   SafeCleanup();
00400 
00401   return 0;
00402 }
00403 
00404 std::string bayer_string(dc1394color_filter_t pattern, unsigned int bits)
00405 {
00406   if (bits == 8)
00407   {
00408     switch (pattern)
00409       {
00410       case DC1394_COLOR_FILTER_RGGB:
00411         return sensor_msgs::image_encodings::BAYER_RGGB8;
00412       case DC1394_COLOR_FILTER_GBRG:
00413         return sensor_msgs::image_encodings::BAYER_GBRG8;
00414       case DC1394_COLOR_FILTER_GRBG:
00415         return sensor_msgs::image_encodings::BAYER_GRBG8;
00416       case DC1394_COLOR_FILTER_BGGR:
00417         return sensor_msgs::image_encodings::BAYER_BGGR8;
00418       default:
00419         return sensor_msgs::image_encodings::MONO8;
00420       }
00421   }
00422   else if (bits == 16)
00423   {
00424     // FIXME: should add bayer_XXXX16 modes to sensor_msgs?
00425     return sensor_msgs::image_encodings::MONO16;
00426   }
00427 
00428   return sensor_msgs::image_encodings::MONO8;
00429 }
00430 
00432 void Camera1394Stereo::readData(
00433     sensor_msgs::Image& image, 
00434     sensor_msgs::Image& image2)
00435 {
00436   ROS_ASSERT_MSG(camera_, "Attempt to read from camera that is not open.");
00437 
00438   dc1394video_frame_t * frame = NULL;
00439   dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
00440   if (!frame)
00441     {
00442       CAM_EXCEPT(camera1394stereo::Exception, "Unable to capture frame");
00443       return;
00444     }
00445   
00446   dc1394video_frame_t frame1 = *frame;
00447 
00448   if (DoStereoExtract_)
00449     {
00450       // deinterlace frame into two images one on top the other
00451       size_t frame1_size = frame->total_bytes;
00452       frame1.image = (unsigned char *) malloc(frame1_size);
00453       frame1.allocated_image_bytes = frame1_size;
00454       switch (frame->color_coding)
00455         {
00456         case DC1394_COLOR_CODING_MONO16:
00457           frame1.color_coding = DC1394_COLOR_CODING_MONO8;
00458           break;
00459         case DC1394_COLOR_CODING_RAW16:
00460           frame1.color_coding = DC1394_COLOR_CODING_RAW8;
00461           break;
00462         default:
00463           ROS_WARN("Stereo extract in a non 16bit video mode");
00464         };
00465         int err = dc1394_deinterlace_stereo_frames(frame, &frame1, stereoMethod_);
00466         if (err != DC1394_SUCCESS)
00467         {
00468           free(frame1.image);
00469           dc1394_capture_enqueue(camera_, frame);
00470           CAM_EXCEPT(camera1394stereo::Exception, "Could not extract stereo frames");
00471           return;
00472         }
00473     }
00474 
00475   uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
00476 
00477   if (DoBayerConversion_)
00478     {
00479       // debayer frame into RGB8
00480       dc1394video_frame_t frame2;
00481       size_t frame2_size = (frame1.size[0] * frame1.size[1]
00482                             * 3 * sizeof(unsigned char));
00483       frame2.image = (unsigned char *) malloc(frame2_size);
00484       frame2.allocated_image_bytes = frame2_size;
00485       frame2.color_coding = DC1394_COLOR_CODING_RGB8;
00486       frame1.color_filter = BayerPattern_;
00487       int err = dc1394_debayer_frames(&frame1, &frame2, BayerMethod_);
00488       if (err != DC1394_SUCCESS)
00489         {
00490           free(frame2.image);
00491           dc1394_capture_enqueue(camera_, frame);
00492           CAM_EXCEPT(camera1394stereo::Exception, "Could not convert/debayer frames");
00493           return;
00494         }
00495       capture_buffer = reinterpret_cast<uint8_t *>(frame2.image);
00496     }
00497 
00498   ROS_ASSERT(capture_buffer);   
00499 
00500   image.header.stamp = ros::Time( double(frame->timestamp) * 1.e-6 );
00501   image.width = frame->size[0];
00502   image.height = frame->size[1];
00503   image2.header.stamp = image.header.stamp;
00504   image2.width = 0;
00505   image2.height = 0;
00506   image2.step = 0;
00507 
00508   int image_size;
00509   switch (colorCoding_)
00510     {
00511     case DC1394_COLOR_CODING_YUV444:
00512       image.step=image.width*3;
00513       image_size = image.height*image.step;
00514       image.encoding = sensor_msgs::image_encodings::RGB8;
00515       image.data.resize(image_size);
00516       yuv::uyv2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00517                    reinterpret_cast<unsigned char *> (&image.data[0]),
00518                    image.width * image.height);
00519       break;
00520     case DC1394_COLOR_CODING_YUV411:
00521       image.step=image.width*3;
00522       image_size = image.height*image.step;
00523       image.encoding = sensor_msgs::image_encodings::RGB8;
00524       image.data.resize(image_size);
00525       yuv::uyyvyy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00526                       reinterpret_cast<unsigned char *> (&image.data[0]),
00527                       image.width * image.height);
00528       break;
00529     case DC1394_COLOR_CODING_YUV422:
00530       image.step=image.width*3;
00531       image_size = image.height*image.step;
00532       image.encoding = sensor_msgs::image_encodings::RGB8;
00533       image.data.resize(image_size);
00534       yuv::uyvy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00535                     reinterpret_cast<unsigned char *> (&image.data[0]),
00536                     image.width * image.height);
00537       break;
00538     case DC1394_COLOR_CODING_RGB8:
00539       image.step=image.width*3;
00540       image_size = image.height*image.step;
00541       image.encoding = sensor_msgs::image_encodings::RGB8;
00542       image.data.resize(image_size);
00543       memcpy(&image.data[0], capture_buffer, image_size);
00544       break;
00545     case DC1394_COLOR_CODING_MONO8:
00546     case DC1394_COLOR_CODING_RAW8:
00547       if (!DoBayerConversion_)
00548         {
00549           image.step=image.width;
00550           image_size = image.height*image.step;
00551           // set Bayer encoding in ROS Image message
00552           image.encoding = bayer_string(BayerPattern_, 8);
00553           image.data.resize(image_size);
00554           memcpy(&image.data[0], capture_buffer, image_size);
00555         }
00556       else
00557         {
00558           image.step=image.width*3;
00559           image_size = image.height*image.step;
00560           image.encoding = sensor_msgs::image_encodings::RGB8;
00561           image.data.resize(image_size);
00562           memcpy(&image.data[0], capture_buffer, image_size);
00563         } 
00564       break;
00565     case DC1394_COLOR_CODING_MONO16:
00566     case DC1394_COLOR_CODING_RAW16:
00567       if (DoStereoExtract_ && DoBayerConversion_)
00568         {
00569           image2.width = image.width;
00570           image2.height = image.height;
00571           image2.step = image.step = image.width*3;
00572           image_size = image.height*image.step;
00573           image2.encoding = image.encoding = sensor_msgs::image_encodings::RGB8;
00574           image.data.resize(image_size);
00575           image2.data.resize(image_size);
00576           memcpy(&image.data[0], capture_buffer, image_size);
00577           memcpy(&image2.data[0], capture_buffer+image_size, image_size);
00578         }
00579       else if (DoStereoExtract_)
00580         {
00581           image2.width = image.width;
00582           image2.height = image.height;
00583           image.step = image.width;
00584           image2.step = image.step;
00585           image_size = image.height*image.step;
00586           image.encoding = bayer_string(BayerPattern_, 8);
00587           image2.encoding = image.encoding;
00588           image.data.resize(image_size);
00589           image2.data.resize(image_size);
00590           memcpy(&image.data[0], capture_buffer, image_size);
00591           memcpy(&image2.data[0], capture_buffer+image_size, image_size);
00592         }
00593       else if (DoBayerConversion_)
00594         {
00595           // @todo test Bayer conversions for mono16
00596           image.step=image.width*3;
00597           image_size = image.height*image.step;
00598           image.encoding = sensor_msgs::image_encodings::RGB8;
00599           image.data.resize(image_size);
00600           memcpy(&image.data[0], capture_buffer, image_size);
00601         } 
00602       else
00603         {
00604           image.step=image.width*2;
00605           image_size = image.height*image.step;
00606           image.encoding = bayer_string(BayerPattern_, 16);
00607           image.is_bigendian = false;    // check Bumblebee2 endianness
00608           image.data.resize(image_size);
00609           memcpy(&image.data[0], capture_buffer, image_size);
00610         }
00611       break;
00612     default:
00613         {
00614           CAM_EXCEPT(camera1394stereo::Exception, "Unknown image mode and coding");
00615           return;
00616         }
00617     }
00618 
00619   dc1394_capture_enqueue(camera_, frame);
00620 
00621   if (DoStereoExtract_ || DoBayerConversion_) 
00622     { 
00623       free(capture_buffer);
00624       if (DoStereoExtract_ && DoBayerConversion_)
00625          free(frame1.image); 
00626     }
00627 }


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