void grab_frame(void) { int errnum; CamContext *cc; static int next_device_number=0; int data_ok = 0; #ifdef USE_COPY cc = cc_all[next_device_number]; CamContext_grab_next_frame_blocking(cc,raw_pixels,-1); // block forever errnum = cam_iface_have_error(); if (errnum == CAM_IFACE_FRAME_TIMEOUT) { cam_iface_clear_error(); return; // wait again } if (errnum == CAM_IFACE_FRAME_DATA_MISSING_ERROR) { cam_iface_clear_error(); fprintf(stdout,"M"); fflush(stdout); } else if (errnum == CAM_IFACE_FRAME_INTERRUPTED_SYSCALL) { cam_iface_clear_error(); fprintf(stdout,"I"); fflush(stdout); } else if (errnum == CAM_IFACE_FRAME_DATA_CORRUPT_ERROR) { cam_iface_clear_error(); fprintf(stdout,"C"); fflush(stdout); } else { _check_error(); data_ok = 1; } next_device_number++; next_device_number = next_device_number % ncams; if (data_ok) { upload_image_data_to_opengl(raw_pixels,cc->coding,next_device_number); } #else CamContext_point_next_frame_blocking(cc,&raw_pixels,-1.0f); _check_error(); upload_image_data_to_opengl(raw_pixels,cc->coding,next_device_number); CamContext_unpoint_frame(cc); _check_error(); #endif glutPostRedisplay(); /* trigger display redraw */ }
int main(int argc, char** argv) { CamContext **cc; unsigned char **pixels; int num_cameras; int num_buffers; double last_fps_print; int n_frames; int have_frame; int buffer_size; int num_modes, num_props; char mode_string[255]; int i,mode_number,camno; CameraPropertyInfo cam_props; long prop_value; int prop_auto; int errnum; int left, top, width, height; int do_num_frames; CameraPixelCoding coding; cam_iface_constructor_func_t new_CamContext; char save_fname[100]; cam_iface_startup_with_version_check(); _check_error(); if (argc>1) { if (strcmp(argv[1],"forever")==0) { do_num_frames = -1; } else if (sscanf(argv[1],"%d",&do_num_frames)==0) { show_usage(argv[0]); } } else { do_num_frames = 50; } for (i=0;i<argc;i++) { printf("%d: %s\n",i,argv[i]); } printf("using driver %s\n",cam_iface_get_driver_name()); num_cameras = cam_iface_get_num_cameras(); printf("%d cameras found\n",cam_iface_get_num_cameras()); cc = (CamContext**)malloc( num_cameras*sizeof(CamContext**)); if (cc==NULL) { printf("error allocating memory, will now exit\n"); cam_iface_shutdown(); _check_error(); exit(1); } pixels = (unsigned char **)malloc( num_cameras*sizeof(unsigned char**)); if (pixels==NULL) { printf("error allocating memory, will now exit\n"); cam_iface_shutdown(); _check_error(); exit(1); } for (camno=0; camno<num_cameras; camno++) { printf("initializing camera number %d\n",camno); cam_iface_get_num_modes(camno, &num_modes); _check_error(); printf("%d mode(s) available:\n",num_modes); mode_number = 0; for (i=0; i<num_modes; i++) { cam_iface_get_mode_string(camno,i,mode_string,255); if (strstr(mode_string,"FORMAT7_0")!=NULL) { if (strstr(mode_string,"MONO8")!=NULL) { // pick this mode mode_number = i; } } printf(" %d: %s\n",i,mode_string); } num_buffers = 5; new_CamContext = cam_iface_get_constructor_func(camno); cc[camno] = new_CamContext(camno,num_buffers,mode_number,NULL); _check_error(); CamContext_get_frame_roi(cc[camno], &left, &top, &width, &height); _check_error(); CamContext_get_num_framebuffers(cc[camno],&num_buffers); printf("allocated %d buffers\n",num_buffers); CamContext_get_num_camera_properties(cc[camno],&num_props); _check_error(); for (i=0; i<num_props; i++) { CamContext_get_camera_property_info(cc[camno],i,&cam_props); _check_error(); if (strcmp(cam_props.name,"white balance")==0) { fprintf(stderr,"WARNING: ignoring white balance property\n"); continue; } if (cam_props.is_present) { CamContext_get_camera_property(cc[camno],i,&prop_value,&prop_auto); _check_error(); printf(" %s: %ld (%s)\n",cam_props.name,prop_value, prop_auto ? "AUTO" : "MANUAL"); } else { printf(" %s: not present\n",cam_props.name); } if (cam_props.has_auto_mode) { prop_auto = 1; CamContext_set_camera_property(cc[camno],i,prop_value,prop_auto); _check_error(); printf(" %s: set to AUTO\n",cam_props.name); } } CamContext_get_buffer_size(cc[camno],&buffer_size); _check_error(); if (buffer_size == 0) { fprintf(stderr,"buffer size was 0 in %s, line %d\n",__FILE__,__LINE__); exit(1); } pixels[camno] = (unsigned char *)malloc( buffer_size ); if (pixels[camno]==NULL) { fprintf(stderr,"couldn't allocate memory in %s, line %d\n",__FILE__,__LINE__); exit(1); } CamContext_start_camera(cc[camno]); _check_error(); } last_fps_print = my_floattime(); n_frames = 0; if (do_num_frames < 0) { printf("will now run forever. press Ctrl-C to interrupt\n"); } else { printf("will now grab %d frames.\n",do_num_frames); } while (1) { if (do_num_frames<0) break; have_frame = 0; for (camno=0; camno<num_cameras; camno++) { CamContext_grab_next_frame_blocking(cc[camno],pixels[camno],0.001f); // timeout after 1 msec errnum = cam_iface_have_error(); if (errnum == CAM_IFACE_FRAME_TIMEOUT) { cam_iface_clear_error(); continue; // wait again on next camera } else if (errnum == CAM_IFACE_FRAME_DATA_MISSING_ERROR) { cam_iface_clear_error(); fprintf(stdout,"M"); fflush(stdout); continue; // wait again on next camera } else if (errnum == CAM_IFACE_FRAME_INTERRUPTED_SYSCALL) { cam_iface_clear_error(); fprintf(stdout,"I"); fflush(stdout); continue; // wait again on next camera } _check_error(); have_frame = 1; fprintf(stdout,"%d",camno); fflush(stdout); } if (!have_frame) { continue; } do_num_frames--; } for (camno=0; camno<num_cameras; camno++) { coding = cc[camno]->coding; printf("\n"); delete_CamContext(cc[camno]); _check_error(); if (coding==CAM_IFACE_MONO8) { snprintf(save_fname, 100, "image_camera%d.pgm", camno); save_pgm(save_fname, pixels[camno], width, height); printf("saved last image as %s\n",save_fname); } else { printf("do not know how to save sample image for this format\n"); } free(pixels[camno]); } free(pixels); cam_iface_shutdown(); _check_error(); return 0; }
int main(int argc, char** argv) { CamContext *cc; unsigned char *pixels; int device_number,ncams,num_buffers; double last_fps_print, now, t_diff; double fps; int n_frames; int buffer_size; int num_modes, num_props, num_trigger_modes; char mode_string[255]; int i,mode_number; CameraPropertyInfo cam_props; long prop_value; int prop_auto; int errnum; int left, top; int width, height; fmf_v3_header_part1 hstart; fmf_v3_header_part2 hstop; CameraPixelCoding coding; char * format_str; int bpp; cam_iface_constructor_func_t new_CamContext; Camwire_id cam_info_struct; FILE* fd; char * filename; cam_iface_startup_with_version_check(); _check_error(); for (i=0;i<argc;i++) { printf("%d: %s\n",i,argv[i]); } printf("using driver %s\n",cam_iface_get_driver_name()); ncams = cam_iface_get_num_cameras(); _check_error(); if (ncams<1) { printf("no cameras found, will now exit\n"); cam_iface_shutdown(); _check_error(); exit(1); } _check_error(); printf("%d camera(s) found.\n",ncams); for (i=0; i<ncams; i++) { cam_iface_get_camera_info(i, &cam_info_struct); printf(" camera %d:\n",i); printf(" vendor: %s\n",cam_info_struct.vendor); printf(" model: %s\n",cam_info_struct.model); printf(" chip: %s\n",cam_info_struct.chip); } device_number = ncams-1; printf("choosing camera %d\n",device_number); cam_iface_get_num_modes(device_number, &num_modes); _check_error(); printf("%d mode(s) available:\n",num_modes); mode_number = 0; for (i=0; i<num_modes; i++) { cam_iface_get_mode_string(device_number,i,mode_string,255); if (strstr(mode_string,"FORMAT7_0")!=NULL) { if (strstr(mode_string,"MONO8")!=NULL) { // pick this mode mode_number = i; } } printf(" %d: %s\n",i,mode_string); } printf("Choosing mode %d\n",mode_number); num_buffers = 5; new_CamContext = cam_iface_get_constructor_func(device_number); cc = new_CamContext(device_number,num_buffers,mode_number); _check_error(); CamContext_get_frame_roi(cc, &left, &top, &width, &height); _check_error(); CamContext_get_num_framebuffers(cc,&num_buffers); printf("allocated %d buffers\n",num_buffers); CamContext_get_num_camera_properties(cc,&num_props); _check_error(); for (i=0; i<num_props; i++) { CamContext_get_camera_property_info(cc,i,&cam_props); _check_error(); if (strcmp(cam_props.name,"white balance")==0) { fprintf(stderr,"WARNING: ignoring white balance property\n"); continue; } if (cam_props.is_present) { CamContext_get_camera_property(cc,i,&prop_value,&prop_auto); _check_error(); printf(" %s: %ld\n",cam_props.name,prop_value); } else { printf(" %s: not present\n",cam_props.name); } } CamContext_get_buffer_size(cc,&buffer_size); _check_error(); if (buffer_size == 0) { fprintf(stderr,"buffer size was 0 in %s, line %d\n",__FILE__,__LINE__); exit(1); } #define USE_COPY #ifdef USE_COPY pixels = (unsigned char *)malloc( buffer_size ); if (pixels==NULL) { fprintf(stderr,"couldn't allocate memory in %s, line %d\n",__FILE__,__LINE__); exit(1); } #endif CamContext_start_camera(cc); _check_error(); last_fps_print = my_floattime(); n_frames = 0; CamContext_get_num_trigger_modes( cc, &num_trigger_modes ); _check_error(); printf("trigger modes:\n"); for (i =0; i<num_trigger_modes; i++) { CamContext_get_trigger_mode_string( cc, i, mode_string, 255 ); printf(" %d: %s\n",i,mode_string); } printf("\n"); coding = cc->coding; bpp = 8; switch (coding) { case CAM_IFACE_MONO8_BAYER_BGGR: format_str = "MONO8:BGGR"; break; case CAM_IFACE_MONO8_BAYER_RGGB: format_str = "MONO8:RGGB"; break; case CAM_IFACE_MONO8_BAYER_GRBG: format_str = "MONO8:GRBG"; break; case CAM_IFACE_MONO8_BAYER_GBRG: format_str = "MONO8:GBRG"; break; case CAM_IFACE_MONO8: format_str = "MONO8"; break; case CAM_IFACE_YUV422: format_str = "YUV422"; bpp = 16; break; default: fprintf(stderr,"do not know how to save sample image for this format\n"); exit(1); } hstart.version = 3; hstart.len_format = strlen(format_str); hstop.rows = height; hstop.cols = width; hstop.bytes_per_chunk = sizeof(double) + height*width*bpp/8; /* timestamp, image */ hstop.n_frames = 0; hstop.bpp = bpp; filename = "movie.fmf"; fd = fopen(filename,"w"); /* write the FMF v3 header */ fwrite(&hstart,sizeof(fmf_v3_header_part1),1,fd); fwrite(format_str,hstart.len_format,1,fd); fwrite(&hstop,sizeof(fmf_v3_header_part2),1,fd); /* grab frames forever */ printf("Press Ctrl-C to quit. Will now save .fmf movie.\n"); while (1) { #ifdef USE_COPY //CamContext_grab_next_frame_blocking(cc,pixels,0.2); // timeout after 200 msec CamContext_grab_next_frame_blocking(cc,pixels,-1.0f); // never timeout errnum = cam_iface_have_error(); if (errnum == CAM_IFACE_FRAME_TIMEOUT) { cam_iface_clear_error(); fprintf(stdout,"T"); fflush(stdout); continue; // wait again } if (errnum == CAM_IFACE_FRAME_DATA_MISSING_ERROR) { cam_iface_clear_error(); fprintf(stdout,"M"); fflush(stdout); } else if (errnum == CAM_IFACE_FRAME_INTERRUPTED_SYSCALL) { cam_iface_clear_error(); fprintf(stdout,"I"); fflush(stdout); } else if (errnum == CAM_IFACE_FRAME_DATA_CORRUPT_ERROR) { cam_iface_clear_error(); fprintf(stdout,"C"); fflush(stdout); } else { _check_error(); fprintf(stdout,"."); fflush(stdout); } now = my_floattime(); n_frames += 1; #else CamContext_point_next_frame_blocking(cc,&pixels,-1.0f); now = my_floattime(); n_frames += 1; _check_error(); fprintf(stdout,"."); fflush(stdout); CamContext_unpoint_frame(cc); _check_error(); #endif fwrite(&now,sizeof(double),1,fd); fwrite(pixels,height*width*bpp/8,1,fd); t_diff = now-last_fps_print; if (t_diff > 5.0) { fps = n_frames/t_diff; fprintf(stdout,"%.1f fps\n",fps); last_fps_print = now; n_frames = 0; } } printf("\n"); delete_CamContext(cc); _check_error(); cam_iface_shutdown(); _check_error(); #ifdef USE_COPY free(pixels); #endif return 0; }
int CameraNode::run() { ros::WallTime t_prev, t_now; int dt_avg; // reset timing information dt_avg = 0; t_prev = ros::WallTime::now(); while (ros::ok()) { std::vector<uint8_t> data(step*height); CamContext_grab_next_frame_blocking(cc,&data[0],-1.0f); // never timeout int errnum = cam_iface_have_error(); if (errnum == CAM_IFACE_FRAME_TIMEOUT) { cam_iface_clear_error(); continue; // wait again } if (errnum == CAM_IFACE_FRAME_DATA_MISSING_ERROR) { cam_iface_clear_error(); } else if (errnum == CAM_IFACE_FRAME_INTERRUPTED_SYSCALL) { cam_iface_clear_error(); } else if (errnum == CAM_IFACE_FRAME_DATA_CORRUPT_ERROR) { cam_iface_clear_error(); } else { _check_error(); if (!_got_frame) { ROS_INFO("receiving images"); _got_frame = true; } if(dt_avg++ == 9) { ros::WallTime t_now = ros::WallTime::now(); ros::WallDuration dur = t_now - t_prev; t_prev = t_now; std_msgs::Float32 rate; rate.data = 10 / dur.toSec(); _pub_rate.publish(rate); dt_avg = 0; } sensor_msgs::Image msg; if (_host_timestamp) { msg.header.stamp = ros::Time::now(); } else { double timestamp; CamContext_get_last_timestamp(cc,×tamp); _check_error(); msg.header.stamp = ros::Time(timestamp); } unsigned long framenumber; CamContext_get_last_framenumber(cc,&framenumber); _check_error(); msg.header.seq = framenumber; msg.header.frame_id = "0"; msg.height = height; msg.width = width; msg.encoding = encoding; msg.step = step; msg.data = data; // get current CameraInfo data cam_info = cam_info_manager->getCameraInfo(); cam_info.header.stamp = msg.header.stamp; cam_info.header.seq = msg.header.seq; cam_info.header.frame_id = msg.header.frame_id; cam_info.height = height; cam_info.width = width; _pub_image.publish(msg, cam_info); } ros::spinOnce(); } CamContext_stop_camera(cc); cam_iface_shutdown(); return 0; }