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


camera1394
Author(s): Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy
autogenerated on Thu Jun 6 2019 19:34:17