From b85ee548ef7b9819d5805444d97325d0552b072f Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Tue, 26 Oct 2021 09:53:28 +0200 Subject: [PATCH 01/21] Update package author and maintainer (#4) --- package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index b915faf..a941ccf 100644 --- a/package.xml +++ b/package.xml @@ -4,8 +4,9 @@ 4.0.0 camera_aravis: A complete and comfortable GenICam (USB3Vision and GigEVision) based camera driver for ROS (ethernet and usb). - Dominik A. Klein, Fraunhofer FKIE + Dominik Kleiser, Fraunhofer IOSB + Dominik Kleiser, Fraunhofer IOSB Dominik A. Klein, Fraunhofer FKIE Steve Safarik, Straw Lab Andrew Straw, Straw Lab @@ -13,7 +14,7 @@ LGPL v2 - https://github.com/fkie-forks/camera_aravis + https://github.com/FraunhoferIOSB/camera_aravis https://github.com/AravisProject/aravis catkin From 5a5f12dff8b56c208ac585810c10e99b6d513bff Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Wed, 27 Oct 2021 12:45:23 +0200 Subject: [PATCH 02/21] Fix build for Debian Buster (#5) Add dependency to libglib-dev --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index a941ccf..40585ac 100644 --- a/package.xml +++ b/package.xml @@ -31,6 +31,7 @@ aravis aravis-dev + libglib-dev message_generation message_runtime From c3e0a888905fd3cedb5c57cba4e6cb8f1ed5006d Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Wed, 27 Oct 2021 15:38:22 +0200 Subject: [PATCH 03/21] Add CHANGELOG.rst --- CHANGELOG.rst | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 CHANGELOG.rst diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..adc1a98 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,19 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package camera_aravis +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* 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 From 7ca090a10d1964d65c7ddb00861a1851daf4d9a8 Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Wed, 27 Oct 2021 15:39:51 +0200 Subject: [PATCH 04/21] 4.0.0 --- CHANGELOG.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index adc1a98..b2159e5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.0 (2021-10-27) +------------------ * Major refactoring * Add support for ROS Noetic and aravis-0.6 * Fix several bugs (see git history) From 0237433dbf0c08bab257781b8f9846ccc3c2146b Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Fri, 25 Mar 2022 16:28:18 +0100 Subject: [PATCH 05/21] Prepare changelog for forthcoming release --- CHANGELOG.rst | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b2159e5..701ba68 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 From a8d6734f2a66f7d594a394464ca800ddbcedf2ae Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Fri, 25 Mar 2022 16:34:43 +0100 Subject: [PATCH 06/21] 4.0.1 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 701ba68..2383f2a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/package.xml b/package.xml index 40585ac..852d386 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ camera_aravis - 4.0.0 + 4.0.1 camera_aravis: A complete and comfortable GenICam (USB3Vision and GigEVision) based camera driver for ROS (ethernet and usb). Dominik Kleiser, Fraunhofer IOSB From 8a857c5682715beed21dc5ab81f0dfdcdd4ee93a Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Wed, 4 May 2022 14:35:26 +0200 Subject: [PATCH 07/21] Merge branch 'master' into noetic-devel Squashed commit of the following: commit 52d773e8a01f438d4c4d6feec8a4a935bd457152 Author: Dominik Kleiser Date: Wed May 4 14:33:45 2022 +0200 Update README.md Update build farm badge commit a49c355686506355d96f689e79b62208fa577939 Author: Dominik Kleiser Date: Wed May 4 14:00:58 2022 +0200 Update README.md Update README.md badges commit d5a558b521d06a43e60c5c6472dbc89faa4562ec Merge: 8992a1b 6349cd6 Author: Thomas Emter <12640262+thomasemter@users.noreply.github.com> Date: Mon May 2 10:18:05 2022 +0200 Merge pull request #11 from tonyromarock/pr-extended-camera-info Pr extended camera info commit 6349cd6bbef277a0f0ca033549f545b783e97f40 Author: tas Date: Fri Apr 22 13:58:05 2022 +0200 set reasonable height and width when not given in the CameraInfo commit 61e14b3b8113116e3dcc387de7ee5cf0ecb8e620 Author: pemo Date: Thu Apr 21 22:03:02 2022 +0200 set GError to NULL to avoid the aravis warning when using a camera with custom feature names (e.g.: Basler) commit 7adeac3ed2fbb2c06638aff64330e2c82628c743 Author: pemo Date: Thu Apr 21 22:01:41 2022 +0200 add the optional ExtendedCameraInfo message to publish additional camera acquisition parameters --- CMakeLists.txt | 2 + README.md | 8 +- include/camera_aravis/camera_aravis_nodelet.h | 9 ++ msg/ExtendedCameraInfo.msg | 14 ++ src/camera_aravis_nodelet.cpp | 149 +++++++++++++++++- 5 files changed, 170 insertions(+), 12 deletions(-) create mode 100644 msg/ExtendedCameraInfo.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a478e9..f3d62de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ generate_dynamic_reconfigure_options(cfg/CameraAravis.cfg) add_message_files( FILES CameraAutoInfo.msg + ExtendedCameraInfo.msg ) add_service_files( @@ -41,6 +42,7 @@ add_service_files( generate_messages( DEPENDENCIES std_msgs + sensor_msgs ) catkin_package( diff --git a/README.md b/README.md index f125890..9bd1004 100644 --- a/README.md +++ b/README.md @@ -25,10 +25,10 @@ To run it in a given namespace, which is the better way to do it: ------------------------ ## Continuous Integration -service | Noetic | Melodic | Master ----------- | ------- | ------- | ------ -GitHub | TODO | TODO | [![CI](https://github.com/FraunhoferIOSB/camera_aravis/actions/workflows/industrial_ci_action.yml/badge.svg?branch=ci)](https://github.com/FraunhoferIOSB/camera_aravis/actions/workflows/industrial_ci_action.yml/badge.svg?branch=master) -build farm | TODO | TODO | TODO +| service | Noetic | Master +| ---------- | ------- | ------ +| GitHub | - | [![CI](https://github.com/FraunhoferIOSB/camera_aravis/actions/workflows/industrial_ci_action.yml/badge.svg?branch=ci)](https://github.com/FraunhoferIOSB/camera_aravis/actions/workflows/industrial_ci_action.yml/badge.svg?branch=master) +| Build Farm | [![Noetic](https://build.ros.org/job/Ndev__camera_aravis__ubuntu_focal_amd64/6/badge/icon?style=plastic&subject=Noetic)](https://build.ros.org/job/Ndev__camera_aravis__ubuntu_focal_amd64/6/) | - ------------------------ ## Configuration diff --git a/include/camera_aravis/camera_aravis_nodelet.h b/include/camera_aravis/camera_aravis_nodelet.h index 63a14a0..00b7d63 100644 --- a/include/camera_aravis/camera_aravis_nodelet.h +++ b/include/camera_aravis/camera_aravis_nodelet.h @@ -63,6 +63,7 @@ extern "C" { #include #include #include +#include #include #include @@ -222,6 +223,9 @@ class CameraAravisNodelet : public nodelet::Nodelet void setAutoMaster(bool value); void setAutoSlave(bool value); + void setExtendedCameraInfo(std::string channel_name, size_t stream_id); + void fillExtendedCameraInfoMessage(ExtendedCameraInfo &msg); + // Extra stream options for GigEVision streams. void tuneGvStream(ArvGvStream *p_stream); @@ -288,6 +292,7 @@ class CameraAravisNodelet : public nodelet::Nodelet std::vector cam_pubs_; std::vector> p_camera_info_managers_; + std::vector> p_camera_info_node_handles_; std::vector camera_infos_; std::unique_ptr p_stb_; @@ -300,6 +305,9 @@ class CameraAravisNodelet : public nodelet::Nodelet ros::Publisher auto_pub_; ros::Subscriber auto_sub_; + boost::recursive_mutex extended_camera_info_mutex_; + std::vector extended_camera_info_pubs_; + Config config_; Config config_min_; Config config_max_; @@ -347,6 +355,7 @@ class CameraAravisNodelet : public nodelet::Nodelet gint num_streams_; std::vector p_streams_; std::vector stream_names_; + bool extended_camera_info_; std::vector p_buffer_pools_; int32_t acquire_ = 0; std::vector convert_formats; diff --git a/msg/ExtendedCameraInfo.msg b/msg/ExtendedCameraInfo.msg new file mode 100644 index 0000000..d6d0586 --- /dev/null +++ b/msg/ExtendedCameraInfo.msg @@ -0,0 +1,14 @@ +# Contains the standard sensor_msgs/CameraInfo and additional information about the image acquisition parameters. +# This includes parameters on the exposure, gain, black level, white balance and device temperature. + +sensor_msgs/CameraInfo camera_info # contains a header and calibration parameters + +float64 exposure_time # in microseconds +float64 gain # unit is device-specific +float64 black_level # unit is device-specific + +float64 white_balance_red # unit is device-specific +float64 white_balance_green # unit is device-specific +float64 white_balance_blue # unit is device-specific + +float64 temperature # in degrees Celsius diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index e3793d3..8c03080 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -584,8 +584,10 @@ void CameraAravisNodelet::onInit() p_streams_.push_back(NULL); p_buffer_pools_.push_back(CameraBufferPool::Ptr()); p_camera_info_managers_.push_back(NULL); + p_camera_info_node_handles_.push_back(NULL); camera_infos_.push_back(sensor_msgs::CameraInfoPtr()); cam_pubs_.push_back(image_transport::CameraPublisher()); + extended_camera_info_pubs_.push_back(ros::Publisher()); } // Get parameter bounds. @@ -690,6 +692,9 @@ void CameraAravisNodelet::onInit() setAutoMaster(config_.AutoMaster); setAutoSlave(config_.AutoSlave); + // publish an extended camera info message + pnh.param("ExtendedCameraInfo", extended_camera_info_, false); + // should we publish tf transforms to camera optical frame? bool pub_tf_optical; pnh.param("publish_tf", pub_tf_optical, false); @@ -746,9 +751,24 @@ void CameraAravisNodelet::onInit() for(int i = 0; i < num_streams_; i++) { // Start the camerainfo manager. - p_camera_info_managers_[i].reset(new camera_info_manager::CameraInfoManager(pnh, stream_names_[i], calib_urls[i])); + std::string camera_info_frame_id = config_.frame_id; + if(!stream_names_[i].empty()) + camera_info_frame_id = config_.frame_id + '/' + stream_names_[i]; + + // Use separate node handles for CameraInfoManagers when using a Multisource Camera + if(!stream_names_[i].empty()) { + p_camera_info_node_handles_[i].reset(new ros::NodeHandle(pnh, stream_names_[i])); + p_camera_info_managers_[i].reset(new camera_info_manager::CameraInfoManager(*p_camera_info_node_handles_[i], camera_info_frame_id, calib_urls[i])); + } else { + p_camera_info_managers_[i].reset(new camera_info_manager::CameraInfoManager(pnh, camera_info_frame_id, calib_urls[i])); + } + + ROS_INFO("Reset %s Camera Info Manager", stream_names_[i].c_str()); ROS_INFO("%s Calib URL: %s", stream_names_[i].c_str(), calib_urls[i].c_str()); + + // publish an ExtendedCameraInfo message + setExtendedCameraInfo(stream_names_[i], i); } // update the reconfigure config @@ -1237,6 +1257,22 @@ void CameraAravisNodelet::setAutoSlave(bool value) } } +void CameraAravisNodelet::setExtendedCameraInfo(std::string channel_name, size_t stream_id) +{ + if (extended_camera_info_) + { + if (channel_name.empty()) { + extended_camera_info_pubs_[stream_id] = getNodeHandle().advertise(ros::names::remap("extended_camera_info"), 1, true); + } else { + extended_camera_info_pubs_[stream_id] = getNodeHandle().advertise(ros::names::remap(channel_name + "/extended_camera_info"), 1, true); + } + } + else + { + extended_camera_info_pubs_[stream_id].shutdown(); + } +} + // Extra stream options for GigEVision streams. void CameraAravisNodelet::tuneGvStream(ArvGvStream *p_stream) { @@ -1587,19 +1623,40 @@ void CameraAravisNodelet::newBufferReady(ArvStream *p_stream, CameraAravisNodele } (*p_can->camera_infos_[stream_id]) = p_can->p_camera_info_managers_[stream_id]->getCameraInfo(); p_can->camera_infos_[stream_id]->header = msg_ptr->header; - p_can->camera_infos_[stream_id]->width = p_can->roi_.width; - p_can->camera_infos_[stream_id]->height = p_can->roi_.height; + if (p_can->camera_infos_[stream_id]->width == 0 || p_can->camera_infos_[stream_id]->height == 0) { + ROS_WARN_STREAM_ONCE( + "The fields image_width and image_height seem not to be set in " + "the YAML specified by 'camera_info_url' parameter. Please set " + "them there, because actual image size and specified image size " + "can be different due to the region of interest (ROI) feature. In " + "the YAML the image size should be the one on which the camera was " + "calibrated. See CameraInfo.msg specification!"); + p_can->camera_infos_[stream_id]->width = p_can->roi_.width; + p_can->camera_infos_[stream_id]->height = p_can->roi_.height; + } + p_can->cam_pubs_[stream_id].publish(msg_ptr, p_can->camera_infos_[stream_id]); + if (p_can->extended_camera_info_) { + ExtendedCameraInfo extended_camera_info_msg; + p_can->extended_camera_info_mutex_.lock(); + arv_camera_gv_select_stream_channel(p_can->p_camera_, stream_id); + extended_camera_info_msg.camera_info = *(p_can->camera_infos_[stream_id]); + p_can->fillExtendedCameraInfoMessage(extended_camera_info_msg); + p_can->extended_camera_info_mutex_.unlock(); + p_can->extended_camera_info_pubs_[stream_id].publish(extended_camera_info_msg); + } + // check PTP status, camera cannot recover from "Faulty" by itself if (p_can->use_ptp_stamp_) p_can->resetPtpClock(); } else { - ROS_WARN("(%s) Frame error: %s", frame_id.c_str(), szBufferStatusFromInt[arv_buffer_get_status(p_buffer)]); - + if (arv_buffer_get_status(p_buffer) != ARV_BUFFER_STATUS_SUCCESS) { + ROS_WARN("(%s) Frame error: %s", frame_id.c_str(), szBufferStatusFromInt[arv_buffer_get_status(p_buffer)]); + } arv_stream_push_buffer(p_stream, p_buffer); } } @@ -1612,6 +1669,83 @@ void CameraAravisNodelet::newBufferReady(ArvStream *p_stream, CameraAravisNodele } } +void CameraAravisNodelet::fillExtendedCameraInfoMessage(ExtendedCameraInfo &msg) +{ + const char *vendor_name = arv_camera_get_vendor_name(p_camera_); + + if (strcmp("Basler", vendor_name) == 0) { + msg.exposure_time = arv_device_get_float_feature_value(p_device_, "ExposureTimeAbs"); + } + else if (implemented_features_["ExposureTime"]) + { + msg.exposure_time = arv_device_get_float_feature_value(p_device_, "ExposureTime"); + } + + if (strcmp("Basler", vendor_name) == 0) { + msg.gain = static_cast(arv_device_get_integer_feature_value(p_device_, "GainRaw")); + } + else if (implemented_features_["Gain"]) + { + msg.gain = arv_device_get_float_feature_value(p_device_, "Gain"); + } + if (strcmp("Basler", vendor_name) == 0) { + arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "All"); + msg.black_level = static_cast(arv_device_get_integer_feature_value(p_device_, "BlackLevelRaw")); + } else if (strcmp("JAI Corporation", vendor_name) == 0) { + // Reading the black level register for both streams of the JAI FS 3500D takes too long. + // The frame rate the drops below 10 fps. + msg.black_level = 0; + } else { + arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "All"); + msg.black_level = arv_device_get_float_feature_value(p_device_, "BlackLevel"); + } + + // White balance as TIS is providing + if (strcmp("The Imaging Source Europe GmbH", vendor_name) == 0) + { + msg.white_balance_red = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceRedRegister") / 255.; + msg.white_balance_green = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceGreenRegister") / 255.; + msg.white_balance_blue = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceBlueRegister") / 255.; + } + // the JAI cameras become too slow when reading out the DigitalRed and DigitalBlue values + // the white balance is adjusted by adjusting the Gain values for Red and Blue pixels + else if (strcmp("JAI Corporation", vendor_name) == 0) + { + msg.white_balance_red = 1.0; + msg.white_balance_green = 1.0; + msg.white_balance_blue = 1.0; + } + // the Basler cameras use the 'BalanceRatioAbs' keyword instead + else if (strcmp("Basler", vendor_name) == 0) + { + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red"); + msg.white_balance_red = arv_device_get_float_feature_value(p_device_, "BalanceRatioAbs"); + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green"); + msg.white_balance_green = arv_device_get_float_feature_value(p_device_, "BalanceRatioAbs"); + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue"); + msg.white_balance_blue = arv_device_get_float_feature_value(p_device_, "BalanceRatioAbs"); + } + // the standard way + else if (implemented_features_["BalanceRatio"] && implemented_features_["BalanceRatioSelector"]) + { + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red"); + msg.white_balance_red = arv_device_get_float_feature_value(p_device_, "BalanceRatio"); + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green"); + msg.white_balance_green = arv_device_get_float_feature_value(p_device_, "BalanceRatio"); + arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue"); + msg.white_balance_blue = arv_device_get_float_feature_value(p_device_, "BalanceRatio"); + } + + if (strcmp("Basler", vendor_name) == 0) { + msg.temperature = static_cast(arv_device_get_float_feature_value(p_device_, "TemperatureAbs")); + } + else if (implemented_features_["DeviceTemperature"]) + { + msg.temperature = arv_device_get_float_feature_value(p_device_, "DeviceTemperature"); + } + +} + void CameraAravisNodelet::controlLostCallback(ArvDevice *p_gv_device, gpointer can_instance) { CameraAravisNodelet *p_can = (CameraAravisNodelet*)can_instance; @@ -1684,7 +1818,6 @@ void CameraAravisNodelet::discoverFeatures() std::list todo; todo.push_front((ArvDomNode*)arv_gc_get_node(gc, "Root")); - GError *error = NULL; while (!todo.empty()) { @@ -1713,8 +1846,8 @@ void CameraAravisNodelet::discoverFeatures() //if (!(ARV_IS_GC_CATEGORY(node) || ARV_IS_GC_ENUM_ENTRY(node) /*|| ARV_IS_GC_PORT(node)*/)) { ArvGcFeatureNode *fnode = ARV_GC_FEATURE_NODE(node); const std::string fname(arv_gc_feature_node_get_name(fnode)); - const bool usable = arv_gc_feature_node_is_available(fnode, &error) - && arv_gc_feature_node_is_implemented(fnode, &error); + const bool usable = arv_gc_feature_node_is_available(fnode, NULL) + && arv_gc_feature_node_is_implemented(fnode, NULL); ROS_INFO_STREAM("Feature " << fname << " is " << usable); implemented_features_.emplace(fname, usable); //} From 9a8df7de53492e7fb1847cbe0d6b4511aa47091a Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Wed, 4 May 2022 15:38:58 +0200 Subject: [PATCH 08/21] Prepare changelog for forthcoming release --- CHANGELOG.rst | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 2383f2a..e5fed00 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 @@ -16,7 +23,6 @@ Changelog for package camera_aravis * Major refactoring * Add support for ROS Noetic and aravis-0.6 * Fix several bugs (see git history) - * Add new features: * Support for multisource cameras From fc8b883f4c03fe41d3a1482971bf9bae82010a96 Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Wed, 4 May 2022 15:40:05 +0200 Subject: [PATCH 09/21] 4.0.2 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index e5fed00..3ad8c5e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 diff --git a/package.xml b/package.xml index 852d386..6a67653 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ camera_aravis - 4.0.1 + 4.0.2 camera_aravis: A complete and comfortable GenICam (USB3Vision and GigEVision) based camera driver for ROS (ethernet and usb). Dominik Kleiser, Fraunhofer IOSB From e5495075aa5f43b284b4896fffb092c5029d2e6e Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Fri, 8 Jul 2022 10:18:25 +0200 Subject: [PATCH 10/21] Prepare changelog for forthcoming release --- CHANGELOG.rst | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3ad8c5e..790ab4b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,13 +2,23 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 From b3e6b7ae849417f9475a336a53c81cb40fbc5658 Mon Sep 17 00:00:00 2001 From: Dominik Kleiser Date: Fri, 8 Jul 2022 10:27:05 +0200 Subject: [PATCH 11/21] 4.0.3 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 790ab4b..fd35f9d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +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 `_) diff --git a/package.xml b/package.xml index 6a67653..36722b9 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ camera_aravis - 4.0.2 + 4.0.3 camera_aravis: A complete and comfortable GenICam (USB3Vision and GigEVision) based camera driver for ROS (ethernet and usb). Dominik Kleiser, Fraunhofer IOSB From a49c13e126866cb8338f40c1a7d7bd3f75510fba Mon Sep 17 00:00:00 2001 From: "Ruf, Boitumelo" Date: Fri, 23 Dec 2022 10:50:12 +0100 Subject: [PATCH 12/21] Prepare changelog for forthcoming release --- CHANGELOG.rst | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fd35f9d..d813b4e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* 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 `_) From 3a444a4cb5e06770e97d251b68b0b2cc2701d99a Mon Sep 17 00:00:00 2001 From: "Ruf, Boitumelo" Date: Fri, 23 Dec 2022 10:51:17 +0100 Subject: [PATCH 13/21] 4.0.4 --- CHANGELOG.rst | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d813b4e..0d3e900 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package camera_aravis ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +4.0.4 (2022-12-23) +------------------ * Update package maintainer * Refactor node params (`#21 `_) * Refactor node params diff --git a/package.xml b/package.xml index 0e112d7..0448af1 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ camera_aravis - 4.0.3 + 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 From 22cffd58e22454aeae8f3a239b545edca69c5a67 Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Wed, 26 Jul 2023 17:10:42 +0200 Subject: [PATCH 14/21] Update Readme.md --- README.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/README.md b/README.md index bd312c3..8b4a522 100644 --- a/README.md +++ b/README.md @@ -104,4 +104,19 @@ The solution is to start with a base of ROS time, and to accumulate the dt's fro To accomodate the difference in clock rates, a PID controller gently pulls the result toward ROS time. +## 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. From 97ef253bc10cc0296e9e4a68c04116e3fbe871fe Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Mon, 15 Jan 2024 10:03:26 +0100 Subject: [PATCH 15/21] Added two seconds sleep in camera initialization --- src/camera_aravis_nodelet.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index e9f1412..7454118 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) @@ -247,6 +249,8 @@ void CameraAravisNodelet::onInit() 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 = @@ -1529,7 +1533,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 +1541,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 +1549,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; From 102bd87e6ef976d925c55957a12a3fb4af051eeb Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Tue, 16 Jan 2024 09:28:19 +0100 Subject: [PATCH 16/21] Improved support for Precision Time Protocol --- README.md | 25 ++++++++++- include/camera_aravis/camera_aravis_nodelet.h | 5 +++ launch/camera_aravis.launch | 9 ++-- src/camera_aravis_nodelet.cpp | 45 ++++++++++++++++--- 4 files changed, 75 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 8b4a522..381c96e 100644 --- a/README.md +++ b/README.md @@ -101,9 +101,32 @@ 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: ```""``` + ## Known Issues ### Slow read of white balance and black level values diff --git a/include/camera_aravis/camera_aravis_nodelet.h b/include/camera_aravis/camera_aravis_nodelet.h index 8a1b6e5..3071607 100644 --- a/include/camera_aravis/camera_aravis_nodelet.h +++ b/include/camera_aravis/camera_aravis_nodelet.h @@ -89,7 +89,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; diff --git a/launch/camera_aravis.launch b/launch/camera_aravis.launch index 21b774a..6cc96f1 100644 --- a/launch/camera_aravis.launch +++ b/launch/camera_aravis.launch @@ -30,9 +30,6 @@ - - - @@ -46,6 +43,12 @@ + + + + + + diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index 7454118..632c692 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -99,7 +99,12 @@ 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_); + 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? @@ -616,13 +621,43 @@ 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()); + if (ptp_status_str == std::string("Faulty") || + ptp_status_str == std::string("Disabled") || + ptp_status_str == std::string("Initializing")) { - 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()); + } } } From d745515c3d62c2a6d034b47f38e3781965520234 Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Tue, 16 Jan 2024 15:15:47 +0100 Subject: [PATCH 17/21] bugfix in resetting ptp timestamp --- src/camera_aravis_nodelet.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index 632c692..17bceeb 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -643,7 +643,8 @@ void CameraAravisNodelet::resetPtpClock() arv_device_get_string_feature_value(p_device_, ptp_status_feature_.c_str()); if (ptp_status_str == std::string("Faulty") || ptp_status_str == std::string("Disabled") || - ptp_status_str == std::string("Initializing")) + ptp_status_str == std::string("Initializing") || + ! arv_device_get_boolean_feature_value(p_device_, ptp_enable_feature_.c_str())); { ROS_INFO("Resetting ptp clock (was set to %s)", ptp_status_str.c_str()); From 299dca1be6ab430cd2aafae52314295e1cbdc02d Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Tue, 16 Jan 2024 16:17:58 +0100 Subject: [PATCH 18/21] Feature: read and publish camera diagnostics --- CMakeLists.txt | 9 +- README.md | 26 +++ include/camera_aravis/camera_aravis_nodelet.h | 12 ++ launch/camera_aravis.launch | 2 + launch/camera_diagnostics.yaml | 18 ++ msg/CameraDiagnostics.msg | 6 + package.xml | 1 + src/camera_aravis_nodelet.cpp | 197 ++++++++++++++++++ 8 files changed, 270 insertions(+), 1 deletion(-) create mode 100644 launch/camera_diagnostics.yaml create mode 100644 msg/CameraDiagnostics.msg 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 381c96e..6b12e3e 100644 --- a/README.md +++ b/README.md @@ -127,6 +127,32 @@ 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 diff --git a/include/camera_aravis/camera_aravis_nodelet.h b/include/camera_aravis/camera_aravis_nodelet.h index 3071607..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 @@ -173,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); @@ -201,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 6cc96f1..bb42a02 100644 --- a/launch/camera_aravis.launch +++ b/launch/camera_aravis.launch @@ -30,6 +30,8 @@ + + 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 0448af1..935bc86 100644 --- a/package.xml +++ b/package.xml @@ -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 17bceeb..cf2c18b 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -61,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; @@ -125,6 +131,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(); @@ -335,6 +345,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; @@ -1441,6 +1490,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(); From 3a0e0b25eb8a31c5e918d14234efa7b1bb8dd570 Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Tue, 16 Jan 2024 16:47:41 +0100 Subject: [PATCH 19/21] bugfix: Opened device output --- src/camera_aravis_nodelet.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index cf2c18b..c86fa04 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -169,8 +169,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)); From e93e1508b6a397e18cf67b98e58245a9e61ff3de Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Wed, 17 Jan 2024 11:55:46 +0100 Subject: [PATCH 20/21] Added parameter init_params_from_dyn_reconfigure and minor bugfix --- README.md | 4 +++ src/camera_aravis_nodelet.cpp | 52 +++++++++++++++++++---------------- 2 files changed, 33 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index 6b12e3e..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. diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index c86fa04..f7f192b 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -110,6 +110,8 @@ void CameraAravisNodelet::onInit() 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", false); 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? @@ -242,26 +244,29 @@ 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 @@ -694,10 +699,11 @@ void CameraAravisNodelet::resetPtpClock() // a PTP slave can take the following states: Slave, Listening, Uncalibrated, Faulty, Disabled std::string ptp_status_str = arv_device_get_string_feature_value(p_device_, ptp_status_feature_.c_str()); - if (ptp_status_str == std::string("Faulty") || - ptp_status_str == std::string("Disabled") || - ptp_status_str == std::string("Initializing") || - ! arv_device_get_boolean_feature_value(p_device_, ptp_enable_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("Resetting ptp clock (was set to %s)", ptp_status_str.c_str()); From 7627413c1aa7b09b3adcb18b4e71e5fed707eda3 Mon Sep 17 00:00:00 2001 From: Boitumelo Ruf Date: Wed, 17 Jan 2024 12:16:19 +0100 Subject: [PATCH 21/21] minor change for backwards compatibility --- src/camera_aravis_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp index f7f192b..d7db8d8 100644 --- a/src/camera_aravis_nodelet.cpp +++ b/src/camera_aravis_nodelet.cpp @@ -111,7 +111,7 @@ void CameraAravisNodelet::onInit() 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", false); + 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?