From 55b878eb81c83f1e9185955c05f4dd6d861a0356 Mon Sep 17 00:00:00 2001 From: Stian Pedersen Date: Tue, 11 Jun 2024 13:16:55 +0200 Subject: [PATCH 1/2] Update all C++ code to match ros2 style This is purely a code formatting change using the .clang-format from ament_clang_format in ROS2. This is done in a pre-commit to make code review easier. MISC --- .clang-format | 79 +-- .../include/auto_generated_include_wrapper.h | 16 +- .../include/capture_settings_controller.h | 43 +- zivid_camera/include/config_utils_common.h | 6 +- zivid_camera/include/zivid_camera.h | 88 +-- .../src/capture_settings_controller.cpp | 162 +++-- zivid_camera/src/node.cpp | 28 +- zivid_camera/src/nodelet.cpp | 16 +- zivid_camera/src/settings_generator.cpp | 397 ++++------- zivid_camera/src/zivid_camera.cpp | 464 +++++++------ zivid_camera/test/test_zivid_camera.cpp | 647 +++++++++--------- zivid_samples/src/sample_capture.cpp | 36 +- zivid_samples/src/sample_capture_2d.cpp | 31 +- zivid_samples/src/sample_capture_and_save.cpp | 32 +- .../src/sample_capture_assistant.cpp | 44 +- .../sample_capture_with_settings_from_yml.cpp | 28 +- 16 files changed, 988 insertions(+), 1129 deletions(-) diff --git a/.clang-format b/.clang-format index c5881310..aaca8594 100644 --- a/.clang-format +++ b/.clang-format @@ -1,65 +1,20 @@ +# Based on https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format --- -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignEscapedNewlinesLeft: false -AlignTrailingComments: true -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AllowShortFunctionsOnASingleLine: None -AllowShortLoopsOnASingleLine: false -AlwaysBreakTemplateDeclarations: true -AlwaysBreakBeforeMultilineStrings: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true -BinPackParameters: true -ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: false -SpaceAfterCStyleCast: false +Language: Cpp +BasedOnStyle: Google -# Configure each individual brace in BraceWrapping +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterEnum: true BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: { - AfterClass: 'true' - AfterControlStatement: 'true' - AfterEnum : 'true' - AfterFunction : 'true' - AfterNamespace : 'true' - AfterStruct : 'true' - AfterUnion : 'true' - BeforeCatch : 'true' - BeforeElse : 'true' - IndentBraces : 'false' -} +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false diff --git a/zivid_camera/include/auto_generated_include_wrapper.h b/zivid_camera/include/auto_generated_include_wrapper.h index 8ab6558c..8b8f39f9 100644 --- a/zivid_camera/include/auto_generated_include_wrapper.h +++ b/zivid_camera/include/auto_generated_include_wrapper.h @@ -7,16 +7,16 @@ #pragma GCC system_header #endif -#include -#include -#include -#include +#include +#include #include -#include #include +#include #include -#include -#include #include -#include #include +#include +#include +#include +#include +#include diff --git a/zivid_camera/include/capture_settings_controller.h b/zivid_camera/include/capture_settings_controller.h index 64b1333e..278454ee 100644 --- a/zivid_camera/include/capture_settings_controller.h +++ b/zivid_camera/include/capture_settings_controller.h @@ -1,13 +1,12 @@ #pragma once -#include "auto_generated_include_wrapper.h" - #include #include #include - #include +#include "auto_generated_include_wrapper.h" + namespace Zivid { class Camera; @@ -28,37 +27,43 @@ class ConfigDRServer; /// on, handles callbacks and updates the internal config state. It also provides methods to convert /// from/to Zivid::Settings/Settings2D objects. /// -template +template < + typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> class CaptureSettingsController { - static_assert(std::is_same_v || - std::is_same_v); - static_assert(std::is_same_v || - std::is_same_v); - static_assert(std::is_same_v || - std::is_same_v); + static_assert( + std::is_same_v || + std::is_same_v); + static_assert( + std::is_same_v || + std::is_same_v); + static_assert( + std::is_same_v || + std::is_same_v); public: - CaptureSettingsController(ros::NodeHandle& nh, Zivid::Camera& camera, const std::string& config_node_name, - std::size_t num_acquisition_servers); + CaptureSettingsController( + ros::NodeHandle & nh, Zivid::Camera & camera, const std::string & config_node_name, + std::size_t num_acquisition_servers); ~CaptureSettingsController(); ZividSettingsType zividSettings() const; - void setZividSettings(const ZividSettingsType& settings); + void setZividSettings(const ZividSettingsType & settings); std::size_t numAcquisitionConfigServers() const; private: using SettingsConfigTypeDRServer = ConfigDRServer; using SettingsAcquisitionConfigTypeDRServer = - ConfigDRServer; + ConfigDRServer; std::string config_node_name_; Zivid::CameraInfo camera_info_; std::unique_ptr general_config_dr_server_; - std::vector> acquisition_config_dr_servers_; + std::vector> + acquisition_config_dr_servers_; }; -extern template class CaptureSettingsController; -extern template class CaptureSettingsController; +extern template class CaptureSettingsController< + Zivid::Settings, zivid_camera::SettingsConfig, zivid_camera::SettingsAcquisitionConfig>; +extern template class CaptureSettingsController< + Zivid::Settings2D, zivid_camera::Settings2DConfig, zivid_camera::Settings2DAcquisitionConfig>; } // namespace zivid_camera diff --git a/zivid_camera/include/config_utils_common.h b/zivid_camera/include/config_utils_common.h index b7277888..1ed38d05 100644 --- a/zivid_camera/include/config_utils_common.h +++ b/zivid_camera/include/config_utils_common.h @@ -5,10 +5,10 @@ // during the build. template -ConfigType zividSettingsToConfig(const ZividSettings& s) = delete; +ConfigType zividSettingsToConfig(const ZividSettings & s) = delete; template -ConfigType zividSettingsMinConfig(const Zivid::Camera& camera) = delete; +ConfigType zividSettingsMinConfig(const Zivid::Camera & camera) = delete; template -ConfigType zividSettingsMaxConfig(const Zivid::Camera& camera) = delete; +ConfigType zividSettingsMaxConfig(const Zivid::Camera & camera) = delete; diff --git a/zivid_camera/include/zivid_camera.h b/zivid_camera/include/zivid_camera.h index df045590..4b22165c 100644 --- a/zivid_camera/include/zivid_camera.h +++ b/zivid_camera/include/zivid_camera.h @@ -1,20 +1,16 @@ #pragma once -#include "auto_generated_include_wrapper.h" -#include "capture_settings_controller.h" - -#include -#include - -#include - -#include - -#include - #include #include #include +#include +#include +#include +#include +#include + +#include "auto_generated_include_wrapper.h" +#include "capture_settings_controller.h" namespace Zivid { @@ -33,25 +29,29 @@ enum class CameraStatus class ZividCamera { public: - ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv); + ZividCamera(ros::NodeHandle & nh, ros::NodeHandle & priv); private: - void onCameraConnectionKeepAliveTimeout(const ros::TimerEvent& event); + void onCameraConnectionKeepAliveTimeout(const ros::TimerEvent & event); void reconnectToCameraIfNecessary(); void setCameraStatus(CameraStatus camera_status); - bool cameraInfoModelNameServiceHandler(CameraInfoModelName::Request& req, CameraInfoModelName::Response& res); - bool cameraInfoSerialNumberServiceHandler(CameraInfoSerialNumber::Request& req, - CameraInfoSerialNumber::Response& res); - bool captureServiceHandler(Capture::Request& req, Capture::Response& res); - bool captureAndSaveServiceHandler(CaptureAndSave::Request& req, CaptureAndSave::Response& res); - bool capture2DServiceHandler(Capture2D::Request& req, Capture2D::Response& res); - bool captureAssistantSuggestSettingsServiceHandler(CaptureAssistantSuggestSettings::Request& req, - CaptureAssistantSuggestSettings::Response& res); - bool loadSettingsFromFileServiceHandler(LoadSettingsFromFile::Request& req, LoadSettingsFromFile::Response&); - bool loadSettings2DFromFileServiceHandler(LoadSettings2DFromFile::Request& req, LoadSettings2DFromFile::Response&); + bool cameraInfoModelNameServiceHandler( + CameraInfoModelName::Request & req, CameraInfoModelName::Response & res); + bool cameraInfoSerialNumberServiceHandler( + CameraInfoSerialNumber::Request & req, CameraInfoSerialNumber::Response & res); + bool captureServiceHandler(Capture::Request & req, Capture::Response & res); + bool captureAndSaveServiceHandler(CaptureAndSave::Request & req, CaptureAndSave::Response & res); + bool capture2DServiceHandler(Capture2D::Request & req, Capture2D::Response & res); + bool captureAssistantSuggestSettingsServiceHandler( + CaptureAssistantSuggestSettings::Request & req, + CaptureAssistantSuggestSettings::Response & res); + bool loadSettingsFromFileServiceHandler( + LoadSettingsFromFile::Request & req, LoadSettingsFromFile::Response &); + bool loadSettings2DFromFileServiceHandler( + LoadSettings2DFromFile::Request & req, LoadSettings2DFromFile::Response &); void serviceHandlerHandleCameraConnectionLoss(); - bool isConnectedServiceHandler(IsConnected::Request& req, IsConnected::Response& res); - void publishFrame(const Zivid::Frame& frame); + bool isConnectedServiceHandler(IsConnected::Request & req, IsConnected::Response & res); + void publishFrame(const Zivid::Frame & frame); Zivid::Frame invokeCaptureAndPublishFrame(); bool shouldPublishPointsXYZ() const; bool shouldPublishPointsXYZRGBA() const; @@ -60,24 +60,30 @@ class ZividCamera bool shouldPublishSnrImg() const; bool shouldPublishNormalsXYZ() const; std_msgs::Header makeHeader(); - void publishPointCloudXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud); - void publishPointCloudXYZRGBA(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud); - void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::PointCloud& point_cloud); - void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::Image& image); - void publishDepthImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::PointCloud& point_cloud); - void publishSnrImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::PointCloud& point_cloud); - void publishNormalsXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud); - sensor_msgs::CameraInfoConstPtr makeCameraInfo(const std_msgs::Header& header, std::size_t width, std::size_t height, - const Zivid::CameraIntrinsics& intrinsics); + void publishPointCloudXYZ(const std_msgs::Header & header, const Zivid::PointCloud & point_cloud); + void publishPointCloudXYZRGBA( + const std_msgs::Header & header, const Zivid::PointCloud & point_cloud); + void publishColorImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::PointCloud & point_cloud); + void publishColorImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::Image & image); + void publishDepthImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::PointCloud & point_cloud); + void publishSnrImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::PointCloud & point_cloud); + void publishNormalsXYZ(const std_msgs::Header & header, const Zivid::PointCloud & point_cloud); + sensor_msgs::CameraInfoConstPtr makeCameraInfo( + const std_msgs::Header & header, std::size_t width, std::size_t height, + const Zivid::CameraIntrinsics & intrinsics); using Capture3DSettingsController = - CaptureSettingsController; + CaptureSettingsController; using Capture2DSettingsController = - CaptureSettingsController; + CaptureSettingsController; ros::NodeHandle nh_; ros::NodeHandle priv_; diff --git a/zivid_camera/src/capture_settings_controller.cpp b/zivid_camera/src/capture_settings_controller.cpp index 9f15da2d..e8fcd174 100644 --- a/zivid_camera/src/capture_settings_controller.cpp +++ b/zivid_camera/src/capture_settings_controller.cpp @@ -1,36 +1,35 @@ #include "capture_settings_controller.h" -#include "SettingsConfigUtils.h" -#include "SettingsAcquisitionConfigUtils.h" -#include "Settings2DConfigUtils.h" -#include "Settings2DAcquisitionConfigUtils.h" - #include #include - #include +#include "Settings2DAcquisitionConfigUtils.h" +#include "Settings2DConfigUtils.h" +#include "SettingsAcquisitionConfigUtils.h" +#include "SettingsConfigUtils.h" + namespace { template -void recursivelyFillInUnsetWithCameraDefault(Node& node, const Zivid::CameraInfo& cameraInfo) +void recursivelyFillInUnsetWithCameraDefault(Node & node, const Zivid::CameraInfo & cameraInfo) { - if constexpr (Node::nodeType == Zivid::DataModel::NodeType::group || - Node::nodeType == Zivid::DataModel::NodeType::leafDataModelList) - { - node.forEach([&cameraInfo](auto& child) { recursivelyFillInUnsetWithCameraDefault(child, cameraInfo); }); - } - else if (!node.hasValue()) - { + if constexpr ( + Node::nodeType == Zivid::DataModel::NodeType::group || + Node::nodeType == Zivid::DataModel::NodeType::leafDataModelList) { + node.forEach( + [&cameraInfo](auto & child) { recursivelyFillInUnsetWithCameraDefault(child, cameraInfo); }); + } else if (!node.hasValue()) { static_assert(Node::nodeType == Zivid::DataModel::NodeType::leafValue); node = Zivid::Experimental::SettingsInfo::defaultValue(cameraInfo); } } template -ZividSettingsType fillInUnsetWithCameraDefault(const ZividSettingsType& settings, const Zivid::CameraInfo& cameraInfo) +ZividSettingsType fillInUnsetWithCameraDefault( + const ZividSettingsType & settings, const Zivid::CameraInfo & cameraInfo) { - ZividSettingsType copy{ settings }; + ZividSettingsType copy{settings}; recursivelyFillInUnsetWithCameraDefault(copy, cameraInfo); return copy; } @@ -43,12 +42,16 @@ template class ConfigDRServer { public: - ConfigDRServer(const std::string& name, ros::NodeHandle& nh, const Zivid::Camera& camera) - : name_(name), dr_server_(dr_server_mutex_, ros::NodeHandle(nh, name_)), config_(ConfigType::__getDefault__()) + ConfigDRServer(const std::string & name, ros::NodeHandle & nh, const Zivid::Camera & camera) + : name_(name), + dr_server_(dr_server_mutex_, ros::NodeHandle(nh, name_)), + config_(ConfigType::__getDefault__()) { - static_assert(std::is_same_v || std::is_same_v || - std::is_same_v || - std::is_same_v); + static_assert( + std::is_same_v || + std::is_same_v || + std::is_same_v || + std::is_same_v); const auto config_min = zividSettingsMinConfig(camera); dr_server_.setConfigMin(config_min); @@ -57,11 +60,11 @@ class ConfigDRServer dr_server_.setConfigMax(config_max); const auto config_default = zividSettingsToConfig( - Zivid::Experimental::SettingsInfo::defaultValue(camera.info())); + Zivid::Experimental::SettingsInfo::defaultValue(camera.info())); dr_server_.setConfigDefault(config_default); setConfig(config_default); - auto cb = [this](const ConfigType& config, uint32_t /*level*/) { + auto cb = [this](const ConfigType & config, uint32_t /*level*/) { ROS_INFO("Configuration '%s' changed", name_.c_str()); config_ = config; }; @@ -69,21 +72,15 @@ class ConfigDRServer dr_server_.setCallback(CallbackType(cb)); } - void setConfig(const ConfigType& cfg) + void setConfig(const ConfigType & cfg) { config_ = cfg; dr_server_.updateConfig(config_); } - const ConfigType& config() const - { - return config_; - } + const ConfigType & config() const { return config_; } - const std::string& name() const - { - return name_; - } + const std::string & name() const { return name_; } private: std::string name_; @@ -92,40 +89,42 @@ class ConfigDRServer ConfigType config_; }; -template -CaptureSettingsController::CaptureSettingsController(ros::NodeHandle& nh, - Zivid::Camera& camera, - const std::string& config_node_name, - std::size_t num_acquisition_servers) - : config_node_name_{ config_node_name }, camera_info_{ camera.info() } +template < + typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> +CaptureSettingsController:: + CaptureSettingsController( + ros::NodeHandle & nh, Zivid::Camera & camera, const std::string & config_node_name, + std::size_t num_acquisition_servers) +: config_node_name_{config_node_name}, camera_info_{camera.info()} { - general_config_dr_server_ = std::make_unique(config_node_name_, nh, camera); - - ROS_INFO("Setting up %ld %s/acquisition_ dynamic_reconfigure servers", num_acquisition_servers, - config_node_name_.c_str()); - for (std::size_t i = 0; i < num_acquisition_servers; i++) - { - acquisition_config_dr_servers_.push_back(std::make_unique( + general_config_dr_server_ = + std::make_unique(config_node_name_, nh, camera); + + ROS_INFO( + "Setting up %ld %s/acquisition_ dynamic_reconfigure servers", num_acquisition_servers, + config_node_name_.c_str()); + for (std::size_t i = 0; i < num_acquisition_servers; i++) { + acquisition_config_dr_servers_.push_back( + std::make_unique( config_node_name_ + "/acquisition_" + std::to_string(i), nh, camera)); } } -template -CaptureSettingsController::~CaptureSettingsController() = default; +template < + typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> +CaptureSettingsController:: + ~CaptureSettingsController() = default; -template -ZividSettingsType -CaptureSettingsController::zividSettings() const +template < + typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> +ZividSettingsType CaptureSettingsController< + ZividSettingsType, SettingsConfigType, SettingsAcquisitionConfigType>::zividSettings() const { ZividSettingsType settings; applyConfigToZividSettings(general_config_dr_server_->config(), settings); - for (const auto& dr_config_server : acquisition_config_dr_servers_) - { - if (dr_config_server->config().enabled) - { + for (const auto & dr_config_server : acquisition_config_dr_servers_) { + if (dr_config_server->config().enabled) { ROS_DEBUG("Config %s is enabled", dr_config_server->name().c_str()); typename ZividSettingsType::Acquisition acquisition; applyConfigToZividSettings(dr_config_server->config(), acquisition); @@ -135,29 +134,27 @@ CaptureSettingsController -void CaptureSettingsController::setZividSettings( - const ZividSettingsType& settings) +template < + typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> +void CaptureSettingsController< + ZividSettingsType, SettingsConfigType, + SettingsAcquisitionConfigType>::setZividSettings(const ZividSettingsType & settings) { const auto numAcquisitions = settings.acquisitions().size(); - if (numAcquisitions == 0) - { + if (numAcquisitions == 0) { throw std::runtime_error("At least one Acquisition must be provided"); } - if (numAcquisitions > numAcquisitionConfigServers()) - { + if (numAcquisitions > numAcquisitionConfigServers()) { std::stringstream error; error << "The number of acquisitions (" + std::to_string(numAcquisitions) + ") " - << "is larger than the number of dynamic_reconfigure " + config_node_name_ + "/acquisition_ servers (" + << "is larger than the number of dynamic_reconfigure " + config_node_name_ + + "/acquisition_ servers (" << std::to_string(numAcquisitionConfigServers()) << "). "; - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { error << "Increase launch parameter max_capture_acquisitions. " << "See README.md for more information."; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { error << "In 2D mode only a single acquisiton is supported."; } throw std::runtime_error(error.str()); @@ -167,19 +164,16 @@ void CaptureSettingsControllersetConfig(zividSettingsToConfig(filledSettings)); - const auto& acquisitions = filledSettings.acquisitions(); - for (std::size_t i = 0; i < acquisitions.size(); i++) - { + const auto & acquisitions = filledSettings.acquisitions(); + for (std::size_t i = 0; i < acquisitions.size(); i++) { auto config = zividSettingsToConfig(acquisitions.at(i)); config.enabled = true; acquisition_config_dr_servers_[i]->setConfig(config); } // Remaining acquisitions that are enabled must be disabled - for (std::size_t i = acquisitions.size(); i < acquisition_config_dr_servers_.size(); i++) - { - if (acquisition_config_dr_servers_[i]->config().enabled) - { + for (std::size_t i = acquisitions.size(); i < acquisition_config_dr_servers_.size(); i++) { + if (acquisition_config_dr_servers_[i]->config().enabled) { ROS_INFO_STREAM("Acquisition " << i << " was enabled, so disabling it"); auto config = acquisition_config_dr_servers_[i]->config(); config.enabled = false; @@ -188,16 +182,18 @@ void CaptureSettingsController -std::size_t CaptureSettingsController::numAcquisitionConfigServers() const +template < + typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> +std::size_t CaptureSettingsController< + ZividSettingsType, SettingsConfigType, + SettingsAcquisitionConfigType>::numAcquisitionConfigServers() const { return acquisition_config_dr_servers_.size(); } -template class CaptureSettingsController; -template class CaptureSettingsController; +template class CaptureSettingsController< + Zivid::Settings, zivid_camera::SettingsConfig, zivid_camera::SettingsAcquisitionConfig>; +template class CaptureSettingsController< + Zivid::Settings2D, zivid_camera::Settings2DConfig, zivid_camera::Settings2DAcquisitionConfig>; } // namespace zivid_camera diff --git a/zivid_camera/src/node.cpp b/zivid_camera/src/node.cpp index de736280..7eb16935 100644 --- a/zivid_camera/src/node.cpp +++ b/zivid_camera/src/node.cpp @@ -1,26 +1,21 @@ #include #include + #include -int main(int argc, char** argv) +int main(int argc, char ** argv) { - try - { + try { ros::init(argc, argv, "zivid_camera"); - } - catch (const std::exception& e) - { + } catch (const std::exception & e) { std::cerr << "Failed to initialize ROS: " << e.what() << std::endl; return EXIT_FAILURE; - } - catch (...) - { + } catch (...) { std::cerr << "Failed to initialize ROS (unknown exception)" << std::endl; return EXIT_FAILURE; } - try - { + try { ROS_INFO("Creating a nodelet::Loader"); nodelet::Loader nodelet; nodelet::M_string remap(ros::names::getRemappings()); @@ -30,8 +25,7 @@ int main(int argc, char** argv) ROS_INFO("Loading nodelet '%s'", nodelet_name); const bool loaded = nodelet.load(ros::this_node::getName(), nodelet_name, remap, nargv); - if (!loaded) - { + if (!loaded) { ROS_FATAL("Failed to load nodelet '%s'!", nodelet_name); return EXIT_FAILURE; } @@ -39,14 +33,10 @@ int main(int argc, char** argv) ROS_INFO("Successfully loaded nodelet '%s'", nodelet_name); ros::spin(); return EXIT_SUCCESS; - } - catch (const std::exception& e) - { + } catch (const std::exception & e) { std::cerr << "Exception occurred: " << e.what() << std::endl; return EXIT_FAILURE; - } - catch (...) - { + } catch (...) { std::cerr << "Unknown exception occurred" << std::endl; return EXIT_FAILURE; } diff --git a/zivid_camera/src/nodelet.cpp b/zivid_camera/src/nodelet.cpp index 9ad49515..90f27f3f 100644 --- a/zivid_camera/src/nodelet.cpp +++ b/zivid_camera/src/nodelet.cpp @@ -1,28 +1,24 @@ #include "nodelet.h" -#include "zivid_camera.h" #include #include -#include #include +#include + +#include "zivid_camera.h" namespace zivid_camera { void ZividNodelet::onInit() { - try - { + try { // Important: use non-multi-threaded callback queues (getNodeHandle and getPrivateNodeHandle). camera = std::make_unique(getNodeHandle(), getPrivateNodeHandle()); - } - catch (const std::exception& e) - { + } catch (const std::exception & e) { NODELET_ERROR_STREAM("Failed to initialize camera driver. Exception: \"" << e.what() << "\""); throw; - } - catch (...) - { + } catch (...) { NODELET_ERROR_STREAM("Failed to initialize camera driver (unknown exception)"); throw; } diff --git a/zivid_camera/src/settings_generator.cpp b/zivid_camera/src/settings_generator.cpp index 330dcd5b..01e79065 100644 --- a/zivid_camera/src/settings_generator.cpp +++ b/zivid_camera/src/settings_generator.cpp @@ -6,16 +6,14 @@ #include #include #include - #include +#include #include -#include #include +#include #include #include - -#include #include #include #include @@ -38,7 +36,7 @@ struct IsInList : std::false_type template struct IsInList - : std::integral_constant::value || IsInList::value> +: std::integral_constant::value || IsInList::value> { }; @@ -55,19 +53,16 @@ struct IsEnumSetting : std::is_enum { }; -void writeToFile(const std::string& file_name, const std::string& text) +void writeToFile(const std::string & file_name, const std::string & text) { - if (boost::filesystem::exists(file_name)) - { + if (boost::filesystem::exists(file_name)) { std::ifstream file(file_name); - if (!file.is_open()) - { + if (!file.is_open()) { throw std::runtime_error("Unable to open file '" + file_name + "' to check its contents!"); } auto file_contents_ss = std::ostringstream{}; file_contents_ss << file.rdbuf(); - if (file_contents_ss.str() == text) - { + if (file_contents_ss.str() == text) { // If the file already exists, and content is identical to "text" then do not modify the file. // This ensures that we avoid unnecessary rebuilds. return; @@ -75,22 +70,19 @@ void writeToFile(const std::string& file_name, const std::string& text) } std::ofstream cfg_file(file_name); - if (!cfg_file || !cfg_file.is_open()) - { + if (!cfg_file || !cfg_file.is_open()) { throw std::runtime_error("Unable to open file '" + file_name + "' for writing!"); } cfg_file << text; cfg_file.close(); - if (!cfg_file) - { + if (!cfg_file) { throw std::runtime_error("Failed to write to file '" + file_name + "'!"); } } std::string toUpperCaseFirst(std::string value) { - if (value.empty()) - { + if (value.empty()) { throw std::invalid_argument("value is empty"); } value[0] = std::toupper(value[0]); @@ -103,12 +95,11 @@ std::string convertSettingsPathToConfigPath() auto path = std::string(SettingsNode::path); const auto root_path = std::string(SettingsRootGroup::path); - if (!root_path.empty()) - { + if (!root_path.empty()) { const auto expected_prefix = root_path + "/"; - if (path.substr(0, expected_prefix.length()) != expected_prefix) - { - throw std::runtime_error("Expected path '" + path + "' to begin with '" + expected_prefix + "'"); + if (path.substr(0, expected_prefix.length()) != expected_prefix) { + throw std::runtime_error( + "Expected path '" + path + "' to begin with '" + expected_prefix + "'"); } path = path.substr(expected_prefix.length()); } @@ -122,22 +113,19 @@ std::string convertSettingsPathToConfigPath() template std::string zividSettingsTypeName() { - if constexpr (std::is_same_v || - IsInTuple::value || - std::is_same_v || - IsInTuple::value) - { + if constexpr ( + std::is_same_v || + IsInTuple::value || + std::is_same_v || + IsInTuple::value) { return Zivid::Settings::name; - } - else if constexpr (std::is_same_v || - IsInTuple::value || - std::is_same_v || - IsInTuple::value) - { + } else if constexpr ( + std::is_same_v || + IsInTuple::value || + std::is_same_v || + IsInTuple::value) { return Zivid::Settings2D::name; - } - else - { + } else { static_assert(DependentFalse::value, "Unhandled type."); } } @@ -148,8 +136,7 @@ std::string fullyQualifiedZividTypeName() std::stringstream ss; ss << "Zivid::" + zividSettingsTypeName(); const auto path = std::string(SettingsNode::path); - if (!path.empty()) - { + if (!path.empty()) { ss << "::" << boost::replace_all_copy(path, "/", "::"); } return ss.str(); @@ -164,7 +151,7 @@ std::string settingEnumValueToRosEnumName(typename SettingsNode::ValueType value // zivid_camera::Settings_X. Since we may have several different enum settings with the // same enum value X, we need to scope the enum name with the full path to the setting. // This ensures there are no collisions now or in the future. - const auto value_str = toUpperCaseFirst(SettingsNode{ value }.toString()); + const auto value_str = toUpperCaseFirst(SettingsNode{value}.toString()); const auto path = boost::replace_all_copy(SettingsNode::path, "/", ""); return path + value_str; } @@ -175,15 +162,17 @@ std::string generateEnumConstant(typename SettingsNode::ValueType value) static_assert(IsEnumSetting::value); const auto name = settingEnumValueToRosEnumName(value); const auto int_value = static_cast(value); - const auto description = toUpperCaseFirst(SettingsNode{ value }.toString()); - return (boost::format(R"(gen.const("%1%", int_t, %2%, "%3%"))") % name % int_value % description).str(); + const auto description = toUpperCaseFirst(SettingsNode{value}.toString()); + return (boost::format(R"(gen.const("%1%", int_t, %2%, "%3%"))") % name % int_value % description) + .str(); } template class DynamicReconfigureCfgGenerator { public: - DynamicReconfigureCfgGenerator(const std::string& class_name) : class_name_(class_name), insert_enabled_(false) + DynamicReconfigureCfgGenerator(const std::string & class_name) + : class_name_(class_name), insert_enabled_(false) { } @@ -194,62 +183,39 @@ class DynamicReconfigureCfgGenerator // correct static hard-coded default value for the setting here. To correctly use the // Zivid driver the user must load the default values via dynamic_reconfigure at runtime. using ValueType = typename SettingsNode::ValueType; - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { return false; - } - else if constexpr (Zivid::DataModel::HasValidRange::value) - { + } else if constexpr (Zivid::DataModel::HasValidRange::value) { return SettingsNode::validRange().min(); - } - else if constexpr (IsEnumSetting::value) - { + } else if constexpr (IsEnumSetting::value) { return *SettingsNode::validValues().begin(); - } - else if constexpr (std::is_same_v>) - { - return ValueType{ 0.0, 0.0 }; - } - else if constexpr (std::is_same::value) - { - return ValueType{ 0.0f, 0.0f, 0.0f }; - } - else - { - return ValueType{ 0 }; + } else if constexpr (std::is_same_v>) { + return ValueType{0.0, 0.0}; + } else if constexpr (std::is_same::value) { + return ValueType{0.0f, 0.0f, 0.0f}; + } else { + return ValueType{0}; } } template auto convertValueToRosValue(ValueType value) { - if constexpr (std::is_same_v || std::is_same_v || - std::is_same_v) - { + if constexpr ( + std::is_same_v || std::is_same_v || + std::is_same_v) { return value; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { return static_cast(value); - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { return static_cast(value.count()); - } - else if constexpr (std::is_enum_v) - { + } else if constexpr (std::is_enum_v) { return static_cast(value); - } - else if constexpr (std::is_same_v>) - { + } else if constexpr (std::is_same_v>) { return value; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { return value; - } - else - { + } else { static_assert(DependentFalse::value, "Could not convert ValueType to ROS type."); } } @@ -257,45 +223,32 @@ class DynamicReconfigureCfgGenerator template std::string rosTypeName() { - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { return "bool_t"; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { return "double_t"; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { return "int_t"; - } - else if constexpr (std::is_same_v> || std::is_same_v) - { + } else if constexpr ( + std::is_same_v> || std::is_same_v) { return "double_t"; - } - else - { - static_assert(DependentFalse::value, "Could not convert RosType to a ROS typename string."); + } else { + static_assert( + DependentFalse::value, "Could not convert RosType to a ROS typename string."); } } template std::string rosTypeToString(RosType v) { - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { return v ? "True" : "False"; - } - else if constexpr (std::is_same_v || std::is_same_v) - { + } else if constexpr (std::is_same_v || std::is_same_v) { return std::to_string(v); - } - else if constexpr (std::is_same_v> || std::is_same_v) - { + } else if constexpr ( + std::is_same_v> || std::is_same_v) { return "0.0"; - } - else - { + } else { static_assert(DependentFalse::value, "Could not convert RosType to a string value."); } } @@ -306,13 +259,13 @@ class DynamicReconfigureCfgGenerator return rosTypeToString(convertValueToRosValue(v)); } - std::string rosGeneratedEnumVariableName(const std::string& setting_name) + std::string rosGeneratedEnumVariableName(const std::string & setting_name) { return setting_name + "_enum"; } template - void apply(const SettingsNode& node) + void apply(const SettingsNode & node) { const auto setting_name = convertSettingsPathToConfigPath(); const auto level = "0"; @@ -322,60 +275,50 @@ class DynamicReconfigureCfgGenerator const auto type_name = rosTypeName(); const auto default_value_str = valueTypeToRosTypeString(default_value); - if constexpr (IsEnumSetting::value) - { + if constexpr (IsEnumSetting::value) { const auto valid_values = node.validValues(); std::vector enum_constants; - std::transform(valid_values.cbegin(), valid_values.cend(), std::back_inserter(enum_constants), - [&](const auto value) { return generateEnumConstant(value); }); + std::transform( + valid_values.cbegin(), valid_values.cend(), std::back_inserter(enum_constants), + [&](const auto value) { return generateEnumConstant(value); }); ss_ << rosGeneratedEnumVariableName(setting_name) << " = gen.enum([\n " - << boost::algorithm::join(enum_constants, ",\n ") << "\n],\n \"" << description << "\")\n"; + << boost::algorithm::join(enum_constants, ",\n ") << "\n],\n \"" << description + << "\")\n"; } - if constexpr (std::is_same_v, Zivid::PointXYZ>) - { - for (const auto comp : std::array{ "x", "y", "z" }) - { + if constexpr (std::is_same_v, Zivid::PointXYZ>) { + for (const auto comp : std::array{"x", "y", "z"}) { const auto comp_name = setting_name + "_" + comp; ss_ << "gen.add(\"" << comp_name << "\", " << type_name << ", " << level << ", " << "\"" << description + " [" + comp + "]" << "\", " << default_value_str << ")\n"; } - } - else if constexpr (std::is_same_v, Zivid::Range>) - { - for (const auto comp : std::array{ "min", "max" }) - { + } else if constexpr (std::is_same_v< + std::decay_t, Zivid::Range>) { + for (const auto comp : std::array{"min", "max"}) { const auto comp_name = setting_name + "_" + comp; ss_ << "gen.add(\"" << comp_name << "\", " << type_name << ", " << level << ", " << "\"" << description + " [" + comp + "]" << "\", " << default_value_str << ")\n"; } - } - else - { + } else { ss_ << "gen.add(\"" << setting_name << "\", " << type_name << ", " << level << ", " << "\"" << description << "\", " << default_value_str; - if constexpr (Zivid::DataModel::HasValidRange::value) - { + if constexpr (Zivid::DataModel::HasValidRange::value) { ss_ << ", " << valueTypeToRosTypeString(node.validRange().min()) << ", " << valueTypeToRosTypeString(node.validRange().max()); - } - else if constexpr (IsEnumSetting::value) - { + } else if constexpr (IsEnumSetting::value) { const auto min_index = 0; const auto max_index = node.validValues().size() - 1; - ss_ << ", " << min_index << ", " << max_index << ", edit_method=" << rosGeneratedEnumVariableName(setting_name); + ss_ << ", " << min_index << ", " << max_index + << ", edit_method=" << rosGeneratedEnumVariableName(setting_name); } ss_ << ")\n"; } } - void insertEnabled() - { - insert_enabled_ = true; - } + void insertEnabled() { insert_enabled_ = true; } std::string str() { @@ -388,9 +331,9 @@ class DynamicReconfigureCfgGenerator "from dynamic_reconfigure.parameter_generator_catkin import *\n\n"; res << "gen = ParameterGenerator()\n"; - if (insert_enabled_) - { - res << "gen.add(\"enabled\", bool_t, 0, \"When this acquisition is enabled it will be included in captures\", " + if (insert_enabled_) { + res << "gen.add(\"enabled\", bool_t, 0, \"When this acquisition is enabled it will be " + "included in captures\", " "False)\n"; } res << ss_.str(); @@ -408,12 +351,13 @@ template class ApplyConfigToZividSettingsGenerator { public: - ApplyConfigToZividSettingsGenerator(const std::string& config_class_name) : config_class_name_(config_class_name) + ApplyConfigToZividSettingsGenerator(const std::string & config_class_name) + : config_class_name_(config_class_name) { } template - void apply(const SettingsNode& node) + void apply(const SettingsNode & node) { using ValueType = typename SettingsNode::ValueType; @@ -421,42 +365,32 @@ class ApplyConfigToZividSettingsGenerator const auto setting_node_class_name = fullyQualifiedZividTypeName(); ss_ << " s.set(" + setting_node_class_name + "{ "; - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { ss_ << "static_cast(" + cfg_id + ")"; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { ss_ << "std::chrono::microseconds(" + cfg_id + ")"; - } - else if constexpr (IsEnumSetting::value) - { + } else if constexpr (IsEnumSetting::value) { ss_ << "\n" << " [value = " + cfg_id + "](){\n" << " switch(value) {\n"; const auto valid_values = node.validValues(); - for (const auto& enum_value : valid_values) - { + for (const auto & enum_value : valid_values) { ss_ << " case zivid_camera::" << config_class_name_ << "_" << settingEnumValueToRosEnumName(enum_value) << ":\n" - << " return " + setting_node_class_name + "::" + SettingsNode{ enum_value }.toString() + ";\n"; + << " return " + setting_node_class_name + + "::" + SettingsNode{enum_value}.toString() + ";\n"; } ss_ << " }\n" - << " throw std::runtime_error(\"Could not convert int value \" + std::to_string(value) + \"" + << " throw std::runtime_error(\"Could not convert int value \" + " + "std::to_string(value) + \"" << " to setting of type " + setting_node_class_name + ".\");\n" << " }()\n"; - } - else if constexpr (std::is_same_v) - { - ss_ << "static_cast(" << cfg_id << "_x), static_cast(" << cfg_id << "_y), static_cast(" - << cfg_id << "_z)"; - } - else if constexpr (std::is_same_v>) - { + } else if constexpr (std::is_same_v) { + ss_ << "static_cast(" << cfg_id << "_x), static_cast(" << cfg_id + << "_y), static_cast(" << cfg_id << "_z)"; + } else if constexpr (std::is_same_v>) { ss_ << cfg_id << "_min, " << cfg_id << "_max "; - } - else - { + } else { ss_ << cfg_id; } ss_ << " });\n"; @@ -467,8 +401,8 @@ class ApplyConfigToZividSettingsGenerator const auto root_type_fq = fullyQualifiedZividTypeName(); std::stringstream res; - res << "inline static void applyConfigToZividSettings(const zivid_camera::" << config_class_name_ << "Config& cfg, " - << root_type_fq << "& s)\n"; + res << "inline static void applyConfigToZividSettings(const zivid_camera::" + << config_class_name_ << "Config& cfg, " << root_type_fq << "& s)\n"; res << "{\n"; res << ss_.str(); res << "}\n"; @@ -484,50 +418,37 @@ template class ZividSettingsToConfigGenerator { public: - ZividSettingsToConfigGenerator(const std::string& config_class_name) : config_class_name_(config_class_name) + ZividSettingsToConfigGenerator(const std::string & config_class_name) + : config_class_name_(config_class_name) { } template - void apply(const SettingsNode&) + void apply(const SettingsNode &) { using ValueType = typename SettingsNode::ValueType; const auto cfg_id = "cfg." + convertSettingsPathToConfigPath(); const auto zivid_node_class_name = fullyQualifiedZividTypeName(); const auto value_str = "s.get<" + zivid_node_class_name + ">().value()"; - if constexpr (std::is_same_v) - { - for (const auto comp : std::array{ "x", "y", "z" }) - { + if constexpr (std::is_same_v) { + for (const auto comp : std::array{"x", "y", "z"}) { ss_ << " " + cfg_id + "_" + comp + " = " << "static_cast(" + value_str + "." + comp + ");\n"; } - } - else if constexpr (std::is_same_v>) - { - for (const auto comp : std::array{ "min", "max" }) - { + } else if constexpr (std::is_same_v>) { + for (const auto comp : std::array{"min", "max"}) { ss_ << " " + cfg_id + "_" + comp + " = " << value_str + "." + comp + "();\n"; } - } - else - { + } else { ss_ << " " + cfg_id + " = "; - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { ss_ << "static_cast(" + value_str + ".count());\n"; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { ss_ << "static_cast(" + value_str + ");\n"; - } - else if constexpr (IsEnumSetting::value) - { + } else if constexpr (IsEnumSetting::value) { ss_ << "static_cast(" + value_str + ");\n"; - } - else - { + } else { ss_ << value_str + ";\n"; } } @@ -564,25 +485,24 @@ class MinMaxConfigGenerator Max, }; - MinMaxConfigGenerator(const std::string& config_class_name, Type type) - : config_class_name_(config_class_name), type_(type) + MinMaxConfigGenerator(const std::string & config_class_name, Type type) + : config_class_name_(config_class_name), type_(type) { } template - void apply(const SettingsNode&) + void apply(const SettingsNode &) { - if constexpr (Zivid::DataModel::HasValidRange::value) - { + if constexpr (Zivid::DataModel::HasValidRange::value) { using ValueType = typename SettingsNode::ValueType; - const auto cfg_id = "cfg." + convertSettingsPathToConfigPath(); + const auto cfg_id = + "cfg." + convertSettingsPathToConfigPath(); const auto zivid_node_class_name = fullyQualifiedZividTypeName(); const auto value_str = [&]() { - std::string prefix = - "Zivid::Experimental::SettingsInfo::validRange<" + zivid_node_class_name + ">(camera.info())"; - switch (type_) - { + std::string prefix = "Zivid::Experimental::SettingsInfo::validRange<" + + zivid_node_class_name + ">(camera.info())"; + switch (type_) { case Type::Min: return prefix + ".min()"; case Type::Max: @@ -593,16 +513,11 @@ class MinMaxConfigGenerator ss_ << " " + cfg_id + " = "; - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { ss_ << "static_cast(" + value_str + ".count());\n"; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { ss_ << "static_cast(" + value_str + ");\n"; - } - else - { + } else { ss_ << value_str + ";\n"; } } @@ -610,8 +525,7 @@ class MinMaxConfigGenerator std::string initializeConfigFunction() const { - switch (type_) - { + switch (type_) { case Type::Min: return "__getMin__"; case Type::Max: @@ -625,8 +539,7 @@ class MinMaxConfigGenerator const auto full_class_name = "zivid_camera::" + config_class_name_ + "Config"; const auto function_name_config_type = [&]() { - switch (type_) - { + switch (type_) { case Type::Min: return "Min"; case Type::Max: @@ -638,7 +551,8 @@ class MinMaxConfigGenerator const auto root_type_fq = fullyQualifiedZividTypeName(); std::stringstream res; - res << "template<> inline " << full_class_name << " zividSettings" << function_name_config_type << "Config"; + res << "template<> inline " << full_class_name << " zividSettings" << function_name_config_type + << "Config"; res << "<" << full_class_name << ", " << root_type_fq << ">(const Zivid::Camera& camera)\n"; res << "{\n"; res << " auto cfg = " + full_class_name << "::" << initializeConfigFunction() << "();\n"; @@ -663,16 +577,16 @@ template class ConfigUtilsHeaderGenerator { public: - ConfigUtilsHeaderGenerator(const std::string& config_class_name) - : apply_config_zivid_settings_gen(config_class_name) - , zivid_settings_to_config_gen(config_class_name) - , min_config_gen(config_class_name, MinMaxConfigGenerator::Type::Min) - , max_config_gen(config_class_name, MinMaxConfigGenerator::Type::Max) + ConfigUtilsHeaderGenerator(const std::string & config_class_name) + : apply_config_zivid_settings_gen(config_class_name), + zivid_settings_to_config_gen(config_class_name), + min_config_gen(config_class_name, MinMaxConfigGenerator::Type::Min), + max_config_gen(config_class_name, MinMaxConfigGenerator::Type::Max) { } template - void apply(const SettingsNode& node) + void apply(const SettingsNode & node) { apply_config_zivid_settings_gen.apply(node); zivid_settings_to_config_gen.apply(node); @@ -706,29 +620,27 @@ template class Generator { public: - Generator(const std::string& config_class_name) - : config_class_name_(config_class_name) - , dynamic_reconfigure_cfg_gen_(config_class_name) - , config_utils_header_gen_(config_class_name) + Generator(const std::string & config_class_name) + : config_class_name_(config_class_name), + dynamic_reconfigure_cfg_gen_(config_class_name), + config_utils_header_gen_(config_class_name) { } template - void apply(const SettingsNode& node) + void apply(const SettingsNode & node) { dynamic_reconfigure_cfg_gen_.apply(node); config_utils_header_gen_.apply(node); } - void insertEnabled() - { - dynamic_reconfigure_cfg_gen_.insertEnabled(); - } + void insertEnabled() { dynamic_reconfigure_cfg_gen_.insertEnabled(); } void writeToFiles() { writeToFile(config_class_name_ + ".cfg", dynamic_reconfigure_cfg_gen_.str()); - writeToFile("generated_headers/" + config_class_name_ + "ConfigUtils.h", config_utils_header_gen_.str()); + writeToFile( + "generated_headers/" + config_class_name_ + "ConfigUtils.h", config_utils_header_gen_.str()); } private: @@ -738,24 +650,19 @@ class Generator }; template -void traverseSettingsTree(const SettingsNode& node, GeneratorType& generator) +void traverseSettingsTree(const SettingsNode & node, GeneratorType & generator) { - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { // Acquisitions ignored here. Acqusitition is handled separately. - } - else if constexpr (SettingsNode::nodeType == Zivid::DataModel::NodeType::group) - { - node.forEach([&](const auto& child) { traverseSettingsTree(child, generator); }); - } - else - { + } else if constexpr (SettingsNode::nodeType == Zivid::DataModel::NodeType::group) { + node.forEach([&](const auto & child) { traverseSettingsTree(child, generator); }); + } else { generator.apply(node); } } template -void addSettingsType(const std::string& cfgPrefix) +void addSettingsType(const std::string & cfgPrefix) { Generator capture_general_gen(cfgPrefix); traverseSettingsTree(SettingsType{}, capture_general_gen); @@ -769,7 +676,7 @@ void addSettingsType(const std::string& cfgPrefix) } // namespace -int main(int /*argc*/, char** /*argv*/) +int main(int /*argc*/, char ** /*argv*/) { addSettingsType("Settings"); addSettingsType("Settings2D"); diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 21002513..1c61296a 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -1,27 +1,26 @@ #include "zivid_camera.h" -#include -#include -#include -#include - +#include +#include #include #include #include #include -#include -#include - -#include #include +#include +#include +#include +#include +#include +#include #include #include -#include namespace { -sensor_msgs::PointField createPointField(std::string name, uint32_t offset, uint8_t datatype, uint32_t count) +sensor_msgs::PointField createPointField( + std::string name, uint32_t offset, uint8_t datatype, uint32_t count) { sensor_msgs::PointField point_field; point_field.name = name; @@ -31,13 +30,11 @@ sensor_msgs::PointField createPointField(std::string name, uint32_t offset, uint return point_field; } -bool big_endian() -{ - return BOOST_ENDIAN_BIG_BYTE; -} +bool big_endian() { return BOOST_ENDIAN_BIG_BYTE; } template -void fillCommonMsgFields(T& msg, const std_msgs::Header& header, std::size_t width, std::size_t height) +void fillCommonMsgFields( + T & msg, const std_msgs::Header & header, std::size_t width, std::size_t height) { msg.header = header; msg.height = static_cast(height); @@ -45,32 +42,34 @@ void fillCommonMsgFields(T& msg, const std_msgs::Header& header, std::size_t wid msg.is_bigendian = big_endian(); } -sensor_msgs::ImagePtr makeImage(const std_msgs::Header& header, const std::string& encoding, std::size_t width, - std::size_t height) +sensor_msgs::ImagePtr makeImage( + const std_msgs::Header & header, const std::string & encoding, std::size_t width, + std::size_t height) { auto image = boost::make_shared(); fillCommonMsgFields(*image, header, width, height); image->encoding = encoding; - const auto bytes_per_pixel = static_cast(sensor_msgs::image_encodings::numChannels(encoding) * - sensor_msgs::image_encodings::bitDepth(encoding) / 8); + const auto bytes_per_pixel = static_cast( + sensor_msgs::image_encodings::numChannels(encoding) * + sensor_msgs::image_encodings::bitDepth(encoding) / 8); image->step = static_cast(bytes_per_pixel * width); return image; } template -sensor_msgs::ImagePtr makePointCloudImage(const Zivid::PointCloud& point_cloud, const std_msgs::Header& header, - const std::string& encoding) +sensor_msgs::ImagePtr makePointCloudImage( + const Zivid::PointCloud & point_cloud, const std_msgs::Header & header, + const std::string & encoding) { auto image = makeImage(header, encoding, point_cloud.width(), point_cloud.height()); image->data.resize(image->step * image->height); - point_cloud.copyData(reinterpret_cast(image->data.data())); + point_cloud.copyData(reinterpret_cast(image->data.data())); return image; } std::string toString(zivid_camera::CameraStatus camera_status) { - switch (camera_status) - { + switch (camera_status) { case zivid_camera::CameraStatus::Connected: return "Connected"; case zivid_camera::CameraStatus::Disconnected: @@ -93,45 +92,46 @@ Zivid::Application makeZividApplication() namespace zivid_camera { -ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv) - : nh_(nh) - , priv_(priv) - , camera_status_(CameraStatus::Idle) - , use_latched_publisher_for_points_xyz_(false) - , use_latched_publisher_for_points_xyzrgba_(false) - , use_latched_publisher_for_color_image_(false) - , use_latched_publisher_for_depth_image_(false) - , use_latched_publisher_for_snr_image_(false) - , use_latched_publisher_for_normals_xyz_(false) - , image_transport_(nh_) - , zivid_(makeZividApplication()) - , header_seq_(0) +ZividCamera::ZividCamera(ros::NodeHandle & nh, ros::NodeHandle & priv) +: nh_(nh), + priv_(priv), + camera_status_(CameraStatus::Idle), + use_latched_publisher_for_points_xyz_(false), + use_latched_publisher_for_points_xyzrgba_(false), + use_latched_publisher_for_color_image_(false), + use_latched_publisher_for_depth_image_(false), + use_latched_publisher_for_snr_image_(false), + use_latched_publisher_for_normals_xyz_(false), + image_transport_(nh_), + zivid_(makeZividApplication()), + header_seq_(0) { ROS_INFO("Zivid ROS driver version %s", ZIVID_ROS_DRIVER_VERSION); ROS_INFO("Node's namespace is '%s'", nh_.getNamespace().c_str()); - if (nh_.getNamespace() == "") - { + if (nh_.getNamespace() == "/") { // Require the user to specify the namespace that this node will run in. // See REP-135 http://www.ros.org/reps/rep-0135.html - throw std::runtime_error("Zivid ROS driver started in the global namespace ('/')! This is unsupported. " - "Please specify namespace, e.g. using the ROS_NAMESPACE environment variable."); + throw std::runtime_error( + "Zivid ROS driver started in the global namespace ('/')! This is unsupported. " + "Please specify namespace, e.g. using the ROS_NAMESPACE environment variable."); } ROS_INFO_STREAM("Built towards Zivid Core version " << ZIVID_CORE_VERSION); ROS_INFO_STREAM("Running with Zivid Core version " << Zivid::Version::coreLibraryVersion()); - if (Zivid::Version::coreLibraryVersion() != ZIVID_CORE_VERSION) - { - throw std::runtime_error("Zivid library mismatch! The running Zivid Core version does not match the " - "version this ROS driver was built towards. Hint: Try to clean and re-build your project " - "from scratch."); + if (Zivid::Version::coreLibraryVersion() != ZIVID_CORE_VERSION) { + throw std::runtime_error( + "Zivid library mismatch! The running Zivid Core version does not match the " + "version this ROS driver was built towards. Hint: Try to clean and re-build your project " + "from scratch."); } std::string serial_number; priv_.param("serial_number", serial_number, ""); int max_capture_acquisitions; - priv_.param("max_capture_acquisitions", max_capture_acquisitions, 10); + priv_.param( + "max_capture_acquisitions", max_capture_acquisitions, 10); priv_.param("frame_id", frame_id_, "zivid_optical_frame"); @@ -139,81 +139,76 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv) priv_.param("file_camera_path", file_camera_path, ""); const bool file_camera_mode = !file_camera_path.empty(); - priv_.param("use_latched_publisher_for_points_xyz", use_latched_publisher_for_points_xyz_, false); - priv_.param("use_latched_publisher_for_points_xyzrgba", use_latched_publisher_for_points_xyzrgba_, false); - priv_.param("use_latched_publisher_for_color_image", use_latched_publisher_for_color_image_, false); - priv_.param("use_latched_publisher_for_depth_image", use_latched_publisher_for_depth_image_, false); - priv_.param("use_latched_publisher_for_snr_image", use_latched_publisher_for_snr_image_, false); - priv_.param("use_latched_publisher_for_normals_xyz", use_latched_publisher_for_normals_xyz_, false); + priv_.param( + "use_latched_publisher_for_points_xyz", use_latched_publisher_for_points_xyz_, false); + priv_.param( + "use_latched_publisher_for_points_xyzrgba", use_latched_publisher_for_points_xyzrgba_, false); + priv_.param( + "use_latched_publisher_for_color_image", use_latched_publisher_for_color_image_, false); + priv_.param( + "use_latched_publisher_for_depth_image", use_latched_publisher_for_depth_image_, false); + priv_.param( + "use_latched_publisher_for_snr_image", use_latched_publisher_for_snr_image_, false); + priv_.param( + "use_latched_publisher_for_normals_xyz", use_latched_publisher_for_normals_xyz_, false); bool update_firmware_automatically = true; - priv_.param("update_firmware_automatically", update_firmware_automatically, update_firmware_automatically); + priv_.param( + "update_firmware_automatically", update_firmware_automatically, update_firmware_automatically); - if (file_camera_mode) - { + if (file_camera_mode) { ROS_INFO("Creating file camera from file '%s'", file_camera_path.c_str()); camera_ = zivid_.createFileCamera(file_camera_path); - } - else - { + } else { auto cameras = zivid_.cameras(); ROS_INFO_STREAM(cameras.size() << " cameras found"); - if (cameras.empty()) - { + if (cameras.empty()) { throw std::runtime_error("No cameras found. Ensure that the camera is connected to your PC."); - } - else if (serial_number.empty()) - { + } else if (serial_number.empty()) { camera_ = [&]() { ROS_INFO("Selecting first available camera"); - for (auto& c : cameras) - { - if (c.state().isAvailable()) - return c; + for (auto & c : cameras) { + if (c.state().isAvailable()) return c; } - throw std::runtime_error("No available cameras found. Is the camera in use by another process?"); + throw std::runtime_error( + "No available cameras found. Is the camera in use by another process?"); }(); - } - else - { - if (serial_number.find(":") == 0) - { + } else { + if (serial_number.find(":") == 0) { serial_number = serial_number.substr(1); } camera_ = [&]() { ROS_INFO("Searching for camera with serial number '%s' ...", serial_number.c_str()); - for (auto& c : cameras) - { - if (c.info().serialNumber() == Zivid::CameraInfo::SerialNumber(serial_number)) - return c; + for (auto & c : cameras) { + if (c.info().serialNumber() == Zivid::CameraInfo::SerialNumber(serial_number)) return c; } throw std::runtime_error("No camera found with serial number '" + serial_number + "'"); }(); } - if (!Zivid::Firmware::isUpToDate(camera_)) - { - if (update_firmware_automatically) - { - ROS_INFO("The camera firmware is not up-to-date, and update_firmware_automatically is true, starting update"); - Zivid::Firmware::update(camera_, [](double progress, const std::string& state) { + if (!Zivid::Firmware::isUpToDate(camera_)) { + if (update_firmware_automatically) { + ROS_INFO( + "The camera firmware is not up-to-date, and update_firmware_automatically is true, " + "starting update"); + Zivid::Firmware::update(camera_, [](double progress, const std::string & state) { ROS_INFO(" [%.0f%%] %s", progress, state.c_str()); }); ROS_INFO("Firmware update completed"); - } - else - { - ROS_ERROR("The camera firmware is not up-to-date, and update_firmware_automatically is false. Throwing error."); - throw std::runtime_error("The firmware on camera '" + camera_.info().serialNumber().value() + - "' is not up to date. The launch parameter update_firmware_automatically " - "is set to false. Please update the firmware on this camera manually."); + } else { + ROS_ERROR( + "The camera firmware is not up-to-date, and update_firmware_automatically is false. " + "Throwing error."); + throw std::runtime_error( + "The firmware on camera '" + camera_.info().serialNumber().value() + + "' is not up to date. The launch parameter update_firmware_automatically " + "is set to false. Please update the firmware on this camera manually."); } } } ROS_INFO_STREAM(camera_); - if (!file_camera_mode) - { + if (!file_camera_mode) { ROS_INFO_STREAM("Connecting to camera '" << camera_.info().serialNumber() << "'"); camera_.connect(); } @@ -221,56 +216,59 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv) setCameraStatus(CameraStatus::Connected); camera_connection_keepalive_timer_ = - nh_.createTimer(ros::Duration(10), &ZividCamera::onCameraConnectionKeepAliveTimeout, this); + nh_.createTimer(ros::Duration(10), &ZividCamera::onCameraConnectionKeepAliveTimeout, this); - capture_settings_controller_ = - std::make_unique(nh_, camera_, "settings", max_capture_acquisitions); + capture_settings_controller_ = std::make_unique( + nh_, camera_, "settings", max_capture_acquisitions); // HDR is not supported in 2D mode, but for future-proofing the 2D configuration API is analogous // to 3D except there is only 1 acquisition. - capture_2d_settings_controller_ = std::make_unique(nh_, camera_, "settings_2d", 1); + capture_2d_settings_controller_ = + std::make_unique(nh_, camera_, "settings_2d", 1); ROS_INFO("Advertising topics"); points_xyz_publisher_ = - nh_.advertise("points/xyz", 1, use_latched_publisher_for_points_xyz_); - points_xyzrgba_publisher_ = - nh_.advertise("points/xyzrgba", 1, use_latched_publisher_for_points_xyzrgba_); - color_image_publisher_ = - image_transport_.advertiseCamera("color/image_color", 1, use_latched_publisher_for_color_image_); - depth_image_publisher_ = image_transport_.advertiseCamera("depth/image", 1, use_latched_publisher_for_depth_image_); - snr_image_publisher_ = image_transport_.advertiseCamera("snr/image", 1, use_latched_publisher_for_snr_image_); - normals_xyz_publisher_ = - nh_.advertise("normals/xyz", 1, use_latched_publisher_for_normals_xyz_); + nh_.advertise("points/xyz", 1, use_latched_publisher_for_points_xyz_); + points_xyzrgba_publisher_ = nh_.advertise( + "points/xyzrgba", 1, use_latched_publisher_for_points_xyzrgba_); + color_image_publisher_ = image_transport_.advertiseCamera( + "color/image_color", 1, use_latched_publisher_for_color_image_); + depth_image_publisher_ = + image_transport_.advertiseCamera("depth/image", 1, use_latched_publisher_for_depth_image_); + snr_image_publisher_ = + image_transport_.advertiseCamera("snr/image", 1, use_latched_publisher_for_snr_image_); + normals_xyz_publisher_ = nh_.advertise( + "normals/xyz", 1, use_latched_publisher_for_normals_xyz_); ROS_INFO("Advertising services"); - camera_info_model_name_service_ = - nh_.advertiseService("camera_info/model_name", &ZividCamera::cameraInfoModelNameServiceHandler, this); - camera_info_serial_number_service_ = - nh_.advertiseService("camera_info/serial_number", &ZividCamera::cameraInfoSerialNumberServiceHandler, this); - is_connected_service_ = nh_.advertiseService("is_connected", &ZividCamera::isConnectedServiceHandler, this); + camera_info_model_name_service_ = nh_.advertiseService( + "camera_info/model_name", &ZividCamera::cameraInfoModelNameServiceHandler, this); + camera_info_serial_number_service_ = nh_.advertiseService( + "camera_info/serial_number", &ZividCamera::cameraInfoSerialNumberServiceHandler, this); + is_connected_service_ = + nh_.advertiseService("is_connected", &ZividCamera::isConnectedServiceHandler, this); capture_service_ = nh_.advertiseService("capture", &ZividCamera::captureServiceHandler, this); capture_and_save_service_ = - nh_.advertiseService("capture_and_save", &ZividCamera::captureAndSaveServiceHandler, this); - capture_2d_service_ = nh_.advertiseService("capture_2d", &ZividCamera::capture2DServiceHandler, this); + nh_.advertiseService("capture_and_save", &ZividCamera::captureAndSaveServiceHandler, this); + capture_2d_service_ = + nh_.advertiseService("capture_2d", &ZividCamera::capture2DServiceHandler, this); capture_assistant_suggest_settings_service_ = nh_.advertiseService( - "capture_assistant/suggest_settings", &ZividCamera::captureAssistantSuggestSettingsServiceHandler, this); - load_settings_from_file_service_ = - nh_.advertiseService("load_settings_from_file", &ZividCamera::loadSettingsFromFileServiceHandler, this); - load_settings_2d_from_file_service_ = - nh_.advertiseService("load_settings_2d_from_file", &ZividCamera::loadSettings2DFromFileServiceHandler, this); + "capture_assistant/suggest_settings", + &ZividCamera::captureAssistantSuggestSettingsServiceHandler, this); + load_settings_from_file_service_ = nh_.advertiseService( + "load_settings_from_file", &ZividCamera::loadSettingsFromFileServiceHandler, this); + load_settings_2d_from_file_service_ = nh_.advertiseService( + "load_settings_2d_from_file", &ZividCamera::loadSettings2DFromFileServiceHandler, this); ROS_INFO("Zivid camera driver is now ready!"); } -void ZividCamera::onCameraConnectionKeepAliveTimeout(const ros::TimerEvent&) +void ZividCamera::onCameraConnectionKeepAliveTimeout(const ros::TimerEvent &) { ROS_DEBUG_STREAM(__func__); - try - { + try { reconnectToCameraIfNecessary(); - } - catch (const std::exception& e) - { + } catch (const std::exception & e) { ROS_INFO("%s failed with exception '%s'", __func__, e.what()); } } @@ -280,73 +278,65 @@ void ZividCamera::reconnectToCameraIfNecessary() ROS_DEBUG_STREAM(__func__); const auto state = camera_.state(); - if (state.isConnected().value()) - { + if (state.isConnected().value()) { setCameraStatus(CameraStatus::Connected); - } - else - { + } else { setCameraStatus(CameraStatus::Disconnected); // The camera handle needs to be refreshed to ensure we get the correct // "available" status. This is a bug in the API. auto cameras = zivid_.cameras(); - for (auto& c : cameras) - { - if (camera_.info().serialNumber() == c.info().serialNumber()) - { + for (auto & c : cameras) { + if (camera_.info().serialNumber() == c.info().serialNumber()) { camera_ = c; } } - if (state.isAvailable().value()) - { - ROS_INFO_STREAM("The camera '" << camera_.info().serialNumber() - << "' is not connected but is available. Re-connecting ..."); + if (state.isAvailable().value()) { + ROS_INFO_STREAM( + "The camera '" << camera_.info().serialNumber() + << "' is not connected but is available. Re-connecting ..."); camera_.connect(); ROS_INFO("Successfully reconnected to camera!"); setCameraStatus(CameraStatus::Connected); - } - else - { - ROS_INFO_STREAM("The camera '" << camera_.info().serialNumber() << "' is not connected nor available."); + } else { + ROS_INFO_STREAM( + "The camera '" << camera_.info().serialNumber() << "' is not connected nor available."); } } } void ZividCamera::setCameraStatus(CameraStatus camera_status) { - if (camera_status_ != camera_status) - { + if (camera_status_ != camera_status) { std::stringstream ss; - ss << "Camera status changed to " << toString(camera_status) << " (was " << toString(camera_status_) << ")"; - if (camera_status == CameraStatus::Connected) - { + ss << "Camera status changed to " << toString(camera_status) << " (was " + << toString(camera_status_) << ")"; + if (camera_status == CameraStatus::Connected) { ROS_INFO_STREAM(ss.str()); - } - else - { + } else { ROS_WARN_STREAM(ss.str()); } camera_status_ = camera_status; } } -bool ZividCamera::cameraInfoModelNameServiceHandler(zivid_camera::CameraInfoModelName::Request&, - zivid_camera::CameraInfoModelName::Response& res) +bool ZividCamera::cameraInfoModelNameServiceHandler( + zivid_camera::CameraInfoModelName::Request &, zivid_camera::CameraInfoModelName::Response & res) { res.model_name = camera_.info().modelName().toString(); return true; } -bool ZividCamera::cameraInfoSerialNumberServiceHandler(zivid_camera::CameraInfoSerialNumber::Request&, - zivid_camera::CameraInfoSerialNumber::Response& res) +bool ZividCamera::cameraInfoSerialNumberServiceHandler( + zivid_camera::CameraInfoSerialNumber::Request &, + zivid_camera::CameraInfoSerialNumber::Response & res) { res.serial_number = camera_.info().serialNumber().toString(); return true; } -bool ZividCamera::captureServiceHandler(Capture::Request&, Capture::Response&) +bool ZividCamera::captureServiceHandler(Capture::Request &, Capture::Response &) { ROS_DEBUG_STREAM(__func__); @@ -355,7 +345,8 @@ bool ZividCamera::captureServiceHandler(Capture::Request&, Capture::Response&) return true; } -bool ZividCamera::captureAndSaveServiceHandler(CaptureAndSave::Request& req, CaptureAndSave::Response&) +bool ZividCamera::captureAndSaveServiceHandler( + CaptureAndSave::Request & req, CaptureAndSave::Response &) { ROS_DEBUG_STREAM(__func__); @@ -366,7 +357,7 @@ bool ZividCamera::captureAndSaveServiceHandler(CaptureAndSave::Request& req, Cap return true; } -bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Response&) +bool ZividCamera::capture2DServiceHandler(Capture2D::Request &, Capture2D::Response &) { ROS_DEBUG_STREAM(__func__); @@ -374,15 +365,13 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon const auto settings2D = capture_2d_settings_controller_->zividSettings(); - if (settings2D.acquisitions().isEmpty()) - { + if (settings2D.acquisitions().isEmpty()) { throw std::runtime_error("capture_2d called with 0 enabled acquisitions!"); } ROS_DEBUG_STREAM(settings2D); auto frame2D = camera_.capture(settings2D); - if (shouldPublishColorImg()) - { + if (shouldPublishColorImg()) { const auto header = makeHeader(); auto image = frame2D.imageRGBA(); const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_); @@ -392,27 +381,26 @@ bool ZividCamera::capture2DServiceHandler(Capture2D::Request&, Capture2D::Respon return true; } -bool ZividCamera::captureAssistantSuggestSettingsServiceHandler(CaptureAssistantSuggestSettings::Request& req, - CaptureAssistantSuggestSettings::Response&) +bool ZividCamera::captureAssistantSuggestSettingsServiceHandler( + CaptureAssistantSuggestSettings::Request & req, CaptureAssistantSuggestSettings::Response &) { ROS_DEBUG_STREAM(__func__ << ": Request: " << req); serviceHandlerHandleCameraConnectionLoss(); - if (capture_settings_controller_->numAcquisitionConfigServers() < 10) - { - throw std::runtime_error("To use the CaptureAssistant the launch parameter 'max_capture_acquisitions' " - "must be at least 10, since the Capture Assistant may suggest up to 10 acquisitions. " - "See README.md for more information."); + if (capture_settings_controller_->numAcquisitionConfigServers() < 10) { + throw std::runtime_error( + "To use the CaptureAssistant the launch parameter 'max_capture_acquisitions' " + "must be at least 10, since the Capture Assistant may suggest up to 10 acquisitions. " + "See README.md for more information."); } using SuggestSettingsParameters = Zivid::CaptureAssistant::SuggestSettingsParameters; - const auto max_capture_time = - std::chrono::round(std::chrono::duration{ req.max_capture_time.toSec() }); + const auto max_capture_time = std::chrono::round( + std::chrono::duration{req.max_capture_time.toSec()}); const auto ambient_light_frequency = [&req]() { - switch (req.ambient_light_frequency) - { + switch (req.ambient_light_frequency) { case CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE: return SuggestSettingsParameters::AmbientLightFrequency::none; case CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_50HZ: @@ -420,53 +408,56 @@ bool ZividCamera::captureAssistantSuggestSettingsServiceHandler(CaptureAssistant case CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_60HZ: return SuggestSettingsParameters::AmbientLightFrequency::hz60; } - throw std::runtime_error("Unhandled AMBIENT_LIGHT_FREQUENCY value: " + std::to_string(req.ambient_light_frequency)); + throw std::runtime_error( + "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + std::to_string(req.ambient_light_frequency)); }(); - SuggestSettingsParameters suggest_settings_parameters{ SuggestSettingsParameters::MaxCaptureTime{ max_capture_time }, - ambient_light_frequency }; + SuggestSettingsParameters suggest_settings_parameters{ + SuggestSettingsParameters::MaxCaptureTime{max_capture_time}, ambient_light_frequency}; ROS_INFO_STREAM("Getting suggested settings using parameters: " << suggest_settings_parameters); - const auto suggested_settings = Zivid::CaptureAssistant::suggestSettings(camera_, suggest_settings_parameters); - ROS_INFO_STREAM("CaptureAssistant::suggestSettings returned " << suggested_settings.acquisitions().size() - << " acquisitions"); + const auto suggested_settings = + Zivid::CaptureAssistant::suggestSettings(camera_, suggest_settings_parameters); + ROS_INFO_STREAM( + "CaptureAssistant::suggestSettings returned " << suggested_settings.acquisitions().size() + << " acquisitions"); capture_settings_controller_->setZividSettings(suggested_settings); return true; } -bool ZividCamera::loadSettingsFromFileServiceHandler(LoadSettingsFromFile::Request& req, - LoadSettingsFromFile::Response&) +bool ZividCamera::loadSettingsFromFileServiceHandler( + LoadSettingsFromFile::Request & req, LoadSettingsFromFile::Response &) { ROS_DEBUG_STREAM(__func__ << ": Request: " << req); - capture_settings_controller_->setZividSettings(Zivid::Settings{ req.file_path.c_str() }); + capture_settings_controller_->setZividSettings(Zivid::Settings{req.file_path.c_str()}); return true; } -bool ZividCamera::loadSettings2DFromFileServiceHandler(LoadSettings2DFromFile::Request& req, - LoadSettings2DFromFile::Response&) +bool ZividCamera::loadSettings2DFromFileServiceHandler( + LoadSettings2DFromFile::Request & req, LoadSettings2DFromFile::Response &) { ROS_DEBUG_STREAM(__func__ << ": Request: " << req); - capture_2d_settings_controller_->setZividSettings(Zivid::Settings2D{ req.file_path.c_str() }); + capture_2d_settings_controller_->setZividSettings(Zivid::Settings2D{req.file_path.c_str()}); return true; } void ZividCamera::serviceHandlerHandleCameraConnectionLoss() { reconnectToCameraIfNecessary(); - if (camera_status_ != CameraStatus::Connected) - { - throw std::runtime_error("Unable to capture since the camera is not connected. Please re-connect the camera and " - "try again."); + if (camera_status_ != CameraStatus::Connected) { + throw std::runtime_error( + "Unable to capture since the camera is not connected. Please re-connect the camera and " + "try again."); } } -bool ZividCamera::isConnectedServiceHandler(IsConnected::Request&, IsConnected::Response& res) +bool ZividCamera::isConnectedServiceHandler(IsConnected::Request &, IsConnected::Response & res) { res.is_connected = camera_status_ == CameraStatus::Connected; return true; } -void ZividCamera::publishFrame(const Zivid::Frame& frame) +void ZividCamera::publishFrame(const Zivid::Frame & frame) { const bool publish_points_xyz = shouldPublishPointsXYZ(); const bool publish_points_xyzrgba = shouldPublishPointsXYZRGBA(); @@ -475,45 +466,40 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) const bool publish_snr_img = shouldPublishSnrImg(); const bool publish_normals_xyz = shouldPublishNormalsXYZ(); - if (publish_points_xyz || publish_points_xyzrgba || publish_color_img || publish_depth_img || publish_snr_img || - publish_normals_xyz) - { + if ( + publish_points_xyz || publish_points_xyzrgba || publish_color_img || publish_depth_img || + publish_snr_img || publish_normals_xyz) { const auto header = makeHeader(); auto point_cloud = frame.pointCloud(); // Transform point cloud from millimeters (Zivid SDK) to meter (ROS). const float scale = 0.001f; - const auto transformationMatrix = Zivid::Matrix4x4{ scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1 }; + const auto transformationMatrix = + Zivid::Matrix4x4{scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1}; point_cloud.transform(transformationMatrix); - if (publish_points_xyz) - { + if (publish_points_xyz) { publishPointCloudXYZ(header, point_cloud); } - if (publish_points_xyzrgba) - { + if (publish_points_xyzrgba) { publishPointCloudXYZRGBA(header, point_cloud); } - if (publish_color_img || publish_depth_img || publish_snr_img) - { + if (publish_color_img || publish_depth_img || publish_snr_img) { const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_); - const auto camera_info = makeCameraInfo(header, point_cloud.width(), point_cloud.height(), intrinsics); + const auto camera_info = + makeCameraInfo(header, point_cloud.width(), point_cloud.height(), intrinsics); - if (publish_color_img) - { + if (publish_color_img) { publishColorImage(header, camera_info, point_cloud); } - if (publish_depth_img) - { + if (publish_depth_img) { publishDepthImage(header, camera_info, point_cloud); } - if (publish_snr_img) - { + if (publish_snr_img) { publishSnrImage(header, camera_info, point_cloud); } } - if (publish_normals_xyz) - { + if (publish_normals_xyz) { publishNormalsXYZ(header, point_cloud); } } @@ -521,7 +507,8 @@ void ZividCamera::publishFrame(const Zivid::Frame& frame) bool ZividCamera::shouldPublishPointsXYZRGBA() const { - return points_xyzrgba_publisher_.getNumSubscribers() > 0 || use_latched_publisher_for_points_xyzrgba_; + return points_xyzrgba_publisher_.getNumSubscribers() > 0 || + use_latched_publisher_for_points_xyzrgba_; } bool ZividCamera::shouldPublishPointsXYZ() const @@ -558,7 +545,8 @@ std_msgs::Header ZividCamera::makeHeader() return header; } -void ZividCamera::publishPointCloudXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud) +void ZividCamera::publishPointCloudXYZ( + const std_msgs::Header & header, const Zivid::PointCloud & point_cloud) { ROS_DEBUG_STREAM("Publishing " << points_xyz_publisher_.getTopic()); @@ -577,11 +565,12 @@ void ZividCamera::publishPointCloudXYZ(const std_msgs::Header& header, const Ziv msg->point_step = sizeof(ZividDataType); msg->row_step = msg->point_step * msg->width; msg->data.resize(msg->row_step * msg->height); - point_cloud.copyData(reinterpret_cast(msg->data.data())); + point_cloud.copyData(reinterpret_cast(msg->data.data())); points_xyz_publisher_.publish(msg); } -void ZividCamera::publishPointCloudXYZRGBA(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud) +void ZividCamera::publishPointCloudXYZRGBA( + const std_msgs::Header & header, const Zivid::PointCloud & point_cloud) { ROS_DEBUG_STREAM("Publishing " << points_xyzrgba_publisher_.getTopic()); @@ -600,46 +589,54 @@ void ZividCamera::publishPointCloudXYZRGBA(const std_msgs::Header& header, const msg->point_step = sizeof(ZividDataType); msg->row_step = msg->point_step * msg->width; msg->data.resize(msg->row_step * msg->height); - point_cloud.copyData(reinterpret_cast(msg->data.data())); + point_cloud.copyData(reinterpret_cast(msg->data.data())); points_xyzrgba_publisher_.publish(msg); } -void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::PointCloud& point_cloud) +void ZividCamera::publishColorImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::PointCloud & point_cloud) { ROS_DEBUG_STREAM("Publishing " << color_image_publisher_.getTopic() << " from point cloud"); - auto image = makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::RGBA8); + auto image = + makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::RGBA8); color_image_publisher_.publish(image, camera_info); } -void ZividCamera::publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::Image& image) +void ZividCamera::publishColorImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::Image & image) { ROS_DEBUG_STREAM("Publishing " << color_image_publisher_.getTopic() << " from Image"); auto msg = makeImage(header, sensor_msgs::image_encodings::RGBA8, image.width(), image.height()); - const auto uint8_ptr_begin = reinterpret_cast(image.data()); - const auto uint8_ptr_end = reinterpret_cast(image.data() + image.size()); + const auto uint8_ptr_begin = reinterpret_cast(image.data()); + const auto uint8_ptr_end = reinterpret_cast(image.data() + image.size()); msg->data = std::vector(uint8_ptr_begin, uint8_ptr_end); color_image_publisher_.publish(msg, camera_info); } -void ZividCamera::publishDepthImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::PointCloud& point_cloud) +void ZividCamera::publishDepthImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::PointCloud & point_cloud) { ROS_DEBUG_STREAM("Publishing " << depth_image_publisher_.getTopic()); - auto image = makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::TYPE_32FC1); + auto image = makePointCloudImage( + point_cloud, header, sensor_msgs::image_encodings::TYPE_32FC1); depth_image_publisher_.publish(image, camera_info); } -void ZividCamera::publishSnrImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info, - const Zivid::PointCloud& point_cloud) +void ZividCamera::publishSnrImage( + const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const Zivid::PointCloud & point_cloud) { ROS_DEBUG_STREAM("Publishing " << snr_image_publisher_.getTopic()); - auto image = makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::TYPE_32FC1); + auto image = + makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::TYPE_32FC1); snr_image_publisher_.publish(image, camera_info); } -void ZividCamera::publishNormalsXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud) +void ZividCamera::publishNormalsXYZ( + const std_msgs::Header & header, const Zivid::PointCloud & point_cloud) { ROS_DEBUG_STREAM("Publishing " << normals_xyz_publisher_.getTopic()); @@ -654,13 +651,13 @@ void ZividCamera::publishNormalsXYZ(const std_msgs::Header& header, const Zivid: msg->point_step = sizeof(ZividDataType); msg->row_step = msg->point_step * msg->width; msg->data.resize(msg->row_step * msg->height); - point_cloud.copyData(reinterpret_cast(msg->data.data())); + point_cloud.copyData(reinterpret_cast(msg->data.data())); normals_xyz_publisher_.publish(msg); } -sensor_msgs::CameraInfoConstPtr ZividCamera::makeCameraInfo(const std_msgs::Header& header, std::size_t width, - std::size_t height, - const Zivid::CameraIntrinsics& intrinsics) +sensor_msgs::CameraInfoConstPtr ZividCamera::makeCameraInfo( + const std_msgs::Header & header, std::size_t width, std::size_t height, + const Zivid::CameraIntrinsics & intrinsics) { auto msg = boost::make_shared(); msg->header = header; @@ -714,8 +711,7 @@ Zivid::Frame ZividCamera::invokeCaptureAndPublishFrame() const auto settings = capture_settings_controller_->zividSettings(); - if (settings.acquisitions().isEmpty()) - { + if (settings.acquisitions().isEmpty()) { throw std::runtime_error("capture called with 0 enabled acquisitions!"); } diff --git a/zivid_camera/test/test_zivid_camera.cpp b/zivid_camera/test/test_zivid_camera.cpp index 2be7fda4..6eacd651 100644 --- a/zivid_camera/test/test_zivid_camera.cpp +++ b/zivid_camera/test/test_zivid_camera.cpp @@ -4,37 +4,34 @@ #pragma clang diagnostic ignored "-Wglobal-constructors" // error triggered by gtest fixtures #endif -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include +#include #include #include #include -#include #include - #include +#include #include -#include #include - -#include "gtest_include_wrapper.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include "gtest_include_wrapper.h" using SecondsD = std::chrono::duration; @@ -46,14 +43,14 @@ struct DependentFalse : std::false_type }; template -ros::Duration toRosDuration(const std::chrono::duration& d) +ros::Duration toRosDuration(const std::chrono::duration & d) { - return ros::Duration{ std::chrono::duration_cast(d).count() }; + return ros::Duration{std::chrono::duration_cast(d).count()}; } std::string testDataDir() { - return (boost::filesystem::path{ __FILE__ }.parent_path() / "data").string(); + return (boost::filesystem::path{__FILE__}.parent_path() / "data").string(); } } // namespace @@ -75,10 +72,7 @@ class ZividNodeTestBase : public testing::Test std::cerr << "End of test " << test_name << "\n"; } - void printLine() - { - std::cerr << std::string(80, '-') << "\n"; - } + void printLine() { std::cerr << std::string(80, '-') << "\n"; } }; class ZividNodeTest : public ZividNodeTestBase @@ -86,17 +80,20 @@ class ZividNodeTest : public ZividNodeTestBase protected: ros::NodeHandle nh_; - const ros::Duration node_ready_wait_duration{ 45 }; - const ros::Duration short_wait_duration{ 0.5 }; - const ros::Duration medium_wait_duration{ 1.0 }; - const ros::Duration dr_get_max_wait_duration{ 5 }; + const ros::Duration node_ready_wait_duration{45}; + const ros::Duration short_wait_duration{0.5}; + const ros::Duration medium_wait_duration{1.0}; + const ros::Duration dr_get_max_wait_duration{5}; static constexpr auto capture_service_name = "/zivid_camera/capture"; static constexpr auto capture_and_save_service_name = "/zivid_camera/capture_and_save"; static constexpr auto capture_2d_service_name = "/zivid_camera/capture_2d"; - static constexpr auto capture_assistant_suggest_settings_service_name = "/zivid_camera/capture_assistant/" - "suggest_settings"; - static constexpr auto load_settings_from_file_service_name = "/zivid_camera/load_settings_from_file"; - static constexpr auto load_settings_2d_from_file_service_name = "/zivid_camera/load_settings_2d_from_file"; + static constexpr auto capture_assistant_suggest_settings_service_name = + "/zivid_camera/capture_assistant/" + "suggest_settings"; + static constexpr auto load_settings_from_file_service_name = + "/zivid_camera/load_settings_from_file"; + static constexpr auto load_settings_2d_from_file_service_name = + "/zivid_camera/load_settings_2d_from_file"; static constexpr auto color_camera_info_topic_name = "/zivid_camera/color/camera_info"; static constexpr auto color_image_color_topic_name = "/zivid_camera/color/image_color"; @@ -115,36 +112,27 @@ class ZividNodeTest : public ZividNodeTestBase class SubscriptionWrapper { public: - static SubscriptionWrapper make(ros::NodeHandle& nh, const std::string& name) + static SubscriptionWrapper make(ros::NodeHandle & nh, const std::string & name) { auto w = SubscriptionWrapper(); - boost::function&)> cb = [impl = w.impl_.get()](const auto& v) mutable { - impl->num_messages_++; - impl->last_message_ = *v; - }; + boost::function &)> cb = + [impl = w.impl_.get()](const auto & v) mutable { + impl->num_messages_++; + impl->last_message_ = *v; + }; w.impl_->subscriber_ = nh.subscribe(name, 1, cb); return w; } - const std::optional& lastMessage() const - { - return impl_->last_message_; - } + const std::optional & lastMessage() const { return impl_->last_message_; } - std::size_t numMessages() const - { - return impl_->num_messages_; - } + std::size_t numMessages() const { return impl_->num_messages_; } private: - SubscriptionWrapper() : impl_(std::make_unique()) - { - } + SubscriptionWrapper() : impl_(std::make_unique()) {} struct Impl { - Impl() : num_messages_(0) - { - } + Impl() : num_messages_(0) {} ros::Subscriber subscriber_; std::optional last_message_; std::size_t num_messages_; @@ -154,19 +142,22 @@ class ZividNodeTest : public ZividNodeTestBase void enableFirst3DAcquisition() { - dynamic_reconfigure::Client acquisition_0_client("/zivid_camera/settings/" - "acquisition_0/"); + dynamic_reconfigure::Client acquisition_0_client( + "/zivid_camera/settings/" + "acquisition_0/"); zivid_camera::SettingsAcquisitionConfig acquisition_0_cfg; - ASSERT_TRUE(acquisition_0_client.getDefaultConfiguration(acquisition_0_cfg, dr_get_max_wait_duration)); + ASSERT_TRUE( + acquisition_0_client.getDefaultConfiguration(acquisition_0_cfg, dr_get_max_wait_duration)); acquisition_0_cfg.enabled = true; ASSERT_TRUE(acquisition_0_client.setConfiguration(acquisition_0_cfg)); } void enableFirst2DAcquisition() { - dynamic_reconfigure::Client acquisition_0_client("/zivid_camera/" - "settings_2d/" - "acquisition_0/"); + dynamic_reconfigure::Client acquisition_0_client( + "/zivid_camera/" + "settings_2d/" + "acquisition_0/"); zivid_camera::Settings2DAcquisitionConfig cfg; ASSERT_TRUE(acquisition_0_client.getDefaultConfiguration(cfg, dr_get_max_wait_duration)); cfg.enabled = true; @@ -182,7 +173,7 @@ class ZividNodeTest : public ZividNodeTestBase } template - SubscriptionWrapper subscribe(const std::string& name) + SubscriptionWrapper subscribe(const std::string & name) { return SubscriptionWrapper::make(nh_, name); } @@ -190,16 +181,18 @@ class ZividNodeTest : public ZividNodeTestBase class AllCaptureTopicsSubscriber { public: - AllCaptureTopicsSubscriber(ZividNodeTest& nodeTest) - : color_camera_info_sub_(nodeTest.subscribe(color_camera_info_topic_name)) - , color_image_color_sub_(nodeTest.subscribe(color_image_color_topic_name)) - , depth_camera_info_sub_(nodeTest.subscribe(depth_camera_info_topic_name)) - , depth_image_sub_(nodeTest.subscribe(depth_image_topic_name)) - , snr_camera_info_sub_(nodeTest.subscribe(snr_camera_info_topic_name)) - , snr_image_sub_(nodeTest.subscribe(snr_image_topic_name)) - , points_xyz_sub_(nodeTest.subscribe(points_xyz_topic_name)) - , points_xyzrgba_sub_(nodeTest.subscribe(points_xyzrgba_topic_name)) - , normals_xyz_sub_(nodeTest.subscribe(normals_xyz_topic_name)) + AllCaptureTopicsSubscriber(ZividNodeTest & nodeTest) + : color_camera_info_sub_( + nodeTest.subscribe(color_camera_info_topic_name)), + color_image_color_sub_(nodeTest.subscribe(color_image_color_topic_name)), + depth_camera_info_sub_( + nodeTest.subscribe(depth_camera_info_topic_name)), + depth_image_sub_(nodeTest.subscribe(depth_image_topic_name)), + snr_camera_info_sub_(nodeTest.subscribe(snr_camera_info_topic_name)), + snr_image_sub_(nodeTest.subscribe(snr_image_topic_name)), + points_xyz_sub_(nodeTest.subscribe(points_xyz_topic_name)), + points_xyzrgba_sub_(nodeTest.subscribe(points_xyzrgba_topic_name)), + normals_xyz_sub_(nodeTest.subscribe(normals_xyz_topic_name)) { } @@ -228,28 +221,30 @@ class ZividNodeTest : public ZividNodeTestBase }; template - void assertArrayFloatEq(const A& actual, const B& expected) const + void assertArrayFloatEq(const A & actual, const B & expected) const { ASSERT_EQ(actual.size(), expected.size()); - for (std::size_t i = 0; i < actual.size(); i++) - { + for (std::size_t i = 0; i < actual.size(); i++) { ASSERT_FLOAT_EQ(actual[i], expected[i]); } } - void assertSensorMsgsPointCloud2Meta(const sensor_msgs::PointCloud2& point_cloud, std::size_t width, - std::size_t height, std::size_t point_step) + void assertSensorMsgsPointCloud2Meta( + const sensor_msgs::PointCloud2 & point_cloud, std::size_t width, std::size_t height, + std::size_t point_step) { ASSERT_EQ(point_cloud.width, width); ASSERT_EQ(point_cloud.height, height); ASSERT_EQ(point_cloud.point_step, point_step); ASSERT_EQ(point_cloud.row_step, point_cloud.width * point_cloud.point_step); ASSERT_EQ(point_cloud.is_dense, false); - ASSERT_EQ(point_cloud.data.size(), point_cloud.width * point_cloud.height * point_cloud.point_step); + ASSERT_EQ( + point_cloud.data.size(), point_cloud.width * point_cloud.height * point_cloud.point_step); } - void assertSensorMsgsImageMeta(const sensor_msgs::Image& image, std::size_t width, std::size_t height, - std::size_t bytes_per_pixel, const std::string& encoding) + void assertSensorMsgsImageMeta( + const sensor_msgs::Image & image, std::size_t width, std::size_t height, + std::size_t bytes_per_pixel, const std::string & encoding) { ASSERT_EQ(image.width, width); ASSERT_EQ(image.height, height); @@ -259,13 +254,12 @@ class ZividNodeTest : public ZividNodeTestBase ASSERT_EQ(image.is_bigendian, false); } - void assertSensorMsgsImageContents(const sensor_msgs::Image& image, - const Zivid::Array2D& expected_rgba) + void assertSensorMsgsImageContents( + const sensor_msgs::Image & image, const Zivid::Array2D & expected_rgba) { ASSERT_EQ(image.width, expected_rgba.width()); ASSERT_EQ(image.height, expected_rgba.height()); - for (std::size_t i = 0; i < expected_rgba.size(); i++) - { + for (std::size_t i = 0; i < expected_rgba.size(); i++) { const auto expectedPixel = expected_rgba(i); const auto index = i * 4U; ASSERT_EQ(image.data[index], expectedPixel.r); @@ -277,8 +271,9 @@ class ZividNodeTest : public ZividNodeTestBase } template - void assertPointCloud2Field(const FieldType& field, const std::string& name, uint32_t offset, uint32_t datatype, - uint32_t count) + void assertPointCloud2Field( + const FieldType & field, const std::string & name, uint32_t offset, uint32_t datatype, + uint32_t count) { ASSERT_EQ(field.name, name); ASSERT_EQ(field.offset, offset); @@ -286,7 +281,7 @@ class ZividNodeTest : public ZividNodeTestBase ASSERT_EQ(field.count, count); } - void assertCameraInfoForFileCamera(const sensor_msgs::CameraInfo& ci) const + void assertCameraInfoForFileCamera(const sensor_msgs::CameraInfo & ci) const { ASSERT_EQ(ci.width, 1944U); ASSERT_EQ(ci.height, 1200U); @@ -295,25 +290,25 @@ class ZividNodeTest : public ZividNodeTestBase // [fx 0 cx] // K = [ 0 fy cy] // [ 0 0 1] - assertArrayFloatEq(ci.K, std::array{ 1781.448, 0, 990.49268, 0, 1781.5297, 585.81781, 0, 0, 1 }); + assertArrayFloatEq( + ci.K, std::array{1781.448, 0, 990.49268, 0, 1781.5297, 585.81781, 0, 0, 1}); // R = I - assertArrayFloatEq(ci.R, std::array{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }); + assertArrayFloatEq(ci.R, std::array{1, 0, 0, 0, 1, 0, 0, 0, 1}); // [fx' 0 cx' Tx] // P = [ 0 fy' cy' Ty] // [ 0 0 1 0] - assertArrayFloatEq(ci.P, - std::array{ 1781.448, 0, 990.49268, 0, 0, 1781.5297, 585.81781, 0, 0, 0, 1, 0 }); + assertArrayFloatEq( + ci.P, + std::array{1781.448, 0, 990.49268, 0, 0, 1781.5297, 585.81781, 0, 0, 0, 1, 0}); } }; class TestWithFileCamera : public ZividNodeTest { protected: - TestWithFileCamera() : camera_(zivid_.createFileCamera(file_camera_path)) - { - } + TestWithFileCamera() : camera_(zivid_.createFileCamera(file_camera_path)) {} Zivid::Application zivid_; Zivid::Camera camera_; }; @@ -381,40 +376,36 @@ class CaptureOutputTest : public TestWithFileCamera protected: Zivid::PointCloud captureViaSDKDefaultSettings() { - return camera_.capture(Zivid::Settings{ Zivid::Settings::Acquisitions{ Zivid::Settings::Acquisition{} } }) - .pointCloud(); + return camera_ + .capture(Zivid::Settings{Zivid::Settings::Acquisitions{Zivid::Settings::Acquisition{}}}) + .pointCloud(); } Zivid::Frame2D capture2DViaSDKDefaultSettings() { - return camera_.capture(Zivid::Settings2D{ Zivid::Settings2D::Acquisitions{ Zivid::Settings2D::Acquisition{} } }); + return camera_.capture( + Zivid::Settings2D{Zivid::Settings2D::Acquisitions{Zivid::Settings2D::Acquisition{}}}); } - Zivid::PointCloud captureViaSDK(const Zivid::Settings& settings) + Zivid::PointCloud captureViaSDK(const Zivid::Settings & settings) { return camera_.capture(settings).pointCloud(); } void compareFloat(float a, float b, float delta = 1e-6f) const { - if (std::isnan(a)) - { + if (std::isnan(a)) { ASSERT_TRUE(std::isnan(b)); - } - else - { + } else { ASSERT_NEAR(a, b, delta); } } void comparePointCoordinate(float ros_coordinate, float sdk_coordinate) const { - if (std::isnan(ros_coordinate)) - { + if (std::isnan(ros_coordinate)) { ASSERT_TRUE(std::isnan(sdk_coordinate)); - } - else - { + } else { // Output from the SDK is millimeters. In the ROS driver a transform is applied to convert // the ROS points to meters. const float delta = 0.000001f; @@ -429,7 +420,7 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZGBA) enableFirst3DAcquisitionAndCapture(); - const auto& last_pc2 = points_sub.lastMessage(); + const auto & last_pc2 = points_sub.lastMessage(); ASSERT_TRUE(last_pc2.has_value()); assertSensorMsgsPointCloud2Meta(*last_pc2, 1944U, 1200U, 16U); ASSERT_EQ(last_pc2->fields.size(), 4U); @@ -442,21 +433,21 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZGBA) const auto expected_xyzrgba = point_cloud.copyData(); ASSERT_EQ(last_pc2->width, expected_xyzrgba.width()); ASSERT_EQ(last_pc2->height, expected_xyzrgba.height()); - for (std::size_t i = 0; i < expected_xyzrgba.size(); i++) - { - const uint8_t* point_ptr = &last_pc2->data[i * last_pc2->point_step]; - const float x = *reinterpret_cast(&point_ptr[0]); - const float y = *reinterpret_cast(&point_ptr[4]); - const float z = *reinterpret_cast(&point_ptr[8]); - const uint32_t argb = *reinterpret_cast(&point_ptr[12]); - const auto& expected = expected_xyzrgba(i); + for (std::size_t i = 0; i < expected_xyzrgba.size(); i++) { + const uint8_t * point_ptr = &last_pc2->data[i * last_pc2->point_step]; + const float x = *reinterpret_cast(&point_ptr[0]); + const float y = *reinterpret_cast(&point_ptr[4]); + const float z = *reinterpret_cast(&point_ptr[8]); + const uint32_t argb = *reinterpret_cast(&point_ptr[12]); + const auto & expected = expected_xyzrgba(i); comparePointCoordinate(x, expected.point.x); comparePointCoordinate(y, expected.point.y); comparePointCoordinate(z, expected.point.z); const auto expected_argb = static_cast(expected.color.a << 24) | static_cast(expected.color.r << 16) | - static_cast(expected.color.g << 8) | static_cast(expected.color.b); + static_cast(expected.color.g << 8) | + static_cast(expected.color.b); ASSERT_EQ(argb, expected_argb); } } @@ -467,10 +458,11 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZ) enableFirst3DAcquisitionAndCapture(); - const auto& point_cloud = points_sub.lastMessage(); + const auto & point_cloud = points_sub.lastMessage(); ASSERT_TRUE(point_cloud.has_value()); - assertSensorMsgsPointCloud2Meta(*point_cloud, 1944U, 1200U, - 16U); // 3x4 bytes for xyz + 4 bytes padding (w) = 16 bytes total + assertSensorMsgsPointCloud2Meta( + *point_cloud, 1944U, 1200U, + 16U); // 3x4 bytes for xyz + 4 bytes padding (w) = 16 bytes total ASSERT_EQ(point_cloud->fields.size(), 3U); assertPointCloud2Field(point_cloud->fields[0], "x", 0, sensor_msgs::PointField::FLOAT32, 1); assertPointCloud2Field(point_cloud->fields[1], "y", 4, sensor_msgs::PointField::FLOAT32, 1); @@ -480,12 +472,11 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZ) auto expected_xyz = point_cloud_sdk.copyData(); ASSERT_EQ(point_cloud->width, expected_xyz.width()); ASSERT_EQ(point_cloud->height, expected_xyz.height()); - for (std::size_t i = 0; i < expected_xyz.size(); i++) - { - const uint8_t* point_ptr = &point_cloud->data[i * point_cloud->point_step]; - const float x = *reinterpret_cast(&point_ptr[0]); - const float y = *reinterpret_cast(&point_ptr[4]); - const float z = *reinterpret_cast(&point_ptr[8]); + for (std::size_t i = 0; i < expected_xyz.size(); i++) { + const uint8_t * point_ptr = &point_cloud->data[i * point_cloud->point_step]; + const float x = *reinterpret_cast(&point_ptr[0]); + const float y = *reinterpret_cast(&point_ptr[4]); + const float z = *reinterpret_cast(&point_ptr[8]); const auto expected = expected_xyz(i); comparePointCoordinate(x, expected.x); @@ -497,7 +488,8 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZ) TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) { auto points_sub = subscribe(points_xyz_topic_name); - dynamic_reconfigure::Client settings_client("/zivid_camera/settings/"); + dynamic_reconfigure::Client settings_client( + "/zivid_camera/settings/"); zivid_camera::SettingsConfig configOriginal; ASSERT_TRUE(settings_client.getDefaultConfiguration(configOriginal, dr_get_max_wait_duration)); auto config = configOriginal; @@ -516,25 +508,24 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) ASSERT_TRUE(settings_client.setConfiguration(config)); enableFirst3DAcquisitionAndCapture(); - const auto& point_cloud = points_sub.lastMessage(); + const auto & point_cloud = points_sub.lastMessage(); ASSERT_TRUE(point_cloud.has_value()); const auto point_cloud_sdk = captureViaSDK(Zivid::Settings{ - Zivid::Settings::Acquisitions{ Zivid::Settings::Acquisition{} }, - Zivid::Settings::RegionOfInterest::Box{ - Zivid::Settings::RegionOfInterest::Box::Enabled::yes, - Zivid::Settings::RegionOfInterest::Box::PointO{ -370.5, -288, 886 }, - Zivid::Settings::RegionOfInterest::Box::PointA{ -354, 191.5, 673 }, - Zivid::Settings::RegionOfInterest::Box::PointB{ 420, -250, 876.5 }, - Zivid::Settings::RegionOfInterest::Box::Extents{ -2, 200.5 }, - }, + Zivid::Settings::Acquisitions{Zivid::Settings::Acquisition{}}, + Zivid::Settings::RegionOfInterest::Box{ + Zivid::Settings::RegionOfInterest::Box::Enabled::yes, + Zivid::Settings::RegionOfInterest::Box::PointO{-370.5, -288, 886}, + Zivid::Settings::RegionOfInterest::Box::PointA{-354, 191.5, 673}, + Zivid::Settings::RegionOfInterest::Box::PointB{420, -250, 876.5}, + Zivid::Settings::RegionOfInterest::Box::Extents{-2, 200.5}, + }, }); auto expected = point_cloud_sdk.copyData(); const auto numNanZ = [&] { size_t count = 0; - for (size_t i = 0; i < expected.size(); ++i) - { + for (size_t i = 0; i < expected.size(); ++i) { count += std::isnan(expected(i).z); } return count; @@ -544,12 +535,11 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) ASSERT_GT(numNanZ, 500000); ASSERT_LT(numNanZ, expected.size() - 500000); - for (size_t i = 0; i < expected.size(); ++i) - { - const uint8_t* point_ptr = &point_cloud->data[i * point_cloud->point_step]; - const float x = *reinterpret_cast(&point_ptr[0]); - const float y = *reinterpret_cast(&point_ptr[4]); - const float z = *reinterpret_cast(&point_ptr[8]); + for (size_t i = 0; i < expected.size(); ++i) { + const uint8_t * point_ptr = &point_cloud->data[i * point_cloud->point_step]; + const float x = *reinterpret_cast(&point_ptr[0]); + const float y = *reinterpret_cast(&point_ptr[4]); + const float z = *reinterpret_cast(&point_ptr[8]); comparePointCoordinate(x, expected(i).x); comparePointCoordinate(y, expected(i).y); comparePointCoordinate(z, expected(i).z); @@ -588,10 +578,9 @@ TEST_F(CaptureOutputTest, testCaptureDepthImage) const auto expected_z = point_cloud.copyData(); ASSERT_EQ(image->width, expected_z.width()); ASSERT_EQ(image->height, expected_z.height()); - for (std::size_t i = 0; i < expected_z.size(); i++) - { + for (std::size_t i = 0; i < expected_z.size(); i++) { const auto expected = expected_z(i); - const float z = *reinterpret_cast(image->data.data() + i * bytes_per_pixel); + const float z = *reinterpret_cast(image->data.data() + i * bytes_per_pixel); comparePointCoordinate(z, expected.z); } } @@ -611,10 +600,9 @@ TEST_F(CaptureOutputTest, testCaptureSNRImage) const auto expected_snr = point_cloud.copyData(); ASSERT_EQ(image->width, expected_snr.width()); ASSERT_EQ(image->height, expected_snr.height()); - for (std::size_t i = 0; i < expected_snr.size(); i++) - { + for (std::size_t i = 0; i < expected_snr.size(); i++) { const auto expected = expected_snr(i); - const float snr = *reinterpret_cast(image->data.data() + i * bytes_per_pixel); + const float snr = *reinterpret_cast(image->data.data() + i * bytes_per_pixel); ASSERT_EQ(snr, expected.value); } } @@ -625,14 +613,18 @@ TEST_F(CaptureOutputTest, testCaptureNormals) enableFirst3DAcquisitionAndCapture(); - const auto& point_cloud = normals_sub.lastMessage(); + const auto & point_cloud = normals_sub.lastMessage(); ASSERT_TRUE(point_cloud.has_value()); - assertSensorMsgsPointCloud2Meta(*point_cloud, 1944U, 1200U, - 3U * sizeof(float)); // 12 bytes total + assertSensorMsgsPointCloud2Meta( + *point_cloud, 1944U, 1200U, + 3U * sizeof(float)); // 12 bytes total ASSERT_EQ(point_cloud->fields.size(), 3U); - assertPointCloud2Field(point_cloud->fields[0], "normal_x", 0, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(point_cloud->fields[1], "normal_y", 4, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(point_cloud->fields[2], "normal_z", 8, sensor_msgs::PointField::FLOAT32, 1); + assertPointCloud2Field( + point_cloud->fields[0], "normal_x", 0, sensor_msgs::PointField::FLOAT32, 1); + assertPointCloud2Field( + point_cloud->fields[1], "normal_y", 4, sensor_msgs::PointField::FLOAT32, 1); + assertPointCloud2Field( + point_cloud->fields[2], "normal_z", 8, sensor_msgs::PointField::FLOAT32, 1); auto point_cloud_sdk = captureViaSDKDefaultSettings(); auto expected_normal_xyz_before_transform = point_cloud_sdk.copyData(); @@ -642,18 +634,19 @@ TEST_F(CaptureOutputTest, testCaptureNormals) // Transform from mm to m (like is done internally in Zivid driver) const float scale = 0.001f; - point_cloud_sdk.transform(Zivid::Matrix4x4{ scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1 }); + point_cloud_sdk.transform( + Zivid::Matrix4x4{scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1}); auto expected_normal_xyz_after_transform = point_cloud_sdk.copyData(); - ASSERT_EQ(expected_normal_xyz_after_transform.size(), expected_normal_xyz_before_transform.size()); + ASSERT_EQ( + expected_normal_xyz_after_transform.size(), expected_normal_xyz_before_transform.size()); - for (std::size_t i = 0; i < expected_normal_xyz_before_transform.size(); i++) - { - const uint8_t* cloud_ptr = &point_cloud->data[i * point_cloud->point_step]; - const float normal_x = *reinterpret_cast(&cloud_ptr[0]); - const float normal_y = *reinterpret_cast(&cloud_ptr[4]); - const float normal_z = *reinterpret_cast(&cloud_ptr[8]); + for (std::size_t i = 0; i < expected_normal_xyz_before_transform.size(); i++) { + const uint8_t * cloud_ptr = &point_cloud->data[i * point_cloud->point_step]; + const float normal_x = *reinterpret_cast(&cloud_ptr[0]); + const float normal_y = *reinterpret_cast(&cloud_ptr[4]); + const float normal_z = *reinterpret_cast(&cloud_ptr[8]); - const auto& expected_sdk_before_transform = expected_normal_xyz_before_transform(i); + const auto & expected_sdk_before_transform = expected_normal_xyz_before_transform(i); // We do a transform in the ROS driver to scale from mm to meters. However, `expected_normal_xyz` // are calculated without transform, so we need a slightly higher delta to compare. constexpr float delta = 0.001f; @@ -661,7 +654,7 @@ TEST_F(CaptureOutputTest, testCaptureNormals) ASSERT_NO_FATAL_FAILURE(compareFloat(normal_y, expected_sdk_before_transform.y, delta)); ASSERT_NO_FATAL_FAILURE(compareFloat(normal_z, expected_sdk_before_transform.z, delta)); - const auto& expected_sdk_after_transform = expected_normal_xyz_after_transform(i); + const auto & expected_sdk_after_transform = expected_normal_xyz_after_transform(i); ASSERT_NO_FATAL_FAILURE(compareFloat(normal_x, expected_sdk_after_transform.x)); ASSERT_NO_FATAL_FAILURE(compareFloat(normal_y, expected_sdk_after_transform.y)); ASSERT_NO_FATAL_FAILURE(compareFloat(normal_z, expected_sdk_after_transform.z)); @@ -673,11 +666,13 @@ TEST_F(TestWithFileCamera, testSettingsEngine) enableFirst3DAcquisition(); auto points_sub = subscribe(points_xyz_topic_name); - dynamic_reconfigure::Client settings_client("/zivid_camera/settings/"); + dynamic_reconfigure::Client settings_client( + "/zivid_camera/settings/"); zivid_camera::SettingsConfig settings_cfg; ASSERT_TRUE(settings_client.getDefaultConfiguration(settings_cfg, dr_get_max_wait_duration)); -#if ZIVID_CORE_VERSION_MAJOR >= 3 || (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 12) +#if ZIVID_CORE_VERSION_MAJOR >= 3 || \ + (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 12) ASSERT_EQ(settings_cfg.engine, zivid_camera::Settings_EnginePhase); settings_cfg.engine = zivid_camera::Settings_EngineStripe; #else @@ -694,7 +689,8 @@ TEST_F(TestWithFileCamera, testSettingsEngine) ASSERT_FALSE(ros::service::call(capture_service_name, capture)); ASSERT_EQ(points_sub.numMessages(), 0U); -#if ZIVID_CORE_VERSION_MAJOR >= 3 || (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 12) +#if ZIVID_CORE_VERSION_MAJOR >= 3 || \ + (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 12) settings_cfg.engine = zivid_camera::Settings_EnginePhase; #else settings_cfg.experimental_engine = zivid_camera::Settings_ExperimentalEnginePhase; @@ -746,13 +742,14 @@ TEST_F(CaptureOutputTest, testCapture2D) short_wait_duration.sleep(); assert_num_topics_received(1); - auto verify_image_and_camera_info = [this](const auto& img, const auto& info) { + auto verify_image_and_camera_info = [this](const auto & img, const auto & info) { assertCameraInfoForFileCamera(info); assertSensorMsgsImageMeta(img, 1944U, 1200U, 4U, "rgba8"); assertSensorMsgsImageContents(img, capture2DViaSDKDefaultSettings().imageRGBA()); }; - verify_image_and_camera_info(*color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage()); + verify_image_and_camera_info( + *color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage()); short_wait_duration.sleep(); assert_num_topics_received(1); @@ -760,13 +757,14 @@ TEST_F(CaptureOutputTest, testCapture2D) ASSERT_TRUE(ros::service::call(capture_2d_service_name, capture)); short_wait_duration.sleep(); assert_num_topics_received(2); - verify_image_and_camera_info(*color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage()); + verify_image_and_camera_info( + *color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage()); } class CaptureAndSaveTest : public TestWithFileCamera { protected: - void capture_and_save_to_path(const std::string& file_path, bool expect_success) + void capture_and_save_to_path(const std::string & file_path, bool expect_success) { AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this); medium_wait_duration.sleep(); @@ -775,13 +773,10 @@ class CaptureAndSaveTest : public TestWithFileCamera zivid_camera::CaptureAndSave capture_and_save; capture_and_save.request.file_path = file_path; all_capture_topics_subscriber.assert_num_topics_received(0); - if (expect_success) - { + if (expect_success) { ASSERT_TRUE(ros::service::call(capture_and_save_service_name, capture_and_save)); ASSERT_TRUE(boost::filesystem::exists(file_path)); - } - else - { + } else { ASSERT_FALSE(ros::service::call(capture_and_save_service_name, capture_and_save)); ASSERT_FALSE(boost::filesystem::exists(file_path)); } @@ -790,10 +785,7 @@ class CaptureAndSaveTest : public TestWithFileCamera } }; -TEST_F(CaptureAndSaveTest, testCaptureAndSaveNoPath) -{ - capture_and_save_to_path("", false); -} +TEST_F(CaptureAndSaveTest, testCaptureAndSaveNoPath) { capture_and_save_to_path("", false); } TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidPath) { @@ -836,9 +828,10 @@ class DynamicReconfigureMinMaxDefaultTest : public TestWithFileCamera } template - void testGeneralMinMaxDefault(const std::string& service_name) + void testGeneralMinMaxDefault(const std::string & service_name) { - ASSERT_TRUE(ros::service::waitForService(service_name + "/set_parameters", short_wait_duration)); + ASSERT_TRUE( + ros::service::waitForService(service_name + "/set_parameters", short_wait_duration)); dynamic_reconfigure::Client client(service_name); @@ -864,24 +857,30 @@ class DynamicReconfigureMinMaxDefaultTest : public TestWithFileCamera ASSERT_EQ(min_cfg.processing_color_balance_red, sdkValidRange().min()); ASSERT_EQ(max_cfg.processing_color_balance_red, sdkValidRange().max()); - if constexpr (std::is_same_v) - { - using OutlierRemovalThreshold = typename ZividSettingsType::Processing::Filters::Outlier::Removal::Threshold; - ASSERT_EQ(default_cfg.processing_filters_outlier_removal_threshold, sdkDefaultValue()); - ASSERT_EQ(min_cfg.processing_filters_outlier_removal_threshold, sdkValidRange().min()); - ASSERT_EQ(max_cfg.processing_filters_outlier_removal_threshold, sdkValidRange().max()); + if constexpr (std::is_same_v) { + using OutlierRemovalThreshold = + typename ZividSettingsType::Processing::Filters::Outlier::Removal::Threshold; + ASSERT_EQ( + default_cfg.processing_filters_outlier_removal_threshold, + sdkDefaultValue()); + ASSERT_EQ( + min_cfg.processing_filters_outlier_removal_threshold, + sdkValidRange().min()); + ASSERT_EQ( + max_cfg.processing_filters_outlier_removal_threshold, + sdkValidRange().max()); } } template - void testAcquisitionMinMaxDefault(const std::string& prefix, std::size_t num_acquisition_servers) + void testAcquisitionMinMaxDefault(const std::string & prefix, std::size_t num_acquisition_servers) { - for (std::size_t i = 0; i < num_acquisition_servers; i++) - { - ASSERT_TRUE(ros::service::waitForService(prefix + "acquisition_" + std::to_string(i) + "/set_parameters", - short_wait_duration)); + for (std::size_t i = 0; i < num_acquisition_servers; i++) { + ASSERT_TRUE(ros::service::waitForService( + prefix + "acquisition_" + std::to_string(i) + "/set_parameters", short_wait_duration)); - dynamic_reconfigure::Client client(prefix + "acquisition_" + std::to_string(i) + "/"); + dynamic_reconfigure::Client client( + prefix + "acquisition_" + std::to_string(i) + "/"); ConfigType default_cfg; ASSERT_TRUE(client.getDefaultConfiguration(default_cfg, dr_get_max_wait_duration)); ConfigType min_cfg; @@ -912,7 +911,8 @@ class DynamicReconfigureMinMaxDefaultTest : public TestWithFileCamera ASSERT_EQ(max_cfg.gain, sdkValidRange().max()); } ASSERT_FALSE(ros::service::waitForService( - prefix + "acquisition_" + std::to_string(num_acquisition_servers) + "/set_parameters", short_wait_duration)); + prefix + "acquisition_" + std::to_string(num_acquisition_servers) + "/set_parameters", + short_wait_duration)); } }; @@ -920,54 +920,51 @@ TEST_F(DynamicReconfigureMinMaxDefaultTest, testDynamicReconfigureSettingsMinMax { // Test the default, min and max configuration of the file camera used in the test suite. This // file camera is of model "Zivid One". - testGeneralMinMaxDefault("/zivid_camera/settings/"); - testAcquisitionMinMaxDefault("/zivid_camera/settings/", 10); - - testGeneralMinMaxDefault("/zivid_camera/settings_2d/"); - testAcquisitionMinMaxDefault("/zivid_camera/" - "settings_2d/", - 1); + testGeneralMinMaxDefault( + "/zivid_camera/settings/"); + testAcquisitionMinMaxDefault( + "/zivid_camera/settings/", 10); + + testGeneralMinMaxDefault( + "/zivid_camera/settings_2d/"); + testAcquisitionMinMaxDefault( + "/zivid_camera/" + "settings_2d/", + 1); } class TestWithSettingsClients : public TestWithFileCamera { protected: TestWithSettingsClients() - : settings_client_("/zivid_camera/settings"), settings_2d_client_("/zivid_camera/settings_2d") + : settings_client_("/zivid_camera/settings"), settings_2d_client_("/zivid_camera/settings_2d") { settings_acquisition_clients_.reserve(num_settings_acquisition_dr_servers); - for (std::size_t i = 0; i < num_settings_acquisition_dr_servers; i++) - { + for (std::size_t i = 0; i < num_settings_acquisition_dr_servers; i++) { using Client = dynamic_reconfigure::Client; settings_acquisition_clients_.emplace_back( - std::make_unique("/zivid_camera/settings/acquisition_" + std::to_string(i))); + std::make_unique("/zivid_camera/settings/acquisition_" + std::to_string(i))); } settings_2d_acquisition_clients_.reserve(num_settings_2d_acquisition_dr_servers); - for (std::size_t i = 0; i < num_settings_2d_acquisition_dr_servers; i++) - { + for (std::size_t i = 0; i < num_settings_2d_acquisition_dr_servers; i++) { using Client = dynamic_reconfigure::Client; settings_2d_acquisition_clients_.emplace_back( - std::make_unique("/zivid_camera/settings_2d/acquisition_" + std::to_string(i))); + std::make_unique("/zivid_camera/settings_2d/acquisition_" + std::to_string(i))); } } template auto settingsConfig() { - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { zivid_camera::SettingsConfig cfg; EXPECT_TRUE(settings_client_.getCurrentConfiguration(cfg, dr_get_max_wait_duration)); return cfg; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { zivid_camera::Settings2DConfig cfg; EXPECT_TRUE(settings_2d_client_.getCurrentConfiguration(cfg, dr_get_max_wait_duration)); return cfg; - } - else - { + } else { static_assert(DependentFalse::value, "Unsupported ZividSettingsType"); } } @@ -975,20 +972,17 @@ class TestWithSettingsClients : public TestWithFileCamera template auto settingsAcquisitionConfig(std::size_t i) const { - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { zivid_camera::SettingsAcquisitionConfig cfg; - EXPECT_TRUE(settings_acquisition_clients_[i]->getCurrentConfiguration(cfg, dr_get_max_wait_duration)); + EXPECT_TRUE( + settings_acquisition_clients_[i]->getCurrentConfiguration(cfg, dr_get_max_wait_duration)); return cfg; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { zivid_camera::Settings2DAcquisitionConfig cfg; - EXPECT_TRUE(settings_2d_acquisition_clients_[i]->getCurrentConfiguration(cfg, dr_get_max_wait_duration)); + EXPECT_TRUE(settings_2d_acquisition_clients_[i]->getCurrentConfiguration( + cfg, dr_get_max_wait_duration)); return cfg; - } - else - { + } else { static_assert(DependentFalse::value, "Unsupported ZividSettingsType"); } } @@ -997,10 +991,8 @@ class TestWithSettingsClients : public TestWithFileCamera auto numEnabledAcquisitions() const { std::size_t enabled_acquisitions = 0; - for (std::size_t i = 0; i < maxAllowedAcquisitions(); i++) - { - if (settingsAcquisitionConfig(i).enabled) - { + for (std::size_t i = 0; i < maxAllowedAcquisitions(); i++) { + if (settingsAcquisitionConfig(i).enabled) { enabled_acquisitions++; } } @@ -1010,41 +1002,37 @@ class TestWithSettingsClients : public TestWithFileCamera template std::size_t maxAllowedAcquisitions() const { - if constexpr (std::is_same_v) - { + if constexpr (std::is_same_v) { return num_settings_acquisition_dr_servers; - } - else if constexpr (std::is_same_v) - { + } else if constexpr (std::is_same_v) { return num_settings_2d_acquisition_dr_servers; - } - else - { + } else { static_assert(DependentFalse::value, "Unsupported ZividSettingsType"); } } template - void compareSettingsWithNodeState(const ZividSettingsType& settings) + void compareSettingsWithNodeState(const ZividSettingsType & settings) { compareSettingsConfigWithSettings(settings, settingsConfig()); - const auto& acquisitions = settings.acquisitions(); + const auto & acquisitions = settings.acquisitions(); ASSERT_EQ(acquisitions.size(), numEnabledAcquisitions()); - for (std::size_t i = 0; i < acquisitions.size(); i++) - { - compareSettingsAcquisitionConfigWithSettings(acquisitions[i], settingsAcquisitionConfig(i)); + for (std::size_t i = 0; i < acquisitions.size(); i++) { + compareSettingsAcquisitionConfigWithSettings( + acquisitions[i], settingsAcquisitionConfig(i)); } - for (std::size_t i = acquisitions.size(); i < maxAllowedAcquisitions(); i++) - { + for (std::size_t i = acquisitions.size(); i < maxAllowedAcquisitions(); + i++) { ASSERT_EQ(false, settingsAcquisitionConfig(i).enabled); } } private: template - void compareSettingsAcquisitionConfigWithSettings(const ZividSettingsAcquisitionType& a, const CfgType& cfg) const + void compareSettingsAcquisitionConfigWithSettings( + const ZividSettingsAcquisitionType & a, const CfgType & cfg) const { ASSERT_EQ(true, cfg.enabled); ASSERT_EQ(a.aperture().value(), cfg.aperture); @@ -1053,27 +1041,42 @@ class TestWithSettingsClients : public TestWithFileCamera ASSERT_EQ(a.gain().value(), cfg.gain); } - void compareSettingsConfigWithSettings(const Zivid::Settings& s, const zivid_camera::SettingsConfig& cfg) const + void compareSettingsConfigWithSettings( + const Zivid::Settings & s, const zivid_camera::SettingsConfig & cfg) const { - const auto& color = s.processing().color(); + const auto & color = s.processing().color(); ASSERT_EQ(color.balance().blue().value(), cfg.processing_color_balance_blue); ASSERT_EQ(color.balance().green().value(), cfg.processing_color_balance_green); ASSERT_EQ(color.balance().red().value(), cfg.processing_color_balance_red); ASSERT_EQ(color.gamma().value(), cfg.processing_color_gamma); - const auto& filters = s.processing().filters(); - ASSERT_EQ(filters.noise().removal().isEnabled().value(), cfg.processing_filters_noise_removal_enabled); - ASSERT_EQ(filters.noise().removal().threshold().value(), cfg.processing_filters_noise_removal_threshold); - ASSERT_EQ(filters.smoothing().gaussian().isEnabled().value(), cfg.processing_filters_smoothing_gaussian_enabled); - ASSERT_EQ(filters.smoothing().gaussian().sigma().value(), cfg.processing_filters_smoothing_gaussian_sigma); - ASSERT_EQ(filters.outlier().removal().isEnabled().value(), cfg.processing_filters_outlier_removal_enabled); - ASSERT_EQ(filters.outlier().removal().threshold().value(), cfg.processing_filters_outlier_removal_threshold); - ASSERT_EQ(filters.reflection().removal().isEnabled().value(), cfg.processing_filters_reflection_removal_enabled); + const auto & filters = s.processing().filters(); + ASSERT_EQ( + filters.noise().removal().isEnabled().value(), cfg.processing_filters_noise_removal_enabled); + ASSERT_EQ( + filters.noise().removal().threshold().value(), + cfg.processing_filters_noise_removal_threshold); + ASSERT_EQ( + filters.smoothing().gaussian().isEnabled().value(), + cfg.processing_filters_smoothing_gaussian_enabled); + ASSERT_EQ( + filters.smoothing().gaussian().sigma().value(), + cfg.processing_filters_smoothing_gaussian_sigma); + ASSERT_EQ( + filters.outlier().removal().isEnabled().value(), + cfg.processing_filters_outlier_removal_enabled); + ASSERT_EQ( + filters.outlier().removal().threshold().value(), + cfg.processing_filters_outlier_removal_threshold); + ASSERT_EQ( + filters.reflection().removal().isEnabled().value(), + cfg.processing_filters_reflection_removal_enabled); } - void compareSettingsConfigWithSettings(const Zivid::Settings2D& s, const zivid_camera::Settings2DConfig& cfg) const + void compareSettingsConfigWithSettings( + const Zivid::Settings2D & s, const zivid_camera::Settings2DConfig & cfg) const { - const auto& color = s.processing().color(); + const auto & color = s.processing().color(); ASSERT_EQ(color.balance().blue().value(), cfg.processing_color_balance_blue); ASSERT_EQ(color.balance().green().value(), cfg.processing_color_balance_green); ASSERT_EQ(color.balance().red().value(), cfg.processing_color_balance_red); @@ -1083,39 +1086,40 @@ class TestWithSettingsClients : public TestWithFileCamera private: dynamic_reconfigure::Client settings_client_; std::vector>> - settings_acquisition_clients_; + settings_acquisition_clients_; dynamic_reconfigure::Client settings_2d_client_; - std::vector>> - settings_2d_acquisition_clients_; + std::vector< + std::unique_ptr>> + settings_2d_acquisition_clients_; }; class LoadSettingsTest : public TestWithSettingsClients { protected: template - void testLoadSettingsFromFile(const std::string& serviceName, const std::vector& fileNames) + void testLoadSettingsFromFile( + const std::string & serviceName, const std::vector & fileNames) { ASSERT_TRUE(ros::service::waitForService(serviceName, short_wait_duration)); - auto testLoadSettings = [&](const auto& fileName) { + auto testLoadSettings = [&](const auto & fileName) { CmdType cmd; cmd.request.file_path = testDataDir() + "/settings/" + fileName; ASSERT_TRUE(ros::service::call(serviceName, cmd)); short_wait_duration.sleep(); - auto expectedSettings = ZividSettingsType{ cmd.request.file_path }; + auto expectedSettings = ZividSettingsType{cmd.request.file_path}; recursivelyFillInUnsetWithCameraDefault(expectedSettings, camera_.info()); compareSettingsWithNodeState(expectedSettings); }; - for (const auto& fileName : fileNames) - { + for (const auto & fileName : fileNames) { testLoadSettings(fileName); } } template - void testLoadInvalidSettingsGivesError(const std::string& serviceName) + void testLoadInvalidSettingsGivesError(const std::string & serviceName) { ASSERT_TRUE(ros::service::waitForService(serviceName, short_wait_duration)); CmdType cmd; @@ -1125,7 +1129,7 @@ class LoadSettingsTest : public TestWithSettingsClients } template - void testLoadNonExistentFileGivesError(const std::string& serviceName) + void testLoadNonExistentFileGivesError(const std::string & serviceName) { ASSERT_TRUE(ros::service::waitForService(serviceName, short_wait_duration)); CmdType cmd; @@ -1136,15 +1140,16 @@ class LoadSettingsTest : public TestWithSettingsClients private: template - static void recursivelyFillInUnsetWithCameraDefault(Node& node, const Zivid::CameraInfo& cameraInfo) + static void recursivelyFillInUnsetWithCameraDefault( + Node & node, const Zivid::CameraInfo & cameraInfo) { - if constexpr (Node::nodeType == Zivid::DataModel::NodeType::group || - Node::nodeType == Zivid::DataModel::NodeType::leafDataModelList) - { - node.forEach([&cameraInfo](auto& child) { recursivelyFillInUnsetWithCameraDefault(child, cameraInfo); }); - } - else if (!node.hasValue()) - { + if constexpr ( + Node::nodeType == Zivid::DataModel::NodeType::group || + Node::nodeType == Zivid::DataModel::NodeType::leafDataModelList) { + node.forEach([&cameraInfo](auto & child) { + recursivelyFillInUnsetWithCameraDefault(child, cameraInfo); + }); + } else if (!node.hasValue()) { static_assert(Node::nodeType == Zivid::DataModel::NodeType::leafValue); node = Zivid::Experimental::SettingsInfo::defaultValue(cameraInfo); } @@ -1154,47 +1159,53 @@ class LoadSettingsTest : public TestWithSettingsClients TEST_F(LoadSettingsTest, testLoadSettingsFromFile) { testLoadSettingsFromFile( - load_settings_from_file_service_name, - { "3d/single.yml", "3d/hdr.yml", "3d/hdr_with_not_set_values.yml", "3d/single.yml" }); + load_settings_from_file_service_name, + {"3d/single.yml", "3d/hdr.yml", "3d/hdr_with_not_set_values.yml", "3d/single.yml"}); } TEST_F(LoadSettingsTest, testLoadSettings2DFromFile) { testLoadSettingsFromFile( - load_settings_2d_from_file_service_name, - { "2d/single_1.yml", "2d/single_2.yml", "2d/single_with_not_set_values.yml", "2d/single_1.yml" }); + load_settings_2d_from_file_service_name, + {"2d/single_1.yml", "2d/single_2.yml", "2d/single_with_not_set_values.yml", "2d/single_1.yml"}); } TEST_F(LoadSettingsTest, testLoadSettingsFromInvalidFileGivesError) { - testLoadInvalidSettingsGivesError(load_settings_from_file_service_name); + testLoadInvalidSettingsGivesError( + load_settings_from_file_service_name); } TEST_F(LoadSettingsTest, testLoadSettings2DFromInvalidFileGivesError) { - testLoadInvalidSettingsGivesError(load_settings_2d_from_file_service_name); + testLoadInvalidSettingsGivesError( + load_settings_2d_from_file_service_name); } TEST_F(LoadSettingsTest, testLoadSettingsFromNonExistentFileGivesError) { - testLoadNonExistentFileGivesError(load_settings_from_file_service_name); + testLoadNonExistentFileGivesError( + load_settings_from_file_service_name); } TEST_F(LoadSettingsTest, testLoadSettings2DFromNonExistentFileGivesError) { - testLoadNonExistentFileGivesError(load_settings_2d_from_file_service_name); + testLoadNonExistentFileGivesError( + load_settings_2d_from_file_service_name); } class ZividCATest : public TestWithSettingsClients { protected: - Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency toAPIAmbientLightFrequency( - zivid_camera::CaptureAssistantSuggestSettings::Request::_ambient_light_frequency_type ambient_light_frequency) + Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency + toAPIAmbientLightFrequency( + zivid_camera::CaptureAssistantSuggestSettings::Request::_ambient_light_frequency_type + ambient_light_frequency) { - using AmbientLightFrequency = Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency; + using AmbientLightFrequency = + Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency; using Request = zivid_camera::CaptureAssistantSuggestSettings::Request; - switch (ambient_light_frequency) - { + switch (ambient_light_frequency) { case Request::AMBIENT_LIGHT_FREQUENCY_NONE: return AmbientLightFrequency::none; case Request::AMBIENT_LIGHT_FREQUENCY_50HZ: @@ -1202,12 +1213,14 @@ class ZividCATest : public TestWithSettingsClients case Request::AMBIENT_LIGHT_FREQUENCY_60HZ: return AmbientLightFrequency::hz60; } - throw std::runtime_error("Could not convert value " + std::to_string(ambient_light_frequency) + " to API enum."); + throw std::runtime_error( + "Could not convert value " + std::to_string(ambient_light_frequency) + " to API enum."); } void performSuggestSettingsAndCompareWithCppAPI( - ros::Duration max_capture_time, - zivid_camera::CaptureAssistantSuggestSettings::Request::_ambient_light_frequency_type ambient_light_frequency) + ros::Duration max_capture_time, + zivid_camera::CaptureAssistantSuggestSettings::Request::_ambient_light_frequency_type + ambient_light_frequency) { zivid_camera::CaptureAssistantSuggestSettings srv; srv.request.max_capture_time = max_capture_time; @@ -1217,28 +1230,29 @@ class ZividCATest : public TestWithSettingsClients Zivid::CaptureAssistant::SuggestSettingsParameters suggest_settings_parameters{ Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime{ - std::chrono::round(SecondsD{ max_capture_time.toSec() }) }, - toAPIAmbientLightFrequency(ambient_light_frequency) - }; - const auto api_settings = Zivid::CaptureAssistant::suggestSettings(camera_, suggest_settings_parameters); + std::chrono::round(SecondsD{max_capture_time.toSec()})}, + toAPIAmbientLightFrequency(ambient_light_frequency)}; + const auto api_settings = + Zivid::CaptureAssistant::suggestSettings(camera_, suggest_settings_parameters); compareSettingsWithNodeState(api_settings); } }; TEST_F(ZividCATest, testCaptureAssistantServiceAvailable) { - ASSERT_TRUE(ros::service::waitForService(capture_assistant_suggest_settings_service_name, short_wait_duration)); + ASSERT_TRUE(ros::service::waitForService( + capture_assistant_suggest_settings_service_name, short_wait_duration)); } TEST_F(ZividCATest, testDifferentMaxCaptureTimeAndAmbientLightFrequency) { using Request = zivid_camera::CaptureAssistantSuggestSettings::Request; - for (double max_capture_time : { 0.2, 1.2, 10.0 }) - { - for (auto ambient_light_frequency : { Request::AMBIENT_LIGHT_FREQUENCY_NONE, Request::AMBIENT_LIGHT_FREQUENCY_50HZ, - Request::AMBIENT_LIGHT_FREQUENCY_60HZ }) - { - performSuggestSettingsAndCompareWithCppAPI(ros::Duration{ max_capture_time }, ambient_light_frequency); + for (double max_capture_time : {0.2, 1.2, 10.0}) { + for (auto ambient_light_frequency : + {Request::AMBIENT_LIGHT_FREQUENCY_NONE, Request::AMBIENT_LIGHT_FREQUENCY_50HZ, + Request::AMBIENT_LIGHT_FREQUENCY_60HZ}) { + performSuggestSettingsAndCompareWithCppAPI( + ros::Duration{max_capture_time}, ambient_light_frequency); } } } @@ -1246,21 +1260,24 @@ TEST_F(ZividCATest, testDifferentMaxCaptureTimeAndAmbientLightFrequency) TEST_F(ZividCATest, testGoingFromMultipleAcquisitionsTo1Acquisition) { using Request = zivid_camera::CaptureAssistantSuggestSettings::Request; - performSuggestSettingsAndCompareWithCppAPI(ros::Duration{ 10.0 }, Request::AMBIENT_LIGHT_FREQUENCY_NONE); + performSuggestSettingsAndCompareWithCppAPI( + ros::Duration{10.0}, Request::AMBIENT_LIGHT_FREQUENCY_NONE); ASSERT_GT(numEnabledAcquisitions(), 1U); - performSuggestSettingsAndCompareWithCppAPI(ros::Duration{ 0.2 }, Request::AMBIENT_LIGHT_FREQUENCY_NONE); + performSuggestSettingsAndCompareWithCppAPI( + ros::Duration{0.2}, Request::AMBIENT_LIGHT_FREQUENCY_NONE); ASSERT_EQ(numEnabledAcquisitions(), 1U); } TEST_F(ZividCATest, testCaptureAssistantWithInvalidMaxCaptureTimeFails) { zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = ros::Duration{ 0.0 }; + srv.request.max_capture_time = ros::Duration{0.0}; ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); - const auto valid_range = Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime::validRange(); - const auto small_delta = std::chrono::milliseconds{ 1 }; + const auto valid_range = + Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime::validRange(); + const auto small_delta = std::chrono::milliseconds{1}; srv.request.max_capture_time = toRosDuration(valid_range.min() - small_delta); ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); srv.request.max_capture_time = toRosDuration(valid_range.max() + small_delta); @@ -1273,23 +1290,23 @@ TEST_F(ZividCATest, testCaptureAssistantWithInvalidMaxCaptureTimeFails) TEST_F(ZividCATest, testCaptureAssistantDefaultAmbientLightFrequencyWorks) { zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = ros::Duration{ 1.0 }; + srv.request.max_capture_time = ros::Duration{1.0}; ASSERT_TRUE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); } TEST_F(ZividCATest, testCaptureAssistantInvalidAmbientLightFrequencyFails) { zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = ros::Duration{ 1.0 }; + srv.request.max_capture_time = ros::Duration{1.0}; srv.request.ambient_light_frequency = 255; ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); srv.request.ambient_light_frequency = - zivid_camera::CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; + zivid_camera::CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; ASSERT_TRUE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_zivid_camera"); diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 439aca72..564ac7ca 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -1,23 +1,21 @@ -#include -#include -#include #include #include -#include #include +#include +#include +#include +#include -#define CHECK(cmd) \ - do \ - { \ - if (!cmd) \ - { \ - throw std::runtime_error{ "\"" #cmd "\" failed!" }; \ - } \ +#define CHECK(cmd) \ + do { \ + if (!cmd) { \ + throw std::runtime_error{"\"" #cmd "\" failed!"}; \ + } \ } while (false) namespace { -const ros::Duration default_wait_duration{ 30 }; +const ros::Duration default_wait_duration{30}; void capture() { @@ -26,7 +24,7 @@ void capture() CHECK(ros::service::call("/zivid_camera/capture", capture)); } -void on_points(const sensor_msgs::PointCloud2ConstPtr&) +void on_points(const sensor_msgs::PointCloud2ConstPtr &) { ROS_INFO("PointCloud received"); capture(); @@ -34,7 +32,7 @@ void on_points(const sensor_msgs::PointCloud2ConstPtr&) } // namespace -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "sample_capture_cpp"); ros::NodeHandle n; @@ -48,8 +46,9 @@ int main(int argc, char** argv) auto points_sub = n.subscribe("/zivid_camera/points/xyzrgba", 1, on_points); - dynamic_reconfigure::Client settings_client("/zivid_camera/" - "settings/"); + dynamic_reconfigure::Client settings_client( + "/zivid_camera/" + "settings/"); // To initialize the settings_config object we need to load the default configuration from the server. // The default values of settings depends on which Zivid camera model is connected. @@ -60,8 +59,9 @@ int main(int argc, char** argv) settings_config.processing_filters_reflection_removal_enabled = true; CHECK(settings_client.setConfiguration(settings_config)); - dynamic_reconfigure::Client acquisition_0_client("/zivid_camera/settings/" - "acquisition_0/"); + dynamic_reconfigure::Client acquisition_0_client( + "/zivid_camera/settings/" + "acquisition_0/"); // To initialize the acquisition_0_config object we need to load the default configuration from the server. // The default values of settings depends on which Zivid camera model is connected. diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index 638ef22b..b705729f 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -1,22 +1,20 @@ -#include -#include #include #include -#include #include +#include +#include +#include -#define CHECK(cmd) \ - do \ - { \ - if (!cmd) \ - { \ - throw std::runtime_error{ "\"" #cmd "\" failed!" }; \ - } \ +#define CHECK(cmd) \ + do { \ + if (!cmd) { \ + throw std::runtime_error{"\"" #cmd "\" failed!"}; \ + } \ } while (false) namespace { -const ros::Duration default_wait_duration{ 30 }; +const ros::Duration default_wait_duration{30}; void capture() { @@ -25,7 +23,7 @@ void capture() CHECK(ros::service::call("/zivid_camera/capture_2d", capture_2d)); } -void on_image_color(const sensor_msgs::ImageConstPtr&) +void on_image_color(const sensor_msgs::ImageConstPtr &) { ROS_INFO("2D color image received"); capture(); @@ -33,7 +31,7 @@ void on_image_color(const sensor_msgs::ImageConstPtr&) } // namespace -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "sample_capture_2d"); ros::NodeHandle n; @@ -48,9 +46,10 @@ int main(int argc, char** argv) auto image_color_sub = n.subscribe("/zivid_camera/color/image_color", 1, on_image_color); ROS_INFO("Configuring image settings"); - dynamic_reconfigure::Client acquisition_0_client("/zivid_camera/" - "settings_2d/" - "acquisition_0/"); + dynamic_reconfigure::Client acquisition_0_client( + "/zivid_camera/" + "settings_2d/" + "acquisition_0/"); // To initialize the cfg object we need to load the default configuration from the server. // The default values of settings depends on which Zivid camera model is connected. diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 63dadee5..17758479 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -1,23 +1,21 @@ -#include -#include -#include #include #include #include #include +#include +#include +#include -#define CHECK(cmd) \ - do \ - { \ - if (!cmd) \ - { \ - throw std::runtime_error{ "\"" #cmd "\" failed!" }; \ - } \ +#define CHECK(cmd) \ + do { \ + if (!cmd) { \ + throw std::runtime_error{"\"" #cmd "\" failed!"}; \ + } \ } while (false) namespace { -const ros::Duration default_wait_duration{ 30 }; +const ros::Duration default_wait_duration{30}; void capture_and_save() { @@ -31,8 +29,9 @@ void capture_and_save() void enable_first_acquisition() { - dynamic_reconfigure::Client acquisition_0_client("/zivid_camera/settings/" - "acquisition_0/"); + dynamic_reconfigure::Client acquisition_0_client( + "/zivid_camera/settings/" + "acquisition_0/"); // To initialize the acquisition_0_config object we need to load the default configuration from the server. // The default values of settings depends on which Zivid camera model is connected. @@ -46,8 +45,9 @@ void enable_first_acquisition() void enable_diagnostics() { - dynamic_reconfigure::Client settings_client("/zivid_camera/" - "settings/"); + dynamic_reconfigure::Client settings_client( + "/zivid_camera/" + "settings/"); zivid_camera::SettingsConfig settings_config; CHECK(settings_client.getDefaultConfiguration(settings_config, default_wait_duration)); @@ -58,7 +58,7 @@ void enable_diagnostics() } // namespace -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "sample_capture_and_save_cpp"); ros::NodeHandle n; diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 6e616c9f..260a0e80 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -1,32 +1,32 @@ -#include -#include +#include #include #include -#include +#include +#include -#define CHECK(cmd) \ - do \ - { \ - if (!cmd) \ - { \ - throw std::runtime_error{ "\"" #cmd "\" failed!" }; \ - } \ +#define CHECK(cmd) \ + do { \ + if (!cmd) { \ + throw std::runtime_error{"\"" #cmd "\" failed!"}; \ + } \ } while (false) namespace { -const ros::Duration default_wait_duration{ 30 }; -constexpr auto ca_suggest_settings_service_name = "/zivid_camera/capture_assistant/suggest_settings"; +const ros::Duration default_wait_duration{30}; +constexpr auto ca_suggest_settings_service_name = + "/zivid_camera/capture_assistant/suggest_settings"; void capture_assistant_suggest_settings() { zivid_camera::CaptureAssistantSuggestSettings cass; - cass.request.max_capture_time = ros::Duration{ 1.20 }; + cass.request.max_capture_time = ros::Duration{1.20}; cass.request.ambient_light_frequency = - zivid_camera::CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; + zivid_camera::CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; - ROS_INFO_STREAM("Calling " << ca_suggest_settings_service_name - << " with max capture time = " << cass.request.max_capture_time << " sec"); + ROS_INFO_STREAM( + "Calling " << ca_suggest_settings_service_name + << " with max capture time = " << cass.request.max_capture_time << " sec"); CHECK(ros::service::call(ca_suggest_settings_service_name, cass)); } @@ -37,19 +37,13 @@ void capture() CHECK(ros::service::call("/zivid_camera/capture", capture)); } -void on_points(const sensor_msgs::PointCloud2ConstPtr&) -{ - ROS_INFO("PointCloud received"); -} +void on_points(const sensor_msgs::PointCloud2ConstPtr &) { ROS_INFO("PointCloud received"); } -void on_image_color(const sensor_msgs::ImageConstPtr&) -{ - ROS_INFO("2D color image received"); -} +void on_image_color(const sensor_msgs::ImageConstPtr &) { ROS_INFO("2D color image received"); } } // namespace -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "sample_capture_assistant_cpp"); ros::NodeHandle n; diff --git a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp index 35551f9b..efa379d7 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp @@ -1,25 +1,23 @@ -#include -#include -#include -#include #include #include -#include -#include #include +#include +#include +#include +#include +#include +#include -#define CHECK(cmd) \ - do \ - { \ - if (!cmd) \ - { \ - throw std::runtime_error{ "\"" #cmd "\" failed!" }; \ - } \ +#define CHECK(cmd) \ + do { \ + if (!cmd) { \ + throw std::runtime_error{"\"" #cmd "\" failed!"}; \ + } \ } while (false) namespace { -const ros::Duration default_wait_duration{ 30 }; +const ros::Duration default_wait_duration{30}; void capture() { @@ -30,7 +28,7 @@ void capture() } // namespace -int main(int argc, char** argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "sample_capture_with_settings_from_yml_cpp"); ros::NodeHandle n; From aef677f17133a84adce2ca8450d8b59e96f0ac62 Mon Sep 17 00:00:00 2001 From: Stian Pedersen Date: Tue, 11 Jun 2024 13:17:28 +0200 Subject: [PATCH 2/2] Support for ROS 2 This commit updates the zivid driver to support ROS2. See the CHANGELOG.md for user-facing changes and see the README.md for a general description of the ROS2 driver. Co-authored-by: Michael R. P. Ragazzon --- .clang-format | 4 +- .github/workflows/ROS-commit.yml | 32 +- .gitignore | 4 + CHANGELOG.md | 16 + CONTRIBUTING.md | 3 + README.md | 662 +++---- continuous-integration/.gitattributes | 2 + continuous-integration/build.sh | 32 +- continuous-integration/lint.sh | 17 +- continuous-integration/lint_cpp.sh | 25 - continuous-integration/notify_teams.py | 29 + .../run_build_and_test_in_docker.sh | 1 + .../setup/setup_build_and_test.sh | 30 +- continuous-integration/test.sh | 21 +- pyproject.toml | 2 + zivid_camera/CMakeLists.txt | 234 +-- zivid_camera/cmake/CompilerWarnings.cmake | 37 +- zivid_camera/config/rviz/zivid.rviz | 202 +++ .../include/auto_generated_include_wrapper.h | 22 - .../include/capture_settings_controller.h | 69 - zivid_camera/include/config_utils_common.h | 14 - zivid_camera/include/nodelet.h | 17 - zivid_camera/include/zivid_camera.h | 121 -- .../include/zivid_camera/visibility.hpp | 68 + .../include/zivid_camera/zivid_camera.hpp | 180 ++ zivid_camera/launch/visualize.launch | 4 +- zivid_camera/nodelets.xml | 7 - zivid_camera/package.xml | 54 +- zivid_camera/rviz/camera_view.rviz | 185 -- .../src/capture_settings_controller.cpp | 199 --- zivid_camera/src/node.cpp | 43 - zivid_camera/src/nodelet.cpp | 37 - zivid_camera/src/settings_generator.cpp | 684 -------- zivid_camera/src/zivid_camera.cpp | 968 +++++++---- zivid_camera/srv/Capture.srv | 1 - zivid_camera/srv/Capture2D.srv | 1 - zivid_camera/srv/CaptureAndSave.srv | 2 - zivid_camera/srv/LoadSettings2DFromFile.srv | 2 - zivid_camera/srv/LoadSettingsFromFile.srv | 2 - .../test/data/settings/2d/single_1.yml | 17 - .../test/data/settings/2d/single_2.yml | 18 - .../2d/single_with_not_set_values.yml | 18 - zivid_camera/test/data/settings/3d/hdr.yml | 56 - .../settings/3d/hdr_with_not_set_values.yml | 56 - zivid_camera/test/data/settings/3d/single.yml | 46 - .../test/data/settings/invalid_file.yml | 2 - zivid_camera/test/gtest_include_wrapper.h | 6 - zivid_camera/test/test_zivid_camera.cpp | 1543 ++++++++--------- zivid_camera/test/test_zivid_camera.test | 7 - zivid_interfaces/CMakeLists.txt | 19 + zivid_interfaces/package.xml | 26 + .../srv/CameraInfoModelName.srv | 0 .../srv/CameraInfoSerialNumber.srv | 0 zivid_interfaces/srv/CaptureAndSave.srv | 4 + .../srv/CaptureAssistantSuggestSettings.srv | 3 +- .../srv/IsConnected.srv | 0 zivid_samples/CMakeLists.txt | 116 +- zivid_samples/launch/sample.launch | 9 +- zivid_samples/launch/sample_with_rviz.launch | 6 + .../zivid_camera_with_serial_number.launch | 6 + zivid_samples/package.xml | 45 +- zivid_samples/scripts/sample_capture.py | 108 +- zivid_samples/scripts/sample_capture_2d.py | 101 +- .../scripts/sample_capture_and_save.py | 116 +- .../scripts/sample_capture_assistant.py | 108 +- .../sample_capture_with_settings_from_file.py | 86 + .../sample_capture_with_settings_from_yml.py | 38 - zivid_samples/src/sample_capture.cpp | 151 +- zivid_samples/src/sample_capture_2d.cpp | 132 +- zivid_samples/src/sample_capture_and_save.cpp | 158 +- .../src/sample_capture_assistant.cpp | 128 +- ...sample_capture_with_settings_from_file.cpp | 84 + .../sample_capture_with_settings_from_yml.cpp | 57 - 73 files changed, 3270 insertions(+), 4033 deletions(-) create mode 100644 .gitignore create mode 100644 CONTRIBUTING.md create mode 100644 continuous-integration/.gitattributes delete mode 100755 continuous-integration/lint_cpp.sh create mode 100644 pyproject.toml create mode 100644 zivid_camera/config/rviz/zivid.rviz delete mode 100644 zivid_camera/include/auto_generated_include_wrapper.h delete mode 100644 zivid_camera/include/capture_settings_controller.h delete mode 100644 zivid_camera/include/config_utils_common.h delete mode 100644 zivid_camera/include/nodelet.h delete mode 100644 zivid_camera/include/zivid_camera.h create mode 100644 zivid_camera/include/zivid_camera/visibility.hpp create mode 100644 zivid_camera/include/zivid_camera/zivid_camera.hpp delete mode 100644 zivid_camera/nodelets.xml delete mode 100644 zivid_camera/rviz/camera_view.rviz delete mode 100644 zivid_camera/src/capture_settings_controller.cpp delete mode 100644 zivid_camera/src/node.cpp delete mode 100644 zivid_camera/src/nodelet.cpp delete mode 100644 zivid_camera/src/settings_generator.cpp delete mode 100644 zivid_camera/srv/Capture.srv delete mode 100644 zivid_camera/srv/Capture2D.srv delete mode 100644 zivid_camera/srv/CaptureAndSave.srv delete mode 100644 zivid_camera/srv/LoadSettings2DFromFile.srv delete mode 100644 zivid_camera/srv/LoadSettingsFromFile.srv delete mode 100644 zivid_camera/test/data/settings/2d/single_1.yml delete mode 100644 zivid_camera/test/data/settings/2d/single_2.yml delete mode 100644 zivid_camera/test/data/settings/2d/single_with_not_set_values.yml delete mode 100644 zivid_camera/test/data/settings/3d/hdr.yml delete mode 100644 zivid_camera/test/data/settings/3d/hdr_with_not_set_values.yml delete mode 100644 zivid_camera/test/data/settings/3d/single.yml delete mode 100644 zivid_camera/test/data/settings/invalid_file.yml delete mode 100644 zivid_camera/test/gtest_include_wrapper.h delete mode 100644 zivid_camera/test/test_zivid_camera.test create mode 100644 zivid_interfaces/CMakeLists.txt create mode 100644 zivid_interfaces/package.xml rename {zivid_camera => zivid_interfaces}/srv/CameraInfoModelName.srv (100%) rename {zivid_camera => zivid_interfaces}/srv/CameraInfoSerialNumber.srv (100%) create mode 100644 zivid_interfaces/srv/CaptureAndSave.srv rename {zivid_camera => zivid_interfaces}/srv/CaptureAssistantSuggestSettings.srv (67%) rename {zivid_camera => zivid_interfaces}/srv/IsConnected.srv (100%) create mode 100644 zivid_samples/launch/sample_with_rviz.launch create mode 100644 zivid_samples/launch/zivid_camera_with_serial_number.launch create mode 100755 zivid_samples/scripts/sample_capture_with_settings_from_file.py delete mode 100755 zivid_samples/scripts/sample_capture_with_settings_from_yml.py create mode 100644 zivid_samples/src/sample_capture_with_settings_from_file.cpp delete mode 100644 zivid_samples/src/sample_capture_with_settings_from_yml.cpp diff --git a/.clang-format b/.clang-format index aaca8594..2867c95c 100644 --- a/.clang-format +++ b/.clang-format @@ -1,4 +1,5 @@ -# Based on https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format +# Style from `ament_clang_format` linter. +# https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format --- Language: Cpp BasedOnStyle: Google @@ -18,3 +19,4 @@ ContinuationIndentWidth: 2 DerivePointerAlignment: false PointerAlignment: Middle ReflowComments: false +... diff --git a/.github/workflows/ROS-commit.yml b/.github/workflows/ROS-commit.yml index b6016f11..052fcf53 100644 --- a/.github/workflows/ROS-commit.yml +++ b/.github/workflows/ROS-commit.yml @@ -21,7 +21,7 @@ jobs: lfs: false - name: Run code_analysis.sh run: | - CI_TEST_OS=ros:noetic-ros-base-focal \ + CI_TEST_OS=ros:humble-ros-base-jammy \ ./continuous-integration/run_code_analysis_in_docker.sh - name: Notify Teams if: ${{ failure() && github.ref == 'refs/heads/master' }} @@ -34,17 +34,25 @@ jobs: strategy: fail-fast: true matrix: - os: ['ros:noetic-ros-base-focal'] + os: ['ros:humble-ros-base-jammy', 'ros:iron-ros-core-jammy', 'ros:jazzy-ros-core-noble'] compiler: ['g++', 'clang++'] include: - - os: 'ros:noetic-ros-base-focal' - compiler: 'g++-10' - - os: 'ros:noetic-ros-base-focal' - compiler: 'g++-11' - - os: 'ros:noetic-ros-base-focal' - compiler: 'clang++-11' - - os: 'ros:noetic-ros-base-focal' - compiler: 'clang++-12' + - os: 'ros:humble-ros-base-jammy' + compiler: 'g++-12' + - os: 'ros:humble-ros-base-jammy' + compiler: 'g++-13' + - os: 'ros:humble-ros-base-jammy' + compiler: 'clang++-15' + - os: 'ros:jazzy-ros-core-noble' + compiler: 'clang++-14' + - os: 'ros:jazzy-ros-core-noble' + compiler: 'clang++-15' + - os: 'ros:jazzy-ros-core-noble' + compiler: 'clang++-16' + - os: 'ros:jazzy-ros-core-noble' + compiler: 'clang++-17' + - os: 'ros:jazzy-ros-core-noble' + compiler: 'clang++-18' steps: - name: Check out code uses: actions/checkout@v2 @@ -68,7 +76,7 @@ jobs: fail-fast: true matrix: zivid-version: ['2.9.0+4dbba385-1', '2.10.0+8ce7dae3-2', '2.10.1+50b274e8-7', '2.11.0+95829246-1', '2.11.1+de9b5dae-1', '2.12.0+6afd4961-1', '2.13.0+99a4ce9e-1'] - ros-distro: ['ros:noetic-ros-base-focal'] + ros-distro: ['ros:humble-ros-base-jammy'] steps: - name: Check out code uses: actions/checkout@v2 @@ -78,7 +86,7 @@ jobs: run: | CI_TEST_ZIVID_VERSION=${{ matrix.zivid-version }} \ CI_TEST_OS=${{ matrix.ros-distro }} \ - CI_TEST_COMPILER="g++-7" \ + CI_TEST_COMPILER="g++" \ CI_TEST_DOWNLOAD_TELICAM=${{ (matrix.zivid-version == '2.12.0+6afd4961-1' || matrix.zivid-version == '2.13.0+99a4ce9e-1') && '0' || '1' }} \ ./continuous-integration/run_build_and_test_in_docker.sh - name: Notify Teams diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..7f99c5a1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build/ +install/ +log/ +.idea/ diff --git a/CHANGELOG.md b/CHANGELOG.md index 53a5d70d..0b3ac5d5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,22 @@ This project adheres to [Semantic Versioning](https://semver.org). +# 3.0.0 + +* Driver is updated with support for ROS 2. ROS 1 is no longer supported. If you need support + for ROS 1, check one of the earlier releases below, or use the branch `ros1-master`. +* Capture settings must be set via .yml files or YAML strings, which can be exported from + Zivid Studio or the Zivid SDK. The driver no longer supports changing individual Zivid + settings nodes directly from ROS. See the Configuration section of the [README](./README.md) + for more details. +* Service descriptions have been moved to a separate ROS package named `zivid_interfaces`. +* The `capture` and `capture_2d` services now use the standard `std_srvs/srv/Trigger` type. +* The `load_settings_from_file` and `load_settings_2d_from_file` services are removed. Settings + files must now be configured via parameters. See the Configuration section of the + [README](./README.md) for more details. +* The data type of the `rgba` field in the `points/xyzrgba` topic is changed from `FLOAT32` to + `UINT32`. + ## 2.5.0 * Fixed an issue where the driver when running as a nodelet could not attach to a manager diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..309be1e1 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the 3-Clause BSD License, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/README.md b/README.md index 711cd035..c4704ee0 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ This is the official ROS driver for [Zivid 3D cameras](https://www.zivid.com/). -_A preview branch with support for ROS 2 is [now available here](https://github.com/zivid/zivid-ros/tree/ros2-preview)!_ +This driver provides support for ROS2. If you are looking for ROS1 support, please see the [`ros1-master` branch](https://github.com/zivid/zivid-ros/tree/ros1-master). [![Build Status][ci-badge]][ci-url] ![Zivid Image][header-image] @@ -13,9 +13,9 @@ _A preview branch with support for ROS 2 is [now available here](https://github. [**Installation**](#installation) | [**Getting Started**](#getting-started) | [**Launching**](#launching-the-driver) | +[**Configuration**](#configuration) | [**Services**](#services) | [**Topics**](#topics) | -[**Configuration**](#configuration) | [**Samples**](#samples) | [**FAQ**](#frequently-asked-questions) @@ -23,18 +23,11 @@ _A preview branch with support for ROS 2 is [now available here](https://github. ## Installation -### ROS - -This driver supports Ubuntu 20.04 with ROS Noetic. Follow the official [ROS installation instructions](http://wiki.ros.org/ROS/Installation) for -your OS. For support for earlier operating systems, see [releases](https://github.com/zivid/zivid-ros/releases). - -Also install catkin and git: +### Support -Ubuntu 20.04: -```bash -sudo apt-get update -sudo apt-get install -y python3-catkin-tools python3-osrf-pycommon git -``` +This driver supports Ubuntu 20.04 / 22.04 / 24.04 with ROS2. Follow the official [ROS installation instructions](https://docs.ros.org/) for +your OS. If you are looking for the ROS1 driver, please see +the [`ros1-master` branch](https://github.com/zivid/zivid-ros/tree/ros1-master). ### Zivid SDK @@ -53,153 +46,113 @@ An OpenCL 1.2 compatible GPU with driver installed is required by the SDK. Follo A C++17 compiler is required. -Ubuntu 20.04: ```bash sudo apt-get install -y g++ ``` ### Downloading and building Zivid ROS driver -First, load the setup.bash script into the current session. +First, source the `setup.bash` script for your ROS distribution in your terminal: -Ubuntu 20.04: ```bash -source /opt/ros/noetic/setup.bash +source /opt/ros/$ROS_DISTRO/setup.bash ``` -Create the workspace and src directory: +Then create the workspace and src directory: ```bash -mkdir -p ~/catkin_ws/src +mkdir -p ~/ros2_ws/src ``` Clone the Zivid ROS project into the src directory: ```bash -cd ~/catkin_ws/src +cd ~/ros2_ws/src git clone https://github.com/zivid/zivid-ros.git ``` -Install dependencies: +Initialize rosdep: ```bash -cd ~/catkin_ws -sudo apt-get update +cd ~/ros2_ws/src +sudo rosdep init rosdep update -rosdep install --from-paths src --ignore-src -r -y ``` -Finally, build the driver. +Install dependencies: +```bash +cd ~/ros2_ws/src +rosdep install -i --from-path ./ -y +``` + +Finally, build the driver: -Ubuntu 20.04: ```bash -catkin build +cd ~/ros2_ws +colcon build --symlink-install ``` ## Getting started -Connect the Zivid camera to your ethernet or USB3 port on your PC. You can use the ZividListCameras command-line +Connect the Zivid camera to your PC. You can use the `ZividListCameras` command-line tool available in the "Zivid Tools" package to confirm that your system has been configured correctly, and that the camera is discovered by your PC. You can also open Zivid Studio and connect to the camera. Close Zivid Studio before continuing with the rest of this guide. -Launch `sample_capture_assistant.py` to test that everything is working: +Run the sample_capture_cpp via the launch script to check that everything is working. ```bash -cd ~/catkin_ws && source devel/setup.bash -roslaunch zivid_samples sample.launch type:=sample_capture_assistant.py +cd ~/ros2_ws && source install/setup.bash +ros2 launch zivid_samples sample_with_rviz.launch sample:=sample_capture_cpp ``` -This will start the `zivid_camera` driver node, the -[sample_capture_assistant.py](./zivid_samples/scripts/sample_capture_assistant.py) node, as well as -[rviz](https://wiki.ros.org/rviz) to visualize the point cloud and the 2D color and depth images -and [rqt_reconfigure](https://wiki.ros.org/rqt_reconfigure) to view or change capture settings. - -The `sample_capture_assistant.py` node will first call the -[capture_assistant/suggest_settings](#capture_assistantsuggest_settings) service to find suggested -capture settings for your particular scene, then call the [capture](#capture) service to -capture using those settings. If everything is working, the point cloud, color image and depth image -should be visible in rviz. - -You can adjust the maximum capture time by changing variable `max_capture_time` in -[sample_capture_assistant.py](./zivid_samples/scripts/sample_capture_assistant.py) and -re-launching the sample. - -

- -

+This will start the `zivid_camera` driver node, the `sample_capture_cpp` node, and `rviz`. +The `zivid_camera` driver will connect to the first available Zivid camera, and +then `sample_capture_cpp` will trigger captures repeatedly. The resulting point cloud and +color image should be visible in `rviz`. A more detailed description of the `zivid_camera` driver follows below. -For sample code in C++ and Python, see the [Samples](#samples) section. +For sample code, see the [Samples](#samples) section. ## Launching the driver -It is required to specify a [namespace](http://wiki.ros.org/Nodes#Remapping_Arguments.A.22Pushing_Down.22) -when starting the driver. All the services, topics and configurations will be pushed into this namespace. - -To launch the driver, first start `roscore` in a seperate terminal window: +To launch the driver, use `ros2 run`: ```bash -roscore +cd ~/ros2_ws && source install/setup.bash +ros2 run zivid_camera zivid_camera ``` -In another terminal run: -```bash -cd ~/catkin_ws && source devel/setup.bash -``` - -Then, launch the driver either as a node or a [nodelet](http://wiki.ros.org/nodelet). To launch as a node: - -```bash -ROS_NAMESPACE=zivid_camera rosrun zivid_camera zivid_camera_node -``` - -To launch as a [nodelet](http://wiki.ros.org/nodelet): - -```bash -ROS_NAMESPACE=zivid_camera rosrun nodelet nodelet standalone zivid_camera/nodelet -``` +The driver will by default connect to the first available Zivid camera. +This behavior can be overridden by setting the `serial_number` launch parameter, see below. ### Launch Parameters (advanced) The following parameters can be specified when starting the driver. Note that all the parameters are -optional, and typically not required to set. +optional. -For example, to run the zivid_camera driver with `frame_id` specified, append `_frame_id:=camera1` to the -`rosrun` command: +For example, to run the zivid_camera driver with a specific `serial_number` specified: ```bash -ROS_NAMESPACE=zivid_camera rosrun zivid_camera zivid_camera_node _frame_id:=camera1 +ros2 run zivid_camera zivid_camera --ros-args -p serial_number:=ABCD1234 ``` -Or, if using `roslaunch` specify the parameter using ``: +Or you can use a [launch file](./zivid_samples/launch/zivid_camera_with_serial_number.launch) +and invoke it as: -```xml - - - - - +```bash +ros2 launch zivid_samples zivid_camera_with_serial_number.launch serial_number:=ABCD1234 ``` `file_camera_path` (string, default: "") > Specify the path to a file camera to use instead of a real Zivid camera. This can be used to > develop without access to hardware. The file camera returns the same point cloud for every capture. -> [Click here to download a file camera.](https://www.zivid.com/software/FileCameraZividOne.zip) +> [Visit our knowledgebase to download file camera.](https://support.zivid.com/en/latest/academy/camera/file-camera.html) `frame_id` (string, default: "zivid_optical_frame") > Specify the frame_id used for all published images and point clouds. -`max_capture_acquisitions` (int, default: 10) -> Specify the number of dynamic_reconfigure `settings/acquisition_` nodes that are created. This number -> defines the maximum number of acquisitions that can be a part of a 3D capture. All `settings/acquisition_` -> nodes are by default enabled=false (see section [Configuration](#configuration)). If you need to -> perform 3D HDR capture with more than 10 enabled acquisitions then increase this number. Otherwise it can -> be left as default. We do not recommend lowering this setting, especially if you are using the -> [capture_assistant/suggest_settings](#capture_assistantsuggest_settings) service. - `serial_number` (string, default: "") -> Specify the serial number of the Zivid camera to use. Important: When passing this value via -> the command line or rosparam the serial number must be prefixed with a colon (`:12345`). -> This parameter is optional. By default the driver will connect to the first available camera. +> Specify the serial number of the Zivid camera to use. This parameter is optional. By default, the +> driver will connect to the first available camera. `update_firmware_automatically` (bool, default: true) > Specify if the firmware of the connected camera should be automatically updated to the correct @@ -208,45 +161,55 @@ Or, if using `roslaunch` specify the parameter using ``: > Read more about [firmware update in our knowledgebase][firmware-update-kb-url]. > This parameter is optional, and by default it is true. -## Services +## Configuration -### capture_assistant/suggest_settings -[zivid_camera/CaptureAssistantSuggestSettings.srv](./zivid_camera/srv/CaptureAssistantSuggestSettings.srv) +The capture settings used by the `zivid_camera` ROS driver must be configured using YAML, +which can be exported from Zivid Studio or the API, or downloaded as .yml files from our [knowledge +base][presets-kb-url]. -Invoke this service to analyze your scene and find suggested settings for your particular scene, -camera distance, ambient lighting conditions, etc. The suggested settings are configured on this -node and accessible via dynamic_reconfigure, see section [Configuration](#configuration). When this -service has returned you can invoke the [capture](#capture) service to trigger a 3D capture using -these suggested settings. +For convenience, the Zivid ROS driver supports configuring capture settings in two ways: Using file path +to a .yml file, or as a YAML string. -This service has two parameters: +The following ROS parameters control which settings are used when capturing with the driver. Note +that you must set _either_ the `_file_path` or the `_yaml` parameter. If both `_file_path` and `_yaml` +parameters are set to a non-empty string at the same time, then the driver will throw an exception +when capturing. By default, all settings parameters are empty. -`max_capture_time` (duration): -> Specify the maximum capture time for the settings suggested by the Capture Assistant. A longer -> capture time may be required to get good data for more challenging scenes. Minimum value is -> 0.2 sec and maximum value is 10.0 sec. +### 3D capture -`ambient_light_frequency` (uint8): -> Possible values are: `AMBIENT_LIGHT_FREQUENCY_NONE`, `AMBIENT_LIGHT_FREQUENCY_50HZ`, -> `AMBIENT_LIGHT_FREQUENCY_60HZ`. Can be used to ensure that the suggested settings are compatible -> with the frequency of the ambient light in the scene. If ambient light is unproblematic, use -> `AMBIENT_LIGHT_FREQUENCY_NONE` for optimal performance. Default is `AMBIENT_LIGHT_FREQUENCY_NONE`. +`settings_file_path` (string, default: "") +> Specify the path to a .yml file that contains the settings you want to use. -See [Sample Capture Assistant](#sample-capture-assistant) for code example. +`settings_yaml` (string, default: "") +> Specify a YAML string that contains the settings you want to use. For example, you can copy the contents of a .yml +> file saved from Zivid Studio. + +The service `capture_assistant/suggest_settings` will modify the settings parameters automatically. + +### 2D capture + +`settings_2d_file_path` (string, default: "") +> Specify the path to a .yml file that contains the settings you want to use. + +`settings_2d_yaml` (string, default: "") +> Specify a YAML string that contains the 2D settings you want to use. For example, you can copy the contents of a +> .yml file saved from Zivid Studio. + +## Services ### capture -[zivid_camera/Capture.srv](./zivid_camera/srv/Capture.srv) +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) Invoke this service to trigger a 3D capture. See section [Configuration](#configuration) for how to -configure the 3D capture settings. The resulting point cloud is published on topics [points/xyz](#points/xyz) and -[points/xyzrgba](#points/xyzrgba), color image is published on topic [color/image_color](#colorimage_color), -and depth image is published on topic [depth/image](depthimage). Camera calibration is published on +configure the 3D capture settings. The resulting point cloud is published on topics [points/xyz](#pointsxyz) and +[points/xyzrgba](#pointsxyzrgba), color image is published on topic [color/image_color](#colorimage_color), +and depth image is published on topic [depth/image](#depthimage). Camera calibration is published on topics [color/camera_info](#colorcamera_info) and [depth/camera_info](#depthcamera_info). See [Sample Capture](#sample-capture) for code example. ### capture_2d -[zivid_camera/Capture2D.srv](./zivid_camera/srv/Capture2D.srv) +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) Invoke this service to trigger a 2D capture. See section [Configuration](#configuration) for how to configure the 2D capture settings. The resulting 2D image is published to topic @@ -255,402 +218,231 @@ capture, see [capture](#capture). See [Sample Capture 2D](#sample-capture-2d) for code example. -### load_settings_from_file -[zivid_camera/LoadSettingsFromFile.srv](./zivid_camera/srv/LoadSettingsFromFile.srv) - -Loads 3D settings from a `.yml` file saved from Zivid Studio or the Zivid SDK. The settings are -visible via dynamic_reconfigure, see section [Configuration](#configuration). -When this service has returned you can invoke the [capture](#capture) service to trigger a 3D -capture using these settings. - -See [Sample Capture with Settings from File](#sample-capture-with-settings-from-file) for code example. - -### load_settings_2d_from_file -[zivid_camera/LoadSettings2DFromFile.srv](./zivid_camera/srv/LoadSettings2DFromFile.srv) - -Loads 2D settings from a `.yml` file saved from Zivid Studio or the Zivid SDK. The settings are -visible via dynamic_reconfigure, see section [Configuration](#configuration). When this -service has returned you can invoke the [capture_2d](#capture) service to trigger a 2D capture -using these settings. - ### capture_and_save -[zivid_camera/CaptureAndSave.srv](./zivid_camera/srv/CaptureAndSave.srv) +[zivid_interfaces/srv/CaptureAndSave.srv](./zivid_interfaces/srv/CaptureAndSave.srv) -It does exactly the same as the [capture](#capture) service in addition to saving the frame to +It does exactly the same as the [capture](#capture) service, in addition it will save the frame to a file. This service takes a path as an argument. The chosen format is detected via the file extension. See [knowledge base](https://support.zivid.com/en/latest/reference-articles/point-cloud-structure-and-output-formats.html#zivid-output-formats) for a list of available output formats. See [Sample Capture and Save](#sample-capture-and-save) for code example. +### capture_assistant/suggest_settings +[zivid_interfaces/srv/CaptureAssistantSuggestSettings.srv](./zivid_interfaces/srv/CaptureAssistantSuggestSettings.srv) + +Invoke this service to analyze your scene and find suggested settings for your particular scene, +camera distance, ambient lighting conditions, etc. This service will automatically update the node parameter +`settings_yaml` with the suggested settings, see section [Configuration](#configuration). +When this service has returned, you can invoke the [capture](#capture) service to trigger a 3D capture using +these suggested settings. + +This service has two parameters: + +`max_capture_time` (duration): +> Specify the maximum capture time for the settings suggested by the Capture Assistant. A longer +> capture time may be required to get good data for more challenging scenes. Minimum value is +> 0.2 sec and maximum value is 10.0 sec. + +`ambient_light_frequency` (uint8): +> Possible values are: `AMBIENT_LIGHT_FREQUENCY_NONE`, `AMBIENT_LIGHT_FREQUENCY_50HZ`, +> `AMBIENT_LIGHT_FREQUENCY_60HZ`. Can be used to ensure that the suggested settings are compatible +> with the frequency of the ambient light in the scene. If ambient light is unproblematic, use +> `AMBIENT_LIGHT_FREQUENCY_NONE` for optimal performance. Default is `AMBIENT_LIGHT_FREQUENCY_NONE`. + +See [Sample Capture Assistant](#sample-capture-assistant) for code example. + ### camera_info/model_name -[zivid_camera/CameraInfoModelName.srv](./zivid_camera/srv/CameraInfoModelName.srv) +[zivid_interfaces/srv/CameraInfoModelName.srv](./zivid_interfaces/srv/CameraInfoModelName.srv) Returns the camera's model name. ### camera_info/serial_number -[zivid_camera/CameraInfoSerialNumber.srv](./zivid_camera/srv/CameraInfoSerialNumber.srv) +[zivid_interfaces/srv/CameraInfoSerialNumber.srv](./zivid_interfaces/srv/CameraInfoSerialNumber.srv) Returns the camera's serial number. ### is_connected -[zivid_camera/IsConnected.srv](./zivid_camera/srv/IsConnected.srv) +[zivid_interfaces/srv/IsConnected.srv](./zivid_interfaces/srv/IsConnected.srv) Returns if the camera is currently in `Connected` state (from the perspective of the ROS driver). The connection status is updated by the driver every 10 seconds and before each [capture](#capture) service call. If the camera is not in `Connected` state the driver will attempt to re-connect to the camera when it detects that the camera is available. This can happen if the camera is -power-cycled or the ethernet/USB cable is unplugged and then replugged. +power-cycled or the USB/Ethernet cable is unplugged and then replugged. ## Topics The Zivid ROS driver provides several topics providing 3D, color, SNR and camera calibration -data as a result of calling [capture](#capture) and/or [capture_2d](#capture_2d). -The different output topics provides flexibility for different use cases. Note that for -performance reasons no messages are generated or sent on topics with zero active subscribers. +data as a result of calling capture/capture_2d services. The different output topics provides +flexibility for different use cases. Note that for performance reasons no messages are generated +or sent on topics with zero active subscribers. ### color/camera_info -[sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) +[sensor_msgs/msg/CameraInfo](https://docs.ros2.org/latest/api/sensor_msgs/msg/CameraInfo.html) Camera calibration and metadata. ### color/image_color -[sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html) +[sensor_msgs/msg/Image](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) Color/RGBA image. RGBA image is published as a result of invoking the [capture](#capture) or [capture_2d](#capture_2d) service. Images are encoded as "rgba8", where the alpha channel is always 255. ### depth/camera_info -[sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) +[sensor_msgs/msg/CameraInfo](https://docs.ros2.org/latest/api/sensor_msgs/msg/CameraInfo.html) Camera calibration and metadata. ### depth/image -[sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html) +[sensor_msgs/msg/Image](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) Depth image. Each pixel contains the z-value (along the camera Z axis) in meters. The image is encoded as 32-bit float. Pixels where z-value is missing are NaN. ### points/xyzrgba -[sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) +[sensor_msgs/msg/PointCloud2](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html) Point cloud data. Sent as a result of the [capture](#capture) service. The output is in the camera's optical frame, where x is right, y is down and z is forward. The included point fields are x, y, z (in meters) and rgba (color). ### points/xyz -[sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) +[sensor_msgs/msg/PointCloud2](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html) -Point cloud data. This topic is similar to topic [points/xyzrgba](#points/xyzrgba), except +Point cloud data. This topic is similar to topic [points/xyzrgba](#pointsxyzrgba), except that only the XYZ 3D coordinates are included. This topic is recommended if you don't need the RGBA values. ### snr/camera_info -[sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) +[sensor_msgs/msg/CameraInfo](https://docs.ros2.org/latest/api/sensor_msgs/msg/CameraInfo.html) Camera calibration and metadata. ### snr/image -[sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html) +[sensor_msgs/msg/Image](https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html) Each pixel contains the SNR (signal-to-noise ratio) value. The image is encoded as 32-bit float. Published as a part of the [capture](#capture) service. ### normals/xyz -[sensor_msgs/PointCloud2](http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) +[sensor_msgs/msg/PointCloud2](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud2.html) Normals for the point cloud. The included fields are normal x, y and z coordinates. Each coordinate is a float value. There are no additional padding floats, so point-step is 12 bytes (3*4 bytes). The normals are unit vectors. Note that subscribing to this topic will cause some additional processing time for calculating the normals. -## Configuration +## Samples -The `zivid_camera` node supports both single-acquisition (2D and 3D) and multi-acquisition -HDR captures (3D only). 3D HDR-capture works by taking several acquisitions with different -settings (for example different exposure time) and combining them into one high-quality point -cloud. For more information about HDR capture, visit our -[knowledge base][hdr-getting-good-point-clouds-url]. - -The capture settings available in the `zivid_camera` node matches the settings in the Zivid API. -To become more familiar with the different settings and what they do, see the API reference for the -[Settings](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings.html) -and [Settings2D](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D.html) -classes, or use Zivid Studio. - -The settings can be viewed and configured using [dynamic_reconfigure](https://wiki.ros.org/dynamic_reconfigure). -Use [rqt_reconfigure](https://wiki.ros.org/rqt_reconfigure) to view/change the settings using a GUI: - -```bash -rosrun rqt_reconfigure rqt_reconfigure -``` - -If you want to experiment with the capture settings, launch the [Sample Capture](#sample-capture) sample, -which will capture in a loop forever, and use [rqt_reconfigure](https://wiki.ros.org/rqt_reconfigure) -to adjust the settings. - -The available capture settings are organized into a hierarchy of configuration nodes. 3D settings are available -under the `/settings` namespace, while 2D settings are available under `/settings_2d`. - -``` -/settings - ... - /acquisition_0 - ... - /acquisition_1 - ... - ... - /acquisition_9 - ... -/settings_2d - ... - /acquisition_0 - ... -``` - -**Important notice for C++ users:** The default value and min/max values of the settings are dependent on -what Zivid camera model you are using. Therefore you need to query the server for the correct values after -you have connected to the camera. To initialize a settings Config object you should **not** use the static -`__getDefault()__` methods of the auto-generated C++ config classes (`zivid_camera::SettingsConfig`, -`zivid_camera::SettingsAcquisitionConfig`, `zivid_camera::Settings2DConfig`, and -`zivid_camera::Settings2DAcquisitionConfig`). Instead, you should query the server for the default -values using `dynamic_reconfigure::Client::getDefaultConfiguration()`. See the [C++ samples](#samples) -for how to do this. For Python users this is already handled by dynamic_reconfigure.client.Client. - -**Tip:** Use the [load_settings_from_file](#load_settings_from_file) -or [load_settings_2d_from_file](#load_settings_2d_from_file) service to load 3D/2D settings from a -.yml file saved from Zivid Studio or the Zivid SDK. - -**Tip:** The Capture Assistant feature can be used to find optimized 3D capture settings for your -scene. Refer to service [capture_assistant/suggest_settings](#capture_assistantsuggest_settings). - -### 3D Settings - -#### Acquisition settings - -`settings/acquisition_/` contains settings for an individual acquisition. By default `` can be 0 to 9 for a -total of 10 acquisitions. The total number of acquisitions can be configured using the launch parameter -`max_capture_acquisitions` (see section [Launch Parameters](#launch-parameters-advanced) above). - -`settings/acquisition_/enabled` controls if acquisition `` will be included when the [capture](#capture) service is -invoked. If only one acquisition is enabled the [capture](#capture) service performs a 3D single-capture. If more than -one acquisition is enabled the [capture](#capture) service will perform a 3D HDR-capture. By default `enabled` is false. -In order to capture a point cloud at least one acquisition needs to be enabled. - -| Name | Type | Zivid API Setting | Note | -|------------------------------------------|--------|--------------------------------|--------| -| `settings/acquisition_/enabled` | bool | -| `settings/acquisition_/aperture` | double | [Settings::Acquisition::Aperture](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Acquisition_1_1Aperture.html) -| `settings/acquisition_/brightness` | double | [Settings::Acquisition::Brightness](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Acquisition_1_1Brightness.html) -| `settings/acquisition_/exposure_time` | int | [Settings::Acquisition::ExposureTime](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Acquisition_1_1ExposureTime.html) | Microseconds -| `settings/acquisition_/gain` | double | [Settings::Acquisition::Gain](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Acquisition_1_1Gain.html) - - -#### Processing settings - -Settings related to processing, like color balance and filter settings. - -| Name | Type | Zivid API Setting | -|----------------------------------------------------------|--------|----------------------------------------| -| `settings/processing_color_balance_blue` | double | [Settings::Processing::Color::Balance::Blue](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Color_1_1Balance_1_1Blue.html) -| `settings/processing_color_balance_green` | double | [Settings::Processing::Color::Balance::Green](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Color_1_1Balance_1_1Green.html) -| `settings/processing_color_balance_red` | double | [Settings::Processing::Color::Balance::Red](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Color_1_1Balance_1_1Red.html) -| `settings/processing_color_gamma` | double | [Settings::Processing::Color::Gamma](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Color_1_1Gamma.html) -| `settings/processing_filters_cluster_removal_enabled` | bool | [Settings::Processing::Filters::Cluster::Removal::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Cluster_1_1Removal_1_1Enabled.html) -| `settings/processing_filters_cluster_removal_max_neighbor_distance` | double | [Settings::Processing::Filters::Cluster::Removal::MaxNeighborDistance](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Cluster_1_1Removal_1_1MaxNeighborDistance.html) -| `settings/processing_filters_cluster_removal_min_area` | double | [Settings::Processing::Filters::Cluster::Removal::MinArea](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Cluster_1_1Removal_1_1MinArea.html) -| `settings/processing_filters_hole_repair_enabled` | bool | [Settings::Processing::Filters::Hole::Repair::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Hole_1_1Repair_1_1Enabled.html) -| `settings/processing_filters_hole_repair_hole_size` | double | [Settings::Processing::Filters::Hole::Repair::HoleSize](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Hole_1_1Repair_1_1HoleSize.html) -| `settings/processing_filters_hole_repair_strictness` | int | [Settings::Processing::Filters::Hole::Repair::Strictness](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Hole_1_1Repair_1_1Strictness.html) -| `settings/processing_filters_noise_removal_enabled` | bool | [Settings::Processing::Filters::Noise::Removal::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Noise_1_1Removal_1_1Enabled.html) -| `settings/processing_filters_noise_removal_threshold` | double | [Settings::Processing::Filters::Noise::Removal::Threshold](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Noise_1_1Removal_1_1Threshold.html) -| `settings/processing_filters_noise_suppression_enabled` | bool | [Settings::Processing::Filters::Noise::Suppression::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Noise_1_1Suppression_1_1Enabled.html) -| `settings/processing_filters_noise_repair_enabled` | bool | [Settings::Processing::Filters::Noise::Repair::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Noise_1_1Repair_1_1Enabled.html) -| `settings/processing_filters_outlier_removal_enabled` | bool | [Settings::Processing::Filters::Outlier::Removal::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Outlier_1_1Removal_1_1Enabled.html) -| `settings/processing_filters_outlier_removal_threshold` | double | [Settings::Processing::Filters::Outlier::Removal::Threshold](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Outlier_1_1Removal_1_1Threshold.html) -| `settings/processing_filters_reflection_removal_enabled` | bool | [Settings::Processing::Filters::Reflection::Removal::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Reflection_1_1Removal_1_1Enabled.html) -| `settings/processing_filters_reflection_removal_mode` | enum | [Settings::Processing::Filters::Reflection::Removal::Mode](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Reflection_1_1Removal_1_1Mode.html) -| `settings/processing_filters_smoothing_gaussian_enabled` | bool | [Settings::Processing::Filters::Smoothing::Gaussian::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Smoothing_1_1Gaussian_1_1Enabled.html) -| `settings/processing_filters_smoothing_gaussian_sigma` | double | [Settings::Processing::Filters::Smoothing::Gaussian::Sigma](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Smoothing_1_1Gaussian_1_1Sigma.html) -| `settings/processing_resampling` | enum | [Settings::Processing::Resampling](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Resampling.html) - -#### Other settings - -| Name | Type | Zivid API Setting | -|------------------------------------------------------------------------------------|--------|--------------------------------| -| `settings/diagnostics_enabled` | bool | [Settings::Diagnostics::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Diagnostics_1_1Enabled.html) -| `settings/engine` | enum | [Settings::Engine](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Engine.html) -| `settings/sampling_color` | enum | [Settings::Sampling::Color](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Sampling_1_1Color.html) -| `settings/sampling_pixel` | enum | [Settings::Sampling::Pixel](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Sampling_1_1Pixel.html) -| `settings/region_of_interest_box_enabled` | bool | [Settings::RegionOfInterest::Box::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1Enabled.html) -| `settings/region_of_interest_box_extents_min` | double | [Settings::RegionOfInterest::Box::Extents](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1Extents.html) -| `settings/region_of_interest_box_extents_max` | double | [Settings::RegionOfInterest::Box::Extents](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1Extents.html) -| `settings/region_of_interest_box_point_a_x` | double | [Settings::RegionOfInterest::Box::PointA](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointA.html) -| `settings/region_of_interest_box_point_a_y` | double | [Settings::RegionOfInterest::Box::PointA](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointA.html) -| `settings/region_of_interest_box_point_a_z` | double | [Settings::RegionOfInterest::Box::PointA](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointA.html) -| `settings/region_of_interest_box_point_b_x` | double | [Settings::RegionOfInterest::Box::PointB](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointB.html) -| `settings/region_of_interest_box_point_b_y` | double | [Settings::RegionOfInterest::Box::PointB](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointB.html) -| `settings/region_of_interest_box_point_b_z` | double | [Settings::RegionOfInterest::Box::PointB](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointB.html) -| `settings/region_of_interest_box_point_o_x` | double | [Settings::RegionOfInterest::Box::PointO](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointO.html) -| `settings/region_of_interest_box_point_o_y` | double | [Settings::RegionOfInterest::Box::PointO](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointO.html) -| `settings/region_of_interest_box_point_o_z` | double | [Settings::RegionOfInterest::Box::PointO](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Box_1_1PointO.html) -| `settings/region_of_interest_depth_enabled` | bool | [Settings::RegionOfInterest::Depth::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Depth_1_1Enabled.html) -| `settings/region_of_interest_depth_range_min` | double | [Settings::RegionOfInterest::Depth::Range](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Depth_1_1Range.html) -| `settings/region_of_interest_depth_range_max` | double | [Settings::RegionOfInterest::Depth::Range](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1RegionOfInterest_1_1Depth_1_1Range.html) - -#### Experimental settings - -Note that these settings may be changed, renamed or removed in future SDK releases. - -| Name | Type | Zivid API Setting | -|------------------------------------------------------------------------------------|--------|--------------------------------| -| `settings/processing_color_experimental_mode` | enum | [Settings::Processing::Color::Experimental::Mode](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Color_1_1Experimental_1_1Mode.html) -| `settings/processing_filters_experimental_contrast_distortion_correction_enabled` | bool | [Settings::Processing::Filters::Experimental::ContrastDistortion::Correction::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Experimental_1_1ContrastDistortion_1_1Correction_1_1Enabled.html) -| `settings/processing_filters_experimental_contrast_distortion_correction_strength` | double | [Settings::Processing::Filters::Experimental::ContrastDistortion::Correction::Strength](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Experimental_1_1ContrastDistortion_1_1Correction_1_1Strength.html) -| `settings/processing_filters_experimental_contrast_distortion_removal_enabled` | bool | [Settings::Processing::Filters::Experimental::ContrastDistortion::Removal::Enabled](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Experimental_1_1ContrastDistortion_1_1Removal_1_1Enabled.html) -| `settings/processing_filters_experimental_contrast_distortion_removal_threshold` | double | [Settings::Processing::Filters::Experimental::ContrastDistortion::Removal::Threshold](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings_1_1Processing_1_1Filters_1_1Experimental_1_1ContrastDistortion_1_1Removal_1_1Threshold.html) - -### 2D settings - -#### Acquisition settings - -To trigger a 2D capture, invoke the [capture_2d](#capture_2d) service. Note that -`settings_2d/acquisition_0/enabled` is default false, and must be set to true before -calling the [capture_2d](#capture_2d) service, otherwise the service will return an error. - -| Name | Type | Zivid API Setting | Note | -|-------------------------------------------|--------|--------------------------------|--------| -| `settings_2d/acquisition_0/enabled` | bool | -| `settings_2d/acquisition_0/aperture` | double | [Settings2D::Acquisition::Aperture](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Acquisition_1_1Aperture.html) -| `settings_2d/acquisition_0/brightness` | double | [Settings2D::Acquisition::Brightness](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Acquisition_1_1Brightness.html) -| `settings_2d/acquisition_0/exposure_time` | int | [Settings2D::Acquisition::ExposureTime](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Acquisition_1_1ExposureTime.html) | Microseconds -| `settings_2d/acquisition_0/gain` | double | [Settings2D::Acquisition::Gain](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Acquisition_1_1Gain.html) - -#### Processing settings - -`settings_2d/` contains settings related to processing of the captured image. - -| Name | Type | Zivid API Setting | -|----------------------------------------------|--------|----------------------------------------| -| `settings_2d/processing_color_balance_blue` | double | [Settings2D::Processing::Color::Balance::Blue](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Processing_1_1Color_1_1Balance_1_1Blue.html) -| `settings_2d/processing_color_balance_green` | double | [Settings2D::Processing::Color::Balance::Green](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Processing_1_1Color_1_1Balance_1_1Green.html) -| `settings_2d/processing_color_balance_red` | double | [Settings2D::Processing::Color::Balance::Red](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Processing_1_1Color_1_1Balance_1_1Red.html) -| `settings_2d/processing_color_gamma` | double | [Settings2D::Processing::Color::Gamma](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Processing_1_1Color_1_1Gamma.html) - - -#### Other settings - -| Name | Type | Zivid API Setting | -|----------------------------------------------|--------|----------------------------------------| -| `settings_2d/sampling_pixel` | enum | [Settings2D::Sampling::Pixel](https://downloads.zivid.com/sdk/releases/2.13.1+18e79e79-1/doc/cpp/classZivid_1_1Settings2D_1_1Sampling_1_1Pixel.html) +In the `zivid_samples` package we have added samples that demonstrate how to use +the Zivid ROS driver. These samples can be used as a starting point for your project. +To launch the Python samples using `ros2 launch`, you need `python` to be available as a command. +For example, the `python-is-python3` package can be installed to achieve this, by running the +following command: -## Samples +```bash +sudo apt install python-is-python3 +``` -In the `zivid_samples` package we have added samples in C++ and Python that demonstrate how to use -the Zivid ROS driver. These samples can be used as a starting point for your project. +On Windows, the Python samples cannot be launched using `ros2 launch`. Instead, please launch the +samples using `ros2 run zivid_samples .py` together with +`ros2 run zivid_camera zivid_camera` in another terminal window. -### Sample Capture Assistant +### Sample Capture -This sample shows how to use the Capture Assistant to capture with suggested settings for your -particular scene. This sample first calls the -[capture_assistant/suggest_settings](#capture_assistantsuggest_settings) service to get the suggested -settings. It then calls the [capture](#capture) service to invoke the 3D capture using those settings. +This sample performs single-acquisition 3D captures in a loop. This sample shows how to [configure](#configuration) +the capture settings, how to subscribe to the [points/xyzrgba](#pointsxyzrgba) topic, and how to invoke the +[capture](#capture) service. -Source code: [C++](./zivid_samples/src/sample_capture_assistant.cpp), [Python](./zivid_samples/scripts/sample_capture_assistant.py) +Source code: [C++](./zivid_samples/src/sample_capture.cpp) [Python](./zivid_samples/scripts/sample_capture.py) -Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`): ```bash -roslaunch zivid_samples sample.launch type:=sample_capture_assistant_cpp -roslaunch zivid_samples sample.launch type:=sample_capture_assistant.py +ros2 launch zivid_samples sample.launch sample:=sample_capture_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture.py ``` -Using rosrun (when `roscore` and `zivid_camera` are running): +Using ros2 run (when `zivid_camera` node is already running): ```bash -rosrun zivid_samples sample_capture_assistant_cpp -rosrun zivid_samples sample_capture_assistant.py +ros2 run zivid_samples sample_capture_cpp +ros2 run zivid_samples sample_capture.py ``` -### Sample Capture +### Sample Capture 2D -This sample performs single-acquisition 3D captures repeatedly. This sample shows how to [configure](#configuration) -the capture settings, how to subscribe to the [points/xyzrgba](#points/xyzrgba) topic, and how to invoke the -[capture](#capture) service. +This sample performs single-acquisition 2D captures in a loop. This sample shows how to [configure](#configuration) the +capture 2d settings with a YAML string, how to subscribe to the [color/image_color](#colorimage_color) topic, and how to +invoke the [capture_2d](#capture_2d) service. -Source code: [C++](./zivid_samples/src/sample_capture.cpp), [Python](./zivid_samples/scripts/sample_capture.py) +Source code: [C++](./zivid_samples/src/sample_capture_2d.cpp) [Python](./zivid_samples/scripts/sample_capture_2d.py) -Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`): ```bash -roslaunch zivid_samples sample.launch type:=sample_capture_cpp -roslaunch zivid_samples sample.launch type:=sample_capture.py +ros2 launch zivid_samples sample.launch sample:=sample_capture_2d_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_2d.py ``` -Using rosrun (when `roscore` and `zivid_camera` are running): +Using ros2 run (when `zivid_camera` node is already running): ```bash -rosrun zivid_samples sample_capture_cpp -rosrun zivid_samples sample_capture.py +ros2 run zivid_samples sample_capture_2d_cpp +ros2 run zivid_samples sample_capture_2d.py ``` -### Sample Capture 2D +### Sample Capture and Save -This sample performs 2D captures repeatedly. This sample shows how to [configure](#configuration) -the 2D capture settings, how to subscribe to the [color/image_color](#colorimage_color) topic, and -how to invoke the [capture_2d](#capture_2d) service. +This sample performs a capture, and stores the resulting frame to file. This sample shows how to +[configure](#configuration) the capture settings with a YAML string, how to invoke the +[capture_and_save](#capture_and_save) service, and how to read the response from the service call. -Source code: [C++](./zivid_samples/src/sample_capture_2d.cpp), [Python](./zivid_samples/scripts/sample_capture_2d.py) +Source code: [C++](./zivid_samples/src/sample_capture_and_save.cpp) [Python](./zivid_samples/scripts/sample_capture_and_save.py) -Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`): ```bash -roslaunch zivid_samples sample.launch type:=sample_capture_2d_cpp -roslaunch zivid_samples sample.launch type:=sample_capture_2d.py +ros2 launch zivid_samples sample.launch sample:=sample_capture_and_save_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_and_save.py ``` -Using rosrun (when `roscore` and `zivid_camera` are running): + +Using ros2 run (when `zivid_camera` node is already running): + ```bash -rosrun zivid_samples sample_capture_2d_cpp -rosrun zivid_samples sample_capture_2d.py +ros2 run zivid_samples sample_capture_and_save_cpp +ros2 run zivid_samples sample_capture_and_save.py ``` -### Sample Capture with Settings from File - -This sample shows how to [configure](#configuration) the 3D capture settings -by invoking the [load_settings_from_file](#load_settings_from_file) service and -how to invoke the [capture](#capture) service. +### Sample Capture Assistant -:note: -An equivalent service [load_settings_2d_from_file](#load_settings_2d_from_file) exists -for 2D capture settings. +This sample shows how to invoke the [capture_assistant/suggest_settings](#capture_assistantsuggest_settings) service to +suggest and set capture settings. Then, it shows how to subscribe to the [points/xyzrgba](#pointsxyzrgba) and +[color/image_color](#colorimage_color) topics, and finally invoke the [capture](#capture) service. -Source code: [C++](./zivid_samples/src/sample_capture_with_settings_from_yml.cpp), [Python](./zivid_samples/scripts/sample_capture_with_settings_from_yml.py) +Source code: [C++](./zivid_samples/src/sample_capture_assistant.cpp) [Python](./zivid_samples/scripts/sample_capture_assistant.py) -Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`): ```bash -roslaunch zivid_samples sample.launch type:=sample_capture_with_settings_from_yml_cpp -roslaunch zivid_samples sample.launch type:=sample_capture_with_settings_from_yml.py +ros2 launch zivid_samples sample.launch sample:=sample_capture_assistant_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_assistant.py ``` -Using rosrun (when `roscore` and `zivid_camera` are running): +Using ros2 run (when `zivid_camera` node is already running): ```bash -rosrun zivid_samples sample_capture_with_settings_from_yml_cpp -rosrun zivid_samples sample_capture_with_settings_from_yml.py +ros2 run zivid_samples sample_capture_assistant_cpp +ros2 run zivid_samples sample_capture_assistant.py ``` -### Sample Capture and Save +### Sample Capture with Settings from File -This sample performs a single-acquisition 3D capture and saves the frame as ZDF file. -This sample shows how to simply [enable the first default acquisition](#configuration), -and how to invoke the [capture_and_save](#capture_and_save) service. -The file is saved to `/tmp/capture_py.zdf` for the Python sample and -`/tmp/capture_cpp.zdf` for the C++ sample. +This sample performs single-acquisition 3D captures in a loop. This sample shows how to [configure](#configuration) the +capture settings from a yaml file, how to subscribe to the [points/xyzrgba](#pointsxyzrgba) topic, and how to invoke the +[capture](#capture) service. -Source code: [C++](./zivid_samples/src/sample_capture_and_save.cpp), [Python](./zivid_samples/scripts/sample_capture_and_save.py) +Source code: [C++](./zivid_samples/src/sample_capture_with_settings_from_file.cpp) [Python](./zivid_samples/scripts/sample_capture_with_settings_from_file.py) -Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`): ```bash -roslaunch zivid_samples sample.launch type:=sample_capture_and_save_cpp -roslaunch zivid_samples sample.launch type:=sample_capture_and_save.py +ros2 launch zivid_samples sample.launch sample:=sample_capture_with_settings_from_file_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_with_settings_from_file.py ``` -Using rosrun (when `roscore` and `zivid_camera` are running): + +Using ros2 run (when `zivid_camera` node is already running): + ```bash -rosrun zivid_samples sample_capture_and_save_cpp -rosrun zivid_samples sample_capture_and_save.py +ros2 run zivid_samples sample_capture_with_settings_from_file_cpp +ros2 run zivid_samples sample_capture_with_settings_from_file.py ``` ## Frequently Asked Questions @@ -658,25 +450,54 @@ rosrun zivid_samples sample_capture_and_save.py ### How to visualize the output from the camera in rviz ```bash -ROS_NAMESPACE=zivid_camera roslaunch zivid_camera visualize.launch +ros2 launch zivid_camera visualize.launch +``` + +### How to connect to specific Zivid serial number + +```bash +ros2 run zivid_camera zivid_camera --ros-args -p serial_number:=ABCD1234 +``` + +### How to start the driver using settings.yml files + +See section [Configuration](#configuration) for more details. +```bash +ros2 run zivid_camera zivid_camera --ros-args -p settings_file_path:=/path/to/settings.yml -p settings_2d_file_path:=/path/to/settings2D.yml +``` + +### How to trigger 3D/2D capture via terminal + +```bash +ros2 service call /capture std_srvs/srv/Trigger +ros2 service call /capture_2d std_srvs/srv/Trigger +``` + +### How to use a file camera + +```bash +ros2 run zivid_camera zivid_camera --ros-args -p file_camera_path:=/usr/share/Zivid/data/FileCameraZivid2L100.zfc ``` -### How to use multiple cameras +Visit our [knowledgebase](https://support.zivid.com/en/latest/academy/camera/file-camera.html) to download file camera. + +### How to use multiple Zivid cameras You can use multiple Zivid cameras simultaneously by starting one node per camera and specifying -unique namespaces per node: +unique namespaces per node. ```bash -ROS_NAMESPACE=camera1 rosrun zivid_camera zivid_camera_node +ros2 run zivid_camera zivid_camera --ros-args --remap __ns:=/zivid_camera1 ``` ```bash -ROS_NAMESPACE=camera2 rosrun zivid_camera zivid_camera_node +ros2 run zivid_camera zivid_camera --ros-args --remap __ns:=/zivid_camera2 ``` -By default the zivid_camera node will connect to the first available/unused camera. We recommend that -you first start the first node, wait for it to be ready (for example, by waiting for the [capture](#capture) -service to be available), then start the second node. This avoids any race conditions where both nodes +You can combine this with a serial_number parameter (see above) to control which node uses which camera. +By default, the zivid_camera node will connect to the first available (unused) camera. We recommend that +you first start the first node, then wait for it to be ready (for example, by waiting for the [capture](#capture) +service to be available), and then start the second node. This avoids any race conditions where both nodes may try to connect to the same camera at the same time. ### How to run the unit and module tests @@ -693,8 +514,8 @@ sudo mv ./FileCameraZivid2M70.zfc /usr/share/Zivid/data/ Then run the tests: ```bash -cd ~/catkin_ws && source devel/setup.bash -catkin run_tests && catkin_test_results ~/catkin_ws +cd ~/ros2_ws/ && source install/setup.bash +colcon test --event-handlers console_direct+ && colcon test-result --all ``` The tests can also be run via [docker](https://www.docker.com/). See the @@ -703,24 +524,36 @@ The tests can also be run via [docker](https://www.docker.com/). See the ### How to enable debug logging The node logs extra information at log level debug, including the settings used when capturing. -Enable debug logging to troubleshoot issues. +Enable debug logging to troubleshoot issues: ```bash -rosconsole set //zivid_camera ros.zivid_camera debug +ros2 run zivid_camera zivid_camera --ros-args --log-level debug ``` -For example, if ROS_NAMESPACE=zivid_camera, +Above will enable debug logging for all components, you can also specify just the zivid_camera +logger like so: +``` +ros2 run zivid_camera zivid_camera --ros-args --log-level zivid_camera:=debug +``` + +### How to compile the project with warnings enabled ```bash -rosconsole set /zivid_camera/zivid_camera ros.zivid_camera debug +colcon build --cmake-args -DCOMPILER_WARNINGS=ON ``` -### How to compile the project with warnings enabled +### How to format the source code + +The CI test for this package enforces the linting defined by `clang-format`. From the code analysis +docker image, run: ```bash -catkin build -DCOMPILER_WARNINGS=ON +find /host -name '*.cpp' -or -name '*.hpp' | xargs clang-format -i ``` +The style follows the one from +[`ament_clang_format`](https://github.com/ament/ament_lint/blob/master/ament_clang_format/doc/index.rst). + ## License This project is licensed under BSD 3-clause license, see the [LICENSE](LICENSE) file for details. @@ -748,3 +581,4 @@ grant agreement No 732287. For more information, visit [rosin-project.eu](http:/ [install-opencl-drivers-ubuntu-url]: https://support.zivid.com/latest/getting-started/software-installation/gpu/install-opencl-drivers-ubuntu.html [hdr-getting-good-point-clouds-url]: https://support.zivid.com/latest/academy/camera/capturing-high-quality-point-clouds/getting-the-right-exposure-for-good-point-clouds.html [firmware-update-kb-url]: https://support.zivid.com/latest/academy/camera/firmware-update.html +[presets-kb-url]: https://support.zivid.com/en/latest/reference-articles/presets-settings.html diff --git a/continuous-integration/.gitattributes b/continuous-integration/.gitattributes new file mode 100644 index 00000000..5e068a82 --- /dev/null +++ b/continuous-integration/.gitattributes @@ -0,0 +1,2 @@ +# Bash does not play well with Windows line endings. +**.sh text eol=lf diff --git a/continuous-integration/build.sh b/continuous-integration/build.sh index e88a3586..3c7b89ae 100755 --- a/continuous-integration/build.sh +++ b/continuous-integration/build.sh @@ -5,28 +5,34 @@ echo Start ["$(basename $0)"] SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" || exit $? ROOT_DIR=$(realpath "$SCRIPT_DIR/..") || exit $? -echo "Creating catkin workspace" -mkdir -p ~/catkin_ws/src || exit $? -cd ~/catkin_ws || exit $? -catkin build || exit $? +echo "Creating workspace" +mkdir -p ~/ros2_ws/src || exit $? +cd ~/ros2_ws || exit $? echo "Adding link to the source folder" -ln -s "$ROOT_DIR" ~/catkin_ws/src || exit $? +ln -s "$ROOT_DIR" ~/ros2_ws/src || exit $? -for package in zivid_camera zivid_samples -do - echo "Verify that $package is found by catkin list" - catkin list --unformatted | grep -q $package || exit $? -done +if [ "$ROS_DISTRO" != "humble" ]; then + echo "Initializing rosdep" + rosdep init || exit $? +fi echo "Updating rosdep" -rosdep update --rosdistro=$ROS_DISTRO || exit $? +rosdep update || exit $? echo "Installing dependencies" -rosdep install --from-paths src --ignore-src -r -y || exit $? +rosdep install -i --from-path src -y || exit $? echo "Building with compiler=$CI_TEST_COMPILER" -catkin build -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER || exit $? +colcon build --symlink --event-handlers console_direct+ --cmake-args -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || exit $? + +echo "Check that the expected packages are found" +source ~/ros2_ws/install/setup.bash +for package in zivid_camera zivid_interfaces zivid_samples +do + echo "Check that $package is found by ros2 pkg list" + ros2 pkg list | grep -q $package || exit $? +done echo Success! ["$(basename $0)"] diff --git a/continuous-integration/lint.sh b/continuous-integration/lint.sh index 487dbcc0..a825373b 100755 --- a/continuous-integration/lint.sh +++ b/continuous-integration/lint.sh @@ -7,6 +7,7 @@ ROOT_DIR=$(realpath "$SCRIPT_DIR/..") pythonFiles=$(find "$ROOT_DIR" -name '*.py') bashFiles=$(find "$ROOT_DIR" -name '*.sh') +cppFiles=$(find "$ROOT_DIR" -name '*.cpp' -or -name '*.hpp') echo Running black on: echo "$pythonFiles" @@ -16,7 +17,19 @@ echo Running shellcheck on: echo "$bashFiles" shellcheck -x -e SC1090,SC2086,SC2046 $bashFiles || exit $? -echo Running code analysis on C++ code: -$SCRIPT_DIR/lint_cpp.sh || exit $? +echo Running clang-format on: +echo "$cppFiles" +clangFormatIssues=0 +for file in $cppFiles; do + diff_output=$(diff -u "$file" <(clang-format "$file")) + if [ -n "$diff_output" ]; then + echo "$diff_output" + clangFormatIssues=1 + fi +done + +if [ $clangFormatIssues -ne 0 ]; then + exit $clangFormatIssues +fi echo Success! ["$(basename $0)"] diff --git a/continuous-integration/lint_cpp.sh b/continuous-integration/lint_cpp.sh deleted file mode 100755 index 8d749771..00000000 --- a/continuous-integration/lint_cpp.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash - -echo Start ["$(basename $0)"] - -SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" -ROOT_DIR=$(realpath "$SCRIPT_DIR/..") - -cppFiles=$(find "$ROOT_DIR" -name '*.cpp') -hFiles=$(find "$ROOT_DIR" -name '*.h') - -if [ -z "$cppFiles$hFiles" ]; then - echo Error: Cannot find C++ source files - exit 1 -fi - -echo "Checking clang-format conformance" -clang-format --version || exit $? -for fileName in $cppFiles $hFiles; do - echo $fileName - diff $fileName \ - <(clang-format $fileName) \ - || exit $? -done - -echo Success! ["$(basename $0)"] diff --git a/continuous-integration/notify_teams.py b/continuous-integration/notify_teams.py index 9eba7f76..5ec3b909 100644 --- a/continuous-integration/notify_teams.py +++ b/continuous-integration/notify_teams.py @@ -1,3 +1,32 @@ +# Copyright 2024 Zivid AS +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Zivid AS nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + import argparse import os import requests diff --git a/continuous-integration/run_build_and_test_in_docker.sh b/continuous-integration/run_build_and_test_in_docker.sh index c703205c..77ffac0b 100755 --- a/continuous-integration/run_build_and_test_in_docker.sh +++ b/continuous-integration/run_build_and_test_in_docker.sh @@ -26,6 +26,7 @@ docker run \ --workdir /host/continuous-integration \ --env CI_TEST_ZIVID_VERSION="$CI_TEST_ZIVID_VERSION" \ --env CI_TEST_COMPILER="$CI_TEST_COMPILER" \ + --env CI_TEST_OS="$CI_TEST_OS" \ --env CI_TEST_DOWNLOAD_TELICAM="$CI_TEST_DOWNLOAD_TELICAM" \ $CI_TEST_OS \ bash -c "./build_and_test.sh" || exit $? diff --git a/continuous-integration/setup/setup_build_and_test.sh b/continuous-integration/setup/setup_build_and_test.sh index 0e5817b8..a37f4a46 100755 --- a/continuous-integration/setup/setup_build_and_test.sh +++ b/continuous-integration/setup/setup_build_and_test.sh @@ -17,15 +17,11 @@ apt-yes install \ clinfo \ wget \ python3-pip \ + python3-rosdep \ + python3-colcon-common-extensions \ unzip \ || exit $? -if [[ "$UBUNTU_VERSION" == "20.04" ]]; then - apt-yes install python3-catkin-tools python3-osrf-pycommon || exit $? -else - apt-yes install python-catkin-tools || exit $? -fi - function install_opencl_cpu_runtime { # Download the key to system keyring INTEL_KEY_URL=https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB @@ -59,11 +55,8 @@ function install_www_deb { echo "Installing compiler $CI_TEST_COMPILER" if [[ "$CI_TEST_COMPILER" == "g++" || - "$CI_TEST_COMPILER" == "g++-7" || - "$CI_TEST_COMPILER" == "g++-8" || - "$CI_TEST_COMPILER" == "g++-9" || - "$CI_TEST_COMPILER" == "g++-10" || - "$CI_TEST_COMPILER" == "g++-11" + "$CI_TEST_COMPILER" == "g++-12" || + "$CI_TEST_COMPILER" == "g++-13" ]]; then apt-yes install software-properties-common || exit $? @@ -72,12 +65,11 @@ if [[ "$CI_TEST_COMPILER" == "g++" || apt-yes install $CI_TEST_COMPILER || exit $? elif [[ "$CI_TEST_COMPILER" == "clang++" || - "$CI_TEST_COMPILER" == "clang++-7" || - "$CI_TEST_COMPILER" == "clang++-8" || - "$CI_TEST_COMPILER" == "clang++-9" || - "$CI_TEST_COMPILER" == "clang++-10" || - "$CI_TEST_COMPILER" == "clang++-11" || - "$CI_TEST_COMPILER" == "clang++-12" ]]; then + "$CI_TEST_COMPILER" == "clang++-14" || + "$CI_TEST_COMPILER" == "clang++-15" || + "$CI_TEST_COMPILER" == "clang++-16" || + "$CI_TEST_COMPILER" == "clang++-17" || + "$CI_TEST_COMPILER" == "clang++-18" ]]; then apt-yes install ${CI_TEST_COMPILER//\+/} || exit $? @@ -90,7 +82,7 @@ echo "Install Zivid debian packages" ZIVID_RELEASE_DIR="https://downloads.zivid.com/sdk/releases/$CI_TEST_ZIVID_VERSION" -if [[ "$UBUNTU_VERSION" == "20.04" ]]; then +if [[ "$UBUNTU_VERSION" == "20.04" || "$UBUNTU_VERSION" == "22.04" || "$UBUNTU_VERSION" == "24.04" ]]; then if [[ "$CI_TEST_DOWNLOAD_TELICAM" == 1 ]]; then install_www_deb "$ZIVID_RELEASE_DIR/u20/zivid-telicam-driver_3.0.1.1-3_amd64.deb" || exit $? @@ -99,7 +91,7 @@ if [[ "$UBUNTU_VERSION" == "20.04" ]]; then else - echo "Unhandled OS $UBUNTU_VERSION" + echo "Unhandled Ubuntu OS $UBUNTU_VERSION" exit 1 fi diff --git a/continuous-integration/test.sh b/continuous-integration/test.sh index c2cd4092..576b87ac 100755 --- a/continuous-integration/test.sh +++ b/continuous-integration/test.sh @@ -4,7 +4,7 @@ echo Start ["$(basename $0)"] SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" || exit $? -cd ~/catkin_ws || exit $? +cd ~/ros2_ws || exit $? echo "Installing Zivid API config file" install -D "$SCRIPT_DIR"/ZividAPIConfigCPU.yml "$HOME"/.config/Zivid/API/Config.yml || exit $? @@ -16,9 +16,24 @@ unzip ./FileCameraZivid2M70.zip -d /usr/share/Zivid/data/ || exit $? rm ./FileCameraZivid2M70.zip || exit $? echo "Running tests" -catkin run_tests -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER || exit $? + +# We exclude `clang_format` here since it has variations between versions, instead we check it during code analysis. +excludeTests="clang_format" + +if [ "$CI_TEST_OS" == "ros:humble-ros-base-jammy" ] || [ "$CI_TEST_OS" == "ros:iron-ros-core-jammy" ]; then + # The listed OSes have issues invoking clang-tidy correctly: + # - Humble seems to not invoke it with the correct C++ language version, resulting in it not finding + # std::optional and std::variant. + # - Iron does not find the compile commands. + # On the other hand, Jazzy seems to work well, so we get the checks when testing on this OS. + echo "Skipping clang-tidy tests since OS is '$CI_TEST_OS'" + excludeTests+="|clang_tidy" +fi + +export GTEST_BREAK_ON_FAILURE=1; +colcon test --event-handlers console_direct+ --ctest-args tests --exclude-regex $excludeTests --output-on-failure --ros-args --log-level debug || exit $? echo "Check for test errors" -catkin_test_results || exit $? +colcon test-result --all || exit $? echo Success! ["$(basename $0)"] diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..ae51d7b5 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,2 @@ +[tool.black] +skip-string-normalization = true # Black prefers double quotes, however, this is incompatible with the `ament_flake8` configuration. diff --git a/zivid_camera/CMakeLists.txt b/zivid_camera/CMakeLists.txt index 471adf5d..afd433c7 100644 --- a/zivid_camera/CMakeLists.txt +++ b/zivid_camera/CMakeLists.txt @@ -1,9 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(zivid_camera LANGUAGES CXX) -if(${CMAKE_VERSION} VERSION_LESS "3.8.0") - add_compile_options(-std=c++17) -else() +if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() @@ -20,201 +18,77 @@ function(turn_on_compiler_warnings_if_enabled TARGET) endif() endfunction() -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - std_msgs - dynamic_reconfigure - message_generation - image_transport - nodelet -) - -find_package(Boost REQUIRED COMPONENTS filesystem) - -find_package(Zivid 2.5.0 COMPONENTS Core REQUIRED) -message(STATUS "Found Zivid version ${Zivid_VERSION}") - -find_package(OpenMP REQUIRED) -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(zivid_interfaces REQUIRED) -set(SETTINGS_GENERATOR_TARGET_NAME ${PROJECT_NAME}_settings_generator) -add_executable(${SETTINGS_GENERATOR_TARGET_NAME} src/settings_generator.cpp) -target_include_directories( - ${SETTINGS_GENERATOR_TARGET_NAME} - SYSTEM PRIVATE - ${catkin_INCLUDE_DIRS} -) -target_link_libraries(${SETTINGS_GENERATOR_TARGET_NAME} PRIVATE Zivid::Core Boost::filesystem) - -set(GENERATOR_TARGET_NAME ${PROJECT_NAME}_generator) -add_custom_target( - ${GENERATOR_TARGET_NAME} - COMMAND mkdir -p ${CMAKE_CURRENT_BINARY_DIR}/state_srv_files - COMMAND mkdir -p ${CMAKE_CURRENT_BINARY_DIR}/generated_headers - COMMAND ${SETTINGS_GENERATOR_TARGET_NAME} - COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/SettingsAcquisition.cfg - COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Settings.cfg - COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Settings2DAcquisition.cfg - COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Settings2D.cfg - BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/SettingsAcquisition.cfg - ${CMAKE_CURRENT_BINARY_DIR}/Settings.cfg - ${CMAKE_CURRENT_BINARY_DIR}/Settings2DAcquisition.cfg - ${CMAKE_CURRENT_BINARY_DIR}/Settings2D.cfg - ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/SettingsAcquisitionConfigUtils.h - ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/SettingsConfigUtils.h - ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/Settings2DAcquisitionConfigUtils.h - ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/Settings2DConfigUtils.h - COMMENT "Running generator" -) +find_package(Zivid 2.9.0 COMPONENTS Core REQUIRED) +message(STATUS "Found Zivid SDK version ${Zivid_VERSION}") -set(LIBRARY_NAME ${PROJECT_NAME}) +set(ZIVID_DRIVER_COMPONENT_NAME ${PROJECT_NAME}_component) -generate_dynamic_reconfigure_options( - ${CMAKE_CURRENT_BINARY_DIR}/Settings.cfg - ${CMAKE_CURRENT_BINARY_DIR}/SettingsAcquisition.cfg - ${CMAKE_CURRENT_BINARY_DIR}/Settings2D.cfg - ${CMAKE_CURRENT_BINARY_DIR}/Settings2DAcquisition.cfg -) -add_dependencies(${PROJECT_NAME}_gencfg ${GENERATOR_TARGET_NAME}) -add_service_files( - DIRECTORY - srv - FILES - Capture.srv - CaptureAndSave.srv - Capture2D.srv - CaptureAssistantSuggestSettings.srv - LoadSettingsFromFile.srv - LoadSettings2DFromFile.srv - CameraInfoModelName.srv - CameraInfoSerialNumber.srv - IsConnected.srv -) -generate_messages( - DEPENDENCIES - sensor_msgs -) -catkin_package( - INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} - LIBRARIES ${LIBRARY_NAME} - CATKIN_DEPENDS message_runtime sensor_msgs std_msgs nodelet -) - -# The catkin functions above sets directory-level include directories for the current -# directory. But the include directories are not set as SYSTEM. Unset the include -# directories that are set by catkin above, and instead manually add these as SYSTEM -# include directories to each target as needed -set_property(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES "") - -# Library add_library( - ${LIBRARY_NAME} + ${ZIVID_DRIVER_COMPONENT_NAME} + SHARED src/zivid_camera.cpp - src/capture_settings_controller.cpp -) -turn_on_compiler_warnings_if_enabled(${LIBRARY_NAME}) -target_include_directories( - ${LIBRARY_NAME} - PRIVATE - include - ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/ -) -target_include_directories( - ${LIBRARY_NAME} - SYSTEM PRIVATE - ${catkin_INCLUDE_DIRS} - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION} -) -target_compile_definitions( - ${LIBRARY_NAME} - PRIVATE - "ZIVID_ROS_DRIVER_VERSION=\"${${PROJECT_NAME}_VERSION}\"" -) -target_link_libraries(${LIBRARY_NAME} PUBLIC ${catkin_LIBRARIES}) -target_link_libraries(${LIBRARY_NAME} PRIVATE Zivid::Core) -add_dependencies( - ${LIBRARY_NAME} - ${PROJECT_NAME}_gencfg - ${catkin_EXPORTED_TARGETS} - ${PROJECT_NAME}_generate_messages_cpp ) -# Node -set(NODE_NAME ${PROJECT_NAME}_node) -add_executable(${NODE_NAME} src/node.cpp) -turn_on_compiler_warnings_if_enabled(${NODE_NAME}) -target_include_directories(${NODE_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) -target_link_libraries(${NODE_NAME} ${LIBRARY_NAME} ${catkin_LIBRARIES}) -add_dependencies( - ${NODE_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${LIBRARY_NAME} -) +turn_on_compiler_warnings_if_enabled(${ZIVID_DRIVER_COMPONENT_NAME}) +target_include_directories(${ZIVID_DRIVER_COMPONENT_NAME} PUBLIC include) -# Nodelet -set(NODELET_NAME ${PROJECT_NAME}_nodelet) -add_library(${NODELET_NAME} src/nodelet.cpp) -turn_on_compiler_warnings_if_enabled(${NODELET_NAME}) -target_include_directories( - ${NODELET_NAME} - SYSTEM PRIVATE - ${catkin_INCLUDE_DIRS} - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION} -) -target_include_directories( - ${NODELET_NAME} - PRIVATE - include -) -target_link_libraries(${NODELET_NAME} ${LIBRARY_NAME} ${catkin_LIBRARIES}) -add_dependencies( - ${NODELET_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ${LIBRARY_NAME} -) +rclcpp_components_register_node(${ZIVID_DRIVER_COMPONENT_NAME} + PLUGIN + "zivid_camera::ZividCamera" + EXECUTABLE + "zivid_camera") + +target_link_libraries(${ZIVID_DRIVER_COMPONENT_NAME} Zivid::Core) +ament_target_dependencies(${ZIVID_DRIVER_COMPONENT_NAME} + rclcpp_components + std_msgs + std_srvs + sensor_msgs + image_transport + zivid_interfaces) + +target_compile_definitions(${ZIVID_DRIVER_COMPONENT_NAME} PRIVATE "ZIVID_CAMERA_ROS_EXPORTING") ############# ## Install ## ############# -install( - TARGETS ${LIBRARY_NAME} ${NODE_NAME} ${NODELET_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +install(TARGETS ${ZIVID_DRIVER_COMPONENT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) -install( - FILES nodelets.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY include DESTINATION include) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) -install( - DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) +if(BUILD_TESTING) -############# -## Testing ## -############# + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() -if(CATKIN_ENABLE_TESTING) - - find_package(rostest REQUIRED) + find_package(ament_cmake_gtest REQUIRED) set(TEST_TARGET_NAME ${PROJECT_NAME}_test) - add_executable(${TEST_TARGET_NAME} EXCLUDE_FROM_ALL test/test_zivid_camera.cpp) - turn_on_compiler_warnings_if_enabled(${TEST_TARGET_NAME}) - target_include_directories( - ${TEST_TARGET_NAME} - SYSTEM PRIVATE - ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION} - ${catkin_INCLUDE_DIRS} + + ament_add_gtest(${TEST_TARGET_NAME} test/test_zivid_camera.cpp TIMEOUT 480) + ament_target_dependencies(${TEST_TARGET_NAME} + rclcpp + std_msgs + sensor_msgs + image_transport + zivid_interfaces ) - target_link_libraries(${TEST_TARGET_NAME} ${LIBRARY_NAME} ${GTEST_LIBRARIES} Zivid::Core ${catkin_LIBRARIES}) - add_rostest(test/test_zivid_camera.test DEPENDENCIES ${TEST_TARGET_NAME}) + target_link_libraries(${TEST_TARGET_NAME} Zivid::Core ${ZIVID_DRIVER_COMPONENT_NAME}) endif() + +ament_package() \ No newline at end of file diff --git a/zivid_camera/cmake/CompilerWarnings.cmake b/zivid_camera/cmake/CompilerWarnings.cmake index 29bd9e04..d849988f 100644 --- a/zivid_camera/cmake/CompilerWarnings.cmake +++ b/zivid_camera/cmake/CompilerWarnings.cmake @@ -1,3 +1,31 @@ +# Copyright 2024 Zivid AS +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Zivid AS nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + function(set_target_warning_compile_options TARGET) if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") @@ -10,10 +38,8 @@ function(set_target_warning_compile_options TARGET) weak-vtables # The vtable must be duplicated in multiple translation units. Small # problem, maybe even linker will resolve this. Must add boilerplate to # fix, not worth it + covered-switch-default # We don't want this warning, because we want the default labels for safety. ) - if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 7.0) - list(APPEND WARNINGS_THAT_SHOULD_BE_IGNORED return-std-move-in-c++11) # Applies to old compilers, we aim for newer C++17 compilers - endif() foreach(WARNING ${WARNINGS_THAT_SHOULD_BE_IGNORED}) list(APPEND TARGET_FLAGS -Wno-${WARNING}) @@ -26,6 +52,11 @@ function(set_target_warning_compile_options TARGET) set(TARGET_FLAGS -Wall -Wextra -Werror -pedantic) target_compile_options(${TARGET} PRIVATE ${TARGET_FLAGS}) + elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") + + set(TARGET_FLAGS /W4 /WX /permissive-) + target_compile_options(${TARGET} PRIVATE ${TARGET_FLAGS}) + else() message(FATAL_ERROR "Unhandled compiler vendor ${CMAKE_CXX_COMPILER_ID}") endif() diff --git a/zivid_camera/config/rviz/zivid.rviz b/zivid_camera/config/rviz/zivid.rviz new file mode 100644 index 00000000..cdc649c1 --- /dev/null +++ b/zivid_camera/config/rviz/zivid.rviz @@ -0,0 +1,202 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /PointCloud21/Topic1 + - /Image1 + - /Image2 + - /Image2/Topic1 + Splitter Ratio: 0.5 + Tree Height: 359 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /points/xyzrgba + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /depth/image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /color/image_color + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: zivid_optical_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.2100631147623062 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.03964072838425636 + Y: 0.002829844132065773 + Z: -7.250940825542784e-07 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: -1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.735422134399414 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002b6000004c2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000002e200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001f2000000c900fffffffb0000000a0049006d00610067006501000002350000015e0000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000399000001660000002800ffffff000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005e9000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 70 + Y: 765 diff --git a/zivid_camera/include/auto_generated_include_wrapper.h b/zivid_camera/include/auto_generated_include_wrapper.h deleted file mode 100644 index 8b8f39f9..00000000 --- a/zivid_camera/include/auto_generated_include_wrapper.h +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#ifdef __clang__ -#pragma clang system_header -#endif -#if defined(__GNUC__) && !defined(__clang__) -#pragma GCC system_header -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include diff --git a/zivid_camera/include/capture_settings_controller.h b/zivid_camera/include/capture_settings_controller.h deleted file mode 100644 index 278454ee..00000000 --- a/zivid_camera/include/capture_settings_controller.h +++ /dev/null @@ -1,69 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "auto_generated_include_wrapper.h" - -namespace Zivid -{ -class Camera; -} - -namespace zivid_camera -{ -template -class ConfigDRServer; - -/// Controller that manages dynamic_reconfigure nodes for Settings and Settings2D -/// -/// This is a templated class that handles Zivid::Settings and Zivid::Settings2D configuration. Note that -/// within the Zivid SDK Settings and Settings2D are similar in structure, but there are some differences -/// in what kind of settings are provided. -/// -/// This controller sets up the dynamic_reconfigure nodes "settings/", "settings/acquisition_0" and so -/// on, handles callbacks and updates the internal config state. It also provides methods to convert -/// from/to Zivid::Settings/Settings2D objects. -/// -template < - typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> -class CaptureSettingsController -{ - static_assert( - std::is_same_v || - std::is_same_v); - static_assert( - std::is_same_v || - std::is_same_v); - static_assert( - std::is_same_v || - std::is_same_v); - -public: - CaptureSettingsController( - ros::NodeHandle & nh, Zivid::Camera & camera, const std::string & config_node_name, - std::size_t num_acquisition_servers); - ~CaptureSettingsController(); - ZividSettingsType zividSettings() const; - void setZividSettings(const ZividSettingsType & settings); - std::size_t numAcquisitionConfigServers() const; - -private: - using SettingsConfigTypeDRServer = ConfigDRServer; - using SettingsAcquisitionConfigTypeDRServer = - ConfigDRServer; - std::string config_node_name_; - Zivid::CameraInfo camera_info_; - std::unique_ptr general_config_dr_server_; - std::vector> - acquisition_config_dr_servers_; -}; - -extern template class CaptureSettingsController< - Zivid::Settings, zivid_camera::SettingsConfig, zivid_camera::SettingsAcquisitionConfig>; -extern template class CaptureSettingsController< - Zivid::Settings2D, zivid_camera::Settings2DConfig, zivid_camera::Settings2DAcquisitionConfig>; - -} // namespace zivid_camera diff --git a/zivid_camera/include/config_utils_common.h b/zivid_camera/include/config_utils_common.h deleted file mode 100644 index 1ed38d05..00000000 --- a/zivid_camera/include/config_utils_common.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -// Helper template functions that convert from a ZividSettings object to current/min/max -// Config object. The explicit specializations of these template functions are auto-generated -// during the build. - -template -ConfigType zividSettingsToConfig(const ZividSettings & s) = delete; - -template -ConfigType zividSettingsMinConfig(const Zivid::Camera & camera) = delete; - -template -ConfigType zividSettingsMaxConfig(const Zivid::Camera & camera) = delete; diff --git a/zivid_camera/include/nodelet.h b/zivid_camera/include/nodelet.h deleted file mode 100644 index 885447f8..00000000 --- a/zivid_camera/include/nodelet.h +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include - -#include - -namespace zivid_camera -{ -class ZividCamera; -class ZividNodelet : public nodelet::Nodelet -{ -private: - void onInit() override; - std::unique_ptr camera; -}; - -} // namespace zivid_camera diff --git a/zivid_camera/include/zivid_camera.h b/zivid_camera/include/zivid_camera.h deleted file mode 100644 index 4b22165c..00000000 --- a/zivid_camera/include/zivid_camera.h +++ /dev/null @@ -1,121 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "auto_generated_include_wrapper.h" -#include "capture_settings_controller.h" - -namespace Zivid -{ -class Settings; -} - -namespace zivid_camera -{ -enum class CameraStatus -{ - Idle, - Connected, - Disconnected -}; - -class ZividCamera -{ -public: - ZividCamera(ros::NodeHandle & nh, ros::NodeHandle & priv); - -private: - void onCameraConnectionKeepAliveTimeout(const ros::TimerEvent & event); - void reconnectToCameraIfNecessary(); - void setCameraStatus(CameraStatus camera_status); - bool cameraInfoModelNameServiceHandler( - CameraInfoModelName::Request & req, CameraInfoModelName::Response & res); - bool cameraInfoSerialNumberServiceHandler( - CameraInfoSerialNumber::Request & req, CameraInfoSerialNumber::Response & res); - bool captureServiceHandler(Capture::Request & req, Capture::Response & res); - bool captureAndSaveServiceHandler(CaptureAndSave::Request & req, CaptureAndSave::Response & res); - bool capture2DServiceHandler(Capture2D::Request & req, Capture2D::Response & res); - bool captureAssistantSuggestSettingsServiceHandler( - CaptureAssistantSuggestSettings::Request & req, - CaptureAssistantSuggestSettings::Response & res); - bool loadSettingsFromFileServiceHandler( - LoadSettingsFromFile::Request & req, LoadSettingsFromFile::Response &); - bool loadSettings2DFromFileServiceHandler( - LoadSettings2DFromFile::Request & req, LoadSettings2DFromFile::Response &); - void serviceHandlerHandleCameraConnectionLoss(); - bool isConnectedServiceHandler(IsConnected::Request & req, IsConnected::Response & res); - void publishFrame(const Zivid::Frame & frame); - Zivid::Frame invokeCaptureAndPublishFrame(); - bool shouldPublishPointsXYZ() const; - bool shouldPublishPointsXYZRGBA() const; - bool shouldPublishColorImg() const; - bool shouldPublishDepthImg() const; - bool shouldPublishSnrImg() const; - bool shouldPublishNormalsXYZ() const; - std_msgs::Header makeHeader(); - void publishPointCloudXYZ(const std_msgs::Header & header, const Zivid::PointCloud & point_cloud); - void publishPointCloudXYZRGBA( - const std_msgs::Header & header, const Zivid::PointCloud & point_cloud); - void publishColorImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, - const Zivid::PointCloud & point_cloud); - void publishColorImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, - const Zivid::Image & image); - void publishDepthImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, - const Zivid::PointCloud & point_cloud); - void publishSnrImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, - const Zivid::PointCloud & point_cloud); - void publishNormalsXYZ(const std_msgs::Header & header, const Zivid::PointCloud & point_cloud); - sensor_msgs::CameraInfoConstPtr makeCameraInfo( - const std_msgs::Header & header, std::size_t width, std::size_t height, - const Zivid::CameraIntrinsics & intrinsics); - - using Capture3DSettingsController = - CaptureSettingsController; - using Capture2DSettingsController = - CaptureSettingsController; - - ros::NodeHandle nh_; - ros::NodeHandle priv_; - ros::Timer camera_connection_keepalive_timer_; - CameraStatus camera_status_; - bool use_latched_publisher_for_points_xyz_; - bool use_latched_publisher_for_points_xyzrgba_; - bool use_latched_publisher_for_color_image_; - bool use_latched_publisher_for_depth_image_; - bool use_latched_publisher_for_snr_image_; - bool use_latched_publisher_for_normals_xyz_; - ros::Publisher points_xyz_publisher_; - ros::Publisher points_xyzrgba_publisher_; - image_transport::ImageTransport image_transport_; - image_transport::CameraPublisher color_image_publisher_; - image_transport::CameraPublisher depth_image_publisher_; - image_transport::CameraPublisher snr_image_publisher_; - ros::Publisher normals_xyz_publisher_; - ros::ServiceServer camera_info_serial_number_service_; - ros::ServiceServer camera_info_model_name_service_; - ros::ServiceServer capture_service_; - ros::ServiceServer capture_and_save_service_; - ros::ServiceServer capture_2d_service_; - ros::ServiceServer capture_assistant_suggest_settings_service_; - ros::ServiceServer load_settings_from_file_service_; - ros::ServiceServer load_settings_2d_from_file_service_; - ros::ServiceServer is_connected_service_; - std::unique_ptr capture_settings_controller_; - std::unique_ptr capture_2d_settings_controller_; - Zivid::Application zivid_; - Zivid::Camera camera_; - std::string frame_id_; - unsigned int header_seq_; -}; -} // namespace zivid_camera diff --git a/zivid_camera/include/zivid_camera/visibility.hpp b/zivid_camera/include/zivid_camera/visibility.hpp new file mode 100644 index 00000000..4ffd03d9 --- /dev/null +++ b/zivid_camera/include/zivid_camera/visibility.hpp @@ -0,0 +1,68 @@ +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + +#ifdef __GNUC__ +#define ZIVID_CAMERA_ROS_EXPORT __attribute__((dllexport)) +#define ZIVID_CAMERA_ROS_IMPORT __attribute__((dllimport)) +#else +#define ZIVID_CAMERA_ROS_EXPORT __declspec(dllexport) +#define ZIVID_CAMERA_ROS_IMPORT __declspec(dllimport) +#endif + +#ifdef ZIVID_CAMERA_ROS_EXPORTING +#define ZIVID_CAMERA_ROS_PUBLIC ZIVID_CAMERA_ROS_EXPORT +#else +#define ZIVID_CAMERA_ROS_PUBLIC ZIVID_CAMERA_ROS_IMPORT +#endif + +#define ZIVID_CAMERA_ROS_PUBLIC_TYPE ZIVID_CAMERA_ROS_PUBLIC + +#define ZIVID_CAMERA_ROS_LOCAL + +#else + +#define ZIVID_CAMERA_ROS_EXPORT __attribute__((visibility("default"))) +#define ZIVID_CAMERA_ROS_IMPORT + +#if __GNUC__ >= 4 +#define ZIVID_CAMERA_ROS_PUBLIC __attribute__((visibility("default"))) +#define ZIVID_CAMERA_ROS_LOCAL __attribute__((visibility("hidden"))) +#else +#define ZIVID_CAMERA_ROS_PUBLIC +#define ZIVID_CAMERA_ROS_LOCAL +#endif + +#define ZIVID_CAMERA_ROS_PUBLIC_TYPE +#endif diff --git a/zivid_camera/include/zivid_camera/zivid_camera.hpp b/zivid_camera/include/zivid_camera/zivid_camera.hpp new file mode 100644 index 00000000..2f58093e --- /dev/null +++ b/zivid_camera/include/zivid_camera/zivid_camera.hpp @@ -0,0 +1,180 @@ +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Zivid +{ +class Application; +class Camera; +class CameraIntrinsics; +struct ColorRGBA; +class Frame; +template +class Image; +class PointCloud; +class Settings2D; +class Settings; +} // namespace Zivid + +namespace zivid_camera +{ +enum class CameraStatus +{ + Idle, + Connected, + Disconnected +}; +template +class CaptureSettingsController; + +class ZividCamera : public rclcpp::Node +{ +public: + ZIVID_CAMERA_ROS_PUBLIC ZividCamera(const rclcpp::NodeOptions & options); + ~ZividCamera() override; + ZIVID_CAMERA_ROS_PUBLIC Zivid::Application & zividApplication(); + +private: + void onCameraConnectionKeepAliveTimeout(); + void reconnectToCameraIfNecessary(); + void setCameraStatus(CameraStatus camera_status); + rcl_interfaces::msg::SetParametersResult setParametersCallback( + const std::vector & parameters); + void cameraInfoModelNameServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void cameraInfoSerialNumberServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void captureServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void captureAndSaveServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void capture2DServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void captureAssistantSuggestSettingsServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void serviceHandlerHandleCameraConnectionLoss(); + void isConnectedServiceHandler( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void publishFrame(const Zivid::Frame & frame); + Zivid::Frame invokeCaptureAndPublishFrame(const Zivid::Settings & settings); + bool shouldPublishPointsXYZ() const; + bool shouldPublishPointsXYZRGBA() const; + bool shouldPublishColorImg() const; + bool shouldPublishDepthImg() const; + bool shouldPublishSnrImg() const; + bool shouldPublishNormalsXYZ() const; + std_msgs::msg::Header makeHeader(); + void publishPointCloudXYZ( + const std_msgs::msg::Header & header, const Zivid::PointCloud & point_cloud); + void publishPointCloudXYZRGBA( + const std_msgs::msg::Header & header, const Zivid::PointCloud & point_cloud); + void publishColorImage( + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, + const Zivid::PointCloud & point_cloud); + void publishColorImage( + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, + const Zivid::Image & image); + void publishDepthImage( + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, + const Zivid::PointCloud & point_cloud); + void publishSnrImage( + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, + const Zivid::PointCloud & point_cloud); + void publishNormalsXYZ( + const std_msgs::msg::Header & header, const Zivid::PointCloud & point_cloud); + sensor_msgs::msg::CameraInfo::ConstSharedPtr makeCameraInfo( + const std_msgs::msg::Header & header, std::size_t width, std::size_t height, + const Zivid::CameraIntrinsics & intrinsics); + [[noreturn]] void logErrorAndThrowRuntimeException(const std::string & message); + + OnSetParametersCallbackHandle::SharedPtr set_parameters_callback_handle_; + rclcpp::TimerBase::SharedPtr camera_connection_keepalive_timer_; + bool use_latched_publisher_for_points_xyz_{false}; + bool use_latched_publisher_for_points_xyzrgba_{false}; + bool use_latched_publisher_for_color_image_{false}; + bool use_latched_publisher_for_depth_image_{false}; + bool use_latched_publisher_for_snr_image_{false}; + bool use_latched_publisher_for_normals_xyz_{false}; + rclcpp::Publisher::SharedPtr points_xyz_publisher_; + rclcpp::Publisher::SharedPtr points_xyzrgba_publisher_; + image_transport::CameraPublisher color_image_publisher_; + image_transport::CameraPublisher depth_image_publisher_; + image_transport::CameraPublisher snr_image_publisher_; + rclcpp::Publisher::SharedPtr normals_xyz_publisher_; + rclcpp::Service::SharedPtr + camera_info_serial_number_service_; + rclcpp::Service::SharedPtr + camera_info_model_name_service_; + rclcpp::Service::SharedPtr capture_service_; + rclcpp::Service::SharedPtr capture_and_save_service_; + rclcpp::Service::SharedPtr capture_2d_service_; + rclcpp::Service::SharedPtr + capture_assistant_suggest_settings_service_; + rclcpp::Service::SharedPtr is_connected_service_; + + std::unique_ptr zivid_; + CameraStatus camera_status_{CameraStatus::Idle}; + std::unique_ptr> settings_controller_; + std::unique_ptr> settings_2d_controller_; + std::unique_ptr camera_; + std::string frame_id_; +}; +} // namespace zivid_camera diff --git a/zivid_camera/launch/visualize.launch b/zivid_camera/launch/visualize.launch index dd63ec07..2c1d8238 100644 --- a/zivid_camera/launch/visualize.launch +++ b/zivid_camera/launch/visualize.launch @@ -1,3 +1,3 @@ - - + + \ No newline at end of file diff --git a/zivid_camera/nodelets.xml b/zivid_camera/nodelets.xml deleted file mode 100644 index 69f8577f..00000000 --- a/zivid_camera/nodelets.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Nodelet for Zivid camera. - - - diff --git a/zivid_camera/package.xml b/zivid_camera/package.xml index 688bad88..424a9dc1 100644 --- a/zivid_camera/package.xml +++ b/zivid_camera/package.xml @@ -1,37 +1,37 @@ - + + zivid_camera - 2.5.0 - Driver for using the Zivid 3D cameras in ROS. + 3.0.0 + Driver for using the Zivid 3D cameras in ROS 2. Zivid BSD3 https://zivid.com https://github.com/zivid/zivid-ros https://github.com/zivid/zivid-ros/issues - Zivid - catkin - roscpp - sensor_msgs - std_msgs - dynamic_reconfigure - message_generation - rostest - image_transport - nodelet - roscpp - sensor_msgs - std_msgs - dynamic_reconfigure - image_transport - roscpp - sensor_msgs - std_msgs - dynamic_reconfigure - message_runtime - image_transport - nodelet - rosunit + ament_cmake + rclcpp + rclcpp_components + sensor_msgs + std_msgs + std_srvs + image_transport + builtin_interfaces + zivid_interfaces + ros2launch + + ament_cmake_gtest + ament_lint_auto + ament_cmake_clang_format + ament_cmake_clang_tidy + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + - + ament_cmake diff --git a/zivid_camera/rviz/camera_view.rviz b/zivid_camera/rviz/camera_view.rviz deleted file mode 100644 index 9da1d113..00000000 --- a/zivid_camera/rviz/camera_view.rviz +++ /dev/null @@ -1,185 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 363 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Image - - Class: rviz/Displays - Help Height: 70 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /PointCloud21 - Splitter Ratio: 0.5 - Tree Height: 204 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz/Image - Enabled: true - Image Topic: depth/image - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Class: rviz/Image - Enabled: true - Image Topic: color/image_color - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.0005000000237487257 - Style: Spheres - Topic: points/xyzrgba - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: zivid_optical_frame - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 0.9563431739807129 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.016968511044979095 - Y: 0.07087600231170654 - Z: 0.8280874490737915 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: -1.5697963237762451 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.6835784912109375 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1412 - Hide Left Dock: false - Hide Right Dock: false - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002b80000052afc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000001a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003d000001c10000001600fffffffb0000000a0049006d00610067006501000002040000020e0000001600fffffffb000000100044006900730070006c00610079007301000004180000014f000000c900fffffffb0000000a0049006d00610067006501000001930000013e0000000000000000fb0000000a0049006d00610067006501000002d7000001000000000000000000000000010000010f0000052afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000052a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065000000000000000a00000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005ea0000052a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2493 - X: 2627 - Y: 0 diff --git a/zivid_camera/src/capture_settings_controller.cpp b/zivid_camera/src/capture_settings_controller.cpp deleted file mode 100644 index e8fcd174..00000000 --- a/zivid_camera/src/capture_settings_controller.cpp +++ /dev/null @@ -1,199 +0,0 @@ -#include "capture_settings_controller.h" - -#include -#include -#include - -#include "Settings2DAcquisitionConfigUtils.h" -#include "Settings2DConfigUtils.h" -#include "SettingsAcquisitionConfigUtils.h" -#include "SettingsConfigUtils.h" - -namespace -{ -template -void recursivelyFillInUnsetWithCameraDefault(Node & node, const Zivid::CameraInfo & cameraInfo) -{ - if constexpr ( - Node::nodeType == Zivid::DataModel::NodeType::group || - Node::nodeType == Zivid::DataModel::NodeType::leafDataModelList) { - node.forEach( - [&cameraInfo](auto & child) { recursivelyFillInUnsetWithCameraDefault(child, cameraInfo); }); - } else if (!node.hasValue()) { - static_assert(Node::nodeType == Zivid::DataModel::NodeType::leafValue); - node = Zivid::Experimental::SettingsInfo::defaultValue(cameraInfo); - } -} - -template -ZividSettingsType fillInUnsetWithCameraDefault( - const ZividSettingsType & settings, const Zivid::CameraInfo & cameraInfo) -{ - ZividSettingsType copy{settings}; - recursivelyFillInUnsetWithCameraDefault(copy, cameraInfo); - return copy; -} - -} // namespace - -namespace zivid_camera -{ -template -class ConfigDRServer -{ -public: - ConfigDRServer(const std::string & name, ros::NodeHandle & nh, const Zivid::Camera & camera) - : name_(name), - dr_server_(dr_server_mutex_, ros::NodeHandle(nh, name_)), - config_(ConfigType::__getDefault__()) - { - static_assert( - std::is_same_v || - std::is_same_v || - std::is_same_v || - std::is_same_v); - - const auto config_min = zividSettingsMinConfig(camera); - dr_server_.setConfigMin(config_min); - - const auto config_max = zividSettingsMaxConfig(camera); - dr_server_.setConfigMax(config_max); - - const auto config_default = zividSettingsToConfig( - Zivid::Experimental::SettingsInfo::defaultValue(camera.info())); - dr_server_.setConfigDefault(config_default); - setConfig(config_default); - - auto cb = [this](const ConfigType & config, uint32_t /*level*/) { - ROS_INFO("Configuration '%s' changed", name_.c_str()); - config_ = config; - }; - using CallbackType = typename decltype(dr_server_)::CallbackType; - dr_server_.setCallback(CallbackType(cb)); - } - - void setConfig(const ConfigType & cfg) - { - config_ = cfg; - dr_server_.updateConfig(config_); - } - - const ConfigType & config() const { return config_; } - - const std::string & name() const { return name_; } - -private: - std::string name_; - boost::recursive_mutex dr_server_mutex_; - dynamic_reconfigure::Server dr_server_; - ConfigType config_; -}; - -template < - typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> -CaptureSettingsController:: - CaptureSettingsController( - ros::NodeHandle & nh, Zivid::Camera & camera, const std::string & config_node_name, - std::size_t num_acquisition_servers) -: config_node_name_{config_node_name}, camera_info_{camera.info()} -{ - general_config_dr_server_ = - std::make_unique(config_node_name_, nh, camera); - - ROS_INFO( - "Setting up %ld %s/acquisition_ dynamic_reconfigure servers", num_acquisition_servers, - config_node_name_.c_str()); - for (std::size_t i = 0; i < num_acquisition_servers; i++) { - acquisition_config_dr_servers_.push_back( - std::make_unique( - config_node_name_ + "/acquisition_" + std::to_string(i), nh, camera)); - } -} - -template < - typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> -CaptureSettingsController:: - ~CaptureSettingsController() = default; - -template < - typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> -ZividSettingsType CaptureSettingsController< - ZividSettingsType, SettingsConfigType, SettingsAcquisitionConfigType>::zividSettings() const -{ - ZividSettingsType settings; - applyConfigToZividSettings(general_config_dr_server_->config(), settings); - - for (const auto & dr_config_server : acquisition_config_dr_servers_) { - if (dr_config_server->config().enabled) { - ROS_DEBUG("Config %s is enabled", dr_config_server->name().c_str()); - typename ZividSettingsType::Acquisition acquisition; - applyConfigToZividSettings(dr_config_server->config(), acquisition); - settings.acquisitions().emplaceBack(std::move(acquisition)); - } - } - return settings; -} - -template < - typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> -void CaptureSettingsController< - ZividSettingsType, SettingsConfigType, - SettingsAcquisitionConfigType>::setZividSettings(const ZividSettingsType & settings) -{ - const auto numAcquisitions = settings.acquisitions().size(); - if (numAcquisitions == 0) { - throw std::runtime_error("At least one Acquisition must be provided"); - } - - if (numAcquisitions > numAcquisitionConfigServers()) { - std::stringstream error; - error << "The number of acquisitions (" + std::to_string(numAcquisitions) + ") " - << "is larger than the number of dynamic_reconfigure " + config_node_name_ + - "/acquisition_ servers (" - << std::to_string(numAcquisitionConfigServers()) << "). "; - if constexpr (std::is_same_v) { - error << "Increase launch parameter max_capture_acquisitions. " - << "See README.md for more information."; - } else if constexpr (std::is_same_v) { - error << "In 2D mode only a single acquisiton is supported."; - } - throw std::runtime_error(error.str()); - } - - const auto filledSettings = fillInUnsetWithCameraDefault(settings, camera_info_); - ROS_DEBUG_STREAM("Updating settings to " << filledSettings); - general_config_dr_server_->setConfig(zividSettingsToConfig(filledSettings)); - - const auto & acquisitions = filledSettings.acquisitions(); - for (std::size_t i = 0; i < acquisitions.size(); i++) { - auto config = zividSettingsToConfig(acquisitions.at(i)); - config.enabled = true; - acquisition_config_dr_servers_[i]->setConfig(config); - } - - // Remaining acquisitions that are enabled must be disabled - for (std::size_t i = acquisitions.size(); i < acquisition_config_dr_servers_.size(); i++) { - if (acquisition_config_dr_servers_[i]->config().enabled) { - ROS_INFO_STREAM("Acquisition " << i << " was enabled, so disabling it"); - auto config = acquisition_config_dr_servers_[i]->config(); - config.enabled = false; - acquisition_config_dr_servers_[i]->setConfig(config); - } - } -} - -template < - typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType> -std::size_t CaptureSettingsController< - ZividSettingsType, SettingsConfigType, - SettingsAcquisitionConfigType>::numAcquisitionConfigServers() const -{ - return acquisition_config_dr_servers_.size(); -} - -template class CaptureSettingsController< - Zivid::Settings, zivid_camera::SettingsConfig, zivid_camera::SettingsAcquisitionConfig>; -template class CaptureSettingsController< - Zivid::Settings2D, zivid_camera::Settings2DConfig, zivid_camera::Settings2DAcquisitionConfig>; - -} // namespace zivid_camera diff --git a/zivid_camera/src/node.cpp b/zivid_camera/src/node.cpp deleted file mode 100644 index 7eb16935..00000000 --- a/zivid_camera/src/node.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include - -#include - -int main(int argc, char ** argv) -{ - try { - ros::init(argc, argv, "zivid_camera"); - } catch (const std::exception & e) { - std::cerr << "Failed to initialize ROS: " << e.what() << std::endl; - return EXIT_FAILURE; - } catch (...) { - std::cerr << "Failed to initialize ROS (unknown exception)" << std::endl; - return EXIT_FAILURE; - } - - try { - ROS_INFO("Creating a nodelet::Loader"); - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - - const auto nodelet_name = "zivid_camera/nodelet"; - ROS_INFO("Loading nodelet '%s'", nodelet_name); - - const bool loaded = nodelet.load(ros::this_node::getName(), nodelet_name, remap, nargv); - if (!loaded) { - ROS_FATAL("Failed to load nodelet '%s'!", nodelet_name); - return EXIT_FAILURE; - } - - ROS_INFO("Successfully loaded nodelet '%s'", nodelet_name); - ros::spin(); - return EXIT_SUCCESS; - } catch (const std::exception & e) { - std::cerr << "Exception occurred: " << e.what() << std::endl; - return EXIT_FAILURE; - } catch (...) { - std::cerr << "Unknown exception occurred" << std::endl; - return EXIT_FAILURE; - } -} diff --git a/zivid_camera/src/nodelet.cpp b/zivid_camera/src/nodelet.cpp deleted file mode 100644 index 90f27f3f..00000000 --- a/zivid_camera/src/nodelet.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "nodelet.h" - -#include -#include - -#include -#include - -#include "zivid_camera.h" - -namespace zivid_camera -{ -void ZividNodelet::onInit() -{ - try { - // Important: use non-multi-threaded callback queues (getNodeHandle and getPrivateNodeHandle). - camera = std::make_unique(getNodeHandle(), getPrivateNodeHandle()); - } catch (const std::exception & e) { - NODELET_ERROR_STREAM("Failed to initialize camera driver. Exception: \"" << e.what() << "\""); - throw; - } catch (...) { - NODELET_ERROR_STREAM("Failed to initialize camera driver (unknown exception)"); - throw; - } -} -} // namespace zivid_camera - -#ifdef __clang__ -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wglobal-constructors" -#endif - -PLUGINLIB_EXPORT_CLASS(zivid_camera::ZividNodelet, nodelet::Nodelet) - -#ifdef __clang__ -#pragma clang diagnostic pop -#endif diff --git a/zivid_camera/src/settings_generator.cpp b/zivid_camera/src/settings_generator.cpp deleted file mode 100644 index 01e79065..00000000 --- a/zivid_camera/src/settings_generator.cpp +++ /dev/null @@ -1,684 +0,0 @@ -// This program generates dynamic_reconfigure .cfg files, as well as c++ header files, -// for our settings types (Settings, Settings2D, Settings::Acquisition and -// Settings2D::Acquisition). The utility header files are used by the zivid_camera library. -// This program is compiled and run during the build stage of the zivid_camera library. - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace -{ -template -struct DependentFalse : std::false_type -{ -}; - -template -struct IsInList : std::false_type -{ -}; - -template -struct IsInList -: std::integral_constant::value || IsInList::value> -{ -}; - -template -struct IsInTuple; - -template -struct IsInTuple> : IsInList -{ -}; - -template -struct IsEnumSetting : std::is_enum -{ -}; - -void writeToFile(const std::string & file_name, const std::string & text) -{ - if (boost::filesystem::exists(file_name)) { - std::ifstream file(file_name); - if (!file.is_open()) { - throw std::runtime_error("Unable to open file '" + file_name + "' to check its contents!"); - } - auto file_contents_ss = std::ostringstream{}; - file_contents_ss << file.rdbuf(); - if (file_contents_ss.str() == text) { - // If the file already exists, and content is identical to "text" then do not modify the file. - // This ensures that we avoid unnecessary rebuilds. - return; - } - } - - std::ofstream cfg_file(file_name); - if (!cfg_file || !cfg_file.is_open()) { - throw std::runtime_error("Unable to open file '" + file_name + "' for writing!"); - } - cfg_file << text; - cfg_file.close(); - if (!cfg_file) { - throw std::runtime_error("Failed to write to file '" + file_name + "'!"); - } -} - -std::string toUpperCaseFirst(std::string value) -{ - if (value.empty()) { - throw std::invalid_argument("value is empty"); - } - value[0] = std::toupper(value[0]); - return value; -} - -template -std::string convertSettingsPathToConfigPath() -{ - auto path = std::string(SettingsNode::path); - const auto root_path = std::string(SettingsRootGroup::path); - - if (!root_path.empty()) { - const auto expected_prefix = root_path + "/"; - if (path.substr(0, expected_prefix.length()) != expected_prefix) { - throw std::runtime_error( - "Expected path '" + path + "' to begin with '" + expected_prefix + "'"); - } - path = path.substr(expected_prefix.length()); - } - - path = boost::replace_all_copy(path, "/", "_"); - const std::regex re("([^_^])([A-Z])"); - path = std::regex_replace(path, re, "$1_$2"); // Convert e.g. ExposureTime to Exposure_Time - return boost::algorithm::to_lower_copy(path); -} - -template -std::string zividSettingsTypeName() -{ - if constexpr ( - std::is_same_v || - IsInTuple::value || - std::is_same_v || - IsInTuple::value) { - return Zivid::Settings::name; - } else if constexpr ( - std::is_same_v || - IsInTuple::value || - std::is_same_v || - IsInTuple::value) { - return Zivid::Settings2D::name; - } else { - static_assert(DependentFalse::value, "Unhandled type."); - } -} - -template -std::string fullyQualifiedZividTypeName() -{ - std::stringstream ss; - ss << "Zivid::" + zividSettingsTypeName(); - const auto path = std::string(SettingsNode::path); - if (!path.empty()) { - ss << "::" << boost::replace_all_copy(path, "/", "::"); - } - return ss.str(); -} - -template -std::string settingEnumValueToRosEnumName(typename SettingsNode::ValueType value) -{ - // During build dynamic_reconfigure will auto-generate variables for C++ and Python - // for all enum values, scoped directly under the top level .cfg name. So, for example, - // an enum value named X listed in Settings.cfg would auto-generate an const int variable - // zivid_camera::Settings_X. Since we may have several different enum settings with the - // same enum value X, we need to scope the enum name with the full path to the setting. - // This ensures there are no collisions now or in the future. - const auto value_str = toUpperCaseFirst(SettingsNode{value}.toString()); - const auto path = boost::replace_all_copy(SettingsNode::path, "/", ""); - return path + value_str; -} - -template -std::string generateEnumConstant(typename SettingsNode::ValueType value) -{ - static_assert(IsEnumSetting::value); - const auto name = settingEnumValueToRosEnumName(value); - const auto int_value = static_cast(value); - const auto description = toUpperCaseFirst(SettingsNode{value}.toString()); - return (boost::format(R"(gen.const("%1%", int_t, %2%, "%3%"))") % name % int_value % description) - .str(); -} - -template -class DynamicReconfigureCfgGenerator -{ -public: - DynamicReconfigureCfgGenerator(const std::string & class_name) - : class_name_(class_name), insert_enabled_(false) - { - } - - template - auto cfgFileDefaultValue() - { - // The default value of settings varies per Zivid camera model. Thus, we cannot set a - // correct static hard-coded default value for the setting here. To correctly use the - // Zivid driver the user must load the default values via dynamic_reconfigure at runtime. - using ValueType = typename SettingsNode::ValueType; - if constexpr (std::is_same_v) { - return false; - } else if constexpr (Zivid::DataModel::HasValidRange::value) { - return SettingsNode::validRange().min(); - } else if constexpr (IsEnumSetting::value) { - return *SettingsNode::validValues().begin(); - } else if constexpr (std::is_same_v>) { - return ValueType{0.0, 0.0}; - } else if constexpr (std::is_same::value) { - return ValueType{0.0f, 0.0f, 0.0f}; - } else { - return ValueType{0}; - } - } - - template - auto convertValueToRosValue(ValueType value) - { - if constexpr ( - std::is_same_v || std::is_same_v || - std::is_same_v) { - return value; - } else if constexpr (std::is_same_v) { - return static_cast(value); - } else if constexpr (std::is_same_v) { - return static_cast(value.count()); - } else if constexpr (std::is_enum_v) { - return static_cast(value); - } else if constexpr (std::is_same_v>) { - return value; - } else if constexpr (std::is_same_v) { - return value; - } else { - static_assert(DependentFalse::value, "Could not convert ValueType to ROS type."); - } - } - - template - std::string rosTypeName() - { - if constexpr (std::is_same_v) { - return "bool_t"; - } else if constexpr (std::is_same_v) { - return "double_t"; - } else if constexpr (std::is_same_v) { - return "int_t"; - } else if constexpr ( - std::is_same_v> || std::is_same_v) { - return "double_t"; - } else { - static_assert( - DependentFalse::value, "Could not convert RosType to a ROS typename string."); - } - } - - template - std::string rosTypeToString(RosType v) - { - if constexpr (std::is_same_v) { - return v ? "True" : "False"; - } else if constexpr (std::is_same_v || std::is_same_v) { - return std::to_string(v); - } else if constexpr ( - std::is_same_v> || std::is_same_v) { - return "0.0"; - } else { - static_assert(DependentFalse::value, "Could not convert RosType to a string value."); - } - } - - template - std::string valueTypeToRosTypeString(ValueType v) - { - return rosTypeToString(convertValueToRosValue(v)); - } - - std::string rosGeneratedEnumVariableName(const std::string & setting_name) - { - return setting_name + "_enum"; - } - - template - void apply(const SettingsNode & node) - { - const auto setting_name = convertSettingsPathToConfigPath(); - const auto level = "0"; - // Newlines must be converted to \\n so that the auto-generated files end up being correct - const auto description = boost::replace_all_copy(node.description, "\n", R"(\\n)"); - const auto default_value = cfgFileDefaultValue(); - const auto type_name = rosTypeName(); - const auto default_value_str = valueTypeToRosTypeString(default_value); - - if constexpr (IsEnumSetting::value) { - const auto valid_values = node.validValues(); - std::vector enum_constants; - std::transform( - valid_values.cbegin(), valid_values.cend(), std::back_inserter(enum_constants), - [&](const auto value) { return generateEnumConstant(value); }); - ss_ << rosGeneratedEnumVariableName(setting_name) << " = gen.enum([\n " - << boost::algorithm::join(enum_constants, ",\n ") << "\n],\n \"" << description - << "\")\n"; - } - - if constexpr (std::is_same_v, Zivid::PointXYZ>) { - for (const auto comp : std::array{"x", "y", "z"}) { - const auto comp_name = setting_name + "_" + comp; - ss_ << "gen.add(\"" << comp_name << "\", " << type_name << ", " << level << ", " - << "\"" << description + " [" + comp + "]" - << "\", " << default_value_str << ")\n"; - } - } else if constexpr (std::is_same_v< - std::decay_t, Zivid::Range>) { - for (const auto comp : std::array{"min", "max"}) { - const auto comp_name = setting_name + "_" + comp; - ss_ << "gen.add(\"" << comp_name << "\", " << type_name << ", " << level << ", " - << "\"" << description + " [" + comp + "]" - << "\", " << default_value_str << ")\n"; - } - } else { - ss_ << "gen.add(\"" << setting_name << "\", " << type_name << ", " << level << ", " - << "\"" << description << "\", " << default_value_str; - - if constexpr (Zivid::DataModel::HasValidRange::value) { - ss_ << ", " << valueTypeToRosTypeString(node.validRange().min()) << ", " - << valueTypeToRosTypeString(node.validRange().max()); - } else if constexpr (IsEnumSetting::value) { - const auto min_index = 0; - const auto max_index = node.validValues().size() - 1; - ss_ << ", " << min_index << ", " << max_index - << ", edit_method=" << rosGeneratedEnumVariableName(setting_name); - } - ss_ << ")\n"; - } - } - - void insertEnabled() { insert_enabled_ = true; } - - std::string str() - { - std::stringstream res; - res << "#!/usr/bin/env python\n\n" - "# This is an auto-generated cfg file. Do not edit!\n\n" - "PACKAGE = \"zivid_camera\"\n" - "import roslib\n" - "roslib.load_manifest(PACKAGE);\n" - "from dynamic_reconfigure.parameter_generator_catkin import *\n\n"; - - res << "gen = ParameterGenerator()\n"; - if (insert_enabled_) { - res << "gen.add(\"enabled\", bool_t, 0, \"When this acquisition is enabled it will be " - "included in captures\", " - "False)\n"; - } - res << ss_.str(); - res << "gen.generate(PACKAGE, \"zivid_camera\", \"" + class_name_ + "\")\n"; - return res.str(); - } - -private: - std::string class_name_; - bool insert_enabled_; - std::stringstream ss_; -}; - -template -class ApplyConfigToZividSettingsGenerator -{ -public: - ApplyConfigToZividSettingsGenerator(const std::string & config_class_name) - : config_class_name_(config_class_name) - { - } - - template - void apply(const SettingsNode & node) - { - using ValueType = typename SettingsNode::ValueType; - - const auto cfg_id = "cfg." + convertSettingsPathToConfigPath(); - const auto setting_node_class_name = fullyQualifiedZividTypeName(); - ss_ << " s.set(" + setting_node_class_name + "{ "; - - if constexpr (std::is_same_v) { - ss_ << "static_cast(" + cfg_id + ")"; - } else if constexpr (std::is_same_v) { - ss_ << "std::chrono::microseconds(" + cfg_id + ")"; - } else if constexpr (IsEnumSetting::value) { - ss_ << "\n" - << " [value = " + cfg_id + "](){\n" - << " switch(value) {\n"; - const auto valid_values = node.validValues(); - for (const auto & enum_value : valid_values) { - ss_ << " case zivid_camera::" << config_class_name_ << "_" - << settingEnumValueToRosEnumName(enum_value) << ":\n" - << " return " + setting_node_class_name + - "::" + SettingsNode{enum_value}.toString() + ";\n"; - } - ss_ << " }\n" - << " throw std::runtime_error(\"Could not convert int value \" + " - "std::to_string(value) + \"" - << " to setting of type " + setting_node_class_name + ".\");\n" - << " }()\n"; - } else if constexpr (std::is_same_v) { - ss_ << "static_cast(" << cfg_id << "_x), static_cast(" << cfg_id - << "_y), static_cast(" << cfg_id << "_z)"; - } else if constexpr (std::is_same_v>) { - ss_ << cfg_id << "_min, " << cfg_id << "_max "; - } else { - ss_ << cfg_id; - } - ss_ << " });\n"; - } - - std::string str() - { - const auto root_type_fq = fullyQualifiedZividTypeName(); - - std::stringstream res; - res << "inline static void applyConfigToZividSettings(const zivid_camera::" - << config_class_name_ << "Config& cfg, " << root_type_fq << "& s)\n"; - res << "{\n"; - res << ss_.str(); - res << "}\n"; - return res.str(); - } - -private: - std::string config_class_name_; - std::stringstream ss_; -}; - -template -class ZividSettingsToConfigGenerator -{ -public: - ZividSettingsToConfigGenerator(const std::string & config_class_name) - : config_class_name_(config_class_name) - { - } - - template - void apply(const SettingsNode &) - { - using ValueType = typename SettingsNode::ValueType; - const auto cfg_id = "cfg." + convertSettingsPathToConfigPath(); - const auto zivid_node_class_name = fullyQualifiedZividTypeName(); - const auto value_str = "s.get<" + zivid_node_class_name + ">().value()"; - - if constexpr (std::is_same_v) { - for (const auto comp : std::array{"x", "y", "z"}) { - ss_ << " " + cfg_id + "_" + comp + " = " - << "static_cast(" + value_str + "." + comp + ");\n"; - } - } else if constexpr (std::is_same_v>) { - for (const auto comp : std::array{"min", "max"}) { - ss_ << " " + cfg_id + "_" + comp + " = " << value_str + "." + comp + "();\n"; - } - } else { - ss_ << " " + cfg_id + " = "; - if constexpr (std::is_same_v) { - ss_ << "static_cast(" + value_str + ".count());\n"; - } else if constexpr (std::is_same_v) { - ss_ << "static_cast(" + value_str + ");\n"; - } else if constexpr (IsEnumSetting::value) { - ss_ << "static_cast(" + value_str + ");\n"; - } else { - ss_ << value_str + ";\n"; - } - } - } - - std::string str() const - { - const auto full_class_name = "zivid_camera::" + config_class_name_ + "Config"; - const auto root_type_fq = fullyQualifiedZividTypeName(); - - std::stringstream res; - res << "template<> inline " << full_class_name << " zividSettingsToConfig"; - res << "<" << full_class_name << ">(const " << root_type_fq << "& s)\n"; - res << "{\n"; - res << " auto cfg = " + full_class_name << "::__getDefault__();\n"; - res << ss_.str(); - res << " return cfg;\n"; - res << "}\n"; - return res.str(); - } - -private: - std::string config_class_name_; - std::stringstream ss_; -}; - -template -class MinMaxConfigGenerator -{ -public: - enum class Type - { - Min, - Max, - }; - - MinMaxConfigGenerator(const std::string & config_class_name, Type type) - : config_class_name_(config_class_name), type_(type) - { - } - - template - void apply(const SettingsNode &) - { - if constexpr (Zivid::DataModel::HasValidRange::value) { - using ValueType = typename SettingsNode::ValueType; - const auto cfg_id = - "cfg." + convertSettingsPathToConfigPath(); - const auto zivid_node_class_name = fullyQualifiedZividTypeName(); - - const auto value_str = [&]() { - std::string prefix = "Zivid::Experimental::SettingsInfo::validRange<" + - zivid_node_class_name + ">(camera.info())"; - switch (type_) { - case Type::Min: - return prefix + ".min()"; - case Type::Max: - return prefix + ".max()"; - } - throw std::runtime_error(std::string(__func__) + ": Unhandled enum: " + toString(type_)); - }(); - - ss_ << " " + cfg_id + " = "; - - if constexpr (std::is_same_v) { - ss_ << "static_cast(" + value_str + ".count());\n"; - } else if constexpr (std::is_same_v) { - ss_ << "static_cast(" + value_str + ");\n"; - } else { - ss_ << value_str + ";\n"; - } - } - } - - std::string initializeConfigFunction() const - { - switch (type_) { - case Type::Min: - return "__getMin__"; - case Type::Max: - return "__getMax__"; - } - throw std::runtime_error(std::string(__func__) + ": Unhandled enum: " + toString(type_)); - } - - std::string str() const - { - const auto full_class_name = "zivid_camera::" + config_class_name_ + "Config"; - - const auto function_name_config_type = [&]() { - switch (type_) { - case Type::Min: - return "Min"; - case Type::Max: - return "Max"; - } - throw std::runtime_error(std::string(__func__) + ": Unhandled enum: " + toString(type_)); - }(); - - const auto root_type_fq = fullyQualifiedZividTypeName(); - - std::stringstream res; - res << "template<> inline " << full_class_name << " zividSettings" << function_name_config_type - << "Config"; - res << "<" << full_class_name << ", " << root_type_fq << ">(const Zivid::Camera& camera)\n"; - res << "{\n"; - res << " auto cfg = " + full_class_name << "::" << initializeConfigFunction() << "();\n"; - res << ss_.str(); - res << " return cfg;\n"; - res << "}\n"; - return res.str(); - } - - static std::string toString(Type t) - { - return std::to_string(static_cast>(t)); - } - -private: - std::string config_class_name_; - Type type_; - std::stringstream ss_; -}; - -template -class ConfigUtilsHeaderGenerator -{ -public: - ConfigUtilsHeaderGenerator(const std::string & config_class_name) - : apply_config_zivid_settings_gen(config_class_name), - zivid_settings_to_config_gen(config_class_name), - min_config_gen(config_class_name, MinMaxConfigGenerator::Type::Min), - max_config_gen(config_class_name, MinMaxConfigGenerator::Type::Max) - { - } - - template - void apply(const SettingsNode & node) - { - apply_config_zivid_settings_gen.apply(node); - zivid_settings_to_config_gen.apply(node); - min_config_gen.apply(node); - max_config_gen.apply(node); - } - - std::string str() - { - std::stringstream res; - res << "#pragma once\n\n"; - res << "// This is an auto-generated header. Do not edit.\n\n"; - res << "#include \n"; - res << "#include \n\n"; - res << "#include \"config_utils_common.h\"\n\n"; - - res << apply_config_zivid_settings_gen.str() << "\n"; - res << zivid_settings_to_config_gen.str() << "\n"; - res << min_config_gen.str() << "\n"; - res << max_config_gen.str() << "\n"; - return res.str(); - } - - ApplyConfigToZividSettingsGenerator apply_config_zivid_settings_gen; - ZividSettingsToConfigGenerator zivid_settings_to_config_gen; - MinMaxConfigGenerator min_config_gen; - MinMaxConfigGenerator max_config_gen; -}; - -template -class Generator -{ -public: - Generator(const std::string & config_class_name) - : config_class_name_(config_class_name), - dynamic_reconfigure_cfg_gen_(config_class_name), - config_utils_header_gen_(config_class_name) - { - } - - template - void apply(const SettingsNode & node) - { - dynamic_reconfigure_cfg_gen_.apply(node); - config_utils_header_gen_.apply(node); - } - - void insertEnabled() { dynamic_reconfigure_cfg_gen_.insertEnabled(); } - - void writeToFiles() - { - writeToFile(config_class_name_ + ".cfg", dynamic_reconfigure_cfg_gen_.str()); - writeToFile( - "generated_headers/" + config_class_name_ + "ConfigUtils.h", config_utils_header_gen_.str()); - } - -private: - std::string config_class_name_; - DynamicReconfigureCfgGenerator dynamic_reconfigure_cfg_gen_; - ConfigUtilsHeaderGenerator config_utils_header_gen_; -}; - -template -void traverseSettingsTree(const SettingsNode & node, GeneratorType & generator) -{ - if constexpr (std::is_same_v) { - // Acquisitions ignored here. Acqusitition is handled separately. - } else if constexpr (SettingsNode::nodeType == Zivid::DataModel::NodeType::group) { - node.forEach([&](const auto & child) { traverseSettingsTree(child, generator); }); - } else { - generator.apply(node); - } -} - -template -void addSettingsType(const std::string & cfgPrefix) -{ - Generator capture_general_gen(cfgPrefix); - traverseSettingsTree(SettingsType{}, capture_general_gen); - capture_general_gen.writeToFiles(); - - Generator capture_acquisition_gen(cfgPrefix + "Acquisition"); - traverseSettingsTree(typename SettingsType::Acquisition{}, capture_acquisition_gen); - capture_acquisition_gen.insertEnabled(); - capture_acquisition_gen.writeToFiles(); -} - -} // namespace - -int main(int /*argc*/, char ** /*argv*/) -{ - addSettingsType("Settings"); - addSettingsType("Settings2D"); - return 0; -} diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 1c61296a..97b2e771 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -1,28 +1,61 @@ -#include "zivid_camera.h" - +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include #include +#include #include #include #include +#include #include #include -#include -#include -#include -#include -#include -#include #include +#include +#include #include +#include #include +#include namespace { -sensor_msgs::PointField createPointField( +template +struct DependentFalse : std::false_type +{ +}; + +sensor_msgs::msg::PointField createPointField( std::string name, uint32_t offset, uint8_t datatype, uint32_t count) { - sensor_msgs::PointField point_field; + sensor_msgs::msg::PointField point_field; point_field.name = name; point_field.offset = offset; point_field.datatype = datatype; @@ -30,23 +63,30 @@ sensor_msgs::PointField createPointField( return point_field; } -bool big_endian() { return BOOST_ENDIAN_BIG_BYTE; } +bool bigEndian() +{ + union { + uint32_t i; + char c[4]; + } b = {0x01020304}; + return b.c[0] == 1; +} template void fillCommonMsgFields( - T & msg, const std_msgs::Header & header, std::size_t width, std::size_t height) + T & msg, const std_msgs::msg::Header & header, std::size_t width, std::size_t height) { msg.header = header; msg.height = static_cast(height); msg.width = static_cast(width); - msg.is_bigendian = big_endian(); + msg.is_bigendian = bigEndian(); } -sensor_msgs::ImagePtr makeImage( - const std_msgs::Header & header, const std::string & encoding, std::size_t width, +sensor_msgs::msg::Image::SharedPtr makeImage( + const std_msgs::msg::Header & header, const std::string & encoding, std::size_t width, std::size_t height) { - auto image = boost::make_shared(); + auto image = std::make_shared(); fillCommonMsgFields(*image, header, width, height); image->encoding = encoding; const auto bytes_per_pixel = static_cast( @@ -57,8 +97,8 @@ sensor_msgs::ImagePtr makeImage( } template -sensor_msgs::ImagePtr makePointCloudImage( - const Zivid::PointCloud & point_cloud, const std_msgs::Header & header, +sensor_msgs::msg::Image::SharedPtr makePointCloudImage( + const Zivid::PointCloud & point_cloud, const std_msgs::msg::Header & header, const std::string & encoding) { auto image = makeImage(header, encoding, point_cloud.width(), point_cloud.height()); @@ -67,6 +107,15 @@ sensor_msgs::ImagePtr makePointCloudImage( return image; } +rclcpp::QoS getQoSLatched(bool use_latched_publisher) +{ + rclcpp::QoS qos{rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)}; + if (use_latched_publisher) { + qos.durability(rclcpp::DurabilityPolicy::TransientLocal); + } + return qos; +} + std::string toString(zivid_camera::CameraStatus camera_status) { switch (camera_status) { @@ -76,14 +125,79 @@ std::string toString(zivid_camera::CameraStatus camera_status) return "Disconnected"; case zivid_camera::CameraStatus::Idle: return "Idle"; + default: // NOLINT(clang-diagnostic-covered-switch-default) + throw std::runtime_error("Enum `camera_status` out of range."); } - return "N/A"; +} + +template +auto serializeZividDataModel(const ZividDataModel & dm) +{ +#if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) + std::stringstream ss; + Zivid::Detail::save(dm, ss); + return ss.str(); +#else + return dm.serialize(); +#endif +} + +template +auto deserializeZividDataModel(const std::string & serialized) +{ +#if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) + ZividDataModel dm; + std::stringstream ss; + ss << serialized; + Zivid::Detail::load(dm, ss); + return dm; +#else + return ZividDataModel::fromSerialized(serialized); +#endif +} + +template +void runFunctionAndCatchExceptions( + Function && function, ResponseSharedPtr & response, const Logger & logger, + const std::string & operation) +{ + try { + function(); + response->success = true; + } catch (const std::exception & exception) { + const auto exception_message = Zivid::toString(exception); + RCLCPP_ERROR_STREAM( + logger, operation + " failed with exception: \"" << exception_message << "\""); + response->success = false; + response->message = exception_message; + } +} + +template +auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger & logger) +{ + try { + return function(); + } catch (const std::exception & exception) { + const auto exception_message = Zivid::toString(exception); + RCLCPP_ERROR_STREAM(logger, "A function failed with exception: \"" + exception_message + "\""); + throw; + } +} + +template +[[noreturn]] void logErrorToLoggerAndThrowRuntimeException( + const Logger & logger, const std::string & message) +{ + RCLCPP_ERROR(logger, "%s", message.c_str()); + throw std::runtime_error(message); } Zivid::Application makeZividApplication() { -#if ZIVID_CORE_VERSION_MAJOR >= 3 || (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 13) - return Zivid::Detail::createApplicationForWrapper(Zivid::Detail::EnvironmentInfo::Wrapper::ros1); +#if ZIVID_CORE_VERSION_MAJOR >= 3 || \ + (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 13) + return Zivid::Detail::createApplicationForWrapper(Zivid::Detail::EnvironmentInfo::Wrapper::ros2); #else return Zivid::Application(); #endif @@ -92,216 +206,295 @@ Zivid::Application makeZividApplication() namespace zivid_camera { -ZividCamera::ZividCamera(ros::NodeHandle & nh, ros::NodeHandle & priv) -: nh_(nh), - priv_(priv), - camera_status_(CameraStatus::Idle), - use_latched_publisher_for_points_xyz_(false), - use_latched_publisher_for_points_xyzrgba_(false), - use_latched_publisher_for_color_image_(false), - use_latched_publisher_for_depth_image_(false), - use_latched_publisher_for_snr_image_(false), - use_latched_publisher_for_normals_xyz_(false), - image_transport_(nh_), - zivid_(makeZividApplication()), - header_seq_(0) -{ - ROS_INFO("Zivid ROS driver version %s", ZIVID_ROS_DRIVER_VERSION); - - ROS_INFO("Node's namespace is '%s'", nh_.getNamespace().c_str()); - if (nh_.getNamespace() == "/") { - // Require the user to specify the namespace that this node will run in. - // See REP-135 http://www.ros.org/reps/rep-0135.html - throw std::runtime_error( - "Zivid ROS driver started in the global namespace ('/')! This is unsupported. " - "Please specify namespace, e.g. using the ROS_NAMESPACE environment variable."); + +namespace ParamNames +{ +constexpr auto serial_number = "serial_number"; +constexpr auto file_camera_path = "file_camera_path"; +constexpr auto frame_id = "frame_id"; +} // namespace ParamNames + +template +class CaptureSettingsController +{ +public: + explicit CaptureSettingsController(rclcpp::Node & node) + : node_(node), + file_path_param_{baseName() + std::string{"_file_path"}}, + yaml_string_param_{baseName() + std::string{"_yaml"}} + { + for (const auto & param : {yaml_string_param_, file_path_param_}) { + node_.declare_parameter(param, ""); + } } - ROS_INFO_STREAM("Built towards Zivid Core version " << ZIVID_CORE_VERSION); - ROS_INFO_STREAM("Running with Zivid Core version " << Zivid::Version::coreLibraryVersion()); - if (Zivid::Version::coreLibraryVersion() != ZIVID_CORE_VERSION) { - throw std::runtime_error( - "Zivid library mismatch! The running Zivid Core version does not match the " - "version this ROS driver was built towards. Hint: Try to clean and re-build your project " - "from scratch."); + SettingsType currentSettings() const + { + if (cached_settings_.has_value()) { + RCLCPP_DEBUG_STREAM(node_.get_logger(), "Using cached settings"); + return *cached_settings_; + } + + const auto settings_file_path = node_.get_parameter(file_path_param_).as_string(); + const auto settings_yaml = node_.get_parameter(yaml_string_param_).as_string(); + + if (!settings_file_path.empty() && !settings_yaml.empty()) { + logErrorToLoggerAndThrowRuntimeException( + node_.get_logger(), + "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + + "' parameters are non-empty! Please set only one of these parameters."); + } else if (settings_file_path.empty() && settings_yaml.empty()) { + logErrorToLoggerAndThrowRuntimeException( + node_.get_logger(), "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + + "' parameters are empty! Please set one of these parameters."); + } + + if (!settings_yaml.empty()) { + RCLCPP_DEBUG_STREAM(node_.get_logger(), "Using settings from yml string"); + cached_settings_ = deserializeZividDataModel(settings_yaml); + } else { + RCLCPP_DEBUG_STREAM( + node_.get_logger(), "Using settings from file '" << settings_file_path << "'"); + cached_settings_ = SettingsType{settings_file_path}; + } + + return *cached_settings_; + } + + void setSettings(const SettingsType & settings) + { + RCLCPP_DEBUG_STREAM(node_.get_logger(), "Setting settings from " << settings.name << " object"); + node_.set_parameter(rclcpp::Parameter{yaml_string_param_, serializeZividDataModel(settings)}); + node_.set_parameter(rclcpp::Parameter{file_path_param_, ""}); + } + + void onSetParameter(const std::string & parameterName) + { + if ( + cached_settings_.has_value() && + (parameterName == file_path_param_ || parameterName == yaml_string_param_)) { + RCLCPP_DEBUG_STREAM( + node_.get_logger(), "Resetting cached settings due to updated parameter " << parameterName); + cached_settings_.reset(); + } + } + +private: + constexpr auto baseName() const + { + if constexpr (std::is_same_v) { + return "settings"; + } else if constexpr (std::is_same_v) { + return "settings_2d"; + } else { + static_assert(DependentFalse::value, "Unhandled node type"); + } } - std::string serial_number; - priv_.param("serial_number", serial_number, ""); + rclcpp::Node & node_; + std::string file_path_param_; + std::string yaml_string_param_; + mutable std::optional cached_settings_; +}; + +ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) +: rclcpp::Node{"zivid_camera", options}, + set_parameters_callback_handle_{this->add_on_set_parameters_callback( + std::bind(&ZividCamera::setParametersCallback, this, std::placeholders::_1))}, + zivid_{std::make_unique(makeZividApplication())}, + camera_status_{CameraStatus::Idle}, + settings_controller_{std::make_unique>(*this)}, + settings_2d_controller_{std::make_unique>(*this)} +{ + // Disable buffering on stdout + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - int max_capture_acquisitions; - priv_.param( - "max_capture_acquisitions", max_capture_acquisitions, 10); + RCLCPP_INFO_STREAM(get_logger(), "Zivid ROS driver"); + RCLCPP_INFO(get_logger(), "The node's namespace is '%s'", get_namespace()); + RCLCPP_INFO_STREAM(get_logger(), "Running Zivid Core version " << ZIVID_CORE_VERSION); - priv_.param("frame_id", frame_id_, "zivid_optical_frame"); + const auto serial_number = declare_parameter(ParamNames::serial_number, ""); - std::string file_camera_path; - priv_.param("file_camera_path", file_camera_path, ""); + frame_id_ = declare_parameter(ParamNames::frame_id, "zivid_optical_frame"); + + const auto file_camera_path = declare_parameter(ParamNames::file_camera_path, ""); const bool file_camera_mode = !file_camera_path.empty(); - priv_.param( - "use_latched_publisher_for_points_xyz", use_latched_publisher_for_points_xyz_, false); - priv_.param( - "use_latched_publisher_for_points_xyzrgba", use_latched_publisher_for_points_xyzrgba_, false); - priv_.param( - "use_latched_publisher_for_color_image", use_latched_publisher_for_color_image_, false); - priv_.param( - "use_latched_publisher_for_depth_image", use_latched_publisher_for_depth_image_, false); - priv_.param( - "use_latched_publisher_for_snr_image", use_latched_publisher_for_snr_image_, false); - priv_.param( - "use_latched_publisher_for_normals_xyz", use_latched_publisher_for_normals_xyz_, false); - - bool update_firmware_automatically = true; - priv_.param( - "update_firmware_automatically", update_firmware_automatically, update_firmware_automatically); - - if (file_camera_mode) { - ROS_INFO("Creating file camera from file '%s'", file_camera_path.c_str()); - camera_ = zivid_.createFileCamera(file_camera_path); - } else { - auto cameras = zivid_.cameras(); - ROS_INFO_STREAM(cameras.size() << " cameras found"); + use_latched_publisher_for_points_xyz_ = + declare_parameter("use_latched_publisher_for_points_xyz", false); + + use_latched_publisher_for_points_xyzrgba_ = + declare_parameter("use_latched_publisher_for_points_xyzrgba", false); + + use_latched_publisher_for_color_image_ = + declare_parameter("use_latched_publisher_for_color_image", false); + + use_latched_publisher_for_depth_image_ = + declare_parameter("use_latched_publisher_for_depth_image", false); + + use_latched_publisher_for_snr_image_ = + declare_parameter("use_latched_publisher_for_snr_image", false); + + use_latched_publisher_for_normals_xyz_ = + declare_parameter("use_latched_publisher_for_normals_xyz", false); + + const bool update_firmware_automatically = + declare_parameter("update_firmware_automatically", true); + + camera_ = std::make_unique([&]() { + if (file_camera_mode) { + RCLCPP_INFO(get_logger(), "Creating file camera from file '%s'", file_camera_path.c_str()); + return zivid_->createFileCamera(file_camera_path); + } + auto cameras = zivid_->cameras(); + RCLCPP_INFO_STREAM(get_logger(), cameras.size() << " camera(s) found"); + if (cameras.empty()) { - throw std::runtime_error("No cameras found. Ensure that the camera is connected to your PC."); - } else if (serial_number.empty()) { - camera_ = [&]() { - ROS_INFO("Selecting first available camera"); - for (auto & c : cameras) { - if (c.state().isAvailable()) return c; + logErrorAndThrowRuntimeException( + "No cameras found. Ensure that the camera is connected to your PC."); + } else if (!serial_number.empty()) { + RCLCPP_INFO( + get_logger(), "Searching for camera with serial number '%s' ...", serial_number.c_str()); + for (auto & c : cameras) { + if (c.info().serialNumber() == Zivid::CameraInfo::SerialNumber(serial_number)) { + return c; } - throw std::runtime_error( - "No available cameras found. Is the camera in use by another process?"); - }(); - } else { - if (serial_number.find(":") == 0) { - serial_number = serial_number.substr(1); } - camera_ = [&]() { - ROS_INFO("Searching for camera with serial number '%s' ...", serial_number.c_str()); - for (auto & c : cameras) { - if (c.info().serialNumber() == Zivid::CameraInfo::SerialNumber(serial_number)) return c; - } - throw std::runtime_error("No camera found with serial number '" + serial_number + "'"); - }(); + logErrorAndThrowRuntimeException( + "No camera found with serial number '" + serial_number + "'"); } - - if (!Zivid::Firmware::isUpToDate(camera_)) { - if (update_firmware_automatically) { - ROS_INFO( - "The camera firmware is not up-to-date, and update_firmware_automatically is true, " - "starting update"); - Zivid::Firmware::update(camera_, [](double progress, const std::string & state) { - ROS_INFO(" [%.0f%%] %s", progress, state.c_str()); - }); - ROS_INFO("Firmware update completed"); - } else { - ROS_ERROR( - "The camera firmware is not up-to-date, and update_firmware_automatically is false. " - "Throwing error."); - throw std::runtime_error( - "The firmware on camera '" + camera_.info().serialNumber().value() + - "' is not up to date. The launch parameter update_firmware_automatically " - "is set to false. Please update the firmware on this camera manually."); + RCLCPP_INFO(get_logger(), "Selecting first available camera"); + for (auto & c : cameras) { + if (c.state().isAvailable()) { + return c; } } + logErrorAndThrowRuntimeException( + "No available cameras found! Use ZividListCameras or ZividStudio to see all connected " + "cameras and their status."); + }()); + + if (!Zivid::Firmware::isUpToDate(*camera_)) { + if (update_firmware_automatically) { + RCLCPP_INFO( + get_logger(), + "The camera firmware is not up-to-date, and update_firmware_automatically is true, " + "starting update"); + Zivid::Firmware::update( + *camera_, [logger = get_logger()](double progress, const std::string & state) { + RCLCPP_INFO(logger, " [%.0f%%] %s", progress, state.c_str()); + }); + RCLCPP_INFO(get_logger(), "Firmware update completed"); + } else { + logErrorAndThrowRuntimeException( + "The firmware on camera '" + camera_->info().serialNumber().value() + + "' is not up to date. The launch parameter update_firmware_automatically " + "is set to false. Please update the firmware on this camera manually."); + } } - ROS_INFO_STREAM(camera_); + RCLCPP_INFO_STREAM(get_logger(), *camera_); if (!file_camera_mode) { - ROS_INFO_STREAM("Connecting to camera '" << camera_.info().serialNumber() << "'"); - camera_.connect(); + RCLCPP_INFO_STREAM( + get_logger(), "Connecting to camera '" << camera_->info().serialNumber() << "'"); + camera_->connect(); } - ROS_INFO_STREAM("Connected to camera '" << camera_.info().serialNumber() << "'"); + RCLCPP_INFO_STREAM( + get_logger(), "Connected to camera '" << camera_->info().serialNumber() << "'"); setCameraStatus(CameraStatus::Connected); - camera_connection_keepalive_timer_ = - nh_.createTimer(ros::Duration(10), &ZividCamera::onCameraConnectionKeepAliveTimeout, this); - - capture_settings_controller_ = std::make_unique( - nh_, camera_, "settings", max_capture_acquisitions); - - // HDR is not supported in 2D mode, but for future-proofing the 2D configuration API is analogous - // to 3D except there is only 1 acquisition. - capture_2d_settings_controller_ = - std::make_unique(nh_, camera_, "settings_2d", 1); - - ROS_INFO("Advertising topics"); - points_xyz_publisher_ = - nh_.advertise("points/xyz", 1, use_latched_publisher_for_points_xyz_); - points_xyzrgba_publisher_ = nh_.advertise( - "points/xyzrgba", 1, use_latched_publisher_for_points_xyzrgba_); - color_image_publisher_ = image_transport_.advertiseCamera( - "color/image_color", 1, use_latched_publisher_for_color_image_); - depth_image_publisher_ = - image_transport_.advertiseCamera("depth/image", 1, use_latched_publisher_for_depth_image_); - snr_image_publisher_ = - image_transport_.advertiseCamera("snr/image", 1, use_latched_publisher_for_snr_image_); - normals_xyz_publisher_ = nh_.advertise( - "normals/xyz", 1, use_latched_publisher_for_normals_xyz_); - - ROS_INFO("Advertising services"); - camera_info_model_name_service_ = nh_.advertiseService( - "camera_info/model_name", &ZividCamera::cameraInfoModelNameServiceHandler, this); - camera_info_serial_number_service_ = nh_.advertiseService( - "camera_info/serial_number", &ZividCamera::cameraInfoSerialNumberServiceHandler, this); - is_connected_service_ = - nh_.advertiseService("is_connected", &ZividCamera::isConnectedServiceHandler, this); - capture_service_ = nh_.advertiseService("capture", &ZividCamera::captureServiceHandler, this); - capture_and_save_service_ = - nh_.advertiseService("capture_and_save", &ZividCamera::captureAndSaveServiceHandler, this); - capture_2d_service_ = - nh_.advertiseService("capture_2d", &ZividCamera::capture2DServiceHandler, this); - capture_assistant_suggest_settings_service_ = nh_.advertiseService( - "capture_assistant/suggest_settings", - &ZividCamera::captureAssistantSuggestSettingsServiceHandler, this); - load_settings_from_file_service_ = nh_.advertiseService( - "load_settings_from_file", &ZividCamera::loadSettingsFromFileServiceHandler, this); - load_settings_2d_from_file_service_ = nh_.advertiseService( - "load_settings_2d_from_file", &ZividCamera::loadSettings2DFromFileServiceHandler, this); - - ROS_INFO("Zivid camera driver is now ready!"); -} - -void ZividCamera::onCameraConnectionKeepAliveTimeout(const ros::TimerEvent &) -{ - ROS_DEBUG_STREAM(__func__); + camera_connection_keepalive_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::seconds(10), + std::bind(&ZividCamera::onCameraConnectionKeepAliveTimeout, this)); + + RCLCPP_INFO(get_logger(), "Advertising topics"); + + points_xyz_publisher_ = create_publisher( + "points/xyz", getQoSLatched(use_latched_publisher_for_points_xyz_)); + + points_xyzrgba_publisher_ = create_publisher( + "points/xyzrgba", getQoSLatched(use_latched_publisher_for_points_xyzrgba_)); + + normals_xyz_publisher_ = create_publisher( + "normals/xyz", getQoSLatched(use_latched_publisher_for_normals_xyz_)); + + color_image_publisher_ = image_transport::create_camera_publisher( + this, "color/image_color", + getQoSLatched(use_latched_publisher_for_color_image_).get_rmw_qos_profile()); + depth_image_publisher_ = image_transport::create_camera_publisher( + this, "depth/image", + getQoSLatched(use_latched_publisher_for_depth_image_).get_rmw_qos_profile()); + snr_image_publisher_ = image_transport::create_camera_publisher( + this, "snr/image", getQoSLatched(use_latched_publisher_for_snr_image_).get_rmw_qos_profile()); + + RCLCPP_INFO(get_logger(), "Advertising services"); + + using namespace std::placeholders; + + camera_info_model_name_service_ = create_service( + "camera_info/model_name", + std::bind(&ZividCamera::cameraInfoModelNameServiceHandler, this, _1, _2, _3)); + + camera_info_serial_number_service_ = + create_service( + "camera_info/serial_number", + std::bind(&ZividCamera::cameraInfoSerialNumberServiceHandler, this, _1, _2, _3)); + + is_connected_service_ = create_service( + "is_connected", std::bind(&ZividCamera::isConnectedServiceHandler, this, _1, _2, _3)); + + capture_service_ = create_service( + "capture", std::bind(&ZividCamera::captureServiceHandler, this, _1, _2, _3)); + + capture_and_save_service_ = create_service( + "capture_and_save", std::bind(&ZividCamera::captureAndSaveServiceHandler, this, _1, _2, _3)); + + capture_2d_service_ = create_service( + "capture_2d", std::bind(&ZividCamera::capture2DServiceHandler, this, _1, _2, _3)); + + capture_assistant_suggest_settings_service_ = + create_service( + "capture_assistant/suggest_settings", + std::bind(&ZividCamera::captureAssistantSuggestSettingsServiceHandler, this, _1, _2, _3)); + + RCLCPP_INFO(get_logger(), "Zivid camera driver is now ready!"); +} + +ZividCamera::~ZividCamera() = default; + +Zivid::Application & ZividCamera::zividApplication() { return *zivid_; } + +void ZividCamera::onCameraConnectionKeepAliveTimeout() +{ + RCLCPP_DEBUG_STREAM(get_logger(), __func__); try { reconnectToCameraIfNecessary(); } catch (const std::exception & e) { - ROS_INFO("%s failed with exception '%s'", __func__, e.what()); + RCLCPP_INFO(get_logger(), "%s failed with exception '%s'", __func__, e.what()); } } void ZividCamera::reconnectToCameraIfNecessary() { - ROS_DEBUG_STREAM(__func__); + RCLCPP_DEBUG_STREAM(get_logger(), __func__); - const auto state = camera_.state(); + const auto state = camera_->state(); if (state.isConnected().value()) { setCameraStatus(CameraStatus::Connected); } else { setCameraStatus(CameraStatus::Disconnected); - // The camera handle needs to be refreshed to ensure we get the correct - // "available" status. This is a bug in the API. - auto cameras = zivid_.cameras(); - for (auto & c : cameras) { - if (camera_.info().serialNumber() == c.info().serialNumber()) { - camera_ = c; - } - } - if (state.isAvailable().value()) { - ROS_INFO_STREAM( - "The camera '" << camera_.info().serialNumber() - << "' is not connected but is available. Re-connecting ..."); - camera_.connect(); - ROS_INFO("Successfully reconnected to camera!"); + RCLCPP_INFO_STREAM( + get_logger(), "The camera '" << camera_->info().serialNumber() + << "' is not connected but is available. Re-connecting ..."); + camera_->connect(); + RCLCPP_INFO(get_logger(), "Successfully reconnected to camera!"); setCameraStatus(CameraStatus::Connected); } else { - ROS_INFO_STREAM( - "The camera '" << camera_.info().serialNumber() << "' is not connected nor available."); + RCLCPP_INFO_STREAM( + get_logger(), + "The camera '" << camera_->info().serialNumber() << "' is not connected nor available."); } } } @@ -313,152 +506,176 @@ void ZividCamera::setCameraStatus(CameraStatus camera_status) ss << "Camera status changed to " << toString(camera_status) << " (was " << toString(camera_status_) << ")"; if (camera_status == CameraStatus::Connected) { - ROS_INFO_STREAM(ss.str()); + RCLCPP_INFO_STREAM(get_logger(), ss.str()); } else { - ROS_WARN_STREAM(ss.str()); + RCLCPP_WARN_STREAM(get_logger(), ss.str()); } camera_status_ = camera_status; } } -bool ZividCamera::cameraInfoModelNameServiceHandler( - zivid_camera::CameraInfoModelName::Request &, zivid_camera::CameraInfoModelName::Response & res) +rcl_interfaces::msg::SetParametersResult ZividCamera::setParametersCallback( + const std::vector & parameters) { - res.model_name = camera_.info().modelName().toString(); - return true; + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + RCLCPP_DEBUG_STREAM( + get_logger(), "Set parameter '" << param.get_name() << "' (" << param.get_type_name() + << ") to '" << param.value_to_string() << "'"); + if (settings_controller_) { + settings_controller_->onSetParameter(param.get_name()); + } + if (settings_2d_controller_) { + settings_2d_controller_->onSetParameter(param.get_name()); + } + } + return result; } -bool ZividCamera::cameraInfoSerialNumberServiceHandler( - zivid_camera::CameraInfoSerialNumber::Request &, - zivid_camera::CameraInfoSerialNumber::Response & res) +void ZividCamera::cameraInfoModelNameServiceHandler( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr response) { - res.serial_number = camera_.info().serialNumber().toString(); - return true; + RCLCPP_INFO_STREAM(get_logger(), __func__); + response->model_name = camera_->info().modelName().toString(); } -bool ZividCamera::captureServiceHandler(Capture::Request &, Capture::Response &) +void ZividCamera::cameraInfoSerialNumberServiceHandler( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr response) { - ROS_DEBUG_STREAM(__func__); - - invokeCaptureAndPublishFrame(); - - return true; + RCLCPP_INFO_STREAM(get_logger(), __func__); + response->serial_number = camera_->info().serialNumber().toString(); } -bool ZividCamera::captureAndSaveServiceHandler( - CaptureAndSave::Request & req, CaptureAndSave::Response &) +void ZividCamera::captureServiceHandler( + const std::shared_ptr, const std::shared_ptr, + std::shared_ptr response) { - ROS_DEBUG_STREAM(__func__); + RCLCPP_INFO_STREAM(get_logger(), __func__); - const auto frame = invokeCaptureAndPublishFrame(); - ROS_INFO("Saving frame to '%s'", req.file_path.c_str()); - frame.save(req.file_path); + const auto settings = runFunctionAndCatchExceptionsAndRethrow( + [&] { return settings_controller_->currentSettings(); }, get_logger()); - return true; + runFunctionAndCatchExceptions( + [&]() { invokeCaptureAndPublishFrame(settings); }, response, get_logger(), "Capture"); } -bool ZividCamera::capture2DServiceHandler(Capture2D::Request &, Capture2D::Response &) +void ZividCamera::captureAndSaveServiceHandler( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) { - ROS_DEBUG_STREAM(__func__); + RCLCPP_INFO_STREAM(get_logger(), __func__); + const auto settings = runFunctionAndCatchExceptionsAndRethrow( + [&] { return settings_controller_->currentSettings(); }, get_logger()); - serviceHandlerHandleCameraConnectionLoss(); + runFunctionAndCatchExceptions( + [&]() { + const auto frame = invokeCaptureAndPublishFrame(settings); + const auto destination_path = request->file_path; + RCLCPP_INFO(get_logger(), "Saving frame to '%s'", destination_path.c_str()); + frame.save(destination_path); + }, + response, get_logger(), "CaptureAndSave"); +} - const auto settings2D = capture_2d_settings_controller_->zividSettings(); +void ZividCamera::capture2DServiceHandler( + const std::shared_ptr, const std::shared_ptr, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); - if (settings2D.acquisitions().isEmpty()) { - throw std::runtime_error("capture_2d called with 0 enabled acquisitions!"); - } + serviceHandlerHandleCameraConnectionLoss(); - ROS_DEBUG_STREAM(settings2D); - auto frame2D = camera_.capture(settings2D); - if (shouldPublishColorImg()) { - const auto header = makeHeader(); - auto image = frame2D.imageRGBA(); - const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_); - const auto camera_info = makeCameraInfo(header, image.width(), image.height(), intrinsics); - publishColorImage(header, camera_info, image); - } - return true; + const auto settings2D = runFunctionAndCatchExceptionsAndRethrow( + [&] { return settings_2d_controller_->currentSettings(); }, get_logger()); + + runFunctionAndCatchExceptions( + [&]() { + auto frame2D = camera_->capture(settings2D); + if (shouldPublishColorImg()) { + const auto header = makeHeader(); + auto image = frame2D.imageRGBA(); + const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(*camera_); + const auto camera_info = makeCameraInfo(header, image.width(), image.height(), intrinsics); + publishColorImage(header, camera_info, image); + } + }, + response, get_logger(), "Capture2D"); } -bool ZividCamera::captureAssistantSuggestSettingsServiceHandler( - CaptureAssistantSuggestSettings::Request & req, CaptureAssistantSuggestSettings::Response &) +void ZividCamera::captureAssistantSuggestSettingsServiceHandler( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) { - ROS_DEBUG_STREAM(__func__ << ": Request: " << req); + RCLCPP_INFO_STREAM(get_logger(), __func__); serviceHandlerHandleCameraConnectionLoss(); - if (capture_settings_controller_->numAcquisitionConfigServers() < 10) { - throw std::runtime_error( - "To use the CaptureAssistant the launch parameter 'max_capture_acquisitions' " - "must be at least 10, since the Capture Assistant may suggest up to 10 acquisitions. " - "See README.md for more information."); - } - using SuggestSettingsParameters = Zivid::CaptureAssistant::SuggestSettingsParameters; - const auto max_capture_time = std::chrono::round( - std::chrono::duration{req.max_capture_time.toSec()}); - const auto ambient_light_frequency = [&req]() { - switch (req.ambient_light_frequency) { - case CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE: + const auto max_capture_time = + rclcpp::Duration{request->max_capture_time}.to_chrono(); + const auto ambient_light_frequency = [this, &request]() { + using RosRequestTypes = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + switch (request->ambient_light_frequency) { + case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_NONE: return SuggestSettingsParameters::AmbientLightFrequency::none; - case CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_50HZ: + case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_50HZ: return SuggestSettingsParameters::AmbientLightFrequency::hz50; - case CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_60HZ: + case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_60HZ: return SuggestSettingsParameters::AmbientLightFrequency::hz60; + default: + logErrorAndThrowRuntimeException( + "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + + std::to_string(request->ambient_light_frequency)); } - throw std::runtime_error( - "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + std::to_string(req.ambient_light_frequency)); }(); SuggestSettingsParameters suggest_settings_parameters{ SuggestSettingsParameters::MaxCaptureTime{max_capture_time}, ambient_light_frequency}; - ROS_INFO_STREAM("Getting suggested settings using parameters: " << suggest_settings_parameters); - const auto suggested_settings = - Zivid::CaptureAssistant::suggestSettings(camera_, suggest_settings_parameters); - ROS_INFO_STREAM( - "CaptureAssistant::suggestSettings returned " << suggested_settings.acquisitions().size() - << " acquisitions"); - capture_settings_controller_->setZividSettings(suggested_settings); - - return true; -} + RCLCPP_INFO_STREAM( + get_logger(), "Getting suggested settings using parameters: " << suggest_settings_parameters); + const auto suggested_settings = runFunctionAndCatchExceptionsAndRethrow( + [&]() { + return Zivid::CaptureAssistant::suggestSettings(*camera_, suggest_settings_parameters); + }, + get_logger()); -bool ZividCamera::loadSettingsFromFileServiceHandler( - LoadSettingsFromFile::Request & req, LoadSettingsFromFile::Response &) -{ - ROS_DEBUG_STREAM(__func__ << ": Request: " << req); - capture_settings_controller_->setZividSettings(Zivid::Settings{req.file_path.c_str()}); - return true; -} + RCLCPP_INFO_STREAM( + get_logger(), "CaptureAssistant::suggestSettings returned " + << suggested_settings.acquisitions().size() << " acquisitions"); -bool ZividCamera::loadSettings2DFromFileServiceHandler( - LoadSettings2DFromFile::Request & req, LoadSettings2DFromFile::Response &) -{ - ROS_DEBUG_STREAM(__func__ << ": Request: " << req); - capture_2d_settings_controller_->setZividSettings(Zivid::Settings2D{req.file_path.c_str()}); - return true; + settings_controller_->setSettings(suggested_settings); + response->suggested_settings = serializeZividDataModel(suggested_settings); } void ZividCamera::serviceHandlerHandleCameraConnectionLoss() { reconnectToCameraIfNecessary(); if (camera_status_ != CameraStatus::Connected) { - throw std::runtime_error( + logErrorAndThrowRuntimeException( "Unable to capture since the camera is not connected. Please re-connect the camera and " "try again."); } } -bool ZividCamera::isConnectedServiceHandler(IsConnected::Request &, IsConnected::Response & res) +void ZividCamera::isConnectedServiceHandler( + const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr response) { - res.is_connected = camera_status_ == CameraStatus::Connected; - return true; + response->is_connected = camera_status_ == CameraStatus::Connected; } void ZividCamera::publishFrame(const Zivid::Frame & frame) { + RCLCPP_INFO_STREAM(get_logger(), __func__); const bool publish_points_xyz = shouldPublishPointsXYZ(); const bool publish_points_xyzrgba = shouldPublishPointsXYZRGBA(); const bool publish_color_img = shouldPublishColorImg(); @@ -474,9 +691,9 @@ void ZividCamera::publishFrame(const Zivid::Frame & frame) // Transform point cloud from millimeters (Zivid SDK) to meter (ROS). const float scale = 0.001f; - const auto transformationMatrix = + const auto transformation_matrix = Zivid::Matrix4x4{scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, scale, 0, 0, 0, 0, 1}; - point_cloud.transform(transformationMatrix); + point_cloud.transform(transformation_matrix); if (publish_points_xyz) { publishPointCloudXYZ(header, point_cloud); @@ -485,7 +702,7 @@ void ZividCamera::publishFrame(const Zivid::Frame & frame) publishPointCloudXYZRGBA(header, point_cloud); } if (publish_color_img || publish_depth_img || publish_snr_img) { - const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(camera_); + const auto intrinsics = Zivid::Experimental::Calibration::intrinsics(*camera_); const auto camera_info = makeCameraInfo(header, point_cloud.width(), point_cloud.height(), intrinsics); @@ -502,18 +719,23 @@ void ZividCamera::publishFrame(const Zivid::Frame & frame) if (publish_normals_xyz) { publishNormalsXYZ(header, point_cloud); } + } else { + RCLCPP_WARN_STREAM( + get_logger(), + __func__ << ": capture was called, but no subscribers active and 0 messages sent"); } } bool ZividCamera::shouldPublishPointsXYZRGBA() const { - return points_xyzrgba_publisher_.getNumSubscribers() > 0 || + return points_xyzrgba_publisher_->get_subscription_count() > 0 || use_latched_publisher_for_points_xyzrgba_; } bool ZividCamera::shouldPublishPointsXYZ() const { - return points_xyz_publisher_.getNumSubscribers() > 0 || use_latched_publisher_for_points_xyz_; + return points_xyz_publisher_->get_subscription_count() > 0 || + use_latched_publisher_for_points_xyz_; } bool ZividCamera::shouldPublishColorImg() const @@ -533,54 +755,54 @@ bool ZividCamera::shouldPublishSnrImg() const bool ZividCamera::shouldPublishNormalsXYZ() const { - return normals_xyz_publisher_.getNumSubscribers() > 0 || use_latched_publisher_for_normals_xyz_; + return normals_xyz_publisher_->get_subscription_count() > 0 || + use_latched_publisher_for_normals_xyz_; } -std_msgs::Header ZividCamera::makeHeader() +std_msgs::msg::Header ZividCamera::makeHeader() { - std_msgs::Header header; - header.seq = header_seq_++; - header.stamp = ros::Time::now(); + std_msgs::msg::Header header; + header.stamp = get_clock()->now(); header.frame_id = frame_id_; return header; } void ZividCamera::publishPointCloudXYZ( - const std_msgs::Header & header, const Zivid::PointCloud & point_cloud) + const std_msgs::msg::Header & header, const Zivid::PointCloud & point_cloud) { - ROS_DEBUG_STREAM("Publishing " << points_xyz_publisher_.getTopic()); + RCLCPP_INFO_STREAM(get_logger(), "Publishing " << points_xyz_publisher_->get_topic_name()); - // We are using the Zivid::XYZW type here for compatibility with the pcl::PointXYZ type, which contains an - // padding float for performance reasons. We could use the "pcl_conversion" utility functions to construct - // the PointCloud2 message. However, those are observed to add significant overhead due to extra unnecssary - // copies of the data. + // We are using the Zivid::XYZW type here for compatibility with the pcl::PointXYZ type, which + // contains a padding float for performance reasons. We could use the "pcl_conversion" utility + // functions to construct the PointCloud2 message. However, those are observed to add significant + // overhead due to extra unnecessary copies of the data. using ZividDataType = Zivid::PointXYZW; - auto msg = boost::make_shared(); + auto msg = std::make_unique(); fillCommonMsgFields(*msg, header, point_cloud.width(), point_cloud.height()); msg->fields.reserve(3); - msg->fields.push_back(createPointField("x", 0, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("y", 4, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("z", 8, sensor_msgs::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("x", 0, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("y", 4, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("z", 8, sensor_msgs::msg::PointField::FLOAT32, 1)); msg->is_dense = false; msg->point_step = sizeof(ZividDataType); msg->row_step = msg->point_step * msg->width; msg->data.resize(msg->row_step * msg->height); point_cloud.copyData(reinterpret_cast(msg->data.data())); - points_xyz_publisher_.publish(msg); + points_xyz_publisher_->publish(std::move(msg)); } void ZividCamera::publishPointCloudXYZRGBA( - const std_msgs::Header & header, const Zivid::PointCloud & point_cloud) + const std_msgs::msg::Header & header, const Zivid::PointCloud & point_cloud) { - ROS_DEBUG_STREAM("Publishing " << points_xyzrgba_publisher_.getTopic()); + RCLCPP_INFO_STREAM(get_logger(), "Publishing " << points_xyzrgba_publisher_->get_topic_name()); - auto msg = boost::make_shared(); + auto msg = std::make_unique(); fillCommonMsgFields(*msg, header, point_cloud.width(), point_cloud.height()); msg->fields.reserve(4); - msg->fields.push_back(createPointField("x", 0, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("y", 4, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("z", 8, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("rgba", 12, sensor_msgs::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("x", 0, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("y", 4, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("z", 8, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("rgba", 12, sensor_msgs::msg::PointField::UINT32, 1)); msg->is_dense = false; // Note that the "rgba" field is actually byte order "bgra" on little-endian systems. For this @@ -590,76 +812,95 @@ void ZividCamera::publishPointCloudXYZRGBA( msg->row_step = msg->point_step * msg->width; msg->data.resize(msg->row_step * msg->height); point_cloud.copyData(reinterpret_cast(msg->data.data())); - points_xyzrgba_publisher_.publish(msg); + points_xyzrgba_publisher_->publish(std::move(msg)); } void ZividCamera::publishColorImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, const Zivid::PointCloud & point_cloud) { - ROS_DEBUG_STREAM("Publishing " << color_image_publisher_.getTopic() << " from point cloud"); + RCLCPP_INFO_STREAM( + get_logger(), "Publishing " << color_image_publisher_.getTopic() << " from point cloud"); auto image = makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::RGBA8); color_image_publisher_.publish(image, camera_info); } void ZividCamera::publishColorImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, const Zivid::Image & image) { - ROS_DEBUG_STREAM("Publishing " << color_image_publisher_.getTopic() << " from Image"); + RCLCPP_INFO_STREAM( + get_logger(), "Publishing " << color_image_publisher_.getTopic() << " from Image"); auto msg = makeImage(header, sensor_msgs::image_encodings::RGBA8, image.width(), image.height()); const auto uint8_ptr_begin = reinterpret_cast(image.data()); + +#ifdef __clang__ +#if __has_warning("-Wunsafe-buffer-usage") +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunsafe-buffer-usage" +#endif +#endif const auto uint8_ptr_end = reinterpret_cast(image.data() + image.size()); +#ifdef __clang__ +#if __has_warning("-Wunsafe-buffer-usage") +#pragma clang diagnostic pop +#endif +#endif + msg->data = std::vector(uint8_ptr_begin, uint8_ptr_end); color_image_publisher_.publish(msg, camera_info); } void ZividCamera::publishDepthImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, const Zivid::PointCloud & point_cloud) { - ROS_DEBUG_STREAM("Publishing " << depth_image_publisher_.getTopic()); + RCLCPP_INFO_STREAM(get_logger(), "Publishing " << depth_image_publisher_.getTopic()); auto image = makePointCloudImage( point_cloud, header, sensor_msgs::image_encodings::TYPE_32FC1); depth_image_publisher_.publish(image, camera_info); } void ZividCamera::publishSnrImage( - const std_msgs::Header & header, const sensor_msgs::CameraInfoConstPtr & camera_info, + const std_msgs::msg::Header & header, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info, const Zivid::PointCloud & point_cloud) { - ROS_DEBUG_STREAM("Publishing " << snr_image_publisher_.getTopic()); + RCLCPP_INFO_STREAM(get_logger(), "Publishing " << snr_image_publisher_.getTopic()); auto image = makePointCloudImage(point_cloud, header, sensor_msgs::image_encodings::TYPE_32FC1); snr_image_publisher_.publish(image, camera_info); } void ZividCamera::publishNormalsXYZ( - const std_msgs::Header & header, const Zivid::PointCloud & point_cloud) + const std_msgs::msg::Header & header, const Zivid::PointCloud & point_cloud) { - ROS_DEBUG_STREAM("Publishing " << normals_xyz_publisher_.getTopic()); + RCLCPP_INFO_STREAM(get_logger(), "Publishing " << normals_xyz_publisher_->get_topic_name()); using ZividDataType = Zivid::NormalXYZ; - auto msg = boost::make_shared(); + auto msg = std::make_unique(); fillCommonMsgFields(*msg, header, point_cloud.width(), point_cloud.height()); msg->fields.reserve(3); - msg->fields.push_back(createPointField("normal_x", 0, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("normal_y", 4, sensor_msgs::PointField::FLOAT32, 1)); - msg->fields.push_back(createPointField("normal_z", 8, sensor_msgs::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("normal_x", 0, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("normal_y", 4, sensor_msgs::msg::PointField::FLOAT32, 1)); + msg->fields.push_back(createPointField("normal_z", 8, sensor_msgs::msg::PointField::FLOAT32, 1)); msg->is_dense = false; msg->point_step = sizeof(ZividDataType); msg->row_step = msg->point_step * msg->width; msg->data.resize(msg->row_step * msg->height); point_cloud.copyData(reinterpret_cast(msg->data.data())); - normals_xyz_publisher_.publish(msg); + normals_xyz_publisher_->publish(std::move(msg)); } -sensor_msgs::CameraInfoConstPtr ZividCamera::makeCameraInfo( - const std_msgs::Header & header, std::size_t width, std::size_t height, +sensor_msgs::msg::CameraInfo::ConstSharedPtr ZividCamera::makeCameraInfo( + const std_msgs::msg::Header & header, std::size_t width, std::size_t height, const Zivid::CameraIntrinsics & intrinsics) { - auto msg = boost::make_shared(); + auto msg = std::make_shared(); msg->header = header; msg->width = static_cast(width); msg->height = static_cast(height); @@ -667,59 +908,72 @@ sensor_msgs::CameraInfoConstPtr ZividCamera::makeCameraInfo( // k1, k2, t1, t2, k3 const auto distortion = intrinsics.distortion(); - msg->D.resize(5); - msg->D[0] = distortion.k1().value(); - msg->D[1] = distortion.k2().value(); - msg->D[2] = distortion.p1().value(); - msg->D[3] = distortion.p2().value(); - msg->D[4] = distortion.k3().value(); + msg->d.resize(5); + msg->d[0] = distortion.k1().value(); + msg->d[1] = distortion.k2().value(); + msg->d[2] = distortion.p1().value(); + msg->d[3] = distortion.p2().value(); + msg->d[4] = distortion.k3().value(); // Intrinsic camera matrix for the raw (distorted) images. // [fx 0 cx] // K = [ 0 fy cy] // [ 0 0 1] const auto camera_matrix = intrinsics.cameraMatrix(); - msg->K[0] = camera_matrix.fx().value(); - msg->K[2] = camera_matrix.cx().value(); - msg->K[4] = camera_matrix.fy().value(); - msg->K[5] = camera_matrix.cy().value(); - msg->K[8] = 1; + msg->k[0] = camera_matrix.fx().value(); + msg->k[2] = camera_matrix.cx().value(); + msg->k[4] = camera_matrix.fy().value(); + msg->k[5] = camera_matrix.cy().value(); + msg->k[8] = 1; // R (identity) - msg->R[0] = 1; - msg->R[4] = 1; - msg->R[8] = 1; + msg->r[0] = 1; + msg->r[4] = 1; + msg->r[8] = 1; // Projection/camera matrix // [fx' 0 cx' Tx] // P = [ 0 fy' cy' Ty] // [ 0 0 1 0] - msg->P[0] = camera_matrix.fx().value(); - msg->P[2] = camera_matrix.cx().value(); - msg->P[5] = camera_matrix.fy().value(); - msg->P[6] = camera_matrix.cy().value(); - msg->P[10] = 1; + msg->p[0] = camera_matrix.fx().value(); + msg->p[2] = camera_matrix.cx().value(); + msg->p[5] = camera_matrix.fy().value(); + msg->p[6] = camera_matrix.cy().value(); + msg->p[10] = 1; return msg; } -Zivid::Frame ZividCamera::invokeCaptureAndPublishFrame() +Zivid::Frame ZividCamera::invokeCaptureAndPublishFrame(const Zivid::Settings & settings) { - ROS_DEBUG_STREAM(__func__); + RCLCPP_INFO_STREAM(get_logger(), __func__); serviceHandlerHandleCameraConnectionLoss(); - const auto settings = capture_settings_controller_->zividSettings(); - - if (settings.acquisitions().isEmpty()) { - throw std::runtime_error("capture called with 0 enabled acquisitions!"); - } - - ROS_INFO("Capturing with %zd acquisition(s)", settings.acquisitions().size()); - ROS_DEBUG_STREAM(settings); - const auto frame = camera_.capture(settings); + RCLCPP_INFO(get_logger(), "Capturing with %zd acquisition(s)", settings.acquisitions().size()); + RCLCPP_DEBUG_STREAM(get_logger(), settings); + const auto frame = camera_->capture(settings); publishFrame(frame); return frame; } +void ZividCamera::logErrorAndThrowRuntimeException(const std::string & message) +{ + logErrorToLoggerAndThrowRuntimeException(get_logger(), message); +} + } // namespace zivid_camera + +#include "rclcpp_components/register_node_macro.hpp" + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wglobal-constructors" +#pragma clang diagnostic ignored "-Wexit-time-destructors" +#endif + +RCLCPP_COMPONENTS_REGISTER_NODE(zivid_camera::ZividCamera) + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif diff --git a/zivid_camera/srv/Capture.srv b/zivid_camera/srv/Capture.srv deleted file mode 100644 index ed97d539..00000000 --- a/zivid_camera/srv/Capture.srv +++ /dev/null @@ -1 +0,0 @@ ---- diff --git a/zivid_camera/srv/Capture2D.srv b/zivid_camera/srv/Capture2D.srv deleted file mode 100644 index ed97d539..00000000 --- a/zivid_camera/srv/Capture2D.srv +++ /dev/null @@ -1 +0,0 @@ ---- diff --git a/zivid_camera/srv/CaptureAndSave.srv b/zivid_camera/srv/CaptureAndSave.srv deleted file mode 100644 index 9d71fa93..00000000 --- a/zivid_camera/srv/CaptureAndSave.srv +++ /dev/null @@ -1,2 +0,0 @@ -string file_path ---- diff --git a/zivid_camera/srv/LoadSettings2DFromFile.srv b/zivid_camera/srv/LoadSettings2DFromFile.srv deleted file mode 100644 index 9d71fa93..00000000 --- a/zivid_camera/srv/LoadSettings2DFromFile.srv +++ /dev/null @@ -1,2 +0,0 @@ -string file_path ---- diff --git a/zivid_camera/srv/LoadSettingsFromFile.srv b/zivid_camera/srv/LoadSettingsFromFile.srv deleted file mode 100644 index 9d71fa93..00000000 --- a/zivid_camera/srv/LoadSettingsFromFile.srv +++ /dev/null @@ -1,2 +0,0 @@ -string file_path ---- diff --git a/zivid_camera/test/data/settings/2d/single_1.yml b/zivid_camera/test/data/settings/2d/single_1.yml deleted file mode 100644 index 89ae6db7..00000000 --- a/zivid_camera/test/data/settings/2d/single_1.yml +++ /dev/null @@ -1,17 +0,0 @@ -__version__: - serializer: 1 - data: 3 -Settings2D: - Acquisitions: - - Acquisition: - Aperture: 4 - Brightness: 1.6 - ExposureTime: 10000 - Gain: 3.5 - Processing: - Color: - Balance: - Blue: 1.5 - Green: 1.86 - Red: 2.1 - Gamma: 0.85 diff --git a/zivid_camera/test/data/settings/2d/single_2.yml b/zivid_camera/test/data/settings/2d/single_2.yml deleted file mode 100644 index 15c74d64..00000000 --- a/zivid_camera/test/data/settings/2d/single_2.yml +++ /dev/null @@ -1,18 +0,0 @@ -__version__: - serializer: 1 - data: 3 -Settings2D: - Acquisitions: - - Acquisition: - Aperture: 8 - Brightness: 1.2 - ExposureTime: 8333 - Gain: 2.1 - Processing: - Color: - Balance: - Blue: 1.0 - Green: 2.0 - Red: 3.0 - Gamma: 1.5 - diff --git a/zivid_camera/test/data/settings/2d/single_with_not_set_values.yml b/zivid_camera/test/data/settings/2d/single_with_not_set_values.yml deleted file mode 100644 index 5d5ca43c..00000000 --- a/zivid_camera/test/data/settings/2d/single_with_not_set_values.yml +++ /dev/null @@ -1,18 +0,0 @@ -__version__: - serializer: 1 - data: 3 -Settings2D: - Acquisitions: - - Acquisition: - Aperture: __not_set__ - Brightness: __not_set__ - ExposureTime: __not_set__ - Gain: 2.88 - Processing: - Color: - Balance: - Blue: __not_set__ - Green: 7.7 - Red: __not_set__ - Gamma: __not_set__ - diff --git a/zivid_camera/test/data/settings/3d/hdr.yml b/zivid_camera/test/data/settings/3d/hdr.yml deleted file mode 100644 index cef99e61..00000000 --- a/zivid_camera/test/data/settings/3d/hdr.yml +++ /dev/null @@ -1,56 +0,0 @@ -__version__: - serializer: 1 - data: 8 -Settings: - Acquisitions: - - Acquisition: - Aperture: 4.75682846001088 - Brightness: 1.6 - ExposureTime: 10000 - Gain: 3.5 - - Acquisition: - Aperture: 7.33603234563737 - Brightness: 1 - ExposureTime: 30000 - Gain: 4.5 - - Acquisition: - Aperture: 11.3137084989848 - Brightness: 1.2 - ExposureTime: 45000 - Gain: 6.5 - Experimental: - Engine: phase - Processing: - Color: - Balance: - Blue: 1.5 - Green: 1.86 - Red: 2.1 - Experimental: - ToneMapping: - Enabled: always - Gamma: 0.85 - Filters: - Experimental: - ContrastDistortion: - Correction: - Enabled: no - Strength: 0.3 - Removal: - Enabled: yes - Threshold: 0.2 - Noise: - Removal: - Enabled: yes - Threshold: 3 - Outlier: - Removal: - Enabled: yes - Threshold: 14 - Reflection: - Removal: - Enabled: no - Smoothing: - Gaussian: - Enabled: yes - Sigma: 1.75 diff --git a/zivid_camera/test/data/settings/3d/hdr_with_not_set_values.yml b/zivid_camera/test/data/settings/3d/hdr_with_not_set_values.yml deleted file mode 100644 index 16ac2aed..00000000 --- a/zivid_camera/test/data/settings/3d/hdr_with_not_set_values.yml +++ /dev/null @@ -1,56 +0,0 @@ -__version__: - serializer: 1 - data: 8 -Settings: - Acquisitions: - - Acquisition: - Aperture: 2.14 - Brightness: __not_set__ - ExposureTime: __not_set__ - Gain: __not_set__ - - Acquisition: - Aperture: __not_set__ - Brightness: 0.95 - ExposureTime: __not_set__ - Gain: __not_set__ - - Acquisition: - Aperture: __not_set__ - Brightness: __not_set__ - ExposureTime: 100000 - Gain: __not_set__ - Experimental: - Engine: phase - Processing: - Color: - Balance: - Blue: __not_set__ - Green: __not_set__ - Red: 2.2 - Experimental: - ToneMapping: - Enabled: __not_set__ - Gamma: 1.1 - Filters: - Experimental: - ContrastDistortion: - Correction: - Enabled: __not_set__ - Strength: 0.35 - Removal: - Enabled: no - Threshold: __not_set__ - Noise: - Removal: - Enabled: __not_set__ - Threshold: __not_set__ - Outlier: - Removal: - Enabled: __not_set__ - Threshold: 21 - Reflection: - Removal: - Enabled: __not_set__ - Smoothing: - Gaussian: - Enabled: no - Sigma: __not_set__ diff --git a/zivid_camera/test/data/settings/3d/single.yml b/zivid_camera/test/data/settings/3d/single.yml deleted file mode 100644 index 5ca3c3f9..00000000 --- a/zivid_camera/test/data/settings/3d/single.yml +++ /dev/null @@ -1,46 +0,0 @@ -__version__: - serializer: 1 - data: 8 -Settings: - Acquisitions: - - Acquisition: - Aperture: 3 - Brightness: 1.1 - ExposureTime: 12500 - Gain: 3.0 - Experimental: - Engine: phase - Processing: - Color: - Balance: - Blue: 1.6 - Green: 1.0 - Red: 2.0 - Experimental: - ToneMapping: - Enabled: hdrOnly - Gamma: 1.5 - Filters: - Experimental: - ContrastDistortion: - Correction: - Enabled: yes - Strength: 0.2 - Removal: - Enabled: no - Threshold: 0.1 - Noise: - Removal: - Enabled: no - Threshold: 4 - Outlier: - Removal: - Enabled: no - Threshold: 10 - Reflection: - Removal: - Enabled: yes - Smoothing: - Gaussian: - Enabled: no - Sigma: 1.20 diff --git a/zivid_camera/test/data/settings/invalid_file.yml b/zivid_camera/test/data/settings/invalid_file.yml deleted file mode 100644 index 2266f1ea..00000000 --- a/zivid_camera/test/data/settings/invalid_file.yml +++ /dev/null @@ -1,2 +0,0 @@ -Settings: - X: Y diff --git a/zivid_camera/test/gtest_include_wrapper.h b/zivid_camera/test/gtest_include_wrapper.h deleted file mode 100644 index 3040d44d..00000000 --- a/zivid_camera/test/gtest_include_wrapper.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once -#ifdef __clang__ -#pragma clang system_header -#endif - -#include diff --git a/zivid_camera/test/test_zivid_camera.cpp b/zivid_camera/test/test_zivid_camera.cpp index 6eacd651..f8b5469d 100644 --- a/zivid_camera/test/test_zivid_camera.cpp +++ b/zivid_camera/test/test_zivid_camera.cpp @@ -1,8 +1,30 @@ -#ifdef __clang__ -#pragma clang diagnostic push -// Errors to ignore for this entire file -#pragma clang diagnostic ignored "-Wglobal-constructors" // error triggered by gtest fixtures -#endif +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. #include #include @@ -10,48 +32,65 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "gtest_include_wrapper.h" - -using SecondsD = std::chrono::duration; +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#define ZIVID_SAMPLE_DATA_DIR "C:\\ProgramData\\Zivid\\" +#else +#define ZIVID_SAMPLE_DATA_DIR "/usr/share/Zivid/data/" +#endif namespace { -template -struct DependentFalse : std::false_type -{ -}; +constexpr auto file_camera_path = ZIVID_SAMPLE_DATA_DIR "FileCameraZivid2M70.zfc"; -template -ros::Duration toRosDuration(const std::chrono::duration & d) +std::shared_ptr zivid_ros_node; + +std::filesystem::path getTemporaryFilePath(const std::string & name) { - return ros::Duration{std::chrono::duration_cast(d).count()}; + return std::filesystem::temp_directory_path() / name; } -std::string testDataDir() +class TmpFile { - return (boost::filesystem::path{__FILE__}.parent_path() / "data").string(); -} +public: + TmpFile(const std::string & name, const std::string & content) + : m_path{getTemporaryFilePath(name)} + { + if (std::filesystem::exists(m_path)) { + throw std::runtime_error{"Temporary file already exists: " + m_path.string()}; + } + std::ofstream file{m_path}; + file << content; + file.close(); + } + + TmpFile(const TmpFile &) noexcept = delete; + TmpFile(TmpFile &&) noexcept = delete; + TmpFile & operator=(const TmpFile &) = delete; + TmpFile & operator=(TmpFile &&) = delete; + + ~TmpFile() { std::filesystem::remove(m_path); } + + std::string string() const { return m_path.string(); } + +private: + std::filesystem::path m_path; +}; } // namespace @@ -78,133 +117,276 @@ class ZividNodeTestBase : public testing::Test class ZividNodeTest : public ZividNodeTestBase { protected: - ros::NodeHandle nh_; - - const ros::Duration node_ready_wait_duration{45}; - const ros::Duration short_wait_duration{0.5}; - const ros::Duration medium_wait_duration{1.0}; - const ros::Duration dr_get_max_wait_duration{5}; - static constexpr auto capture_service_name = "/zivid_camera/capture"; - static constexpr auto capture_and_save_service_name = "/zivid_camera/capture_and_save"; - static constexpr auto capture_2d_service_name = "/zivid_camera/capture_2d"; + rclcpp::Node::SharedPtr test_node_; + rclcpp::executors::SingleThreadedExecutor executor_; + + static constexpr auto short_wait_duration{std::chrono::milliseconds{500}}; + static constexpr auto default_service_timeout{std::chrono::seconds{10}}; + static constexpr auto capture_service_timeout{std::chrono::seconds{20}}; + static constexpr auto capture_service_name = "capture"; + static constexpr auto capture_and_save_service_name = "capture_and_save"; + static constexpr auto capture_2d_service_name = "capture_2d"; static constexpr auto capture_assistant_suggest_settings_service_name = - "/zivid_camera/capture_assistant/" + "capture_assistant/" "suggest_settings"; - static constexpr auto load_settings_from_file_service_name = - "/zivid_camera/load_settings_from_file"; - static constexpr auto load_settings_2d_from_file_service_name = - "/zivid_camera/load_settings_2d_from_file"; - - static constexpr auto color_camera_info_topic_name = "/zivid_camera/color/camera_info"; - static constexpr auto color_image_color_topic_name = "/zivid_camera/color/image_color"; - static constexpr auto depth_camera_info_topic_name = "/zivid_camera/depth/camera_info"; - static constexpr auto depth_image_topic_name = "/zivid_camera/depth/image"; - static constexpr auto snr_camera_info_topic_name = "/zivid_camera/depth/camera_info"; - static constexpr auto snr_image_topic_name = "/zivid_camera/snr/image"; - static constexpr auto points_xyz_topic_name = "/zivid_camera/points/xyz"; - static constexpr auto points_xyzrgba_topic_name = "/zivid_camera/points/xyzrgba"; - static constexpr auto normals_xyz_topic_name = "/zivid_camera/normals/xyz"; - static constexpr size_t num_settings_acquisition_dr_servers = 10; - static constexpr size_t num_settings_2d_acquisition_dr_servers = 1; - static constexpr auto file_camera_path = "/usr/share/Zivid/data/FileCameraZivid2M70.zfc"; + + static constexpr auto color_camera_info_topic_name = "color/camera_info"; + static constexpr auto color_image_color_topic_name = "color/image_color"; + static constexpr auto depth_camera_info_topic_name = "depth/camera_info"; + static constexpr auto depth_image_topic_name = "depth/image"; + static constexpr auto snr_camera_info_topic_name = "depth/camera_info"; + static constexpr auto snr_image_topic_name = "snr/image"; + static constexpr auto points_xyz_topic_name = "points/xyz"; + static constexpr auto points_xyzrgba_topic_name = "points/xyzrgba"; + static constexpr auto normals_xyz_topic_name = "normals/xyz"; + + static constexpr auto parameter_settings_file_path = "settings_file_path"; + static constexpr auto parameter_settings_yaml = "settings_yaml"; + static constexpr auto parameter_settings_2d_file_path = "settings_2d_file_path"; + static constexpr auto parameter_settings_2d_yaml = "settings_2d_yaml"; + + ZividNodeTest() : test_node_(rclcpp::Node::make_shared("test_node")) + { + executor_.add_node(test_node_); + executor_.add_node(zivid_ros_node); + + // Reset test state + setNodeParameter(parameter_settings_file_path, ""); + setNodeParameter(parameter_settings_yaml, ""); + setNodeParameter(parameter_settings_2d_file_path, ""); + setNodeParameter(parameter_settings_2d_yaml, ""); + } + + template + decltype(auto) doSrvRequest( + const std::string & service, std::shared_ptr request, + std::chrono::milliseconds timeout = default_service_timeout) + { + auto client = test_node_->create_client(service); + if (!client->wait_for_service(timeout)) { + throw std::runtime_error("The service '" + service + "' is not ready"); + } + + auto future = client->async_send_request(request); + + if ( + executor_.spin_until_future_complete(future, timeout) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "Failed to call service " << service); + throw std::runtime_error( + "Failed to invoke service '" + service + "'. No response within timeout."); + } + + return future.get(); + } + + template + decltype(auto) doEmptySrvRequest( + const std::string & service, std::chrono::milliseconds timeout = default_service_timeout) + { + auto request = std::make_shared(); + return doSrvRequest(service, request, timeout); + } + + decltype(auto) doStdSrvsTriggerRequest( + const std::string & service, std::chrono::milliseconds timeout = default_service_timeout) + { + return doEmptySrvRequest(service, timeout); + } template class SubscriptionWrapper { public: - static SubscriptionWrapper make(ros::NodeHandle & nh, const std::string & name) + static SubscriptionWrapper make(rclcpp::Node::SharedPtr node, const std::string & topic) { auto w = SubscriptionWrapper(); - boost::function &)> cb = - [impl = w.impl_.get()](const auto & v) mutable { - impl->num_messages_++; - impl->last_message_ = *v; - }; - w.impl_->subscriber_ = nh.subscribe(name, 1, cb); + std::function &)> cb = [impl_ptr = + w.impl_.get()](auto msg) { + impl_ptr->num_messages_++; + impl_ptr->last_message_ = msg; + }; + w.impl_->subscription_ = node->create_subscription(topic, 10, cb); return w; } - const std::optional & lastMessage() const { return impl_->last_message_; } + decltype(auto) lastMessage() const { return impl_->last_message_; } std::size_t numMessages() const { return impl_->num_messages_; } private: - SubscriptionWrapper() : impl_(std::make_unique()) {} + SubscriptionWrapper() : impl_{std::make_unique()} {} + struct Impl { - Impl() : num_messages_(0) {} - ros::Subscriber subscriber_; - std::optional last_message_; - std::size_t num_messages_; + typename rclcpp::Subscription::SharedPtr subscription_; + typename Type::ConstSharedPtr last_message_; + std::size_t num_messages_ = 0; }; + std::unique_ptr impl_; }; - void enableFirst3DAcquisition() + std::string getNodeStringParameter(const std::string & key) + { + return zivid_ros_node->get_parameter(key).as_string(); + } + + void setNodeParameter(const std::string & key, const std::string & value) + { + auto set_result = zivid_ros_node->set_parameter(rclcpp::Parameter{key, value}); + ASSERT_TRUE(set_result.successful); + ASSERT_EQ(getNodeStringParameter(key), value); + } + + decltype(auto) doCaptureUsingFilePath(const std::string & ymlContent) + { + setNodeParameter(parameter_settings_yaml, ""); + auto tmp_file = TmpFile("settings.yml", ymlContent); + setNodeParameter(parameter_settings_file_path, tmp_file.string()); + return doStdSrvsTriggerRequest(capture_service_name, capture_service_timeout); + } + + decltype(auto) doCapture2DUsingFilePath(const std::string & ymlContent) + { + setNodeParameter(parameter_settings_2d_yaml, ""); + auto tmp_file = TmpFile("settings_2d.yml", ymlContent); + setNodeParameter(parameter_settings_2d_file_path, tmp_file.string()); + return doStdSrvsTriggerRequest(capture_2d_service_name, capture_service_timeout); + } + + decltype(auto) doCaptureUsingYmlString(const std::string & yml) + { + setNodeParameter(parameter_settings_file_path, ""); + setNodeParameter(parameter_settings_yaml, yml); + return doStdSrvsTriggerRequest(capture_service_name, capture_service_timeout); + } + + decltype(auto) doCapture2DUsingYmlString(const std::string & yml) + { + setNodeParameter(parameter_settings_2d_file_path, ""); + setNodeParameter(parameter_settings_2d_yaml, yml); + return doStdSrvsTriggerRequest(capture_2d_service_name, capture_service_timeout); + } + + constexpr decltype(auto) defaultSingleAcquisitionSettingsYml() + { + return + R"( +__version__: + serializer: 1 + data: 17 +Settings: + Acquisitions: + - Acquisition: +)"; + } + + constexpr decltype(auto) defaultSingleAcquisitionSettings2DYml() { - dynamic_reconfigure::Client acquisition_0_client( - "/zivid_camera/settings/" - "acquisition_0/"); - zivid_camera::SettingsAcquisitionConfig acquisition_0_cfg; - ASSERT_TRUE( - acquisition_0_client.getDefaultConfiguration(acquisition_0_cfg, dr_get_max_wait_duration)); - acquisition_0_cfg.enabled = true; - ASSERT_TRUE(acquisition_0_client.setConfiguration(acquisition_0_cfg)); + return + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: +)"; } - void enableFirst2DAcquisition() + decltype(auto) doSingleDefaultAcquisitionCaptureUsingFilePath() { - dynamic_reconfigure::Client acquisition_0_client( - "/zivid_camera/" - "settings_2d/" - "acquisition_0/"); - zivid_camera::Settings2DAcquisitionConfig cfg; - ASSERT_TRUE(acquisition_0_client.getDefaultConfiguration(cfg, dr_get_max_wait_duration)); - cfg.enabled = true; - ASSERT_TRUE(acquisition_0_client.setConfiguration(cfg)); + return doCaptureUsingFilePath(defaultSingleAcquisitionSettingsYml()); } - void enableFirst3DAcquisitionAndCapture() + decltype(auto) doSingleDefaultAcquisitionCapture2DUsingFilePath() { - enableFirst3DAcquisition(); - zivid_camera::Capture capture; - ASSERT_TRUE(ros::service::call(capture_service_name, capture)); - short_wait_duration.sleep(); + return doCapture2DUsingFilePath(defaultSingleAcquisitionSettings2DYml()); + } + + template + auto serializeZividDataModel(const ZividDataModel & dm) + { +#if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) + std::stringstream ss; + Zivid::Detail::save(dm, ss); + return ss.str(); +#else + return dm.serialize(); +#endif + } + + template + auto deserializeZividDataModel(const std::string & serialized) + { +#if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) + ZividDataModel dm; + std::stringstream ss; + ss << serialized; + Zivid::Detail::load(dm, ss); + return dm; +#else + return ZividDataModel::fromSerialized(serialized); +#endif + } + + template + auto verifyTriggerResponseSuccess(const ResponseSharedPtr & response) + { + ASSERT_TRUE(response->success); + ASSERT_EQ(response->message, ""); + } + + template + auto verifyTriggerResponseError(const ResponseSharedPtr & response) + { + ASSERT_FALSE(response->success); + ASSERT_NE(response->message, ""); + } + + template + auto verifyTriggerResponseError(const ResponseSharedPtr & response, const std::string & message) + { + verifyTriggerResponseError(response); + ASSERT_EQ(response->message, message); } template - SubscriptionWrapper subscribe(const std::string & name) + SubscriptionWrapper subscribe(const std::string & topic) { - return SubscriptionWrapper::make(nh_, name); + return SubscriptionWrapper::make(test_node_, topic); } class AllCaptureTopicsSubscriber { public: - AllCaptureTopicsSubscriber(ZividNodeTest & nodeTest) + explicit AllCaptureTopicsSubscriber(ZividNodeTest & nodeTest) : color_camera_info_sub_( - nodeTest.subscribe(color_camera_info_topic_name)), - color_image_color_sub_(nodeTest.subscribe(color_image_color_topic_name)), + nodeTest.subscribe(color_camera_info_topic_name)), + color_image_color_sub_( + nodeTest.subscribe(color_image_color_topic_name)), depth_camera_info_sub_( - nodeTest.subscribe(depth_camera_info_topic_name)), - depth_image_sub_(nodeTest.subscribe(depth_image_topic_name)), - snr_camera_info_sub_(nodeTest.subscribe(snr_camera_info_topic_name)), - snr_image_sub_(nodeTest.subscribe(snr_image_topic_name)), - points_xyz_sub_(nodeTest.subscribe(points_xyz_topic_name)), - points_xyzrgba_sub_(nodeTest.subscribe(points_xyzrgba_topic_name)), - normals_xyz_sub_(nodeTest.subscribe(normals_xyz_topic_name)) + nodeTest.subscribe(depth_camera_info_topic_name)), + depth_image_sub_(nodeTest.subscribe(depth_image_topic_name)), + snr_camera_info_sub_( + nodeTest.subscribe(snr_camera_info_topic_name)), + snr_image_sub_(nodeTest.subscribe(snr_image_topic_name)), + points_xyz_sub_(nodeTest.subscribe(points_xyz_topic_name)), + points_xyzrgba_sub_( + nodeTest.subscribe(points_xyzrgba_topic_name)), + normals_xyz_sub_(nodeTest.subscribe(normals_xyz_topic_name)) { } - SubscriptionWrapper color_camera_info_sub_; - SubscriptionWrapper color_image_color_sub_; - SubscriptionWrapper depth_camera_info_sub_; - SubscriptionWrapper depth_image_sub_; - SubscriptionWrapper snr_camera_info_sub_; - SubscriptionWrapper snr_image_sub_; - SubscriptionWrapper points_xyz_sub_; - SubscriptionWrapper points_xyzrgba_sub_; - SubscriptionWrapper normals_xyz_sub_; + SubscriptionWrapper color_camera_info_sub_; + SubscriptionWrapper color_image_color_sub_; + SubscriptionWrapper depth_camera_info_sub_; + SubscriptionWrapper depth_image_sub_; + SubscriptionWrapper snr_camera_info_sub_; + SubscriptionWrapper snr_image_sub_; + SubscriptionWrapper points_xyz_sub_; + SubscriptionWrapper points_xyzrgba_sub_; + SubscriptionWrapper normals_xyz_sub_; void assert_num_topics_received(std::size_t numTopics) { @@ -220,17 +402,38 @@ class ZividNodeTest : public ZividNodeTestBase } }; - template - void assertArrayFloatEq(const A & actual, const B & expected) const + class AllCapture2DTopicsSubscriber + { + public: + explicit AllCapture2DTopicsSubscriber(ZividNodeTest & node_test) + : color_camera_info_sub_( + node_test.subscribe(color_camera_info_topic_name)), + color_image_color_sub_( + node_test.subscribe(color_image_color_topic_name)) + { + } + + SubscriptionWrapper color_camera_info_sub_; + SubscriptionWrapper color_image_color_sub_; + + void assert_num_topics_received(std::size_t num_topics) + { + ASSERT_EQ(color_camera_info_sub_.numMessages(), num_topics); + ASSERT_EQ(color_image_color_sub_.numMessages(), num_topics); + } + }; + + template + void assertArrayDoubleEq( + const std::array & actual, const std::array & expected) const { - ASSERT_EQ(actual.size(), expected.size()); - for (std::size_t i = 0; i < actual.size(); i++) { - ASSERT_FLOAT_EQ(actual[i], expected[i]); + for (std::size_t i = 0; i < N; i++) { + EXPECT_DOUBLE_EQ(actual[i], expected[i]); } } void assertSensorMsgsPointCloud2Meta( - const sensor_msgs::PointCloud2 & point_cloud, std::size_t width, std::size_t height, + const sensor_msgs::msg::PointCloud2 & point_cloud, std::size_t width, std::size_t height, std::size_t point_step) { ASSERT_EQ(point_cloud.width, width); @@ -243,7 +446,7 @@ class ZividNodeTest : public ZividNodeTestBase } void assertSensorMsgsImageMeta( - const sensor_msgs::Image & image, std::size_t width, std::size_t height, + const sensor_msgs::msg::Image & image, std::size_t width, std::size_t height, std::size_t bytes_per_pixel, const std::string & encoding) { ASSERT_EQ(image.width, width); @@ -255,18 +458,18 @@ class ZividNodeTest : public ZividNodeTestBase } void assertSensorMsgsImageContents( - const sensor_msgs::Image & image, const Zivid::Array2D & expected_rgba) + const sensor_msgs::msg::Image & image, const Zivid::Array2D & expected_rgba) { ASSERT_EQ(image.width, expected_rgba.width()); ASSERT_EQ(image.height, expected_rgba.height()); for (std::size_t i = 0; i < expected_rgba.size(); i++) { - const auto expectedPixel = expected_rgba(i); + const auto expected_pixel = expected_rgba(i); const auto index = i * 4U; - ASSERT_EQ(image.data[index], expectedPixel.r); - ASSERT_EQ(image.data[index + 1], expectedPixel.g); - ASSERT_EQ(image.data[index + 2], expectedPixel.b); - ASSERT_EQ(image.data[index + 3], expectedPixel.a); - ASSERT_EQ(expectedPixel.a, 255); + ASSERT_EQ(image.data[index], expected_pixel.r); + ASSERT_EQ(image.data[index + 1], expected_pixel.g); + ASSERT_EQ(image.data[index + 2], expected_pixel.b); + ASSERT_EQ(image.data[index + 3], expected_pixel.a); + ASSERT_EQ(expected_pixel.a, 255); } } @@ -281,7 +484,7 @@ class ZividNodeTest : public ZividNodeTestBase ASSERT_EQ(field.count, count); } - void assertCameraInfoForFileCamera(const sensor_msgs::CameraInfo & ci) const + void assertCameraInfoForFileCamera(const sensor_msgs::msg::CameraInfo & ci) const { ASSERT_EQ(ci.width, 1944U); ASSERT_EQ(ci.height, 1200U); @@ -290,87 +493,232 @@ class ZividNodeTest : public ZividNodeTestBase // [fx 0 cx] // K = [ 0 fy cy] // [ 0 0 1] - assertArrayFloatEq( - ci.K, std::array{1781.448, 0, 990.49268, 0, 1781.5297, 585.81781, 0, 0, 1}); + assertArrayDoubleEq( + ci.k, + std::array{ + 1781.447998046875, 0, 990.49267578125, 0, 1781.5296630859375, 585.81781005859375, 0, 0, 1}); // R = I - assertArrayFloatEq(ci.R, std::array{1, 0, 0, 0, 1, 0, 0, 0, 1}); + assertArrayDoubleEq(ci.r, std::array{1, 0, 0, 0, 1, 0, 0, 0, 1}); // [fx' 0 cx' Tx] // P = [ 0 fy' cy' Ty] // [ 0 0 1 0] - assertArrayFloatEq( - ci.P, - std::array{1781.448, 0, 990.49268, 0, 0, 1781.5297, 585.81781, 0, 0, 0, 1, 0}); + assertArrayDoubleEq( + ci.p, std::array{ + 1781.447998046875, 0, 990.49267578125, 0, 0, 1781.5296630859375, 585.81781005859375, + 0, 0, 0, 1, 0}); } }; class TestWithFileCamera : public ZividNodeTest { protected: - TestWithFileCamera() : camera_(zivid_.createFileCamera(file_camera_path)) {} - Zivid::Application zivid_; + TestWithFileCamera() + : camera_(zivid_ros_node->zividApplication().createFileCamera(file_camera_path)) + { + } Zivid::Camera camera_; }; -TEST_F(ZividNodeTest, testWaitForNodeReady) +TEST_F(ZividNodeTest, testCaptureServiceReady) { - // This test must be the first test that is run - ASSERT_TRUE(ros::service::waitForService(capture_service_name, node_ready_wait_duration)); + auto client = test_node_->create_client(capture_service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds{45})); } TEST_F(ZividNodeTest, testServiceCameraInfoModelName) { - zivid_camera::CameraInfoModelName model_name; - ASSERT_TRUE(ros::service::call("/zivid_camera/camera_info/model_name", model_name)); - ASSERT_EQ(model_name.response.model_name, std::string("FileCamera-") + ZIVID_CORE_VERSION); + auto model_name_request = + doEmptySrvRequest("camera_info/model_name"); + ASSERT_EQ(model_name_request->model_name, std::string("FileCamera-") + ZIVID_CORE_VERSION); } TEST_F(ZividNodeTest, testServiceCameraInfoSerialNumber) { - zivid_camera::CameraInfoSerialNumber serial_number; - ASSERT_TRUE(ros::service::call("/zivid_camera/camera_info/serial_number", serial_number)); - ASSERT_EQ(serial_number.response.serial_number, "F1"); + auto serial_number_request = + doEmptySrvRequest("camera_info/serial_number"); + ASSERT_EQ(serial_number_request->serial_number, "F1"); } TEST_F(ZividNodeTest, testServiceIsConnected) { - zivid_camera::IsConnected is_connected; - ASSERT_TRUE(ros::service::call("/zivid_camera/is_connected", is_connected)); - ASSERT_EQ(is_connected.response.is_connected, true); + auto is_connected_request = doEmptySrvRequest("is_connected"); + ASSERT_EQ(is_connected_request->is_connected, true); } -TEST_F(ZividNodeTest, testCapturePublishesTopics) +TEST_F(ZividNodeTest, testCaptureConfigurationThrowsIfBothPathAndYmlSet) { - AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this); + auto run_test = [&]( + const auto & service_name, const auto & path_param, const auto & yml_param, + const auto & yml_content) { + auto color_image_sub = subscribe(color_image_color_topic_name); + auto assert_num_topics_received = [&](auto num_topics) { + ASSERT_EQ(color_image_sub.numMessages(), num_topics); + }; + auto tmp_file = TmpFile("settings.yml", yml_content); + setNodeParameter(path_param, tmp_file.string()); + setNodeParameter(yml_param, yml_content); + ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); + assert_num_topics_received(0); + + setNodeParameter(path_param, ""); + ASSERT_TRUE(doStdSrvsTriggerRequest(service_name)); + executor_.spin_some(); + assert_num_topics_received(1); + + setNodeParameter(path_param, tmp_file.string()); + setNodeParameter(yml_param, ""); + ASSERT_TRUE(doStdSrvsTriggerRequest(service_name)); + executor_.spin_some(); + assert_num_topics_received(2); + + setNodeParameter(path_param, tmp_file.string()); + setNodeParameter(yml_param, yml_content); + ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); + executor_.spin_some(); + assert_num_topics_received(2); + + // Reset for next test + setNodeParameter(path_param, ""); + setNodeParameter(yml_param, ""); + ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); + executor_.spin_some(); + assert_num_topics_received(2); + }; - medium_wait_duration.sleep(); - all_capture_topics_subscriber.assert_num_topics_received(0); + run_test( + capture_service_name, parameter_settings_file_path, parameter_settings_yaml, + defaultSingleAcquisitionSettingsYml()); + run_test( + capture_2d_service_name, parameter_settings_2d_file_path, parameter_settings_2d_yaml, + defaultSingleAcquisitionSettings2DYml()); +} - zivid_camera::Capture capture; - // Capture fails when no acquisitions are enabled - ASSERT_FALSE(ros::service::call(capture_service_name, capture)); - short_wait_duration.sleep(); - all_capture_topics_subscriber.assert_num_topics_received(0); +TEST_F(TestWithFileCamera, testCaptureExceptionsReturnErrors) +{ + AllCaptureTopicsSubscriber topics_subscriber(*this); + + const auto settingsYmlInvalid = + R"( +__version__: + serializer: 1 + data: 17 +Settings: + Acquisitions: + - Acquisition: + Experimental: + Engine: stripe + Processing: + Filters: + Experimental: + ContrastDistortion: + Correction: + Enabled: yes + Reflection: + Removal: + Enabled: yes +)"; + + // This capture throws because Stripe engine is unsupported on this file camera. + verifyTriggerResponseError( + doCaptureUsingFilePath(settingsYmlInvalid), + "This file camera can only be used with the phase engine."); + verifyTriggerResponseError( + doCaptureUsingYmlString(settingsYmlInvalid), + "This file camera can only be used with the phase engine."); + topics_subscriber.assert_num_topics_received(0U); + + const auto settingsYmlPhase = + R"( +__version__: + serializer: 1 + data: 17 +Settings: + Acquisitions: + - Acquisition: + Experimental: + Engine: phase +)"; + + verifyTriggerResponseSuccess(doCaptureUsingFilePath(settingsYmlPhase)); + topics_subscriber.assert_num_topics_received(1U); + verifyTriggerResponseSuccess(doCaptureUsingYmlString(settingsYmlPhase)); + topics_subscriber.assert_num_topics_received(2U); +} + +TEST_F(TestWithFileCamera, testCapture2DExceptionsReturnErrors) +{ + AllCapture2DTopicsSubscriber topics_subscriber(*this); + + const auto settings2DYmlInvalid = + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + - Acquisition: +)"; + + // This capture throws because 2D capture does not support multiple acquisitions + verifyTriggerResponseError(doCapture2DUsingFilePath(settings2DYmlInvalid)); + verifyTriggerResponseError(doCapture2DUsingYmlString(settings2DYmlInvalid)); + topics_subscriber.assert_num_topics_received(0U); + + const auto settings2DYmlValid = + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: +)"; + + verifyTriggerResponseSuccess(doCapture2DUsingFilePath(settings2DYmlValid)); + topics_subscriber.assert_num_topics_received(1U); + verifyTriggerResponseSuccess(doCapture2DUsingYmlString(settings2DYmlValid)); + topics_subscriber.assert_num_topics_received(2U); +} - enableFirst3DAcquisition(); +TEST_F(ZividNodeTest, testRepeatedCapturePublishesTopics) +{ + AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this); + all_capture_topics_subscriber.assert_num_topics_received(0); - ASSERT_TRUE(ros::service::call(capture_service_name, capture)); - short_wait_duration.sleep(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); all_capture_topics_subscriber.assert_num_topics_received(1); - ASSERT_TRUE(ros::service::call(capture_service_name, capture)); - short_wait_duration.sleep(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); all_capture_topics_subscriber.assert_num_topics_received(2); - ASSERT_TRUE(ros::service::call(capture_service_name, capture)); - short_wait_duration.sleep(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); all_capture_topics_subscriber.assert_num_topics_received(3); - short_wait_duration.sleep(); + rclcpp::sleep_for(short_wait_duration); all_capture_topics_subscriber.assert_num_topics_received(3); } +TEST_F(ZividNodeTest, testRepeatedCapture2DPublishesTopics) +{ + AllCapture2DTopicsSubscriber all_capture_2d_topics_subscriber(*this); + all_capture_2d_topics_subscriber.assert_num_topics_received(0); + + doSingleDefaultAcquisitionCapture2DUsingFilePath(); + all_capture_2d_topics_subscriber.assert_num_topics_received(1); + + doSingleDefaultAcquisitionCapture2DUsingFilePath(); + all_capture_2d_topics_subscriber.assert_num_topics_received(2); + + doSingleDefaultAcquisitionCapture2DUsingFilePath(); + all_capture_2d_topics_subscriber.assert_num_topics_received(3); + + rclcpp::sleep_for(short_wait_duration); + all_capture_2d_topics_subscriber.assert_num_topics_received(3); +} + class CaptureOutputTest : public TestWithFileCamera { protected: @@ -381,12 +729,6 @@ class CaptureOutputTest : public TestWithFileCamera .pointCloud(); } - Zivid::Frame2D capture2DViaSDKDefaultSettings() - { - return camera_.capture( - Zivid::Settings2D{Zivid::Settings2D::Acquisitions{Zivid::Settings2D::Acquisition{}}}); - } - Zivid::PointCloud captureViaSDK(const Zivid::Settings & settings) { return camera_.capture(settings).pointCloud(); @@ -416,18 +758,18 @@ class CaptureOutputTest : public TestWithFileCamera TEST_F(CaptureOutputTest, testCapturePointsXYZGBA) { - auto points_sub = subscribe(points_xyzrgba_topic_name); + auto points_sub = subscribe(points_xyzrgba_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); - const auto & last_pc2 = points_sub.lastMessage(); - ASSERT_TRUE(last_pc2.has_value()); + const auto last_pc2 = points_sub.lastMessage(); + ASSERT_TRUE(last_pc2); assertSensorMsgsPointCloud2Meta(*last_pc2, 1944U, 1200U, 16U); ASSERT_EQ(last_pc2->fields.size(), 4U); - assertPointCloud2Field(last_pc2->fields[0], "x", 0, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(last_pc2->fields[1], "y", 4, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(last_pc2->fields[2], "z", 8, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(last_pc2->fields[3], "rgba", 12, sensor_msgs::PointField::FLOAT32, 1); + assertPointCloud2Field(last_pc2->fields[0], "x", 0, sensor_msgs::msg::PointField::FLOAT32, 1); + assertPointCloud2Field(last_pc2->fields[1], "y", 4, sensor_msgs::msg::PointField::FLOAT32, 1); + assertPointCloud2Field(last_pc2->fields[2], "z", 8, sensor_msgs::msg::PointField::FLOAT32, 1); + assertPointCloud2Field(last_pc2->fields[3], "rgba", 12, sensor_msgs::msg::PointField::UINT32, 1); const auto point_cloud = captureViaSDKDefaultSettings(); const auto expected_xyzrgba = point_cloud.copyData(); @@ -454,19 +796,19 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZGBA) TEST_F(CaptureOutputTest, testCapturePointsXYZ) { - auto points_sub = subscribe(points_xyz_topic_name); + auto points_sub = subscribe(points_xyz_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); const auto & point_cloud = points_sub.lastMessage(); - ASSERT_TRUE(point_cloud.has_value()); + ASSERT_TRUE(point_cloud); assertSensorMsgsPointCloud2Meta( *point_cloud, 1944U, 1200U, 16U); // 3x4 bytes for xyz + 4 bytes padding (w) = 16 bytes total ASSERT_EQ(point_cloud->fields.size(), 3U); - assertPointCloud2Field(point_cloud->fields[0], "x", 0, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(point_cloud->fields[1], "y", 4, sensor_msgs::PointField::FLOAT32, 1); - assertPointCloud2Field(point_cloud->fields[2], "z", 8, sensor_msgs::PointField::FLOAT32, 1); + assertPointCloud2Field(point_cloud->fields[0], "x", 0, sensor_msgs::msg::PointField::FLOAT32, 1); + assertPointCloud2Field(point_cloud->fields[1], "y", 4, sensor_msgs::msg::PointField::FLOAT32, 1); + assertPointCloud2Field(point_cloud->fields[2], "z", 8, sensor_msgs::msg::PointField::FLOAT32, 1); auto point_cloud_sdk = captureViaSDKDefaultSettings(); auto expected_xyz = point_cloud_sdk.copyData(); @@ -487,29 +829,32 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZ) TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) { - auto points_sub = subscribe(points_xyz_topic_name); - dynamic_reconfigure::Client settings_client( - "/zivid_camera/settings/"); - zivid_camera::SettingsConfig configOriginal; - ASSERT_TRUE(settings_client.getDefaultConfiguration(configOriginal, dr_get_max_wait_duration)); - auto config = configOriginal; - config.region_of_interest_box_enabled = true; - config.region_of_interest_box_point_o_x = -370.5; - config.region_of_interest_box_point_o_y = -288; - config.region_of_interest_box_point_o_z = 886; - config.region_of_interest_box_point_a_x = -354; - config.region_of_interest_box_point_a_y = 191.5; - config.region_of_interest_box_point_a_z = 673; - config.region_of_interest_box_point_b_x = 420; - config.region_of_interest_box_point_b_y = -250; - config.region_of_interest_box_point_b_z = 876.5; - config.region_of_interest_box_extents_min = -2; - config.region_of_interest_box_extents_max = 200.5; - ASSERT_TRUE(settings_client.setConfiguration(config)); - - enableFirst3DAcquisitionAndCapture(); + auto points_sub = subscribe(points_xyz_topic_name); + + const auto settings_yaml = + R"( +__version__: + serializer: 1 + data: 17 +Settings: + Acquisitions: + - Acquisition: + Aperture: __not_set__ + Brightness: __not_set__ + ExposureTime: __not_set__ + Gain: __not_set__ + RegionOfInterest: + Box: + Enabled: yes + Extents: [-2, 200.5] + PointA: [-354, 191.5, 673] + PointB: [420, -250, 876.5] + PointO: [-370.5, -288, 886] +)"; + + doCaptureUsingFilePath(settings_yaml); const auto & point_cloud = points_sub.lastMessage(); - ASSERT_TRUE(point_cloud.has_value()); + ASSERT_TRUE(point_cloud); const auto point_cloud_sdk = captureViaSDK(Zivid::Settings{ Zivid::Settings::Acquisitions{Zivid::Settings::Acquisition{}}, @@ -523,7 +868,7 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) }); auto expected = point_cloud_sdk.copyData(); - const auto numNanZ = [&] { + const auto num_z_nan = [&] { size_t count = 0; for (size_t i = 0; i < expected.size(); ++i) { count += std::isnan(expected(i).z); @@ -532,8 +877,8 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) }(); // Verify that we have some number of points left (to verify that the ROI box did // not set everything to NaN) - ASSERT_GT(numNanZ, 500000); - ASSERT_LT(numNanZ, expected.size() - 500000); + ASSERT_GT(num_z_nan, 500000); + ASSERT_LT(num_z_nan, expected.size() - 500000); for (size_t i = 0; i < expected.size(); ++i) { const uint8_t * point_ptr = &point_cloud->data[i * point_cloud->point_step]; @@ -544,17 +889,15 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) comparePointCoordinate(y, expected(i).y); comparePointCoordinate(z, expected(i).z); } - - ASSERT_TRUE(settings_client.setConfiguration(configOriginal)); } TEST_F(CaptureOutputTest, testCapture3DColorImage) { - auto color_image_sub = subscribe(color_image_color_topic_name); + auto color_image_sub = subscribe(color_image_color_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); const auto image = color_image_sub.lastMessage(); - ASSERT_TRUE(image.has_value()); + ASSERT_TRUE(image); const std::size_t bytes_per_pixel = 4U; assertSensorMsgsImageMeta(*image, 1944U, 1200U, bytes_per_pixel, "rgba8"); @@ -565,12 +908,12 @@ TEST_F(CaptureOutputTest, testCapture3DColorImage) TEST_F(CaptureOutputTest, testCaptureDepthImage) { - auto depth_image_sub = subscribe(depth_image_topic_name); + auto depth_image_sub = subscribe(depth_image_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); const auto image = depth_image_sub.lastMessage(); - ASSERT_TRUE(image.has_value()); + ASSERT_TRUE(image); const std::size_t bytes_per_pixel = 4U; assertSensorMsgsImageMeta(*image, 1944U, 1200U, bytes_per_pixel, "32FC1"); @@ -587,12 +930,12 @@ TEST_F(CaptureOutputTest, testCaptureDepthImage) TEST_F(CaptureOutputTest, testCaptureSNRImage) { - auto snr_image_sub = subscribe(snr_image_topic_name); + auto snr_image_sub = subscribe(snr_image_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); const auto image = snr_image_sub.lastMessage(); - ASSERT_TRUE(image.has_value()); + ASSERT_TRUE(image); const std::size_t bytes_per_pixel = 4U; assertSensorMsgsImageMeta(*image, 1944U, 1200U, bytes_per_pixel, "32FC1"); @@ -609,22 +952,22 @@ TEST_F(CaptureOutputTest, testCaptureSNRImage) TEST_F(CaptureOutputTest, testCaptureNormals) { - auto normals_sub = subscribe(normals_xyz_topic_name); + auto normals_sub = subscribe(normals_xyz_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); const auto & point_cloud = normals_sub.lastMessage(); - ASSERT_TRUE(point_cloud.has_value()); + ASSERT_TRUE(point_cloud); assertSensorMsgsPointCloud2Meta( *point_cloud, 1944U, 1200U, 3U * sizeof(float)); // 12 bytes total ASSERT_EQ(point_cloud->fields.size(), 3U); assertPointCloud2Field( - point_cloud->fields[0], "normal_x", 0, sensor_msgs::PointField::FLOAT32, 1); + point_cloud->fields[0], "normal_x", 0, sensor_msgs::msg::PointField::FLOAT32, 1); assertPointCloud2Field( - point_cloud->fields[1], "normal_y", 4, sensor_msgs::PointField::FLOAT32, 1); + point_cloud->fields[1], "normal_y", 4, sensor_msgs::msg::PointField::FLOAT32, 1); assertPointCloud2Field( - point_cloud->fields[2], "normal_z", 8, sensor_msgs::PointField::FLOAT32, 1); + point_cloud->fields[2], "normal_z", 8, sensor_msgs::msg::PointField::FLOAT32, 1); auto point_cloud_sdk = captureViaSDKDefaultSettings(); auto expected_normal_xyz_before_transform = point_cloud_sdk.copyData(); @@ -647,8 +990,9 @@ TEST_F(CaptureOutputTest, testCaptureNormals) const float normal_z = *reinterpret_cast(&cloud_ptr[8]); const auto & expected_sdk_before_transform = expected_normal_xyz_before_transform(i); - // We do a transform in the ROS driver to scale from mm to meters. However, `expected_normal_xyz` - // are calculated without transform, so we need a slightly higher delta to compare. + // We do a transform in the ROS driver to scale from mm to meters. However, + // `expected_normal_xyz` are calculated without transform, so we need a slightly higher + // delta to compare. constexpr float delta = 0.001f; ASSERT_NO_FATAL_FAILURE(compareFloat(normal_x, expected_sdk_before_transform.x, delta)); ASSERT_NO_FATAL_FAILURE(compareFloat(normal_y, expected_sdk_before_transform.y, delta)); @@ -661,53 +1005,15 @@ TEST_F(CaptureOutputTest, testCaptureNormals) } } -TEST_F(TestWithFileCamera, testSettingsEngine) -{ - enableFirst3DAcquisition(); - auto points_sub = subscribe(points_xyz_topic_name); - - dynamic_reconfigure::Client settings_client( - "/zivid_camera/settings/"); - zivid_camera::SettingsConfig settings_cfg; - ASSERT_TRUE(settings_client.getDefaultConfiguration(settings_cfg, dr_get_max_wait_duration)); - -#if ZIVID_CORE_VERSION_MAJOR >= 3 || \ - (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 12) - ASSERT_EQ(settings_cfg.engine, zivid_camera::Settings_EnginePhase); - settings_cfg.engine = zivid_camera::Settings_EngineStripe; -#else - ASSERT_EQ(settings_cfg.experimental_engine, zivid_camera::Settings_ExperimentalEnginePhase); - settings_cfg.experimental_engine = zivid_camera::Settings_ExperimentalEngineStripe; -#endif - - settings_cfg.processing_filters_reflection_removal_enabled = true; - settings_cfg.processing_filters_experimental_contrast_distortion_correction_enabled = true; - ASSERT_TRUE(settings_client.setConfiguration(settings_cfg)); - - zivid_camera::Capture capture; - // Capture fails here because file camera does not support Stripe engine - ASSERT_FALSE(ros::service::call(capture_service_name, capture)); - ASSERT_EQ(points_sub.numMessages(), 0U); - -#if ZIVID_CORE_VERSION_MAJOR >= 3 || \ - (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 12) - settings_cfg.engine = zivid_camera::Settings_EnginePhase; -#else - settings_cfg.experimental_engine = zivid_camera::Settings_ExperimentalEnginePhase; -#endif - ASSERT_TRUE(settings_client.setConfiguration(settings_cfg)); - ASSERT_TRUE(ros::service::call(capture_service_name, capture)); - short_wait_duration.sleep(); - ASSERT_EQ(points_sub.numMessages(), 1U); -} - TEST_F(ZividNodeTest, testCaptureCameraInfo) { - auto color_camera_info_sub = subscribe(color_camera_info_topic_name); - auto depth_camera_info_sub = subscribe(depth_camera_info_topic_name); - auto snr_camera_info_sub = subscribe(snr_camera_info_topic_name); + auto color_camera_info_sub = + subscribe(color_camera_info_topic_name); + auto depth_camera_info_sub = + subscribe(depth_camera_info_topic_name); + auto snr_camera_info_sub = subscribe(snr_camera_info_topic_name); - enableFirst3DAcquisitionAndCapture(); + doSingleDefaultAcquisitionCaptureUsingFilePath(); ASSERT_EQ(color_camera_info_sub.numMessages(), 1U); ASSERT_EQ(depth_camera_info_sub.numMessages(), 1U); @@ -718,493 +1024,149 @@ TEST_F(ZividNodeTest, testCaptureCameraInfo) assertCameraInfoForFileCamera(*snr_camera_info_sub.lastMessage()); } -TEST_F(CaptureOutputTest, testCapture2D) +class Capture2DOutputTest : public CaptureOutputTest { - auto color_camera_info_sub = subscribe(color_camera_info_topic_name); - auto color_image_color_sub = subscribe(color_image_color_topic_name); - - auto assert_num_topics_received = [&](std::size_t num_topics) { - ASSERT_EQ(color_camera_info_sub.numMessages(), num_topics); - ASSERT_EQ(color_image_color_sub.numMessages(), num_topics); - }; - - short_wait_duration.sleep(); - assert_num_topics_received(0); +public: + void testCapture2D(const std::string & settings_yaml) + { + AllCapture2DTopicsSubscriber all_capture_2d_topics_subscriber(*this); - // Capture fails when no acquisitions are enabled - zivid_camera::Capture2D capture; - ASSERT_FALSE(ros::service::call(capture_2d_service_name, capture)); - short_wait_duration.sleep(); - assert_num_topics_received(0); + all_capture_2d_topics_subscriber.assert_num_topics_received(0); + doCapture2DUsingFilePath(settings_yaml); - enableFirst2DAcquisition(); - ASSERT_TRUE(ros::service::call(capture_2d_service_name, capture)); - short_wait_duration.sleep(); - assert_num_topics_received(1); + rclcpp::sleep_for(short_wait_duration); + all_capture_2d_topics_subscriber.assert_num_topics_received(1); - auto verify_image_and_camera_info = [this](const auto & img, const auto & info) { - assertCameraInfoForFileCamera(info); - assertSensorMsgsImageMeta(img, 1944U, 1200U, 4U, "rgba8"); - assertSensorMsgsImageContents(img, capture2DViaSDKDefaultSettings().imageRGBA()); - }; + const auto frame_2d_from_sdk = + camera_.capture(deserializeZividDataModel(settings_yaml)); - verify_image_and_camera_info( - *color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage()); + auto verify_image_and_camera_info = [&](const auto & img, const auto & info) { + assertCameraInfoForFileCamera(info); + assertSensorMsgsImageMeta(img, 1944U, 1200U, 4U, "rgba8"); + assertSensorMsgsImageContents(img, frame_2d_from_sdk.imageRGBA()); + }; - short_wait_duration.sleep(); - assert_num_topics_received(1); + verify_image_and_camera_info( + *all_capture_2d_topics_subscriber.color_image_color_sub_.lastMessage(), + *all_capture_2d_topics_subscriber.color_camera_info_sub_.lastMessage()); - ASSERT_TRUE(ros::service::call(capture_2d_service_name, capture)); - short_wait_duration.sleep(); - assert_num_topics_received(2); - verify_image_and_camera_info( - *color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage()); -} + rclcpp::sleep_for(short_wait_duration); + all_capture_2d_topics_subscriber.assert_num_topics_received(1); -class CaptureAndSaveTest : public TestWithFileCamera -{ -protected: - void capture_and_save_to_path(const std::string & file_path, bool expect_success) - { - AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this); - medium_wait_duration.sleep(); + doCapture2DUsingFilePath(settings_yaml); - enableFirst3DAcquisition(); - zivid_camera::CaptureAndSave capture_and_save; - capture_and_save.request.file_path = file_path; - all_capture_topics_subscriber.assert_num_topics_received(0); - if (expect_success) { - ASSERT_TRUE(ros::service::call(capture_and_save_service_name, capture_and_save)); - ASSERT_TRUE(boost::filesystem::exists(file_path)); - } else { - ASSERT_FALSE(ros::service::call(capture_and_save_service_name, capture_and_save)); - ASSERT_FALSE(boost::filesystem::exists(file_path)); - } - medium_wait_duration.sleep(); - all_capture_topics_subscriber.assert_num_topics_received(1); + rclcpp::sleep_for(short_wait_duration); + all_capture_2d_topics_subscriber.assert_num_topics_received(2); + verify_image_and_camera_info( + *all_capture_2d_topics_subscriber.color_image_color_sub_.lastMessage(), + *all_capture_2d_topics_subscriber.color_camera_info_sub_.lastMessage()); } }; -TEST_F(CaptureAndSaveTest, testCaptureAndSaveNoPath) { capture_and_save_to_path("", false); } - -TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidPath) -{ - capture_and_save_to_path("invalid_path", false); -} - -TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidExtension) -{ - capture_and_save_to_path("/tmp/invalid_extension.wrong", false); -} - -TEST_F(CaptureAndSaveTest, testCaptureAndSaveZDF) -{ - capture_and_save_to_path("/tmp/valid.zdf", true); -} - -TEST_F(CaptureAndSaveTest, testCaptureAndSavePLY) +TEST_F(Capture2DOutputTest, testCapture2DDefaultSettings) { - capture_and_save_to_path("/tmp/valid.ply", true); + testCapture2D( + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + Aperture: 4 + )"); } -TEST_F(CaptureAndSaveTest, testCaptureAndSavePCD) +TEST_F(Capture2DOutputTest, testCapture2DCustomColorBalance) { - capture_and_save_to_path("/tmp/valid.pcd", true); + testCapture2D( + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + Aperture: 4 + Processing: + Color: + Balance: + Red: 2 + Blue: 3 + Green: 4 + )"); } -class DynamicReconfigureMinMaxDefaultTest : public TestWithFileCamera -{ -protected: - template - typename Setting::ValueType sdkDefaultValue() - { - return Zivid::Experimental::SettingsInfo::defaultValue(camera_.info()).value(); - } - - template - auto sdkValidRange() - { - return Zivid::Experimental::SettingsInfo::validRange(camera_.info()); - } - - template - void testGeneralMinMaxDefault(const std::string & service_name) - { - ASSERT_TRUE( - ros::service::waitForService(service_name + "/set_parameters", short_wait_duration)); - - dynamic_reconfigure::Client client(service_name); - - ConfigType default_cfg; - ASSERT_TRUE(client.getDefaultConfiguration(default_cfg, dr_get_max_wait_duration)); - ConfigType min_cfg; - ASSERT_TRUE(client.getMinConfiguration(min_cfg, dr_get_max_wait_duration)); - ConfigType max_cfg; - ASSERT_TRUE(client.getMaxConfiguration(max_cfg, dr_get_max_wait_duration)); - - using BlueBalance = typename ZividSettingsType::Processing::Color::Balance::Blue; - ASSERT_EQ(default_cfg.processing_color_balance_blue, sdkDefaultValue()); - ASSERT_EQ(min_cfg.processing_color_balance_blue, sdkValidRange().min()); - ASSERT_EQ(max_cfg.processing_color_balance_blue, sdkValidRange().max()); - - using GreenBalance = typename ZividSettingsType::Processing::Color::Balance::Green; - ASSERT_EQ(default_cfg.processing_color_balance_green, sdkDefaultValue()); - ASSERT_EQ(min_cfg.processing_color_balance_green, sdkValidRange().min()); - ASSERT_EQ(max_cfg.processing_color_balance_green, sdkValidRange().max()); - - using RedBalance = typename ZividSettingsType::Processing::Color::Balance::Red; - ASSERT_EQ(default_cfg.processing_color_balance_red, sdkDefaultValue()); - ASSERT_EQ(min_cfg.processing_color_balance_red, sdkValidRange().min()); - ASSERT_EQ(max_cfg.processing_color_balance_red, sdkValidRange().max()); - - if constexpr (std::is_same_v) { - using OutlierRemovalThreshold = - typename ZividSettingsType::Processing::Filters::Outlier::Removal::Threshold; - ASSERT_EQ( - default_cfg.processing_filters_outlier_removal_threshold, - sdkDefaultValue()); - ASSERT_EQ( - min_cfg.processing_filters_outlier_removal_threshold, - sdkValidRange().min()); - ASSERT_EQ( - max_cfg.processing_filters_outlier_removal_threshold, - sdkValidRange().max()); - } - } - - template - void testAcquisitionMinMaxDefault(const std::string & prefix, std::size_t num_acquisition_servers) - { - for (std::size_t i = 0; i < num_acquisition_servers; i++) { - ASSERT_TRUE(ros::service::waitForService( - prefix + "acquisition_" + std::to_string(i) + "/set_parameters", short_wait_duration)); - - dynamic_reconfigure::Client client( - prefix + "acquisition_" + std::to_string(i) + "/"); - ConfigType default_cfg; - ASSERT_TRUE(client.getDefaultConfiguration(default_cfg, dr_get_max_wait_duration)); - ConfigType min_cfg; - ASSERT_TRUE(client.getMinConfiguration(min_cfg, dr_get_max_wait_duration)); - ConfigType max_cfg; - ASSERT_TRUE(client.getMaxConfiguration(max_cfg, dr_get_max_wait_duration)); - - ASSERT_EQ(default_cfg.enabled, false); - - using Aperture = typename ZividSettingsType::Acquisition::Aperture; - ASSERT_EQ(default_cfg.aperture, sdkDefaultValue()); - ASSERT_EQ(min_cfg.aperture, sdkValidRange().min()); - ASSERT_EQ(max_cfg.aperture, sdkValidRange().max()); - - using Brightness = typename ZividSettingsType::Acquisition::Brightness; - ASSERT_EQ(default_cfg.brightness, sdkDefaultValue()); - ASSERT_EQ(min_cfg.brightness, sdkValidRange().min()); - ASSERT_EQ(max_cfg.brightness, sdkValidRange().max()); - - using ExposureTime = typename ZividSettingsType::Acquisition::ExposureTime; - ASSERT_EQ(default_cfg.exposure_time, sdkDefaultValue().count()); - ASSERT_EQ(min_cfg.exposure_time, sdkValidRange().min().count()); - ASSERT_EQ(max_cfg.exposure_time, sdkValidRange().max().count()); - - using Gain = typename ZividSettingsType::Acquisition::Gain; - ASSERT_EQ(default_cfg.gain, sdkDefaultValue()); - ASSERT_EQ(min_cfg.gain, sdkValidRange().min()); - ASSERT_EQ(max_cfg.gain, sdkValidRange().max()); - } - ASSERT_FALSE(ros::service::waitForService( - prefix + "acquisition_" + std::to_string(num_acquisition_servers) + "/set_parameters", - short_wait_duration)); - } -}; - -TEST_F(DynamicReconfigureMinMaxDefaultTest, testDynamicReconfigureSettingsMinMaxDefaultValue) -{ - // Test the default, min and max configuration of the file camera used in the test suite. This - // file camera is of model "Zivid One". - testGeneralMinMaxDefault( - "/zivid_camera/settings/"); - testAcquisitionMinMaxDefault( - "/zivid_camera/settings/", 10); - - testGeneralMinMaxDefault( - "/zivid_camera/settings_2d/"); - testAcquisitionMinMaxDefault( - "/zivid_camera/" - "settings_2d/", - 1); -} - -class TestWithSettingsClients : public TestWithFileCamera +class CaptureAndSaveTest : public TestWithFileCamera { protected: - TestWithSettingsClients() - : settings_client_("/zivid_camera/settings"), settings_2d_client_("/zivid_camera/settings_2d") + void captureAndSaveToPath(const std::string & file_path, bool expect_success) { - settings_acquisition_clients_.reserve(num_settings_acquisition_dr_servers); - for (std::size_t i = 0; i < num_settings_acquisition_dr_servers; i++) { - using Client = dynamic_reconfigure::Client; - settings_acquisition_clients_.emplace_back( - std::make_unique("/zivid_camera/settings/acquisition_" + std::to_string(i))); - } - settings_2d_acquisition_clients_.reserve(num_settings_2d_acquisition_dr_servers); - for (std::size_t i = 0; i < num_settings_2d_acquisition_dr_servers; i++) { - using Client = dynamic_reconfigure::Client; - settings_2d_acquisition_clients_.emplace_back( - std::make_unique("/zivid_camera/settings_2d/acquisition_" + std::to_string(i))); - } - } + AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this); - template - auto settingsConfig() - { - if constexpr (std::is_same_v) { - zivid_camera::SettingsConfig cfg; - EXPECT_TRUE(settings_client_.getCurrentConfiguration(cfg, dr_get_max_wait_duration)); - return cfg; - } else if constexpr (std::is_same_v) { - zivid_camera::Settings2DConfig cfg; - EXPECT_TRUE(settings_2d_client_.getCurrentConfiguration(cfg, dr_get_max_wait_duration)); - return cfg; - } else { - static_assert(DependentFalse::value, "Unsupported ZividSettingsType"); - } - } + setNodeParameter(parameter_settings_yaml, defaultSingleAcquisitionSettingsYml()); - template - auto settingsAcquisitionConfig(std::size_t i) const - { - if constexpr (std::is_same_v) { - zivid_camera::SettingsAcquisitionConfig cfg; - EXPECT_TRUE( - settings_acquisition_clients_[i]->getCurrentConfiguration(cfg, dr_get_max_wait_duration)); - return cfg; - } else if constexpr (std::is_same_v) { - zivid_camera::Settings2DAcquisitionConfig cfg; - EXPECT_TRUE(settings_2d_acquisition_clients_[i]->getCurrentConfiguration( - cfg, dr_get_max_wait_duration)); - return cfg; - } else { - static_assert(DependentFalse::value, "Unsupported ZividSettingsType"); - } - } + auto request = std::make_shared(); + request->file_path = file_path; - template - auto numEnabledAcquisitions() const - { - std::size_t enabled_acquisitions = 0; - for (std::size_t i = 0; i < maxAllowedAcquisitions(); i++) { - if (settingsAcquisitionConfig(i).enabled) { - enabled_acquisitions++; - } + if (std::filesystem::exists(file_path)) { + std::filesystem::remove(file_path); } - return enabled_acquisitions; - } - - template - std::size_t maxAllowedAcquisitions() const - { - if constexpr (std::is_same_v) { - return num_settings_acquisition_dr_servers; - } else if constexpr (std::is_same_v) { - return num_settings_2d_acquisition_dr_servers; + all_capture_topics_subscriber.assert_num_topics_received(0); + if (expect_success) { + auto response = doSrvRequest( + capture_and_save_service_name, request, std::chrono::seconds{10}); + ASSERT_TRUE(response); + verifyTriggerResponseSuccess(response); + ASSERT_TRUE(std::filesystem::exists(file_path)); } else { - static_assert(DependentFalse::value, "Unsupported ZividSettingsType"); - } - } - - template - void compareSettingsWithNodeState(const ZividSettingsType & settings) - { - compareSettingsConfigWithSettings(settings, settingsConfig()); - - const auto & acquisitions = settings.acquisitions(); - ASSERT_EQ(acquisitions.size(), numEnabledAcquisitions()); - - for (std::size_t i = 0; i < acquisitions.size(); i++) { - compareSettingsAcquisitionConfigWithSettings( - acquisitions[i], settingsAcquisitionConfig(i)); - } - for (std::size_t i = acquisitions.size(); i < maxAllowedAcquisitions(); - i++) { - ASSERT_EQ(false, settingsAcquisitionConfig(i).enabled); - } - } - -private: - template - void compareSettingsAcquisitionConfigWithSettings( - const ZividSettingsAcquisitionType & a, const CfgType & cfg) const - { - ASSERT_EQ(true, cfg.enabled); - ASSERT_EQ(a.aperture().value(), cfg.aperture); - ASSERT_EQ(a.brightness().value(), cfg.brightness); - ASSERT_EQ(a.exposureTime().value().count(), cfg.exposure_time); - ASSERT_EQ(a.gain().value(), cfg.gain); - } - - void compareSettingsConfigWithSettings( - const Zivid::Settings & s, const zivid_camera::SettingsConfig & cfg) const - { - const auto & color = s.processing().color(); - ASSERT_EQ(color.balance().blue().value(), cfg.processing_color_balance_blue); - ASSERT_EQ(color.balance().green().value(), cfg.processing_color_balance_green); - ASSERT_EQ(color.balance().red().value(), cfg.processing_color_balance_red); - ASSERT_EQ(color.gamma().value(), cfg.processing_color_gamma); - - const auto & filters = s.processing().filters(); - ASSERT_EQ( - filters.noise().removal().isEnabled().value(), cfg.processing_filters_noise_removal_enabled); - ASSERT_EQ( - filters.noise().removal().threshold().value(), - cfg.processing_filters_noise_removal_threshold); - ASSERT_EQ( - filters.smoothing().gaussian().isEnabled().value(), - cfg.processing_filters_smoothing_gaussian_enabled); - ASSERT_EQ( - filters.smoothing().gaussian().sigma().value(), - cfg.processing_filters_smoothing_gaussian_sigma); - ASSERT_EQ( - filters.outlier().removal().isEnabled().value(), - cfg.processing_filters_outlier_removal_enabled); - ASSERT_EQ( - filters.outlier().removal().threshold().value(), - cfg.processing_filters_outlier_removal_threshold); - ASSERT_EQ( - filters.reflection().removal().isEnabled().value(), - cfg.processing_filters_reflection_removal_enabled); - } - - void compareSettingsConfigWithSettings( - const Zivid::Settings2D & s, const zivid_camera::Settings2DConfig & cfg) const - { - const auto & color = s.processing().color(); - ASSERT_EQ(color.balance().blue().value(), cfg.processing_color_balance_blue); - ASSERT_EQ(color.balance().green().value(), cfg.processing_color_balance_green); - ASSERT_EQ(color.balance().red().value(), cfg.processing_color_balance_red); - ASSERT_EQ(color.gamma().value(), cfg.processing_color_gamma); - } - -private: - dynamic_reconfigure::Client settings_client_; - std::vector>> - settings_acquisition_clients_; - dynamic_reconfigure::Client settings_2d_client_; - std::vector< - std::unique_ptr>> - settings_2d_acquisition_clients_; -}; - -class LoadSettingsTest : public TestWithSettingsClients -{ -protected: - template - void testLoadSettingsFromFile( - const std::string & serviceName, const std::vector & fileNames) - { - ASSERT_TRUE(ros::service::waitForService(serviceName, short_wait_duration)); - - auto testLoadSettings = [&](const auto & fileName) { - CmdType cmd; - cmd.request.file_path = testDataDir() + "/settings/" + fileName; - ASSERT_TRUE(ros::service::call(serviceName, cmd)); - short_wait_duration.sleep(); - - auto expectedSettings = ZividSettingsType{cmd.request.file_path}; - recursivelyFillInUnsetWithCameraDefault(expectedSettings, camera_.info()); - compareSettingsWithNodeState(expectedSettings); - }; - - for (const auto & fileName : fileNames) { - testLoadSettings(fileName); - } - } - - template - void testLoadInvalidSettingsGivesError(const std::string & serviceName) - { - ASSERT_TRUE(ros::service::waitForService(serviceName, short_wait_duration)); - CmdType cmd; - cmd.request.file_path = testDataDir() + "/settings/invalid_file.yml"; - ASSERT_TRUE(boost::filesystem::exists(cmd.request.file_path)); - ASSERT_FALSE(ros::service::call(serviceName, cmd)); - } - - template - void testLoadNonExistentFileGivesError(const std::string & serviceName) - { - ASSERT_TRUE(ros::service::waitForService(serviceName, short_wait_duration)); - CmdType cmd; - cmd.request.file_path = "/tmp/foo/bar"; - ASSERT_FALSE(boost::filesystem::exists(cmd.request.file_path)); - ASSERT_FALSE(ros::service::call(serviceName, cmd)); - } - -private: - template - static void recursivelyFillInUnsetWithCameraDefault( - Node & node, const Zivid::CameraInfo & cameraInfo) - { - if constexpr ( - Node::nodeType == Zivid::DataModel::NodeType::group || - Node::nodeType == Zivid::DataModel::NodeType::leafDataModelList) { - node.forEach([&cameraInfo](auto & child) { - recursivelyFillInUnsetWithCameraDefault(child, cameraInfo); - }); - } else if (!node.hasValue()) { - static_assert(Node::nodeType == Zivid::DataModel::NodeType::leafValue); - node = Zivid::Experimental::SettingsInfo::defaultValue(cameraInfo); + auto response = doSrvRequest( + capture_and_save_service_name, request, std::chrono::seconds{10}); + ASSERT_TRUE(response); + verifyTriggerResponseError(response); + ASSERT_FALSE(std::filesystem::exists(file_path)); } + executor_.spin_some(); + all_capture_topics_subscriber.assert_num_topics_received(1); } }; -TEST_F(LoadSettingsTest, testLoadSettingsFromFile) -{ - testLoadSettingsFromFile( - load_settings_from_file_service_name, - {"3d/single.yml", "3d/hdr.yml", "3d/hdr_with_not_set_values.yml", "3d/single.yml"}); -} +TEST_F(CaptureAndSaveTest, testCaptureAndSaveEmptyPathProvided) { captureAndSaveToPath("", false); } -TEST_F(LoadSettingsTest, testLoadSettings2DFromFile) +TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidPath) { - testLoadSettingsFromFile( - load_settings_2d_from_file_service_name, - {"2d/single_1.yml", "2d/single_2.yml", "2d/single_with_not_set_values.yml", "2d/single_1.yml"}); + captureAndSaveToPath("invalid_path", false); } -TEST_F(LoadSettingsTest, testLoadSettingsFromInvalidFileGivesError) +TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidExtension) { - testLoadInvalidSettingsGivesError( - load_settings_from_file_service_name); + captureAndSaveToPath(getTemporaryFilePath("invalid_extension.wrong").string(), false); } -TEST_F(LoadSettingsTest, testLoadSettings2DFromInvalidFileGivesError) +TEST_F(CaptureAndSaveTest, testCaptureAndSaveZDF) { - testLoadInvalidSettingsGivesError( - load_settings_2d_from_file_service_name); + captureAndSaveToPath(getTemporaryFilePath("valid.zdf").string(), true); } -TEST_F(LoadSettingsTest, testLoadSettingsFromNonExistentFileGivesError) +TEST_F(CaptureAndSaveTest, testCaptureAndSavePLY) { - testLoadNonExistentFileGivesError( - load_settings_from_file_service_name); + captureAndSaveToPath(getTemporaryFilePath("valid.ply").string(), true); } -TEST_F(LoadSettingsTest, testLoadSettings2DFromNonExistentFileGivesError) +TEST_F(CaptureAndSaveTest, testCaptureAndSavePCD) { - testLoadNonExistentFileGivesError( - load_settings_2d_from_file_service_name); + captureAndSaveToPath(getTemporaryFilePath("valid.pcd").string(), true); } -class ZividCATest : public TestWithSettingsClients +class ZividCATest : public CaptureOutputTest { protected: Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency - toAPIAmbientLightFrequency( - zivid_camera::CaptureAssistantSuggestSettings::Request::_ambient_light_frequency_type - ambient_light_frequency) + toAPIAmbientLightFrequency(int ambient_light_frequency) { + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; using AmbientLightFrequency = Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency; - using Request = zivid_camera::CaptureAssistantSuggestSettings::Request; switch (ambient_light_frequency) { case Request::AMBIENT_LIGHT_FREQUENCY_NONE: return AmbientLightFrequency::none; @@ -1214,105 +1176,136 @@ class ZividCATest : public TestWithSettingsClients return AmbientLightFrequency::hz60; } throw std::runtime_error( - "Could not convert value " + std::to_string(ambient_light_frequency) + " to API enum."); + "Could not convert value " + std::to_string(ambient_light_frequency) + " to Zivid API enum."); + } + + decltype(auto) doCaptureAssistantRequest( + int ambient_light_frequency, std::chrono::milliseconds duration) + { + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + auto request = std::make_shared(); + request->ambient_light_frequency = ambient_light_frequency; + request->max_capture_time = rclcpp::Duration{duration}; + + return doSrvRequest( + capture_assistant_suggest_settings_service_name, request); } void performSuggestSettingsAndCompareWithCppAPI( - ros::Duration max_capture_time, - zivid_camera::CaptureAssistantSuggestSettings::Request::_ambient_light_frequency_type - ambient_light_frequency) + std::chrono::milliseconds max_capture_time, int ambient_light_frequency) { - zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = max_capture_time; - srv.request.ambient_light_frequency = ambient_light_frequency; - ASSERT_TRUE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); - short_wait_duration.sleep(); + // Set both settings_file and settings_yaml before calling the CA service + setNodeParameter(parameter_settings_file_path, "foo"); + setNodeParameter(parameter_settings_yaml, "bar"); + + auto points_sub = subscribe(points_xyz_topic_name); + + doCaptureAssistantRequest(ambient_light_frequency, max_capture_time); Zivid::CaptureAssistant::SuggestSettingsParameters suggest_settings_parameters{ - Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime{ - std::chrono::round(SecondsD{max_capture_time.toSec()})}, + Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime{max_capture_time}, toAPIAmbientLightFrequency(ambient_light_frequency)}; - const auto api_settings = + const auto api_suggested_settings = Zivid::CaptureAssistant::suggestSettings(camera_, suggest_settings_parameters); - compareSettingsWithNodeState(api_settings); + + executor_.spin_some(); + + ASSERT_EQ( + getNodeStringParameter(parameter_settings_yaml), + serializeZividDataModel(api_suggested_settings)); + // settings_file_path parameter has been reset to empty + ASSERT_EQ(getNodeStringParameter(parameter_settings_file_path), ""); + ASSERT_EQ(points_sub.numMessages(), 0U); + + // Triggering capture now should work + doStdSrvsTriggerRequest(capture_service_name, capture_service_timeout); + executor_.spin_some(); + ASSERT_EQ(points_sub.numMessages(), 1U); } }; TEST_F(ZividCATest, testCaptureAssistantServiceAvailable) { - ASSERT_TRUE(ros::service::waitForService( - capture_assistant_suggest_settings_service_name, short_wait_duration)); + auto client = test_node_->create_client( + capture_assistant_suggest_settings_service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds{3})); } TEST_F(ZividCATest, testDifferentMaxCaptureTimeAndAmbientLightFrequency) { - using Request = zivid_camera::CaptureAssistantSuggestSettings::Request; - for (double max_capture_time : {0.2, 1.2, 10.0}) { + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + for (std::chrono::milliseconds max_capture_time : + {std::chrono::milliseconds{200}, std::chrono::milliseconds{1'200}, + std::chrono::milliseconds{10'000}}) { for (auto ambient_light_frequency : {Request::AMBIENT_LIGHT_FREQUENCY_NONE, Request::AMBIENT_LIGHT_FREQUENCY_50HZ, Request::AMBIENT_LIGHT_FREQUENCY_60HZ}) { - performSuggestSettingsAndCompareWithCppAPI( - ros::Duration{max_capture_time}, ambient_light_frequency); + performSuggestSettingsAndCompareWithCppAPI(max_capture_time, ambient_light_frequency); } } } -TEST_F(ZividCATest, testGoingFromMultipleAcquisitionsTo1Acquisition) +TEST_F(ZividCATest, testCaptureAssistantWithMaxCaptureTimeZeroFails) { - using Request = zivid_camera::CaptureAssistantSuggestSettings::Request; - performSuggestSettingsAndCompareWithCppAPI( - ros::Duration{10.0}, Request::AMBIENT_LIGHT_FREQUENCY_NONE); - ASSERT_GT(numEnabledAcquisitions(), 1U); - - performSuggestSettingsAndCompareWithCppAPI( - ros::Duration{0.2}, Request::AMBIENT_LIGHT_FREQUENCY_NONE); - ASSERT_EQ(numEnabledAcquisitions(), 1U); + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + ASSERT_THROW( + doCaptureAssistantRequest(Request::AMBIENT_LIGHT_FREQUENCY_NONE, std::chrono::milliseconds{0}), + std::exception); } -TEST_F(ZividCATest, testCaptureAssistantWithInvalidMaxCaptureTimeFails) +TEST_F(ZividCATest, testCaptureAssistantWithMaxCaptureTimeMinMax) { - zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = ros::Duration{0.0}; - ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); - + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + constexpr auto small_delta = std::chrono::milliseconds{1}; const auto valid_range = Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime::validRange(); - const auto small_delta = std::chrono::milliseconds{1}; - srv.request.max_capture_time = toRosDuration(valid_range.min() - small_delta); - ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); - srv.request.max_capture_time = toRosDuration(valid_range.max() + small_delta); - ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); - - srv.request.max_capture_time = toRosDuration(valid_range.max()); - ASSERT_TRUE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); + + ASSERT_THROW( + doCaptureAssistantRequest( + Request::AMBIENT_LIGHT_FREQUENCY_NONE, valid_range.min() - small_delta), + std::exception); + + ASSERT_TRUE(doCaptureAssistantRequest(Request::AMBIENT_LIGHT_FREQUENCY_NONE, valid_range.min())); + + ASSERT_THROW( + doCaptureAssistantRequest( + Request::AMBIENT_LIGHT_FREQUENCY_NONE, valid_range.max() + small_delta), + std::exception); + + ASSERT_TRUE(doCaptureAssistantRequest(Request::AMBIENT_LIGHT_FREQUENCY_NONE, valid_range.max())); } TEST_F(ZividCATest, testCaptureAssistantDefaultAmbientLightFrequencyWorks) { - zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = ros::Duration{1.0}; - ASSERT_TRUE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + auto request = std::make_shared(); + request->max_capture_time = rclcpp::Duration{std::chrono::seconds{1}}; + ASSERT_TRUE(doSrvRequest( + capture_assistant_suggest_settings_service_name, request)); } TEST_F(ZividCATest, testCaptureAssistantInvalidAmbientLightFrequencyFails) { - zivid_camera::CaptureAssistantSuggestSettings srv; - srv.request.max_capture_time = ros::Duration{1.0}; - srv.request.ambient_light_frequency = 255; - ASSERT_FALSE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); - - srv.request.ambient_light_frequency = - zivid_camera::CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; - ASSERT_TRUE(ros::service::call(capture_assistant_suggest_settings_service_name, srv)); + using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + ASSERT_THROW(doCaptureAssistantRequest(255, std::chrono::seconds{1}), std::exception); + ASSERT_TRUE( + doCaptureAssistantRequest(Request::AMBIENT_LIGHT_FREQUENCY_60HZ, std::chrono::seconds{1})); } int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_zivid_camera"); - ros::AsyncSpinner spinner(1); - spinner.start(); - ros::NodeHandle nh; - return RUN_ALL_TESTS(); + rclcpp::init(argc, argv); + + auto node_options = + rclcpp::NodeOptions{}.append_parameter_override("file_camera_path", file_camera_path); + zivid_ros_node = std::make_shared(node_options); + + const auto return_code = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + zivid_ros_node.reset(); + + return return_code; } diff --git a/zivid_camera/test/test_zivid_camera.test b/zivid_camera/test/test_zivid_camera.test deleted file mode 100644 index 3d3e0f5e..00000000 --- a/zivid_camera/test/test_zivid_camera.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - diff --git a/zivid_interfaces/CMakeLists.txt b/zivid_interfaces/CMakeLists.txt new file mode 100644 index 00000000..454d17ea --- /dev/null +++ b/zivid_interfaces/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) + +project(zivid_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(zivid_interfaces + "srv/CaptureAndSave.srv" + "srv/CaptureAssistantSuggestSettings.srv" + "srv/CameraInfoModelName.srv" + "srv/CameraInfoSerialNumber.srv" + "srv/IsConnected.srv" + DEPENDENCIES + builtin_interfaces) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() \ No newline at end of file diff --git a/zivid_interfaces/package.xml b/zivid_interfaces/package.xml new file mode 100644 index 00000000..7ce6b0c3 --- /dev/null +++ b/zivid_interfaces/package.xml @@ -0,0 +1,26 @@ + + + + zivid_interfaces + 3.0.0 + Zivid interfaces + Zivid + BSD3 + https://zivid.com + https://github.com/zivid/zivid-ros + https://github.com/zivid/zivid-ros/issues + + ament_cmake + + builtin_interfaces + rosidl_default_generators + + builtin_interfaces + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/zivid_camera/srv/CameraInfoModelName.srv b/zivid_interfaces/srv/CameraInfoModelName.srv similarity index 100% rename from zivid_camera/srv/CameraInfoModelName.srv rename to zivid_interfaces/srv/CameraInfoModelName.srv diff --git a/zivid_camera/srv/CameraInfoSerialNumber.srv b/zivid_interfaces/srv/CameraInfoSerialNumber.srv similarity index 100% rename from zivid_camera/srv/CameraInfoSerialNumber.srv rename to zivid_interfaces/srv/CameraInfoSerialNumber.srv diff --git a/zivid_interfaces/srv/CaptureAndSave.srv b/zivid_interfaces/srv/CaptureAndSave.srv new file mode 100644 index 00000000..0d20693b --- /dev/null +++ b/zivid_interfaces/srv/CaptureAndSave.srv @@ -0,0 +1,4 @@ +string file_path +--- +bool success +string message diff --git a/zivid_camera/srv/CaptureAssistantSuggestSettings.srv b/zivid_interfaces/srv/CaptureAssistantSuggestSettings.srv similarity index 67% rename from zivid_camera/srv/CaptureAssistantSuggestSettings.srv rename to zivid_interfaces/srv/CaptureAssistantSuggestSettings.srv index 56a35ad7..ca79fe9e 100644 --- a/zivid_camera/srv/CaptureAssistantSuggestSettings.srv +++ b/zivid_interfaces/srv/CaptureAssistantSuggestSettings.srv @@ -2,6 +2,7 @@ uint8 AMBIENT_LIGHT_FREQUENCY_NONE=0 uint8 AMBIENT_LIGHT_FREQUENCY_50HZ=1 uint8 AMBIENT_LIGHT_FREQUENCY_60HZ=2 -duration max_capture_time +builtin_interfaces/Duration max_capture_time uint8 ambient_light_frequency --- +string suggested_settings \ No newline at end of file diff --git a/zivid_camera/srv/IsConnected.srv b/zivid_interfaces/srv/IsConnected.srv similarity index 100% rename from zivid_camera/srv/IsConnected.srv rename to zivid_interfaces/srv/IsConnected.srv diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index cd41f6b6..2d841420 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -1,82 +1,54 @@ cmake_minimum_required(VERSION 3.5) project(zivid_samples) -set(CMAKE_CXX_STANDARD 11) -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - zivid_camera - dynamic_reconfigure -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() -catkin_package( - CATKIN_DEPENDS zivid_camera -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -################# -## C++ Samples ## -################# +function(zivid_add_cpp_sample sample_name) + cmake_parse_arguments(PARSE_ARGV 1 ARG "" "" "DEPENDENCIES") + set(target_name "${sample_name}_cpp") -function(register_cpp_sample) - cmake_parse_arguments( - ARG - "" - "NAME;SRC" - "" - ${ARGN} - ) - add_executable(${ARG_NAME} ${ARG_SRC}) - add_dependencies(${ARG_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - target_include_directories(${ARG_NAME} - SYSTEM PRIVATE - ${catkin_INCLUDE_DIRS} - ) - target_link_libraries(${ARG_NAME} - ${catkin_LIBRARIES} - ) - install(TARGETS ${ARG_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) + add_executable(${target_name} "src/${sample_name}.cpp") + ament_target_dependencies(${target_name} ${ARG_DEPENDENCIES}) + install(TARGETS ${target_name} DESTINATION lib/${PROJECT_NAME}) endfunction() -register_cpp_sample(NAME sample_capture_cpp SRC src/sample_capture.cpp) -register_cpp_sample(NAME sample_capture_and_save_cpp SRC src/sample_capture_and_save.cpp) -register_cpp_sample(NAME sample_capture_2d_cpp SRC src/sample_capture_2d.cpp) -register_cpp_sample(NAME sample_capture_assistant_cpp SRC src/sample_capture_assistant.cpp) -register_cpp_sample(NAME sample_capture_with_settings_from_yml_cpp SRC src/sample_capture_with_settings_from_yml.cpp) - -#################### -## Python Samples ## -#################### - -function(register_python_sample) - cmake_parse_arguments( - ARG - "" - "SRC" - "" - ${ARGN} - ) - install(PROGRAMS - ${ARG_SRC} - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) +function(zivid_add_python_sample sample_name) + install(PROGRAMS "scripts/${sample_name}.py" DESTINATION lib/${PROJECT_NAME}) endfunction() -register_python_sample(SRC scripts/sample_capture.py) -register_python_sample(SRC scripts/sample_capture_and_save.py) -register_python_sample(SRC scripts/sample_capture_2d.py) -register_python_sample(SRC scripts/sample_capture_assistant.py) -register_python_sample(SRC scripts/sample_capture_with_settings_from_yml.py) - -#################### -## Launch scripts ## -#################### - -install( - DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(zivid_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +zivid_add_cpp_sample(sample_capture DEPENDENCIES rclcpp sensor_msgs std_srvs) +zivid_add_cpp_sample(sample_capture_2d DEPENDENCIES rclcpp sensor_msgs std_srvs) +zivid_add_cpp_sample(sample_capture_and_save DEPENDENCIES rclcpp zivid_interfaces) +zivid_add_cpp_sample(sample_capture_assistant DEPENDENCIES rclcpp zivid_interfaces sensor_msgs std_srvs) +zivid_add_cpp_sample(sample_capture_with_settings_from_file DEPENDENCIES rclcpp sensor_msgs std_srvs ament_index_cpp) + +zivid_add_python_sample(sample_capture) +zivid_add_python_sample(sample_capture_2d) +zivid_add_python_sample(sample_capture_and_save) +zivid_add_python_sample(sample_capture_assistant) +zivid_add_python_sample(sample_capture_with_settings_from_file) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY settings DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/zivid_samples/launch/sample.launch b/zivid_samples/launch/sample.launch index 247cfb2b..6323a28a 100644 --- a/zivid_samples/launch/sample.launch +++ b/zivid_samples/launch/sample.launch @@ -1,8 +1,5 @@ - - - - - - + + + diff --git a/zivid_samples/launch/sample_with_rviz.launch b/zivid_samples/launch/sample_with_rviz.launch new file mode 100644 index 00000000..af5b9765 --- /dev/null +++ b/zivid_samples/launch/sample_with_rviz.launch @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/zivid_samples/launch/zivid_camera_with_serial_number.launch b/zivid_samples/launch/zivid_camera_with_serial_number.launch new file mode 100644 index 00000000..693c19dc --- /dev/null +++ b/zivid_samples/launch/zivid_camera_with_serial_number.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/zivid_samples/package.xml b/zivid_samples/package.xml index ad1fa228..30ca160b 100644 --- a/zivid_samples/package.xml +++ b/zivid_samples/package.xml @@ -1,26 +1,35 @@ - + + zivid_samples - 2.5.0 - Contains C++ and Python samples demonstrating - use of the zivid_camera package. + 3.0.0 + Contains C++ and Python samples demonstrating use of the zivid_camera package. Zivid BSD3 https://zivid.com https://github.com/zivid/zivid-ros https://github.com/zivid/zivid-ros/issues - catkin - roscpp - rospy - std_msgs - zivid_camera - roscpp - rospy - std_msgs - zivid_camera - roscpp - rospy - std_msgs - zivid_camera - dynamic_reconfigure + ament_cmake + rclcpp + rclcpp_components + sensor_msgs + std_msgs + image_transport + builtin_interfaces + zivid_interfaces + ament_index_cpp + rclpy + + ament_lint_auto + ament_cmake_clang_format + ament_cmake_clang_tidy + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + + + ament_cmake + diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index 565b7e4f..b8065aff 100755 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -1,51 +1,93 @@ #!/usr/bin/env python -import rospy -import rosnode -import dynamic_reconfigure.client -from zivid_camera.srv import * -from std_msgs.msg import String +import sys + +from rcl_interfaces.srv import SetParameters +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import Trigger + +class Sample(Node): -class Sample: def __init__(self): - rospy.init_node("sample_capture_py", anonymous=True) + super().__init__('sample_capture_py') - rospy.loginfo("Starting sample_capture.py") + self.capture_service = self.create_client(Trigger, 'capture') + while not self.capture_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info('capture service not available, waiting again...') - rospy.wait_for_service("/zivid_camera/capture", 30.0) + self._set_settings() - rospy.Subscriber("/zivid_camera/points/xyzrgba", PointCloud2, self.on_points) + self.subscription = self.create_subscription( + PointCloud2, 'points/xyzrgba', self.on_points, 10 + ) - self.capture_service = rospy.ServiceProxy("/zivid_camera/capture", Capture) + def _set_settings(self): + self.get_logger().info('Setting parameter `settings_yaml`') + settings_parameter = Parameter( + 'settings_yaml', + Parameter.Type.STRING, + """ +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +""", + ).to_parameter_msg() - rospy.loginfo("Enabling the reflection filter") - settings_client = dynamic_reconfigure.client.Client("/zivid_camera/settings/") - settings_config = {"processing_filters_reflection_removal_enabled": True} - settings_client.update_configuration(settings_config) + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') - rospy.loginfo("Enabling and configure the first acquisition") - acquisition_0_client = dynamic_reconfigure.client.Client( - "/zivid_camera/settings/acquisition_0" + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) ) - acquisition_0_config = { - "enabled": True, - "aperture": 5.66, - "exposure_time": 20000, - } - acquisition_0_client.update_configuration(acquisition_0_config) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): - rospy.loginfo("Calling capture service") - self.capture_service() + self.get_logger().info('Calling capture service') + return self.capture_service.call_async(Trigger.Request()) + + def on_points(self, msg): + self.get_logger().info( + f'Received point cloud of size {msg.width} x {msg.height}' + ) + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') + + while rclpy.ok(): + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info('Capture complete') - def on_points(self, data): - rospy.loginfo("PointCloud received") - self.capture() + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) -if __name__ == "__main__": - s = Sample() - s.capture() - rospy.spin() +if __name__ == '__main__': + main() diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index c3640309..fd5c9065 100755 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -1,49 +1,86 @@ #!/usr/bin/env python -import rospy -import rosnode -import dynamic_reconfigure.client -from zivid_camera.srv import * -from std_msgs.msg import String +import sys + +from rcl_interfaces.srv import SetParameters +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter from sensor_msgs.msg import Image +from std_srvs.srv import Trigger -class Sample: - def __init__(self): - rospy.init_node("sample_capture_2d_py", anonymous=True) +class Sample(Node): - rospy.loginfo("Starting sample_capture_2d.py") + def __init__(self): + super().__init__('sample_capture_2d_py') - rospy.wait_for_service("/zivid_camera/capture_2d", 30.0) + self.capture_2d_service = self.create_client(Trigger, 'capture_2d') + while not self.capture_2d_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info('capture_2d service not available, waiting again...') - rospy.Subscriber("/zivid_camera/color/image_color", Image, self.on_image_color) + self._set_settings_2d() - self.capture_2d_service = rospy.ServiceProxy( - "/zivid_camera/capture_2d", Capture2D + self.subscription = self.create_subscription( + Image, 'color/image_color', self.on_image_color, 10 ) - rospy.loginfo("Configuring 2D settings") - acquisition_0_client = dynamic_reconfigure.client.Client( - "/zivid_camera/settings_2d/acquisition_0" + def _set_settings_2d(self): + self.get_logger().info('Setting parameter `settings_2d_yaml`') + settings_parameter = Parameter( + 'settings_2d_yaml', + Parameter.Type.STRING, + """ +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + Aperture: 2.83 + Brightness: 1.0 + ExposureTime: 10000 + Gain: 2.5 +""", + ).to_parameter_msg() + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) ) - acquisition_0_config = { - "enabled": True, - "aperture": 2.83, - "exposure_time": 10000, - "brightness": 1.0, - } - acquisition_0_client.update_configuration(acquisition_0_config) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): - rospy.loginfo("Calling capture service") - self.capture_2d_service() + self.get_logger().info('Calling capture_2d service') + return self.capture_2d_service.call_async(Trigger.Request()) + + def on_image_color(self, msg): + self.get_logger().info(f'Received image of size {msg.width} x {msg.height}') + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') + + while rclpy.ok(): + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info('Capture complete') - def on_image_color(self, data): - rospy.loginfo("Color image received") - self.capture() + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) -if __name__ == "__main__": - s = Sample() - s.capture() - rospy.spin() +if __name__ == '__main__': + main() diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index ea807a98..45d5e84b 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -1,49 +1,97 @@ #!/usr/bin/env python -import rospy -import rosnode -import dynamic_reconfigure.client -from zivid_camera.srv import * -from std_msgs.msg import String +import sys +import tempfile +from rcl_interfaces.srv import SetParameters +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from zivid_interfaces.srv import CaptureAndSave -class Sample: - def __init__(self): - rospy.init_node("sample_capture_and_save_py", anonymous=True) - rospy.loginfo("Starting sample_capture_and_save.py") +class Sample(Node): - rospy.wait_for_service("/zivid_camera/capture_and_save", 30.0) + def __init__(self): + super().__init__('sample_capture_and_save_py') - self.capture_and_save_service = rospy.ServiceProxy( - "/zivid_camera/capture_and_save", CaptureAndSave + self.capture_and_save_service = self.create_client( + CaptureAndSave, 'capture_and_save' ) + while not self.capture_and_save_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info( + 'capture_and_save service not available, waiting again...' + ) + + self._set_settings() + + def _set_settings(self): + self.get_logger().info('Setting parameter `settings_yaml`') + settings_parameter = Parameter( + 'settings_yaml', + Parameter.Type.STRING, + """ +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +""", + ).to_parameter_msg() - self._enable_first_acquistion() - self._enable_diagnostics() + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) + ) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): - rospy.loginfo("Calling capture_and_save service") - file_path = "/tmp/capture_py.zdf" - self.capture_and_save_service(file_path) - rospy.loginfo(f"Your .zdf file is now available here: {file_path}") - - def _enable_first_acquistion(self): - rospy.loginfo("Enabling the first acquisition") - acquisition_0_client = dynamic_reconfigure.client.Client( - "/zivid_camera/settings/acquisition_0" + file_path = tempfile.gettempdir() + '/zivid_sample_capture_and_save.zdf' + self.get_logger().info( + f'Calling capture_and_save service with file path: {file_path}' ) - acquisition_0_config = {"enabled": True} - acquisition_0_client.update_configuration(acquisition_0_config) + request = CaptureAndSave.Request(file_path=file_path) + return self.capture_and_save_service.call_async(request) + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + future = sample.capture() + + rclpy.spin_until_future_complete(sample, future) + + response: CaptureAndSave.Response = future.result() + if not response.success: + sample.get_logger().error(f'Failed capture and save: {response.message}') + + sample.get_logger().info('Capture and save complete') + + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') + rclpy.spin(sample) - def _enable_diagnostics(self): - rospy.loginfo("Enabling diagnostics mode") - settings_client = dynamic_reconfigure.client.Client("/zivid_camera/settings/") - settings_config = {"diagnostics_enabled": True} - settings_client.update_configuration(settings_config) + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) -if __name__ == "__main__": - s = Sample() - s.capture() - rospy.spin() +if __name__ == '__main__': + main() diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py index 3dae4ee2..4137d150 100755 --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -1,54 +1,90 @@ #!/usr/bin/env python -import rospy -import rosnode -from zivid_camera.srv import * -from std_msgs.msg import String -from sensor_msgs.msg import PointCloud2, Image +import sys +from builtin_interfaces.msg import Duration +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import Trigger +from zivid_interfaces.srv import CaptureAssistantSuggestSettings -class Sample: - def __init__(self): - rospy.init_node("sample_capture_assistant_py", anonymous=True) - - rospy.loginfo("Starting sample_capture_assistant.py") - ca_suggest_settings_service = "/zivid_camera/capture_assistant/suggest_settings" +class Sample(Node): - rospy.wait_for_service(ca_suggest_settings_service, 30.0) + def __init__(self): + super().__init__('sample_capture_assistant_py') - self.capture_assistant_service = rospy.ServiceProxy( - ca_suggest_settings_service, CaptureAssistantSuggestSettings + self.capture_assistant_suggest_settings_service = self.create_client( + CaptureAssistantSuggestSettings, 'capture_assistant/suggest_settings' ) - self.capture_service = rospy.ServiceProxy("/zivid_camera/capture", Capture) + while not self.capture_assistant_suggest_settings_service.wait_for_service( + timeout_sec=3.0 + ): + self.get_logger().info( + 'Capture assistant service not available, waiting again...' + ) - rospy.Subscriber("/zivid_camera/points/xyzrgba", PointCloud2, self.on_points) - rospy.Subscriber("/zivid_camera/color/image_color", Image, self.on_image_color) + self.capture_service = self.create_client(Trigger, 'capture') + while not self.capture_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info('Capture service not available, waiting again...') - def capture_assistant_suggest_settings(self): - max_capture_time = rospy.Duration.from_sec(1.20) - rospy.loginfo( - "Calling capture assistant service with max capture time = %.2f sec", - max_capture_time.to_sec(), + self.points_subscription = self.create_subscription( + PointCloud2, 'points/xyzrgba', self.on_points, 10 ) - self.capture_assistant_service( - max_capture_time=max_capture_time, - ambient_light_frequency=CaptureAssistantSuggestSettingsRequest.AMBIENT_LIGHT_FREQUENCY_NONE, + self.image_subscription = self.create_subscription( + Image, 'color/image_color', self.on_image_color, 10 ) + def capture_assistant_suggest_settings(self): + self.get_logger().info('Calling capture assistant service') + request = CaptureAssistantSuggestSettings.Request( + max_capture_time=Duration(sec=2), + ambient_light_frequency=( + CaptureAssistantSuggestSettings.Request.AMBIENT_LIGHT_FREQUENCY_NONE + ), + ) + return self.capture_assistant_suggest_settings_service.call_async(request) + def capture(self): - rospy.loginfo("Calling capture service") - self.capture_service() + self.get_logger().info('Calling capture service') + return self.capture_service.call_async(Trigger.Request()) + + def on_points(self, msg): + self.get_logger().info( + f'Received point cloud of size {msg.width} x {msg.height}' + ) + + def on_image_color(self, msg): + self.get_logger().info(f'Received image of size {msg.width} x {msg.height}') + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + future = sample.capture_assistant_suggest_settings() + rclpy.spin_until_future_complete(sample, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to suggest settings') + sample.get_logger().info('Capture assistant complete') + + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info('Capture complete') - def on_points(self, data): - rospy.loginfo("PointCloud received") + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') + rclpy.spin(sample) - def on_image_color(self, data): - rospy.loginfo("2D color image received") + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) -if __name__ == "__main__": - s = Sample() - s.capture_assistant_suggest_settings() - s.capture() - rospy.spin() +if __name__ == '__main__': + main() diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py new file mode 100755 index 00000000..0c20d193 --- /dev/null +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python + +import sys + +from ament_index_python.packages import get_package_share_directory +from rcl_interfaces.srv import SetParameters +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import Trigger + + +class Sample(Node): + + def __init__(self): + super().__init__('sample_capture_with_settings_from_file_py') + + self.capture_service = self.create_client(Trigger, 'capture') + while not self.capture_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info('capture service not available, waiting again...') + + self._set_settings() + + self.subscription = self.create_subscription( + PointCloud2, 'points/xyzrgba', self.on_points, 10 + ) + + def _set_settings(self): + path_to_settings_yml = ( + get_package_share_directory('zivid_samples') + + '/settings/camera_settings.yml' + ) + self.get_logger().info( + 'Setting parameter `settings_file_path` to: ' + path_to_settings_yml + ) + + settings_parameter = Parameter( + 'settings_file_path', + Parameter.Type.STRING, + path_to_settings_yml, + ).to_parameter_msg() + + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) + ) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') + + def capture(self): + self.get_logger().info('Calling capture service') + return self.capture_service.call_async(Trigger.Request()) + + def on_points(self, msg): + self.get_logger().info( + f'Received point cloud of size {msg.width} x {msg.height}' + ) + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info('Capture complete') + + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') + rclpy.spin(sample) + + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == '__main__': + main() diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_yml.py b/zivid_samples/scripts/sample_capture_with_settings_from_yml.py deleted file mode 100755 index 4cf35467..00000000 --- a/zivid_samples/scripts/sample_capture_with_settings_from_yml.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python - -import rospy -import rospkg -import rosnode -import dynamic_reconfigure.client -from zivid_camera.srv import * -from std_msgs.msg import String -from sensor_msgs.msg import PointCloud2 - - -class Sample: - def __init__(self): - rospy.init_node("sample_capture_with_settings_from_yml_py", anonymous=True) - - rospy.loginfo("Starting sample_capture_with_settings_from_yml.py") - - rospy.wait_for_service("/zivid_camera/capture", 30.0) - - self.capture_service = rospy.ServiceProxy("/zivid_camera/capture", Capture) - - samples_path = rospkg.RosPack().get_path("zivid_samples") - settings_path = samples_path + "/settings/camera_settings.yml" - rospy.loginfo("Loading settings from %s", settings_path) - self.load_settings_from_file_service = rospy.ServiceProxy( - "/zivid_camera/load_settings_from_file", LoadSettingsFromFile - ) - self.load_settings_from_file_service(settings_path) - - def capture(self): - rospy.loginfo("Calling capture service") - self.capture_service() - - -if __name__ == "__main__": - s = Sample() - s.capture() - rospy.spin() diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 564ac7ca..60e1dd59 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -1,84 +1,99 @@ -#include -#include -#include -#include -#include -#include -#include - -#define CHECK(cmd) \ - do { \ - if (!cmd) { \ - throw std::runtime_error{"\"" #cmd "\" failed!"}; \ - } \ - } while (false) - -namespace +#include +#include +#include +#include +#include + +/* + * This sample shows how to set the settings_file_path parameter of the zivid node, subscribe for + * the points/xyzrgba topic, and invoke the capture service. When a point cloud is received, a new + * capture is triggered. + */ + +void fatal_error(const rclcpp::Logger & logger, const std::string & message) { -const ros::Duration default_wait_duration{30}; + RCLCPP_ERROR_STREAM(logger, message); + throw std::runtime_error(message); +} -void capture() +void set_settings(const std::shared_ptr & node) { - ROS_INFO("Calling capture service"); - zivid_camera::Capture capture; - CHECK(ros::service::call("/zivid_camera/capture", capture)); + RCLCPP_INFO(node->get_logger(), "Setting parameter 'settings_yaml'"); + const std::string settings_yml = + R"( +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +)"; + + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); + } + + auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}); + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to set `settings_yaml` parameter"); + } } -void on_points(const sensor_msgs::PointCloud2ConstPtr &) +auto create_capture_client(std::shared_ptr & node) { - ROS_INFO("PointCloud received"); - capture(); + auto client = node->create_client("capture"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "capture service is available"); + return client; } -} // namespace - -int main(int argc, char ** argv) +int main(int argc, char * argv[]) { - ros::init(argc, argv, "sample_capture_cpp"); - ros::NodeHandle n; - - ROS_INFO("Starting sample_capture.cpp"); - - CHECK(ros::service::waitForService("/zivid_camera/capture", default_wait_duration)); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture node"); - ros::AsyncSpinner spinner(1); - spinner.start(); + set_settings(node); - auto points_sub = n.subscribe("/zivid_camera/points/xyzrgba", 1, on_points); + auto capture_client = create_capture_client(node); + auto trigger_capture = [&]() { + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + capture_client->async_send_request(std::make_shared()); + }; - dynamic_reconfigure::Client settings_client( - "/zivid_camera/" - "settings/"); + auto points_xyzrgba_subscription = node->create_subscription( + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of size %d x %d", msg->width, msg->height); + trigger_capture(); + }); - // To initialize the settings_config object we need to load the default configuration from the server. - // The default values of settings depends on which Zivid camera model is connected. - zivid_camera::SettingsConfig settings_config; - CHECK(settings_client.getDefaultConfiguration(settings_config, default_wait_duration)); + trigger_capture(); - ROS_INFO("Enabling the reflection removal filter"); - settings_config.processing_filters_reflection_removal_enabled = true; - CHECK(settings_client.setConfiguration(settings_config)); + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); - dynamic_reconfigure::Client acquisition_0_client( - "/zivid_camera/settings/" - "acquisition_0/"); + rclcpp::shutdown(); - // To initialize the acquisition_0_config object we need to load the default configuration from the server. - // The default values of settings depends on which Zivid camera model is connected. - zivid_camera::SettingsAcquisitionConfig acquisition_0_config; - CHECK(acquisition_0_client.getDefaultConfiguration(acquisition_0_config, default_wait_duration)); - - ROS_INFO("Enabling and configuring the first acquisition"); - acquisition_0_config.enabled = true; - acquisition_0_config.aperture = 5.66; - acquisition_0_config.exposure_time = 20000; - CHECK(acquisition_0_client.setConfiguration(acquisition_0_config)); - - ROS_INFO("Calling capture"); - - capture(); - - ros::waitForShutdown(); - - return 0; -} \ No newline at end of file + return EXIT_SUCCESS; +} diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index b705729f..739e7ec9 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -1,70 +1,94 @@ -#include -#include -#include -#include -#include -#include - -#define CHECK(cmd) \ - do { \ - if (!cmd) { \ - throw std::runtime_error{"\"" #cmd "\" failed!"}; \ - } \ - } while (false) - -namespace +#include +#include +#include +#include + +/* + * This sample shows how to set the settings_2d_yaml parameter of the zivid node, subscribe for the + * color/image_color topic, and invoke the capture_2d service. When an image is received, a new + * capture is triggered. + */ + +void fatal_error(const rclcpp::Logger & logger, const std::string & message) { -const ros::Duration default_wait_duration{30}; + RCLCPP_ERROR_STREAM(logger, message); + throw std::runtime_error(message); +} -void capture() +void set_settings_2d(const std::shared_ptr & node) { - ROS_INFO("Calling capture_2d service"); - zivid_camera::Capture2D capture_2d; - CHECK(ros::service::call("/zivid_camera/capture_2d", capture_2d)); + RCLCPP_INFO_STREAM(node->get_logger(), "Setting parameter `settings_2d_yaml`"); + const std::string settings_2d_yaml = + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + Aperture: 2.83 + Brightness: 1.0 + ExposureTime: 10000 + Gain: 2.5 +)"; + + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); + } + + auto result = + param_client->set_parameters({rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}); + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to set `settings_2d_yaml` parameter"); + } } -void on_image_color(const sensor_msgs::ImageConstPtr &) +auto create_capture_2d_client(std::shared_ptr & node) { - ROS_INFO("2D color image received"); - capture(); + auto client = node->create_client("capture_2d"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture_2d service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "capture_2d service is available"); + return client; } -} // namespace - -int main(int argc, char ** argv) +int main(int argc, char * argv[]) { - ros::init(argc, argv, "sample_capture_2d"); - ros::NodeHandle n; + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_2d"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_2d node"); - ROS_INFO("Starting sample_capture_2d.cpp"); + set_settings_2d(node); - CHECK(ros::service::waitForService("/zivid_camera/capture_2d", default_wait_duration)); + auto capture_2d_client = create_capture_2d_client(node); + auto trigger_capture = [&]() { + RCLCPP_INFO(node->get_logger(), "Triggering 2d capture"); + capture_2d_client->async_send_request(std::make_shared()); + }; - ros::AsyncSpinner spinner(1); - spinner.start(); + auto color_image_color_subscription = node->create_subscription( + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received image of size %d x %d", msg->width, msg->height); + trigger_capture(); + }); - auto image_color_sub = n.subscribe("/zivid_camera/color/image_color", 1, on_image_color); + trigger_capture(); - ROS_INFO("Configuring image settings"); - dynamic_reconfigure::Client acquisition_0_client( - "/zivid_camera/" - "settings_2d/" - "acquisition_0/"); + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); - // To initialize the cfg object we need to load the default configuration from the server. - // The default values of settings depends on which Zivid camera model is connected. - zivid_camera::Settings2DAcquisitionConfig acquisition_0_config; - CHECK(acquisition_0_client.getDefaultConfiguration(acquisition_0_config, default_wait_duration)); + rclcpp::shutdown(); - acquisition_0_config.enabled = true; - acquisition_0_config.aperture = 5.66; - acquisition_0_config.exposure_time = 10000; - acquisition_0_config.brightness = 1.0; - CHECK(acquisition_0_client.setConfiguration(acquisition_0_config)); - - capture(); - - ros::waitForShutdown(); - - return 0; -} \ No newline at end of file + return EXIT_SUCCESS; +} diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 17758479..8c8cf7d2 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -1,82 +1,106 @@ -#include -#include -#include -#include -#include -#include -#include - -#define CHECK(cmd) \ - do { \ - if (!cmd) { \ - throw std::runtime_error{"\"" #cmd "\" failed!"}; \ - } \ - } while (false) - -namespace +#include +#include +#include +#include + +/* + * This sample shows how to set the settings_yaml parameter of the zivid node, then invoke the + * capture_and_save service, and read the response from the service call. If successful, the + * captured frame is stored in a temporary directory. + */ + +void fatal_error(const rclcpp::Logger & logger, const std::string & message) { -const ros::Duration default_wait_duration{30}; - -void capture_and_save() -{ - ROS_INFO("Calling capture_and_save service"); - zivid_camera::CaptureAndSave capture_and_save; - std::string file_path = "/tmp/capture_cpp.zdf"; - capture_and_save.request.file_path = file_path; - CHECK(ros::service::call("/zivid_camera/capture_and_save", capture_and_save)); - ROS_INFO("Your .zdf file is now available here: %s", file_path.c_str()); + RCLCPP_ERROR_STREAM(logger, message); + throw std::runtime_error(message); } -void enable_first_acquisition() +void set_settings(const std::shared_ptr & node) { - dynamic_reconfigure::Client acquisition_0_client( - "/zivid_camera/settings/" - "acquisition_0/"); - - // To initialize the acquisition_0_config object we need to load the default configuration from the server. - // The default values of settings depends on which Zivid camera model is connected. - zivid_camera::SettingsAcquisitionConfig acquisition_0_config; - CHECK(acquisition_0_client.getDefaultConfiguration(acquisition_0_config, default_wait_duration)); - - ROS_INFO("Enabling the first acquisition"); - acquisition_0_config.enabled = true; - CHECK(acquisition_0_client.setConfiguration(acquisition_0_config)); + RCLCPP_INFO(node->get_logger(), "Setting parameter 'settings_yaml'"); + const std::string settings_yml = + R"( +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +)"; + + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); + } + + auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}); + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to set `settings_yaml` parameter"); + } } -void enable_diagnostics() +void capture_and_save(const std::shared_ptr & node) { - dynamic_reconfigure::Client settings_client( - "/zivid_camera/" - "settings/"); - zivid_camera::SettingsConfig settings_config; - CHECK(settings_client.getDefaultConfiguration(settings_config, default_wait_duration)); - - ROS_INFO("Enabling diagnostics mode"); - settings_config.diagnostics_enabled = true; - CHECK(settings_client.setConfiguration(settings_config)); + using zivid_interfaces::srv::CaptureAndSave; + + auto client = node->create_client("capture_and_save"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture_and_save service to appear..."); + } + + const std::string filename = "zivid_sample_capture_and_save.zdf"; + + auto request = std::make_shared(); + request->file_path = (std::filesystem::temp_directory_path() / filename).string(); + RCLCPP_INFO( + node->get_logger(), "Sending capture and save request with file path: %s", + request->file_path.c_str()); + + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to call the capture_and_save service"); + } + + auto capture_response = result.get(); + if (!capture_response->success) { + fatal_error( + node->get_logger(), + "Capture and save operation was unsuccessful: " + capture_response->message); + } + + RCLCPP_INFO(node->get_logger(), "Capture and save operation successful"); } -} // namespace - -int main(int argc, char ** argv) +int main(int argc, char * argv[]) { - ros::init(argc, argv, "sample_capture_and_save_cpp"); - ros::NodeHandle n; - - ROS_INFO("Starting sample_capture_and_save.cpp"); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_and_save"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_and_save node"); - CHECK(ros::service::waitForService("/zivid_camera/capture_and_save", default_wait_duration)); + set_settings(node); - ros::AsyncSpinner spinner(1); - spinner.start(); + capture_and_save(node); - enable_first_acquisition(); + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); - enable_diagnostics(); + rclcpp::shutdown(); - capture_and_save(); - - ros::waitForShutdown(); - - return 0; -} \ No newline at end of file + return EXIT_SUCCESS; +} diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 260a0e80..3c1d8678 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -1,68 +1,92 @@ -#include -#include -#include -#include -#include - -#define CHECK(cmd) \ - do { \ - if (!cmd) { \ - throw std::runtime_error{"\"" #cmd "\" failed!"}; \ - } \ - } while (false) - -namespace +#include +#include +#include +#include +#include +#include + +/* + * This sample shows how to use the capture assistant service to suggest and set the capture + * settings parameter of the zivid node. Then, it shows how to subscribe to the points/xyzrgba and + * color/image_color topics, and finally how to invoke the capture service. + */ + +void fatal_error(const rclcpp::Logger & logger, const std::string & message) { -const ros::Duration default_wait_duration{30}; -constexpr auto ca_suggest_settings_service_name = - "/zivid_camera/capture_assistant/suggest_settings"; + RCLCPP_ERROR_STREAM(logger, message); + throw std::runtime_error(message); +} -void capture_assistant_suggest_settings() +void capture_assistant_suggest_settings(const std::shared_ptr & node) { - zivid_camera::CaptureAssistantSuggestSettings cass; - cass.request.max_capture_time = ros::Duration{1.20}; - cass.request.ambient_light_frequency = - zivid_camera::CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; - - ROS_INFO_STREAM( - "Calling " << ca_suggest_settings_service_name - << " with max capture time = " << cass.request.max_capture_time << " sec"); - CHECK(ros::service::call(ca_suggest_settings_service_name, cass)); + using zivid_interfaces::srv::CaptureAssistantSuggestSettings; + + auto client = + node->create_client("capture_assistant/suggest_settings"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture assistant service to appear..."); + } + + auto request = std::make_shared(); + request->max_capture_time = rclcpp::Duration::from_seconds(2.0); + request->ambient_light_frequency = + CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; + + auto result = client->async_send_request(request); + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to call capture assistant service"); + } } -void capture() +void capture(const std::shared_ptr & node) { - ROS_INFO("Calling capture service"); - zivid_camera::Capture capture; - CHECK(ros::service::call("/zivid_camera/capture", capture)); + auto capture_client = node->create_client("capture"); + while (!capture_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + auto result = + capture_client->async_send_request(std::make_shared()); + + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to trigger capture"); + } } -void on_points(const sensor_msgs::PointCloud2ConstPtr &) { ROS_INFO("PointCloud received"); } - -void on_image_color(const sensor_msgs::ImageConstPtr &) { ROS_INFO("2D color image received"); } - -} // namespace - -int main(int argc, char ** argv) +int main(int argc, char * argv[]) { - ros::init(argc, argv, "sample_capture_assistant_cpp"); - ros::NodeHandle n; - - ROS_INFO("Starting sample_capture_assistant.cpp"); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_assistant"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_assistant node"); - CHECK(ros::service::waitForService(ca_suggest_settings_service_name, default_wait_duration)); + auto points_xyzrgba_subscription = node->create_subscription( + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of size %d x %d", msg->width, msg->height); + }); - ros::AsyncSpinner spinner(1); - spinner.start(); + auto color_image_color_subscription = node->create_subscription( + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received image of size %d x %d", msg->width, msg->height); + }); - auto points_sub = n.subscribe("/zivid_camera/points/xyzrgba", 1, on_points); - auto image_color_sub = n.subscribe("/zivid_camera/color/image_color", 1, on_image_color); + capture_assistant_suggest_settings(node); - capture_assistant_suggest_settings(); + capture(node); - capture(); + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); - ros::waitForShutdown(); + rclcpp::shutdown(); - return 0; -} \ No newline at end of file + return EXIT_SUCCESS; +} diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp new file mode 100644 index 00000000..8018950b --- /dev/null +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -0,0 +1,84 @@ +#include +#include +#include +#include +#include +#include + +/* + * This sample shows how to set the settings_file_path parameter of the zivid node, subscribe for + * the points/xyzrgba topic, and invoke the capture service. When a point cloud is received, a new + * capture is triggered. + */ + +void fatal_error(const rclcpp::Logger & logger, const std::string & message) +{ + RCLCPP_ERROR_STREAM(logger, message); + throw std::runtime_error(message); +} + +void set_settings(const std::shared_ptr & node) +{ + const auto share_directory = ament_index_cpp::get_package_share_directory("zivid_samples"); + const auto path_to_settings_yml = share_directory + "/settings/camera_settings.yml"; + RCLCPP_INFO_STREAM( + node->get_logger(), + "Setting parameter `settings_file_path` to '" << path_to_settings_yml << "'"); + + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); + } + + auto result = + param_client->set_parameters({rclcpp::Parameter("settings_file_path", path_to_settings_yml)}); + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to set `settings_file_path` parameter"); + } +} + +auto create_capture_client(std::shared_ptr & node) +{ + auto client = node->create_client("capture"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "Service is available"); + return client; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_with_settings_from_file"); + RCLCPP_INFO(node->get_logger(), "Started the sample node"); + + auto points_xyzrgba_subscription = node->create_subscription( + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of size %d x %d", msg->width, msg->height); + }); + + set_settings(node); + + auto client = create_capture_client(node); + + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + client->async_send_request(std::make_shared()); + + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + + rclcpp::shutdown(); + + return EXIT_SUCCESS; +} diff --git a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp b/zivid_samples/src/sample_capture_with_settings_from_yml.cpp deleted file mode 100644 index efa379d7..00000000 --- a/zivid_samples/src/sample_capture_with_settings_from_yml.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define CHECK(cmd) \ - do { \ - if (!cmd) { \ - throw std::runtime_error{"\"" #cmd "\" failed!"}; \ - } \ - } while (false) - -namespace -{ -const ros::Duration default_wait_duration{30}; - -void capture() -{ - ROS_INFO("Calling capture service"); - zivid_camera::Capture capture; - CHECK(ros::service::call("/zivid_camera/capture", capture)); -} - -} // namespace - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "sample_capture_with_settings_from_yml_cpp"); - ros::NodeHandle n; - - ROS_INFO("Starting sample_capture_with_settings_from_yml.cpp"); - - CHECK(ros::service::waitForService("/zivid_camera/capture", default_wait_duration)); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - std::string samples_path = ros::package::getPath("zivid_samples"); - std::string settings_path = samples_path + "/settings/camera_settings.yml"; - ROS_INFO("Loading settings from: %s", settings_path.c_str()); - zivid_camera::LoadSettingsFromFile load_settings_from_file; - load_settings_from_file.request.file_path = settings_path; - CHECK(ros::service::call("/zivid_camera/load_settings_from_file", load_settings_from_file)); - - ROS_INFO("Calling capture"); - - capture(); - - ros::waitForShutdown(); - - return 0; -} \ No newline at end of file