From 57b0d83cdf049827f2c15acbdb9e14539532baa5 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 28 May 2024 12:13:33 +0200 Subject: [PATCH 01/28] build: Set CMAKE_CXX_STANDARD_REQUIRED This is needed for clangd LSP working properly. --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index e5de08b..e295312 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(trifinger_cameras) set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED on) # libraries need to be position independent for building Python modules set(CMAKE_POSITION_INDEPENDENT_CODE ON) From 5f35b9c90bfe949d00bbac61f1a242696043f8d1 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 28 May 2024 12:23:48 +0200 Subject: [PATCH 02/28] Add CameraSettings struct and function to load from file Add struct `CameraSettings` which is supposed to hold all configurable settings of the cameras (currently frame rate and the path to the pylon settings file). Further add functions to get default values and to load from TOML files. To avoid having to either pass around the path to the TOML file in higher-level scripts or to assume a hard-coded default path, use an environment variable TRICAMERA_CONFIG_FILE to specify the path. Note: This commit only adds the struct but does not yet use it. It is the first step in getting rid of the hard-coded frame rate. --- CMakeLists.txt | 16 +++++ .../pylon_camera_settings.hpp | 65 ++++++++++++++++++ src/pylon_camera_settings.cpp | 66 +++++++++++++++++++ 3 files changed, 147 insertions(+) create mode 100644 include/trifinger_cameras/pylon_camera_settings.hpp create mode 100644 src/pylon_camera_settings.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e295312..2f1fc00 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(sensor_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) +find_package(tomlplusplus REQUIRED) find_package(Pylon) if (${Pylon_FOUND}) @@ -46,6 +47,21 @@ set(install_targets "") # Libraries +add_library(pylon_camera_settings + src/pylon_camera_settings.cpp +) +target_include_directories(pylon_camera_settings PUBLIC + $ + $ + ${OpenCV_INCLUDE_DIRS}) +target_link_libraries(pylon_camera_settings + tomlplusplus::tomlplusplus +) +ament_target_dependencies(pylon_camera_settings + ament_index_cpp +) +list(APPEND install_targets pylon_camera_settings) + add_library(camera_observations src/camera_observation.cpp src/tricamera_observation.cpp diff --git a/include/trifinger_cameras/pylon_camera_settings.hpp b/include/trifinger_cameras/pylon_camera_settings.hpp new file mode 100644 index 0000000..2aeccae --- /dev/null +++ b/include/trifinger_cameras/pylon_camera_settings.hpp @@ -0,0 +1,65 @@ +/** + * @file + * @copyright 2020 Max Planck Gesellschaft. All rights reserved. + * @license BSD 3-clause + */ +#pragma once + +#include +#include +#include + +#include + +namespace trifinger_cameras +{ +//! Get path to default Pylon settings file. +std::string get_default_pylon_settings_file(); + +/** + * @brief Bundles all configurable camera settings. + */ +struct PylonCameraSettings +{ + /** + * @brief Frame rate at which images are fetched from the camera. + * + * Important: This must not be higher than ``AcquisitionFrameRate`` which is + * defined in the Pylon settings file. + */ + float frame_rate_fps; + + /** + * @brief Path to the file with the settings that are sent to the Pylon + * camera. + */ + std::string pylon_settings_file; + + /** + * @brief Get default settings. + */ + static PylonCameraSettings defaults(); + + /** + * @brief Load settings + * + * If the environment variable ``TRICAMERA_CONFIG_FILE`` is defined, try + * loading settings from the file it points to (using @ref load(const + * std::filesystem::path&)), otherwise return default settings.. + */ + static PylonCameraSettings load(); + + /** + * @brief Load settings from TOML file. + * + * For fields that are not specified in the provided file, the default + * values as provided by @ref CameraSettings::defaults() are used. + * + * @param file Path to TOML file. + */ + static PylonCameraSettings load(const std::filesystem::path& file); +}; + +std::ostream& operator<<(std::ostream& os, const PylonCameraSettings& cp); + +} // namespace trifinger_cameras diff --git a/src/pylon_camera_settings.cpp b/src/pylon_camera_settings.cpp new file mode 100644 index 0000000..9ffda38 --- /dev/null +++ b/src/pylon_camera_settings.cpp @@ -0,0 +1,66 @@ +/** + * @file + * @copyright 2020 Max Planck Gesellschaft. All rights reserved. + * @license BSD 3-clause + */ +#include + +#include + +#include + +namespace trifinger_cameras +{ +constexpr std::string_view ENV_VARIABLE_CONFIG_FILE = "TRICAMERA_CONFIG_FILE"; +constexpr std::string_view TOML_SECTION = "tricamera"; + +std::string get_default_pylon_settings_file() +{ + return ament_index_cpp::get_package_share_directory("trifinger_cameras") + + "/config/pylon_camera_settings.txt"; +} + +PylonCameraSettings defaults() +{ + return {.frame_rate_fps = 10, + .pylon_settings_file = get_default_pylon_settings_file()}; +} + +PylonCameraSettings load() +{ + const char* config_file_path = + std::getenv(std::string(ENV_VARIABLE_CONFIG_FILE).c_str()); + if (config_file_path) + { + return PylonCameraSettings::load(config_file_path); + } + else + { + return PylonCameraSettings::defaults(); + } +} + +PylonCameraSettings load(const std::filesystem::path& file) +{ + PylonCameraSettings defaults = PylonCameraSettings::defaults(); + + auto toml_settings = toml::parse_file(file.string()); + + float fps = + toml_settings[TOML_SECTION]["fps"].value_or(defaults.frame_rate_fps); + std::string_view pylon_settings_file = + toml_settings[TOML_SECTION]["pylon_settings_file"].value_or( + defaults.pylon_settings_file); + + return {.frame_rate_fps = fps, + .pylon_settings_file = std::string{pylon_settings_file}}; +} + +std::ostream& operator<<(std::ostream& os, const PylonCameraSettings& cs) +{ + os << "CameraSettings:" << std::endl + << "\tfps: " << cs.frame_rate_fps << std::endl + << "\tpylon_settings_file: " << cs.pylon_settings_file << std::endl; + return os; +} +} // namespace trifinger_cameras From 580c7de28bf4a09d889fdd83fbdb89b34055cd8e Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 28 May 2024 15:06:58 +0200 Subject: [PATCH 03/28] Reimplement Settings I realised that frame rate and the pylon settings file are actually used by different classes (TriCameraDriver and PylonDriver), so having them together in one structure was a bit messy. This commit rewrites the settings implementation to use a separate struct for each module/class that needs configuration but still bundles the loading in one central `Settings` class. Configuration for the different classes is expected to be in separate sections of the same TOML file: ```toml [pylon_driver] pylon_settings_file = "path/to/pylon_settings_file.txt" [tricamera_driver] frame_rate_fps = 10 ``` While the TOML file is parsed as a whole in the constructor, the structs for the different sections are only instantiated on demand, when the corresponding getter is called. --- CMakeLists.txt | 12 +- .../pylon_camera_settings.hpp | 65 ----------- include/trifinger_cameras/settings.hpp | 106 ++++++++++++++++++ src/pylon_camera_settings.cpp | 66 ----------- src/settings.cpp | 96 ++++++++++++++++ 5 files changed, 208 insertions(+), 137 deletions(-) delete mode 100644 include/trifinger_cameras/pylon_camera_settings.hpp create mode 100644 include/trifinger_cameras/settings.hpp delete mode 100644 src/pylon_camera_settings.cpp create mode 100644 src/settings.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f1fc00..2af6684 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,20 +47,20 @@ set(install_targets "") # Libraries -add_library(pylon_camera_settings - src/pylon_camera_settings.cpp +add_library(settings + src/settings.cpp ) -target_include_directories(pylon_camera_settings PUBLIC +target_include_directories(settings PUBLIC $ $ ${OpenCV_INCLUDE_DIRS}) -target_link_libraries(pylon_camera_settings +target_link_libraries(settings tomlplusplus::tomlplusplus ) -ament_target_dependencies(pylon_camera_settings +ament_target_dependencies(settings ament_index_cpp ) -list(APPEND install_targets pylon_camera_settings) +list(APPEND install_targets settings) add_library(camera_observations src/camera_observation.cpp diff --git a/include/trifinger_cameras/pylon_camera_settings.hpp b/include/trifinger_cameras/pylon_camera_settings.hpp deleted file mode 100644 index 2aeccae..0000000 --- a/include/trifinger_cameras/pylon_camera_settings.hpp +++ /dev/null @@ -1,65 +0,0 @@ -/** - * @file - * @copyright 2020 Max Planck Gesellschaft. All rights reserved. - * @license BSD 3-clause - */ -#pragma once - -#include -#include -#include - -#include - -namespace trifinger_cameras -{ -//! Get path to default Pylon settings file. -std::string get_default_pylon_settings_file(); - -/** - * @brief Bundles all configurable camera settings. - */ -struct PylonCameraSettings -{ - /** - * @brief Frame rate at which images are fetched from the camera. - * - * Important: This must not be higher than ``AcquisitionFrameRate`` which is - * defined in the Pylon settings file. - */ - float frame_rate_fps; - - /** - * @brief Path to the file with the settings that are sent to the Pylon - * camera. - */ - std::string pylon_settings_file; - - /** - * @brief Get default settings. - */ - static PylonCameraSettings defaults(); - - /** - * @brief Load settings - * - * If the environment variable ``TRICAMERA_CONFIG_FILE`` is defined, try - * loading settings from the file it points to (using @ref load(const - * std::filesystem::path&)), otherwise return default settings.. - */ - static PylonCameraSettings load(); - - /** - * @brief Load settings from TOML file. - * - * For fields that are not specified in the provided file, the default - * values as provided by @ref CameraSettings::defaults() are used. - * - * @param file Path to TOML file. - */ - static PylonCameraSettings load(const std::filesystem::path& file); -}; - -std::ostream& operator<<(std::ostream& os, const PylonCameraSettings& cp); - -} // namespace trifinger_cameras diff --git a/include/trifinger_cameras/settings.hpp b/include/trifinger_cameras/settings.hpp new file mode 100644 index 0000000..5b5a95e --- /dev/null +++ b/include/trifinger_cameras/settings.hpp @@ -0,0 +1,106 @@ +/** + * @file + * @copyright 2020 Max Planck Gesellschaft. All rights reserved. + * @license BSD 3-clause + */ +#pragma once + +#include +#include +#include + +#include +#include + +namespace trifinger_cameras +{ +//! Settings of the PylonDriver. +struct PylonDriverSettings +{ + //! Name of the corresponding section in the config file + static constexpr std::string_view CONFIG_SECTION = "pylon_driver"; + + /** + * @brief Path to the file with the settings that are sent to the Pylon + * camera. + */ + std::string pylon_settings_file; + + //! Load from TOML table, using default values for unspecified parameters. + static std::shared_ptr load_from_toml( + const toml::table& config); +}; + +//! Settings of the TriCameraDriver. +struct TriCameraDriverSettings +{ + //! Name of the corresponding section in the config file + static constexpr std::string_view CONFIG_SECTION = "tricamera_driver"; + + /** + * @brief Frame rate at which images are fetched from the camera. + * + * Important: This must not be higher than ``AcquisitionFrameRate`` which is + * defined in the Pylon settings file. + */ + float frame_rate_fps; + + //! Load from TOML table, using default values for unspecified parameters. + static std::shared_ptr load_from_toml( + const toml::table& config); +}; + +/** + * @brief Central class for loading settings. + * + * This class is the central facility to load settings for this package from a + * TOML file. The file can have the following tables/keys: + * + * ```toml + * [pylon_driver] + * pylon_settings_file = "path/to/pylon_settings_file.txt" + * + * [tricamera_driver] + * frame_rate_fps = 10 + * ``` + * + * All values are optional. If not specified, default values are used. + * + * Constructing the class will parse the TOML file but actual settings objects + * for the different modules are only created when the corresponding getter is + * called the first time. + */ +class Settings +{ +public: + //! Name of the environment variable to specify the config file path. + static constexpr std::string_view ENV_VARIABLE_CONFIG_FILE = + "TRIFINGER_CAMERA_CONFIG"; + + /** + * @brief Load configuration from file specified via environment variable. + * + * If the environment variable ``TRIFINGER_CAMERA_CONFIG`` is defined, it is + * expected to contain the path to the configuration file, which is then + * loaded. If it is not defined, default values are used for all + * parameters. + */ + Settings(); + + //! Load configuration from the specified file. + Settings(const std::filesystem::path& file); + + //! Get settings for the PylonDriver. + std::shared_ptr get_pylon_driver_settings(); + + //! Get settings for the TriCameraDriver. + std::shared_ptr + get_tricamera_driver_settings(); + +private: + toml::table config_; + std::shared_ptr pylon_driver_settings_; + std::shared_ptr tricamera_driver_settings_; +}; + +} // namespace trifinger_cameras diff --git a/src/pylon_camera_settings.cpp b/src/pylon_camera_settings.cpp deleted file mode 100644 index 9ffda38..0000000 --- a/src/pylon_camera_settings.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file - * @copyright 2020 Max Planck Gesellschaft. All rights reserved. - * @license BSD 3-clause - */ -#include - -#include - -#include - -namespace trifinger_cameras -{ -constexpr std::string_view ENV_VARIABLE_CONFIG_FILE = "TRICAMERA_CONFIG_FILE"; -constexpr std::string_view TOML_SECTION = "tricamera"; - -std::string get_default_pylon_settings_file() -{ - return ament_index_cpp::get_package_share_directory("trifinger_cameras") + - "/config/pylon_camera_settings.txt"; -} - -PylonCameraSettings defaults() -{ - return {.frame_rate_fps = 10, - .pylon_settings_file = get_default_pylon_settings_file()}; -} - -PylonCameraSettings load() -{ - const char* config_file_path = - std::getenv(std::string(ENV_VARIABLE_CONFIG_FILE).c_str()); - if (config_file_path) - { - return PylonCameraSettings::load(config_file_path); - } - else - { - return PylonCameraSettings::defaults(); - } -} - -PylonCameraSettings load(const std::filesystem::path& file) -{ - PylonCameraSettings defaults = PylonCameraSettings::defaults(); - - auto toml_settings = toml::parse_file(file.string()); - - float fps = - toml_settings[TOML_SECTION]["fps"].value_or(defaults.frame_rate_fps); - std::string_view pylon_settings_file = - toml_settings[TOML_SECTION]["pylon_settings_file"].value_or( - defaults.pylon_settings_file); - - return {.frame_rate_fps = fps, - .pylon_settings_file = std::string{pylon_settings_file}}; -} - -std::ostream& operator<<(std::ostream& os, const PylonCameraSettings& cs) -{ - os << "CameraSettings:" << std::endl - << "\tfps: " << cs.frame_rate_fps << std::endl - << "\tpylon_settings_file: " << cs.pylon_settings_file << std::endl; - return os; -} -} // namespace trifinger_cameras diff --git a/src/settings.cpp b/src/settings.cpp new file mode 100644 index 0000000..bf45989 --- /dev/null +++ b/src/settings.cpp @@ -0,0 +1,96 @@ +/** + * @file + * @copyright 2020 Max Planck Gesellschaft. All rights reserved. + * @license BSD 3-clause + */ +#include + +#include + +namespace trifinger_cameras +{ +std::shared_ptr PylonDriverSettings::load_from_toml( + const toml::table& config) +{ + auto section = config[CONFIG_SECTION]; + auto cfg = std::make_shared(); + + // default value of pylon_settings_file requires some lookup, so use an `if` + // to only do that if actually needed + std::optional pylon_settings_file = + section["pylon_settings_file"].value(); + if (!pylon_settings_file) + { + pylon_settings_file = + ament_index_cpp::get_package_share_directory("trifinger_cameras") + + "/config/pylon_camera_settings.txt"; + } + cfg->pylon_settings_file = *pylon_settings_file; + + return cfg; +} + +std::ostream& operator<<(std::ostream& os, const PylonDriverSettings& s) +{ + os << "PylonDriverSettings:" << std::endl + << "\tpylon_settings_file: " << s.pylon_settings_file << std::endl; + return os; +} + +std::shared_ptr +TriCameraDriverSettings::load_from_toml(const toml::table& config) +{ + auto section = config[CONFIG_SECTION]; + auto cfg = std::make_shared(); + + cfg->frame_rate_fps = section["frame_rate_fps"].value_or(10.0f); + + return cfg; +} + +std::ostream& operator<<(std::ostream& os, const TriCameraDriverSettings& s) +{ + os << "TriCameraDriverSettings:" << std::endl + << "\tframe_rate_fps: " << s.frame_rate_fps << std::endl; + return os; +} + +Settings::Settings() +{ + const char* config_file_path = + std::getenv(std::string(ENV_VARIABLE_CONFIG_FILE).c_str()); + if (config_file_path) + { + config_ = toml::parse_file(config_file_path); + } + // If no file is specified, simply do nothing here. This results in config_ + // being an empty toml::table and thus the `load_from_toml()` functions + // should fill all members with default values. +} + +Settings::Settings(const std::filesystem::path& file) +{ + config_ = toml::parse_file(file.string()); +} + +std::shared_ptr Settings::get_pylon_driver_settings() +{ + if (!pylon_driver_settings_) + { + pylon_driver_settings_ = PylonDriverSettings::load_from_toml(config_); + } + return pylon_driver_settings_; +} + +std::shared_ptr +Settings::get_tricamera_driver_settings() +{ + if (!tricamera_driver_settings_) + { + tricamera_driver_settings_ = + TriCameraDriverSettings::load_from_toml(config_); + } + return tricamera_driver_settings_; +} + +} // namespace trifinger_cameras From 1c9b0b93b276bf96d2fdda66167c7bfe07f08d4e Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 28 May 2024 16:55:09 +0200 Subject: [PATCH 04/28] test: Add unit tests for Settings --- CMakeLists.txt | 12 ++-- package.xml | 2 +- tests/test_settings.cpp | 148 ++++++++++++++++++++++++++++++++++++++++ 3 files changed, 157 insertions(+), 5 deletions(-) create mode 100644 tests/test_settings.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2af6684..4d97c89 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -262,23 +262,27 @@ install( ) - # Tests if (BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_nose REQUIRED) # install files that are used by the tests install(FILES tests/camera_calib.yml DESTINATION share/${PROJECT_NAME}/tests) - ament_add_gtest(test_camera_observation tests/test_camera_observation.cpp) + ament_add_gmock(test_settings tests/test_settings.cpp) + target_link_libraries(test_settings + settings + ) + + ament_add_gmock(test_camera_observation tests/test_camera_observation.cpp) target_link_libraries(test_camera_observation ${OpenCV_LIBRARIES} camera_observations ) - ament_add_gtest(test_shared_memory_camera_data + ament_add_gmock(test_shared_memory_camera_data tests/test_shared_memory_camera_data.cpp) target_link_libraries(test_shared_memory_camera_data ${OpenCV_LIBRARIES} diff --git a/package.xml b/package.xml index 4f25c8e..13d9bef 100644 --- a/package.xml +++ b/package.xml @@ -31,7 +31,7 @@ python-scipy python-opencv - ament_cmake_gtest + ament_cmake_gmock ament_cmake_nose ament_index_python diff --git a/tests/test_settings.cpp b/tests/test_settings.cpp new file mode 100644 index 0000000..899d8f9 --- /dev/null +++ b/tests/test_settings.cpp @@ -0,0 +1,148 @@ +/** + * @file + * @brief Tests for Settings etc. + * @copyright Copyright (c) 2019, Max Planck Gesellschaft. + */ +#include +#include +#include +#include +#include +#include +#include + +using namespace trifinger_cameras; +using ::testing::NotNull; + +// `ends_with` taken from https://stackoverflow.com/a/42844629 by Pavel P (CC +// BY-SA 4.0) +static bool ends_with(std::string_view str, std::string_view suffix) +{ + return str.size() >= suffix.size() && + str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0; +} + +TEST(TestSettings, load_env_no_file) +{ + // make sure the env variable is _not_ set + unsetenv(std::string(Settings::ENV_VARIABLE_CONFIG_FILE).c_str()); + Settings s; + + auto pylon_driver_settings = s.get_pylon_driver_settings(); + auto tricamera_driver_settings = s.get_tricamera_driver_settings(); + ASSERT_THAT(pylon_driver_settings, NotNull()); + ASSERT_THAT(tricamera_driver_settings, NotNull()); + EXPECT_TRUE(ends_with(pylon_driver_settings->pylon_settings_file, + "config/pylon_camera_settings.txt")); + EXPECT_FLOAT_EQ(tricamera_driver_settings->frame_rate_fps, 10.); +} + +TEST(TestSettings, load_env_file_with_full_config) +{ + std::string tmpfile = std::tmpnam(nullptr); + std::ofstream out(tmpfile); + out << R"TOML( +[pylon_driver] +pylon_settings_file = "path/to/file.txt" + +[tricamera_driver] +frame_rate_fps = 42.1 + +[unrelated_section] +should_not_harm = true +)TOML"; + out.close(); + + setenv(std::string(Settings::ENV_VARIABLE_CONFIG_FILE).c_str(), + tmpfile.c_str(), + true); + Settings s; + + auto pylon_driver_settings = s.get_pylon_driver_settings(); + auto tricamera_driver_settings = s.get_tricamera_driver_settings(); + ASSERT_THAT(pylon_driver_settings, NotNull()); + ASSERT_THAT(tricamera_driver_settings, NotNull()); + EXPECT_EQ(pylon_driver_settings->pylon_settings_file, "path/to/file.txt"); + EXPECT_FLOAT_EQ(tricamera_driver_settings->frame_rate_fps, 42.1); + + std::remove(tmpfile.c_str()); +} + +TEST(TestSettings, load_file_without_config) +{ + std::string tmpfile = std::tmpnam(nullptr); + std::ofstream out(tmpfile); + out << "# no config here"; + out.close(); + + Settings s(tmpfile); + + auto pylon_driver_settings = s.get_pylon_driver_settings(); + auto tricamera_driver_settings = s.get_tricamera_driver_settings(); + ASSERT_THAT(pylon_driver_settings, NotNull()); + ASSERT_THAT(tricamera_driver_settings, NotNull()); + EXPECT_TRUE(ends_with(pylon_driver_settings->pylon_settings_file, + "config/pylon_camera_settings.txt")); + EXPECT_FLOAT_EQ(tricamera_driver_settings->frame_rate_fps, 10.0); + + std::remove(tmpfile.c_str()); +} + +TEST(TestSettings, load_file_with_full_config) +{ + std::string tmpfile = std::tmpnam(nullptr); + std::ofstream out(tmpfile); + out << R"TOML( +[pylon_driver] +pylon_settings_file = "path/to/file.txt" + +[tricamera_driver] +frame_rate_fps = 42.1 + +[unrelated_section] +should_not_harm = true +)TOML"; + out.close(); + + Settings s(tmpfile); + + auto pylon_driver_settings = s.get_pylon_driver_settings(); + auto tricamera_driver_settings = s.get_tricamera_driver_settings(); + ASSERT_THAT(pylon_driver_settings, NotNull()); + ASSERT_THAT(tricamera_driver_settings, NotNull()); + EXPECT_EQ(pylon_driver_settings->pylon_settings_file, "path/to/file.txt"); + EXPECT_FLOAT_EQ(tricamera_driver_settings->frame_rate_fps, 42.1); + + std::remove(tmpfile.c_str()); +} + +TEST(TestSettings, load_file_with_partial_config) +{ + std::string tmpfile = std::tmpnam(nullptr); + std::ofstream out(tmpfile); + out << R"TOML( +[pylon_driver] +pylon_settings_file = "path/to/file.txt" + +[tricamera_driver] +# section exists but no value +)TOML"; + out.close(); + + Settings s(tmpfile); + + auto pylon_driver_settings = s.get_pylon_driver_settings(); + auto tricamera_driver_settings = s.get_tricamera_driver_settings(); + ASSERT_THAT(pylon_driver_settings, NotNull()); + ASSERT_THAT(tricamera_driver_settings, NotNull()); + EXPECT_EQ(pylon_driver_settings->pylon_settings_file, "path/to/file.txt"); + EXPECT_FLOAT_EQ(tricamera_driver_settings->frame_rate_fps, 10.0); + + std::remove(tmpfile.c_str()); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From a098807232cb2022e08b7141a349508d797b7603 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 28 May 2024 16:59:32 +0200 Subject: [PATCH 05/28] Use Settings in PylonDriver Use `Settings` to get path to the Pylon settings file. --- CMakeLists.txt | 4 +--- include/trifinger_cameras/pylon_driver.hpp | 5 ++++- src/pylon_driver.cpp | 14 ++++++-------- 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d97c89..56808fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,9 +104,7 @@ if (${Pylon_FOUND}) ${Pylon_LIBRARIES} robot_interfaces::robot_interfaces camera_observations - ) - ament_target_dependencies(pylon_driver - ament_index_cpp + settings ) list(APPEND install_targets pylon_driver) diff --git a/include/trifinger_cameras/pylon_driver.hpp b/include/trifinger_cameras/pylon_driver.hpp index 915befc..f9d8ed7 100644 --- a/include/trifinger_cameras/pylon_driver.hpp +++ b/include/trifinger_cameras/pylon_driver.hpp @@ -23,6 +23,7 @@ #include #include +#include namespace trifinger_cameras { @@ -39,7 +40,8 @@ class PylonDriver : public robot_interfaces::SensorDriver * to half their original size. */ PylonDriver(const std::string& device_user_id, - bool downsample_images = true); + bool downsample_images = true, + Settings settings = Settings()); ~PylonDriver(); @@ -62,6 +64,7 @@ class PylonDriver : public robot_interfaces::SensorDriver CameraObservation get_observation(); private: + std::shared_ptr settings_; const std::string device_user_id_; const bool downsample_images_; Pylon::PylonAutoInitTerm auto_init_term_; diff --git a/src/pylon_driver.cpp b/src/pylon_driver.cpp index d514ace..93efe9c 100644 --- a/src/pylon_driver.cpp +++ b/src/pylon_driver.cpp @@ -9,7 +9,6 @@ #include #include -#include #include namespace trifinger_cameras @@ -54,8 +53,11 @@ cv::Mat BGR2BayerBG(const cv::Mat& bgr_image) } PylonDriver::PylonDriver(const std::string& device_user_id, - bool downsample_images) - : device_user_id_(device_user_id), downsample_images_(downsample_images) + bool downsample_images, + Settings settings) + : settings_(settings.get_pylon_driver_settings()), + device_user_id_(device_user_id), + downsample_images_(downsample_images) { try { @@ -248,12 +250,8 @@ cv::Mat PylonDriver::downsample_raw_image(const cv::Mat& image) void PylonDriver::set_camera_configuration() { - const std::string filename = - ament_index_cpp::get_package_share_directory("trifinger_cameras") + - "/config/pylon_camera_settings.txt"; - Pylon::CFeaturePersistence::Load( - filename.c_str(), &camera_.GetNodeMap(), true); + settings_->pylon_settings_file.c_str(), &camera_.GetNodeMap(), true); // Use the following command to generate a config file with the current // settings: From 406fa95d696f47041b6e17b43f2d737a67e5fc7c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 28 May 2024 17:10:48 +0200 Subject: [PATCH 06/28] Use Settings in TriCameraDriver Expect a `Settings` instance as argument (optional, by default use the "load file from env variable"-constructor). Use it to set the `rate` at which images are fetched. Also pass it on to PylonDriver instances, so the file is only read once. This now technically allows to change the frame rate of the TriCameraDriver. Note however, that currently the rate of 10Hz is still assumed in some other parts of the code, so changing it via the configuration may not yet work properly in all cases. --- CMakeLists.txt | 1 + include/trifinger_cameras/tricamera_driver.hpp | 6 +++--- src/tricamera_driver.cpp | 16 +++++++++------- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 56808fc..66cb39e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,7 @@ if (${Pylon_FOUND}) ${Pylon_LIBRARIES} robot_interfaces::robot_interfaces pylon_driver + settings ) list(APPEND install_targets tricamera_driver) diff --git a/include/trifinger_cameras/tricamera_driver.hpp b/include/trifinger_cameras/tricamera_driver.hpp index 8bea2fb..398efc8 100644 --- a/include/trifinger_cameras/tricamera_driver.hpp +++ b/include/trifinger_cameras/tricamera_driver.hpp @@ -24,8 +24,7 @@ class TriCameraDriver { public: //! @brief Rate at which images are acquired. - static constexpr std::chrono::milliseconds rate = - std::chrono::milliseconds(100); + std::chrono::milliseconds rate; /** * @param device_id_1 device user id of first camera @@ -37,7 +36,8 @@ class TriCameraDriver TriCameraDriver(const std::string& device_id_1, const std::string& device_id_2, const std::string& device_id_3, - bool downsample_images = true); + bool downsample_images = true, + Settings settings = Settings()); /** * @brief Get the latest observation from the three cameras diff --git a/src/tricamera_driver.cpp b/src/tricamera_driver.cpp index c177024..4e2afbe 100644 --- a/src/tricamera_driver.cpp +++ b/src/tricamera_driver.cpp @@ -5,24 +5,26 @@ * @copyright 2020, Max Planck Gesellschaft. All rights reserved. * @license BSD 3-clause */ +#include #include #include namespace trifinger_cameras { -// this needs to be declared here... -constexpr std::chrono::milliseconds TriCameraDriver::rate; - TriCameraDriver::TriCameraDriver(const std::string& device_id_1, const std::string& device_id_2, const std::string& device_id_3, - bool downsample_images) + bool downsample_images, + Settings settings) : last_update_time_(std::chrono::system_clock::now()), - camera1_(device_id_1, downsample_images), - camera2_(device_id_2, downsample_images), - camera3_(device_id_3, downsample_images) + camera1_(device_id_1, downsample_images, settings), + camera2_(device_id_2, downsample_images, settings), + camera3_(device_id_3, downsample_images, settings) { + auto cfg = Settings().get_tricamera_driver_settings(); + float rate_sec = 1.f / cfg->frame_rate_fps; + rate = std::chrono::milliseconds(std::lround(rate_sec * 1000)); } TriCameraObservation TriCameraDriver::get_observation() From 21513fc80a89875eb07f7cf62f2eff49662f5532 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 10:27:58 +0200 Subject: [PATCH 07/28] doc: Add documentation for Settings --- doc/configuration.rst | 83 ++++++++++++++++++++++++++ doc/pylon_camera_settings.rst | 12 ++++ doc_mainpage.rst | 1 + include/trifinger_cameras/settings.hpp | 15 ++--- 4 files changed, 101 insertions(+), 10 deletions(-) create mode 100644 doc/configuration.rst create mode 100644 doc/pylon_camera_settings.rst diff --git a/doc/configuration.rst b/doc/configuration.rst new file mode 100644 index 0000000..6b42418 --- /dev/null +++ b/doc/configuration.rst @@ -0,0 +1,83 @@ +.. _configuration: + +************* +Configuration +************* + +Some parameters of the Pylon driver classes can be configured via a TOML configuration +file. + +Configuration Options +===================== + +The configuration file has to be in TOML format. Below is an overview of the supported +sections/settings and their default values. All settings are optional. If not +specified the default values are used. + +.. code-block:: toml + + [pylon_driver] + pylon_settings_file = "path/to/default_pylon_camera_settings.txt" + + [tricamera_driver] + frame_rate_fps = 10.0 + + +pylon_driver +------------ + +Settings concerning all Pylon cameras. + +.. list-table:: + + * - ``pylon_settings_file`` + - Path to the file containing the settings for the Pylon SDK (see + :ref:`pylon_settings_file`). If not set, the default settings file shipped with + this package is used (see :ref:`default_pylon_camera_settings`). + + +tricamera_driver +---------------- + +Settings specific to :cpp:class:`~trifinger_cameras::TriCameraDriver`. + +.. list-table:: + + * - ``frame_rate_fps`` + - Frequency at which frames are fetched from the cameras. **Important:** This is + limited by the ``AcquisitionFrameRate`` setting in the + :ref:`pylon_settings_file`, i.e. make sure to set ``AcquisitionFrameRate >= + frame_rate_fps``. + + +How to use the custom configuration +=================================== + +To use the file, simply write the path to the environment variable +``TRIFINGER_CAMERA_CONFIG``. + +Alternatively, if you instantiate the :cpp:class:`~trifinger_cameras::PylonDriver` or +:cpp:class:`~trifinger_cameras::TriCameraDriver` classes in your own code, you can create a +:cpp:class:`~trifinger_cameras::Settings` instance (specify the path to the TOML file in the constructor) +and pass this to the constructor of the driver class. + + +.. _pylon_settings_file: + +Pylon Settings +============== + +The driver for Pylon cameras uses the Pylon SDK, which allows configuration of camera +settings like frame rate, white balancing, etc. +These settings are stored in a file, which is loaded by the +:cpp:class:`~trifinger_cameras::PylonDriver` class during initialisation. The path to +this file can be configured through the ``pylon_settings_file`` setting (see above). + +This package ships with a default settings file (see +:ref:`default_pylon_camera_settings`). For simple changes like adjusting the frame +rate, you may simply copy that file and change the corresponding values. +For more complex changes, it is recommended to use the graphical interface of the +Pylon Viewer application (part of the SDK) and then save the settings to a file using +TODO. + +.. todo:: instructions on how to save settings to file diff --git a/doc/pylon_camera_settings.rst b/doc/pylon_camera_settings.rst new file mode 100644 index 0000000..f5ae316 --- /dev/null +++ b/doc/pylon_camera_settings.rst @@ -0,0 +1,12 @@ +:orphan: + +.. _default_pylon_camera_settings: + +***************************** +Default Pylon Camera Settings +***************************** + +This is the Pylon camera settings file that is shipped with this package and used by +default. For more information see :ref:`pylon_settings_file`. + +.. literalinclude:: /PKG/config/pylon_camera_settings.txt diff --git a/doc_mainpage.rst b/doc_mainpage.rst index 203be47..89badef 100644 --- a/doc_mainpage.rst +++ b/doc_mainpage.rst @@ -8,6 +8,7 @@ The source code is hosted on GitHub_. :hidden: doc/installation.rst + doc/configuration.rst doc/executables.rst doc/pylon.rst doc/troubleshooting.rst diff --git a/include/trifinger_cameras/settings.hpp b/include/trifinger_cameras/settings.hpp index 5b5a95e..23d04e8 100644 --- a/include/trifinger_cameras/settings.hpp +++ b/include/trifinger_cameras/settings.hpp @@ -53,22 +53,17 @@ struct TriCameraDriverSettings /** * @brief Central class for loading settings. * - * This class is the central facility to load settings for this package from a - * TOML file. The file can have the following tables/keys: + * @verbatim embed:rst:leading-asterisk * - * ```toml - * [pylon_driver] - * pylon_settings_file = "path/to/pylon_settings_file.txt" + * Central facility to load settings for this package from a TOML file. * - * [tricamera_driver] - * frame_rate_fps = 10 - * ``` - * - * All values are optional. If not specified, default values are used. + * For information about the config file format, see :ref:`configuration`. * * Constructing the class will parse the TOML file but actual settings objects * for the different modules are only created when the corresponding getter is * called the first time. + * + * @endverbatim */ class Settings { From d89dcf76f9460374a4ef48583fd6e6ac5f087ba9 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 10:28:02 +0200 Subject: [PATCH 08/28] Update CHANGELOG --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3e91700..d92a4d9 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,9 @@ significantly out of focus. - Executable `pylon_write_device_user_id_to_camera` to set the "DeviceUserID" of Pylon cameras. +- Make some settings (e.g. frame rate) configurable through a TOML config file. For + more information see + [documentation](https://open-dynamic-robot-initiative.github.io/trifinger_cameras/doc/configuration.html) ### Removed - Obsolete script `verify_calibration.py` From 72114de16ca5eb510cf9de5ac8b8c33ca39d7b5c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 11:27:15 +0200 Subject: [PATCH 09/28] fix: Open camera when no device_user_id is given & refactor Move the code for connecting to a Pylon camera out of the constructor of PylonDriver to a standalone function. This allows reusing the code. While doing this, I noticed that the steps to open the camera and start grabbing images where accidentally done in the `else`-block handling the case when a DeviceUserID is specified. I remember that connecting to a camera without user id did work in the past, probably due to that. Moving the relevant function calls out of the if/else should fix it. --- include/trifinger_cameras/pylon_driver.hpp | 11 ++ src/pylon_driver.cpp | 130 ++++++++++----------- 2 files changed, 76 insertions(+), 65 deletions(-) diff --git a/include/trifinger_cameras/pylon_driver.hpp b/include/trifinger_cameras/pylon_driver.hpp index f9d8ed7..c654147 100644 --- a/include/trifinger_cameras/pylon_driver.hpp +++ b/include/trifinger_cameras/pylon_driver.hpp @@ -27,6 +27,17 @@ namespace trifinger_cameras { +/** + * @brief Connect to Pylon camera. + * + * @param device_user_id The user-defined name of the camera. Can be set with + * the executable `pylon_write_device_user_id_to_camera`. + * @param camera Pointer to the Pylon::CInstantCamera instance to which the + * camera will be attached. + */ +void pylon_connect(std::string_view device_user_id, + Pylon::CInstantCamera* camera); + /** * @brief Driver for interacting with a camera via Pylon and storing * images using OpenCV. diff --git a/src/pylon_driver.cpp b/src/pylon_driver.cpp index 93efe9c..a3d35b5 100644 --- a/src/pylon_driver.cpp +++ b/src/pylon_driver.cpp @@ -52,78 +52,81 @@ cv::Mat BGR2BayerBG(const cv::Mat& bgr_image) return bayer_img; } -PylonDriver::PylonDriver(const std::string& device_user_id, - bool downsample_images, - Settings settings) - : settings_(settings.get_pylon_driver_settings()), - device_user_id_(device_user_id), - downsample_images_(downsample_images) +void pylon_connect(std::string_view device_user_id, + Pylon::CInstantCamera* camera) { - try + Pylon::PylonInitialize(); + Pylon::CTlFactory& tl_factory = Pylon::CTlFactory::GetInstance(); + Pylon::DeviceInfoList_t device_list; + + if (tl_factory.EnumerateDevices(device_list) == 0) { - Pylon::CTlFactory& tl_factory = Pylon::CTlFactory::GetInstance(); - Pylon::PylonInitialize(); - Pylon::DeviceInfoList_t device_list; + Pylon::PylonTerminate(); + throw std::runtime_error("No devices present, please connect one."); + } - if (tl_factory.EnumerateDevices(device_list) == 0) + Pylon::DeviceInfoList_t::const_iterator device_iterator; + if (device_user_id.empty()) + { + std::cout << "No device ID specified. Creating a camera object " + "with the first device id in the device list." + << std::endl; + device_iterator = device_list.begin(); + camera->Attach(tl_factory.CreateDevice(*device_iterator)); + } + else + { + bool found_desired_device = false; + + for (device_iterator = device_list.begin(); + device_iterator != device_list.end(); + ++device_iterator) { - Pylon::PylonTerminate(); - throw std::runtime_error("No devices present, please connect one."); + std::string device_user_id_found( + device_iterator->GetUserDefinedName()); + if (device_user_id == device_user_id_found) + { + found_desired_device = true; + break; + } } - Pylon::DeviceInfoList_t::const_iterator device_iterator; - if (device_user_id.empty()) + if (found_desired_device) { - device_iterator = device_list.begin(); - camera_.Attach(tl_factory.CreateDevice(*device_iterator)); - std::cout << "No device ID specified. Creating a camera object " - "with the first device id in the device list." - << std::endl; + camera->Attach(tl_factory.CreateDevice(*device_iterator)); } else { - bool found_desired_device = false; - - for (device_iterator = device_list.begin(); - device_iterator != device_list.end(); - ++device_iterator) - { - std::string device_user_id_found( - device_iterator->GetUserDefinedName()); - if (device_user_id == device_user_id_found) - { - found_desired_device = true; - break; - } - } - - if (found_desired_device) - { - camera_.Attach(tl_factory.CreateDevice(*device_iterator)); - } - else - { - Pylon::PylonTerminate(); - throw std::runtime_error( - "Device id " + device_user_id_ + - " doesn't correspond to any " - "connected devices, please retry with a valid id."); - } - - camera_.Open(); - camera_.MaxNumBuffer = 5; - - set_camera_configuration(); - - camera_.StartGrabbing(Pylon::GrabStrategy_LatestImageOnly); + Pylon::PylonTerminate(); + throw std::runtime_error( + "Device id " + std::string(device_user_id) + + " doesn't correspond to any " + "connected devices, please retry with a valid id."); } + } + + camera->Open(); + camera->MaxNumBuffer = 5; +} +PylonDriver::PylonDriver(const std::string& device_user_id, + bool downsample_images, + Settings settings) + : settings_(settings.get_pylon_driver_settings()), + device_user_id_(device_user_id), + downsample_images_(downsample_images) +{ + try + { + pylon_connect(device_user_id, &camera_); + set_camera_configuration(); + camera_.StartGrabbing(Pylon::GrabStrategy_LatestImageOnly); format_converter_.OutputPixelFormat = Pylon::PixelType_BGR8packed; } catch (const Pylon::GenericException& e) { - // convert Pylon exceptions to an std exception, so it is understood by - // pybind11 + // convert Pylon exceptions to an std exception, so it is understood + // by pybind11 throw std::runtime_error("Camera Error (" + device_user_id_ + "): " + e.what()); } @@ -152,7 +155,8 @@ CameraObservation PylonDriver::get_observation() if (ptr_grab_result->GrabSucceeded()) { - // ensure that the actual image size matches with the expected one + // ensure that the actual image size matches with the expected + // one if (ptr_grab_result->GetHeight() / 2 != image_frame.height || ptr_grab_result->GetWidth() / 2 != image_frame.width) { @@ -172,7 +176,8 @@ CameraObservation PylonDriver::get_observation() Pylon::CPylonImage pylon_image_bgr; format_converter_.Convert(pylon_image_bgr, ptr_grab_result); - // NOTE: the cv::Mat points to the memory of pylon_image_bgr! + // NOTE: the cv::Mat points to the memory of + // pylon_image_bgr! cv::Mat image_bgr = cv::Mat(ptr_grab_result->GetHeight(), ptr_grab_result->GetWidth(), @@ -213,8 +218,8 @@ CameraObservation PylonDriver::get_observation() } catch (const Pylon::GenericException& e) { - // convert Pylon exceptions to an std exception, so it is understood by - // pybind11 + // convert Pylon exceptions to an std exception, so it is understood + // by pybind11 throw std::runtime_error("Camera Error (" + device_user_id_ + "): " + e.what()); } @@ -252,10 +257,5 @@ void PylonDriver::set_camera_configuration() { Pylon::CFeaturePersistence::Load( settings_->pylon_settings_file.c_str(), &camera_.GetNodeMap(), true); - - // Use the following command to generate a config file with the current - // settings: - // Pylon::CFeaturePersistence::Save("/tmp/camera_settings.txt", - // &camera_.GetNodeMap()); } } // namespace trifinger_cameras From 0f066203d77c41d6cb685593012ab45ee574892b Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 11:33:20 +0200 Subject: [PATCH 10/28] Add executable pylon_dump_camera_settings This is a simple tool to load the current settings of a Pylon camera and print it to stdout (so it can be piped into a file). This makes it easier to produce a new Pylon settings file for use with PylonDriver. --- CMakeLists.txt | 15 +++++ src/pylon_dump_camera_settings.cpp | 91 ++++++++++++++++++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 src/pylon_dump_camera_settings.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 66cb39e..86c9157 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(robot_interfaces REQUIRED) find_package(serialization_utils REQUIRED) find_package(yaml_utils REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(cli_utils REQUIRED) find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) @@ -148,6 +149,20 @@ if (${Pylon_FOUND}) target_link_libraries(pylon_write_device_user_id_to_camera ${Pylon_LIBRARIES}) list(APPEND install_targets pylon_write_device_user_id_to_camera) + add_executable(pylon_dump_camera_settings + src/pylon_dump_camera_settings.cpp + ) + target_include_directories(pylon_dump_camera_settings PUBLIC + $ + $ + ${Pylon_INCLUDE_DIRS} + ) + target_link_libraries(pylon_dump_camera_settings + ${Pylon_LIBRARIES} + pylon_driver + cli_utils::program_options + ) + list(APPEND install_targets pylon_dump_camera_settings) # Set library names to variables, so we can use the variable instead of the # direct name below. In case, Pylon drivers are not built, the variables diff --git a/src/pylon_dump_camera_settings.cpp b/src/pylon_dump_camera_settings.cpp new file mode 100644 index 0000000..c0b84df --- /dev/null +++ b/src/pylon_dump_camera_settings.cpp @@ -0,0 +1,91 @@ +/** + * @file + * @brief Connect to Pylon camera and print its settings to stdout. + * @copyright 2020 Max Planck Gesellschaft. All rights reserved. + * @license BSD 3-clause + */ +#include + +#include + +#include + +class Args : public cli_utils::ProgramOptions +{ +public: + std::string device_user_id = ""; + + std::string help() const override + { + return R"(Connect to Pylon camera and print its settings to stdout. + + If no device name is specified, uses the first camera that is found (so make + sure only one camera is connected in that case). + +Usage: pylon_dump_camera_settings [] + +)"; + } + + // in add_options the arguments are defined + void add_options(boost::program_options::options_description &options, + boost::program_options::positional_options_description + &positional) override + { + namespace po = boost::program_options; + + // The chaining of parentheses calls does not go well with clang-format, + // so better disable auto-formatting for this block. + + // clang-format off + options.add_options() + ("device_user_id", + po::value(&device_user_id), + "'DeviceUserID' of the camera.") + ; + // clang-format on + + // mark 'message' as positional argument + positional.add("device_user_id", 1); + } +}; + +int main(int argc, char *argv[]) +{ + Args args; + if (!args.parse_args(argc, argv)) + { + return 1; + } + + // need braces so that Pylon::CInstantCamera instance is destructed before the end + // (otherwise segfaults) + { + Pylon::CInstantCamera camera; + try + { + trifinger_cameras::pylon_connect(args.device_user_id, &camera); + } + catch (const Pylon::GenericException &e) + { + std::cerr << "ERROR: " << e.what() << std::endl; + return 1; + } + catch (const std::exception &e) + { + std::cerr << "ERROR: " << e.what() << std::endl; + return 1; + } + + Pylon::String_t output; + Pylon::CFeaturePersistence::SaveToString(output, &camera.GetNodeMap()); + std::cout << output << std::endl; + + camera.Close(); + } + + // Releases all pylon resources. + Pylon::PylonTerminate(); + + return 0; +} From 6f94f9347f9fe49b42768026ba7d8de46d13d905 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 12:05:32 +0200 Subject: [PATCH 11/28] doc: Add documentation for pylon-related executables --- doc/configuration.rst | 4 +-- doc/executables.rst | 64 +++++++++++++++++++++++++++++++++++++------ doc/pylon.rst | 39 +++++++++++++++++--------- doc_mainpage.rst | 7 ++++- 4 files changed, 88 insertions(+), 26 deletions(-) diff --git a/doc/configuration.rst b/doc/configuration.rst index 6b42418..fe76d33 100644 --- a/doc/configuration.rst +++ b/doc/configuration.rst @@ -78,6 +78,4 @@ This package ships with a default settings file (see rate, you may simply copy that file and change the corresponding values. For more complex changes, it is recommended to use the graphical interface of the Pylon Viewer application (part of the SDK) and then save the settings to a file using -TODO. - -.. todo:: instructions on how to save settings to file +:ref:`executable_pylon_dump_camera_settings`. diff --git a/doc/executables.rst b/doc/executables.rst index a49ec18..05e8c18 100644 --- a/doc/executables.rst +++ b/doc/executables.rst @@ -5,17 +5,16 @@ Executables The package contains a number of executables for calibration, testing, data analysis, etc. -They can all be run like this: +.. note:: -:: - - ros2 run trifinger_cameras [] + All executables described here can be run via ``ros2 run``, i.e. like this: -For example + .. code-block:: sh -:: + $ ros2 run trifinger_cameras - ros2 run trifinger_cameras tricamera_log_viewer path/to/camera_data.dat + This part is omitted in the examples below for better readability but should be added + when actually executing them. .. todo:: @@ -28,13 +27,11 @@ For example - calibrate_trifingerpro_cameras - camera_log_viewer - charuco_board - - check_camera_sharpness - convert_to_camera_pose - detect_aruco_marker - load_camera_config_test - overlay_camera_stream - overlay_real_and_rendered_images - - pylon_list_cameras - record_image_dataset - tricamera_log_converter - tricamera_log_viewer @@ -59,6 +56,7 @@ viewer applications for basic tests. Run with ``--help`` to get a full list of options for each demo. +.. _executable_check_camera_sharpness: check_camera_sharpness ====================== @@ -90,3 +88,51 @@ As argument, the ID of the camera needs to be passed. Example: Additional arguments can be used to set initial values for the parameters. Run with ``--help`` to get a complete list. + + + +.. _executable_pylon_list_cameras: + +pylon_list_cameras +================== + +List all currently connected Pylon cameras. Doesn't expect any arguments, i.e. simply +run + +.. code-block:: sh + + $ pylon_list_cameras + + +.. _executable_pylon_write_device_user_id_to_camera: + +pylon_write_device_user_id_to_camera +==================================== + +Writes the "DeviceUserID" to a Pylon camera. This ID is needed to distinguish cameras +if multiple of them are connected. See also :ref:`pylon_set_device_user_id`. + +Expects as argument the ID and writes it to the first camera found, so **make sure no +other camera is connected** before running the command. + +.. code-block:: sh + + $ pylon_write_device_user_id_to_camera "some_id" + +Once written, the "DeviceUserID" will be displayed by the PylonViewerApp (unfortunately +it's not possible to modify it there). + + +.. _executable_pylon_dump_camera_settings: + +pylon_dump_camera_settings +========================== + +Connect to a Pylon camera and print its settings to stdout. When saved to a file, this +can be used as ``pylon_settings_file`` in the Pylon driver (see :ref:`settings`). + +Usage (saving the settings to a file "camera_settings.txt": + +.. code-block:: sh + + $ pylon_dump_camera_settings "device_user_id" > camera_settings.txt diff --git a/doc/pylon.rst b/doc/pylon.rst index e1c83fb..1e86aa8 100644 --- a/doc/pylon.rst +++ b/doc/pylon.rst @@ -37,28 +37,41 @@ to set up the udev rules on the host system (see last step in the section above). -Configuring Cameras -=================== +.. _pylon_set_device_user_id: + +Set Device Name +=============== The Pylon camera drivers in trifinger_cameras expect a unique "DeviceUserID" written to the camera to be able to identify it (especially important for the :cpp:class:`~trifinger_cameras::TriCameraDriver` where the three cameras need to be distinguished. -This ID can be set using using the ``pylon_write_device_user_id_to_camera`` -command that is included in the package, using the following steps: - -1. Connect the camera to the computer. **Make sure no other camera is - connected** (the tool will simply write to the first camera found). -2. Run - - .. code-block:: sh - - ros2 run trifinger_cameras pylon_write_device_user_id_to_camera "some_id" +This ID can be set using using the +:ref:`executable_pylon_write_device_user_id_to_camera` command that is included in the +package. Once written, the "DeviceUserID" will be displayed by the PylonViewerApp -(unfortunately it's not possible to modifiy it there). +(unfortunately it's not possible to modify it there). For the TriFinger robots, we use the IDs "camera60", "camera180" and "camera300" based on their approximate angular position relative to the fingers, see :ref:`trifinger_docs:finger_and_camera_names`. + + +Camera Configuration +==================== + +For configuration of camera settings like frame rate, white balancing, etc., see +:ref:`pylon_settings_file`. + + +Command Line Tools +================== + +This package contains a number of command line tools for general handling of Pylon +cameras: + +- :ref:`executable_pylon_list_cameras` +- :ref:`executable_pylon_write_device_user_id_to_camera` +- :ref:`executable_pylon_dump_camera_settings` diff --git a/doc_mainpage.rst b/doc_mainpage.rst index 89badef..1ad03a4 100644 --- a/doc_mainpage.rst +++ b/doc_mainpage.rst @@ -9,8 +9,13 @@ The source code is hosted on GitHub_. doc/installation.rst doc/configuration.rst - doc/executables.rst doc/pylon.rst doc/troubleshooting.rst +.. toctree:: + :caption: References + :hidden: + + doc/executables.rst + .. _GitHub: https://github.com/open-dynamic-robot-initiative/trifinger_cameras From 5e15d494d53b066e4a0425b17c821bcf498fb16c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 16:18:30 +0200 Subject: [PATCH 12/28] (check_camera_sharpness) Do not assume camera rate The previous implementation seemed to assume a hard-coded frame rate of 10 fps. This wasn't even correct as PylonDriver uses the camera frame rate which is configured via the Pylon settings file (currently 100fps). Instead add a parameter "--update-freq" (still defaulting to 10) and use that to compute the sleep duration of `cv2.waitKey`. Behaviour should be mostly the same as before (with default values, the sleep duration just changes from 90 to 100) but no explicit camera frame rate is assumed anymore. --- scripts/check_camera_sharpness.py | 39 +++++++++++++++++-------------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/scripts/check_camera_sharpness.py b/scripts/check_camera_sharpness.py index 6c47ad2..98056e2 100644 --- a/scripts/check_camera_sharpness.py +++ b/scripts/check_camera_sharpness.py @@ -10,6 +10,8 @@ This test is also performed in robot_fingers/trifingerpro_post_submission.py. """ + +from __future__ import annotations import argparse import dataclasses import typing @@ -24,7 +26,7 @@ def add_label( image: np.ndarray, label: str, position: typing.Literal["top", "bottom"] -): +) -> np.ndarray: """Add label to the given image. Args: @@ -59,9 +61,7 @@ def add_label( return image -def auto_canny_params( - image: np.ndarray, sigma: float = 0.33 -) -> typing.Tuple[float, float]: +def auto_canny_params(image: np.ndarray, sigma: float = 0.33) -> tuple[float, float]: median = np.median(image) lower = max(0, (1.0 - sigma) * median) upper = min(255, (1.0 + sigma) * median) @@ -85,7 +85,7 @@ def set_edge_mean_threshold(self, value: float) -> None: self.edge_mean_threshold = value -def main(): +def main() -> None: argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "camera_id", @@ -116,20 +116,29 @@ def main(): default=3, help="Buffer size in seconds for the smoothed edge mean.", ) + argparser.add_argument( + "--update-freq", + type=float, + default=10.0, + help="""Frequency at which new images are fetched from the camera. Should not + be higher than the actual frame rate of the camera. + """, + ) args = argparser.parse_args() camera_data = trifinger_cameras.camera.SingleProcessData() camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) - camera_backend = trifinger_cameras.camera.Backend( # noqa + camera_backend = trifinger_cameras.camera.Backend( # noqa: F841 camera_driver, camera_data ) camera_frontend = trifinger_cameras.camera.Frontend(camera_data) params = Params(args.canny[0], args.canny[1], args.threshold) - camera_fps = 10 - mean_buffer = deque([], maxlen=camera_fps * args.buffer_size) + mean_buffer: deque[float] = deque( + [], maxlen=int(args.update_freq * args.buffer_size) + ) window_name = f"Image Stream [{args.camera_id}]" cv2.namedWindow(window_name) @@ -141,9 +150,7 @@ def main(): max(200, params.canny_threshold1), params.set_canny_threshold1, ) - cv2.setTrackbarPos( - "Canny threshold 1", window_name, int(params.canny_threshold1) - ) + cv2.setTrackbarPos("Canny threshold 1", window_name, int(params.canny_threshold1)) cv2.createTrackbar( "Canny threshold 2", window_name, @@ -151,9 +158,7 @@ def main(): max(400, params.canny_threshold2), params.set_canny_threshold2, ) - cv2.setTrackbarPos( - "Canny threshold 2", window_name, int(params.canny_threshold2) - ) + cv2.setTrackbarPos("Canny threshold 2", window_name, int(params.canny_threshold2)) cv2.createTrackbar( "Edge mean threshold", window_name, @@ -165,6 +170,7 @@ def main(): "Edge mean threshold", window_name, int(params.edge_mean_threshold) ) + rate_ms = int(1000 / args.update_freq) while True: observation = camera_frontend.get_latest_observation() image = utils.convert_image(observation.image) @@ -197,15 +203,14 @@ def main(): add_label(image, args.camera_id, "top") add_label( image, - "Canny mean: %.1f | smoothed: %.1f" - % (edges_mean, mean_buffer_mean), + "Canny mean: %.1f | smoothed: %.1f" % (edges_mean, mean_buffer_mean), "bottom", ) cv2.imshow(window_name, image) # stop if either "q" or ESC is pressed - if cv2.waitKey(90) in [ord("q"), 27]: # 27 = ESC + if cv2.waitKey(rate_ms) in [ord("q"), 27]: # 27 = ESC break From 0a1f605d7f414f4cd5c682cab03ac876db63b3ae Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 29 May 2024 16:51:17 +0200 Subject: [PATCH 13/28] PyBulletTriCameraDriver: Use Settings to get frame rate --- CMakeLists.txt | 1 + .../pybullet_tricamera_driver.hpp | 7 ++++++- src/pybullet_tricamera_driver.cpp | 21 +++++++++++++++---- 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 86c9157..da05b0e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -179,6 +179,7 @@ target_include_directories(pybullet_tricamera_driver PUBLIC ) target_link_libraries(pybullet_tricamera_driver camera_observations + settings serialization_utils::serialization_utils robot_interfaces::robot_interfaces pybind11_opencv::pybind11_opencv diff --git a/include/trifinger_cameras/pybullet_tricamera_driver.hpp b/include/trifinger_cameras/pybullet_tricamera_driver.hpp index ffa207d..8a502b6 100644 --- a/include/trifinger_cameras/pybullet_tricamera_driver.hpp +++ b/include/trifinger_cameras/pybullet_tricamera_driver.hpp @@ -11,6 +11,7 @@ #include #include #include +#include namespace trifinger_cameras { @@ -23,7 +24,8 @@ class PyBulletTriCameraDriver : public robot_interfaces::SensorDriver< public: PyBulletTriCameraDriver( robot_interfaces::TriFingerTypes::BaseDataPtr robot_data, - bool render_images = true); + bool render_images = true, + Settings settings = Settings()); /** * @brief Get the latest observation from the three cameras @@ -47,6 +49,9 @@ class PyBulletTriCameraDriver : public robot_interfaces::SensorDriver< //! @brief Last robot time index at which a camera observation was returned. // Needed for time synchronisation. time_series::Index last_update_robot_time_index_; + + //! Number of robot time steps after which the next frame should be fetched. + int frame_rate_in_robot_steps_; }; } // namespace trifinger_cameras diff --git a/src/pybullet_tricamera_driver.cpp b/src/pybullet_tricamera_driver.cpp index 1a2eaa2..432aace 100644 --- a/src/pybullet_tricamera_driver.cpp +++ b/src/pybullet_tricamera_driver.cpp @@ -16,13 +16,22 @@ namespace py = pybind11; namespace trifinger_cameras { +constexpr int ROBOT_STEPS_PER_SECOND = 1000; + PyBulletTriCameraDriver::PyBulletTriCameraDriver( robot_interfaces::TriFingerTypes::BaseDataPtr robot_data, - bool render_images) + bool render_images, + Settings settings) : render_images_(render_images), robot_data_(robot_data), last_update_robot_time_index_(0) { + // compute frame rate in robot steps + float frame_rate_fps = + settings.get_tricamera_driver_settings()->frame_rate_fps; + frame_rate_in_robot_steps_ = + std::lround(ROBOT_STEPS_PER_SECOND / frame_rate_fps); + // initialize Python interpreter if not already done if (!Py_IsInitialized()) { @@ -57,11 +66,15 @@ PyBulletTriCameraDriver::get_observation() } else { - // Synchronize with robot backend: To achieve 10 Hz, there should be - // one camera observation every 100 robot steps. - while (robot_t < last_update_robot_time_index_ + 100) + // Synchronize with robot backend: To achieve the desired frame rate, + // there should be one camera observation every + // `frame_rate_in_robot_steps_` robot steps. + while (robot_t < + last_update_robot_time_index_ + frame_rate_in_robot_steps_) { using namespace std::chrono_literals; + // TODO the sleep here might be problematic if very high frame rate is + // required std::this_thread::sleep_for(10ms); auto new_robot_t = robot_data_->observation->newest_timeindex(false); From 320d86afd27798a9cc72bee338e99843c82c9576 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 09:27:01 +0200 Subject: [PATCH 14/28] Add Python bindings for Settings --- CMakeLists.txt | 2 +- srcpy/py_camera_types.cpp | 16 ++++++++++++++++ srcpy/py_tricamera_types.cpp | 1 + 3 files changed, 18 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index da05b0e..4bd1899 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -321,5 +321,5 @@ ament_export_libraries( pybullet_tricamera_driver camera_calibration_parser ) -ament_export_dependencies(yaml_utils sensor_msgs) +ament_export_dependencies(yaml_utils sensor_msgs tomlplusplus) ament_package(CONFIG_EXTRAS cmake/cfg_extras.cmake.in) diff --git a/srcpy/py_camera_types.cpp b/srcpy/py_camera_types.cpp index b20a1ba..9a0ad83 100644 --- a/srcpy/py_camera_types.cpp +++ b/srcpy/py_camera_types.cpp @@ -9,6 +9,7 @@ #include #include +#include #ifdef Pylon_FOUND #include #endif @@ -44,4 +45,19 @@ PYBIND11_MODULE(py_camera_types, m) .def_readwrite("timestamp", &CameraObservation::timestamp, "Timestamp when the image was acquired."); + + pybind11::class_>( + m, "PylonDriverSettings") + .def_readonly("pylon_settings_file", + &PylonDriverSettings::pylon_settings_file); + pybind11::class_>( + m, "TriCameraDriverSettings") + .def_readonly("frame_rate_fps", + &TriCameraDriverSettings::frame_rate_fps); + pybind11::class_(m, "Settings") + .def(pybind11::init<>()) + .def("get_pylon_driver_settings", &Settings::get_pylon_driver_settings) + .def("get_tricamera_driver_settings", + &Settings::get_tricamera_driver_settings); } diff --git a/srcpy/py_tricamera_types.cpp b/srcpy/py_tricamera_types.cpp index 6ed1165..82f0901 100644 --- a/srcpy/py_tricamera_types.cpp +++ b/srcpy/py_tricamera_types.cpp @@ -33,6 +33,7 @@ PYBIND11_MODULE(py_tricamera_types, m) pybind11::arg("camera2"), pybind11::arg("camera3"), pybind11::arg("downsample_images") = true) + .def_readonly("rate", &TriCameraDriver::rate) .def("get_observation", &TriCameraDriver::get_observation); #endif From 83ea9410565d67b2f0e56d0ddb082d5708713676 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 12:29:50 +0200 Subject: [PATCH 15/28] pylon_list_cameras: Keep stdout clean if there are no cameras --- src/pylon_list_cameras.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pylon_list_cameras.cpp b/src/pylon_list_cameras.cpp index 7b979f7..27b6f60 100644 --- a/src/pylon_list_cameras.cpp +++ b/src/pylon_list_cameras.cpp @@ -21,7 +21,7 @@ int main() if (tl_factory.EnumerateDevices(device_list) == 0) { - std::cout << "No cameras found." << std::endl; + std::cerr << "No cameras found." << std::endl; } else { From 2bed6f415a87e000f8906b1bd9a758163349499c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 13:44:27 +0200 Subject: [PATCH 16/28] docs: Update info on writing pylon device user id --- doc/executables.rst | 13 +++++++++---- doc/pylon.rst | 25 +++++++++++++++++-------- 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/doc/executables.rst b/doc/executables.rst index 05e8c18..222542e 100644 --- a/doc/executables.rst +++ b/doc/executables.rst @@ -109,8 +109,9 @@ run pylon_write_device_user_id_to_camera ==================================== -Writes the "DeviceUserID" to a Pylon camera. This ID is needed to distinguish cameras -if multiple of them are connected. See also :ref:`pylon_set_device_user_id`. +Writes a custom device name ("DeviceUserID") to a Pylon camera. This ID is needed to +distinguish cameras if multiple of them are connected. See also +:ref:`pylon_set_device_user_id`. Expects as argument the ID and writes it to the first camera found, so **make sure no other camera is connected** before running the command. @@ -119,8 +120,12 @@ other camera is connected** before running the command. $ pylon_write_device_user_id_to_camera "some_id" -Once written, the "DeviceUserID" will be displayed by the PylonViewerApp (unfortunately -it's not possible to modify it there). +After writing, the camera needs to be reset for the change to become active (e.g. by +unplugging and plugging in again). + +Starting with version 6 of the Pylon SDK, the "Device User ID" can also be set using the +"pylon Viewer" application that ships with the SDK. Older versions will list the camera +with the given name but don't seem to have an option to change it. .. _executable_pylon_dump_camera_settings: diff --git a/doc/pylon.rst b/doc/pylon.rst index 1e86aa8..38bdca6 100644 --- a/doc/pylon.rst +++ b/doc/pylon.rst @@ -42,17 +42,22 @@ above). Set Device Name =============== -The Pylon camera drivers in trifinger_cameras expect a unique "DeviceUserID" -written to the camera to be able to identify it (especially important for the -:cpp:class:`~trifinger_cameras::TriCameraDriver` where the three cameras need to +The Pylon camera drivers in trifinger_cameras expect a unique device name +("DeviceUserID") written to the camera to be able to identify it (especially important +for the :cpp:class:`~trifinger_cameras::TriCameraDriver` where the three cameras need to be distinguished. -This ID can be set using using the -:ref:`executable_pylon_write_device_user_id_to_camera` command that is included in the -package. +There are two ways to set the name: -Once written, the "DeviceUserID" will be displayed by the PylonViewerApp -(unfortunately it's not possible to modify it there). +1. Using the :ref:`executable_pylon_write_device_user_id_to_camera` command that is + included in the package. +2. Via the "pylon Viewer" application that is shipped with the Pylon SDK 6 or later + [1]_. The "Device User ID" can be modified in the "Device Control" category of the + camera settings. + +Note that after setting a new name, the camera needs to be reset for the change to +become active (either via the pylon Viewer app or by simply unplugging and plugging in +again). For the TriFinger robots, we use the IDs "camera60", "camera180" and "camera300" based on their approximate angular position relative to the fingers, see @@ -75,3 +80,7 @@ cameras: - :ref:`executable_pylon_list_cameras` - :ref:`executable_pylon_write_device_user_id_to_camera` - :ref:`executable_pylon_dump_camera_settings` + + +.. [1] In version 5, the name will be displayed once set but there doesn't seem to be an + option to modify it. From 392c8f74314de5576e19f2258cdce69889d5b71c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 14:11:07 +0200 Subject: [PATCH 17/28] More informative error if config file parsing fails Add some more context to the error message that is shown when parsing the configuration file fails. Add fmt library for more convenient string formatting. --- CMakeLists.txt | 2 ++ include/trifinger_cameras/settings.hpp | 2 ++ src/settings.cpp | 18 ++++++++++++++++-- 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4bd1899..db933a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(sensor_msgs REQUIRED) find_package(cli_utils REQUIRED) find_package(Eigen3 REQUIRED) +find_package(fmt REQUIRED) find_package(OpenCV REQUIRED) find_package(tomlplusplus REQUIRED) @@ -57,6 +58,7 @@ target_include_directories(settings PUBLIC ${OpenCV_INCLUDE_DIRS}) target_link_libraries(settings tomlplusplus::tomlplusplus + fmt::fmt ) ament_target_dependencies(settings ament_index_cpp diff --git a/include/trifinger_cameras/settings.hpp b/include/trifinger_cameras/settings.hpp index 23d04e8..5d09323 100644 --- a/include/trifinger_cameras/settings.hpp +++ b/include/trifinger_cameras/settings.hpp @@ -96,6 +96,8 @@ class Settings toml::table config_; std::shared_ptr pylon_driver_settings_; std::shared_ptr tricamera_driver_settings_; + + void parse_file(const std::filesystem::path& file); }; } // namespace trifinger_cameras diff --git a/src/settings.cpp b/src/settings.cpp index bf45989..e9aefcb 100644 --- a/src/settings.cpp +++ b/src/settings.cpp @@ -7,6 +7,8 @@ #include +#include + namespace trifinger_cameras { std::shared_ptr PylonDriverSettings::load_from_toml( @@ -61,7 +63,7 @@ Settings::Settings() std::getenv(std::string(ENV_VARIABLE_CONFIG_FILE).c_str()); if (config_file_path) { - config_ = toml::parse_file(config_file_path); + parse_file(config_file_path); } // If no file is specified, simply do nothing here. This results in config_ // being an empty toml::table and thus the `load_from_toml()` functions @@ -70,9 +72,21 @@ Settings::Settings() Settings::Settings(const std::filesystem::path& file) { - config_ = toml::parse_file(file.string()); + parse_file(file); } +void Settings::parse_file(const std::filesystem::path& file) +{ + try + { + config_ = toml::parse_file(file.string()); + } + catch (const toml::parse_error& e) + { + throw std::runtime_error(fmt::format( + "Failed to parse config file '{}': {}", file.c_str(), e.what())); + } +} std::shared_ptr Settings::get_pylon_driver_settings() { if (!pylon_driver_settings_) From 1f444e3cafb4313c426f94c1ee4eef7ca2b78a68 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 14:33:11 +0200 Subject: [PATCH 18/28] Use fmt in PylonDriver --- src/pylon_driver.cpp | 44 +++++++++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/src/pylon_driver.cpp b/src/pylon_driver.cpp index a3d35b5..bc2f9d2 100644 --- a/src/pylon_driver.cpp +++ b/src/pylon_driver.cpp @@ -9,6 +9,7 @@ #include #include +#include #include namespace trifinger_cameras @@ -68,10 +69,12 @@ void pylon_connect(std::string_view device_user_id, Pylon::DeviceInfoList_t::const_iterator device_iterator; if (device_user_id.empty()) { - std::cout << "No device ID specified. Creating a camera object " - "with the first device id in the device list." - << std::endl; device_iterator = device_list.begin(); + + fmt::print( + "No device ID specified. Connecting to first camera in the list " + "({})\n", + device_iterator->GetUserDefinedName()); camera->Attach(tl_factory.CreateDevice(*device_iterator)); } else @@ -99,9 +102,9 @@ void pylon_connect(std::string_view device_user_id, { Pylon::PylonTerminate(); throw std::runtime_error( - "Device id " + std::string(device_user_id) + - " doesn't correspond to any " - "connected devices, please retry with a valid id."); + fmt::format("Device id {} doesn't correspond to any connected " + "devices, please retry with a valid id.", + device_user_id)); } } @@ -127,8 +130,8 @@ PylonDriver::PylonDriver(const std::string& device_user_id, { // convert Pylon exceptions to an std exception, so it is understood // by pybind11 - throw std::runtime_error("Camera Error (" + device_user_id_ + - "): " + e.what()); + throw std::runtime_error( + fmt::format("Camera Error ({}): {}", device_user_id_, e.what())); } } @@ -160,15 +163,14 @@ CameraObservation PylonDriver::get_observation() if (ptr_grab_result->GetHeight() / 2 != image_frame.height || ptr_grab_result->GetWidth() / 2 != image_frame.width) { - std::stringstream msg; - msg << device_user_id_ << ": " - << "Size of grabbed frame (" << ptr_grab_result->GetWidth() - << "x" << ptr_grab_result->GetHeight() - << ") does not match expected size (" - << image_frame.width * 2 << "x" << image_frame.height * 2 - << ")."; - - throw std::length_error(msg.str()); + throw std::length_error( + fmt::format("{}: Size of grabbed frame ({}x{}) does not " + "match expected size ({}x{}).", + device_user_id_, + ptr_grab_result->GetWidth(), + ptr_grab_result->GetHeight(), + image_frame.width * 2, + image_frame.height * 2)); } if (downsample_images_) @@ -212,16 +214,16 @@ CameraObservation PylonDriver::get_observation() } else { - throw std::runtime_error("Failed to grab image from camera " + - device_user_id_ + "."); + throw std::runtime_error(fmt::format( + "Failed to grab image from camera '{}'.", device_user_id_)); } } catch (const Pylon::GenericException& e) { // convert Pylon exceptions to an std exception, so it is understood // by pybind11 - throw std::runtime_error("Camera Error (" + device_user_id_ + - "): " + e.what()); + throw std::runtime_error( + fmt::format("Camera Error ({}): {}", device_user_id_, e.what())); } return image_frame; From 77a977b81268865ea0092aaddbbee5863443c73c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 14:33:31 +0200 Subject: [PATCH 19/28] Nicer error output in demo_camera.py --- demos/demo_camera.py | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/demos/demo_camera.py b/demos/demo_camera.py index 54488a3..65e4427 100644 --- a/demos/demo_camera.py +++ b/demos/demo_camera.py @@ -7,13 +7,15 @@ camera, and the available methods for that. """ import argparse +import sys + import cv2 import trifinger_cameras from trifinger_cameras import utils -def main(): +def main() -> int: argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "--pylon", @@ -39,13 +41,17 @@ def main(): camera_data = trifinger_cameras.camera.SingleProcessData() # camera_data = trifinger_cameras.camera.MultiProcessData("cam", True, 10) - if args.pylon: - camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) - else: - camera_id = int(args.camera_id) if args.camera_id else 0 - camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id) + try: + if args.pylon: + camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) + else: + camera_id = int(args.camera_id) if args.camera_id else 0 + camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id) + except Exception as e: + print("Failed to initialise driver:", e) + return 1 - camera_backend = trifinger_cameras.camera.Backend( # noqa + camera_backend = trifinger_cameras.camera.Backend( # noqa: F841 camera_driver, camera_data ) camera_frontend = trifinger_cameras.camera.Frontend(camera_data) @@ -68,6 +74,8 @@ def main(): print("Save recorded data to file {}".format(args.record)) logger.stop_and_save(args.record) + return 0 + if __name__ == "__main__": - main() + sys.exit(main()) From 40adfa2238fb4d7e6fb87512dd17c7dabb7cecf9 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Wed, 5 Jun 2024 14:33:57 +0200 Subject: [PATCH 20/28] Load device ID from camera if not specified. --- include/trifinger_cameras/pylon_driver.hpp | 5 +++-- src/pylon_driver.cpp | 4 +++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/trifinger_cameras/pylon_driver.hpp b/include/trifinger_cameras/pylon_driver.hpp index c654147..1ac835a 100644 --- a/include/trifinger_cameras/pylon_driver.hpp +++ b/include/trifinger_cameras/pylon_driver.hpp @@ -31,7 +31,8 @@ namespace trifinger_cameras * @brief Connect to Pylon camera. * * @param device_user_id The user-defined name of the camera. Can be set with - * the executable `pylon_write_device_user_id_to_camera`. + * the executable `pylon_write_device_user_id_to_camera`. Pass empty string to simply + * connect to the first camera found. * @param camera Pointer to the Pylon::CInstantCamera instance to which the * camera will be attached. */ @@ -76,7 +77,7 @@ class PylonDriver : public robot_interfaces::SensorDriver private: std::shared_ptr settings_; - const std::string device_user_id_; + std::string device_user_id_; const bool downsample_images_; Pylon::PylonAutoInitTerm auto_init_term_; Pylon::CInstantCamera camera_; diff --git a/src/pylon_driver.cpp b/src/pylon_driver.cpp index bc2f9d2..f236d39 100644 --- a/src/pylon_driver.cpp +++ b/src/pylon_driver.cpp @@ -116,12 +116,14 @@ PylonDriver::PylonDriver(const std::string& device_user_id, bool downsample_images, Settings settings) : settings_(settings.get_pylon_driver_settings()), - device_user_id_(device_user_id), downsample_images_(downsample_images) { try { pylon_connect(device_user_id, &camera_); + // get device user id from camera (useful in case an empty id was passed, in + // which case a random camera is connected) + device_user_id_ = camera_.GetDeviceInfo().GetUserDefinedName(); set_camera_configuration(); camera_.StartGrabbing(Pylon::GrabStrategy_LatestImageOnly); format_converter_.OutputPixelFormat = Pylon::PixelType_BGR8packed; From 9b3a6da2c553f59fe3acfde7e882740fae102439 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 11 Jun 2024 15:46:19 +0200 Subject: [PATCH 21/28] fix: Add cli_utils to package.xml --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 13d9bef..a5df46d 100644 --- a/package.xml +++ b/package.xml @@ -24,6 +24,7 @@ serialization_utils yaml_utils sensor_msgs + cli_utils tf From 570001e545ede603035d2b1085f5a5319354684d Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 11 Jun 2024 16:46:42 +0200 Subject: [PATCH 22/28] fix: Properly export fmt dependency --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index db933a2..7f942e0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,6 +105,7 @@ if (${Pylon_FOUND}) target_link_libraries(pylon_driver ${OpenCV_LIBRARIES} ${Pylon_LIBRARIES} + fmt::fmt robot_interfaces::robot_interfaces camera_observations settings @@ -221,6 +222,7 @@ list(APPEND install_targets load_camera_config_test) add_pybind11_module(py_camera_types srcpy/py_camera_types.cpp LINK_LIBRARIES pybind11_opencv::pybind11_opencv + settings camera_observations opencv_driver ${pylon_driver} @@ -323,5 +325,5 @@ ament_export_libraries( pybullet_tricamera_driver camera_calibration_parser ) -ament_export_dependencies(yaml_utils sensor_msgs tomlplusplus) +ament_export_dependencies(yaml_utils sensor_msgs tomlplusplus fmt) ament_package(CONFIG_EXTRAS cmake/cfg_extras.cmake.in) From ceb2f23241a1455353301f409a418cd6a64c682a Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 11 Jun 2024 16:48:50 +0200 Subject: [PATCH 23/28] build: Remove ament_export_libraries and _include_directories According to the [documentation](https://docs.ros.org/en/humble/How-To-Guides/Ament-CMake-Documentation.html#installing), those macros are not needed for target-based installs and should thus normally not be used anymore. --- CMakeLists.txt | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f942e0..2218d16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,8 +42,6 @@ endif() ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR python/${PROJECT_NAME}) -ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) - set(install_targets "") @@ -316,14 +314,6 @@ endif() -ament_export_include_directories(include ${Pylon_INCLUDE_DIRS}) -ament_export_libraries( - camera_observations - opencv_driver - ${pylon_driver} - ${tricamera_driver} - pybullet_tricamera_driver - camera_calibration_parser -) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(yaml_utils sensor_msgs tomlplusplus fmt) ament_package(CONFIG_EXTRAS cmake/cfg_extras.cmake.in) From 9cc988b7c10c72edd3c06581611e30b09e21f3c6 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 24 Jun 2024 09:46:15 +0200 Subject: [PATCH 24/28] fix linter warnings in demo_tricamera.py --- demos/demo_tricamera.py | 18 +++++++----------- pyproject.toml | 9 +++++++++ 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/demos/demo_tricamera.py b/demos/demo_tricamera.py index 53e0c80..c6e5c41 100644 --- a/demos/demo_tricamera.py +++ b/demos/demo_tricamera.py @@ -4,6 +4,7 @@ and if desired, store the timestamps from them to analyse how well the three cameras are in sync with each other. """ + import argparse import cv2 import pickle @@ -12,7 +13,7 @@ from trifinger_cameras import utils -def main(): +def main() -> None: # noqa: D103 argparser = argparse.ArgumentParser(description=__doc__) argparser.add_argument( "--store-timestamps", @@ -34,15 +35,11 @@ def main(): camera_names = ["camera60", "camera180", "camera300"] if args.multi_process: - camera_data = trifinger_cameras.tricamera.MultiProcessData( - "tricamera", False - ) + camera_data = trifinger_cameras.tricamera.MultiProcessData("tricamera", False) else: camera_data = trifinger_cameras.tricamera.SingleProcessData() - camera_driver = trifinger_cameras.tricamera.TriCameraDriver( - *camera_names - ) - camera_backend = trifinger_cameras.tricamera.Backend( # noqa + camera_driver = trifinger_cameras.tricamera.TriCameraDriver(*camera_names) + camera_backend = trifinger_cameras.tricamera.Backend( # noqa: F841 camera_driver, camera_data ) @@ -67,9 +64,8 @@ def main(): ) if args.store_timestamps: - pickle.dump( - observations_timestamps_list, open(args.store_timestamps, "wb") - ) + with open(args.store_timestamps, "wb") as f: + pickle.dump(observations_timestamps_list, f) if __name__ == "__main__": diff --git a/pyproject.toml b/pyproject.toml index dbc8fa8..d301a90 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,3 +3,12 @@ line-length = 79 [tool.pylint.messages_control] disable = "C0330, C0326" + + +[[tool.mypy.overrides]] +# list all modules for which no type hints are available +module = [ + "cv2", + "trifinger_cameras.*", +] +ignore_missing_imports = true From e6439650e067dfc72bd3b1a529f74c013ebe3b38 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 09:56:13 +0200 Subject: [PATCH 25/28] Add script tricamera_monitor_rate.py Add a script that fetches TriCamera observations to analyse the frame rate. This is mostly meant for developers (for testing/debugging) but might also be useful for users to verify their settings are loaded correctly. --- CHANGELOG.md | 2 + CMakeLists.txt | 1 + doc/executables.rst | 1 + scripts/tricamera_monitor_rate.py | 91 +++++++++++++++++++++++++++++++ 4 files changed, 95 insertions(+) create mode 100644 scripts/tricamera_monitor_rate.py diff --git a/CHANGELOG.md b/CHANGELOG.md index d92a4d9..05d1a4c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,8 @@ - Make some settings (e.g. frame rate) configurable through a TOML config file. For more information see [documentation](https://open-dynamic-robot-initiative.github.io/trifinger_cameras/doc/configuration.html) +- Script `tricamera_monitor_rate.py` for checking the actual frame rate (most meant for + debugging and testing during development) ### Removed - Obsolete script `verify_calibration.py` diff --git a/CMakeLists.txt b/CMakeLists.txt index 2218d16..a668bda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -265,6 +265,7 @@ install_scripts( scripts/record_image_dataset.py scripts/tricamera_log_converter.py scripts/tricamera_log_viewer.py + scripts/tricamera_monitor_rate.py scripts/tricamera_test_connection.py DESTINATION lib/${PROJECT_NAME} diff --git a/doc/executables.rst b/doc/executables.rst index 222542e..7a1ffc4 100644 --- a/doc/executables.rst +++ b/doc/executables.rst @@ -35,6 +35,7 @@ analysis, etc. - record_image_dataset - tricamera_log_converter - tricamera_log_viewer + - tricamera_monitor_rate - tricamera_test_connection diff --git a/scripts/tricamera_monitor_rate.py b/scripts/tricamera_monitor_rate.py new file mode 100644 index 0000000..fb858ca --- /dev/null +++ b/scripts/tricamera_monitor_rate.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +"""Fetch tricamera observations in a loop and print the rate. + +This is meant for easy testing of the actual frame rate. +""" + +import argparse +import itertools +import json +import logging + +import tqdm + +import trifinger_cameras + + +def main() -> None: + argparser = argparse.ArgumentParser(description=__doc__) + argparser.add_argument( + "--store-timestamps", + type=str, + metavar="", + help="""Store the timestamps from the observations of the three cameras + into the specified json file. + """, + ) + argparser.add_argument( + "--multi-process", + action="store_true", + help="""If set, use multiprocess camera data to access backend in other + process. Otherwise run the backend locally. + """, + ) + parser.add_argument( + "--verbose", "-v", action="store_true", help="Enable verbose output." + ) + args = argparser.parse_args() + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="[%(asctime)s] [%(name)s | %(levelname)s] %(message)s", + ) + + camera_names = ["camera60", "camera180", "camera300"] + + if args.multi_process: + logging.debug("Start multi-process data") + camera_data = trifinger_cameras.tricamera.MultiProcessData("tricamera", False) + else: + logging.debug("Start single-process data") + camera_data = trifinger_cameras.tricamera.SingleProcessData() + logging.debug("Start back end") + camera_driver = trifinger_cameras.tricamera.TriCameraDriver(*camera_names) + camera_backend = trifinger_cameras.tricamera.Backend( # noqa: F841 + camera_driver, camera_data + ) + + logging.debug("Start front end") + camera_frontend = trifinger_cameras.tricamera.Frontend(camera_data) + observations_timestamps_list = [] + + # use tqdm to display the frame rate + rate_monitor = tqdm.tqdm() + logging.debug("Get start") + t_start = camera_frontend.get_current_timeindex() + logging.debug(f"Start at t = {t_start}") + try: + for t in itertools.count(start=t_start): + rate_monitor.update() + observation = camera_frontend.get_observation(t) + + observations_timestamps_list.append( + [ + observation.cameras[0].timestamp, + observation.cameras[1].timestamp, + observation.cameras[2].timestamp, + ] + ) + + except Exception as e: + logging.error(e) + + rate_monitor.close() + + if args.store_timestamps: + with open(args.store_timestamps, "w") as f: + json.dump(observations_timestamps_list, f) + + +if __name__ == "__main__": + main() From 3927d861fa15f746b67f9031872c4c489c06a472 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 10:12:25 +0200 Subject: [PATCH 26/28] doc: Update CHANGELOG --- CHANGELOG.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 05d1a4c..c3bfa15 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,10 +9,11 @@ significantly out of focus. - Executable `pylon_write_device_user_id_to_camera` to set the "DeviceUserID" of Pylon cameras. +- Executable `pylon_dump_camera_settings` to save settings of a Pylon camera. - Make some settings (e.g. frame rate) configurable through a TOML config file. For more information see [documentation](https://open-dynamic-robot-initiative.github.io/trifinger_cameras/doc/configuration.html) -- Script `tricamera_monitor_rate.py` for checking the actual frame rate (most meant for +- Executable `tricamera_monitor_rate` for checking the actual frame rate (most meant for debugging and testing during development) ### Removed @@ -20,6 +21,11 @@ ### Fixed - pybind11 build error on Ubuntu 22.04 +- Connecting to camera without specifying DeviceUserID was not working. It now opens + the first camera in the list of connected cameras if no ID is specified. + +### Changed +- `pylon_list_cameras`: Keep stdout clean if there are no cameras. ## [1.0.0] - 2022-06-28 From 37e0dee50302bc2776d24c1f8bf407786a757b02 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 10:20:38 +0200 Subject: [PATCH 27/28] Reformat C++ --- include/trifinger_cameras/pybullet_tricamera_driver.hpp | 2 +- include/trifinger_cameras/pylon_driver.hpp | 4 ++-- src/pybullet_tricamera_driver.cpp | 4 ++-- src/pylon_driver.cpp | 4 ++-- src/pylon_dump_camera_settings.cpp | 4 ++-- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/trifinger_cameras/pybullet_tricamera_driver.hpp b/include/trifinger_cameras/pybullet_tricamera_driver.hpp index 8a502b6..e33580b 100644 --- a/include/trifinger_cameras/pybullet_tricamera_driver.hpp +++ b/include/trifinger_cameras/pybullet_tricamera_driver.hpp @@ -10,8 +10,8 @@ #include #include -#include #include +#include namespace trifinger_cameras { diff --git a/include/trifinger_cameras/pylon_driver.hpp b/include/trifinger_cameras/pylon_driver.hpp index 1ac835a..8c63ae0 100644 --- a/include/trifinger_cameras/pylon_driver.hpp +++ b/include/trifinger_cameras/pylon_driver.hpp @@ -31,8 +31,8 @@ namespace trifinger_cameras * @brief Connect to Pylon camera. * * @param device_user_id The user-defined name of the camera. Can be set with - * the executable `pylon_write_device_user_id_to_camera`. Pass empty string to simply - * connect to the first camera found. + * the executable `pylon_write_device_user_id_to_camera`. Pass empty string to + * simply connect to the first camera found. * @param camera Pointer to the Pylon::CInstantCamera instance to which the * camera will be attached. */ diff --git a/src/pybullet_tricamera_driver.cpp b/src/pybullet_tricamera_driver.cpp index 432aace..b954503 100644 --- a/src/pybullet_tricamera_driver.cpp +++ b/src/pybullet_tricamera_driver.cpp @@ -73,8 +73,8 @@ PyBulletTriCameraDriver::get_observation() last_update_robot_time_index_ + frame_rate_in_robot_steps_) { using namespace std::chrono_literals; - // TODO the sleep here might be problematic if very high frame rate is - // required + // TODO the sleep here might be problematic if very high frame rate + // is required std::this_thread::sleep_for(10ms); auto new_robot_t = robot_data_->observation->newest_timeindex(false); diff --git a/src/pylon_driver.cpp b/src/pylon_driver.cpp index f236d39..1e7d7d9 100644 --- a/src/pylon_driver.cpp +++ b/src/pylon_driver.cpp @@ -121,8 +121,8 @@ PylonDriver::PylonDriver(const std::string& device_user_id, try { pylon_connect(device_user_id, &camera_); - // get device user id from camera (useful in case an empty id was passed, in - // which case a random camera is connected) + // get device user id from camera (useful in case an empty id was + // passed, in which case a random camera is connected) device_user_id_ = camera_.GetDeviceInfo().GetUserDefinedName(); set_camera_configuration(); camera_.StartGrabbing(Pylon::GrabStrategy_LatestImageOnly); diff --git a/src/pylon_dump_camera_settings.cpp b/src/pylon_dump_camera_settings.cpp index c0b84df..5a3c941 100644 --- a/src/pylon_dump_camera_settings.cpp +++ b/src/pylon_dump_camera_settings.cpp @@ -58,8 +58,8 @@ int main(int argc, char *argv[]) return 1; } - // need braces so that Pylon::CInstantCamera instance is destructed before the end - // (otherwise segfaults) + // need braces so that Pylon::CInstantCamera instance is destructed before + // the end (otherwise segfaults) { Pylon::CInstantCamera camera; try From 74b45188ecc816d1a57d5b11b92ebe155a94437b Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 5 Aug 2024 10:23:45 +0200 Subject: [PATCH 28/28] Reformat Python --- demos/demo_camera.py | 4 +++- demos/demo_tricamera.py | 8 ++++++-- python/trifinger_cameras/charuco_board_handler.py | 1 + python/trifinger_cameras/utils.py | 1 + scripts/check_camera_sharpness.py | 15 +++++++++++---- scripts/tricamera_monitor_rate.py | 8 ++++++-- 6 files changed, 28 insertions(+), 9 deletions(-) diff --git a/demos/demo_camera.py b/demos/demo_camera.py index 65e4427..884aef7 100644 --- a/demos/demo_camera.py +++ b/demos/demo_camera.py @@ -43,7 +43,9 @@ def main() -> int: try: if args.pylon: - camera_driver = trifinger_cameras.camera.PylonDriver(args.camera_id) + camera_driver = trifinger_cameras.camera.PylonDriver( + args.camera_id + ) else: camera_id = int(args.camera_id) if args.camera_id else 0 camera_driver = trifinger_cameras.camera.OpenCVDriver(camera_id) diff --git a/demos/demo_tricamera.py b/demos/demo_tricamera.py index c6e5c41..148cf5c 100644 --- a/demos/demo_tricamera.py +++ b/demos/demo_tricamera.py @@ -35,10 +35,14 @@ def main() -> None: # noqa: D103 camera_names = ["camera60", "camera180", "camera300"] if args.multi_process: - camera_data = trifinger_cameras.tricamera.MultiProcessData("tricamera", False) + camera_data = trifinger_cameras.tricamera.MultiProcessData( + "tricamera", False + ) else: camera_data = trifinger_cameras.tricamera.SingleProcessData() - camera_driver = trifinger_cameras.tricamera.TriCameraDriver(*camera_names) + camera_driver = trifinger_cameras.tricamera.TriCameraDriver( + *camera_names + ) camera_backend = trifinger_cameras.tricamera.Backend( # noqa: F841 camera_driver, camera_data ) diff --git a/python/trifinger_cameras/charuco_board_handler.py b/python/trifinger_cameras/charuco_board_handler.py index 002fef8..4baf47f 100644 --- a/python/trifinger_cameras/charuco_board_handler.py +++ b/python/trifinger_cameras/charuco_board_handler.py @@ -1,4 +1,5 @@ """Class for Charuco board detection and camera calibration.""" + import glob import json import os diff --git a/python/trifinger_cameras/utils.py b/python/trifinger_cameras/utils.py index c9755db..cb036a8 100644 --- a/python/trifinger_cameras/utils.py +++ b/python/trifinger_cameras/utils.py @@ -1,4 +1,5 @@ """Utility functions.""" + import typing import cv2 diff --git a/scripts/check_camera_sharpness.py b/scripts/check_camera_sharpness.py index 98056e2..f196479 100644 --- a/scripts/check_camera_sharpness.py +++ b/scripts/check_camera_sharpness.py @@ -61,7 +61,9 @@ def add_label( return image -def auto_canny_params(image: np.ndarray, sigma: float = 0.33) -> tuple[float, float]: +def auto_canny_params( + image: np.ndarray, sigma: float = 0.33 +) -> tuple[float, float]: median = np.median(image) lower = max(0, (1.0 - sigma) * median) upper = min(255, (1.0 + sigma) * median) @@ -150,7 +152,9 @@ def main() -> None: max(200, params.canny_threshold1), params.set_canny_threshold1, ) - cv2.setTrackbarPos("Canny threshold 1", window_name, int(params.canny_threshold1)) + cv2.setTrackbarPos( + "Canny threshold 1", window_name, int(params.canny_threshold1) + ) cv2.createTrackbar( "Canny threshold 2", window_name, @@ -158,7 +162,9 @@ def main() -> None: max(400, params.canny_threshold2), params.set_canny_threshold2, ) - cv2.setTrackbarPos("Canny threshold 2", window_name, int(params.canny_threshold2)) + cv2.setTrackbarPos( + "Canny threshold 2", window_name, int(params.canny_threshold2) + ) cv2.createTrackbar( "Edge mean threshold", window_name, @@ -203,7 +209,8 @@ def main() -> None: add_label(image, args.camera_id, "top") add_label( image, - "Canny mean: %.1f | smoothed: %.1f" % (edges_mean, mean_buffer_mean), + "Canny mean: %.1f | smoothed: %.1f" + % (edges_mean, mean_buffer_mean), "bottom", ) diff --git a/scripts/tricamera_monitor_rate.py b/scripts/tricamera_monitor_rate.py index fb858ca..1518d8c 100644 --- a/scripts/tricamera_monitor_rate.py +++ b/scripts/tricamera_monitor_rate.py @@ -45,12 +45,16 @@ def main() -> None: if args.multi_process: logging.debug("Start multi-process data") - camera_data = trifinger_cameras.tricamera.MultiProcessData("tricamera", False) + camera_data = trifinger_cameras.tricamera.MultiProcessData( + "tricamera", False + ) else: logging.debug("Start single-process data") camera_data = trifinger_cameras.tricamera.SingleProcessData() logging.debug("Start back end") - camera_driver = trifinger_cameras.tricamera.TriCameraDriver(*camera_names) + camera_driver = trifinger_cameras.tricamera.TriCameraDriver( + *camera_names + ) camera_backend = trifinger_cameras.tricamera.Backend( # noqa: F841 camera_driver, camera_data )