dev_camera1394.cpp
Go to the documentation of this file.
00001 
00002 //
00003 // Copyright (C) 2009, 2010, 2012 Patrick Beeson, Jack O'Quin, Ken Tossell
00004 //  ROS port of the Player 1394 camera driver.
00005 //
00006 // Copyright (C) 2004 Nate Koenig, Andrew Howard
00007 //  Player driver for IEEE 1394 digital camera capture
00008 //
00009 // Copyright (C) 2000-2003 Damien Douxchamps, Dan Dennedy
00010 //  Bayer filtering from libdc1394
00011 //
00012 // NOTE: On 4 Jan. 2011, this file was re-licensed under the GNU LGPL
00013 // with permission of the original GPL authors: Nate Koenig, Andrew
00014 // Howard, Damien Douxchamps and Dan Dennedy.
00015 //
00016 // This program is free software; you can redistribute it and/or
00017 // modify it under the terms of the GNU Lesser General Public License
00018 // as published by the Free Software Foundation; either version 2 of
00019 // the License, or (at your option) any later version.
00020 // 
00021 // This program is distributed in the hope that it will be useful, but
00022 // WITHOUT ANY WARRANTY; without even the implied warranty of
00023 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00024 // Lesser General Public License for more details.
00025 // 
00026 // You should have received a copy of the GNU Lesser General Public
00027 // License along with this program; if not, write to the Free Software
00028 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
00029 // 02111-1307, USA.
00030 //
00032 
00033 // $Id$
00034 
00049 #include <stdint.h>
00050 
00051 #include "yuv.h"
00052 #include <sensor_msgs/image_encodings.h>
00053 #include "dev_camera1394.h"
00054 #include "features.h"
00055 #include "modes.h"
00056 
00057 #define NUM_DMA_BUFFERS 4
00058 
00059 // @todo eliminate these macros
00061 #define CAM_EXCEPT(except, msg)                                 \
00062   {                                                             \
00063     char buf[100];                                              \
00064     snprintf(buf, 100, "[Camera1394::%s]: " msg, __FUNCTION__); \
00065     throw except(buf);                                          \
00066   }
00067 
00069 #define CAM_EXCEPT_ARGS(except, msg, ...)                               \
00070   {                                                                     \
00071     char buf[100];                                                      \
00072     snprintf(buf, 100, "[Camera1394::%s]: " msg, __FUNCTION__, __VA_ARGS__); \
00073     throw except(buf);                                                  \
00074   }
00075 
00076 
00077 using namespace camera1394;
00078 
00080 // Constructor
00081 Camera1394::Camera1394():
00082   camera_(NULL)
00083 {}
00084 
00085 Camera1394::~Camera1394() 
00086 {
00087   SafeCleanup();
00088 }
00089 
00090 void Camera1394::findBayerPattern(const char* bayer)
00091 {
00092   // determine Bayer color encoding pattern
00093   // (default is different from any color filter provided by DC1394)
00094   BayerPattern_ = (dc1394color_filter_t) DC1394_COLOR_FILTER_NUM;
00095   if (0 == strcmp(bayer, "bggr"))
00096     {
00097       BayerPattern_ = DC1394_COLOR_FILTER_BGGR;
00098     }
00099   else if (0 == strcmp(bayer, "grbg"))
00100     {
00101       BayerPattern_ = DC1394_COLOR_FILTER_GRBG;
00102     }
00103   else if (0 == strcmp(bayer, "rggb"))
00104     {
00105       BayerPattern_ = DC1394_COLOR_FILTER_RGGB;
00106     }
00107   else if (0 == strcmp(bayer, "gbrg"))
00108     {
00109       BayerPattern_ = DC1394_COLOR_FILTER_GBRG;
00110     }
00111   else if (0 != strcmp(bayer, ""))
00112     {
00113       ROS_ERROR("unknown bayer pattern [%s]", bayer);
00114     }
00115 }
00116 
00117 bool Camera1394::findBayerMethod(const char* method)
00118 {
00119   // Do Bayer conversion in the driver node?
00120   bool DoBayer = false;                 // return value
00121   if (0 != strcmp(method, "")
00122       && BayerPattern_ != DC1394_COLOR_FILTER_NUM)
00123     {
00124       DoBayer = true;                   // decoding in driver
00125       // add method name to message:
00126       ROS_WARN("[%s] Bayer decoding in the driver is DEPRECATED;"
00127                " image_proc decoding preferred.", method);
00128 
00129       // Set decoding method
00130       if (!strcmp(method, "DownSample"))
00131         BayerMethod_ = DC1394_BAYER_METHOD_DOWNSAMPLE;
00132       else if (!strcmp(method, "Simple"))
00133         BayerMethod_ = DC1394_BAYER_METHOD_SIMPLE;
00134       else if (!strcmp(method, "Bilinear"))
00135         BayerMethod_ = DC1394_BAYER_METHOD_BILINEAR;
00136       else if (!strcmp(method, "HQ"))
00137         BayerMethod_ = DC1394_BAYER_METHOD_HQLINEAR;
00138       else if (!strcmp(method, "VNG"))
00139         BayerMethod_ = DC1394_BAYER_METHOD_VNG;
00140       else if (!strcmp(method, "AHD"))
00141         BayerMethod_ = DC1394_BAYER_METHOD_AHD;
00142       else
00143         {
00144           ROS_ERROR("Unknown Bayer method [%s]. Using ROS image_proc instead.",
00145                     method);
00146           DoBayer = false;
00147         }
00148     }
00149   return DoBayer;
00150 }
00151 
00162 int Camera1394::open(camera1394::Camera1394Config &newconfig)
00163 {
00165   // Pad GUID (if specified) with leading zeros
00167 
00168   const static size_t exact_guid_length = 16;
00169   size_t guid_length = newconfig.guid.length();
00170   if (guid_length != 0 && guid_length != exact_guid_length)
00171       {
00172         if (guid_length < exact_guid_length)
00173           {
00174             // pad string with leading zeros
00175             newconfig.guid.insert(0, exact_guid_length - guid_length, '0');
00176           }
00177         else
00178           {
00179             ROS_ERROR_STREAM_THROTTLE(3, "Invalid GUID [" << newconfig.guid
00180                                       << "] specified: " << guid_length
00181                                       << " characters long.");
00182           }
00183       }
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(camera1394::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(camera1394::Exception, "Could not get camera list");
00208       return -1;
00209     }
00210   
00211   if (list->num == 0)
00212     {
00213       CAM_EXCEPT(camera1394::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       uint32_t value[3];
00221       
00222       value[0]= list->ids[i].guid & 0xffffffff;
00223       value[1]= (list->ids[i].guid >>32) & 0x000000ff;
00224       value[2]= (list->ids[i].guid >>40) & 0xfffff;
00225       
00226       sprintf(temp,"%06x%02x%08x", value[2], value[1], value[0]);
00227 
00228       if (guid[0] == '\0')
00229         {
00230           // pad GUID with leading zeros in message
00231           ROS_INFO_STREAM("No GUID specified, using first camera found, GUID: "
00232                           << std::setw(16) << std::setfill('0') << std::hex
00233                           << list->ids[i].guid);
00234         }
00235       else
00236         {
00237           ROS_WARN("Comparing %s to %s", guid, temp);
00238           if (strcmp(temp, guid))
00239             {
00240               ROS_WARN("GUIDs do not match");
00241               continue;
00242             }
00243         }
00244 
00245       // Create a camera
00246       camera_ = dc1394_camera_new (d, list->ids[i].guid);
00247       if (!camera_)
00248         {
00249           ROS_WARN_STREAM("Failed to initialize camera with GUID "
00250                           << std::setw(16) << std::setfill('0') << std::hex
00251                           << list->ids[i].guid);
00252 
00253           SafeCleanup();
00254           break;
00255         }
00256       else
00257         {
00258           ROS_INFO_STREAM("Found camera with GUID "
00259                           << std::setw(16) << std::setfill('0') << std::hex
00260                           << list->ids[i].guid);
00261 
00262           device_id_ = std::string(temp);
00263           break;
00264         }
00265     }
00266   free (temp);
00267   dc1394_camera_free_list (list);
00268   
00269   if (!camera_)
00270     {
00271       if (strcmp(guid,"")==0)
00272         { 
00273           CAM_EXCEPT(camera1394::Exception, "Could not find camera");
00274         }
00275       else
00276         {
00277           CAM_EXCEPT_ARGS(camera1394::Exception,
00278                           "Could not find camera with guid %s", guid);
00279         }
00280       return -1;
00281     }
00282 
00283   ROS_INFO_STREAM("camera model: " << camera_->vendor
00284                   << " " << camera_->model);
00285 
00287   // initialize camera
00289 
00290   // resetting some cameras is not a good idea
00291   if (newconfig.reset_on_open
00292       && DC1394_SUCCESS != dc1394_camera_reset(camera_))
00293     {
00294       // reset failed: log a warning, but continue
00295       ROS_WARN("Unable to reset camera (continuing).");
00296     }
00297 
00298   // first, set parameters that are common between Format7 and other modes
00299   if (false == Modes::setIsoSpeed(camera_, newconfig.iso_speed))
00300     {
00301       SafeCleanup();
00302       CAM_EXCEPT(camera1394::Exception,
00303                  "Unable to set ISO speed; is the camera plugged in?");
00304       return -1;
00305     }
00306 
00307   // set video mode
00308   videoMode_ = Modes::getVideoMode(camera_, newconfig.video_mode);
00309   if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode_))
00310     {
00311       SafeCleanup();
00312       CAM_EXCEPT(camera1394::Exception, "Failed to set video mode");
00313       return -1;
00314     }
00315 
00317   // special handling for Format7 modes
00319 
00320   DoBayerConversion_ = false;
00321 
00322   if (dc1394_is_video_mode_scalable(videoMode_) == DC1394_TRUE)
00323     {
00324       // set Format7 parameters
00325       if (!format7_.start(camera_, videoMode_, newconfig))
00326         {
00327           SafeCleanup();
00328           CAM_EXCEPT(camera1394::Exception, "Format7 start failed");
00329           return -1;
00330         }
00331     }
00332   else
00333     {
00334       // Set frame rate and Bayer method (only valid for non-Format7 modes)
00335       DoBayerConversion_ = findBayerMethod(newconfig.bayer_method.c_str());
00336       if (!Modes::setFrameRate(camera_, videoMode_, newconfig.frame_rate))
00337         {
00338           SafeCleanup();
00339           CAM_EXCEPT(camera1394::Exception, "Failed to set frame rate");
00340           return -1;
00341         }
00342     }
00343 
00344   findBayerPattern(newconfig.bayer_pattern.c_str());
00345 
00346   use_ros_time_ = newconfig.use_ros_time;
00347 
00349   // start the device streaming data
00351 
00352   // Set camera to use DMA, improves performance.
00353   if (DC1394_SUCCESS != dc1394_capture_setup(camera_, NUM_DMA_BUFFERS,
00354                                              DC1394_CAPTURE_FLAGS_DEFAULT))
00355     {
00356       SafeCleanup();
00357       CAM_EXCEPT(camera1394::Exception, "Failed to open device!");
00358       return -1;
00359     }
00360   
00361   // Start transmitting camera data
00362   if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
00363     {
00364       SafeCleanup();
00365       CAM_EXCEPT(camera1394::Exception, "Failed to start device!");
00366       return -1;
00367     }
00368 
00370   // initialize feature settings
00372 
00373   // TODO: pass newconfig here and eliminate initialize() method
00374   features_.reset(new Features(camera_));
00375  
00376   return 0;
00377 }
00378 
00379 
00381 void Camera1394::SafeCleanup()
00382 {
00383   if (camera_)
00384     {
00385       format7_.stop();
00386       dc1394_capture_stop(camera_);
00387       // try to power off the device (#5322):
00388       dc1394_camera_set_power(camera_, DC1394_OFF);
00389       dc1394_camera_free(camera_);
00390       camera_ = NULL;
00391     }
00392 }
00393 
00394 
00396 int Camera1394::close()
00397 {
00398   if (camera_)
00399     {
00400       if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF)
00401           || DC1394_SUCCESS != dc1394_capture_stop(camera_))
00402         ROS_WARN("unable to stop camera");
00403     }
00404 
00405   // Free resources
00406   SafeCleanup();
00407 
00408   return 0;
00409 }
00410 
00411 std::string bayer_string(dc1394color_filter_t pattern, unsigned int bits)
00412 {
00413   if (bits == 8)
00414     {
00415       switch (pattern)
00416         {
00417         case DC1394_COLOR_FILTER_RGGB:
00418           return sensor_msgs::image_encodings::BAYER_RGGB8;
00419         case DC1394_COLOR_FILTER_GBRG:
00420           return sensor_msgs::image_encodings::BAYER_GBRG8;
00421         case DC1394_COLOR_FILTER_GRBG:
00422           return sensor_msgs::image_encodings::BAYER_GRBG8;
00423         case DC1394_COLOR_FILTER_BGGR:
00424           return sensor_msgs::image_encodings::BAYER_BGGR8;
00425         default:
00426           return sensor_msgs::image_encodings::MONO8;
00427         }
00428     }
00429   else if (bits == 16)
00430     {
00431       switch (pattern)
00432         {
00433         case DC1394_COLOR_FILTER_RGGB:
00434           return sensor_msgs::image_encodings::BAYER_RGGB16;
00435         case DC1394_COLOR_FILTER_GBRG:
00436           return sensor_msgs::image_encodings::BAYER_GBRG16;
00437         case DC1394_COLOR_FILTER_GRBG:
00438           return sensor_msgs::image_encodings::BAYER_GRBG16;
00439         case DC1394_COLOR_FILTER_BGGR:
00440           return sensor_msgs::image_encodings::BAYER_BGGR16;
00441         default:
00442           return sensor_msgs::image_encodings::MONO16;
00443         }
00444     }
00445 
00446   // bits should always be 8 or 16, but if not MONO8 is a good default
00447   return sensor_msgs::image_encodings::MONO8;
00448 }
00449 
00451 void Camera1394::readData(sensor_msgs::Image& image)
00452 {
00453   ROS_ASSERT_MSG(camera_, "Attempt to read from camera that is not open.");
00454 
00455   dc1394video_frame_t * frame = NULL;
00456   dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
00457   if (!frame)
00458     {
00459       CAM_EXCEPT(camera1394::Exception, "Unable to capture frame");
00460       return;
00461     }
00462   
00463   uint8_t* capture_buffer;
00464 
00465   if (use_ros_time_)
00466     image.header.stamp = ros::Time::now();
00467   else
00468     image.header.stamp = ros::Time((double) frame->timestamp / 1000000.0);
00469 
00470   dc1394video_frame_t frame2;
00471 
00472   if (DoBayerConversion_)
00473     {
00474       // debayer frame into RGB8
00475       size_t frame2_size = (frame->size[0] * frame->size[1]
00476                             * 3 * sizeof(unsigned char));
00477       frame2.image = (unsigned char *) malloc(frame2_size);
00478       frame2.allocated_image_bytes = frame2_size;
00479       frame2.color_coding = DC1394_COLOR_CODING_RGB8;
00480 
00481       frame->color_filter = BayerPattern_;
00482       int err = dc1394_debayer_frames(frame, &frame2, BayerMethod_);
00483       if (err != DC1394_SUCCESS)
00484         {
00485           free(frame2.image);
00486           dc1394_capture_enqueue(camera_, frame);
00487           CAM_EXCEPT(camera1394::Exception, "Could not convert/debayer frames");
00488           return;
00489         }
00490 
00491       capture_buffer = reinterpret_cast<uint8_t *>(frame2.image);
00492 
00493       image.width = frame2.size[0];
00494       image.height = frame2.size[1];
00495     }
00496   else
00497     {
00498       image.width = frame->size[0];
00499       image.height = frame->size[1];
00500       capture_buffer = reinterpret_cast<uint8_t *>(frame->image);
00501     }
00502 
00503   ROS_ASSERT(capture_buffer);   
00504 
00505   int image_size;  
00506   switch (videoMode_)
00507     {
00508     case DC1394_VIDEO_MODE_160x120_YUV444:
00509       image.step=image.width*3;
00510       image_size = image.height*image.step;
00511       image.encoding = sensor_msgs::image_encodings::RGB8;
00512       image.data.resize(image_size);
00513       yuv::uyv2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00514                    reinterpret_cast<unsigned char *> (&image.data[0]),
00515                    image.width * image.height);
00516       break;
00517     case DC1394_VIDEO_MODE_640x480_YUV411:
00518       image.step=image.width*3;
00519       image_size = image.height*image.step;
00520       image.encoding = sensor_msgs::image_encodings::RGB8;
00521       image.data.resize(image_size);
00522       yuv::uyyvyy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00523                       reinterpret_cast<unsigned char *> (&image.data[0]),
00524                       image.width * image.height);
00525       break;
00526     case DC1394_VIDEO_MODE_320x240_YUV422:
00527     case DC1394_VIDEO_MODE_640x480_YUV422:
00528     case DC1394_VIDEO_MODE_800x600_YUV422:
00529     case DC1394_VIDEO_MODE_1024x768_YUV422:
00530     case DC1394_VIDEO_MODE_1280x960_YUV422:
00531     case DC1394_VIDEO_MODE_1600x1200_YUV422:
00532       image.step=image.width*3;
00533       image_size = image.height*image.step;
00534       image.encoding = sensor_msgs::image_encodings::RGB8;
00535       image.data.resize(image_size);
00536       yuv::uyvy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00537                     reinterpret_cast<unsigned char *> (&image.data[0]),
00538                     image.width * image.height);
00539       break;
00540     case DC1394_VIDEO_MODE_640x480_RGB8:
00541     case DC1394_VIDEO_MODE_800x600_RGB8:
00542     case DC1394_VIDEO_MODE_1024x768_RGB8:
00543     case DC1394_VIDEO_MODE_1280x960_RGB8:
00544     case DC1394_VIDEO_MODE_1600x1200_RGB8:
00545       image.step=image.width*3;
00546       image_size = image.height*image.step;
00547       image.encoding = sensor_msgs::image_encodings::RGB8;
00548       image.data.resize(image_size);
00549       memcpy(&image.data[0], capture_buffer, image_size);
00550       break;
00551     case DC1394_VIDEO_MODE_640x480_MONO8:
00552     case DC1394_VIDEO_MODE_800x600_MONO8:
00553     case DC1394_VIDEO_MODE_1024x768_MONO8:
00554     case DC1394_VIDEO_MODE_1280x960_MONO8:
00555     case DC1394_VIDEO_MODE_1600x1200_MONO8:
00556       if (!DoBayerConversion_)
00557         {
00558           image.step=image.width;
00559           image_size = image.height*image.step;
00560           // set Bayer encoding in ROS Image message
00561           image.encoding = bayer_string(BayerPattern_, 8);
00562           image.data.resize(image_size);
00563           memcpy(&image.data[0], capture_buffer, image_size);
00564         }
00565       else
00566         {
00567           image.step=image.width*3;
00568           image_size = image.height*image.step;
00569           image.encoding = sensor_msgs::image_encodings::RGB8;
00570           image.data.resize(image_size);
00571           memcpy(&image.data[0], capture_buffer, image_size);
00572         } 
00573       break;
00574     case DC1394_VIDEO_MODE_640x480_MONO16:
00575     case DC1394_VIDEO_MODE_800x600_MONO16:
00576     case DC1394_VIDEO_MODE_1024x768_MONO16:
00577     case DC1394_VIDEO_MODE_1280x960_MONO16:
00578     case DC1394_VIDEO_MODE_1600x1200_MONO16:
00579       if (!DoBayerConversion_)
00580         {
00581           image.step=image.width*2;
00582           image_size = image.height*image.step;
00583           image.encoding = bayer_string(BayerPattern_, 16);
00584           image.is_bigendian = true;
00585           image.data.resize(image_size);
00586           memcpy(&image.data[0], capture_buffer, image_size);
00587         }
00588       else
00589         {
00590           // @todo test Bayer conversions for mono16
00591           image.step=image.width*3;
00592           image_size = image.height*image.step;
00593           image.encoding = sensor_msgs::image_encodings::RGB8;
00594           image.data.resize(image_size);
00595           memcpy(&image.data[0], capture_buffer, image_size);
00596         } 
00597       break;
00598     default:
00599       if (dc1394_is_video_mode_scalable(videoMode_))
00600         {
00601           format7_.unpackData(image, capture_buffer);
00602         }
00603       else
00604         {
00605           CAM_EXCEPT(camera1394::Exception, "Unknown image mode");
00606           return;
00607         }
00608     }
00609   dc1394_capture_enqueue(camera_, frame);
00610 
00611   if (DoBayerConversion_) 
00612     free(capture_buffer);
00613 }


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