void arv_camera_set_binning (ArvCamera *camera, gint dx, gint dy) { g_return_if_fail (ARV_IS_CAMERA (camera)); if (dx > 0) arv_device_set_integer_feature_value (camera->priv->device, "BinningHorizontal", dx); if (dy > 0) arv_device_set_integer_feature_value (camera->priv->device, "BinningVertical", dy); }
void arv_camera_set_region (ArvCamera *camera, gint x, gint y, gint width, gint height) { g_return_if_fail (ARV_IS_CAMERA (camera)); if (x >= 0) arv_device_set_integer_feature_value (camera->priv->device, "OffsetX", x); if (y >= 0) arv_device_set_integer_feature_value (camera->priv->device, "OffsetY", y); if (width > 0) arv_device_set_integer_feature_value (camera->priv->device, "Width", width); if (height > 0) arv_device_set_integer_feature_value (camera->priv->device, "Height", height); }
void arv_gv_device_set_packet_size (ArvGvDevice *gv_device, guint packet_size) { g_return_if_fail (packet_size > 0); arv_device_set_integer_feature_value (ARV_DEVICE (gv_device), "GevSCPSPacketSize", packet_size); }
void arv_camera_set_pixel_format (ArvCamera *camera, ArvPixelFormat format) { g_return_if_fail (ARV_IS_CAMERA (camera)); arv_device_set_integer_feature_value (camera->priv->device, "PixelFormat", format); }
void BaslerCamera::SetCaptureMode(const std::string& mode) { if (mode == "continuous") { ROS_INFO(" Setting capture mode: continuous"); arv_device_set_integer_feature_value(device_, "TriggerMode", trigger_modes_["Off"]); int newMode = arv_device_get_integer_feature_value(device_, "TriggerMode"); if (newMode != trigger_modes_["Off"]) { ROS_WARN( "Failed to set capture mode"); } } else if (mode == "triggered") { ROS_INFO(" Setting capture mode: triggered"); if (trigger_control_implementations_.count("Standard") != 0) { ROS_INFO(" Setting TriggerControlImplementation: Standard"); arv_device_set_integer_feature_value(device_, "TriggerControlImplementation", trigger_control_implementations_["Standard"]); } if (trigger_selectors_.count("FrameStart") != 0) { ROS_INFO(" Setting TriggerSelector: FrameStart"); arv_device_set_integer_feature_value(device_, "TriggerSelector", trigger_selectors_["FrameStart"]); } if (trigger_modes_.count("On") != 0) { ROS_INFO(" Setting TriggerMode: On"); arv_device_set_integer_feature_value(device_, "TriggerMode", trigger_modes_["On"]); int newMode = arv_device_get_integer_feature_value(device_, "TriggerMode"); if (newMode != trigger_modes_["On"]) { ROS_WARN("Failed to set capture mode"); } } else { ROS_WARN("Failed to set capture mode"); } } }
void arv_camera_set_gain (ArvCamera *camera, gint gain) { g_return_if_fail (ARV_IS_CAMERA (camera)); if (gain < 0) return; arv_device_set_integer_feature_value (camera->priv->device, "GainRaw", gain); }
void BaslerCamera::SetAutoWhiteBalanceOnce(bool on) { ROS_INFO(" Setting auto white balance once: %d", static_cast<int>(on)); arv_device_set_integer_feature_value(device_,"BalanceWhiteAuto", static_cast<int>(on)); int newValue = arv_device_get_integer_feature_value(device_, "BalanceWhiteAuto"); if (newValue != static_cast<int>(on)) { ROS_WARN( "Failed to set auto white balance once: %d != %d", newValue, static_cast<int>(on)); } }
void BaslerCamera::SetDigitalShift(int shift) { ROS_INFO(" Setting digital shift: %d", shift); arv_device_set_integer_feature_value(device_, "DigitalShift", shift); int newShift = arv_device_get_integer_feature_value(device_, "DigitalShift"); if (newShift != shift) { ROS_WARN( "Failed to set digital shift: %d != %d", newShift, shift); } }
void arv_camera_set_gain (ArvCamera *camera, double gain) { g_return_if_fail (ARV_IS_CAMERA (camera)); if (gain < 0) return; if (camera->priv->use_gain_raw) arv_device_set_integer_feature_value (camera->priv->device, "GainRaw", gain); else arv_device_set_float_feature_value (camera->priv->device, "Gain", gain); }
void BaslerCamera::SetUserOutputSelector(const std::string& output) { ROS_INFO(" Setting User Output Selector : %s", output.c_str()); if (user_output_selector_.count(output) != 0) { arv_device_set_integer_feature_value(device_, "UserOutputSelector", user_output_selector_[output]); int newUserOutput = arv_device_get_integer_feature_value(device_, "UserOutputSelector"); if (newUserOutput != user_output_selector_[output]) { ROS_WARN(" Failed to set user output selector "); } } else { ROS_WARN(" Failed to set user output selector. Unsupported value."); } }
void BaslerCamera::SetLineOutputSelector(const std::string& line) { ROS_INFO(" Setting Line Output Selector : %s", line.c_str()); if (line_output_selector_.count(line) != 0) { arv_device_set_integer_feature_value(device_, "LineOutputSelector", line_output_selector_[line]); int newLineOutput = arv_device_get_integer_feature_value(device_, "LineOutputSelector"); if (newLineOutput != line_output_selector_[line]) { ROS_WARN(" Failed to set line output selector "); } } else { ROS_WARN(" Failed to set line output selector. Unsupported value."); } }
void BaslerCamera::SetLineFormat(const std::string& format) { ROS_INFO(" Setting line format : %s", format.c_str()); if (line_format_.count(format) != 0) { arv_device_set_integer_feature_value(device_, "LineFormat", line_format_[format]); int newLineFormat = arv_device_get_integer_feature_value(device_, "LineFormat"); if (newLineFormat != line_format_[format]) { ROS_WARN(" Failed to set line format %d %d",line_format_[format],newLineFormat); } } else { ROS_WARN(" Failed to set line format. Unsupported value."); } }
void BaslerCamera::SetLineMode(const std::string& mode) { ROS_INFO(" Setting line mode : %s", mode.c_str()); if (line_mode_.count(mode) != 0) { arv_device_set_integer_feature_value(device_, "LineMode", line_mode_[mode]); int newLineMode = arv_device_get_integer_feature_value(device_, "LineMode"); if (newLineMode != line_mode_[mode]) { ROS_WARN(" Failed to set line mode "); } } else { ROS_WARN(" Failed to set line mode. Unsupported value."); } }
void BaslerCamera::SetTriggerSource(const std::string& source) { ROS_INFO(" Setting trigger source: %s", source.c_str()); if (trigger_sources_.count(source) != 0) { arv_device_set_integer_feature_value(device_, "TriggerSource", trigger_sources_[source]); int newSource = arv_device_get_integer_feature_value(device_, "TriggerSource"); if (newSource != trigger_sources_[source]) { ROS_WARN(" Failed to set trigger source"); } } else { ROS_WARN(" Failed to set trigger source. Unsupported value."); } }
void BaslerCamera::SetTriggerActivation(const std::string& activation) { ROS_INFO(" Setting trigger activation: %s", activation.c_str()); if (trigger_activations_.count(activation) != 0) { arv_device_set_integer_feature_value(device_, "TriggerActivation", trigger_activations_[activation]); int newSource = arv_device_get_integer_feature_value(device_, "TriggerActivation"); if (newSource != trigger_activations_[activation]) { ROS_WARN(" Failed to set trigger activation"); } } else { ROS_WARN(" Failed to set trigger activation. Unsupported value."); } }
void BaslerCamera::SetTriggerSelector(const std::string& trigger) { ROS_INFO(" Setting trigger selector: %s", trigger.c_str()); if (trigger_selectors_.count(trigger) != 0) { arv_device_set_integer_feature_value(device_, "TriggerSelector", trigger_selectors_[trigger]); int newSource = arv_device_get_integer_feature_value(device_, "TriggerSelector"); if (newSource != trigger_selectors_[trigger]) { ROS_WARN(" Failed to set trigger selectors"); } } else { ROS_WARN(" Failed to set trigger selector. Unsupported value."); } }
void BaslerCamera::SetTriggerMode(const std::string& mode) { ROS_INFO(" Setting trigger mode: %s", mode.c_str()); if (trigger_modes_.count(mode) != 0) { arv_device_set_integer_feature_value(device_, "TriggerMode", trigger_modes_[mode]); int newMode = arv_device_get_integer_feature_value(device_, "TriggerMode"); if (newMode != trigger_modes_[mode]) { ROS_WARN(" Failed to set trigger Mode"); } } else { ROS_WARN(" Failed to set trigger Mode. Unsupported value."); } }
void BaslerCamera::SetLineSource(const std::string& source) { ROS_INFO(" Setting line source : %s", source.c_str()); if (line_source_.count(source) != 0) { arv_device_set_integer_feature_value(device_, "LineSource", line_source_[source]); int newLineSource = arv_device_get_integer_feature_value(device_, "LineSource"); if (newLineSource != line_source_[source]) { ROS_WARN(" Failed to set line source %d %d", line_source_[source], newLineSource); } } else { ROS_WARN(" Failed to set line source. Unsupported value."); } }
void arv_camera_set_exposure_time (ArvCamera *camera, double exposure_time_us) { g_return_if_fail (ARV_IS_CAMERA (camera)); if (exposure_time_us <= 0) return; switch (camera->priv->series) { case ARV_CAMERA_SERIES_BASLER_SCOUT: arv_device_set_float_feature_value (camera->priv->device, "ExposureTimeBaseAbs", exposure_time_us); arv_device_set_integer_feature_value (camera->priv->device, "ExposureTimeRaw", 1); break; case ARV_CAMERA_SERIES_BASLER_ACE: default: arv_device_set_float_feature_value (camera->priv->device, "ExposureTimeAbs", exposure_time_us); break; } }
void arv_camera_set_trigger (ArvCamera *camera, const char *source) { g_return_if_fail (ARV_IS_CAMERA (camera)); g_return_if_fail (source != NULL); switch (camera->priv->vendor) { case ARV_CAMERA_VENDOR_BASLER: arv_device_set_integer_feature_value (camera->priv->device, "AcquisitionFrameRateEnable", 0); default: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "AcquisitionStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "On"); arv_device_set_string_feature_value (camera->priv->device, "TriggerActivation", "RisingEdge"); arv_device_set_string_feature_value (camera->priv->device, "TriggerSource", source); break; } }
void arv_camera_set_frame_rate (ArvCamera *camera, double frame_rate) { g_return_if_fail (ARV_IS_CAMERA (camera)); if (frame_rate <= 0.0) return; switch (camera->priv->vendor) { case ARV_CAMERA_VENDOR_BASLER: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "AcquisitionStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_integer_feature_value (camera->priv->device, "AcquisitionFrameRateEnable", 1); arv_device_set_float_feature_value (camera->priv->device, "AcquisitionFrameRateAbs", frame_rate); break; case ARV_CAMERA_VENDOR_PROSILICA: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_float_feature_value (camera->priv->device, "AcquisitionFrameRateAbs", frame_rate); break; case ARV_CAMERA_VENDOR_UNKNOWN: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_float_feature_value (camera->priv->device, "AcquisitionFrameRate", frame_rate); break; } }
void arv_camera_set_frame_rate (ArvCamera *camera, double frame_rate) { ArvGcNode *feature; double minimum; double maximum; g_return_if_fail (ARV_IS_CAMERA (camera)); if (frame_rate <= 0.0) return; arv_camera_get_frame_rate_bounds(camera, &minimum, &maximum); if (frame_rate < minimum) frame_rate = minimum; if (frame_rate > maximum) frame_rate = maximum; switch (camera->priv->vendor) { case ARV_CAMERA_VENDOR_BASLER: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "AcquisitionStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_integer_feature_value (camera->priv->device, "AcquisitionFrameRateEnable", 1); arv_device_set_float_feature_value (camera->priv->device, "AcquisitionFrameRateAbs", frame_rate); break; case ARV_CAMERA_VENDOR_PROSILICA: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_float_feature_value (camera->priv->device, "AcquisitionFrameRateAbs", frame_rate); break; case ARV_CAMERA_VENDOR_TIS: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); feature = arv_device_get_feature (camera->priv->device, "FPS"); if (ARV_IS_GC_FEATURE_NODE (feature) && g_strcmp0 (arv_dom_node_get_node_name (ARV_DOM_NODE (feature)), "Enumeration") == 0) { gint64 *values; guint n_values; guint i; values = arv_gc_enumeration_get_available_int_values (ARV_GC_ENUMERATION (feature), &n_values, NULL); for (i = 0; i < n_values; i++) { if (values[i] > 0) { double e; e = (int)((10000000/(double) values[i]) * 100 + 0.5) / 100.0; if (e == frame_rate) { arv_gc_enumeration_set_int_value (ARV_GC_ENUMERATION (feature), values[i], NULL); break; } } } g_free (values); } else arv_device_set_float_feature_value (camera->priv->device, "FPS", frame_rate); break; case ARV_CAMERA_VENDOR_DALSA: case ARV_CAMERA_VENDOR_UNKNOWN: arv_device_set_string_feature_value (camera->priv->device, "TriggerSelector", "FrameStart"); arv_device_set_string_feature_value (camera->priv->device, "TriggerMode", "Off"); arv_device_set_float_feature_value (camera->priv->device, camera->priv->use_acquisition_frame_rate_abs ? "AcquisitionFrameRateAbs": "AcquisitionFrameRate", frame_rate); break; } }
// WriteCameraFeaturesFromRosparam() // Read ROS parameters from this node's namespace, and see if each parameter has a similarly named & typed feature in the camera. Then set the // camera feature to that value. For example, if the parameter camnode/Gain is set to 123.0, then we'll write 123.0 to the Gain feature // in the camera. // // Note that the datatype of the parameter *must* match the datatype of the camera feature, and this can be determined by // looking at the camera's XML file. Camera enum's are string parameters, camera bools are false/true parameters (not 0/1), // integers are integers, doubles are doubles, etc. // void WriteCameraFeaturesFromRosparam(void) { XmlRpc::XmlRpcValue xmlrpcParams; XmlRpc::XmlRpcValue::iterator iter; ArvGcNode *pGcNode; GError *error=NULL; global.phNode->getParam (ros::this_node::getName(), xmlrpcParams); if (xmlrpcParams.getType() == XmlRpc::XmlRpcValue::TypeStruct) { for (iter=xmlrpcParams.begin(); iter!=xmlrpcParams.end(); iter++) { std::string key = iter->first; pGcNode = arv_device_get_feature (global.pDevice, key.c_str()); if (pGcNode && arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error)) { // unsigned long typeValue = arv_gc_feature_node_get_value_type((ArvGcFeatureNode *)pGcNode); // ROS_INFO("%s cameratype=%lu, rosparamtype=%d", key.c_str(), typeValue, static_cast<int>(iter->second.getType())); // We'd like to check the value types too, but typeValue is often given as G_TYPE_INVALID, so ignore it. switch (iter->second.getType()) { case XmlRpc::XmlRpcValue::TypeBoolean://if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeBoolean))// && (typeValue==G_TYPE_INT64)) { int value = (bool)iter->second; arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value); ROS_INFO("Read parameter (bool) %s: %d", key.c_str(), value); } break; case XmlRpc::XmlRpcValue::TypeInt: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeInt))// && (typeValue==G_TYPE_INT64)) { int value = (int)iter->second; arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value); ROS_INFO("Read parameter (int) %s: %d", key.c_str(), value); } break; case XmlRpc::XmlRpcValue::TypeDouble: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeDouble))// && (typeValue==G_TYPE_DOUBLE)) { double value = (double)iter->second; arv_device_set_float_feature_value(global.pDevice, key.c_str(), value); ROS_INFO("Read parameter (float) %s: %f", key.c_str(), value); } break; case XmlRpc::XmlRpcValue::TypeString: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeString))// && (typeValue==G_TYPE_STRING)) { std::string value = (std::string)iter->second; arv_device_set_string_feature_value(global.pDevice, key.c_str(), value.c_str()); ROS_INFO("Read parameter (string) %s: %s", key.c_str(), value.c_str()); } break; case XmlRpc::XmlRpcValue::TypeInvalid: case XmlRpc::XmlRpcValue::TypeDateTime: case XmlRpc::XmlRpcValue::TypeBase64: case XmlRpc::XmlRpcValue::TypeArray: case XmlRpc::XmlRpcValue::TypeStruct: default: ROS_WARN("Unhandled rosparam type in WriteCameraFeaturesFromRosparam()"); } } } } } // WriteCameraFeaturesFromRosparam()
void RosReconfigure_callback(Config &config, uint32_t level) { int changedAcquire; int changedAcquisitionFrameRate; int changedExposureAuto; int changedGainAuto; int changedExposureTimeAbs; int changedGain; int changedAcquisitionMode; int changedTriggerMode; int changedTriggerSource; int changedSoftwarerate; int changedFrameid; int changedFocusPos; int changedMtu; std::string tf_prefix = tf::getPrefixParam(*global.phNode); ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix); if (config.frame_id == "") config.frame_id = "camera"; // Find what the user changed. changedAcquire = (global.config.Acquire != config.Acquire); changedAcquisitionFrameRate = (global.config.AcquisitionFrameRate != config.AcquisitionFrameRate); changedExposureAuto = (global.config.ExposureAuto != config.ExposureAuto); changedExposureTimeAbs = (global.config.ExposureTimeAbs != config.ExposureTimeAbs); changedGainAuto = (global.config.GainAuto != config.GainAuto); changedGain = (global.config.Gain != config.Gain); changedAcquisitionMode = (global.config.AcquisitionMode != config.AcquisitionMode); changedTriggerMode = (global.config.TriggerMode != config.TriggerMode); changedTriggerSource = (global.config.TriggerSource != config.TriggerSource); changedSoftwarerate = (global.config.softwaretriggerrate != config.softwaretriggerrate); changedFrameid = (global.config.frame_id != config.frame_id); changedFocusPos = (global.config.FocusPos != config.FocusPos); changedMtu = (global.config.mtu != config.mtu); // Limit params to legal values. config.AcquisitionFrameRate = CLIP(config.AcquisitionFrameRate, global.configMin.AcquisitionFrameRate, global.configMax.AcquisitionFrameRate); config.ExposureTimeAbs = CLIP(config.ExposureTimeAbs, global.configMin.ExposureTimeAbs, global.configMax.ExposureTimeAbs); config.Gain = CLIP(config.Gain, global.configMin.Gain, global.configMax.Gain); config.FocusPos = CLIP(config.FocusPos, global.configMin.FocusPos, global.configMax.FocusPos); config.frame_id = tf::resolve(tf_prefix, config.frame_id); // Adjust other controls dependent on what the user changed. if (changedExposureTimeAbs || changedGainAuto || ((changedAcquisitionFrameRate || changedGain || changedFrameid || changedAcquisitionMode || changedTriggerSource || changedSoftwarerate) && config.ExposureAuto=="Once")) config.ExposureAuto = "Off"; if (changedGain || changedExposureAuto || ((changedAcquisitionFrameRate || changedExposureTimeAbs || changedFrameid || changedAcquisitionMode || changedTriggerSource || changedSoftwarerate) && config.GainAuto=="Once")) config.GainAuto = "Off"; if (changedAcquisitionFrameRate) config.TriggerMode = "Off"; // Find what changed for any reason. changedAcquire = (global.config.Acquire != config.Acquire); changedAcquisitionFrameRate = (global.config.AcquisitionFrameRate != config.AcquisitionFrameRate); changedExposureAuto = (global.config.ExposureAuto != config.ExposureAuto); changedExposureTimeAbs = (global.config.ExposureTimeAbs != config.ExposureTimeAbs); changedGainAuto = (global.config.GainAuto != config.GainAuto); changedGain = (global.config.Gain != config.Gain); changedAcquisitionMode = (global.config.AcquisitionMode != config.AcquisitionMode); changedTriggerMode = (global.config.TriggerMode != config.TriggerMode); changedTriggerSource = (global.config.TriggerSource != config.TriggerSource); changedSoftwarerate = (global.config.softwaretriggerrate != config.softwaretriggerrate); changedFrameid = (global.config.frame_id != config.frame_id); changedFocusPos = (global.config.FocusPos != config.FocusPos); changedMtu = (global.config.mtu != config.mtu); // Set params into the camera. if (changedExposureTimeAbs) { if (global.isImplementedExposureTimeAbs) { ROS_INFO ("Set ExposureTimeAbs = %f", config.ExposureTimeAbs); arv_device_set_float_feature_value (global.pDevice, "ExposureTimeAbs", config.ExposureTimeAbs); } else ROS_INFO ("Camera does not support ExposureTimeAbs."); } if (changedGain) { if (global.isImplementedGain) { ROS_INFO ("Set gain = %f", config.Gain); //arv_device_set_integer_feature_value (global.pDevice, "GainRaw", config.GainRaw); arv_camera_set_gain (global.pCamera, config.Gain); } else ROS_INFO ("Camera does not support Gain or GainRaw."); } if (changedExposureAuto) { if (global.isImplementedExposureAuto && global.isImplementedExposureTimeAbs) { ROS_INFO ("Set ExposureAuto = %s", config.ExposureAuto.c_str()); arv_device_set_string_feature_value (global.pDevice, "ExposureAuto", config.ExposureAuto.c_str()); if (config.ExposureAuto=="Once") { ros::Duration(2.0).sleep(); config.ExposureTimeAbs = arv_device_get_float_feature_value (global.pDevice, "ExposureTimeAbs"); ROS_INFO ("Get ExposureTimeAbs = %f", config.ExposureTimeAbs); config.ExposureAuto = "Off"; } } else ROS_INFO ("Camera does not support ExposureAuto."); } if (changedGainAuto) { if (global.isImplementedGainAuto && global.isImplementedGain) { ROS_INFO ("Set GainAuto = %s", config.GainAuto.c_str()); arv_device_set_string_feature_value (global.pDevice, "GainAuto", config.GainAuto.c_str()); if (config.GainAuto=="Once") { ros::Duration(2.0).sleep(); //config.GainRaw = arv_device_get_integer_feature_value (global.pDevice, "GainRaw"); config.Gain = arv_camera_get_gain (global.pCamera); ROS_INFO ("Get Gain = %f", config.Gain); config.GainAuto = "Off"; } } else ROS_INFO ("Camera does not support GainAuto."); } if (changedAcquisitionFrameRate) { if (global.isImplementedAcquisitionFrameRate) { ROS_INFO ("Set %s = %f", global.keyAcquisitionFrameRate, config.AcquisitionFrameRate); arv_device_set_float_feature_value (global.pDevice, global.keyAcquisitionFrameRate, config.AcquisitionFrameRate); } else ROS_INFO ("Camera does not support AcquisitionFrameRate."); } if (changedTriggerMode) { if (global.isImplementedTriggerMode) { ROS_INFO ("Set TriggerMode = %s", config.TriggerMode.c_str()); arv_device_set_string_feature_value (global.pDevice, "TriggerMode", config.TriggerMode.c_str()); } else ROS_INFO ("Camera does not support TriggerMode."); } if (changedTriggerSource) { if (global.isImplementedTriggerSource) { ROS_INFO ("Set TriggerSource = %s", config.TriggerSource.c_str()); arv_device_set_string_feature_value (global.pDevice, "TriggerSource", config.TriggerSource.c_str()); } else ROS_INFO ("Camera does not support TriggerSource."); } if ((changedTriggerMode || changedTriggerSource || changedSoftwarerate) && config.TriggerMode=="On" && config.TriggerSource=="Software") { if (global.isImplementedAcquisitionFrameRate) { // The software rate is limited by the camera's internal framerate. Bump up the camera's internal framerate if necessary. config.AcquisitionFrameRate = global.configMax.AcquisitionFrameRate; ROS_INFO ("Set %s = %f", global.keyAcquisitionFrameRate, config.AcquisitionFrameRate); arv_device_set_float_feature_value (global.pDevice, global.keyAcquisitionFrameRate, config.AcquisitionFrameRate); } } if (changedTriggerSource || changedSoftwarerate) { // Recreate the software trigger callback. if (global.idSoftwareTriggerTimer) { g_source_remove(global.idSoftwareTriggerTimer); global.idSoftwareTriggerTimer = 0; } if (!strcmp(config.TriggerSource.c_str(),"Software")) { ROS_INFO ("Set softwaretriggerrate = %f", 1000.0/ceil(1000.0 / config.softwaretriggerrate)); // Turn on software timer callback. global.idSoftwareTriggerTimer = g_timeout_add ((guint)ceil(1000.0 / config.softwaretriggerrate), SoftwareTrigger_callback, global.pCamera); } } if (changedFocusPos) { if (global.isImplementedFocusPos) { ROS_INFO ("Set FocusPos = %d", config.FocusPos); arv_device_set_integer_feature_value(global.pDevice, "FocusPos", config.FocusPos); ros::Duration(1.0).sleep(); config.FocusPos = arv_device_get_integer_feature_value(global.pDevice, "FocusPos"); ROS_INFO ("Get FocusPos = %d", config.FocusPos); } else ROS_INFO ("Camera does not support FocusPos."); } if (changedMtu) { if (global.isImplementedMtu) { ROS_INFO ("Set mtu = %d", config.mtu); arv_device_set_integer_feature_value(global.pDevice, "GevSCPSPacketSize", config.mtu); ros::Duration(1.0).sleep(); config.mtu = arv_device_get_integer_feature_value(global.pDevice, "GevSCPSPacketSize"); ROS_INFO ("Get mtu = %d", config.mtu); } else ROS_INFO ("Camera does not support mtu (i.e. GevSCPSPacketSize)."); } if (changedAcquisitionMode) { if (global.isImplementedAcquisitionMode) { ROS_INFO ("Set AcquisitionMode = %s", config.AcquisitionMode.c_str()); arv_device_set_string_feature_value (global.pDevice, "AcquisitionMode", config.AcquisitionMode.c_str()); ROS_INFO("AcquisitionStop"); arv_device_execute_command (global.pDevice, "AcquisitionStop"); ROS_INFO("AcquisitionStart"); arv_device_execute_command (global.pDevice, "AcquisitionStart"); } else ROS_INFO ("Camera does not support AcquisitionMode."); } if (changedAcquire) { if (config.Acquire) { ROS_INFO("AcquisitionStart"); arv_device_execute_command (global.pDevice, "AcquisitionStart"); } else { ROS_INFO("AcquisitionStop"); arv_device_execute_command (global.pDevice, "AcquisitionStop"); } } global.config = config; } // RosReconfigure_callback()
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()