00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00032
00033
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
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
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
00092
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
00119 bool DoBayer = false;
00120 if (0 != strcmp(method, "")
00121 && BayerPattern_ != DC1394_COLOR_FILTER_NUM)
00122 {
00123 DoBayer = true;
00124
00125 ROS_WARN("[%s] Bayer decoding in the driver is DEPRECATED;"
00126 " image_proc decoding preferred.", method);
00127
00128
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
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
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
00187
00188 const char *guid = newconfig.guid.c_str();
00189 int err;
00190 dc1394_t *d;
00191 dc1394camera_list_t *list;
00192
00193
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
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
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
00288
00289
00290 if (newconfig.reset_on_open
00291 && DC1394_SUCCESS != dc1394_camera_reset(camera_))
00292 {
00293
00294 ROS_WARN("Unable to reset camera (continuing).");
00295 }
00296
00297
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
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
00318
00319 DoBayerConversion_ = false;
00320
00321 if (dc1394_is_video_mode_scalable(videoMode_) == DC1394_TRUE)
00322 {
00323
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
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
00351
00352
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
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
00372
00373
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
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
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
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
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
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
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 }