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; }
CameraNode::CameraNode(ros::NodeHandle &node_priv, int argc, char** argv) : _node_priv(node_priv), _verbose(0), _got_frame(false), _host_timestamp(0), _device_number(-1), _interface_name("") { char mode_string[255]; int _ghost_timestamp = 0; int _phost_timestamp = 0; ros::param::get ("host_timestamp", _ghost_timestamp); _node_priv.getParam("host_timestamp", _phost_timestamp); _host_timestamp = 1 ? _ghost_timestamp || _phost_timestamp : 0; if (_host_timestamp) ROS_INFO("host timestamps ON"); else ROS_INFO("host timestamps OFF"); _node_priv.getParam("verbose", _verbose); /* if the user supplies a devicqe_guid or number (or puts it in the parameter server under the path of this node, use that. For example $ rosrun camiface_ros camnode _device_guid:=Prosilica-02-2020C-06732 */ int param_device_number = -1; _node_priv.getParam("device_number", param_device_number); std::string param_device_guid; _node_priv.getParam("device_guid", param_device_guid); int param_device_guid_int = -1; _node_priv.getParam("device_guid", param_device_guid_int); if (param_device_guid_int != -1 && param_device_guid.empty()) { ROS_WARN("stupid ros weakly typed parameter server - converting guid to string"); std::stringstream out; out << param_device_guid_int; param_device_guid = out.str(); } int param_device_mode = -1; _node_priv.getParam("device_mode", param_device_mode); _node_priv.getParam("interface_name", _interface_name); cam_iface_startup_with_version_check(); _check_error(); ROS_DEBUG("using driver %s",cam_iface_get_driver_name()); int ncams = cam_iface_get_num_cameras(); _check_error(); if (ncams<1) { ROS_WARN("no cameras found, will now exit"); cam_iface_shutdown(); _check_error(); exit(1); } if (_verbose) ROS_INFO("%d camera(s) found",ncams); std::vector<std::string> safe_names; for (int i=0; i<ncams; i++) { Camwire_id cam_info_struct; cam_iface_get_camera_info(i, &cam_info_struct); if (cam_iface_have_error()==CAM_IFACE_CAMERA_NOT_AVAILABLE_ERROR) { ROS_WARN("camera %d: (not available - in use?)",i); cam_iface_clear_error(); safe_names.push_back("?"); continue; } _check_error(); if (_verbose) ROS_INFO("camera %d guid: %s",i,cam_info_struct.chip); std::string sn = make_safe_name(cam_info_struct.chip); safe_names.push_back( sn ); ROS_DEBUG("camera safe name: %s",sn.c_str()); if ((param_device_number != -1) && (param_device_number == i)) { _device_number = i; ROS_INFO("using user supplied device_number %d", param_device_number); } else if (param_device_guid.length() && (cam_info_struct.chip == param_device_guid)) { _device_number = i; ROS_INFO("using user supplied device_guid %s", param_device_guid.c_str()); } } if (safe_names.empty()) { ROS_WARN("No cameras available"); exit(1); } else if (_device_number == -1 && ((param_device_number != -1) || param_device_guid.length())) { ROS_WARN("Selected camera %s (%d) not found", param_device_guid.c_str(), param_device_number); exit(1); } else { Camwire_id cam_info_struct; if (_device_number == -1) { ROS_INFO("No explicitly selected camera"); _device_number = 0; /* choose first camera if the user didn't specify */ } cam_iface_get_camera_info(_device_number, &cam_info_struct); _check_error(); ROS_INFO("choosing camera %d (%s guid:%s) on %s interface", _device_number, cam_info_struct.vendor, cam_info_struct.chip, _interface_name.length() ? _interface_name.c_str() : "any"); } int mode_number = 0; if (param_device_mode != -1) { ROS_INFO("user specified mode"); mode_number = param_device_mode; } else { int num_modes; ROS_INFO("attempting to auto choose mode"); cam_iface_get_num_modes(_device_number, &num_modes); _check_error(); if (_verbose) ROS_INFO("%d mode(s) available:",num_modes); for (int 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; } } if (_verbose) ROS_INFO(" %d: %s",i,mode_string); } } cam_iface_get_mode_string(_device_number,mode_number,mode_string,255); ROS_INFO("chose mode %d (%s)", mode_number, mode_string); cam_iface_constructor_func_t new_CamContext = cam_iface_get_constructor_func(_device_number); cc = new_CamContext( _device_number, 5 /*num buffers*/, mode_number, _interface_name.length() ? _interface_name.c_str() : NULL); _check_error(); int left, top; CamContext_get_frame_roi(cc, &left, &top, &width, &height); _check_error(); // camera nodes must support at least shutter and gain int num_props; CamContext_get_num_camera_properties(cc,&num_props); _check_error(); ROS_DEBUG("%d camera properties:",num_props); for (int i=0; i<num_props; i++) { CameraPropertyInfo cam_props; CamContext_get_camera_property_info(cc,i,&cam_props); _check_error(); if (cam_props.is_present) { if (cam_props.available) { _property_numbers[std::string(cam_props.name)] = i; if (_verbose) ROS_DEBUG(" %s (#%d): ",cam_props.name, i); } } } ROS_INFO("starting camera info manager for: %s", safe_names.at(_device_number).c_str()); cam_info_manager = new camera_info_manager::CameraInfoManager(_node_priv, safe_names.at(_device_number)); image_transport::ImageTransport *transport = new image_transport::ImageTransport(_node_priv); _pub_image = transport->advertiseCamera(_node_priv.resolveName("image_raw"), 1); _pub_rate = _node_priv.advertise<std_msgs::Float32>("framerate", 5); int num_trigger_modes; CamContext_get_num_trigger_modes( cc, &num_trigger_modes ); _check_error(); if (_verbose) ROS_INFO("%d trigger source(s):", num_trigger_modes); for (int i=0; i<num_trigger_modes; i++) { CamContext_get_trigger_mode_string( cc, i, mode_string, 255 ); _trigger_modes[std::string(mode_string)] = i; if (_verbose) ROS_INFO(" %s (#%d)", mode_string, i); } switch (cc->coding) { case CAM_IFACE_MONO8_BAYER_BGGR: step = width; encoding = sensor_msgs::image_encodings::BAYER_BGGR8; break; case CAM_IFACE_MONO8_BAYER_RGGB: step = width; encoding = sensor_msgs::image_encodings::BAYER_RGGB8; break; case CAM_IFACE_MONO8_BAYER_GRBG: step = width; encoding = sensor_msgs::image_encodings::BAYER_GRBG8; break; case CAM_IFACE_MONO8_BAYER_GBRG: step = width; encoding = sensor_msgs::image_encodings::BAYER_GBRG8; break; case CAM_IFACE_MONO8: step = width; encoding = sensor_msgs::image_encodings::MONO8; break; case CAM_IFACE_RGB8: step = width*3; encoding = sensor_msgs::image_encodings::RGB8; break; case CAM_IFACE_YUV422: step = width; encoding = sensor_msgs::image_encodings::YUV422; break; case CAM_IFACE_UNKNOWN: ROS_ERROR("unknown cam_iface pixel coding"); /* fall through */ default: ROS_ERROR("unsupported cam_iface pixel coding: 0x%x", cc->coding); cam_iface_get_mode_string(_device_number,mode_number,mode_string,255); ROS_FATAL("camera mode not supported: %s", mode_string); exit(1); } int framerate = -1; _node_priv.getParam("framerate", framerate); if (framerate > 0) { ROS_INFO("setting framerate -> %d", framerate); CamContext_set_framerate(cc, framerate); } CamContext_start_camera(cc); _check_error(); }