bool CvCaptureCAM_Aravis::open( int index ) { if(create(index)) { // fetch properties bounds pixelFormats = arv_camera_get_available_pixel_formats(camera, &pixelFormatsCnt); arv_camera_get_width_bounds(camera, &widthMin, &widthMax); arv_camera_get_height_bounds(camera, &heightMin, &heightMax); arv_camera_set_region(camera, 0, 0, widthMax, heightMax); if( (fpsAvailable = arv_camera_is_frame_rate_available(camera)) ) arv_camera_get_frame_rate_bounds(camera, &fpsMin, &fpsMax); if( (gainAvailable = arv_camera_is_gain_available(camera)) ) arv_camera_get_gain_bounds (camera, &gainMin, &gainMax); if( (exposureAvailable = arv_camera_is_exposure_time_available(camera)) ) arv_camera_get_exposure_time_bounds (camera, &exposureMin, &exposureMax); // get initial values pixelFormat = arv_camera_get_pixel_format(camera); exposure = exposureAvailable ? arv_camera_get_exposure_time(camera) : 0; gain = gainAvailable ? arv_camera_get_gain(camera) : 0; fps = arv_camera_get_frame_rate(camera); return startCapture(); } return false; }
bool CameraGigeAravis::grabInitialization(){ frameCounter = 0; payload = arv_camera_get_payload (camera); BOOST_LOG_SEV(logger, notification) << "Camera payload : " << payload; pixFormat = arv_camera_get_pixel_format(camera); arv_camera_get_exposure_time_bounds (camera, &exposureMin, &exposureMax); BOOST_LOG_SEV(logger, notification) << "Camera exposure bound min : " << exposureMin; BOOST_LOG_SEV(logger, notification) << "Camera exposure bound max : " << exposureMax; arv_camera_get_gain_bounds (camera, &gainMin, &gainMax); BOOST_LOG_SEV(logger, notification) << "Camera gain bound min : " << gainMin; BOOST_LOG_SEV(logger, notification) << "Camera gain bound max : " << gainMax; arv_camera_set_frame_rate(camera, 30); fps = arv_camera_get_frame_rate(camera); BOOST_LOG_SEV(logger, notification) << "Camera frame rate : " << fps; capsString = arv_pixel_format_to_gst_caps_string(pixFormat); BOOST_LOG_SEV(logger, notification) << "Camera format : " << capsString; gain = arv_camera_get_gain(camera); BOOST_LOG_SEV(logger, notification) << "Camera gain : " << gain; exp = arv_camera_get_exposure_time(camera); BOOST_LOG_SEV(logger, notification) << "Camera exposure : " << exp; cout << endl; cout << "DEVICE SELECTED : " << arv_camera_get_device_id(camera) << endl; cout << "DEVICE NAME : " << arv_camera_get_model_name(camera) << endl; cout << "DEVICE VENDOR : " << arv_camera_get_vendor_name(camera) << endl; cout << "PAYLOAD : " << payload << endl; cout << "Width : " << mWidth << endl << "Height : " << mHeight << endl; cout << "Exp Range : [" << exposureMin << " - " << exposureMax << "]" << endl; cout << "Exp : " << exp << endl; cout << "Gain Range : [" << gainMin << " - " << gainMax << "]" << endl; cout << "Gain : " << gain << endl; cout << "Fps : " << fps << endl; cout << "Type : " << capsString << endl; cout << endl; // Create a new stream object. Open stream on Camera. stream = arv_camera_create_stream(camera, NULL, NULL); if(stream == NULL){ BOOST_LOG_SEV(logger, critical) << "Fail to create stream with arv_camera_create_stream()"; return false; } if (ARV_IS_GV_STREAM(stream)){ bool arv_option_auto_socket_buffer = true; bool arv_option_no_packet_resend = true; unsigned int arv_option_packet_timeout = 20; unsigned int arv_option_frame_retention = 100; if(arv_option_auto_socket_buffer){ g_object_set(stream, // ARV_GV_STREAM_SOCKET_BUFFER_FIXED : socket buffer is set to a given fixed value. // ARV_GV_STREAM_SOCKET_BUFFER_AUTO: socket buffer is set with respect to the payload size. "socket-buffer", ARV_GV_STREAM_SOCKET_BUFFER_AUTO, // Socket buffer size, in bytes. // Allowed values: >= G_MAXULONG // Default value: 0 "socket-buffer-size", 0, NULL); } if(arv_option_no_packet_resend){ // # packet-resend : Enables or disables the packet resend mechanism // If packet resend is disabled and a packet has been lost during transmission, // the grab result for the returned buffer holding the image will indicate that // the grab failed and the image will be incomplete. // // If packet resend is enabled and a packet has been lost during transmission, // a request is sent to the camera. If the camera still has the packet in its // buffer, it will resend the packet. If there are several lost packets in a // row, the resend requests will be combined. g_object_set(stream, // ARV_GV_STREAM_PACKET_RESEND_NEVER: never request a packet resend // ARV_GV_STREAM_PACKET_RESEND_ALWAYS: request a packet resend if a packet was missing // Default value: ARV_GV_STREAM_PACKET_RESEND_ALWAYS "packet-resend", ARV_GV_STREAM_PACKET_RESEND_NEVER, NULL); } g_object_set(stream, // # packet-timeout // The Packet Timeout parameter defines how long (in milliseconds) we will wait for // the next expected packet before it initiates a resend request. // Packet timeout, in µs. // Allowed values: [1000,10000000] // Default value: 40000 "packet-timeout",/* (unsigned) arv_option_packet_timeout * 1000*/(unsigned)40000, // # frame-retention // The Frame Retention parameter sets the timeout (in milliseconds) for the // frame retention timer. Whenever detection of the leader is made for a frame, // the frame retention timer starts. The timer resets after each packet in the // frame is received and will timeout after the last packet is received. If the // timer times out at any time before the last packet is received, the buffer for // the frame will be released and will be indicated as an unsuccessful grab. // Packet retention, in µs. // Allowed values: [1000,10000000] // Default value: 200000 "frame-retention", /*(unsigned) arv_option_frame_retention * 1000*/(unsigned) 200000,NULL); }else return false; // Push 50 buffer in the stream input buffer queue. for (int i = 0; i < 50; i++) arv_stream_push_buffer(stream, arv_buffer_new(payload, NULL)); return true; }
/** * Initialize components for gtk window */ static Win* create_ui () { Win* w = malloc(sizeof(Win)); /* create a new window */ w->main_window = gtk_window_new (GTK_WINDOW_TOPLEVEL); /* area that will display our stream */ w->video_window = gtk_drawing_area_new (); /* ===== framerate settings ===== */ w->framerate_field = gtk_entry_new(); /* connect button event to callback function */ w->framerate_send_button = gtk_button_new_with_label("Set FrameRate"); g_signal_connect (G_OBJECT (w->framerate_send_button), "clicked", G_CALLBACK (framerate_value_changed), w); /* ===== exposure scale ===== */ w->exposure_label = gtk_label_new("Exposure:"); gdouble value; g_object_get (G_OBJECT (p.source), "exposure", &value, NULL); w->exposure_value_label = gtk_label_new("0.0"); /* the label should give enough space to not rezise */ gtk_label_set_width_chars (GTK_LABEL(w->exposure_value_label), 10); /* the scale will have a range from 0 to 100 to use the percentage to implement a log scale */ double rmin = 0; double rmax = 100; double rangelen2 = log(rmax) - log(rmin); double exposure_value = (ABSVAL_SLIDER_TICKS) / rangelen2 * ( log(value) - log(rmin) ); gdouble exposure_lower = 0.0; /* min value */ gdouble exposure_upper = 100.0; /* max value */ gdouble exposure_step_increment = 1.0; /* step size */ gdouble exposure_page_increment = -10.0; /* negatic value, to make page up increase value */ gdouble exposure_page_size = 0.0; GtkObject* wat = gtk_adjustment_new ( exposure_value, exposure_lower, exposure_upper, exposure_step_increment, exposure_page_increment, exposure_page_size); w->exposure_hscale = gtk_hscale_new (GTK_ADJUSTMENT(wat)); gtk_scale_set_draw_value(GTK_SCALE (w->exposure_hscale), FALSE); gtk_widget_set_usize (GTK_WIDGET (w->exposure_hscale), 400, 30); g_signal_connect (G_OBJECT(wat), "value-changed", G_CALLBACK (exposure_value_changed), w); set_slider_label (GTK_LABEL(w->exposure_value_label), value); /* ===== gain part ===== */ w->gain_label = gtk_label_new("Gain:"); double gain_value; gdouble gain_lower; /* min value */ gdouble gain_upper; /* max value */ w->gain_value_label = gtk_label_new("0.0"); g_object_get (G_OBJECT (p.source), "gain", &gain_value, NULL); arv_camera_get_gain_bounds (p.camera, &gain_lower, &gain_upper); set_slider_label (GTK_LABEL(w->gain_value_label), gain_value); gdouble gain_step_increment = 0.1; /* step size */ gdouble gain_page_increment = -1.0; gdouble gain_page_size = 0.0; GtkObject* range_gain = gtk_adjustment_new (gain_value, gain_lower, gain_upper, gain_step_increment, gain_page_increment, gain_page_size); w->gain_hscale = gtk_hscale_new (GTK_ADJUSTMENT(range_gain)); gtk_scale_set_draw_value(GTK_SCALE (w->gain_hscale), FALSE); gtk_widget_set_usize (GTK_WIDGET (w->gain_hscale), 400, 50); g_signal_connect (G_OBJECT(range_gain), "value-changed", G_CALLBACK (gain_value_changed), w); GtkWidget* toggle; toggle = gtk_button_new_with_label("Start/Stop"); g_signal_connect (G_OBJECT (toggle), "clicked", G_CALLBACK (togglePipelineState), NULL); /* ===== organize everything and place in window ===== */ w->controls = gtk_hbox_new (FALSE, 0); gtk_box_pack_start (GTK_BOX (w->controls), toggle, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->framerate_field, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->framerate_send_button, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->exposure_label, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->exposure_value_label, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->exposure_hscale, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->gain_label, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->gain_value_label, FALSE, FALSE, 2); gtk_box_pack_start (GTK_BOX (w->controls), w->gain_hscale, FALSE, FALSE, 2); w->main_hbox = gtk_hbox_new (FALSE, 0); gtk_box_pack_start (GTK_BOX (w->main_hbox), w->video_window, TRUE, TRUE, 0); w->main_box = gtk_vbox_new (FALSE, 0); gtk_box_pack_start (GTK_BOX (w->main_box), w->main_hbox, TRUE, TRUE, 0); gtk_box_pack_start (GTK_BOX (w->main_box), w->controls, FALSE, FALSE, 0); gtk_container_add (GTK_CONTAINER (w->main_window), w->main_box); gtk_window_set_default_size (GTK_WINDOW (w->main_window), HEIGHT, WIDTH); /* show all widgets we just added to out main window */ gtk_widget_show_all (w->main_window); return w; }
int main(int argc, char** argv) { char *pszGuid = NULL; char szGuid[512]; int nInterfaces = 0; int nDevices = 0; int i = 0; const char *pkeyAcquisitionFrameRate[2] = {"AcquisitionFrameRate", "AcquisitionFrameRateAbs"}; ArvGcNode *pGcNode; GError *error=NULL; global.bCancel = FALSE; global.config = global.config.__getDefault__(); global.idSoftwareTriggerTimer = 0; ros::init(argc, argv, "camera"); global.phNode = new ros::NodeHandle(); // Service callback for firing nuc's. // Needed since we cannot open another connection to cameras while streaming. ros::NodeHandle nh; ros::ServiceServer NUCservice = nh.advertiseService("FireNUC", NUCService_callback); //g_type_init (); // Print out some useful info. ROS_INFO ("Attached cameras:"); arv_update_device_list(); nInterfaces = arv_get_n_interfaces(); ROS_INFO ("# Interfaces: %d", nInterfaces); nDevices = arv_get_n_devices(); ROS_INFO ("# Devices: %d", nDevices); for (i=0; i<nDevices; i++) ROS_INFO ("Device%d: %s", i, arv_get_device_id(i)); if (nDevices>0) { // Get the camera guid from either the command-line or as a parameter. if (argc==2) { strcpy(szGuid, argv[1]); pszGuid = szGuid; } else { if (global.phNode->hasParam(ros::this_node::getName()+"/guid")) { std::string stGuid; global.phNode->getParam(ros::this_node::getName()+"/guid", stGuid); strcpy (szGuid, stGuid.c_str()); pszGuid = szGuid; } else pszGuid = NULL; } // Open the camera, and set it up. ROS_INFO("Opening: %s", pszGuid ? pszGuid : "(any)"); while (TRUE) { global.pCamera = arv_camera_new(pszGuid); if (global.pCamera) break; else { ROS_WARN ("Could not open camera %s. Retrying...", pszGuid); ros::Duration(1.0).sleep(); ros::spinOnce(); } } global.pDevice = arv_camera_get_device(global.pCamera); ROS_INFO("Opened: %s-%s", arv_device_get_string_feature_value (global.pDevice, "DeviceVendorName"), arv_device_get_string_feature_value (global.pDevice, "DeviceID")); // See if some basic camera features exist. pGcNode = arv_device_get_feature (global.pDevice, "AcquisitionMode"); global.isImplementedAcquisitionMode = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "GainRaw"); global.isImplementedGain = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "Gain"); global.isImplementedGain |= ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "ExposureTimeAbs"); global.isImplementedExposureTimeAbs = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "ExposureAuto"); global.isImplementedExposureAuto = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "GainAuto"); global.isImplementedGainAuto = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "TriggerSelector"); global.isImplementedTriggerSelector = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "TriggerSource"); global.isImplementedTriggerSource = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "TriggerMode"); global.isImplementedTriggerMode = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "FocusPos"); global.isImplementedFocusPos = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "GevSCPSPacketSize"); global.isImplementedMtu = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; pGcNode = arv_device_get_feature (global.pDevice, "AcquisitionFrameRateEnable"); global.isImplementedAcquisitionFrameRateEnable = ARV_GC_FEATURE_NODE (pGcNode) ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; // Find the key name for framerate. global.keyAcquisitionFrameRate = NULL; for (i=0; i<2; i++) { pGcNode = arv_device_get_feature (global.pDevice, pkeyAcquisitionFrameRate[i]); global.isImplementedAcquisitionFrameRate = pGcNode ? arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error) : FALSE; if (global.isImplementedAcquisitionFrameRate) { global.keyAcquisitionFrameRate = pkeyAcquisitionFrameRate[i]; break; } } // Get parameter bounds. arv_camera_get_exposure_time_bounds (global.pCamera, &global.configMin.ExposureTimeAbs, &global.configMax.ExposureTimeAbs); arv_camera_get_gain_bounds (global.pCamera, &global.configMin.Gain, &global.configMax.Gain); arv_camera_get_sensor_size (global.pCamera, &global.widthSensor, &global.heightSensor); arv_camera_get_width_bounds (global.pCamera, &global.widthRoiMin, &global.widthRoiMax); arv_camera_get_height_bounds (global.pCamera, &global.heightRoiMin, &global.heightRoiMax); if (global.isImplementedFocusPos) { gint64 focusMin64, focusMax64; arv_device_get_integer_feature_bounds (global.pDevice, "FocusPos", &focusMin64, &focusMax64); global.configMin.FocusPos = focusMin64; global.configMax.FocusPos = focusMax64; } else { global.configMin.FocusPos = 0; global.configMax.FocusPos = 0; } global.configMin.AcquisitionFrameRate = 0.0; global.configMax.AcquisitionFrameRate = 1000.0; // Initial camera settings. if (global.isImplementedExposureTimeAbs) arv_device_set_float_feature_value(global.pDevice, "ExposureTimeAbs", global.config.ExposureTimeAbs); if (global.isImplementedGain) arv_camera_set_gain(global.pCamera, global.config.Gain); //arv_device_set_integer_feature_value(global.pDevice, "GainRaw", global.config.GainRaw); if (global.isImplementedAcquisitionFrameRateEnable) arv_device_set_integer_feature_value(global.pDevice, "AcquisitionFrameRateEnable", 1); if (global.isImplementedAcquisitionFrameRate) arv_device_set_float_feature_value(global.pDevice, global.keyAcquisitionFrameRate, global.config.AcquisitionFrameRate); // Set up the triggering. if (global.isImplementedTriggerMode) { if (global.isImplementedTriggerSelector && global.isImplementedTriggerMode) { arv_device_set_string_feature_value(global.pDevice, "TriggerSelector", "AcquisitionStart"); arv_device_set_string_feature_value(global.pDevice, "TriggerMode", "Off"); arv_device_set_string_feature_value(global.pDevice, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value(global.pDevice, "TriggerMode", "Off"); } } WriteCameraFeaturesFromRosparam (); #ifdef TUNING ros::Publisher pubInt64 = global.phNode->advertise<std_msgs::Int64>(ros::this_node::getName()+"/dt", 100); global.ppubInt64 = &pubInt64; #endif // Grab the calibration file url from the param server if exists std::string calibrationURL = ""; // Default looks in .ros/camera_info if (!(ros::param::get(std::string("calibrationURL").append(arv_device_get_string_feature_value (global.pDevice, "DeviceID")), calibrationURL))) { ROS_ERROR("ERROR: Could not read calibrationURL from parameter server"); } // Start the camerainfo manager. global.pCameraInfoManager = new camera_info_manager::CameraInfoManager(ros::NodeHandle(ros::this_node::getName()), arv_device_get_string_feature_value (global.pDevice, "DeviceID"), calibrationURL); // Start the dynamic_reconfigure server. dynamic_reconfigure::Server<Config> reconfigureServer; dynamic_reconfigure::Server<Config>::CallbackType reconfigureCallback; reconfigureCallback = boost::bind(&RosReconfigure_callback, _1, _2); reconfigureServer.setCallback(reconfigureCallback); ros::Duration(2.0).sleep(); // Get parameter current values. global.xRoi=0; global.yRoi=0; global.widthRoi=0; global.heightRoi=0; arv_camera_get_region (global.pCamera, &global.xRoi, &global.yRoi, &global.widthRoi, &global.heightRoi); global.config.ExposureTimeAbs = global.isImplementedExposureTimeAbs ? arv_device_get_float_feature_value (global.pDevice, "ExposureTimeAbs") : 0; global.config.Gain = global.isImplementedGain ? arv_camera_get_gain (global.pCamera) : 0.0; global.pszPixelformat = g_string_ascii_down(g_string_new(arv_device_get_string_feature_value(global.pDevice, "PixelFormat")))->str; global.nBytesPixel = ARV_PIXEL_FORMAT_BYTE_PER_PIXEL(arv_device_get_integer_feature_value(global.pDevice, "PixelFormat")); global.config.FocusPos = global.isImplementedFocusPos ? arv_device_get_integer_feature_value (global.pDevice, "FocusPos") : 0; // Print information. ROS_INFO (" Using Camera Configuration:"); ROS_INFO (" ---------------------------"); ROS_INFO (" Vendor name = %s", arv_device_get_string_feature_value (global.pDevice, "DeviceVendorName")); ROS_INFO (" Model name = %s", arv_device_get_string_feature_value (global.pDevice, "DeviceModelName")); ROS_INFO (" Device id = %s", arv_device_get_string_feature_value (global.pDevice, "DeviceID")); ROS_INFO (" Sensor width = %d", global.widthSensor); ROS_INFO (" Sensor height = %d", global.heightSensor); ROS_INFO (" ROI x,y,w,h = %d, %d, %d, %d", global.xRoi, global.yRoi, global.widthRoi, global.heightRoi); ROS_INFO (" Pixel format = %s", global.pszPixelformat); ROS_INFO (" BytesPerPixel = %d", global.nBytesPixel); ROS_INFO (" Acquisition Mode = %s", global.isImplementedAcquisitionMode ? arv_device_get_string_feature_value (global.pDevice, "AcquisitionMode") : "(not implemented in camera)"); ROS_INFO (" Trigger Mode = %s", global.isImplementedTriggerMode ? arv_device_get_string_feature_value (global.pDevice, "TriggerMode") : "(not implemented in camera)"); ROS_INFO (" Trigger Source = %s", global.isImplementedTriggerSource ? arv_device_get_string_feature_value(global.pDevice, "TriggerSource") : "(not implemented in camera)"); ROS_INFO (" Can set FrameRate: %s", global.isImplementedAcquisitionFrameRate ? "True" : "False"); if (global.isImplementedAcquisitionFrameRate) { global.config.AcquisitionFrameRate = arv_device_get_float_feature_value (global.pDevice, global.keyAcquisitionFrameRate); ROS_INFO (" AcquisitionFrameRate = %g hz", global.config.AcquisitionFrameRate); } ROS_INFO (" Can set Exposure: %s", global.isImplementedExposureTimeAbs ? "True" : "False"); if (global.isImplementedExposureTimeAbs) { ROS_INFO (" Can set ExposureAuto: %s", global.isImplementedExposureAuto ? "True" : "False"); ROS_INFO (" Exposure = %g us in range [%g,%g]", global.config.ExposureTimeAbs, global.configMin.ExposureTimeAbs, global.configMax.ExposureTimeAbs); } ROS_INFO (" Can set Gain: %s", global.isImplementedGain ? "True" : "False"); if (global.isImplementedGain) { ROS_INFO (" Can set GainAuto: %s", global.isImplementedGainAuto ? "True" : "False"); ROS_INFO (" Gain = %f %% in range [%f,%f]", global.config.Gain, global.configMin.Gain, global.configMax.Gain); } ROS_INFO (" Can set FocusPos: %s", global.isImplementedFocusPos ? "True" : "False"); if (global.isImplementedMtu) ROS_INFO (" Network mtu = %lu", arv_device_get_integer_feature_value(global.pDevice, "GevSCPSPacketSize")); ROS_INFO (" ---------------------------"); // // Print the tree of camera features, with their values. // ROS_INFO (" ----------------------------------------------------------------------------------"); // NODEEX nodeex; // ArvGc *pGenicam=0; // pGenicam = arv_device_get_genicam(global.pDevice); // // nodeex.szName = "Root"; // nodeex.pNode = (ArvDomNode *)arv_gc_get_node(pGenicam, nodeex.szName); // nodeex.pNodeSibling = NULL; // PrintDOMTree(pGenicam, nodeex, 0); // ROS_INFO (" ----------------------------------------------------------------------------------"); ArvGvStream *pStream = NULL; while (TRUE) { pStream = CreateStream(); if (pStream) break; else { ROS_WARN("Could not create image stream for %s. Retrying...", pszGuid); ros::Duration(1.0).sleep(); ros::spinOnce(); } } ApplicationData applicationdata; applicationdata.nBuffers=0; applicationdata.main_loop = 0; // Set up image_raw. image_transport::ImageTransport *pTransport = new image_transport::ImageTransport(*global.phNode); global.publisher = pTransport->advertiseCamera(ros::this_node::getName()+"/image_raw", 1); // Connect signals with callbacks. g_signal_connect (pStream, "new-buffer", G_CALLBACK (NewBuffer_callback), &applicationdata); g_signal_connect (global.pDevice, "control-lost", G_CALLBACK (ControlLost_callback), NULL); g_timeout_add_seconds (1, PeriodicTask_callback, &applicationdata); arv_stream_set_emit_signals ((ArvStream *)pStream, TRUE); void (*pSigintHandlerOld)(int); pSigintHandlerOld = signal (SIGINT, set_cancel); arv_device_execute_command (global.pDevice, "AcquisitionStart"); applicationdata.main_loop = g_main_loop_new (NULL, FALSE); g_main_loop_run (applicationdata.main_loop); if (global.idSoftwareTriggerTimer) { g_source_remove(global.idSoftwareTriggerTimer); global.idSoftwareTriggerTimer = 0; } signal (SIGINT, pSigintHandlerOld); g_main_loop_unref (applicationdata.main_loop); guint64 n_completed_buffers; guint64 n_failures; guint64 n_underruns; guint64 n_resent; guint64 n_missing; arv_stream_get_statistics ((ArvStream *)pStream, &n_completed_buffers, &n_failures, &n_underruns); ROS_INFO ("Completed buffers = %Lu", (unsigned long long) n_completed_buffers); ROS_INFO ("Failures = %Lu", (unsigned long long) n_failures); ROS_INFO ("Underruns = %Lu", (unsigned long long) n_underruns); arv_gv_stream_get_statistics (pStream, &n_resent, &n_missing); ROS_INFO ("Resent buffers = %Lu", (unsigned long long) n_resent); ROS_INFO ("Missing = %Lu", (unsigned long long) n_missing); arv_device_execute_command (global.pDevice, "AcquisitionStop"); g_object_unref (pStream); } else ROS_ERROR ("No cameras detected."); delete global.phNode; return 0; } // main()