diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..0d3e900 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,55 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package camera_aravis +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +4.0.4 (2022-12-23) +------------------ +* Update package maintainer +* Refactor node params (`#21 `_) + * Refactor node params + * Rename extended_camera_info\_ -> pub_ext_camera_info\_ + * Move stream parameters to the top of onInit() +* fix: only reset PTP clock when in "Faulty" or "Disabled" state (`#23 `_) +* Update industrial_ci default branch to main +* Contributors: Dominik Kleiser, Peter Mortimer, Ruf, Boitumelo + +4.0.3 (2022-07-08) +------------------ +* Refactor image conversion (`#20 `_) +* Use plain file names for includes (`#17 `_) +* Add verbose flag for feature detection (default = false) (`#19 `_) +* Assume num_streams\_ = 1 if DeviceStreamChannelCount and GevStreamChannelCount unavailable (`#18 `_) +* Add Line0 to Line5 to TriggerSource Enum +* Fix: nodelet namespace +* Fix: onInit deadlock +* Contributors: Dominik Kleiser, Boitumelo Ruf, Thomas Emter, Peter Mortimer, tas, Geoff McIver + +4.0.2 (2022-05-04) +------------------ +* Add optional ExtendedCameraInfo message to publish additional camera acquisition parameters +* Fix: Set reasonable height and width when not given in the CameraInfo +* Contributors: Peter Mortimer + +4.0.1 (2022-03-25) +------------------ +* Add ROS getter/setter services for camera features +* Add support for multistream encoding conversion +* Fix: Pass on the correct encoding for the additional streams of multisource cameras +* Fix: Continuously check the spawning\_ flag +* Fix: Check spawning\_ flag only once during spawnStream +* Contributors: Peter Mortimer, Thomas Emter, Dominik Kleiser + +4.0.0 (2021-10-27) +------------------ +* Major refactoring +* Add support for ROS Noetic and aravis-0.6 +* Fix several bugs (see git history) +* Add new features: + + * Support for multisource cameras + * Zero-copy transport with ROS nodelets + * Camera time synchronization + * Example launch files + +* Update package author and maintainer +* Contributors: Dominik Klein, Floris van Breugel, Gaël Écorchard, Thomas Emter, Peter Mortimer, Dominik Kleiser diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a86017..540940e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS nodelet std_msgs sensor_msgs + diagnostic_msgs message_generation image_transport camera_info_manager @@ -28,12 +29,14 @@ find_package(catkin REQUIRED COMPONENTS ) find_package(Aravis REQUIRED) find_package(GLIB2 REQUIRED) +find_package(yaml-cpp REQUIRED) generate_dynamic_reconfigure_options(cfg/CameraAravis.cfg) add_message_files( FILES CameraAutoInfo.msg + CameraDiagnostics.msg ExtendedCameraInfo.msg ) @@ -53,6 +56,7 @@ generate_messages( DEPENDENCIES std_msgs sensor_msgs + diagnostic_msgs ) catkin_package( @@ -66,7 +70,9 @@ include_directories(cfg include ${catkin_INCLUDE_DIRS} ${Aravis_INCLUDE_DIRS} - ${GLIB2_INCLUDE_DIRS}) + ${GLIB2_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIRS} +) set(LIBRARIES ${catkin_LIBRARIES} @@ -74,6 +80,7 @@ set(LIBRARIES glib-2.0 gmodule-2.0 gobject-2.0 + ${YAML_CPP_LIBRARY_DIRS} ) add_library(${PROJECT_NAME} diff --git a/README.md b/README.md index bd312c3..1b82547 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,10 @@ a variety of camera features via the ROS reconfigure_gui, including the followin * FocusPos (integer) * mtu (integer) +Note that if the camera parameters are to be initialized from the parameters from the dynamic +reconfigure server, the launch parameter ```init_params_from_dyn_reconfigure``` needs to be set +to ```true``` (Default: ```false```); + Note that the above are also the ROS parameter names of their respective feature. You may set initial values for the camera by setting ROS parameters in the camera's namespace. @@ -101,7 +105,71 @@ with the ROS clock on the PC, and furthermore since it comes from a different pi the two clock's rates are slightly different. The solution is to start with a base of ROS time, and to accumulate the dt's from the camera clock. -To accomodate the difference in clock rates, a PID controller gently pulls the result toward +To accommodate the difference in clock rates, a PID controller gently pulls the result toward ROS time. +### Activating PTP Timestamp + +Some cameras support the use of the Precision Time Protocol (PTP) to set the timestamps of the +captured images. To activate it using camera_aravis a couple of launch parameters are available: + +- ```use_ptp_timestamp```: General switch to activate the use of the PTP timestamp within +camera_aravis. Set to ```true``` to activate. + - Type: ```bool``` + - Default: ```false``` +- ```ptp_enable_feature_name```: Feature name on the camera device to enable the use of PTP. + - Type: ```string``` + - Default: ```"GevIEEE1588"``` +- ```ptp_status_feature_name```: Feature name on the camera device to access the status of the PTP. +This is needed to monitor, whether camera_aravis needs to reset the PTP clock. + - Type: ```string``` + - Default: ```"GevIEEE1588Status"``` +- ```ptp_set_cmd_feature_name```: Feature name of the 'Set-Command' on the camera device for PTP. +On some cameras a 'set' or 'synchronization' command needs to be executed after setting the features +above for the PTP to be activated. If this launch parameter is set, the corresponding command will +be executed after the parameters above are set. + - Type: ```string``` + - Default: ```""``` + +## Publishing camera diagnostics / status + +Camera_aravis allows to periodically monitor custom camera features and publish them in a designated +topic named ```~/diagnostics``` in a message type as specified in +[CameraDiagnostics.msg](msg/CameraDiagnostics.msg). In order to configure and customize this +status monitoring, two launch parameters are provided: + +- ```diagnostic_publish_rate```: Rate at which to read and publish the diagnostic data. + - Type: ```double``` + - Default: ```0.1``` (10 seconds) +- ```diagnostic_yaml_url```: URL to yaml file specifying the camera features which are to be +monitored. If left empty (as default) no diagnostic features will be read and published. + - Type: ```string``` + - Default: ```""``` + +An example of such a diagnostic yaml file is given in +[camera_diagnostics.yaml](launch/camera_diagnostics.yaml). This file should hold a list of +```FeatureName``` together with a corresponding ```Type``` (bool, float, int, or string) for each +feature which is to be monitored. If a feature is associated with a feature selector, one can +additionally specify a list of ```Selectors```. Each entry in this list should again have a +```FeatureName``` and ```Type```, as well as a ```Value``` to set. + +For each feature a key-value pair is constructed and published in the ```data``` field of the +message stated above. If a feature as a list of selectors, one key-value pair is constructed for +each Feature-Selector pair. + +## Known Issues + +### Slow read of white balance and black level values + +From [PR#22](https://github.com/FraunhoferIOSB/camera_aravis/pull/22): The white balance and black +level values of some cameras (e.g. Basler acA2440-20gc & JAI FS-3200D-10GE ) can be read more +efficiently by reading from the exact memory locations of the camera instead of using the Selector +features. + +However, since we are not sure on how stable these optimizations based on exact memory locations are +in terms of firmware updates and how well they generalize to other camera models, we have refrained +from merging the pull request into the main repository. + +For more information and details on the implementation, please look into the changes and the +comments inside the pull request. diff --git a/include/camera_aravis/camera_aravis_nodelet.h b/include/camera_aravis/camera_aravis_nodelet.h index 8a1b6e5..663c130 100644 --- a/include/camera_aravis/camera_aravis_nodelet.h +++ b/include/camera_aravis/camera_aravis_nodelet.h @@ -54,6 +54,8 @@ extern "C" { #include #include +#include + #include #include #include @@ -61,6 +63,7 @@ extern "C" { #include #include #include +#include #include #include @@ -89,7 +92,12 @@ class CameraAravisNodelet : public nodelet::Nodelet private: bool verbose_ = false; std::string guid_ = ""; + bool use_ptp_stamp_ = false; + std::string ptp_enable_feature_ = "GevIEEE1588"; + std::string ptp_status_feature_ = "GevIEEE1588Status"; + std::string ptp_set_cmd_feature_ = ""; + bool pub_ext_camera_info_ = false; bool pub_tf_optical_ = false; @@ -168,6 +176,8 @@ class CameraAravisNodelet : public nodelet::Nodelet void publishTfLoop(double rate); + void readAndPublishCameraDiagnostics(double rate) const; + void discoverFeatures(); static void parseStringArgs(std::string in_arg_string, std::vector &out_args); @@ -196,6 +206,13 @@ class CameraAravisNodelet : public nodelet::Nodelet std::thread tf_dyn_thread_; std::atomic_bool tf_thread_active_; + std::string diagnostic_yaml_url_ = ""; + double diagnostic_publish_rate_ = 0.1; + YAML::Node diagnostic_features_; + ros::Publisher diagnostic_pub_; + std::thread diagnostic_thread_; + std::atomic_bool diagnostic_thread_active_; + CameraAutoInfo auto_params_; ros::Publisher auto_pub_; ros::Subscriber auto_sub_; diff --git a/launch/camera_aravis.launch b/launch/camera_aravis.launch index 21b774a..bb42a02 100644 --- a/launch/camera_aravis.launch +++ b/launch/camera_aravis.launch @@ -30,8 +30,7 @@ - - + @@ -46,6 +45,12 @@ + + + + + + diff --git a/launch/camera_diagnostics.yaml b/launch/camera_diagnostics.yaml new file mode 100644 index 0000000..a42f3a6 --- /dev/null +++ b/launch/camera_diagnostics.yaml @@ -0,0 +1,18 @@ +- FeatureName: DeviceTemperature + Type: float + Selectors: + - FeatureName: DeviceTemperatureSelector + Type: string + Value: Mainboard + - FeatureName: DeviceTemperatureSelector + Type: string + Value: Sensor +- FeatureName: PtpEnable + Type: bool +- FeatureName: PtpClockAccuracy + Type: string +- FeatureName: PtpOperationMode + Type: string +- FeatureName: PtpStatus + Type: string + diff --git a/msg/CameraDiagnostics.msg b/msg/CameraDiagnostics.msg new file mode 100644 index 0000000..17ff17b --- /dev/null +++ b/msg/CameraDiagnostics.msg @@ -0,0 +1,6 @@ +# Camera diagnostics message + +std_msgs/Header header + +# Array of key-value pairs holding the diagnostic data +diagnostic_msgs/KeyValue[] data \ No newline at end of file diff --git a/package.xml b/package.xml index 576715f..935bc86 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ camera_aravis - 4.0.0 + 4.0.4 camera_aravis: A complete and comfortable GenICam (USB3Vision and GigEVision) based camera driver for ROS (ethernet and usb). Boitumelo Ruf, Fraunhofer IOSB @@ -24,6 +24,7 @@ nodelet std_msgs sensor_msgs + diagnostic_msgs image_transport camera_info_manager dynamic_reconfigure diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index e9f1412..d7db8d8 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -23,6 +23,8 @@ #include +#include + #include PLUGINLIB_EXPORT_CLASS(camera_aravis::CameraAravisNodelet, nodelet::Nodelet) @@ -59,6 +61,12 @@ CameraAravisNodelet::~CameraAravisNodelet() tf_dyn_thread_.join(); } + diagnostic_thread_active_ = false; + if (diagnostic_thread_.joinable()) + { + diagnostic_thread_.join(); + } + for(int i=0; i < p_streams_.size(); i++) { guint64 n_completed_buffers = 0; @@ -97,7 +105,14 @@ void CameraAravisNodelet::onInit() // Retrieve ros parameters verbose_ = pnh.param("verbose", verbose_); guid_ = pnh.param("guid", guid_); // Get the camera guid as a parameter or use the first device. + use_ptp_stamp_ = pnh.param("use_ptp_timestamp", use_ptp_stamp_); + ptp_enable_feature_ = pnh.param("ptp_enable_feature_name", ptp_enable_feature_); + ptp_status_feature_ = pnh.param("ptp_status_feature_name", ptp_status_feature_); + ptp_set_cmd_feature_ = pnh.param("ptp_set_cmd_feature_name", ptp_set_cmd_feature_); + + bool init_params_from_reconfig = pnh.param("init_params_from_dyn_reconfigure", true); + pub_ext_camera_info_ = pnh.param("ExtendedCameraInfo", pub_ext_camera_info_); // publish an extended camera info message pub_tf_optical_ = pnh.param("publish_tf", pub_tf_optical_); // should we publish tf transforms to camera optical frame? @@ -118,6 +133,10 @@ void CameraAravisNodelet::onInit() pnh.param("camera_info_url", calib_url_args, calib_url_args); parseStringArgs(calib_url_args, calib_urls); + diagnostic_yaml_url_ = pnh.param("diagnostic_yaml_url", diagnostic_yaml_url_); + diagnostic_publish_rate_ = + std::max(0.0, pnh.param("diagnostic_publish_rate", 0.1)); + // Print out some useful info. ROS_INFO("Attached cameras:"); arv_update_device_list(); @@ -152,8 +171,12 @@ void CameraAravisNodelet::onInit() } p_device_ = arv_camera_get_device(p_camera_); - ROS_INFO("Opened: %s-%s", arv_camera_get_vendor_name(p_camera_), - arv_device_get_string_feature_value(p_device_, "DeviceSerialNumber")); + const char* vendor_name = arv_camera_get_vendor_name(p_camera_); + const char* model_name = arv_camera_get_model_name(p_camera_); + const char* device_id = arv_camera_get_device_id(p_camera_); + const char* device_sn = arv_device_get_string_feature_value(p_device_, "DeviceSerialNumber"); + ROS_INFO("Successfully Opened: %s-%s-%s", vendor_name, model_name, + (device_sn) ? device_sn : device_id); // Start the dynamic_reconfigure server. reconfigure_server_.reset(new dynamic_reconfigure::Server(reconfigure_mutex_, pnh)); @@ -221,32 +244,37 @@ void CameraAravisNodelet::onInit() for(int i = 0; i < num_streams_; i++) { arv_camera_gv_select_stream_channel(p_camera_,i); - // Initial camera settings. - if (implemented_features_["ExposureTime"]) - arv_camera_set_exposure_time(p_camera_, config_.ExposureTime); - else if (implemented_features_["ExposureTimeAbs"]) - arv_device_set_float_feature_value(p_device_, "ExposureTimeAbs", config_.ExposureTime); - if (implemented_features_["Gain"]) - arv_camera_set_gain(p_camera_, config_.Gain); - if (implemented_features_["AcquisitionFrameRateEnable"]) - arv_device_set_integer_feature_value(p_device_, "AcquisitionFrameRateEnable", 1); - if (implemented_features_["AcquisitionFrameRate"]) - arv_camera_set_frame_rate(p_camera_, config_.AcquisitionFrameRate); - - // init default to full sensor resolution - arv_camera_set_region (p_camera_, 0, 0, roi_.width_max, roi_.height_max); - - // Set up the triggering. - if (implemented_features_["TriggerMode"] && implemented_features_["TriggerSelector"]) + if(init_params_from_reconfig) { - arv_device_set_string_feature_value(p_device_, "TriggerSelector", "FrameStart"); - arv_device_set_string_feature_value(p_device_, "TriggerMode", "Off"); + // Initial camera settings. + if (implemented_features_["ExposureTime"]) + arv_camera_set_exposure_time(p_camera_, config_.ExposureTime); + else if (implemented_features_["ExposureTimeAbs"]) + arv_device_set_float_feature_value(p_device_, "ExposureTimeAbs", config_.ExposureTime); + if (implemented_features_["Gain"]) + arv_camera_set_gain(p_camera_, config_.Gain); + if (implemented_features_["AcquisitionFrameRateEnable"]) + arv_device_set_integer_feature_value(p_device_, "AcquisitionFrameRateEnable", 1); + if (implemented_features_["AcquisitionFrameRate"]) + arv_camera_set_frame_rate(p_camera_, config_.AcquisitionFrameRate); + + // init default to full sensor resolution + arv_camera_set_region (p_camera_, 0, 0, roi_.width_max, roi_.height_max); + + // Set up the triggering. + if (implemented_features_["TriggerMode"] && implemented_features_["TriggerSelector"]) + { + arv_device_set_string_feature_value(p_device_, "TriggerSelector", "FrameStart"); + arv_device_set_string_feature_value(p_device_, "TriggerMode", "Off"); + } } // possibly set or override from given parameter writeCameraFeaturesFromRosparam(); } + ros::Duration(2.0).sleep(); + // get current state of camera for config_ arv_camera_get_region(p_camera_, &roi_.x, &roi_.y, &roi_.width, &roi_.height); config_.AcquisitionMode = @@ -326,6 +354,45 @@ void CameraAravisNodelet::onInit() } } + // read diagnostic features from yaml file and initialize publishing thread + if(!diagnostic_yaml_url_.empty() && diagnostic_publish_rate_ > 0.0) + { + try + { + diagnostic_features_ = YAML::LoadFile(diagnostic_yaml_url_); + } + catch(const YAML::BadFile& e) + { + ROS_WARN("YAML file cannot be loaded: %s", e.what()); + ROS_WARN("Camera diagnostics will not be published."); + } + catch(const YAML::ParserException& e) + { + ROS_WARN("YAML file is malformed: %s", e.what()); + ROS_WARN("Camera diagnostics will not be published."); + } + + if(diagnostic_features_.size() > 0) + { + diagnostic_pub_ = getNodeHandle().advertise( + ros::names::remap(this->getName() + "/diagnostics"), 1, true); + + diagnostic_thread_active_ = true; + diagnostic_thread_ = std::thread(&CameraAravisNodelet::readAndPublishCameraDiagnostics, this, + diagnostic_publish_rate_); + } + else + { + ROS_WARN("No diagnostic features specified."); + ROS_WARN("Camera diagnostics will not be published."); + } + } + else if(!diagnostic_yaml_url_.empty() && diagnostic_publish_rate_ <= 0.0) + { + ROS_WARN("diagnostic_publish_rate is <= 0.0"); + ROS_WARN("Camera diagnostics will not be published."); + } + // default calibration url is [DeviceSerialNumber/DeviceID].yaml ArvGcNode *p_gc_node; GError *error = NULL; @@ -612,13 +679,45 @@ bool CameraAravisNodelet::setBooleanFeatureCallback(camera_aravis::set_boolean_f void CameraAravisNodelet::resetPtpClock() { + if(ptp_status_feature_.empty() || ptp_enable_feature_.empty()) + { + ROS_WARN("PTP Error: ptp_status_feature_name and/or ptp_enable_feature_name are empty."); + return; + } + + if(!implemented_features_[ptp_status_feature_] || !implemented_features_[ptp_enable_feature_]) + { + if(!implemented_features_[ptp_status_feature_]) + ROS_WARN("PTP Error: ptp_status_feature_name '%s' is not available on the device.", + ptp_status_feature_.c_str()); + if(!implemented_features_[ptp_enable_feature_]) + ROS_WARN("PTP Error: ptp_enable_feature_name '%s' is not available on the device.", + ptp_enable_feature_.c_str()); + return; + } + // a PTP slave can take the following states: Slave, Listening, Uncalibrated, Faulty, Disabled - std::string ptp_status(arv_device_get_string_feature_value(p_device_, "GevIEEE1588Status")); - if (ptp_status == std::string("Faulty") || ptp_status == std::string("Disabled")) + std::string ptp_status_str = + arv_device_get_string_feature_value(p_device_, ptp_status_feature_.c_str()); + bool ptp_is_enabled = arv_device_get_boolean_feature_value(p_device_, ptp_enable_feature_.c_str()); + if (ptp_status_str == "Faulty" || + ptp_status_str == "Disabled" || + ptp_status_str == "Initializing" || + !ptp_is_enabled) { - ROS_INFO("camera_aravis: Reset ptp clock (was set to %s)", ptp_status.c_str()); - arv_device_set_boolean_feature_value(p_device_, "GevIEEE1588", false); - arv_device_set_boolean_feature_value(p_device_, "GevIEEE1588", true); + ROS_INFO("Resetting ptp clock (was set to %s)", ptp_status_str.c_str()); + + arv_device_set_boolean_feature_value(p_device_, ptp_enable_feature_.c_str(), false); + arv_device_set_boolean_feature_value(p_device_, ptp_enable_feature_.c_str(), true); + + if(!ptp_set_cmd_feature_.empty()) + { + if(implemented_features_[ptp_set_cmd_feature_]) + arv_device_execute_command(p_device_, ptp_set_cmd_feature_.c_str()); + else + ROS_WARN("PTP Error: ptp_set_cmd_feature_ '%s' is not available on the device.", + ptp_set_cmd_feature_.c_str()); + } } } @@ -1401,6 +1500,154 @@ void CameraAravisNodelet::publishTfLoop(double rate) } } +void CameraAravisNodelet::readAndPublishCameraDiagnostics(double rate) const +{ + ROS_INFO("Publishing camera diagnostics at %g Hz", rate); + + ros::Rate loop_rate(rate); + + CameraDiagnostics camDiagnosticMsg; + camDiagnosticMsg.header.frame_id = config_.frame_id; + + // function to get feature value at given name and of given type as string + auto getFeatureValueAsStrFn = [&](const std::string &name, const std::string &type) + -> std::string + { + std::string valueStr = ""; + + if(type == "float") + valueStr = std::to_string(arv_device_get_float_feature_value(p_device_, name.c_str())); + else if (type == "int") + valueStr = std::to_string(arv_device_get_integer_feature_value(p_device_, name.c_str())); + else if (type == "bool") + valueStr = arv_device_get_boolean_feature_value(p_device_, name.c_str()) ? "true" : "false"; + else + valueStr = arv_device_get_string_feature_value(p_device_, name.c_str()); + + return valueStr; + }; + + // function to set feature value at given name and of given type from string + auto setFeatureValueFromStrFn = [&](const std::string &name, const std::string &type, + const std::string &valueStr) + { + if(type == "float") + arv_device_set_float_feature_value(p_device_, name.c_str(), std::stod(valueStr)); + else if (type == "int") + arv_device_set_integer_feature_value(p_device_, name.c_str(), std::stoi(valueStr)); + else if (type == "bool") + arv_device_set_boolean_feature_value(p_device_, name.c_str(), + (valueStr == "true" || valueStr == "True" || valueStr == "TRUE")); + else + arv_device_set_string_feature_value(p_device_, name.c_str(), valueStr.c_str()); + }; + + while (ros::ok() && diagnostic_thread_active_) + { + camDiagnosticMsg.header.stamp = ros::Time::now(); + ++camDiagnosticMsg.header.seq; + + camDiagnosticMsg.data.clear(); + + int featureIdx = 1; + for (auto featureDict : diagnostic_features_) { + std::string featureName = featureDict["FeatureName"].IsDefined() + ? featureDict["FeatureName"].as() + : ""; + std::string featureType = featureDict["Type"].IsDefined() + ? featureDict["Type"].as() + : ""; + + if(featureName.empty() || featureType.empty()) + { + ROS_WARN_ONCE( + "Diagnostic feature at index %i does not have a field 'FeatureName' or 'Type'.", + featureIdx); + ROS_WARN_ONCE("Diagnostic feature will be skipped."); + } + else + { + // convert type string to lower case + std::transform(featureType.begin(), featureType.end(), featureType.begin(), + [](unsigned char c){ return std::tolower(c); }); + + // if feature name is found in implemented_feature list and if enabled + if (implemented_features_.find(featureName) != implemented_features_.end() && + implemented_features_.at(featureName)) + { + diagnostic_msgs::KeyValue kvPair; + + // if 'selectors' which correspond to the featureName are defined + if(featureDict["Selectors"].IsDefined() && featureDict["Selectors"].size() > 0) + { + int selectorIdx = 1; + for(auto selectorDict : featureDict["Selectors"]) + { + std::string selectorFeatureName = selectorDict["FeatureName"].IsDefined() + ? selectorDict["FeatureName"].as() + : ""; + std::string selectorType = selectorDict["Type"].IsDefined() + ? selectorDict["Type"].as() + : ""; + std::string selectorValue = selectorDict["Value"].IsDefined() + ? selectorDict["Value"].as() + : ""; + + if(selectorFeatureName.empty() || selectorType.empty() || selectorValue.empty()) + { + ROS_WARN_ONCE( + "Diagnostic feature selector at index %i of feature at index %i does not have a " + "field 'FeatureName', 'Type' or 'Value'.", + selectorIdx, featureIdx); + ROS_WARN_ONCE("Diagnostic feature will be skipped."); + } + else + { + if (implemented_features_.find(selectorFeatureName) != implemented_features_.end() && + implemented_features_.at(selectorFeatureName)) + { + setFeatureValueFromStrFn(selectorFeatureName, selectorType, selectorValue); + + kvPair.key = featureName + "-" + selectorValue; + kvPair.value = getFeatureValueAsStrFn(featureName, featureType); + + camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair)); + } + else + { + ROS_WARN_ONCE("Diagnostic feature selector with name '%s' is not implemented.", + selectorFeatureName.c_str()); + } + + selectorIdx++; + } + } + } + else + { + kvPair.key = featureName; + kvPair.value = getFeatureValueAsStrFn(featureName, featureType); + + camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair)); + } + } + else + { + ROS_WARN_ONCE("Diagnostic feature with name '%s' is not implemented.", + featureName.c_str()); + } + } + + featureIdx++; + } + + diagnostic_pub_.publish(camDiagnosticMsg); + + loop_rate.sleep(); + } + +} + void CameraAravisNodelet::discoverFeatures() { implemented_features_.clear(); @@ -1529,7 +1776,7 @@ void CameraAravisNodelet::writeCameraFeaturesFromRosparam() 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(p_device_, key.c_str(), value); + arv_device_set_integer_feature_value(p_device_, key.c_str(), value); ROS_INFO("Read parameter (int) %s: %d", key.c_str(), value); } break; @@ -1537,7 +1784,7 @@ void CameraAravisNodelet::writeCameraFeaturesFromRosparam() 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(p_device_, key.c_str(), value); + arv_device_set_float_feature_value(p_device_, key.c_str(), value); ROS_INFO("Read parameter (float) %s: %f", key.c_str(), value); } break; @@ -1545,7 +1792,7 @@ void CameraAravisNodelet::writeCameraFeaturesFromRosparam() 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(p_device_, key.c_str(), value.c_str()); + arv_device_set_string_feature_value(p_device_, key.c_str(), value.c_str()); ROS_INFO("Read parameter (string) %s: %s", key.c_str(), value.c_str()); } break;