bool ieee1394capture::init(RoboCompCamera::TCamParams ¶ms_, RoboCompJointMotor::JointMotorPrx head_ , RoboCompDifferentialRobot::DifferentialRobotPrx base_ )
{
params = params_;
head = head_;
base = base_;
int32_t i;
fps = (dc1394framerate_t)params.FPS;//15;
res = (dc1394video_mode_t)0;
switch (fps) {
case 1: fps = DC1394_FRAMERATE_1_875; break;
case 3: fps = DC1394_FRAMERATE_3_75; break;
case 15: fps = DC1394_FRAMERATE_15; break;
case 30: fps = DC1394_FRAMERATE_30; break;
case 60: fps = DC1394_FRAMERATE_60; break;
default: fps = DC1394_FRAMERATE_7_5; break;
}
switch (res) {
case 1:
res = DC1394_VIDEO_MODE_640x480_YUV411;
device_width = 640;
device_height = 480;
break;
case 2:
res = DC1394_VIDEO_MODE_640x480_RGB8;
device_width = 640;
device_height = 480;
break;
default:
res = DC1394_VIDEO_MODE_320x240_YUV422;
device_width = 320;
device_height = 240;
break;
}
/// Get handle
qDebug() << "ieee1394capture::init() -> Initializating first Firewire Card in the system...";
if (!(d = dc1394_new()))
{
qDebug() << "ieee1394capture::init() -> Fatal error: Unable to aquire a handle to the Ieee1394 device";
qDebug() << "Please check if the kernel modules `ieee1394',`raw1394' and `ohci1394' are loaded or if you have read/write access to /dev/raw1394 and to /dev/video1394-0 " ;
return false;
}
CREATED_BUS = true;
/// Create camera interfaces
numCameras = 0;
err = dc1394_camera_enumerate(d, &list);
DC1394_ERR_RTN(err, "Failed to enumerate cameras");
if (list->num == 0)
{
dc1394_log_error("No cameras found");
return 1;
}
numCameras = 0;
for (uint32_t di = 0; di < list->num; di++)
{
if (numCameras >= MAX_CAMERAS)
break;
cameras[numCameras] = dc1394_camera_new(d, list->ids[di].guid);
if (!cameras[numCameras])
{
dc1394_log_warning("Failed to initialize camera with guid %llx", list->ids[di].guid);
continue;
}
printf("Camera #%d\n", numCameras);
numCameras++;
}
dc1394_camera_free_list(list);
if ( numCameras < 1 )
{
qDebug() << "ieee1394capture::init() -> Fatal error: No cameras found in the bus! Called from ";
cleanup();
return false;
}
/// Check if one camera has become the root node
/* for ( int n=0; n < numCameras; n++ )
{
if ( cameraNodeList[n] == numNodes )
{
qDebug() << "ieee1394capture::init() -> Fatal error: Sorry, your camera is the highest numbered node of the bus, and has therefore become the root node." ;
cleanup();
return false;
}
}*/
CREATED_CAMS = true;
/// Setup cameras for capture
qDebug() << "ieee1394capture::init() -> Searching cameras with requested parameters:";
printf("%s\n",params.mode.c_str());
if (params.mode == "MODE_320x240_YUV422")
{
res = DC1394_VIDEO_MODE_320x240_YUV422;
params.width = 320;
params.height = 240;
}
else if (params.mode == "MODE_640x480_YUV422" )
{
res = DC1394_VIDEO_MODE_640x480_YUV422;
params.width = 640;
params.height = 480;
}
else if (params.mode == "MODE_640x480_RGB" )
{
res = DC1394_VIDEO_MODE_640x480_RGB8;
params.width = 640;
params.height = 480;
}
else if (params.mode == "MODE_640x480_YUV411")
{
res = DC1394_VIDEO_MODE_640x480_YUV411;
params.width = 640;
params.height = 480;
}
else if (params.mode == "MODE_640x480_MONO")
{
res = DC1394_VIDEO_MODE_640x480_MONO8;
params.width = 640;
params.height = 480;
}
else if (params.mode == "MODE_640x480_MONO16")
{
res = DC1394_VIDEO_MODE_640x480_MONO16;
params.width = 640;
params.height = 480;
}
else if (params.mode == "MODE_516x338_YUV422")
{
res = DC1394_VIDEO_MODE_FORMAT7_1;
params.width = 516;
params.height = 338;
}
else qFatal("ieee1394capture::init() -> Image Mode %s not available. Aborting...", params.mode.c_str());
params.size = params.width*params.height;
if (params.FPS!=15 and params.FPS!=30)
{
qWarning("ieee1394capture::init() -> Framerate %d not available. Aborting...", params.FPS );
cleanup();
return false;
}
dc1394format7modeset_t info;
for (i = 0; i < numCameras; i++)
{
if (params.mode == "MODE_516x338_YUV422")
{
err = dc1394_format7_get_modeset(cameras[i], &info);
for( int j=0;j<DC1394_VIDEO_MODE_FORMAT7_NUM;j++)
{
qDebug() << info.mode[j].present;
qDebug() << info.mode[j].size_x;
qDebug() << info.mode[j].size_y;
qDebug() << info.mode[j].max_size_x;
qDebug() << info.mode[j].max_size_y;
qDebug() << info.mode[j].pos_x;
qDebug() << info.mode[j].pos_y;
qDebug() << info.mode[j].unit_size_x;
qDebug() << info.mode[j].unit_size_y;
qDebug() << info.mode[j].unit_pos_x;
qDebug() << info.mode[j].unit_pos_y;
qDebug() << info.mode[j].pixnum;
qDebug() << info.mode[j].packet_size; /* in bytes */
qDebug() << info.mode[j].unit_packet_size;
qDebug() << info.mode[j].max_packet_size;
}
}
release_iso_and_bw(i);
err = dc1394_video_set_mode(cameras[i], res);
DC1394_ERR_CLN_RTN(err, cleanup(), "Could not set video mode");
err = dc1394_video_set_iso_speed(cameras[i], DC1394_ISO_SPEED_400);
DC1394_ERR_CLN_RTN(err, cleanup(), "Could not set ISO speed");
//For format 7 modes only
if (params.mode == "MODE_516x338_YUV422")
{
// uint32_t packet_size;
// err=dc1394_format7_set_image_size(cameras[i], res, 514, 384);
// DC1394_ERR_RTN(err,"Could not set image size");
// err=dc1394_format7_get_recommended_packet_size(cameras[i], res, &packet_size);
// DC1394_ERR_RTN(err,"Could not get format 7 recommended packet size");
// err=dc1394_format7_set_roi(cameras[i], res, DC1394_COLOR_CODING_YUV422, packet_size, 0,0, 514, 384);
// DC1394_ERR_RTN(err,"Could not set ROI");
qDebug() << "ya";
}
err = dc1394_video_set_framerate(cameras[i], fps);
DC1394_ERR_CLN_RTN(err, cleanup(), "Could not set framerate");
err = dc1394_capture_setup(cameras[i], NUM_BUFFERS, DC1394_CAPTURE_FLAGS_DEFAULT);
DC1394_ERR_CLN_RTN(err, cleanup(), "Could not setup camera-\nmake sure that the video mode and framerate are\nsupported by your camera");
err = dc1394_video_set_transmission(cameras[i], DC1394_ON);
DC1394_ERR_CLN_RTN(err, cleanup(), "Could not start camera iso transmission");
}
fflush(stdout);
qDebug() << " ieee1394capture::init() -> Iso transmission started.";
///Buffers de imagen
qDebug() << "BUFFERS DE IMAGEN ------------------------------------------";
for ( int i=0; i < numCameras; i++ )
{
AimgBuffer[i] = ( Ipp8u * ) ippsMalloc_8u ( params.size * 9 );
BimgBuffer[i] = ( Ipp8u * ) ippsMalloc_8u ( params.size * 9 );
img8u_lum[i] = AimgBuffer[i];
img8u_YUV[i] = AimgBuffer[i]+params.size;
localYRGBImgBufferPtr[i] = BimgBuffer[i];
qDebug() << "Reservando" << params.size * 9 <<" para localYRGBImgBufferPtr["<<i<<"]";
printf("(de %p a %p)\n", localYRGBImgBufferPtr[i], localYRGBImgBufferPtr[i]+(params.size*9-1));
}
planos[0]=BimgBuffer[0]+params.size*3;
planos[1]=BimgBuffer[0]+ ( params.size*4 );
planos[2]=BimgBuffer[0]+ ( params.size*5 );
//img8u_aux = BimgBuffer[0]+(params.size*6);
imgSize_ipp.width=params.width;
imgSize_ipp.height=params.height;
return true;
}
int main(int argc, const char * argv[]) {
dc1394_t *d;
dc1394camera_list_t *list;
dc1394error_t err;
dc1394camera_t *camera;
dc1394format7modeset_t modeset;
dc1394video_frame_t *frame;
FILE* imagefile;
char filename[256];
int i = 0;
d = dc1394_new();
if (!d) {
return 1;
}
err = dc1394_camera_enumerate(d, &list);
DC1394_ERR_RTN(err, "Failed to enumerate cameras");
if (list->num == 0) {
dc1394_log_error("No cameras found");
dc1394_free(d);
return 1;
}
printf("Detected %d cameras\n", list->num);
// Assume that Ladybug 5 is detected as camera #0
camera = dc1394_camera_new(d, list->ids[0].guid);
if (!camera) {
dc1394_log_error("Failed to initialize camera with guid %llx", list->ids[0].guid);
dc1394_free(d);
}
dc1394_camera_free_list(list);
printf("Using camera %s %s\n", camera->vendor, camera->model);
// Report camera info
err = dc1394_camera_print_info(camera, stdout);
DC1394_ERR_RTN(err, "Could not print camera info");
// Setup video mode, etc...
err = dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B);
DC1394_ERR_RTN(err, "Could not set B mode");
err = dc1394_video_set_iso_speed(camera, DC1394_ISO_SPEED_MAX);
DC1394_ERR_RTN(err, "Could not set max speed");
err = dc1394_video_set_mode(camera, DC1394_VIDEO_MODE_FORMAT7_0);
DC1394_ERR_RTN(err, "Could not set DC1394_VIDEO_MODE_FORMAT7_0");
// Get format7 mode info
err = dc1394_format7_get_modeset(camera, &modeset);
DC1394_ERR_RTN(err, "Could not get format 7 mode info\n");
print_format7_info(&modeset);
// Set format 7 roi
err = dc1394_format7_set_roi(camera,
DC1394_VIDEO_MODE_FORMAT7_0,
modeset.mode[0].color_coding,
modeset.mode[0].max_packet_size,
modeset.mode[0].pos_x,
modeset.mode[0].pos_y,
modeset.mode[0].max_size_x,
modeset.mode[0].max_size_y);
DC1394_ERR_RTN(err, "Could not set max ROI");
// Set capture
err = dc1394_capture_setup(camera, 10, DC1394_CAPTURE_FLAGS_DEFAULT);
DC1394_ERR_RTN(err, "Could not setup capture");
err = dc1394_video_set_transmission(camera, DC1394_ON);
DC1394_ERR_RTN(err, "Could not start transmission");
while (i < NFRAMES) {
// Capture image
printf("Capturing image %d\n", i);
err = dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame);
DC1394_ERR_RTN(err, "Could not dequeue a frame");
// Do something with the image
print_frame_info(frame);
// Save the image
sprintf(filename, "%05d.pgm",i);
imagefile = fopen(filename, "wb");
if ( imagefile == NULL ) {
printf("Could not save image\n");
continue;
}
fprintf(imagefile, "P5\n%u %u 255\n", frame->size[0], frame->size[1]);
fwrite(frame->image, 1, frame->image_bytes, imagefile);
fclose(imagefile);
printf("Saved image %s\n", filename);
err = dc1394_capture_enqueue(camera, frame);
DC1394_ERR_RTN(err, "Could enqueue a frame");
i++;
}
dc1394_camera_free(camera);
dc1394_free(d);
return 0;
}
