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 // @todo eliminate these macros
00059 #define CAM_EXCEPT(except, msg)                                 \
00060   {                                                             \
00061     char buf[100];                                              \
00062     snprintf(buf, 100, "[Camera1394::%s]: " msg, __FUNCTION__); \
00063     throw except(buf);                                          \
00064   }
00065 
00067 #define CAM_EXCEPT_ARGS(except, msg, ...)                               \
00068   {                                                                     \
00069     char buf[100];                                                      \
00070     snprintf(buf, 100, "[Camera1394::%s]: " msg, __FUNCTION__, __VA_ARGS__); \
00071     throw except(buf);                                                  \
00072   }
00073 
00074 
00075 using namespace camera1394;
00076 
00078 // Constructor
00079 Camera1394::Camera1394():
00080   camera_(NULL)
00081 {}
00082 
00083 Camera1394::~Camera1394() 
00084 {
00085   SafeCleanup();
00086 }
00087 
00088 void Camera1394::findBayerPattern(const char* bayer)
00089 {
00090   // determine Bayer color encoding pattern
00091   // (default is different from any color filter provided by DC1394)
00092   BayerPattern_ = (dc1394color_filter_t) DC1394_COLOR_FILTER_NUM;
00093   if (0 == strcmp(bayer, "bggr"))
00094     {
00095       BayerPattern_ = DC1394_COLOR_FILTER_BGGR;
00096     }
00097   else if (0 == strcmp(bayer, "grbg"))
00098     {
00099       BayerPattern_ = DC1394_COLOR_FILTER_GRBG;
00100     }
00101   else if (0 == strcmp(bayer, "rggb"))
00102     {
00103       BayerPattern_ = DC1394_COLOR_FILTER_RGGB;
00104     }
00105   else if (0 == strcmp(bayer, "gbrg"))
00106     {
00107       BayerPattern_ = DC1394_COLOR_FILTER_GBRG;
00108     }
00109   else if (0 != strcmp(bayer, ""))
00110     {
00111       ROS_ERROR("unknown bayer pattern [%s]", bayer);
00112     }
00113 }
00114 
00115 bool Camera1394::findBayerMethod(const char* method)
00116 {
00117   // Do Bayer conversion in the driver node?
00118   bool DoBayer = false;                 // return value
00119   if (0 != strcmp(method, "")
00120       && BayerPattern_ != DC1394_COLOR_FILTER_NUM)
00121     {
00122       DoBayer = true;                   // decoding in driver
00123       // add method name to message:
00124       ROS_WARN("[%s] Bayer decoding in the driver is DEPRECATED;"
00125                " image_proc decoding preferred.", method);
00126 
00127       // Set decoding method
00128       if (!strcmp(method, "DownSample"))
00129         BayerMethod_ = DC1394_BAYER_METHOD_DOWNSAMPLE;
00130       else if (!strcmp(method, "Simple"))
00131         BayerMethod_ = DC1394_BAYER_METHOD_SIMPLE;
00132       else if (!strcmp(method, "Bilinear"))
00133         BayerMethod_ = DC1394_BAYER_METHOD_BILINEAR;
00134       else if (!strcmp(method, "HQ"))
00135         BayerMethod_ = DC1394_BAYER_METHOD_HQLINEAR;
00136       else if (!strcmp(method, "VNG"))
00137         BayerMethod_ = DC1394_BAYER_METHOD_VNG;
00138       else if (!strcmp(method, "AHD"))
00139         BayerMethod_ = DC1394_BAYER_METHOD_AHD;
00140       else
00141         {
00142           ROS_ERROR("Unknown Bayer method [%s]. Using ROS image_proc instead.",
00143                     method);
00144           DoBayer = false;
00145         }
00146     }
00147   return DoBayer;
00148 }
00149 
00160 int Camera1394::open(camera1394::Camera1394Config &newconfig)
00161 {
00163   // Pad GUID (if specified) with leading zeros
00165 
00166   const static size_t exact_guid_length = 16;
00167   size_t guid_length = newconfig.guid.length();
00168   if (guid_length != 0 && guid_length != exact_guid_length)
00169       {
00170         if (guid_length < exact_guid_length)
00171           {
00172             // pad string with leading zeros
00173             newconfig.guid.insert(0, exact_guid_length - guid_length, '0');
00174           }
00175         else
00176           {
00177             ROS_ERROR_STREAM_THROTTLE(3, "Invalid GUID [" << newconfig.guid
00178                                       << "] specified: " << guid_length
00179                                       << " characters long.");
00180           }
00181       }
00182 
00184   // First, look for the camera
00186       
00187   const char *guid = newconfig.guid.c_str();  // C-style GUID for libdc1394
00188   int err;
00189   dc1394_t *d;
00190   dc1394camera_list_t *list;
00191 
00192   // TODO: make error exit paths clean up resources properly
00193   d = dc1394_new ();
00194   if (d == NULL)
00195     {
00196       CAM_EXCEPT(camera1394::Exception,
00197                  "Could not initialize dc1394_context.\n"
00198                  "Make sure /dev/raw1394 exists, you have access permission,\n"
00199                  "and libraw1394 development package is installed.");
00200     }
00201 
00202   err = dc1394_camera_enumerate(d, &list);
00203   if (err != DC1394_SUCCESS)
00204     {
00205       CAM_EXCEPT(camera1394::Exception, "Could not get camera list");
00206       return -1;
00207     }
00208   
00209   if (list->num == 0)
00210     {
00211       CAM_EXCEPT(camera1394::Exception, "No cameras found");
00212       return -1;
00213     }
00214   
00215   char* temp=(char*)malloc(1024*sizeof(char));
00216   for (unsigned i=0; i < list->num; i++)
00217     {
00218       uint32_t value[3];
00219       
00220       value[0]= list->ids[i].guid & 0xffffffff;
00221       value[1]= (list->ids[i].guid >>32) & 0x000000ff;
00222       value[2]= (list->ids[i].guid >>40) & 0xfffff;
00223       
00224       sprintf(temp,"%06x%02x%08x", value[2], value[1], value[0]);
00225 
00226       if (guid[0] == '\0')
00227         {
00228           // pad GUID with leading zeros in message
00229           ROS_INFO_STREAM("No GUID specified, using first camera found, GUID: "
00230                           << std::setw(16) << std::setfill('0') << std::hex
00231                           << list->ids[i].guid);
00232         }
00233       else
00234         {
00235           ROS_WARN("Comparing %s to %s", guid, temp);
00236           if (strcmp(temp, guid))
00237             {
00238               ROS_WARN("GUIDs do not match");
00239               continue;
00240             }
00241         }
00242 
00243       // Create a camera
00244       camera_ = dc1394_camera_new (d, list->ids[i].guid);
00245       if (!camera_)
00246         {
00247           ROS_WARN_STREAM("Failed to initialize camera with GUID "
00248                           << std::setw(16) << std::setfill('0') << std::hex
00249                           << list->ids[i].guid);
00250 
00251           SafeCleanup();
00252           break;
00253         }
00254       else
00255         {
00256           ROS_INFO_STREAM("Found camera with GUID "
00257                           << std::setw(16) << std::setfill('0') << std::hex
00258                           << list->ids[i].guid);
00259 
00260           device_id_ = std::string(temp);
00261           break;
00262         }
00263     }
00264   free (temp);
00265   dc1394_camera_free_list (list);
00266   
00267   if (!camera_)
00268     {
00269       if (strcmp(guid,"")==0)
00270         { 
00271           CAM_EXCEPT(camera1394::Exception, "Could not find camera");
00272         }
00273       else
00274         {
00275           CAM_EXCEPT_ARGS(camera1394::Exception,
00276                           "Could not find camera with guid %s", guid);
00277         }
00278       return -1;
00279     }
00280 
00281   ROS_INFO_STREAM("camera model: " << camera_->vendor
00282                   << " " << camera_->model);
00283 
00285   // initialize camera
00287 
00288   // resetting some cameras is not a good idea
00289   if (newconfig.reset_on_open
00290       && DC1394_SUCCESS != dc1394_camera_reset(camera_))
00291     {
00292       // reset failed: log a warning, but continue
00293       ROS_WARN("Unable to reset camera (continuing).");
00294     }
00295 
00296   // first, set parameters that are common between Format7 and other modes
00297   if (false == Modes::setIsoSpeed(camera_, newconfig.iso_speed))
00298     {
00299       SafeCleanup();
00300       CAM_EXCEPT(camera1394::Exception,
00301                  "Unable to set ISO speed; is the camera plugged in?");
00302       return -1;
00303     }
00304 
00305   // set video mode
00306   videoMode_ = Modes::getVideoMode(camera_, newconfig.video_mode);
00307   if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode_))
00308     {
00309       SafeCleanup();
00310       CAM_EXCEPT(camera1394::Exception, "Failed to set video mode");
00311       return -1;
00312     }
00313 
00315   // special handling for Format7 modes
00317 
00318   DoBayerConversion_ = false;
00319 
00320   if (dc1394_is_video_mode_scalable(videoMode_) == DC1394_TRUE)
00321     {
00322       // set Format7 parameters
00323       if (!format7_.start(camera_, videoMode_, newconfig))
00324         {
00325           SafeCleanup();
00326           CAM_EXCEPT(camera1394::Exception, "Format7 start failed");
00327           return -1;
00328         }
00329     }
00330   else
00331     {
00332       // Set frame rate and Bayer method (only valid for non-Format7 modes)
00333       DoBayerConversion_ = findBayerMethod(newconfig.bayer_method.c_str());
00334       if (!Modes::setFrameRate(camera_, videoMode_, newconfig.frame_rate))
00335         {
00336           SafeCleanup();
00337           CAM_EXCEPT(camera1394::Exception, "Failed to set frame rate");
00338           return -1;
00339         }
00340     }
00341 
00342   findBayerPattern(newconfig.bayer_pattern.c_str());
00343 
00344   use_ros_time_ = newconfig.use_ros_time;
00345   time_offset_ = newconfig.time_offset;
00346 
00348   // start the device streaming data
00350 
00351   // Set camera to use DMA, improves performance.
00352   if (DC1394_SUCCESS != dc1394_capture_setup(camera_, newconfig.num_dma_buffers,
00353                                              DC1394_CAPTURE_FLAGS_DEFAULT))
00354     {
00355       SafeCleanup();
00356       CAM_EXCEPT(camera1394::Exception, "Failed to open device!");
00357       return -1;
00358     }
00359   
00360   // Start transmitting camera data
00361   if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
00362     {
00363       SafeCleanup();
00364       CAM_EXCEPT(camera1394::Exception, "Failed to start device!");
00365       return -1;
00366     }
00367 
00369   // initialize feature settings
00371 
00372   // TODO: pass newconfig here and eliminate initialize() method
00373   features_.reset(new Features(camera_));
00374  
00375   return 0;
00376 }
00377 
00378 
00380 void Camera1394::SafeCleanup()
00381 {
00382   if (camera_)
00383     {
00384       format7_.stop();
00385       dc1394_capture_stop(camera_);
00386       // try to power off the device (#5322):
00387       dc1394_camera_set_power(camera_, DC1394_OFF);
00388       dc1394_camera_free(camera_);
00389       camera_ = NULL;
00390     }
00391 }
00392 
00393 
00395 int Camera1394::close()
00396 {
00397   if (camera_)
00398     {
00399       if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF)
00400           || DC1394_SUCCESS != dc1394_capture_stop(camera_))
00401         ROS_WARN("unable to stop camera");
00402     }
00403 
00404   // Free resources
00405   SafeCleanup();
00406 
00407   return 0;
00408 }
00409 
00410 std::string bayer_string(dc1394color_filter_t pattern, unsigned int bits)
00411 {
00412   if (bits == 8)
00413     {
00414       switch (pattern)
00415         {
00416         case DC1394_COLOR_FILTER_RGGB:
00417           return sensor_msgs::image_encodings::BAYER_RGGB8;
00418         case DC1394_COLOR_FILTER_GBRG:
00419           return sensor_msgs::image_encodings::BAYER_GBRG8;
00420         case DC1394_COLOR_FILTER_GRBG:
00421           return sensor_msgs::image_encodings::BAYER_GRBG8;
00422         case DC1394_COLOR_FILTER_BGGR:
00423           return sensor_msgs::image_encodings::BAYER_BGGR8;
00424         default:
00425           return sensor_msgs::image_encodings::MONO8;
00426         }
00427     }
00428   else if (bits == 16)
00429     {
00430       switch (pattern)
00431         {
00432         case DC1394_COLOR_FILTER_RGGB:
00433           return sensor_msgs::image_encodings::BAYER_RGGB16;
00434         case DC1394_COLOR_FILTER_GBRG:
00435           return sensor_msgs::image_encodings::BAYER_GBRG16;
00436         case DC1394_COLOR_FILTER_GRBG:
00437           return sensor_msgs::image_encodings::BAYER_GRBG16;
00438         case DC1394_COLOR_FILTER_BGGR:
00439           return sensor_msgs::image_encodings::BAYER_BGGR16;
00440         default:
00441           return sensor_msgs::image_encodings::MONO16;
00442         }
00443     }
00444 
00445   // bits should always be 8 or 16, but if not MONO8 is a good default
00446   return sensor_msgs::image_encodings::MONO8;
00447 }
00448 
00450 void Camera1394::readData(sensor_msgs::Image& image)
00451 {
00452   ROS_ASSERT_MSG(camera_, "Attempt to read from camera that is not open.");
00453 
00454   dc1394video_frame_t * frame = NULL;
00455   dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
00456   if (!frame)
00457     {
00458       CAM_EXCEPT(camera1394::Exception, "Unable to capture frame");
00459       return;
00460     }
00461   
00462   uint8_t* capture_buffer;
00463 
00464   if (use_ros_time_)
00465     image.header.stamp = ros::Time::now() + ros::Duration(time_offset_);
00466   else
00467     image.header.stamp = ros::Time((double) frame->timestamp / 1000000.0);
00468 
00469   dc1394video_frame_t frame2;
00470 
00471   if (DoBayerConversion_)
00472     {
00473       // debayer frame into RGB8
00474       size_t frame2_size = (frame->size[0] * frame->size[1]
00475                             * 3 * sizeof(unsigned char));
00476       frame2.image = (unsigned char *) malloc(frame2_size);
00477       frame2.allocated_image_bytes = frame2_size;
00478       frame2.color_coding = DC1394_COLOR_CODING_RGB8;
00479 
00480       frame->color_filter = BayerPattern_;
00481       int err = dc1394_debayer_frames(frame, &frame2, BayerMethod_);
00482       if (err != DC1394_SUCCESS)
00483         {
00484           free(frame2.image);
00485           dc1394_capture_enqueue(camera_, frame);
00486           CAM_EXCEPT(camera1394::Exception, "Could not convert/debayer frames");
00487           return;
00488         }
00489 
00490       capture_buffer = reinterpret_cast<uint8_t *>(frame2.image);
00491 
00492       image.width = frame2.size[0];
00493       image.height = frame2.size[1];
00494     }
00495   else
00496     {
00497       image.width = frame->size[0];
00498       image.height = frame->size[1];
00499       capture_buffer = reinterpret_cast<uint8_t *>(frame->image);
00500     }
00501 
00502   ROS_ASSERT(capture_buffer);   
00503 
00504   int image_size;  
00505   switch (videoMode_)
00506     {
00507     case DC1394_VIDEO_MODE_160x120_YUV444:
00508       image.step=image.width*3;
00509       image_size = image.height*image.step;
00510       image.encoding = sensor_msgs::image_encodings::RGB8;
00511       image.data.resize(image_size);
00512       yuv::uyv2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00513                    reinterpret_cast<unsigned char *> (&image.data[0]),
00514                    image.width * image.height);
00515       break;
00516     case DC1394_VIDEO_MODE_640x480_YUV411:
00517       image.step=image.width*3;
00518       image_size = image.height*image.step;
00519       image.encoding = sensor_msgs::image_encodings::RGB8;
00520       image.data.resize(image_size);
00521       yuv::uyyvyy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00522                       reinterpret_cast<unsigned char *> (&image.data[0]),
00523                       image.width * image.height);
00524       break;
00525     case DC1394_VIDEO_MODE_320x240_YUV422:
00526     case DC1394_VIDEO_MODE_640x480_YUV422:
00527     case DC1394_VIDEO_MODE_800x600_YUV422:
00528     case DC1394_VIDEO_MODE_1024x768_YUV422:
00529     case DC1394_VIDEO_MODE_1280x960_YUV422:
00530     case DC1394_VIDEO_MODE_1600x1200_YUV422:
00531       image.step=image.width*3;
00532       image_size = image.height*image.step;
00533       image.encoding = sensor_msgs::image_encodings::RGB8;
00534       image.data.resize(image_size);
00535       yuv::uyvy2rgb(reinterpret_cast<unsigned char *> (capture_buffer),
00536                     reinterpret_cast<unsigned char *> (&image.data[0]),
00537                     image.width * image.height);
00538       break;
00539     case DC1394_VIDEO_MODE_640x480_RGB8:
00540     case DC1394_VIDEO_MODE_800x600_RGB8:
00541     case DC1394_VIDEO_MODE_1024x768_RGB8:
00542     case DC1394_VIDEO_MODE_1280x960_RGB8:
00543     case DC1394_VIDEO_MODE_1600x1200_RGB8:
00544       image.step=image.width*3;
00545       image_size = image.height*image.step;
00546       image.encoding = sensor_msgs::image_encodings::RGB8;
00547       image.data.resize(image_size);
00548       memcpy(&image.data[0], capture_buffer, image_size);
00549       break;
00550     case DC1394_VIDEO_MODE_640x480_MONO8:
00551     case DC1394_VIDEO_MODE_800x600_MONO8:
00552     case DC1394_VIDEO_MODE_1024x768_MONO8:
00553     case DC1394_VIDEO_MODE_1280x960_MONO8:
00554     case DC1394_VIDEO_MODE_1600x1200_MONO8:
00555       if (!DoBayerConversion_)
00556         {
00557           image.step=image.width;
00558           image_size = image.height*image.step;
00559           // set Bayer encoding in ROS Image message
00560           image.encoding = bayer_string(BayerPattern_, 8);
00561           image.data.resize(image_size);
00562           memcpy(&image.data[0], capture_buffer, image_size);
00563         }
00564       else
00565         {
00566           image.step=image.width*3;
00567           image_size = image.height*image.step;
00568           image.encoding = sensor_msgs::image_encodings::RGB8;
00569           image.data.resize(image_size);
00570           memcpy(&image.data[0], capture_buffer, image_size);
00571         } 
00572       break;
00573     case DC1394_VIDEO_MODE_640x480_MONO16:
00574     case DC1394_VIDEO_MODE_800x600_MONO16:
00575     case DC1394_VIDEO_MODE_1024x768_MONO16:
00576     case DC1394_VIDEO_MODE_1280x960_MONO16:
00577     case DC1394_VIDEO_MODE_1600x1200_MONO16:
00578       if (!DoBayerConversion_)
00579         {
00580           image.step=image.width*2;
00581           image_size = image.height*image.step;
00582           image.encoding = bayer_string(BayerPattern_, 16);
00583           image.is_bigendian = true;
00584           image.data.resize(image_size);
00585           memcpy(&image.data[0], capture_buffer, image_size);
00586         }
00587       else
00588         {
00589           // @todo test Bayer conversions for mono16
00590           image.step=image.width*3;
00591           image_size = image.height*image.step;
00592           image.encoding = sensor_msgs::image_encodings::RGB8;
00593           image.data.resize(image_size);
00594           memcpy(&image.data[0], capture_buffer, image_size);
00595         } 
00596       break;
00597     default:
00598       if (dc1394_is_video_mode_scalable(videoMode_))
00599         {
00600           format7_.unpackData(image, capture_buffer);
00601         }
00602       else
00603         {
00604           CAM_EXCEPT(camera1394::Exception, "Unknown image mode");
00605           return;
00606         }
00607     }
00608   dc1394_capture_enqueue(camera_, frame);
00609 
00610   if (DoBayerConversion_) 
00611     free(capture_buffer);
00612 }


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