From 72d1b647eadde92084a5e5838fbd003262b449d1 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Wed, 26 Jun 2024 11:24:12 +0200 Subject: [PATCH 01/33] Fix MSVC compiler warning in test_zivid_camera --- zivid_camera/test/test_zivid_camera.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/zivid_camera/test/test_zivid_camera.cpp b/zivid_camera/test/test_zivid_camera.cpp index b49fb4f..9b4563d 100644 --- a/zivid_camera/test/test_zivid_camera.cpp +++ b/zivid_camera/test/test_zivid_camera.cpp @@ -434,12 +434,13 @@ class ZividNodeTest : public ZividNodeTestBase } }; - template - void assertArrayFloatEq(const A & actual, const B & expected) const + template + void assertArrayDoubleEq( + const std::array & actual, + const std::array & expected) const { - ASSERT_EQ(actual.size(), expected.size()); - for (std::size_t i = 0; i < actual.size(); i++) { - ASSERT_FLOAT_EQ(actual[i], expected[i]); + for (std::size_t i = 0; i < N; i++) { + EXPECT_DOUBLE_EQ(actual[i], expected[i]); } } @@ -504,18 +505,21 @@ class ZividNodeTest : public ZividNodeTestBase // [fx 0 cx] // K = [ 0 fy cy] // [ 0 0 1] - assertArrayFloatEq( - ci.k, std::array{1781.448, 0, 990.49268, 0, 1781.5297, 585.81781, 0, 0, 1}); + assertArrayDoubleEq( + ci.k, + std::array{1781.447998046875, 0, 990.49267578125, 0, 1781.5296630859375, + 585.81781005859375, 0, 0, 1}); // R = I - assertArrayFloatEq(ci.r, std::array{1, 0, 0, 0, 1, 0, 0, 0, 1}); + assertArrayDoubleEq(ci.r, std::array{1, 0, 0, 0, 1, 0, 0, 0, 1}); // [fx' 0 cx' Tx] // P = [ 0 fy' cy' Ty] // [ 0 0 1 0] - assertArrayFloatEq( + assertArrayDoubleEq( ci.p, - std::array{1781.448, 0, 990.49268, 0, 0, 1781.5297, 585.81781, 0, 0, 0, 1, 0}); + std::array{1781.447998046875, 0, 990.49267578125, 0, 0, 1781.5296630859375, + 585.81781005859375, 0, 0, 0, 1, 0}); } }; From 67da85a0cb4631a2e167de33e38b4d2ed46e08ea Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Tue, 25 Jun 2024 16:27:41 +0200 Subject: [PATCH 02/33] Add Capture 2D sample --- README.md | 18 +++- zivid_samples/CMakeLists.txt | 4 + zivid_samples/src/sample_capture_2d.cpp | 115 ++++++++++++++++++++++++ 3 files changed, 136 insertions(+), 1 deletion(-) create mode 100644 zivid_samples/src/sample_capture_2d.cpp diff --git a/README.md b/README.md index c020c4e..e4a0b6a 100644 --- a/README.md +++ b/README.md @@ -348,7 +348,7 @@ _More samples will soon be provided_. ### Sample Capture This sample performs single-acquisition 3D captures in a loop. This sample shows how to [configure](#configuration) -the capture settings, how to subscribe to the [points/xyzrgba](#points/xyzrgba) topic, and how to invoke the +the capture settings, how to subscribe to the [points/xyzrgba](#pointsxyzrgba) topic, and how to invoke the [capture](#capture) service. Source code: [C++](./zivid_samples/src/sample_capture.cpp) @@ -361,6 +361,22 @@ Using ros2 run (when `zivid_camera` node is already running): ros2 run zivid_samples sample_capture_cpp ``` +### Sample Capture 2D + +This sample performs single-acquisition 2D captures in a loop. This sample shows how to [configure](#configuration) the +capture 2d settings with a YAML string, how to subscribe to the [color/image_color](#colorimage_color) topic, and how to +invoke the [capture_2d](#capture_2d) service. + +Source code: [C++](./zivid_samples/src/sample_capture_2d.cpp) + +```bash +ros2 launch zivid_samples sample.launch sample:=sample_capture_2d_cpp +``` +Using ros2 run (when `zivid_camera` node is already running): +```bash +ros2 run zivid_samples sample_capture_2d_cpp +``` + _More samples will soon be provided_. ## Frequently Asked Questions diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index 1e86aea..ca342cb 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -22,6 +22,10 @@ add_executable(sample_capture_cpp src/sample_capture.cpp) ament_target_dependencies(sample_capture_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) install(TARGETS sample_capture_cpp DESTINATION lib/${PROJECT_NAME}) +add_executable(sample_capture_2d_cpp src/sample_capture_2d.cpp) +ament_target_dependencies(sample_capture_2d_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) +install(TARGETS sample_capture_2d_cpp DESTINATION lib/${PROJECT_NAME}) + if(BUILD_TESTING) # TODO enable linting of the samples #find_package(ament_lint_auto REQUIRED) diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp new file mode 100644 index 0000000..870d804 --- /dev/null +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -0,0 +1,115 @@ +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include + +/* + * This sample shows how to set the settings_2d_yaml parameter of the zivid + * node, subscribe for the color/image_color topic, and invoke the capture_2d service. + * When an image is received, a new capture is triggered. + */ + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_2d"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_2d node"); + + auto client = node->create_client("capture_2d"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + return EXIT_FAILURE; + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture_2d service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "Service is available"); + + auto param_client = std::make_shared(node, "zivid_camera"); + if (!param_client->service_is_ready()) { + RCLCPP_ERROR(node->get_logger(), "Param client is not ready"); + return EXIT_FAILURE; + } + + const std::string settings_2d_yaml = + R"( +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + Aperture: 2.83 + Brightness: 1.0 + ExposureTime: 10000 + Gain: 2.5 +)"; + + RCLCPP_INFO_STREAM( + node->get_logger(), "Settings settings_2d_yaml to '" << settings_2d_yaml << "'"); + + param_client->set_parameters( + {rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}, [&node](auto future) { + auto results = future.get(); + if (results.size() != 1) { + RCLCPP_ERROR_STREAM(node->get_logger(), "Expected 1 result, got " << results.size()); + } else { + if (results[0].successful) { + RCLCPP_INFO(node->get_logger(), "Successfully set settings_2d_yaml"); + } else { + RCLCPP_ERROR(node->get_logger(), "Failed to set settings_2d_yaml"); + } + } + }); + + auto trigger_capture = [&]() { + RCLCPP_INFO(node->get_logger(), "Triggering 2d capture"); + client->async_send_request(std::make_shared()); + }; + + auto color_image_color_subscription = node->create_subscription( + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received image of width %d and height %d", msg->width, msg->height); + RCLCPP_INFO(node->get_logger(), "Re-trigger 2d capture"); + trigger_capture(); + }); + + trigger_capture(); + + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + + rclcpp::shutdown(); + + return EXIT_SUCCESS; +} From f18292a3a9d86cd9633f55bbd6b2596f25e6df04 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Tue, 25 Jun 2024 18:41:55 +0200 Subject: [PATCH 03/33] Add capture assistant sample --- README.md | 16 +++ zivid_samples/CMakeLists.txt | 4 + .../src/sample_capture_assistant.cpp | 127 ++++++++++++++++++ 3 files changed, 147 insertions(+) create mode 100644 zivid_samples/src/sample_capture_assistant.cpp diff --git a/README.md b/README.md index e4a0b6a..e978296 100644 --- a/README.md +++ b/README.md @@ -377,6 +377,22 @@ Using ros2 run (when `zivid_camera` node is already running): ros2 run zivid_samples sample_capture_2d_cpp ``` +### Suggest Settings with Capture Assistant + +This sample shows how to invoke the [capture_assistant/suggest_settings](#capture_assistantsuggest_settings) service to +suggest and set capture settings. Then, it shows how to subscribe to the [points/xyzrgba](#pointsxyzrgba) and +[color/image_color](#colorimage_color) topics, and finally invoke the [capture](#capture) service. + +Source code: [C++](./zivid_samples/src/sample_capture_assistant.cpp) + +```bash +ros2 launch zivid_samples sample.launch sample:=sample_capture_assistant +``` +Using ros2 run (when `zivid_camera` node is already running): +```bash +ros2 run zivid_samples sample_capture_assistant +``` + _More samples will soon be provided_. ## Frequently Asked Questions diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index ca342cb..0ce1f5d 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -26,6 +26,10 @@ add_executable(sample_capture_2d_cpp src/sample_capture_2d.cpp) ament_target_dependencies(sample_capture_2d_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) install(TARGETS sample_capture_2d_cpp DESTINATION lib/${PROJECT_NAME}) +add_executable(sample_capture_assistant_cpp src/sample_capture_assistant.cpp) +ament_target_dependencies(sample_capture_assistant_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) +install(TARGETS sample_capture_assistant_cpp DESTINATION lib/${PROJECT_NAME}) + if(BUILD_TESTING) # TODO enable linting of the samples #find_package(ament_lint_auto REQUIRED) diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp new file mode 100644 index 0000000..6d97902 --- /dev/null +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -0,0 +1,127 @@ +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include +#include + +#include + +/* + * This sample shows how to use the capture assistant service to suggest and set the capture + * settings parameter of the zivid node. Then, it shows how to subscribe to the points/xyzrgba and + * color/image_color topics, and finally invoke the capture service. + */ + +void capture_assistant_suggest_settings(const std::shared_ptr & node) +{ + using zivid_interfaces::srv::CaptureAssistantSuggestSettings; + + auto client = + node->create_client("capture_assistant/suggest_settings"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture assistant service to appear..."); + } + + auto request = std::make_shared(); + request->max_capture_time = rclcpp::Duration::from_seconds(5.0); + request->ambient_light_frequency = + CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; + + auto result = client->async_send_request(request); + + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to call capture assistant service"); + std::terminate(); + } +} + +void capture(const std::shared_ptr & node) +{ + auto capture_client = node->create_client("capture"); + while (!capture_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + node->get_logger(), + "Client interrupted while waiting for service to appear."); + std::terminate(); + } + RCLCPP_INFO( + node->get_logger(), + "Waiting for the capture service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + auto result = + capture_client->async_send_request(std::make_shared()); + + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to trigger capture"); + std::terminate(); + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_assistant"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_assistant node"); + + capture_assistant_suggest_settings(node); + + auto points_xyzrgba_subscription = + node->create_subscription( + "points/xyzrgba", 10, + [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of width %d and height %d", msg->width, + msg->height); + }); + + auto color_image_color_subscription = node->create_subscription( + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received image of width %d and height %d", msg->width, msg->height); + }); + + capture(node); + + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + + rclcpp::shutdown(); + + return EXIT_SUCCESS; +} From 575be01a86f5bc8f1adc76e28e93733dc18961df Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Wed, 26 Jun 2024 17:13:21 +0200 Subject: [PATCH 04/33] Add sample capture and save --- README.md | 20 ++- zivid_samples/CMakeLists.txt | 4 + zivid_samples/src/sample_capture_and_save.cpp | 144 ++++++++++++++++++ 3 files changed, 167 insertions(+), 1 deletion(-) create mode 100644 zivid_samples/src/sample_capture_and_save.cpp diff --git a/README.md b/README.md index e978296..58974f2 100644 --- a/README.md +++ b/README.md @@ -377,7 +377,25 @@ Using ros2 run (when `zivid_camera` node is already running): ros2 run zivid_samples sample_capture_2d_cpp ``` -### Suggest Settings with Capture Assistant +### Sample Capture and Save + +This sample performs a capture, and stores the resulting frame to file. This sample shows how to +[configure](#configuration) the capture settings with a YAML string, how to invoke the +[capture_and_save](#capture_and_save) service, and how to read the response from the service call. + +Source code: [C++](./zivid_samples/src/sample_capture_and_save.cpp) + +```bash +ros2 launch zivid_samples sample.launch sample:=sample_capture_and_save_cpp +``` + +Using ros2 run (when `zivid_camera` node is already running): + +```bash +ros2 run zivid_samples sample_capture_and_save_cpp +``` + +### Sample Capture Assistant This sample shows how to invoke the [capture_assistant/suggest_settings](#capture_assistantsuggest_settings) service to suggest and set capture settings. Then, it shows how to subscribe to the [points/xyzrgba](#pointsxyzrgba) and diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index 0ce1f5d..f45796b 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -30,6 +30,10 @@ add_executable(sample_capture_assistant_cpp src/sample_capture_assistant.cpp) ament_target_dependencies(sample_capture_assistant_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) install(TARGETS sample_capture_assistant_cpp DESTINATION lib/${PROJECT_NAME}) +add_executable(sample_capture_and_save_cpp src/sample_capture_and_save.cpp) +ament_target_dependencies(sample_capture_and_save_cpp rclcpp zivid_interfaces) +install(TARGETS sample_capture_and_save_cpp DESTINATION lib/${PROJECT_NAME}) + if(BUILD_TESTING) # TODO enable linting of the samples #find_package(ament_lint_auto REQUIRED) diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp new file mode 100644 index 0000000..3070fc6 --- /dev/null +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -0,0 +1,144 @@ +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +#include + +/* + * This sample shows how to set the settings_yaml parameter of the zivid node, then invoke the + * capture_and_save service, and read the response from the service call. If successful, the + * captured frame is stored in a temporary directory. + */ + +void set_settings(const std::shared_ptr & node) +{ + const std::string settings_yml = + R"( +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Diagnostics: + Enabled: yes + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +)"; + + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + terminate(); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the param client to appear..."); + } + + auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}, + [&node](auto future) { + auto results = future.get(); + if (results.size() != 1) { + RCLCPP_ERROR_STREAM(node->get_logger(), "Expected 1 result, got " << results.size()); + } else { + if (results[0].successful) { + RCLCPP_INFO(node->get_logger(), "Successfully set settings_yaml"); + } else { + RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml"); + } + } + }); + + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml parameter"); + std::terminate(); + } +} + +void capture_and_save(const std::shared_ptr & node) +{ + using zivid_interfaces::srv::CaptureAndSave; + + auto client = node->create_client("capture_and_save"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture_and_save service to appear..."); + } + + const std::string filename = "capture_and_save_cpp.zdf"; + + auto request = std::make_shared(); + request->file_path = (std::filesystem::temp_directory_path() / filename).string(); + RCLCPP_INFO(node->get_logger(), "Sending capture and save request with file path: %s", + request->file_path.c_str()); + + auto result = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to call capture_and_save service"); + std::terminate(); + } + + auto capture_response = result.get(); + if (!capture_response->success) { + RCLCPP_ERROR(node->get_logger(), "Capture and save operation was unsuccessful: %s", + capture_response->message.c_str()); + std::terminate(); + } + + RCLCPP_INFO(node->get_logger(), "Capture and save operation successful"); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_and_save"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_and_save node"); + + set_settings(node); + + capture_and_save(node); + + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + + rclcpp::shutdown(); + + return EXIT_SUCCESS; +} From baf291c31f20d2919c71e311bd79483c399ba753 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 11 Jul 2024 13:07:10 +0200 Subject: [PATCH 05/33] Add `sample_capture.py` sample --- README.md | 4 +- zivid_samples/CMakeLists.txt | 2 + zivid_samples/launch/sample.launch | 4 +- zivid_samples/package.xml | 1 + zivid_samples/scripts/sample_capture.py | 86 +++++++++++++++++++++++++ 5 files changed, 94 insertions(+), 3 deletions(-) create mode 100644 zivid_samples/scripts/sample_capture.py diff --git a/README.md b/README.md index 58974f2..e59f3ea 100644 --- a/README.md +++ b/README.md @@ -351,14 +351,16 @@ This sample performs single-acquisition 3D captures in a loop. This sample shows the capture settings, how to subscribe to the [points/xyzrgba](#pointsxyzrgba) topic, and how to invoke the [capture](#capture) service. -Source code: [C++](./zivid_samples/src/sample_capture.cpp) +Source code: [C++](./zivid_samples/src/sample_capture.cpp) [Python](./zivid_samples/scripts/sample_capture.py) ```bash ros2 launch zivid_samples sample.launch sample:=sample_capture_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture.py ``` Using ros2 run (when `zivid_camera` node is already running): ```bash ros2 run zivid_samples sample_capture_cpp +ros2 run zivid_samples sample_capture.py ``` ### Sample Capture 2D diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index f45796b..fc19e6d 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -34,6 +34,8 @@ add_executable(sample_capture_and_save_cpp src/sample_capture_and_save.cpp) ament_target_dependencies(sample_capture_and_save_cpp rclcpp zivid_interfaces) install(TARGETS sample_capture_and_save_cpp DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/sample_capture.py DESTINATION lib/${PROJECT_NAME}) + if(BUILD_TESTING) # TODO enable linting of the samples #find_package(ament_lint_auto REQUIRED) diff --git a/zivid_samples/launch/sample.launch b/zivid_samples/launch/sample.launch index 8b24a35..6323a28 100644 --- a/zivid_samples/launch/sample.launch +++ b/zivid_samples/launch/sample.launch @@ -1,5 +1,5 @@ - - \ No newline at end of file + + diff --git a/zivid_samples/package.xml b/zivid_samples/package.xml index a8ceb5d..b6242cb 100644 --- a/zivid_samples/package.xml +++ b/zivid_samples/package.xml @@ -19,6 +19,7 @@ rosidl_default_generators zivid_interfaces ament_index_cpp + rclpy ament_cmake_gtest ament_lint_auto ament_lint_common diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py new file mode 100644 index 0000000..e91345f --- /dev/null +++ b/zivid_samples/scripts/sample_capture.py @@ -0,0 +1,86 @@ +import sys + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.parameter_client import AsyncParameterClient + +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import Trigger + + +class Sample(Node): + + def __init__(self): + super().__init__("sample_capture_py") + + self.capture_service = self.create_client(Trigger, "capture") + while not self.capture_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info("Capture service not available, waiting again...") + + self._set_capture_settings() + + self.subscription = self.create_subscription( + PointCloud2, "points/xyzrgba", self.on_points, 10 + ) + + def _set_capture_settings(self): + self.get_logger().info("Setting parameter 'settings_yaml'") + settings_parameter = Parameter( + "settings_yaml", + Parameter.Type.STRING, + """ +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Diagnostics: + Enabled: yes + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +""", + ).to_parameter_msg() + param_client = AsyncParameterClient(self, "zivid_camera") + param_client.wait_for_services() + future = param_client.set_parameters([settings_parameter]) + rclpy.spin_until_future_complete(self, future) + + def capture(self): + self.get_logger().info("Calling capture service") + return self.capture_service.call_async(Trigger.Request()) + + def on_points(self, msg): + self.get_logger().info( + f"Received point cloud of width {msg.width} and height {msg.height}" + ) + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info("Capture complete") + + rclpy.spin(sample) + + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == "__main__": + main() From 4d30cfdddd9ab491f13851da7009930c694cd1d9 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 12 Jul 2024 11:05:27 +0200 Subject: [PATCH 06/33] Make `sample_capture_with_settings_from_file` sample Change `sample_capture` to capture from yaml settings --- README.md | 20 ++- zivid_samples/CMakeLists.txt | 4 + zivid_samples/src/sample_capture.cpp | 87 ++++++++----- ...sample_capture_with_settings_from_file.cpp | 122 ++++++++++++++++++ 4 files changed, 201 insertions(+), 32 deletions(-) create mode 100644 zivid_samples/src/sample_capture_with_settings_from_file.cpp diff --git a/README.md b/README.md index e59f3ea..0d7d594 100644 --- a/README.md +++ b/README.md @@ -413,7 +413,25 @@ Using ros2 run (when `zivid_camera` node is already running): ros2 run zivid_samples sample_capture_assistant ``` -_More samples will soon be provided_. +### Sample Capture With Settings From File + +This sample performs single-acquisition 3D captures in a loop. This sample shows how to [configure](#configuration) the +capture settings from a yaml file, how to subscribe to the [points/xyzrgba](#pointsxyzrgba) topic, and how to invoke the +[capture](#capture) service. + +Source code: [C++](./zivid_samples/src/sample_capture_with_settings_from_file.cpp) [Python](./zivid_samples/scripts/sample_capture_with_settings_from_file.py) + +```bash +ros2 launch zivid_samples sample.launch sample:=sample_capture_with_settings_from_file_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_with_settings_from_file.py +``` + +Using ros2 run (when `zivid_camera` node is already running): + +```bash +ros2 run zivid_samples sample_capture_with_settings_from_file_cpp +ros2 run zivid_samples sample_capture_with_settings_from_file.py +``` ## Frequently Asked Questions diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index fc19e6d..d751bda 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -34,6 +34,10 @@ add_executable(sample_capture_and_save_cpp src/sample_capture_and_save.cpp) ament_target_dependencies(sample_capture_and_save_cpp rclcpp zivid_interfaces) install(TARGETS sample_capture_and_save_cpp DESTINATION lib/${PROJECT_NAME}) +add_executable(sample_capture_with_settings_from_file_cpp src/sample_capture_with_settings_from_file.cpp) +ament_target_dependencies(sample_capture_with_settings_from_file_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) +install(TARGETS sample_capture_with_settings_from_file_cpp DESTINATION lib/${PROJECT_NAME}) + install(PROGRAMS scripts/sample_capture.py DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 2c88910..05a01b6 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -39,6 +39,61 @@ * When a point cloud is received, a new capture is triggered. */ +void set_settings(const std::shared_ptr & node) +{ + const std::string settings_yml = + R"( +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +)"; + + auto param_client = + std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), + "Client interrupted while waiting for service to appear."); + terminate(); + } + RCLCPP_INFO(node->get_logger(), + "Waiting for the param client to appear..."); + } + + auto result = param_client->set_parameters( + {rclcpp::Parameter("settings_yaml", settings_yml)}, [&node](auto future) { + auto results = future.get(); + if (results.size() != 1) { + RCLCPP_ERROR_STREAM(node->get_logger(), + "Expected 1 result, got " << results.size()); + } else { + if (results[0].successful) { + RCLCPP_INFO(node->get_logger(), "Successfully set settings_yaml"); + } else { + RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml"); + } + } + }); + + if (rclcpp::spin_until_future_complete(node, result) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml parameter"); + std::terminate(); + } +} + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); @@ -60,37 +115,7 @@ int main(int argc, char * argv[]) RCLCPP_INFO(node->get_logger(), "Service is available"); - auto param_client = - std::make_shared(node, "zivid_camera"); - if (!param_client->service_is_ready()) { - RCLCPP_ERROR(node->get_logger(), "Param client is not ready"); - return EXIT_FAILURE; - } - const auto share_directory = - ament_index_cpp::get_package_share_directory("zivid_samples"); - const auto path_to_settings_yml = - share_directory + "/settings/camera_settings.yml"; - - RCLCPP_INFO_STREAM( - node->get_logger(), "Setting settings path to '" - << path_to_settings_yml << "'"); - - param_client->set_parameters( - {rclcpp::Parameter("settings_file_path", path_to_settings_yml)}, - [&node](auto future) { - auto results = future.get(); - if (results.size() != 1) { - RCLCPP_ERROR_STREAM( - node->get_logger(), - "Expected 1 result, got " << results.size()); - } else { - if (results[0].successful) { - RCLCPP_INFO(node->get_logger(), "Successfully set settings path"); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings path"); - } - } - }); + set_settings(node); auto trigger_capture = [&]() { RCLCPP_INFO(node->get_logger(), "Triggering capture"); diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp new file mode 100644 index 0000000..d7825a7 --- /dev/null +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 Zivid AS +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Zivid AS nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include +#include + +/* + * This sample shows how to set the settings_file_path parameter of the zivid + * node, subscribe for the points/xyzrgba topic, and invoke the capture service. + * When a point cloud is received, a new capture is triggered. + */ + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_with_settings_from_file"); + RCLCPP_INFO(node->get_logger(), "Started the sample node"); + + auto client = node->create_client("capture"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + node->get_logger(), + "Client interrupted while waiting for service to appear."); + return EXIT_FAILURE; + } + RCLCPP_INFO( + node->get_logger(), + "Waiting for the capture service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "Service is available"); + + auto param_client = + std::make_shared(node, "zivid_camera"); + if (!param_client->service_is_ready()) { + RCLCPP_ERROR(node->get_logger(), "Param client is not ready"); + return EXIT_FAILURE; + } + const auto share_directory = + ament_index_cpp::get_package_share_directory("zivid_samples"); + const auto path_to_settings_yml = + share_directory + "/settings/camera_settings.yml"; + + RCLCPP_INFO_STREAM( + node->get_logger(), "Setting settings path to '" + << path_to_settings_yml << "'"); + + param_client->set_parameters( + {rclcpp::Parameter("settings_file_path", path_to_settings_yml)}, + [&node](auto future) { + auto results = future.get(); + if (results.size() != 1) { + RCLCPP_ERROR_STREAM( + node->get_logger(), + "Expected 1 result, got " << results.size()); + } else { + if (results[0].successful) { + RCLCPP_INFO(node->get_logger(), "Successfully set settings path"); + } else { + RCLCPP_ERROR(node->get_logger(), "Failed to set settings path"); + } + } + }); + + auto trigger_capture = [&]() { + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + client->async_send_request(std::make_shared()); + }; + + auto points_xyzrgba_subscription = + node->create_subscription( + "points/xyzrgba", 10, + [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), + "Received point cloud of width %d and height %d", + msg->width, msg->height); + RCLCPP_INFO( + node->get_logger(), + "Re-trigger capture"); + trigger_capture(); + }); + + trigger_capture(); + + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + + rclcpp::shutdown(); + + return EXIT_SUCCESS; +} From 44e3e0330322c9b0999d33d7db4a3dbf301bf11a Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 12 Jul 2024 13:42:17 +0200 Subject: [PATCH 07/33] Log exceptions related to Settings from Zivid API --- zivid_camera/src/zivid_camera.cpp | 35 +++++++++++++++++++++++++------ 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 63ace57..c69dada 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -169,6 +169,18 @@ void runFunctionAndCatchExceptions( } } +template +auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger & logger) +{ + try { + return function(); + } catch (const Zivid::Exception & exception) { + const auto exception_message = Zivid::toString(exception); + RCLCPP_ERROR_STREAM(logger, exception_message); + throw; + } +} + template [[noreturn]] void logErrorToLoggerAndThrowRuntimeException( const Logger & logger, @@ -219,6 +231,11 @@ class CaptureSettingsController node_.get_logger(), "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + "' parameters are non-empty! Please set only one of these parameters."); + } else if (settings_file_path.empty() && settings_yaml.empty()) { + logErrorToLoggerAndThrowRuntimeException( + node_.get_logger(), + "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + + "' parameters are empty! Please set one of these parameters."); } else if (!settings_yaml.empty()) { RCLCPP_DEBUG_STREAM(node_.get_logger(), "Using settings from yml string"); return deserializeZividDataModel(settings_yaml); @@ -505,7 +522,8 @@ void ZividCamera::captureServiceHandler( { RCLCPP_INFO_STREAM(get_logger(), __func__); - const auto settings = settings_controller_->currentSettings(); + const auto settings = runFunctionAndCatchExceptionsAndRethrow( + [&] {return settings_controller_->currentSettings();}, get_logger()); runFunctionAndCatchExceptions( [&]() {invokeCaptureAndPublishFrame(settings);}, response, get_logger(), "Capture"); @@ -517,7 +535,8 @@ void ZividCamera::captureAndSaveServiceHandler( std::shared_ptr response) { RCLCPP_INFO_STREAM(get_logger(), __func__); - const auto settings = settings_controller_->currentSettings(); + const auto settings = runFunctionAndCatchExceptionsAndRethrow( + [&] {return settings_controller_->currentSettings();}, get_logger()); runFunctionAndCatchExceptions( [&]() { @@ -537,7 +556,8 @@ void ZividCamera::capture2DServiceHandler( serviceHandlerHandleCameraConnectionLoss(); - const auto settings2D = settings_2d_controller_->currentSettings(); + const auto settings2D = runFunctionAndCatchExceptionsAndRethrow( + [&] {return settings_2d_controller_->currentSettings();}, get_logger()); runFunctionAndCatchExceptions( [&]() { @@ -584,9 +604,12 @@ void ZividCamera::captureAssistantSuggestSettingsServiceHandler( SuggestSettingsParameters suggest_settings_parameters{ SuggestSettingsParameters::MaxCaptureTime{max_capture_time}, ambient_light_frequency}; RCLCPP_INFO_STREAM( - get_logger(), "Getting suggested settings using parameters: " << suggest_settings_parameters); - const auto suggested_settings = - Zivid::CaptureAssistant::suggestSettings(*camera_, suggest_settings_parameters); + get_logger(), + "Getting suggested settings using parameters: " << suggest_settings_parameters); + const auto suggested_settings = runFunctionAndCatchExceptionsAndRethrow( + [&]() { + return Zivid::CaptureAssistant::suggestSettings(*camera_, suggest_settings_parameters); + }, get_logger()); RCLCPP_INFO_STREAM( get_logger(), "CaptureAssistant::suggestSettings returned " From cee20af712ce651d3fda25e9e61a004eb5bd589b Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 12 Jul 2024 14:55:11 +0200 Subject: [PATCH 08/33] Add remaining Python samples --- README.md | 18 ++-- zivid_samples/CMakeLists.txt | 4 + zivid_samples/scripts/sample_capture_2d.py | 78 ++++++++++++++++ .../scripts/sample_capture_and_save.py | 82 +++++++++++++++++ .../scripts/sample_capture_assistant.py | 88 +++++++++++++++++++ .../sample_capture_with_settings_from_file.py | 79 +++++++++++++++++ 6 files changed, 343 insertions(+), 6 deletions(-) create mode 100644 zivid_samples/scripts/sample_capture_2d.py create mode 100644 zivid_samples/scripts/sample_capture_and_save.py create mode 100644 zivid_samples/scripts/sample_capture_assistant.py create mode 100644 zivid_samples/scripts/sample_capture_with_settings_from_file.py diff --git a/README.md b/README.md index 0d7d594..8921f7b 100644 --- a/README.md +++ b/README.md @@ -369,14 +369,16 @@ This sample performs single-acquisition 2D captures in a loop. This sample shows capture 2d settings with a YAML string, how to subscribe to the [color/image_color](#colorimage_color) topic, and how to invoke the [capture_2d](#capture_2d) service. -Source code: [C++](./zivid_samples/src/sample_capture_2d.cpp) +Source code: [C++](./zivid_samples/src/sample_capture_2d.cpp) [Python](./zivid_samples/scripts/sample_capture_2d.py) ```bash ros2 launch zivid_samples sample.launch sample:=sample_capture_2d_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_2d.py ``` Using ros2 run (when `zivid_camera` node is already running): ```bash ros2 run zivid_samples sample_capture_2d_cpp +ros2 run zivid_samples sample_capture_2d.py ``` ### Sample Capture and Save @@ -385,16 +387,18 @@ This sample performs a capture, and stores the resulting frame to file. This sam [configure](#configuration) the capture settings with a YAML string, how to invoke the [capture_and_save](#capture_and_save) service, and how to read the response from the service call. -Source code: [C++](./zivid_samples/src/sample_capture_and_save.cpp) +Source code: [C++](./zivid_samples/src/sample_capture_and_save.cpp) [Python](./zivid_samples/scripts/sample_capture_and_save.py) ```bash ros2 launch zivid_samples sample.launch sample:=sample_capture_and_save_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_and_save.py ``` Using ros2 run (when `zivid_camera` node is already running): ```bash ros2 run zivid_samples sample_capture_and_save_cpp +ros2 run zivid_samples sample_capture_and_save.py ``` ### Sample Capture Assistant @@ -403,17 +407,19 @@ This sample shows how to invoke the [capture_assistant/suggest_settings](#captur suggest and set capture settings. Then, it shows how to subscribe to the [points/xyzrgba](#pointsxyzrgba) and [color/image_color](#colorimage_color) topics, and finally invoke the [capture](#capture) service. -Source code: [C++](./zivid_samples/src/sample_capture_assistant.cpp) +Source code: [C++](./zivid_samples/src/sample_capture_assistant.cpp) [Python](./zivid_samples/scripts/sample_capture_assistant.py) ```bash -ros2 launch zivid_samples sample.launch sample:=sample_capture_assistant +ros2 launch zivid_samples sample.launch sample:=sample_capture_assistant_cpp +ros2 launch zivid_samples sample.launch sample:=sample_capture_assistant.py ``` Using ros2 run (when `zivid_camera` node is already running): ```bash -ros2 run zivid_samples sample_capture_assistant +ros2 run zivid_samples sample_capture_assistant_cpp +ros2 run zivid_samples sample_capture_assistant.py ``` -### Sample Capture With Settings From File +### Sample Capture with Settings from File This sample performs single-acquisition 3D captures in a loop. This sample shows how to [configure](#configuration) the capture settings from a yaml file, how to subscribe to the [points/xyzrgba](#pointsxyzrgba) topic, and how to invoke the diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index d751bda..78c9d1c 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -39,6 +39,10 @@ ament_target_dependencies(sample_capture_with_settings_from_file_cpp rclcpp zivi install(TARGETS sample_capture_with_settings_from_file_cpp DESTINATION lib/${PROJECT_NAME}) install(PROGRAMS scripts/sample_capture.py DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/sample_capture_2d.py DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/sample_capture_and_save.py DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/sample_capture_assistant.py DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS scripts/sample_capture_with_settings_from_file.py DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) # TODO enable linting of the samples diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py new file mode 100644 index 0000000..6fce8a4 --- /dev/null +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -0,0 +1,78 @@ +import sys + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.parameter_client import AsyncParameterClient + +from sensor_msgs.msg import Image +from std_srvs.srv import Trigger + + +class Sample(Node): + + def __init__(self): + super().__init__("sample_capture_2d_py") + + self.capture_2d_service = self.create_client(Trigger, "capture_2d") + while not self.capture_2d_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info("Capture service not available, waiting again...") + + self._set_capture_settings() + + self.subscription = self.create_subscription( + Image, "color/image_color", self.on_image_color, 10 + ) + + def _set_capture_settings(self): + self.get_logger().info("Setting parameter 'settings_2d_yaml'") + settings_parameter = Parameter( + "settings_2d_yaml", + Parameter.Type.STRING, + """ +__version__: + serializer: 1 + data: 3 +Settings2D: + Acquisitions: + - Acquisition: + Aperture: 2.83 + Brightness: 1.0 + ExposureTime: 10000 + Gain: 2.5 +""", + ).to_parameter_msg() + param_client = AsyncParameterClient(self, "zivid_camera") + param_client.wait_for_services() + future = param_client.set_parameters([settings_parameter]) + rclpy.spin_until_future_complete(self, future) + + def capture(self): + self.get_logger().info("Calling capture service") + return self.capture_2d_service.call_async(Trigger.Request()) + + def on_image_color(self, msg): + self.get_logger().info(f"Received image of size {msg.width} x {msg.height}") + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info("Capture complete") + + rclpy.spin(sample) + + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py new file mode 100644 index 0000000..1e9bc12 --- /dev/null +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -0,0 +1,82 @@ +import sys +import tempfile + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.parameter_client import AsyncParameterClient + +from zivid_interfaces.srv import CaptureAndSave + + +class Sample(Node): + + def __init__(self): + super().__init__("sample_capture_and_save_py") + + self.capture_and_save_service = self.create_client( + CaptureAndSave, "capture_and_save" + ) + while not self.capture_and_save_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info("Capture service not available, waiting again...") + + self._set_capture_settings() + + def _set_capture_settings(self): + self.get_logger().info("Setting parameter 'settings_yaml'") + settings_parameter = Parameter( + "settings_yaml", + Parameter.Type.STRING, + """ +__version__: + serializer: 1 + data: 22 +Settings: + Acquisitions: + - Acquisition: + Aperture: 5.66 + ExposureTime: 8333 + Processing: + Filters: + Outlier: + Removal: + Enabled: yes + Threshold: 5 +""", + ).to_parameter_msg() + param_client = AsyncParameterClient(self, "zivid_camera") + param_client.wait_for_services() + future = param_client.set_parameters([settings_parameter]) + rclpy.spin_until_future_complete(self, future) + + def capture(self): + file_path = tempfile.gettempdir() + "/zivid_sample_capture_and_save.zdf" + self.get_logger().info(f"Calling capture service with file path: {file_path}") + request = CaptureAndSave.Request(file_path=file_path) + return self.capture_and_save_service.call_async(request) + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + future = sample.capture() + + rclpy.spin_until_future_complete(sample, future) + + response: CaptureAndSave.Response = future.result() + if not response.success: + sample.get_logger().error(f"Failed capture and save: {response.message}") + + sample.get_logger().info(f"Capture and save complete") + + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py new file mode 100644 index 0000000..25eac55 --- /dev/null +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -0,0 +1,88 @@ +import sys +import tempfile + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.parameter_client import AsyncParameterClient + +from builtin_interfaces.msg import Duration +from sensor_msgs.msg import Image +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import Trigger + +from zivid_interfaces.srv import CaptureAssistantSuggestSettings + + +class Sample(Node): + + def __init__(self): + super().__init__("sample_capture_and_save_py") + + self.capture_assistant_suggest_settings_service = self.create_client( + CaptureAssistantSuggestSettings, "capture_assistant/suggest_settings" + ) + while not self.capture_assistant_suggest_settings_service.wait_for_service( + timeout_sec=3.0 + ): + self.get_logger().info( + "Capture assistant service not available, waiting again..." + ) + + self.capture_service = self.create_client(Trigger, "capture") + while not self.capture_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info("Capture service not available, waiting again...") + + self.points_subscription = self.create_subscription( + PointCloud2, "points/xyzrgba", self.on_points, 10 + ) + self.image_subscription = self.create_subscription( + Image, "color/image_color", self.on_image_color, 10 + ) + + def capture_assistant_suggest_settings(self): + self.get_logger().info("Calling capture assistant service") + request = CaptureAssistantSuggestSettings.Request( + max_capture_time=Duration(sec=2), + ambient_light_frequency=CaptureAssistantSuggestSettings.Request.AMBIENT_LIGHT_FREQUENCY_NONE, + ) + return self.capture_assistant_suggest_settings_service.call_async(request) + + def capture(self): + self.get_logger().info("Calling capture service") + return self.capture_service.call_async(Trigger.Request()) + + def on_points(self, msg): + self.get_logger().info( + f"Received point cloud of size {msg.width} x {msg.height}" + ) + + def on_image_color(self, msg): + self.get_logger().info(f"Received image of size {msg.width} x {msg.height}") + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + future = sample.capture_assistant_suggest_settings() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info(f"Capture assistant complete") + + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info("Capture complete") + + rclpy.spin(sample) + + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py new file mode 100644 index 0000000..36c5850 --- /dev/null +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -0,0 +1,79 @@ +import sys + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.parameter_client import AsyncParameterClient + +from ament_index_python.packages import get_package_share_directory + +from sensor_msgs.msg import PointCloud2 +from std_srvs.srv import Trigger + + +class Sample(Node): + + def __init__(self): + super().__init__("sample_capture_with_settings_from_file_py") + + self.capture_service = self.create_client(Trigger, "capture") + while not self.capture_service.wait_for_service(timeout_sec=3.0): + self.get_logger().info("Capture service not available, waiting again...") + + self._set_capture_settings() + + self.subscription = self.create_subscription( + PointCloud2, "points/xyzrgba", self.on_points, 10 + ) + + def _set_capture_settings(self): + path_to_settings_yml = ( + get_package_share_directory("zivid_samples") + + "/settings/camera_settings.yml" + ) + self.get_logger().info( + "Setting parameter 'settings_file_path' to: " + path_to_settings_yml + ) + + settings_parameter = Parameter( + "settings_file_path", + Parameter.Type.STRING, + path_to_settings_yml, + ).to_parameter_msg() + + param_client = AsyncParameterClient(self, "zivid_camera") + param_client.wait_for_services() + future = param_client.set_parameters([settings_parameter]) + rclpy.spin_until_future_complete(self, future) + + def capture(self): + self.get_logger().info("Calling capture service") + return self.capture_service.call_async(Trigger.Request()) + + def on_points(self, msg): + self.get_logger().info( + f"Received point cloud of size {msg.width} x {msg.height}" + ) + + +def main(args=None): + rclpy.init(args=args) + + try: + sample = Sample() + + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info("Capture complete") + + rclpy.spin(sample) + + except KeyboardInterrupt: + pass + except ExternalShutdownException: + sys.exit(1) + + +if __name__ == "__main__": + main() From d3b27d0dc4e8a5a335fcf8a8f7849cf70518c60a Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 12 Jul 2024 16:19:10 +0200 Subject: [PATCH 09/33] Clean up and improve C++ samples Extract methods and cleanup formatting. Remove some less important error checks for readability. Make 2d capture samples loop, like the capture samples. Better align behavior of C++ and Python samples. --- zivid_samples/CMakeLists.txt | 14 +-- zivid_samples/scripts/sample_capture.py | 13 +- zivid_samples/scripts/sample_capture_2d.py | 9 +- zivid_samples/src/sample_capture.cpp | 90 +++++-------- zivid_samples/src/sample_capture_2d.cpp | 100 ++++++++------- zivid_samples/src/sample_capture_and_save.cpp | 33 ++--- .../src/sample_capture_assistant.cpp | 50 ++++---- ...sample_capture_with_settings_from_file.cpp | 118 ++++++++---------- 8 files changed, 187 insertions(+), 240 deletions(-) diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index 78c9d1c..60530a8 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -19,23 +19,23 @@ find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) add_executable(sample_capture_cpp src/sample_capture.cpp) -ament_target_dependencies(sample_capture_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) +ament_target_dependencies(sample_capture_cpp rclcpp sensor_msgs std_srvs) install(TARGETS sample_capture_cpp DESTINATION lib/${PROJECT_NAME}) add_executable(sample_capture_2d_cpp src/sample_capture_2d.cpp) -ament_target_dependencies(sample_capture_2d_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) +ament_target_dependencies(sample_capture_2d_cpp rclcpp sensor_msgs std_srvs) install(TARGETS sample_capture_2d_cpp DESTINATION lib/${PROJECT_NAME}) -add_executable(sample_capture_assistant_cpp src/sample_capture_assistant.cpp) -ament_target_dependencies(sample_capture_assistant_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) -install(TARGETS sample_capture_assistant_cpp DESTINATION lib/${PROJECT_NAME}) - add_executable(sample_capture_and_save_cpp src/sample_capture_and_save.cpp) ament_target_dependencies(sample_capture_and_save_cpp rclcpp zivid_interfaces) install(TARGETS sample_capture_and_save_cpp DESTINATION lib/${PROJECT_NAME}) +add_executable(sample_capture_assistant_cpp src/sample_capture_assistant.cpp) +ament_target_dependencies(sample_capture_assistant_cpp rclcpp zivid_interfaces sensor_msgs std_srvs) +install(TARGETS sample_capture_assistant_cpp DESTINATION lib/${PROJECT_NAME}) + add_executable(sample_capture_with_settings_from_file_cpp src/sample_capture_with_settings_from_file.cpp) -ament_target_dependencies(sample_capture_with_settings_from_file_cpp rclcpp zivid_interfaces sensor_msgs std_srvs ament_index_cpp) +ament_target_dependencies(sample_capture_with_settings_from_file_cpp rclcpp sensor_msgs std_srvs ament_index_cpp) install(TARGETS sample_capture_with_settings_from_file_cpp DESTINATION lib/${PROJECT_NAME}) install(PROGRAMS scripts/sample_capture.py DESTINATION lib/${PROJECT_NAME}) diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index e91345f..eb1b510 100644 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -39,8 +39,6 @@ def _set_capture_settings(self): - Acquisition: Aperture: 5.66 ExposureTime: 8333 - Diagnostics: - Enabled: yes Processing: Filters: Outlier: @@ -60,7 +58,7 @@ def capture(self): def on_points(self, msg): self.get_logger().info( - f"Received point cloud of width {msg.width} and height {msg.height}" + f"Received point cloud of size {msg.width} x {msg.height}" ) @@ -70,11 +68,12 @@ def main(args=None): try: sample = Sample() - future = sample.capture() - rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info("Capture complete") + sample.get_logger().info("Spinning node.. Press Ctrl+C to abort.") - rclpy.spin(sample) + while rclpy.ok(): + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info("Capture complete") except KeyboardInterrupt: pass diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index 6fce8a4..c17d52e 100644 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -62,11 +62,12 @@ def main(args=None): try: sample = Sample() - future = sample.capture() - rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info("Capture complete") + sample.get_logger().info("Spinning node.. Press Ctrl+C to abort.") - rclpy.spin(sample) + while rclpy.ok(): + future = sample.capture() + rclpy.spin_until_future_complete(sample, future) + sample.get_logger().info("Capture complete") except KeyboardInterrupt: pass diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 05a01b6..d8e20e5 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -28,19 +28,21 @@ #include -#include #include #include #include +#include + /* - * This sample shows how to set the settings_file_path parameter of the zivid - * node, subscribe for the points/xyzrgba topic, and invoke the capture service. - * When a point cloud is received, a new capture is triggered. + * This sample shows how to set the settings_file_path parameter of the zivid node, subscribe for + * the points/xyzrgba topic, and invoke the capture service. When a point cloud is received, a new + * capture is triggered. */ void set_settings(const std::shared_ptr & node) { + RCLCPP_INFO(node->get_logger(), "Setting parameter 'settings_yaml'"); const std::string settings_yml = R"( __version__: @@ -59,82 +61,58 @@ void set_settings(const std::shared_ptr & node) Threshold: 5 )"; - auto param_client = - std::make_shared(node, "zivid_camera"); + auto param_client = std::make_shared(node, "zivid_camera"); while (!param_client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), - "Client interrupted while waiting for service to appear."); - terminate(); + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); } - RCLCPP_INFO(node->get_logger(), - "Waiting for the param client to appear..."); + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); } - auto result = param_client->set_parameters( - {rclcpp::Parameter("settings_yaml", settings_yml)}, [&node](auto future) { - auto results = future.get(); - if (results.size() != 1) { - RCLCPP_ERROR_STREAM(node->get_logger(), - "Expected 1 result, got " << results.size()); - } else { - if (results[0].successful) { - RCLCPP_INFO(node->get_logger(), "Successfully set settings_yaml"); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml"); - } - } - }); - - if (rclcpp::spin_until_future_complete(node, result) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml parameter"); + auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_yaml` parameter"); std::terminate(); } } -int main(int argc, char * argv[]) +auto create_capture_client(std::shared_ptr & node) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("sample_capture"); - RCLCPP_INFO(node->get_logger(), "Started the sample_capture node"); - auto client = node->create_client("capture"); while (!client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { - RCLCPP_ERROR( - node->get_logger(), - "Client interrupted while waiting for service to appear."); - return EXIT_FAILURE; + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); } - RCLCPP_INFO( - node->get_logger(), - "Waiting for the capture service to appear..."); + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); } - RCLCPP_INFO(node->get_logger(), "Service is available"); + RCLCPP_INFO(node->get_logger(), "Capture service is available"); + return client; +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture node"); set_settings(node); + auto capture_client = create_capture_client(node); auto trigger_capture = [&]() { RCLCPP_INFO(node->get_logger(), "Triggering capture"); - client->async_send_request(std::make_shared()); + capture_client->async_send_request(std::make_shared()); }; auto points_xyzrgba_subscription = - node->create_subscription( - "points/xyzrgba", 10, - [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { - RCLCPP_INFO( - node->get_logger(), - "Received point cloud of width %d and height %d", - msg->width, msg->height); - RCLCPP_INFO( - node->get_logger(), - "Re-trigger capture"); - trigger_capture(); - }); + node->create_subscription("points/xyzrgba", 10, [&]( + sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received point cloud of size %d x %d", + msg->width, msg->height); + trigger_capture(); + }); trigger_capture(); diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index 870d804..df7422b 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -26,40 +26,22 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include #include + #include #include +#include + /* - * This sample shows how to set the settings_2d_yaml parameter of the zivid - * node, subscribe for the color/image_color topic, and invoke the capture_2d service. - * When an image is received, a new capture is triggered. + * This sample shows how to set the settings_2d_yaml parameter of the zivid node, subscribe for the + * color/image_color topic, and invoke the capture_2d service. When an image is received, a new + * capture is triggered. */ -int main(int argc, char * argv[]) +void set_settings_2d(const std::shared_ptr & node) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("sample_capture_2d"); - RCLCPP_INFO(node->get_logger(), "Started the sample_capture_2d node"); - - auto client = node->create_client("capture_2d"); - while (!client->wait_for_service(std::chrono::seconds(3))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); - return EXIT_FAILURE; - } - RCLCPP_INFO(node->get_logger(), "Waiting for the capture_2d service to appear..."); - } - - RCLCPP_INFO(node->get_logger(), "Service is available"); - - auto param_client = std::make_shared(node, "zivid_camera"); - if (!param_client->service_is_ready()) { - RCLCPP_ERROR(node->get_logger(), "Param client is not ready"); - return EXIT_FAILURE; - } - + RCLCPP_INFO_STREAM(node->get_logger(), "Setting parameter `settings_2d_yaml`"); const std::string settings_2d_yaml = R"( __version__: @@ -74,35 +56,57 @@ int main(int argc, char * argv[]) Gain: 2.5 )"; - RCLCPP_INFO_STREAM( - node->get_logger(), "Settings settings_2d_yaml to '" << settings_2d_yaml << "'"); - - param_client->set_parameters( - {rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}, [&node](auto future) { - auto results = future.get(); - if (results.size() != 1) { - RCLCPP_ERROR_STREAM(node->get_logger(), "Expected 1 result, got " << results.size()); - } else { - if (results[0].successful) { - RCLCPP_INFO(node->get_logger(), "Successfully set settings_2d_yaml"); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings_2d_yaml"); - } - } - }); + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); + } + + auto result = param_client->set_parameters( + {rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_2d_yaml` parameter"); + std::terminate(); + } +} + +auto create_capture_2d_client(std::shared_ptr & node) +{ + auto client = node->create_client("capture_2d"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); + } + + RCLCPP_INFO(node->get_logger(), "Capture service is available"); + return client; +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_2d"); + RCLCPP_INFO(node->get_logger(), "Started the sample_capture_2d node"); + + set_settings_2d(node); + auto capture_2d_client = create_capture_2d_client(node); auto trigger_capture = [&]() { RCLCPP_INFO(node->get_logger(), "Triggering 2d capture"); - client->async_send_request(std::make_shared()); + capture_2d_client->async_send_request(std::make_shared()); }; auto color_image_color_subscription = node->create_subscription( - "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { - RCLCPP_INFO( - node->get_logger(), "Received image of width %d and height %d", msg->width, msg->height); - RCLCPP_INFO(node->get_logger(), "Re-trigger 2d capture"); + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received image of size %d x %d", msg->width, msg->height); trigger_capture(); - }); + }); trigger_capture(); diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 3070fc6..70a6f39 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -30,6 +30,7 @@ #include +#include #include /* @@ -40,6 +41,7 @@ void set_settings(const std::shared_ptr & node) { + RCLCPP_INFO(node->get_logger(), "Setting parameter 'settings_yaml'"); const std::string settings_yml = R"( __version__: @@ -50,8 +52,6 @@ void set_settings(const std::shared_ptr & node) - Acquisition: Aperture: 5.66 ExposureTime: 8333 - Diagnostics: - Enabled: yes Processing: Filters: Outlier: @@ -64,27 +64,14 @@ void set_settings(const std::shared_ptr & node) while (!param_client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); - terminate(); + std::terminate(); } - RCLCPP_INFO(node->get_logger(), "Waiting for the param client to appear..."); + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); } - auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}, - [&node](auto future) { - auto results = future.get(); - if (results.size() != 1) { - RCLCPP_ERROR_STREAM(node->get_logger(), "Expected 1 result, got " << results.size()); - } else { - if (results[0].successful) { - RCLCPP_INFO(node->get_logger(), "Successfully set settings_yaml"); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml"); - } - } - }); - + auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings_yaml parameter"); + RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_yaml` parameter"); std::terminate(); } } @@ -102,12 +89,12 @@ void capture_and_save(const std::shared_ptr & node) RCLCPP_INFO(node->get_logger(), "Waiting for the capture_and_save service to appear..."); } - const std::string filename = "capture_and_save_cpp.zdf"; + const std::string filename = "zivid_sample_capture_and_save.zdf"; auto request = std::make_shared(); request->file_path = (std::filesystem::temp_directory_path() / filename).string(); RCLCPP_INFO(node->get_logger(), "Sending capture and save request with file path: %s", - request->file_path.c_str()); + request->file_path.c_str()); auto result = client->async_send_request(request); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { @@ -118,14 +105,14 @@ void capture_and_save(const std::shared_ptr & node) auto capture_response = result.get(); if (!capture_response->success) { RCLCPP_ERROR(node->get_logger(), "Capture and save operation was unsuccessful: %s", - capture_response->message.c_str()); + capture_response->message.c_str()); std::terminate(); } RCLCPP_INFO(node->get_logger(), "Capture and save operation successful"); } -int main(int argc, char * argv[]) +int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture_and_save"); diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 6d97902..0a761ec 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -28,25 +28,26 @@ #include -#include #include #include #include #include +#include + /* * This sample shows how to use the capture assistant service to suggest and set the capture * settings parameter of the zivid node. Then, it shows how to subscribe to the points/xyzrgba and - * color/image_color topics, and finally invoke the capture service. + * color/image_color topics, and finally how to invoke the capture service. */ void capture_assistant_suggest_settings(const std::shared_ptr & node) { using zivid_interfaces::srv::CaptureAssistantSuggestSettings; - auto client = - node->create_client("capture_assistant/suggest_settings"); + auto client = node->create_client( + "capture_assistant/suggest_settings"); while (!client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); @@ -56,12 +57,11 @@ void capture_assistant_suggest_settings(const std::shared_ptr & no } auto request = std::make_shared(); - request->max_capture_time = rclcpp::Duration::from_seconds(5.0); + request->max_capture_time = rclcpp::Duration::from_seconds(2.0); request->ambient_light_frequency = CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to call capture assistant service"); std::terminate(); @@ -73,19 +73,15 @@ void capture(const std::shared_ptr & node) auto capture_client = node->create_client("capture"); while (!capture_client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { - RCLCPP_ERROR( - node->get_logger(), - "Client interrupted while waiting for service to appear."); + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); std::terminate(); } - RCLCPP_INFO( - node->get_logger(), - "Waiting for the capture service to appear..."); + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); } RCLCPP_INFO(node->get_logger(), "Triggering capture"); - auto result = - capture_client->async_send_request(std::make_shared()); + auto result = capture_client->async_send_request( + std::make_shared()); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to trigger capture"); @@ -93,28 +89,24 @@ void capture(const std::shared_ptr & node) } } -int main(int argc, char * argv[]) +int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture_assistant"); RCLCPP_INFO(node->get_logger(), "Started the sample_capture_assistant node"); - capture_assistant_suggest_settings(node); - - auto points_xyzrgba_subscription = - node->create_subscription( - "points/xyzrgba", 10, - [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { - RCLCPP_INFO( - node->get_logger(), "Received point cloud of width %d and height %d", msg->width, - msg->height); - }); + auto points_xyzrgba_subscription = node->create_subscription( + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received point cloud of size %d x %d", msg->width, + msg->height); + }); auto color_image_color_subscription = node->create_subscription( - "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { - RCLCPP_INFO( - node->get_logger(), "Received image of width %d and height %d", msg->width, msg->height); - }); + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received image of size %d x %d", msg->width, msg->height); + }); + + capture_assistant_suggest_settings(node); capture(node); diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp index d7825a7..f51d33d 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_file.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -33,85 +33,71 @@ #include #include +#include + /* - * This sample shows how to set the settings_file_path parameter of the zivid - * node, subscribe for the points/xyzrgba topic, and invoke the capture service. - * When a point cloud is received, a new capture is triggered. + * This sample shows how to set the settings_file_path parameter of the zivid node, subscribe for + * the points/xyzrgba topic, and invoke the capture service. When a point cloud is received, a new + * capture is triggered. */ -int main(int argc, char * argv[]) +void set_settings(const std::shared_ptr & node) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("sample_capture_with_settings_from_file"); - RCLCPP_INFO(node->get_logger(), "Started the sample node"); + const auto share_directory = ament_index_cpp::get_package_share_directory("zivid_samples"); + const auto path_to_settings_yml = share_directory + "/settings/camera_settings.yml"; + RCLCPP_INFO_STREAM(node->get_logger(), + "Setting parameter `settings_file_path` to '" << path_to_settings_yml << "'"); + + auto param_client = std::make_shared(node, "zivid_camera"); + while (!param_client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); + } + RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); + } + + auto result = param_client->set_parameters( + {rclcpp::Parameter("settings_file_path", path_to_settings_yml)}); + if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_file_path` parameter"); + std::terminate(); + } +} +auto create_capture_client(std::shared_ptr & node) +{ auto client = node->create_client("capture"); while (!client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { - RCLCPP_ERROR( - node->get_logger(), - "Client interrupted while waiting for service to appear."); - return EXIT_FAILURE; + RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); + std::terminate(); } - RCLCPP_INFO( - node->get_logger(), - "Waiting for the capture service to appear..."); + RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); } RCLCPP_INFO(node->get_logger(), "Service is available"); + return client; +} - auto param_client = - std::make_shared(node, "zivid_camera"); - if (!param_client->service_is_ready()) { - RCLCPP_ERROR(node->get_logger(), "Param client is not ready"); - return EXIT_FAILURE; - } - const auto share_directory = - ament_index_cpp::get_package_share_directory("zivid_samples"); - const auto path_to_settings_yml = - share_directory + "/settings/camera_settings.yml"; - - RCLCPP_INFO_STREAM( - node->get_logger(), "Setting settings path to '" - << path_to_settings_yml << "'"); - - param_client->set_parameters( - {rclcpp::Parameter("settings_file_path", path_to_settings_yml)}, - [&node](auto future) { - auto results = future.get(); - if (results.size() != 1) { - RCLCPP_ERROR_STREAM( - node->get_logger(), - "Expected 1 result, got " << results.size()); - } else { - if (results[0].successful) { - RCLCPP_INFO(node->get_logger(), "Successfully set settings path"); - } else { - RCLCPP_ERROR(node->get_logger(), "Failed to set settings path"); - } - } - }); - - auto trigger_capture = [&]() { - RCLCPP_INFO(node->get_logger(), "Triggering capture"); - client->async_send_request(std::make_shared()); - }; - - auto points_xyzrgba_subscription = - node->create_subscription( - "points/xyzrgba", 10, - [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { - RCLCPP_INFO( - node->get_logger(), - "Received point cloud of width %d and height %d", - msg->width, msg->height); - RCLCPP_INFO( - node->get_logger(), - "Re-trigger capture"); - trigger_capture(); - }); - - trigger_capture(); +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("sample_capture_with_settings_from_file"); + RCLCPP_INFO(node->get_logger(), "Started the sample node"); + + auto points_xyzrgba_subscription = node->create_subscription( + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO(node->get_logger(), "Received point cloud of size %d x %d", msg->width, + msg->height); + }); + + set_settings(node); + + auto client = create_capture_client(node); + + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + client->async_send_request(std::make_shared()); RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); rclcpp::spin(node); From 4e3bfee1e5532b6d83843ee866d673ec34cbd2f3 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 12 Jul 2024 17:08:37 +0200 Subject: [PATCH 10/33] Use native line endings on .sh files --- continuous-integration/.gitattributes | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 continuous-integration/.gitattributes diff --git a/continuous-integration/.gitattributes b/continuous-integration/.gitattributes new file mode 100644 index 0000000..5e068a8 --- /dev/null +++ b/continuous-integration/.gitattributes @@ -0,0 +1,2 @@ +# Bash does not play well with Windows line endings. +**.sh text eol=lf From 6b873ff9f053e3f1954af86c7ce8b504c710b3ed Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Mon, 15 Jul 2024 11:47:38 +0200 Subject: [PATCH 11/33] Format samples in docker container (gives different results from local environment) --- zivid_samples/src/sample_capture.cpp | 16 +++++++++------- zivid_samples/src/sample_capture_2d.cpp | 6 +++--- zivid_samples/src/sample_capture_and_save.cpp | 12 +++++++----- .../src/sample_capture_assistant.cpp | 19 ++++++++++--------- ...sample_capture_with_settings_from_file.cpp | 16 +++++++++------- 5 files changed, 38 insertions(+), 31 deletions(-) diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index d8e20e5..931b0f0 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -92,7 +92,7 @@ auto create_capture_client(std::shared_ptr & node) return client; } -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture"); @@ -107,12 +107,14 @@ int main(int argc, char *argv[]) }; auto points_xyzrgba_subscription = - node->create_subscription("points/xyzrgba", 10, [&]( - sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { - RCLCPP_INFO(node->get_logger(), "Received point cloud of size %d x %d", - msg->width, msg->height); - trigger_capture(); - }); + node->create_subscription( + "points/xyzrgba", 10, [&]( + sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of size %d x %d", + msg->width, msg->height); + trigger_capture(); + }); trigger_capture(); diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index df7422b..a8a4c5c 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -88,7 +88,7 @@ auto create_capture_2d_client(std::shared_ptr & node) return client; } -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture_2d"); @@ -103,10 +103,10 @@ int main(int argc, char *argv[]) }; auto color_image_color_subscription = node->create_subscription( - "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { RCLCPP_INFO(node->get_logger(), "Received image of size %d x %d", msg->width, msg->height); trigger_capture(); - }); + }); trigger_capture(); diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 70a6f39..6090cf1 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -93,8 +93,9 @@ void capture_and_save(const std::shared_ptr & node) auto request = std::make_shared(); request->file_path = (std::filesystem::temp_directory_path() / filename).string(); - RCLCPP_INFO(node->get_logger(), "Sending capture and save request with file path: %s", - request->file_path.c_str()); + RCLCPP_INFO( + node->get_logger(), "Sending capture and save request with file path: %s", + request->file_path.c_str()); auto result = client->async_send_request(request); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { @@ -104,15 +105,16 @@ void capture_and_save(const std::shared_ptr & node) auto capture_response = result.get(); if (!capture_response->success) { - RCLCPP_ERROR(node->get_logger(), "Capture and save operation was unsuccessful: %s", - capture_response->message.c_str()); + RCLCPP_ERROR( + node->get_logger(), "Capture and save operation was unsuccessful: %s", + capture_response->message.c_str()); std::terminate(); } RCLCPP_INFO(node->get_logger(), "Capture and save operation successful"); } -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture_and_save"); diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 0a761ec..77191cd 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -47,7 +47,7 @@ void capture_assistant_suggest_settings(const std::shared_ptr & no using zivid_interfaces::srv::CaptureAssistantSuggestSettings; auto client = node->create_client( - "capture_assistant/suggest_settings"); + "capture_assistant/suggest_settings"); while (!client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); @@ -81,7 +81,7 @@ void capture(const std::shared_ptr & node) RCLCPP_INFO(node->get_logger(), "Triggering capture"); auto result = capture_client->async_send_request( - std::make_shared()); + std::make_shared()); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to trigger capture"); @@ -89,22 +89,23 @@ void capture(const std::shared_ptr & node) } } -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture_assistant"); RCLCPP_INFO(node->get_logger(), "Started the sample_capture_assistant node"); auto points_xyzrgba_subscription = node->create_subscription( - "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { - RCLCPP_INFO(node->get_logger(), "Received point cloud of size %d x %d", msg->width, - msg->height); - }); + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of size %d x %d", msg->width, + msg->height); + }); auto color_image_color_subscription = node->create_subscription( - "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { + "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { RCLCPP_INFO(node->get_logger(), "Received image of size %d x %d", msg->width, msg->height); - }); + }); capture_assistant_suggest_settings(node); diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp index f51d33d..092aca2 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_file.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -45,8 +45,9 @@ void set_settings(const std::shared_ptr & node) { const auto share_directory = ament_index_cpp::get_package_share_directory("zivid_samples"); const auto path_to_settings_yml = share_directory + "/settings/camera_settings.yml"; - RCLCPP_INFO_STREAM(node->get_logger(), - "Setting parameter `settings_file_path` to '" << path_to_settings_yml << "'"); + RCLCPP_INFO_STREAM( + node->get_logger(), + "Setting parameter `settings_file_path` to '" << path_to_settings_yml << "'"); auto param_client = std::make_shared(node, "zivid_camera"); while (!param_client->wait_for_service(std::chrono::seconds(3))) { @@ -80,17 +81,18 @@ auto create_capture_client(std::shared_ptr & node) return client; } -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("sample_capture_with_settings_from_file"); RCLCPP_INFO(node->get_logger(), "Started the sample node"); auto points_xyzrgba_subscription = node->create_subscription( - "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { - RCLCPP_INFO(node->get_logger(), "Received point cloud of size %d x %d", msg->width, - msg->height); - }); + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + RCLCPP_INFO( + node->get_logger(), "Received point cloud of size %d x %d", msg->width, + msg->height); + }); set_settings(node); From c962f848f126b8550b52e51b157897733436ec1f Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Mon, 15 Jul 2024 16:01:49 +0200 Subject: [PATCH 12/33] Enable `zivid_samples` linting and fix resulting issues Replace `ament_lint_common` with individual linters. Added each linter from common except: `ament_cmake_copyright`. The full list for `ament_lint_common` is found here: https://github.com/ament/ament_lint/blob/master/ament_lint_common/package.xml Add a `pyproject.toml` so that the Black formatting is compatible with the `ament_flake8` linter. --- pyproject.toml | 2 + zivid_samples/CMakeLists.txt | 5 +-- zivid_samples/package.xml | 11 +++++- zivid_samples/scripts/sample_capture.py | 24 ++++++------ zivid_samples/scripts/sample_capture_2d.py | 24 ++++++------ .../scripts/sample_capture_and_save.py | 22 +++++------ .../scripts/sample_capture_assistant.py | 39 +++++++++---------- .../sample_capture_with_settings_from_file.py | 30 +++++++------- zivid_samples/src/sample_capture.cpp | 4 +- zivid_samples/src/sample_capture_2d.cpp | 4 +- zivid_samples/src/sample_capture_and_save.cpp | 6 +-- .../src/sample_capture_assistant.cpp | 3 +- ...sample_capture_with_settings_from_file.cpp | 4 +- 13 files changed, 93 insertions(+), 85 deletions(-) create mode 100644 pyproject.toml diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..ae51d7b --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,2 @@ +[tool.black] +skip-string-normalization = true # Black prefers double quotes, however, this is incompatible with the `ament_flake8` configuration. diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index 60530a8..f82e395 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -45,9 +45,8 @@ install(PROGRAMS scripts/sample_capture_assistant.py DESTINATION lib/${PROJECT_N install(PROGRAMS scripts/sample_capture_with_settings_from_file.py DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) - # TODO enable linting of the samples - #find_package(ament_lint_auto REQUIRED) - #ament_lint_auto_find_test_dependencies() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() endif() install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/zivid_samples/package.xml b/zivid_samples/package.xml index b6242cb..349ea59 100644 --- a/zivid_samples/package.xml +++ b/zivid_samples/package.xml @@ -20,9 +20,16 @@ zivid_interfaces ament_index_cpp rclpy - ament_cmake_gtest + ament_lint_auto - ament_lint_common + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_uncrustify + ament_cmake_xmllint + ament_cmake diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index eb1b510..471ae22 100644 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -13,22 +13,22 @@ class Sample(Node): def __init__(self): - super().__init__("sample_capture_py") + super().__init__('sample_capture_py') - self.capture_service = self.create_client(Trigger, "capture") + self.capture_service = self.create_client(Trigger, 'capture') while not self.capture_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info("Capture service not available, waiting again...") + self.get_logger().info('Capture service not available, waiting again...') self._set_capture_settings() self.subscription = self.create_subscription( - PointCloud2, "points/xyzrgba", self.on_points, 10 + PointCloud2, 'points/xyzrgba', self.on_points, 10 ) def _set_capture_settings(self): - self.get_logger().info("Setting parameter 'settings_yaml'") + self.get_logger().info('Setting parameter `settings_yaml`') settings_parameter = Parameter( - "settings_yaml", + 'settings_yaml', Parameter.Type.STRING, """ __version__: @@ -47,18 +47,18 @@ def _set_capture_settings(self): Threshold: 5 """, ).to_parameter_msg() - param_client = AsyncParameterClient(self, "zivid_camera") + param_client = AsyncParameterClient(self, 'zivid_camera') param_client.wait_for_services() future = param_client.set_parameters([settings_parameter]) rclpy.spin_until_future_complete(self, future) def capture(self): - self.get_logger().info("Calling capture service") + self.get_logger().info('Calling capture service') return self.capture_service.call_async(Trigger.Request()) def on_points(self, msg): self.get_logger().info( - f"Received point cloud of size {msg.width} x {msg.height}" + f'Received point cloud of size {msg.width} x {msg.height}' ) @@ -68,12 +68,12 @@ def main(args=None): try: sample = Sample() - sample.get_logger().info("Spinning node.. Press Ctrl+C to abort.") + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') while rclpy.ok(): future = sample.capture() rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info("Capture complete") + sample.get_logger().info('Capture complete') except KeyboardInterrupt: pass @@ -81,5 +81,5 @@ def main(args=None): sys.exit(1) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index c17d52e..12d3033 100644 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -13,22 +13,22 @@ class Sample(Node): def __init__(self): - super().__init__("sample_capture_2d_py") + super().__init__('sample_capture_2d_py') - self.capture_2d_service = self.create_client(Trigger, "capture_2d") + self.capture_2d_service = self.create_client(Trigger, 'capture_2d') while not self.capture_2d_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info("Capture service not available, waiting again...") + self.get_logger().info('Capture service not available, waiting again...') self._set_capture_settings() self.subscription = self.create_subscription( - Image, "color/image_color", self.on_image_color, 10 + Image, 'color/image_color', self.on_image_color, 10 ) def _set_capture_settings(self): - self.get_logger().info("Setting parameter 'settings_2d_yaml'") + self.get_logger().info('Setting parameter `settings_2d_yaml`') settings_parameter = Parameter( - "settings_2d_yaml", + 'settings_2d_yaml', Parameter.Type.STRING, """ __version__: @@ -43,17 +43,17 @@ def _set_capture_settings(self): Gain: 2.5 """, ).to_parameter_msg() - param_client = AsyncParameterClient(self, "zivid_camera") + param_client = AsyncParameterClient(self, 'zivid_camera') param_client.wait_for_services() future = param_client.set_parameters([settings_parameter]) rclpy.spin_until_future_complete(self, future) def capture(self): - self.get_logger().info("Calling capture service") + self.get_logger().info('Calling capture service') return self.capture_2d_service.call_async(Trigger.Request()) def on_image_color(self, msg): - self.get_logger().info(f"Received image of size {msg.width} x {msg.height}") + self.get_logger().info(f'Received image of size {msg.width} x {msg.height}') def main(args=None): @@ -62,12 +62,12 @@ def main(args=None): try: sample = Sample() - sample.get_logger().info("Spinning node.. Press Ctrl+C to abort.") + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') while rclpy.ok(): future = sample.capture() rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info("Capture complete") + sample.get_logger().info('Capture complete') except KeyboardInterrupt: pass @@ -75,5 +75,5 @@ def main(args=None): sys.exit(1) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index 1e9bc12..496a6d0 100644 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -13,20 +13,20 @@ class Sample(Node): def __init__(self): - super().__init__("sample_capture_and_save_py") + super().__init__('sample_capture_and_save_py') self.capture_and_save_service = self.create_client( - CaptureAndSave, "capture_and_save" + CaptureAndSave, 'capture_and_save' ) while not self.capture_and_save_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info("Capture service not available, waiting again...") + self.get_logger().info('Capture service not available, waiting again...') self._set_capture_settings() def _set_capture_settings(self): - self.get_logger().info("Setting parameter 'settings_yaml'") + self.get_logger().info('Setting parameter `settings_yaml`') settings_parameter = Parameter( - "settings_yaml", + 'settings_yaml', Parameter.Type.STRING, """ __version__: @@ -45,14 +45,14 @@ def _set_capture_settings(self): Threshold: 5 """, ).to_parameter_msg() - param_client = AsyncParameterClient(self, "zivid_camera") + param_client = AsyncParameterClient(self, 'zivid_camera') param_client.wait_for_services() future = param_client.set_parameters([settings_parameter]) rclpy.spin_until_future_complete(self, future) def capture(self): - file_path = tempfile.gettempdir() + "/zivid_sample_capture_and_save.zdf" - self.get_logger().info(f"Calling capture service with file path: {file_path}") + file_path = tempfile.gettempdir() + '/zivid_sample_capture_and_save.zdf' + self.get_logger().info(f'Calling capture service with file path: {file_path}') request = CaptureAndSave.Request(file_path=file_path) return self.capture_and_save_service.call_async(request) @@ -68,9 +68,9 @@ def main(args=None): response: CaptureAndSave.Response = future.result() if not response.success: - sample.get_logger().error(f"Failed capture and save: {response.message}") + sample.get_logger().error(f'Failed capture and save: {response.message}') - sample.get_logger().info(f"Capture and save complete") + sample.get_logger().info('Capture and save complete') except KeyboardInterrupt: pass @@ -78,5 +78,5 @@ def main(args=None): sys.exit(1) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py index 25eac55..cb31c35 100644 --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -1,65 +1,64 @@ import sys -import tempfile + +from builtin_interfaces.msg import Duration import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node -from rclpy.parameter import Parameter -from rclpy.parameter_client import AsyncParameterClient -from builtin_interfaces.msg import Duration from sensor_msgs.msg import Image from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger - from zivid_interfaces.srv import CaptureAssistantSuggestSettings class Sample(Node): def __init__(self): - super().__init__("sample_capture_and_save_py") + super().__init__('sample_capture_and_save_py') self.capture_assistant_suggest_settings_service = self.create_client( - CaptureAssistantSuggestSettings, "capture_assistant/suggest_settings" + CaptureAssistantSuggestSettings, 'capture_assistant/suggest_settings' ) while not self.capture_assistant_suggest_settings_service.wait_for_service( timeout_sec=3.0 ): self.get_logger().info( - "Capture assistant service not available, waiting again..." + 'Capture assistant service not available, waiting again...' ) - self.capture_service = self.create_client(Trigger, "capture") + self.capture_service = self.create_client(Trigger, 'capture') while not self.capture_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info("Capture service not available, waiting again...") + self.get_logger().info('Capture service not available, waiting again...') self.points_subscription = self.create_subscription( - PointCloud2, "points/xyzrgba", self.on_points, 10 + PointCloud2, 'points/xyzrgba', self.on_points, 10 ) self.image_subscription = self.create_subscription( - Image, "color/image_color", self.on_image_color, 10 + Image, 'color/image_color', self.on_image_color, 10 ) def capture_assistant_suggest_settings(self): - self.get_logger().info("Calling capture assistant service") + self.get_logger().info('Calling capture assistant service') request = CaptureAssistantSuggestSettings.Request( max_capture_time=Duration(sec=2), - ambient_light_frequency=CaptureAssistantSuggestSettings.Request.AMBIENT_LIGHT_FREQUENCY_NONE, + ambient_light_frequency=( + CaptureAssistantSuggestSettings.Request.AMBIENT_LIGHT_FREQUENCY_NONE + ), ) return self.capture_assistant_suggest_settings_service.call_async(request) def capture(self): - self.get_logger().info("Calling capture service") + self.get_logger().info('Calling capture service') return self.capture_service.call_async(Trigger.Request()) def on_points(self, msg): self.get_logger().info( - f"Received point cloud of size {msg.width} x {msg.height}" + f'Received point cloud of size {msg.width} x {msg.height}' ) def on_image_color(self, msg): - self.get_logger().info(f"Received image of size {msg.width} x {msg.height}") + self.get_logger().info(f'Received image of size {msg.width} x {msg.height}') def main(args=None): @@ -70,11 +69,11 @@ def main(args=None): future = sample.capture_assistant_suggest_settings() rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info(f"Capture assistant complete") + sample.get_logger().info('Capture assistant complete') future = sample.capture() rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info("Capture complete") + sample.get_logger().info('Capture complete') rclpy.spin(sample) @@ -84,5 +83,5 @@ def main(args=None): sys.exit(1) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py index 36c5850..b17df7c 100644 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -1,13 +1,13 @@ import sys +from ament_index_python.packages import get_package_share_directory + import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter from rclpy.parameter_client import AsyncParameterClient -from ament_index_python.packages import get_package_share_directory - from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger @@ -15,45 +15,45 @@ class Sample(Node): def __init__(self): - super().__init__("sample_capture_with_settings_from_file_py") + super().__init__('sample_capture_with_settings_from_file_py') - self.capture_service = self.create_client(Trigger, "capture") + self.capture_service = self.create_client(Trigger, 'capture') while not self.capture_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info("Capture service not available, waiting again...") + self.get_logger().info('Capture service not available, waiting again...') self._set_capture_settings() self.subscription = self.create_subscription( - PointCloud2, "points/xyzrgba", self.on_points, 10 + PointCloud2, 'points/xyzrgba', self.on_points, 10 ) def _set_capture_settings(self): path_to_settings_yml = ( - get_package_share_directory("zivid_samples") - + "/settings/camera_settings.yml" + get_package_share_directory('zivid_samples') + + '/settings/camera_settings.yml' ) self.get_logger().info( - "Setting parameter 'settings_file_path' to: " + path_to_settings_yml + 'Setting parameter `settings_file_path` to: ' + path_to_settings_yml ) settings_parameter = Parameter( - "settings_file_path", + 'settings_file_path', Parameter.Type.STRING, path_to_settings_yml, ).to_parameter_msg() - param_client = AsyncParameterClient(self, "zivid_camera") + param_client = AsyncParameterClient(self, 'zivid_camera') param_client.wait_for_services() future = param_client.set_parameters([settings_parameter]) rclpy.spin_until_future_complete(self, future) def capture(self): - self.get_logger().info("Calling capture service") + self.get_logger().info('Calling capture service') return self.capture_service.call_async(Trigger.Request()) def on_points(self, msg): self.get_logger().info( - f"Received point cloud of size {msg.width} x {msg.height}" + f'Received point cloud of size {msg.width} x {msg.height}' ) @@ -65,7 +65,7 @@ def main(args=None): future = sample.capture() rclpy.spin_until_future_complete(sample, future) - sample.get_logger().info("Capture complete") + sample.get_logger().info('Capture complete') rclpy.spin(sample) @@ -75,5 +75,5 @@ def main(args=None): sys.exit(1) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 931b0f0..1f6386f 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -26,14 +26,14 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include + #include #include #include #include -#include - /* * This sample shows how to set the settings_file_path parameter of the zivid node, subscribe for * the points/xyzrgba topic, and invoke the capture service. When a point cloud is received, a new diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index a8a4c5c..4a81e5b 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -26,13 +26,13 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include + #include #include #include -#include - /* * This sample shows how to set the settings_2d_yaml parameter of the zivid node, subscribe for the * color/image_color topic, and invoke the capture_2d service. When an image is received, a new diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 6090cf1..80cea4c 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -26,13 +26,13 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include +#include + #include #include -#include -#include - /* * This sample shows how to set the settings_yaml parameter of the zivid node, then invoke the * capture_and_save service, and read the response from the service call. If successful, the diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 77191cd..d07e8da 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -26,6 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include + #include #include @@ -34,7 +36,6 @@ #include -#include /* * This sample shows how to use the capture assistant service to suggest and set the capture diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp index 092aca2..f551dff 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_file.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -26,6 +26,8 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include + #include #include @@ -33,8 +35,6 @@ #include #include -#include - /* * This sample shows how to set the settings_file_path parameter of the zivid node, subscribe for * the points/xyzrgba topic, and invoke the capture service. When a point cloud is received, a new From 544746c068f4db196756fcced11c452cf87ff9c9 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Mon, 15 Jul 2024 16:02:36 +0200 Subject: [PATCH 13/33] Remove copyright notice from individual C++ sample files For consistency with our public `zivid-cpp-samples`. --- zivid_samples/src/sample_capture.cpp | 28 ------------------- zivid_samples/src/sample_capture_2d.cpp | 28 ------------------- zivid_samples/src/sample_capture_and_save.cpp | 28 ------------------- .../src/sample_capture_assistant.cpp | 28 ------------------- ...sample_capture_with_settings_from_file.cpp | 28 ------------------- 5 files changed, 140 deletions(-) diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 1f6386f..4d6c7f8 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -1,31 +1,3 @@ -// Copyright 2024 Zivid AS -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Zivid AS nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - #include #include diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index 4a81e5b..34e4566 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -1,31 +1,3 @@ -// Copyright 2024 Zivid AS -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Zivid AS nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - #include #include diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 80cea4c..9151b1c 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -1,31 +1,3 @@ -// Copyright 2024 Zivid AS -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Zivid AS nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - #include #include diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index d07e8da..6295667 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -1,31 +1,3 @@ -// Copyright 2024 Zivid AS -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Zivid AS nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - #include #include diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp index f551dff..4a1a4a6 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_file.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -1,31 +1,3 @@ -// Copyright 2024 Zivid AS -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Zivid AS nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - #include #include From 6c28a00a7de88c56927dd92c49ffc8494c3f1252 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Mon, 15 Jul 2024 17:52:33 +0200 Subject: [PATCH 14/33] Use clang-format instead of uncrustify In particular, clang-format provides a lot more stable output for some given input, and typically works better with IDEs. Replace `ament_lint_common` with the same individual linters, with the following exceptions: - Replace `ament_cmake_uncrustify` with `ament_cmake_clang_format`. - Remove `cament_cpp_lint` with the intention of later adding `ament_clang_tidy`. Additionally, run clang-format during code analysis instead of during testing. --- .clang-format | 22 ++ README.md | 9 +- continuous-integration/lint.sh | 16 ++ continuous-integration/test.sh | 6 +- .../include/zivid_camera/zivid_camera.hpp | 24 +- zivid_camera/package.xml | 10 +- zivid_camera/src/zivid_camera.cpp | 152 +++++------ zivid_camera/test/test_zivid_camera.cpp | 242 ++++++++---------- zivid_samples/package.xml | 3 +- zivid_samples/src/sample_capture.cpp | 17 +- zivid_samples/src/sample_capture_2d.cpp | 12 +- zivid_samples/src/sample_capture_and_save.cpp | 2 - .../src/sample_capture_assistant.cpp | 15 +- ...sample_capture_with_settings_from_file.cpp | 11 +- 14 files changed, 264 insertions(+), 277 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..2867c95 --- /dev/null +++ b/.clang-format @@ -0,0 +1,22 @@ +# Style from `ament_clang_format` linter. +# https://github.com/ament/ament_lint/blob/master/ament_clang_format/ament_clang_format/configuration/.clang-format +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterEnum: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false +... diff --git a/README.md b/README.md index 8921f7b..7e1c663 100644 --- a/README.md +++ b/README.md @@ -538,13 +538,16 @@ colcon build --cmake-args -DCOMPILER_WARNINGS=ON ### How to format the source code -The CI test for this package enforces the linting defined by `ament_lint_auto`. +The CI test for this package enforces the linting defined by `clang-format`. From the code analysis +docker image, run: ```bash -cd ~/ros2_ws/src/zivid-ros -ament_uncrustify --reformat ./ +find /host -name '*.cpp' -or -name '*.hpp' | xargs clang-format -i ``` +The style follows the one from +[`ament_clang_format`](https://github.com/ament/ament_lint/blob/master/ament_clang_format/doc/index.rst). + ## License This project is licensed under BSD 3-clause license, see the [LICENSE](LICENSE) file for details. diff --git a/continuous-integration/lint.sh b/continuous-integration/lint.sh index 97becf2..a825373 100755 --- a/continuous-integration/lint.sh +++ b/continuous-integration/lint.sh @@ -7,6 +7,7 @@ ROOT_DIR=$(realpath "$SCRIPT_DIR/..") pythonFiles=$(find "$ROOT_DIR" -name '*.py') bashFiles=$(find "$ROOT_DIR" -name '*.sh') +cppFiles=$(find "$ROOT_DIR" -name '*.cpp' -or -name '*.hpp') echo Running black on: echo "$pythonFiles" @@ -16,4 +17,19 @@ echo Running shellcheck on: echo "$bashFiles" shellcheck -x -e SC1090,SC2086,SC2046 $bashFiles || exit $? +echo Running clang-format on: +echo "$cppFiles" +clangFormatIssues=0 +for file in $cppFiles; do + diff_output=$(diff -u "$file" <(clang-format "$file")) + if [ -n "$diff_output" ]; then + echo "$diff_output" + clangFormatIssues=1 + fi +done + +if [ $clangFormatIssues -ne 0 ]; then + exit $clangFormatIssues +fi + echo Success! ["$(basename $0)"] diff --git a/continuous-integration/test.sh b/continuous-integration/test.sh index df8c117..d4febe1 100755 --- a/continuous-integration/test.sh +++ b/continuous-integration/test.sh @@ -16,8 +16,12 @@ unzip ./FileCameraZivid2M70.zip -d /usr/share/Zivid/data/ || exit $? rm ./FileCameraZivid2M70.zip || exit $? echo "Running tests" + +# We exclude `clang_format` here since it has variations between versions, instead we check it during code analysis. +excludeTests="clang_format" + export GTEST_BREAK_ON_FAILURE=1; -colcon test --event-handlers console_direct+ --ctest-args tests --output-on-failure --ros-args --log-level debug || exit $? +colcon test --event-handlers console_direct+ --ctest-args tests --exclude-regex $excludeTests --output-on-failure --ros-args --log-level debug || exit $? echo "Check for test errors" colcon test-result --all || exit $? diff --git a/zivid_camera/include/zivid_camera/zivid_camera.hpp b/zivid_camera/include/zivid_camera/zivid_camera.hpp index 027b3a2..f23453f 100644 --- a/zivid_camera/include/zivid_camera/zivid_camera.hpp +++ b/zivid_camera/include/zivid_camera/zivid_camera.hpp @@ -28,23 +28,20 @@ #pragma once +#include #include +#include +#include +#include +#include #include - #include - -#include -#include #include #include #include #include #include -#include -#include -#include - namespace Zivid { class Application; @@ -52,7 +49,7 @@ class Camera; class CameraIntrinsics; struct ColorRGBA; class Frame; -template +template class Image; class PointCloud; class Settings2D; @@ -61,8 +58,13 @@ class Settings; namespace zivid_camera { -enum class CameraStatus { Idle, Connected, Disconnected }; -template +enum class CameraStatus +{ + Idle, + Connected, + Disconnected +}; +template class CaptureSettingsController; class ZividCamera : public rclcpp::Node diff --git a/zivid_camera/package.xml b/zivid_camera/package.xml index 9583acc..368b195 100644 --- a/zivid_camera/package.xml +++ b/zivid_camera/package.xml @@ -20,9 +20,17 @@ rosidl_default_generators zivid_interfaces ros2launch + ament_cmake_gtest ament_lint_auto - ament_lint_common + ament_cmake_clang_format + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + ament_cmake diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index c69dada..6525ca2 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -26,8 +26,6 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include - #include #include #include @@ -40,12 +38,12 @@ #include #include -#include -#include - #include #include +#include #include +#include +#include namespace { @@ -69,7 +67,7 @@ bool big_endian() return b.c[0] == 1; } -template +template void fillCommonMsgFields( T & msg, const std_msgs::msg::Header & header, std::size_t width, std::size_t height) { @@ -93,7 +91,7 @@ sensor_msgs::msg::Image::SharedPtr makeImage( return image; } -template +template sensor_msgs::msg::Image::SharedPtr makePointCloudImage( const Zivid::PointCloud & point_cloud, const std_msgs::msg::Header & header, const std::string & encoding) @@ -126,7 +124,7 @@ std::string toString(zivid_camera::CameraStatus camera_status) return "N/A"; } -template +template auto serializeZividDataModel(const ZividDataModel & dm) { #if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) @@ -138,7 +136,7 @@ auto serializeZividDataModel(const ZividDataModel & dm) #endif } -template +template auto deserializeZividDataModel(const std::string & serialized) { #if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) @@ -152,7 +150,7 @@ auto deserializeZividDataModel(const std::string & serialized) #endif } -template +template void runFunctionAndCatchExceptions( Function && function, ResponseSharedPtr & response, const Logger & logger, const std::string & operation) @@ -169,7 +167,7 @@ void runFunctionAndCatchExceptions( } } -template +template auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger & logger) { try { @@ -181,10 +179,9 @@ auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger } } -template +template [[noreturn]] void logErrorToLoggerAndThrowRuntimeException( - const Logger & logger, - const std::string & message) + const Logger & logger, const std::string & message) { RCLCPP_ERROR(logger, message.c_str()); throw std::runtime_error(message); @@ -202,7 +199,7 @@ constexpr auto file_camera_path = "file_camera_path"; constexpr auto frame_id = "frame_id"; } // namespace ParamNames -template +template class CaptureSettingsController { public: @@ -215,9 +212,8 @@ class CaptureSettingsController using namespace std::placeholders; for (const auto & param : {yaml_string_param_, file_path_param_}) { node_.declare_parameter(param, ""); - cb_handles_.push_back( - param_subscriber_->add_parameter_callback( - param, std::bind(&CaptureSettingsController::parameterCallback, this, _1))); + cb_handles_.push_back(param_subscriber_->add_parameter_callback( + param, std::bind(&CaptureSettingsController::parameterCallback, this, _1))); } } @@ -230,12 +226,11 @@ class CaptureSettingsController logErrorToLoggerAndThrowRuntimeException( node_.get_logger(), "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + - "' parameters are non-empty! Please set only one of these parameters."); + "' parameters are non-empty! Please set only one of these parameters."); } else if (settings_file_path.empty() && settings_yaml.empty()) { logErrorToLoggerAndThrowRuntimeException( - node_.get_logger(), - "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + - "' parameters are empty! Please set one of these parameters."); + node_.get_logger(), "Both '" + file_path_param_ + "' and '" + yaml_string_param_ + + "' parameters are empty! Please set one of these parameters."); } else if (!settings_yaml.empty()) { RCLCPP_DEBUG_STREAM(node_.get_logger(), "Using settings from yml string"); return deserializeZividDataModel(settings_yaml); @@ -321,39 +316,38 @@ ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) const bool update_firmware_automatically = declare_parameter("update_firmware_automatically", true); - camera_ = std::make_unique( - [&]() { - if (file_camera_mode) { - RCLCPP_INFO(get_logger(), "Creating file camera from file '%s'", file_camera_path.c_str()); - return zivid_->createFileCamera(file_camera_path); - } - auto cameras = zivid_->cameras(); - RCLCPP_INFO_STREAM(get_logger(), cameras.size() << " camera(s) found"); - - if (cameras.empty()) { - logErrorAndThrowRuntimeException( - "No cameras found. Ensure that the camera is connected to your PC."); - } else if (!serial_number.empty()) { - RCLCPP_INFO( - get_logger(), "Searching for camera with serial number '%s' ...", serial_number.c_str()); - for (auto & c : cameras) { - if (c.info().serialNumber() == Zivid::CameraInfo::SerialNumber(serial_number)) { - return c; - } - } - logErrorAndThrowRuntimeException( - "No camera found with serial number '" + serial_number + "'"); - } - RCLCPP_INFO(get_logger(), "Selecting first available camera"); + camera_ = std::make_unique([&]() { + if (file_camera_mode) { + RCLCPP_INFO(get_logger(), "Creating file camera from file '%s'", file_camera_path.c_str()); + return zivid_->createFileCamera(file_camera_path); + } + auto cameras = zivid_->cameras(); + RCLCPP_INFO_STREAM(get_logger(), cameras.size() << " camera(s) found"); + + if (cameras.empty()) { + logErrorAndThrowRuntimeException( + "No cameras found. Ensure that the camera is connected to your PC."); + } else if (!serial_number.empty()) { + RCLCPP_INFO( + get_logger(), "Searching for camera with serial number '%s' ...", serial_number.c_str()); for (auto & c : cameras) { - if (c.state().isAvailable()) { + if (c.info().serialNumber() == Zivid::CameraInfo::SerialNumber(serial_number)) { return c; } } logErrorAndThrowRuntimeException( - "No available cameras found! Use ZividListCameras or ZividStudio to see all connected " - "cameras and their status."); - }()); + "No camera found with serial number '" + serial_number + "'"); + } + RCLCPP_INFO(get_logger(), "Selecting first available camera"); + for (auto & c : cameras) { + if (c.state().isAvailable()) { + return c; + } + } + logErrorAndThrowRuntimeException( + "No available cameras found! Use ZividListCameras or ZividStudio to see all connected " + "cameras and their status."); + }()); if (!Zivid::Firmware::isUpToDate(*camera_)) { if (update_firmware_automatically) { @@ -418,8 +412,8 @@ ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) camera_info_serial_number_service_ = create_service( - "camera_info/serial_number", - std::bind(&ZividCamera::cameraInfoSerialNumberServiceHandler, this, _1, _2, _3)); + "camera_info/serial_number", + std::bind(&ZividCamera::cameraInfoSerialNumberServiceHandler, this, _1, _2, _3)); is_connected_service_ = create_service( "is_connected", std::bind(&ZividCamera::isConnectedServiceHandler, this, _1, _2, _3)); @@ -435,18 +429,15 @@ ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) capture_assistant_suggest_settings_service_ = create_service( - "capture_assistant/suggest_settings", - std::bind(&ZividCamera::captureAssistantSuggestSettingsServiceHandler, this, _1, _2, _3)); + "capture_assistant/suggest_settings", + std::bind(&ZividCamera::captureAssistantSuggestSettingsServiceHandler, this, _1, _2, _3)); RCLCPP_INFO(get_logger(), "Zivid camera driver is now ready!"); } ZividCamera::~ZividCamera() = default; -Zivid::Application & ZividCamera::zividApplication() -{ - return *zivid_; -} +Zivid::Application & ZividCamera::zividApplication() { return *zivid_; } void ZividCamera::onCameraConnectionKeepAliveTimeout() { @@ -523,10 +514,10 @@ void ZividCamera::captureServiceHandler( RCLCPP_INFO_STREAM(get_logger(), __func__); const auto settings = runFunctionAndCatchExceptionsAndRethrow( - [&] {return settings_controller_->currentSettings();}, get_logger()); + [&] { return settings_controller_->currentSettings(); }, get_logger()); runFunctionAndCatchExceptions( - [&]() {invokeCaptureAndPublishFrame(settings);}, response, get_logger(), "Capture"); + [&]() { invokeCaptureAndPublishFrame(settings); }, response, get_logger(), "Capture"); } void ZividCamera::captureAndSaveServiceHandler( @@ -536,7 +527,7 @@ void ZividCamera::captureAndSaveServiceHandler( { RCLCPP_INFO_STREAM(get_logger(), __func__); const auto settings = runFunctionAndCatchExceptionsAndRethrow( - [&] {return settings_controller_->currentSettings();}, get_logger()); + [&] { return settings_controller_->currentSettings(); }, get_logger()); runFunctionAndCatchExceptions( [&]() { @@ -557,7 +548,7 @@ void ZividCamera::capture2DServiceHandler( serviceHandlerHandleCameraConnectionLoss(); const auto settings2D = runFunctionAndCatchExceptionsAndRethrow( - [&] {return settings_2d_controller_->currentSettings();}, get_logger()); + [&] { return settings_2d_controller_->currentSettings(); }, get_logger()); runFunctionAndCatchExceptions( [&]() { @@ -587,33 +578,33 @@ void ZividCamera::captureAssistantSuggestSettingsServiceHandler( const auto max_capture_time = rclcpp::Duration{request->max_capture_time}.to_chrono(); const auto ambient_light_frequency = [this, &request]() { - using RosRequestTypes = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; - switch (request->ambient_light_frequency) { - case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_NONE: - return SuggestSettingsParameters::AmbientLightFrequency::none; - case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_50HZ: - return SuggestSettingsParameters::AmbientLightFrequency::hz50; - case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_60HZ: - return SuggestSettingsParameters::AmbientLightFrequency::hz60; - } - logErrorAndThrowRuntimeException( - "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + - std::to_string(request->ambient_light_frequency)); - }(); + using RosRequestTypes = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; + switch (request->ambient_light_frequency) { + case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_NONE: + return SuggestSettingsParameters::AmbientLightFrequency::none; + case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_50HZ: + return SuggestSettingsParameters::AmbientLightFrequency::hz50; + case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_60HZ: + return SuggestSettingsParameters::AmbientLightFrequency::hz60; + } + logErrorAndThrowRuntimeException( + "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + + std::to_string(request->ambient_light_frequency)); + }(); SuggestSettingsParameters suggest_settings_parameters{ SuggestSettingsParameters::MaxCaptureTime{max_capture_time}, ambient_light_frequency}; RCLCPP_INFO_STREAM( - get_logger(), - "Getting suggested settings using parameters: " << suggest_settings_parameters); + get_logger(), "Getting suggested settings using parameters: " << suggest_settings_parameters); const auto suggested_settings = runFunctionAndCatchExceptionsAndRethrow( [&]() { return Zivid::CaptureAssistant::suggestSettings(*camera_, suggest_settings_parameters); - }, get_logger()); + }, + get_logger()); RCLCPP_INFO_STREAM( get_logger(), "CaptureAssistant::suggestSettings returned " - << suggested_settings.acquisitions().size() << " acquisitions"); + << suggested_settings.acquisitions().size() << " acquisitions"); settings_controller_->setSettings(suggested_settings); response->suggested_settings = serializeZividDataModel(suggested_settings); @@ -649,8 +640,7 @@ void ZividCamera::publishFrame(const Zivid::Frame & frame) if ( publish_points_xyz || publish_points_xyzrgba || publish_color_img || publish_depth_img || - publish_snr_img || publish_normals_xyz) - { + publish_snr_img || publish_normals_xyz) { const auto header = makeHeader(); auto point_cloud = frame.pointCloud(); diff --git a/zivid_camera/test/test_zivid_camera.cpp b/zivid_camera/test/test_zivid_camera.cpp index 9b4563d..e99ef50 100644 --- a/zivid_camera/test/test_zivid_camera.cpp +++ b/zivid_camera/test/test_zivid_camera.cpp @@ -36,18 +36,17 @@ #include #include - -#include #include +#include +#include +#include +#include +#include #include #include #include #include #include -#include -#include -#include -#include namespace { @@ -72,20 +71,11 @@ class TmpFile TmpFile & operator=(const TmpFile &) = delete; TmpFile & operator=(TmpFile &&) = delete; - ~TmpFile() - { - std::filesystem::remove(m_path); - } + ~TmpFile() { std::filesystem::remove(m_path); } - const std::filesystem::path & path() const - { - return m_path; - } + const std::filesystem::path & path() const { return m_path; } - std::string string() const - { - return m_path.string(); - } + std::string string() const { return m_path.string(); } private: std::filesystem::path m_path; @@ -110,10 +100,7 @@ class ZividNodeTestBase : public testing::Test std::cerr << "End of test " << test_name << "\n"; } - void printLine() - { - std::cerr << std::string(80, '-') << "\n"; - } + void printLine() { std::cerr << std::string(80, '-') << "\n"; } }; class ZividNodeTest : public ZividNodeTestBase @@ -148,8 +135,7 @@ class ZividNodeTest : public ZividNodeTestBase static constexpr auto parameter_settings_2d_file_path = "settings_2d_file_path"; static constexpr auto parameter_settings_2d_yaml = "settings_2d_yaml"; - ZividNodeTest() - : test_node_(rclcpp::Node::make_shared("test_node")) + ZividNodeTest() : test_node_(rclcpp::Node::make_shared("test_node")) { executor_.add_node(test_node_); executor_.add_node(zivid_ros_node); @@ -161,7 +147,7 @@ class ZividNodeTest : public ZividNodeTestBase setNodeParameter(parameter_settings_2d_yaml, ""); } - template + template decltype(auto) doSrvRequest( const std::string & service, std::shared_ptr request, std::chrono::milliseconds timeout = default_service_timeout) @@ -174,17 +160,16 @@ class ZividNodeTest : public ZividNodeTestBase auto future = client->async_send_request(request); if ( - executor_.spin_until_future_complete(future, timeout) != rclcpp::FutureReturnCode::SUCCESS) - { + executor_.spin_until_future_complete(future, timeout) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "Failed to call service " << service); throw std::runtime_error( - "Failed to invoke service '" + service + "'. No response within timeout."); + "Failed to invoke service '" + service + "'. No response within timeout."); } return future.get(); } - template + template decltype(auto) doEmptySrvRequest( const std::string & service, std::chrono::milliseconds timeout = default_service_timeout) { @@ -198,34 +183,27 @@ class ZividNodeTest : public ZividNodeTestBase return doEmptySrvRequest(service, timeout); } - template + template class SubscriptionWrapper { -public: + public: static SubscriptionWrapper make(rclcpp::Node::SharedPtr node, const std::string & topic) { auto w = SubscriptionWrapper(); std::function &)> cb = [&](auto msg) { - w.impl_->num_messages_++; - w.impl_->last_message_ = msg; - }; + w.impl_->num_messages_++; + w.impl_->last_message_ = msg; + }; w.impl_->subscription_ = node->create_subscription(topic, 10, cb); return w; } - decltype(auto) lastMessage() const - { - return impl_->last_message_; - } + decltype(auto) lastMessage() const { return impl_->last_message_; } - std::size_t numMessages() const - { - return impl_->num_messages_; - } + std::size_t numMessages() const { return impl_->num_messages_; } -private: - SubscriptionWrapper() - : impl_{std::make_unique()} {} + private: + SubscriptionWrapper() : impl_{std::make_unique()} {} struct Impl { @@ -315,7 +293,7 @@ class ZividNodeTest : public ZividNodeTestBase return doCapture2DUsingFilePath(defaultSingleAcquisitionSettings2DYml()); } - template + template auto serializeZividDataModel(const ZividDataModel & dm) { #if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) @@ -327,7 +305,7 @@ class ZividNodeTest : public ZividNodeTestBase #endif } - template + template auto deserializeZividDataModel(const std::string & serialized) { #if (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR <= 12) @@ -341,28 +319,28 @@ class ZividNodeTest : public ZividNodeTestBase #endif } - template + template auto verifyTriggerResponseSuccess(const ResponseSharedPtr & response) { ASSERT_TRUE(response->success); ASSERT_EQ(response->message, ""); } - template + template auto verifyTriggerResponseError(const ResponseSharedPtr & response) { ASSERT_FALSE(response->success); ASSERT_NE(response->message, ""); } - template + template auto verifyTriggerResponseError(const ResponseSharedPtr & response, const std::string & message) { verifyTriggerResponseError(response); ASSERT_EQ(response->message, message); } - template + template SubscriptionWrapper subscribe(const std::string & topic) { return SubscriptionWrapper::make(test_node_, topic); @@ -370,7 +348,7 @@ class ZividNodeTest : public ZividNodeTestBase class AllCaptureTopicsSubscriber { -public: + public: explicit AllCaptureTopicsSubscriber(ZividNodeTest & nodeTest) : color_camera_info_sub_( nodeTest.subscribe(color_camera_info_topic_name)), @@ -415,7 +393,7 @@ class ZividNodeTest : public ZividNodeTestBase class AllCapture2DTopicsSubscriber { -public: + public: explicit AllCapture2DTopicsSubscriber(ZividNodeTest & node_test) : color_camera_info_sub_( node_test.subscribe(color_camera_info_topic_name)), @@ -434,10 +412,9 @@ class ZividNodeTest : public ZividNodeTestBase } }; - template + template void assertArrayDoubleEq( - const std::array & actual, - const std::array & expected) const + const std::array & actual, const std::array & expected) const { for (std::size_t i = 0; i < N; i++) { EXPECT_DOUBLE_EQ(actual[i], expected[i]); @@ -485,7 +462,7 @@ class ZividNodeTest : public ZividNodeTestBase } } - template + template void assertPointCloud2Field( const FieldType & field, const std::string & name, uint32_t offset, uint32_t datatype, uint32_t count) @@ -507,8 +484,8 @@ class ZividNodeTest : public ZividNodeTestBase // [ 0 0 1] assertArrayDoubleEq( ci.k, - std::array{1781.447998046875, 0, 990.49267578125, 0, 1781.5296630859375, - 585.81781005859375, 0, 0, 1}); + std::array{ + 1781.447998046875, 0, 990.49267578125, 0, 1781.5296630859375, 585.81781005859375, 0, 0, 1}); // R = I assertArrayDoubleEq(ci.r, std::array{1, 0, 0, 0, 1, 0, 0, 0, 1}); @@ -517,9 +494,9 @@ class ZividNodeTest : public ZividNodeTestBase // P = [ 0 fy' cy' Ty] // [ 0 0 1 0] assertArrayDoubleEq( - ci.p, - std::array{1781.447998046875, 0, 990.49267578125, 0, 0, 1781.5296630859375, - 585.81781005859375, 0, 0, 0, 1, 0}); + ci.p, std::array{ + 1781.447998046875, 0, 990.49267578125, 0, 0, 1781.5296630859375, 585.81781005859375, + 0, 0, 0, 1, 0}); } }; @@ -562,42 +539,42 @@ TEST_F(ZividNodeTest, testServiceIsConnected) TEST_F(ZividNodeTest, testCaptureConfigurationThrowsIfBothPathAndYmlSet) { auto run_test = [&]( - const auto & service_name, const auto & path_param, const auto & yml_param, - const auto & yml_content) { - auto color_image_sub = subscribe(color_image_color_topic_name); - auto assert_num_topics_received = [&](auto num_topics) { - ASSERT_EQ(color_image_sub.numMessages(), num_topics); - }; - auto tmp_file = TmpFile("settings.yml", yml_content); - setNodeParameter(path_param, tmp_file.string()); - setNodeParameter(yml_param, yml_content); - ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); - assert_num_topics_received(0); - - setNodeParameter(path_param, ""); - ASSERT_TRUE(doStdSrvsTriggerRequest(service_name)); - executor_.spin_some(); - assert_num_topics_received(1); - - setNodeParameter(path_param, tmp_file.string()); - setNodeParameter(yml_param, ""); - ASSERT_TRUE(doStdSrvsTriggerRequest(service_name)); - executor_.spin_some(); - assert_num_topics_received(2); - - setNodeParameter(path_param, tmp_file.string()); - setNodeParameter(yml_param, yml_content); - ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); - executor_.spin_some(); - assert_num_topics_received(2); - - // Reset for next test - setNodeParameter(path_param, ""); - setNodeParameter(yml_param, ""); - ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); - executor_.spin_some(); - assert_num_topics_received(2); + const auto & service_name, const auto & path_param, const auto & yml_param, + const auto & yml_content) { + auto color_image_sub = subscribe(color_image_color_topic_name); + auto assert_num_topics_received = [&](auto num_topics) { + ASSERT_EQ(color_image_sub.numMessages(), num_topics); }; + auto tmp_file = TmpFile("settings.yml", yml_content); + setNodeParameter(path_param, tmp_file.string()); + setNodeParameter(yml_param, yml_content); + ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); + assert_num_topics_received(0); + + setNodeParameter(path_param, ""); + ASSERT_TRUE(doStdSrvsTriggerRequest(service_name)); + executor_.spin_some(); + assert_num_topics_received(1); + + setNodeParameter(path_param, tmp_file.string()); + setNodeParameter(yml_param, ""); + ASSERT_TRUE(doStdSrvsTriggerRequest(service_name)); + executor_.spin_some(); + assert_num_topics_received(2); + + setNodeParameter(path_param, tmp_file.string()); + setNodeParameter(yml_param, yml_content); + ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); + executor_.spin_some(); + assert_num_topics_received(2); + + // Reset for next test + setNodeParameter(path_param, ""); + setNodeParameter(yml_param, ""); + ASSERT_THROW(doStdSrvsTriggerRequest(service_name), std::exception); + executor_.spin_some(); + assert_num_topics_received(2); + }; run_test( capture_service_name, parameter_settings_file_path, parameter_settings_yaml, @@ -737,8 +714,8 @@ class CaptureOutputTest : public TestWithFileCamera Zivid::PointCloud captureViaSDKDefaultSettings() { return camera_ - .capture(Zivid::Settings{Zivid::Settings::Acquisitions{Zivid::Settings::Acquisition{}}}) - .pointCloud(); + .capture(Zivid::Settings{Zivid::Settings::Acquisitions{Zivid::Settings::Acquisition{}}}) + .pointCloud(); } Zivid::PointCloud captureViaSDK(const Zivid::Settings & settings) @@ -799,9 +776,9 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZGBA) comparePointCoordinate(y, expected.point.y); comparePointCoordinate(z, expected.point.z); const auto expected_argb = static_cast(expected.color.a << 24) | - static_cast(expected.color.r << 16) | - static_cast(expected.color.g << 8) | - static_cast(expected.color.b); + static_cast(expected.color.r << 16) | + static_cast(expected.color.g << 8) | + static_cast(expected.color.b); ASSERT_EQ(argb, expected_argb); } } @@ -868,8 +845,7 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) const auto & point_cloud = points_sub.lastMessage(); ASSERT_TRUE(point_cloud); - const auto point_cloud_sdk = captureViaSDK( - Zivid::Settings{ + const auto point_cloud_sdk = captureViaSDK(Zivid::Settings{ Zivid::Settings::Acquisitions{Zivid::Settings::Acquisition{}}, Zivid::Settings::RegionOfInterest::Box{ Zivid::Settings::RegionOfInterest::Box::Enabled::yes, @@ -882,12 +858,12 @@ TEST_F(CaptureOutputTest, testCapturePointsXYZWithROI) auto expected = point_cloud_sdk.copyData(); const auto num_z_nan = [&] { - size_t count = 0; - for (size_t i = 0; i < expected.size(); ++i) { - count += std::isnan(expected(i).z); - } - return count; - }(); + size_t count = 0; + for (size_t i = 0; i < expected.size(); ++i) { + count += std::isnan(expected(i).z); + } + return count; + }(); // Verify that we have some number of points left (to verify that the ROI box did // not set everything to NaN) ASSERT_GT(num_z_nan, 500000); @@ -1054,10 +1030,10 @@ class Capture2DOutputTest : public CaptureOutputTest camera_.capture(deserializeZividDataModel(settings_yaml)); auto verify_image_and_camera_info = [&](const auto & img, const auto & info) { - assertCameraInfoForFileCamera(info); - assertSensorMsgsImageMeta(img, 1944U, 1200U, 4U, "rgba8"); - assertSensorMsgsImageContents(img, frame_2d_from_sdk.imageRGBA()); - }; + assertCameraInfoForFileCamera(info); + assertSensorMsgsImageMeta(img, 1944U, 1200U, 4U, "rgba8"); + assertSensorMsgsImageContents(img, frame_2d_from_sdk.imageRGBA()); + }; verify_image_and_camera_info( *all_capture_2d_topics_subscriber.color_image_color_sub_.lastMessage(), @@ -1144,10 +1120,7 @@ class CaptureAndSaveTest : public TestWithFileCamera } }; -TEST_F(CaptureAndSaveTest, testCaptureAndSaveEmptyPathProvided) -{ - captureAndSaveToPath("", false); -} +TEST_F(CaptureAndSaveTest, testCaptureAndSaveEmptyPathProvided) { captureAndSaveToPath("", false); } TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidPath) { @@ -1159,20 +1132,11 @@ TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidExtension) captureAndSaveToPath("/tmp/invalid_extension.wrong", false); } -TEST_F(CaptureAndSaveTest, testCaptureAndSaveZDF) -{ - captureAndSaveToPath("/tmp/valid.zdf", true); -} +TEST_F(CaptureAndSaveTest, testCaptureAndSaveZDF) { captureAndSaveToPath("/tmp/valid.zdf", true); } -TEST_F(CaptureAndSaveTest, testCaptureAndSavePLY) -{ - captureAndSaveToPath("/tmp/valid.ply", true); -} +TEST_F(CaptureAndSaveTest, testCaptureAndSavePLY) { captureAndSaveToPath("/tmp/valid.ply", true); } -TEST_F(CaptureAndSaveTest, testCaptureAndSavePCD) -{ - captureAndSaveToPath("/tmp/valid.pcd", true); -} +TEST_F(CaptureAndSaveTest, testCaptureAndSavePCD) { captureAndSaveToPath("/tmp/valid.pcd", true); } class ZividCATest : public CaptureOutputTest { @@ -1192,8 +1156,7 @@ class ZividCATest : public CaptureOutputTest return AmbientLightFrequency::hz60; } throw std::runtime_error( - "Could not convert value " + std::to_string( - ambient_light_frequency) + " to Zivid API enum."); + "Could not convert value " + std::to_string(ambient_light_frequency) + " to Zivid API enum."); } decltype(auto) doCaptureAssistantRequest( @@ -1252,13 +1215,11 @@ TEST_F(ZividCATest, testDifferentMaxCaptureTimeAndAmbientLightFrequency) { using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; for (std::chrono::milliseconds max_capture_time : - {std::chrono::milliseconds{200}, std::chrono::milliseconds{1'200}, - std::chrono::milliseconds{10'000}}) - { + {std::chrono::milliseconds{200}, std::chrono::milliseconds{1'200}, + std::chrono::milliseconds{10'000}}) { for (auto ambient_light_frequency : - {Request::AMBIENT_LIGHT_FREQUENCY_NONE, Request::AMBIENT_LIGHT_FREQUENCY_50HZ, - Request::AMBIENT_LIGHT_FREQUENCY_60HZ}) - { + {Request::AMBIENT_LIGHT_FREQUENCY_NONE, Request::AMBIENT_LIGHT_FREQUENCY_50HZ, + Request::AMBIENT_LIGHT_FREQUENCY_60HZ}) { performSuggestSettingsAndCompareWithCppAPI(max_capture_time, ambient_light_frequency); } } @@ -1299,9 +1260,8 @@ TEST_F(ZividCATest, testCaptureAssistantDefaultAmbientLightFrequencyWorks) using Request = zivid_interfaces::srv::CaptureAssistantSuggestSettings::Request; auto request = std::make_shared(); request->max_capture_time = rclcpp::Duration{std::chrono::seconds{1}}; - ASSERT_TRUE( - doSrvRequest( - capture_assistant_suggest_settings_service_name, request)); + ASSERT_TRUE(doSrvRequest( + capture_assistant_suggest_settings_service_name, request)); } TEST_F(ZividCATest, testCaptureAssistantInvalidAmbientLightFrequencyFails) diff --git a/zivid_samples/package.xml b/zivid_samples/package.xml index 349ea59..7b41e0b 100644 --- a/zivid_samples/package.xml +++ b/zivid_samples/package.xml @@ -22,12 +22,11 @@ rclpy ament_lint_auto + ament_cmake_clang_format ament_cmake_cppcheck - ament_cmake_cpplint ament_cmake_flake8 ament_cmake_lint_cmake ament_cmake_pep257 - ament_cmake_uncrustify ament_cmake_xmllint diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 4d6c7f8..e7ebcb1 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -1,7 +1,5 @@ #include - #include - #include #include #include @@ -74,17 +72,14 @@ int main(int argc, char * argv[]) auto capture_client = create_capture_client(node); auto trigger_capture = [&]() { - RCLCPP_INFO(node->get_logger(), "Triggering capture"); - capture_client->async_send_request(std::make_shared()); - }; + RCLCPP_INFO(node->get_logger(), "Triggering capture"); + capture_client->async_send_request(std::make_shared()); + }; - auto points_xyzrgba_subscription = - node->create_subscription( - "points/xyzrgba", 10, [&]( - sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { + auto points_xyzrgba_subscription = node->create_subscription( + "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { RCLCPP_INFO( - node->get_logger(), "Received point cloud of size %d x %d", - msg->width, msg->height); + node->get_logger(), "Received point cloud of size %d x %d", msg->width, msg->height); trigger_capture(); }); diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index 34e4566..6d3e53a 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -1,7 +1,5 @@ #include - #include - #include #include @@ -37,8 +35,8 @@ void set_settings_2d(const std::shared_ptr & node) RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); } - auto result = param_client->set_parameters( - {rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}); + auto result = + param_client->set_parameters({rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_2d_yaml` parameter"); std::terminate(); @@ -70,9 +68,9 @@ int main(int argc, char * argv[]) auto capture_2d_client = create_capture_2d_client(node); auto trigger_capture = [&]() { - RCLCPP_INFO(node->get_logger(), "Triggering 2d capture"); - capture_2d_client->async_send_request(std::make_shared()); - }; + RCLCPP_INFO(node->get_logger(), "Triggering 2d capture"); + capture_2d_client->async_send_request(std::make_shared()); + }; auto color_image_color_subscription = node->create_subscription( "color/image_color", 10, [&](sensor_msgs::msg::Image::ConstSharedPtr msg) -> void { diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 9151b1c..729885b 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -1,8 +1,6 @@ #include #include - #include - #include /* diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 6295667..31d91ba 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -1,14 +1,10 @@ #include - #include - #include #include #include - #include - /* * This sample shows how to use the capture assistant service to suggest and set the capture * settings parameter of the zivid node. Then, it shows how to subscribe to the points/xyzrgba and @@ -19,8 +15,8 @@ void capture_assistant_suggest_settings(const std::shared_ptr & no { using zivid_interfaces::srv::CaptureAssistantSuggestSettings; - auto client = node->create_client( - "capture_assistant/suggest_settings"); + auto client = + node->create_client("capture_assistant/suggest_settings"); while (!client->wait_for_service(std::chrono::seconds(3))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); @@ -53,8 +49,8 @@ void capture(const std::shared_ptr & node) } RCLCPP_INFO(node->get_logger(), "Triggering capture"); - auto result = capture_client->async_send_request( - std::make_shared()); + auto result = + capture_client->async_send_request(std::make_shared()); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to trigger capture"); @@ -71,8 +67,7 @@ int main(int argc, char * argv[]) auto points_xyzrgba_subscription = node->create_subscription( "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { RCLCPP_INFO( - node->get_logger(), "Received point cloud of size %d x %d", msg->width, - msg->height); + node->get_logger(), "Received point cloud of size %d x %d", msg->width, msg->height); }); auto color_image_color_subscription = node->create_subscription( diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp index 4a1a4a6..0feceb7 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_file.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -1,8 +1,6 @@ +#include #include - #include - -#include #include #include #include @@ -30,8 +28,8 @@ void set_settings(const std::shared_ptr & node) RCLCPP_INFO(node->get_logger(), "Waiting for the parameters client to appear..."); } - auto result = param_client->set_parameters( - {rclcpp::Parameter("settings_file_path", path_to_settings_yml)}); + auto result = + param_client->set_parameters({rclcpp::Parameter("settings_file_path", path_to_settings_yml)}); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_file_path` parameter"); std::terminate(); @@ -62,8 +60,7 @@ int main(int argc, char * argv[]) auto points_xyzrgba_subscription = node->create_subscription( "points/xyzrgba", 10, [&](sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -> void { RCLCPP_INFO( - node->get_logger(), "Received point cloud of size %d x %d", msg->width, - msg->height); + node->get_logger(), "Received point cloud of size %d x %d", msg->width, msg->height); }); set_settings(node); From e8c592c3803f5bd065a25196a89096d53fc8c797 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Tue, 16 Jul 2024 13:35:12 +0200 Subject: [PATCH 15/33] Remove g++-14 from test matrix This version is not available on the Ubuntu toolchain test apt repository, and will cause failures when trying to build with it. --- .github/workflows/ROS-commit.yml | 4 ---- continuous-integration/setup/setup_build_and_test.sh | 3 +-- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/.github/workflows/ROS-commit.yml b/.github/workflows/ROS-commit.yml index a56940d..58ce4df 100644 --- a/.github/workflows/ROS-commit.yml +++ b/.github/workflows/ROS-commit.yml @@ -41,12 +41,8 @@ jobs: compiler: 'g++-12' - os: 'ros:humble-ros-base-jammy' compiler: 'g++-13' - - os: 'ros:humble-ros-base-jammy' - compiler: 'g++-14' - os: 'ros:humble-ros-base-jammy' compiler: 'clang++-15' - - os: 'ros:jazzy-ros-core-noble' - compiler: 'g++-14' - os: 'ros:jazzy-ros-core-noble' compiler: 'clang++-14' - os: 'ros:jazzy-ros-core-noble' diff --git a/continuous-integration/setup/setup_build_and_test.sh b/continuous-integration/setup/setup_build_and_test.sh index 66aac65..a37f4a4 100755 --- a/continuous-integration/setup/setup_build_and_test.sh +++ b/continuous-integration/setup/setup_build_and_test.sh @@ -56,8 +56,7 @@ echo "Installing compiler $CI_TEST_COMPILER" if [[ "$CI_TEST_COMPILER" == "g++" || "$CI_TEST_COMPILER" == "g++-12" || - "$CI_TEST_COMPILER" == "g++-13" || - "$CI_TEST_COMPILER" == "g++-14" + "$CI_TEST_COMPILER" == "g++-13" ]]; then apt-yes install software-properties-common || exit $? From 0d392ad2da1a09a89adff4d4898169b44e4f0437 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Tue, 16 Jul 2024 11:41:55 +0200 Subject: [PATCH 16/33] Fix building with other compilers The flag `CMAKE_CXX_COMPILER` was not previously passed to CMake due to the double quotes in the `build.sh` script. This implies that we didn't previously build with clang. Building with clang started emitting some warnings, causing errors. These warnings have been addressed here as well. Also removed warning `return-std-move-in-c++11` as this was removed in Clang 13 and now causing a compile error: https://releases.llvm.org/13.0.0/tools/clang/docs/ReleaseNotes.html --- continuous-integration/build.sh | 2 +- zivid_camera/cmake/CompilerWarnings.cmake | 3 -- .../include/zivid_camera/zivid_camera.hpp | 2 +- zivid_camera/src/zivid_camera.cpp | 34 ++++++++++++++----- 4 files changed, 27 insertions(+), 14 deletions(-) diff --git a/continuous-integration/build.sh b/continuous-integration/build.sh index 373d654..a7946e6 100755 --- a/continuous-integration/build.sh +++ b/continuous-integration/build.sh @@ -25,7 +25,7 @@ rosdep install -i --from-path src -y || exit $? echo "Building with compiler=$CI_TEST_COMPILER" -colcon build --symlink --cmake-args "-DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER" || exit $? +colcon build --symlink --cmake-args -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER || exit $? echo "Check that the expected packages are found" source ~/ros2_ws/install/setup.bash diff --git a/zivid_camera/cmake/CompilerWarnings.cmake b/zivid_camera/cmake/CompilerWarnings.cmake index 0045a9b..ebf09fc 100644 --- a/zivid_camera/cmake/CompilerWarnings.cmake +++ b/zivid_camera/cmake/CompilerWarnings.cmake @@ -39,9 +39,6 @@ function(set_target_warning_compile_options TARGET) # problem, maybe even linker will resolve this. Must add boilerplate to # fix, not worth it ) - if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 7.0) - list(APPEND WARNINGS_THAT_SHOULD_BE_IGNORED return-std-move-in-c++11) # Applies to old compilers, we aim for newer C++17 compilers - endif() foreach(WARNING ${WARNINGS_THAT_SHOULD_BE_IGNORED}) list(APPEND TARGET_FLAGS -Wno-${WARNING}) diff --git a/zivid_camera/include/zivid_camera/zivid_camera.hpp b/zivid_camera/include/zivid_camera/zivid_camera.hpp index f23453f..506dbe6 100644 --- a/zivid_camera/include/zivid_camera/zivid_camera.hpp +++ b/zivid_camera/include/zivid_camera/zivid_camera.hpp @@ -71,7 +71,7 @@ class ZividCamera : public rclcpp::Node { public: ZIVID_CAMERA_ROS_PUBLIC ZividCamera(const rclcpp::NodeOptions & options); - ~ZividCamera(); + ~ZividCamera() override; ZIVID_CAMERA_ROS_PUBLIC Zivid::Application & zividApplication(); private: diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 6525ca2..09b68b5 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -47,6 +47,11 @@ namespace { +template +struct DependentFalse : std::false_type +{ +}; + sensor_msgs::msg::PointField createPointField( std::string name, uint32_t offset, uint8_t datatype, uint32_t count) { @@ -183,7 +188,7 @@ template [[noreturn]] void logErrorToLoggerAndThrowRuntimeException( const Logger & logger, const std::string & message) { - RCLCPP_ERROR(logger, message.c_str()); + RCLCPP_ERROR(logger, "%s", message.c_str()); throw std::runtime_error(message); } @@ -258,12 +263,12 @@ class CaptureSettingsController constexpr auto baseName() const { - if (std::is_same_v) { + if constexpr (std::is_same_v) { return "settings"; - } else if (std::is_same_v) { + } else if constexpr (std::is_same_v) { return "settings_2d"; } else { - static_assert("Unhandled SettingsType"); + static_assert(DependentFalse::value, "Unhandled node type"); } } @@ -282,7 +287,7 @@ ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) settings_2d_controller_{std::make_unique>(*this)} { // Disable buffering on stdout - setvbuf(stdout, NULL, _IONBF, BUFSIZ); + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); RCLCPP_INFO_STREAM(get_logger(), "Zivid ROS driver"); // Fix: add version RCLCPP_INFO(get_logger(), "The node's namespace is '%s'", get_namespace()); @@ -514,10 +519,10 @@ void ZividCamera::captureServiceHandler( RCLCPP_INFO_STREAM(get_logger(), __func__); const auto settings = runFunctionAndCatchExceptionsAndRethrow( - [&] { return settings_controller_->currentSettings(); }, get_logger()); + [&] { return settings_controller_->currentSettings(); }, get_logger()); runFunctionAndCatchExceptions( - [&]() { invokeCaptureAndPublishFrame(settings); }, response, get_logger(), "Capture"); + [&]() { invokeCaptureAndPublishFrame(settings); }, response, get_logger(), "Capture"); } void ZividCamera::captureAndSaveServiceHandler( @@ -527,7 +532,7 @@ void ZividCamera::captureAndSaveServiceHandler( { RCLCPP_INFO_STREAM(get_logger(), __func__); const auto settings = runFunctionAndCatchExceptionsAndRethrow( - [&] { return settings_controller_->currentSettings(); }, get_logger()); + [&] { return settings_controller_->currentSettings(); }, get_logger()); runFunctionAndCatchExceptions( [&]() { @@ -548,7 +553,7 @@ void ZividCamera::capture2DServiceHandler( serviceHandlerHandleCameraConnectionLoss(); const auto settings2D = runFunctionAndCatchExceptionsAndRethrow( - [&] { return settings_2d_controller_->currentSettings(); }, get_logger()); + [&] { return settings_2d_controller_->currentSettings(); }, get_logger()); runFunctionAndCatchExceptions( [&]() { @@ -907,4 +912,15 @@ void ZividCamera::logErrorAndThrowRuntimeException(const std::string & message) } // namespace zivid_camera #include "rclcpp_components/register_node_macro.hpp" + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wglobal-constructors" +#pragma clang diagnostic ignored "-Wexit-time-destructors" +#endif + RCLCPP_COMPONENTS_REGISTER_NODE(zivid_camera::ZividCamera) + +#ifdef __clang__ +#pragma clang diagnostic pop +#endif From 8a5e65e25cb77a8a3b1ce4a8d12447c8e1c5fb6d Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Tue, 16 Jul 2024 15:58:28 +0200 Subject: [PATCH 17/33] Add `ament_clang_tidy` and fix resulting issues Only run clang-tidy on Jazzy, as earlier versions cause issues. --- continuous-integration/build.sh | 2 +- .../run_build_and_test_in_docker.sh | 1 + continuous-integration/test.sh | 10 ++++++++ zivid_camera/cmake/CompilerWarnings.cmake | 1 + zivid_camera/package.xml | 1 + zivid_camera/src/zivid_camera.cpp | 23 +++++++++++++++---- zivid_samples/package.xml | 1 + 7 files changed, 34 insertions(+), 5 deletions(-) diff --git a/continuous-integration/build.sh b/continuous-integration/build.sh index a7946e6..3c7b89a 100755 --- a/continuous-integration/build.sh +++ b/continuous-integration/build.sh @@ -25,7 +25,7 @@ rosdep install -i --from-path src -y || exit $? echo "Building with compiler=$CI_TEST_COMPILER" -colcon build --symlink --cmake-args -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER || exit $? +colcon build --symlink --event-handlers console_direct+ --cmake-args -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER -DCMAKE_EXPORT_COMPILE_COMMANDS=ON || exit $? echo "Check that the expected packages are found" source ~/ros2_ws/install/setup.bash diff --git a/continuous-integration/run_build_and_test_in_docker.sh b/continuous-integration/run_build_and_test_in_docker.sh index c703205..77ffac0 100755 --- a/continuous-integration/run_build_and_test_in_docker.sh +++ b/continuous-integration/run_build_and_test_in_docker.sh @@ -26,6 +26,7 @@ docker run \ --workdir /host/continuous-integration \ --env CI_TEST_ZIVID_VERSION="$CI_TEST_ZIVID_VERSION" \ --env CI_TEST_COMPILER="$CI_TEST_COMPILER" \ + --env CI_TEST_OS="$CI_TEST_OS" \ --env CI_TEST_DOWNLOAD_TELICAM="$CI_TEST_DOWNLOAD_TELICAM" \ $CI_TEST_OS \ bash -c "./build_and_test.sh" || exit $? diff --git a/continuous-integration/test.sh b/continuous-integration/test.sh index d4febe1..576b87a 100755 --- a/continuous-integration/test.sh +++ b/continuous-integration/test.sh @@ -20,6 +20,16 @@ echo "Running tests" # We exclude `clang_format` here since it has variations between versions, instead we check it during code analysis. excludeTests="clang_format" +if [ "$CI_TEST_OS" == "ros:humble-ros-base-jammy" ] || [ "$CI_TEST_OS" == "ros:iron-ros-core-jammy" ]; then + # The listed OSes have issues invoking clang-tidy correctly: + # - Humble seems to not invoke it with the correct C++ language version, resulting in it not finding + # std::optional and std::variant. + # - Iron does not find the compile commands. + # On the other hand, Jazzy seems to work well, so we get the checks when testing on this OS. + echo "Skipping clang-tidy tests since OS is '$CI_TEST_OS'" + excludeTests+="|clang_tidy" +fi + export GTEST_BREAK_ON_FAILURE=1; colcon test --event-handlers console_direct+ --ctest-args tests --exclude-regex $excludeTests --output-on-failure --ros-args --log-level debug || exit $? diff --git a/zivid_camera/cmake/CompilerWarnings.cmake b/zivid_camera/cmake/CompilerWarnings.cmake index ebf09fc..d849988 100644 --- a/zivid_camera/cmake/CompilerWarnings.cmake +++ b/zivid_camera/cmake/CompilerWarnings.cmake @@ -38,6 +38,7 @@ function(set_target_warning_compile_options TARGET) weak-vtables # The vtable must be duplicated in multiple translation units. Small # problem, maybe even linker will resolve this. Must add boilerplate to # fix, not worth it + covered-switch-default # We don't want this warning, because we want the default labels for safety. ) foreach(WARNING ${WARNINGS_THAT_SHOULD_BE_IGNORED}) diff --git a/zivid_camera/package.xml b/zivid_camera/package.xml index 368b195..bd93988 100644 --- a/zivid_camera/package.xml +++ b/zivid_camera/package.xml @@ -24,6 +24,7 @@ ament_cmake_gtest ament_lint_auto ament_cmake_clang_format + ament_cmake_clang_tidy ament_cmake_copyright ament_cmake_cppcheck ament_cmake_flake8 diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 09b68b5..52c984d 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -125,8 +125,9 @@ std::string toString(zivid_camera::CameraStatus camera_status) return "Disconnected"; case zivid_camera::CameraStatus::Idle: return "Idle"; + default: // NOLINT(clang-diagnostic-covered-switch-default) + return "N/A"; } - return "N/A"; } template @@ -591,10 +592,11 @@ void ZividCamera::captureAssistantSuggestSettingsServiceHandler( return SuggestSettingsParameters::AmbientLightFrequency::hz50; case RosRequestTypes::AMBIENT_LIGHT_FREQUENCY_60HZ: return SuggestSettingsParameters::AmbientLightFrequency::hz60; + default: + logErrorAndThrowRuntimeException( + "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + + std::to_string(request->ambient_light_frequency)); } - logErrorAndThrowRuntimeException( - "Unhandled AMBIENT_LIGHT_FREQUENCY value: " + - std::to_string(request->ambient_light_frequency)); }(); SuggestSettingsParameters suggest_settings_parameters{ @@ -796,7 +798,20 @@ void ZividCamera::publishColorImage( get_logger(), "Publishing " << color_image_publisher_.getTopic() << " from Image"); auto msg = makeImage(header, sensor_msgs::image_encodings::RGBA8, image.width(), image.height()); const auto uint8_ptr_begin = reinterpret_cast(image.data()); + +#ifdef __clang__ +#if __has_warning("-Wunsafe-buffer-usage") +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunsafe-buffer-usage" +#endif +#endif const auto uint8_ptr_end = reinterpret_cast(image.data() + image.size()); +#ifdef __clang__ +#if __has_warning("-Wunsafe-buffer-usage") +#pragma clang diagnostic pop +#endif +#endif + msg->data = std::vector(uint8_ptr_begin, uint8_ptr_end); color_image_publisher_.publish(msg, camera_info); } diff --git a/zivid_samples/package.xml b/zivid_samples/package.xml index 7b41e0b..cede0f7 100644 --- a/zivid_samples/package.xml +++ b/zivid_samples/package.xml @@ -23,6 +23,7 @@ ament_lint_auto ament_cmake_clang_format + ament_cmake_clang_tidy ament_cmake_cppcheck ament_cmake_flake8 ament_cmake_lint_cmake From f869396c42101c1757960c56f7fc624b205cf429 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Wed, 17 Jul 2024 15:35:45 +0200 Subject: [PATCH 18/33] Make Python samples executable using `ros2 launch` --- README.md | 11 ++++++++++- zivid_samples/scripts/sample_capture.py | 2 ++ zivid_samples/scripts/sample_capture_2d.py | 2 ++ zivid_samples/scripts/sample_capture_and_save.py | 2 ++ zivid_samples/scripts/sample_capture_assistant.py | 2 ++ .../scripts/sample_capture_with_settings_from_file.py | 2 ++ 6 files changed, 20 insertions(+), 1 deletion(-) mode change 100644 => 100755 zivid_samples/scripts/sample_capture.py mode change 100644 => 100755 zivid_samples/scripts/sample_capture_2d.py mode change 100644 => 100755 zivid_samples/scripts/sample_capture_and_save.py mode change 100644 => 100755 zivid_samples/scripts/sample_capture_assistant.py mode change 100644 => 100755 zivid_samples/scripts/sample_capture_with_settings_from_file.py diff --git a/README.md b/README.md index 7e1c663..6b75a4c 100644 --- a/README.md +++ b/README.md @@ -343,7 +343,16 @@ will cause some additional processing time for calculating the normals. In the `zivid_samples` package we have added samples that demonstrate how to use the Zivid ROS driver. These samples can be used as a starting point for your project. -_More samples will soon be provided_. +To launch the Python samples using `ros2 launch`, you might need `python` registered as a command, +such as by using: + +```bash +sudo apt install python-is-python3 +``` + +On Windows, the Python samples cannot be launched using `ros2 launch`. Instead, please launch the +samples using `ros2 run zivid_samples .py` together with +`ros2 run zivid_camera zivid_camera` in another terminal window. ### Sample Capture diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py old mode 100644 new mode 100755 index 471ae22..e6f2a72 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + import sys import rclpy diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py old mode 100644 new mode 100755 index 12d3033..4c62f61 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + import sys import rclpy diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py old mode 100644 new mode 100755 index 496a6d0..37aec65 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + import sys import tempfile diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py old mode 100644 new mode 100755 index cb31c35..58ca8aa --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + import sys from builtin_interfaces.msg import Duration diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py old mode 100644 new mode 100755 index b17df7c..936f845 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + import sys from ament_index_python.packages import get_package_share_directory From 9950c8cbe05b5e71ea3da3c64bb203c6e282a24c Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 10:18:50 +0200 Subject: [PATCH 19/33] CMake: Use functions to add samples --- zivid_samples/CMakeLists.txt | 46 +++++++++++++++++------------------- 1 file changed, 22 insertions(+), 24 deletions(-) diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index f82e395..ef7c499 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -11,6 +11,19 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +function(zivid_add_cpp_sample sample_name) + cmake_parse_arguments(PARSE_ARGV 1 ARG "" "" "DEPENDENCIES") + set(target_name "${sample_name}_cpp") + + add_executable(${target_name} "src/${sample_name}.cpp") + ament_target_dependencies(${target_name} ${ARG_DEPENDENCIES}) + install(TARGETS ${target_name} DESTINATION lib/${PROJECT_NAME}) +endfunction() + +function(zivid_add_python_sample sample_name) + install(PROGRAMS "scripts/${sample_name}.py" DESTINATION lib/${PROJECT_NAME}) +endfunction() + find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) @@ -18,31 +31,16 @@ find_package(zivid_interfaces REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) -add_executable(sample_capture_cpp src/sample_capture.cpp) -ament_target_dependencies(sample_capture_cpp rclcpp sensor_msgs std_srvs) -install(TARGETS sample_capture_cpp DESTINATION lib/${PROJECT_NAME}) - -add_executable(sample_capture_2d_cpp src/sample_capture_2d.cpp) -ament_target_dependencies(sample_capture_2d_cpp rclcpp sensor_msgs std_srvs) -install(TARGETS sample_capture_2d_cpp DESTINATION lib/${PROJECT_NAME}) - -add_executable(sample_capture_and_save_cpp src/sample_capture_and_save.cpp) -ament_target_dependencies(sample_capture_and_save_cpp rclcpp zivid_interfaces) -install(TARGETS sample_capture_and_save_cpp DESTINATION lib/${PROJECT_NAME}) - -add_executable(sample_capture_assistant_cpp src/sample_capture_assistant.cpp) -ament_target_dependencies(sample_capture_assistant_cpp rclcpp zivid_interfaces sensor_msgs std_srvs) -install(TARGETS sample_capture_assistant_cpp DESTINATION lib/${PROJECT_NAME}) - -add_executable(sample_capture_with_settings_from_file_cpp src/sample_capture_with_settings_from_file.cpp) -ament_target_dependencies(sample_capture_with_settings_from_file_cpp rclcpp sensor_msgs std_srvs ament_index_cpp) -install(TARGETS sample_capture_with_settings_from_file_cpp DESTINATION lib/${PROJECT_NAME}) +zivid_add_cpp_sample(sample_capture DEPENDENCIES rclcpp sensor_msgs std_srvs) +zivid_add_cpp_sample(sample_capture_2d DEPENDENCIES rclcpp sensor_msgs std_srvs) +zivid_add_cpp_sample(sample_capture_and_save DEPENDENCIES rclcpp zivid_interfaces) +zivid_add_cpp_sample(sample_capture_assistant DEPENDENCIES rclcpp zivid_interfaces sensor_msgs std_srvs) +zivid_add_cpp_sample(sample_capture_with_settings_from_file DEPENDENCIES rclcpp sensor_msgs std_srvs ament_index_cpp) -install(PROGRAMS scripts/sample_capture.py DESTINATION lib/${PROJECT_NAME}) -install(PROGRAMS scripts/sample_capture_2d.py DESTINATION lib/${PROJECT_NAME}) -install(PROGRAMS scripts/sample_capture_and_save.py DESTINATION lib/${PROJECT_NAME}) -install(PROGRAMS scripts/sample_capture_assistant.py DESTINATION lib/${PROJECT_NAME}) -install(PROGRAMS scripts/sample_capture_with_settings_from_file.py DESTINATION lib/${PROJECT_NAME}) +zivid_add_python_sample(sample_capture_2d) +zivid_add_python_sample(sample_capture_and_save) +zivid_add_python_sample(sample_capture_assistant) +zivid_add_python_sample(sample_capture_with_settings_from_file) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) From d56f456e5580abae66926348c78c58be60b2905d Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 10:43:35 +0200 Subject: [PATCH 20/33] Add more context to exception error --- zivid_camera/src/zivid_camera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 52c984d..f093edb 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -180,7 +180,7 @@ auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger return function(); } catch (const Zivid::Exception & exception) { const auto exception_message = Zivid::toString(exception); - RCLCPP_ERROR_STREAM(logger, exception_message); + RCLCPP_ERROR_STREAM(logger, "A function threw a Zivid::Exception: \"" + exception_message + "\""); throw; } } From bf51ea2d4d2a9741a442ca04acbe8a755df766b5 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 10:46:27 +0200 Subject: [PATCH 21/33] Make `settings` method naming consistent between C++ and Python samples --- zivid_samples/scripts/sample_capture.py | 4 ++-- zivid_samples/scripts/sample_capture_2d.py | 4 ++-- zivid_samples/scripts/sample_capture_and_save.py | 4 ++-- .../scripts/sample_capture_with_settings_from_file.py | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index e6f2a72..961f892 100755 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -21,13 +21,13 @@ def __init__(self): while not self.capture_service.wait_for_service(timeout_sec=3.0): self.get_logger().info('Capture service not available, waiting again...') - self._set_capture_settings() + self._set_settings() self.subscription = self.create_subscription( PointCloud2, 'points/xyzrgba', self.on_points, 10 ) - def _set_capture_settings(self): + def _set_settings(self): self.get_logger().info('Setting parameter `settings_yaml`') settings_parameter = Parameter( 'settings_yaml', diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index 4c62f61..e73dd32 100755 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -21,13 +21,13 @@ def __init__(self): while not self.capture_2d_service.wait_for_service(timeout_sec=3.0): self.get_logger().info('Capture service not available, waiting again...') - self._set_capture_settings() + self._set_settings_2d() self.subscription = self.create_subscription( Image, 'color/image_color', self.on_image_color, 10 ) - def _set_capture_settings(self): + def _set_settings_2d(self): self.get_logger().info('Setting parameter `settings_2d_yaml`') settings_parameter = Parameter( 'settings_2d_yaml', diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index 37aec65..9a43141 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -23,9 +23,9 @@ def __init__(self): while not self.capture_and_save_service.wait_for_service(timeout_sec=3.0): self.get_logger().info('Capture service not available, waiting again...') - self._set_capture_settings() + self._set_settings() - def _set_capture_settings(self): + def _set_settings(self): self.get_logger().info('Setting parameter `settings_yaml`') settings_parameter = Parameter( 'settings_yaml', diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py index 936f845..5bfe84f 100755 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -23,13 +23,13 @@ def __init__(self): while not self.capture_service.wait_for_service(timeout_sec=3.0): self.get_logger().info('Capture service not available, waiting again...') - self._set_capture_settings() + self._set_settings() self.subscription = self.create_subscription( PointCloud2, 'points/xyzrgba', self.on_points, 10 ) - def _set_capture_settings(self): + def _set_settings(self): path_to_settings_yml = ( get_package_share_directory('zivid_samples') + '/settings/camera_settings.yml' From d295a993cfd2d2ab1d0d81910ce388a084adbad1 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 10:47:51 +0200 Subject: [PATCH 22/33] fixup! Add more context to exception error --- zivid_camera/src/zivid_camera.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index f093edb..870e0b7 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -180,7 +180,8 @@ auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger return function(); } catch (const Zivid::Exception & exception) { const auto exception_message = Zivid::toString(exception); - RCLCPP_ERROR_STREAM(logger, "A function threw a Zivid::Exception: \"" + exception_message + "\""); + RCLCPP_ERROR_STREAM( + logger, "A function threw a Zivid::Exception: \"" + exception_message + "\""); throw; } } From 7263319cb2e7aa5fc3938a480e879bf65eacd1a1 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 14:38:26 +0200 Subject: [PATCH 23/33] Remove AsyncParameterClient from Python samples, add timeout when setting parameters in all samples AsyncParameterClient appears not to be available on ROS2 Humble. --- zivid_samples/scripts/sample_capture.py | 17 ++++++++++++----- zivid_samples/scripts/sample_capture_2d.py | 16 +++++++++++----- .../scripts/sample_capture_and_save.py | 17 ++++++++++++----- .../scripts/sample_capture_assistant.py | 4 +++- .../sample_capture_with_settings_from_file.py | 16 +++++++++++----- zivid_samples/src/sample_capture.cpp | 4 +++- zivid_samples/src/sample_capture_2d.cpp | 4 +++- zivid_samples/src/sample_capture_and_save.cpp | 4 +++- zivid_samples/src/sample_capture_assistant.cpp | 4 +++- .../sample_capture_with_settings_from_file.cpp | 4 +++- 10 files changed, 64 insertions(+), 26 deletions(-) diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index 961f892..42946a0 100755 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -6,8 +6,8 @@ from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.parameter_client import AsyncParameterClient +from rcl_interfaces.srv import SetParameters from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger @@ -49,10 +49,17 @@ def _set_settings(self): Threshold: 5 """, ).to_parameter_msg() - param_client = AsyncParameterClient(self, 'zivid_camera') - param_client.wait_for_services() - future = param_client.set_parameters([settings_parameter]) - rclpy.spin_until_future_complete(self, future) + + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) + ) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): self.get_logger().info('Calling capture service') diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index e73dd32..af97a4a 100755 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -6,8 +6,8 @@ from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.parameter_client import AsyncParameterClient +from rcl_interfaces.srv import SetParameters from sensor_msgs.msg import Image from std_srvs.srv import Trigger @@ -45,10 +45,16 @@ def _set_settings_2d(self): Gain: 2.5 """, ).to_parameter_msg() - param_client = AsyncParameterClient(self, 'zivid_camera') - param_client.wait_for_services() - future = param_client.set_parameters([settings_parameter]) - rclpy.spin_until_future_complete(self, future) + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) + ) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): self.get_logger().info('Calling capture service') diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index 9a43141..bec91a2 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -7,8 +7,8 @@ from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.parameter_client import AsyncParameterClient +from rcl_interfaces.srv import SetParameters from zivid_interfaces.srv import CaptureAndSave @@ -47,10 +47,17 @@ def _set_settings(self): Threshold: 5 """, ).to_parameter_msg() - param_client = AsyncParameterClient(self, 'zivid_camera') - param_client.wait_for_services() - future = param_client.set_parameters([settings_parameter]) - rclpy.spin_until_future_complete(self, future) + + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) + ) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): file_path = tempfile.gettempdir() + '/zivid_sample_capture_and_save.zdf' diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py index 58ca8aa..137247e 100755 --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -70,7 +70,9 @@ def main(args=None): sample = Sample() future = sample.capture_assistant_suggest_settings() - rclpy.spin_until_future_complete(sample, future) + rclpy.spin_until_future_complete(sample, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to suggest settings') sample.get_logger().info('Capture assistant complete') future = sample.capture() diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py index 5bfe84f..ae66f43 100755 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -8,8 +8,8 @@ from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter -from rclpy.parameter_client import AsyncParameterClient +from rcl_interfaces.srv import SetParameters from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger @@ -44,10 +44,16 @@ def _set_settings(self): path_to_settings_yml, ).to_parameter_msg() - param_client = AsyncParameterClient(self, 'zivid_camera') - param_client.wait_for_services() - future = param_client.set_parameters([settings_parameter]) - rclpy.spin_until_future_complete(self, future) + param_client = self.create_client(SetParameters, 'zivid_camera/set_parameters') + while not param_client.wait_for_service(timeout_sec=3): + self.get_logger().info('Parameter service not available, waiting again...') + + future = param_client.call_async( + SetParameters.Request(parameters=[settings_parameter]) + ) + rclpy.spin_until_future_complete(self, future, timeout_sec=30) + if not future.result(): + raise RuntimeError('Failed to set parameters') def capture(self): self.get_logger().info('Calling capture service') diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index e7ebcb1..1334462 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -41,7 +41,9 @@ void set_settings(const std::shared_ptr & node) } auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_yaml` parameter"); std::terminate(); } diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index 6d3e53a..9b40519 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -37,7 +37,9 @@ void set_settings_2d(const std::shared_ptr & node) auto result = param_client->set_parameters({rclcpp::Parameter("settings_2d_yaml", settings_2d_yaml)}); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_2d_yaml` parameter"); std::terminate(); } diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 729885b..7b7c1b4 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -40,7 +40,9 @@ void set_settings(const std::shared_ptr & node) } auto result = param_client->set_parameters({rclcpp::Parameter("settings_yaml", settings_yml)}); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_yaml` parameter"); std::terminate(); } diff --git a/zivid_samples/src/sample_capture_assistant.cpp b/zivid_samples/src/sample_capture_assistant.cpp index 31d91ba..259aded 100644 --- a/zivid_samples/src/sample_capture_assistant.cpp +++ b/zivid_samples/src/sample_capture_assistant.cpp @@ -31,7 +31,9 @@ void capture_assistant_suggest_settings(const std::shared_ptr & no CaptureAssistantSuggestSettings::Request::AMBIENT_LIGHT_FREQUENCY_NONE; auto result = client->async_send_request(request); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to call capture assistant service"); std::terminate(); } diff --git a/zivid_samples/src/sample_capture_with_settings_from_file.cpp b/zivid_samples/src/sample_capture_with_settings_from_file.cpp index 0feceb7..21d724e 100644 --- a/zivid_samples/src/sample_capture_with_settings_from_file.cpp +++ b/zivid_samples/src/sample_capture_with_settings_from_file.cpp @@ -30,7 +30,9 @@ void set_settings(const std::shared_ptr & node) auto result = param_client->set_parameters({rclcpp::Parameter("settings_file_path", path_to_settings_yml)}); - if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { + if ( + rclcpp::spin_until_future_complete(node, result, std::chrono::seconds(30)) != + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(node->get_logger(), "Failed to set `settings_file_path` parameter"); std::terminate(); } From db7a7277e309171c8f5d70b5393cc248f6529599 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 15:21:37 +0200 Subject: [PATCH 24/33] fixup! Remove AsyncParameterClient from Python samples, add timeout when setting parameters in all samples --- zivid_samples/scripts/sample_capture.py | 3 +-- zivid_samples/scripts/sample_capture_2d.py | 3 +-- zivid_samples/scripts/sample_capture_and_save.py | 3 +-- zivid_samples/scripts/sample_capture_assistant.py | 2 -- .../scripts/sample_capture_with_settings_from_file.py | 4 +--- 5 files changed, 4 insertions(+), 11 deletions(-) diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index 42946a0..cb32c79 100755 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -2,12 +2,11 @@ import sys +from rcl_interfaces.srv import SetParameters import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter - -from rcl_interfaces.srv import SetParameters from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index af97a4a..ce2d32f 100755 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -2,12 +2,11 @@ import sys +from rcl_interfaces.srv import SetParameters import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter - -from rcl_interfaces.srv import SetParameters from sensor_msgs.msg import Image from std_srvs.srv import Trigger diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index bec91a2..b5688c5 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -3,12 +3,11 @@ import sys import tempfile +from rcl_interfaces.srv import SetParameters import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter - -from rcl_interfaces.srv import SetParameters from zivid_interfaces.srv import CaptureAndSave diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py index 137247e..5569cca 100755 --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -3,11 +3,9 @@ import sys from builtin_interfaces.msg import Duration - import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node - from sensor_msgs.msg import Image from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py index ae66f43..d1bacf7 100755 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -3,13 +3,11 @@ import sys from ament_index_python.packages import get_package_share_directory - +from rcl_interfaces.srv import SetParameters import rclpy from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.parameter import Parameter - -from rcl_interfaces.srv import SetParameters from sensor_msgs.msg import PointCloud2 from std_srvs.srv import Trigger From f9c59b44782cd7bac7e58601eadd61d7d503923d Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 18:44:32 +0200 Subject: [PATCH 25/33] Throw on out-of-scope enum --- zivid_camera/src/zivid_camera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 870e0b7..12c4b87 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -126,7 +126,7 @@ std::string toString(zivid_camera::CameraStatus camera_status) case zivid_camera::CameraStatus::Idle: return "Idle"; default: // NOLINT(clang-diagnostic-covered-switch-default) - return "N/A"; + throw std::runtime_error("Enum `camera_status` out of range."); } } From db411d21d1cd17fb53a27f28961d3c13d470edbd Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 18:45:36 +0200 Subject: [PATCH 26/33] Smaller wording improvements --- README.md | 5 +++-- zivid_samples/scripts/sample_capture.py | 2 +- zivid_samples/scripts/sample_capture_2d.py | 4 ++-- zivid_samples/scripts/sample_capture_and_save.py | 4 ++-- zivid_samples/scripts/sample_capture_assistant.py | 2 +- .../scripts/sample_capture_with_settings_from_file.py | 2 +- zivid_samples/src/sample_capture.cpp | 2 +- zivid_samples/src/sample_capture_2d.cpp | 4 ++-- zivid_samples/src/sample_capture_and_save.cpp | 2 +- 9 files changed, 14 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 6b75a4c..f436ef1 100644 --- a/README.md +++ b/README.md @@ -343,8 +343,9 @@ will cause some additional processing time for calculating the normals. In the `zivid_samples` package we have added samples that demonstrate how to use the Zivid ROS driver. These samples can be used as a starting point for your project. -To launch the Python samples using `ros2 launch`, you might need `python` registered as a command, -such as by using: +To launch the Python samples using `ros2 launch`, you need `python` to be available as a command. +For example, the `python-is-python3` package can be installed to achieve this, by running the +following command: ```bash sudo apt install python-is-python3 diff --git a/zivid_samples/scripts/sample_capture.py b/zivid_samples/scripts/sample_capture.py index cb32c79..b8065af 100755 --- a/zivid_samples/scripts/sample_capture.py +++ b/zivid_samples/scripts/sample_capture.py @@ -18,7 +18,7 @@ def __init__(self): self.capture_service = self.create_client(Trigger, 'capture') while not self.capture_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info('Capture service not available, waiting again...') + self.get_logger().info('capture service not available, waiting again...') self._set_settings() diff --git a/zivid_samples/scripts/sample_capture_2d.py b/zivid_samples/scripts/sample_capture_2d.py index ce2d32f..fd5c906 100755 --- a/zivid_samples/scripts/sample_capture_2d.py +++ b/zivid_samples/scripts/sample_capture_2d.py @@ -18,7 +18,7 @@ def __init__(self): self.capture_2d_service = self.create_client(Trigger, 'capture_2d') while not self.capture_2d_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info('Capture service not available, waiting again...') + self.get_logger().info('capture_2d service not available, waiting again...') self._set_settings_2d() @@ -56,7 +56,7 @@ def _set_settings_2d(self): raise RuntimeError('Failed to set parameters') def capture(self): - self.get_logger().info('Calling capture service') + self.get_logger().info('Calling capture_2d service') return self.capture_2d_service.call_async(Trigger.Request()) def on_image_color(self, msg): diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index b5688c5..9278cc4 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -20,7 +20,7 @@ def __init__(self): CaptureAndSave, 'capture_and_save' ) while not self.capture_and_save_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info('Capture service not available, waiting again...') + self.get_logger().info('capture_and_save service not available, waiting again...') self._set_settings() @@ -60,7 +60,7 @@ def _set_settings(self): def capture(self): file_path = tempfile.gettempdir() + '/zivid_sample_capture_and_save.zdf' - self.get_logger().info(f'Calling capture service with file path: {file_path}') + self.get_logger().info(f'Calling capture_and_save service with file path: {file_path}') request = CaptureAndSave.Request(file_path=file_path) return self.capture_and_save_service.call_async(request) diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py index 5569cca..8e6081a 100755 --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -15,7 +15,7 @@ class Sample(Node): def __init__(self): - super().__init__('sample_capture_and_save_py') + super().__init__('sample_capture_assistant_py') self.capture_assistant_suggest_settings_service = self.create_client( CaptureAssistantSuggestSettings, 'capture_assistant/suggest_settings' diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py index d1bacf7..e9748ad 100755 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -19,7 +19,7 @@ def __init__(self): self.capture_service = self.create_client(Trigger, 'capture') while not self.capture_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info('Capture service not available, waiting again...') + self.get_logger().info('capture service not available, waiting again...') self._set_settings() diff --git a/zivid_samples/src/sample_capture.cpp b/zivid_samples/src/sample_capture.cpp index 1334462..353bc92 100644 --- a/zivid_samples/src/sample_capture.cpp +++ b/zivid_samples/src/sample_capture.cpp @@ -60,7 +60,7 @@ auto create_capture_client(std::shared_ptr & node) RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); } - RCLCPP_INFO(node->get_logger(), "Capture service is available"); + RCLCPP_INFO(node->get_logger(), "capture service is available"); return client; } diff --git a/zivid_samples/src/sample_capture_2d.cpp b/zivid_samples/src/sample_capture_2d.cpp index 9b40519..b3542b5 100644 --- a/zivid_samples/src/sample_capture_2d.cpp +++ b/zivid_samples/src/sample_capture_2d.cpp @@ -53,10 +53,10 @@ auto create_capture_2d_client(std::shared_ptr & node) RCLCPP_ERROR(node->get_logger(), "Client interrupted while waiting for service to appear."); std::terminate(); } - RCLCPP_INFO(node->get_logger(), "Waiting for the capture service to appear..."); + RCLCPP_INFO(node->get_logger(), "Waiting for the capture_2d service to appear..."); } - RCLCPP_INFO(node->get_logger(), "Capture service is available"); + RCLCPP_INFO(node->get_logger(), "capture_2d service is available"); return client; } diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index 7b7c1b4..d726078 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -71,7 +71,7 @@ void capture_and_save(const std::shared_ptr & node) auto result = client->async_send_request(request); if (rclcpp::spin_until_future_complete(node, result) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(node->get_logger(), "Failed to call capture_and_save service"); + RCLCPP_ERROR(node->get_logger(), "Failed to call the capture_and_save service"); std::terminate(); } From 3cec69c493aadf5dc9bd703bf1c42579180b5825 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Thu, 18 Jul 2024 18:48:58 +0200 Subject: [PATCH 27/33] Don't spin on end of `sample_capture_and_save_cpp` Consistent with Python sample. --- zivid_samples/src/sample_capture_and_save.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index d726078..ec8defd 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -96,9 +96,6 @@ int main(int argc, char * argv[]) capture_and_save(node); - RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); - rclcpp::spin(node); - rclcpp::shutdown(); return EXIT_SUCCESS; From deb57e0e5eef8dd27125cac3444b31b002e19c78 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 19 Jul 2024 10:43:00 +0200 Subject: [PATCH 28/33] fixup! Smaller wording improvements --- zivid_samples/scripts/sample_capture_and_save.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index 9278cc4..940a859 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -20,7 +20,9 @@ def __init__(self): CaptureAndSave, 'capture_and_save' ) while not self.capture_and_save_service.wait_for_service(timeout_sec=3.0): - self.get_logger().info('capture_and_save service not available, waiting again...') + self.get_logger().info( + 'capture_and_save service not available, waiting again...' + ) self._set_settings() @@ -60,7 +62,9 @@ def _set_settings(self): def capture(self): file_path = tempfile.gettempdir() + '/zivid_sample_capture_and_save.zdf' - self.get_logger().info(f'Calling capture_and_save service with file path: {file_path}') + self.get_logger().info( + f'Calling capture_and_save service with file path: {file_path}' + ) request = CaptureAndSave.Request(file_path=file_path) return self.capture_and_save_service.call_async(request) From baad826ccc42f9536eedec21d80c8162b75ff678 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 19 Jul 2024 10:56:16 +0200 Subject: [PATCH 29/33] fixup! CMake: Use functions to add samples --- zivid_samples/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index ef7c499..2d84142 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -37,6 +37,7 @@ zivid_add_cpp_sample(sample_capture_and_save DEPENDENCIES rclcpp zivid_interface zivid_add_cpp_sample(sample_capture_assistant DEPENDENCIES rclcpp zivid_interfaces sensor_msgs std_srvs) zivid_add_cpp_sample(sample_capture_with_settings_from_file DEPENDENCIES rclcpp sensor_msgs std_srvs ament_index_cpp) +zivid_add_python_sample(sample_capture) zivid_add_python_sample(sample_capture_2d) zivid_add_python_sample(sample_capture_and_save) zivid_add_python_sample(sample_capture_assistant) From c0e765c48e92f843d424c5f4f8821bf269ef0885 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 19 Jul 2024 11:04:18 +0200 Subject: [PATCH 30/33] Spin at end on all samples, log info message where it was missing --- zivid_samples/scripts/sample_capture_and_save.py | 3 +++ zivid_samples/scripts/sample_capture_assistant.py | 1 + .../scripts/sample_capture_with_settings_from_file.py | 1 + zivid_samples/src/sample_capture_and_save.cpp | 3 +++ 4 files changed, 8 insertions(+) diff --git a/zivid_samples/scripts/sample_capture_and_save.py b/zivid_samples/scripts/sample_capture_and_save.py index 940a859..45d5e84 100755 --- a/zivid_samples/scripts/sample_capture_and_save.py +++ b/zivid_samples/scripts/sample_capture_and_save.py @@ -84,6 +84,9 @@ def main(args=None): sample.get_logger().info('Capture and save complete') + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') + rclpy.spin(sample) + except KeyboardInterrupt: pass except ExternalShutdownException: diff --git a/zivid_samples/scripts/sample_capture_assistant.py b/zivid_samples/scripts/sample_capture_assistant.py index 8e6081a..4137d15 100755 --- a/zivid_samples/scripts/sample_capture_assistant.py +++ b/zivid_samples/scripts/sample_capture_assistant.py @@ -77,6 +77,7 @@ def main(args=None): rclpy.spin_until_future_complete(sample, future) sample.get_logger().info('Capture complete') + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') rclpy.spin(sample) except KeyboardInterrupt: diff --git a/zivid_samples/scripts/sample_capture_with_settings_from_file.py b/zivid_samples/scripts/sample_capture_with_settings_from_file.py index e9748ad..0c20d19 100755 --- a/zivid_samples/scripts/sample_capture_with_settings_from_file.py +++ b/zivid_samples/scripts/sample_capture_with_settings_from_file.py @@ -73,6 +73,7 @@ def main(args=None): rclpy.spin_until_future_complete(sample, future) sample.get_logger().info('Capture complete') + sample.get_logger().info('Spinning node.. Press Ctrl+C to abort.') rclpy.spin(sample) except KeyboardInterrupt: diff --git a/zivid_samples/src/sample_capture_and_save.cpp b/zivid_samples/src/sample_capture_and_save.cpp index ec8defd..d726078 100644 --- a/zivid_samples/src/sample_capture_and_save.cpp +++ b/zivid_samples/src/sample_capture_and_save.cpp @@ -96,6 +96,9 @@ int main(int argc, char * argv[]) capture_and_save(node); + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + rclcpp::shutdown(); return EXIT_SUCCESS; From 8074d80293ddcd02da53dbf271a2f962a44e47c4 Mon Sep 17 00:00:00 2001 From: "Michael R. P. Ragazzon" Date: Fri, 19 Jul 2024 11:54:38 +0200 Subject: [PATCH 31/33] Catch all `std::exception` --- zivid_camera/src/zivid_camera.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 12c4b87..2ab80af 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -164,7 +164,7 @@ void runFunctionAndCatchExceptions( try { function(); response->success = true; - } catch (const Zivid::Exception & exception) { + } catch (const std::exception & exception) { const auto exception_message = Zivid::toString(exception); RCLCPP_ERROR_STREAM( logger, operation + " failed with exception: \"" << exception_message << "\""); @@ -178,10 +178,9 @@ auto runFunctionAndCatchExceptionsAndRethrow(Function && function, const Logger { try { return function(); - } catch (const Zivid::Exception & exception) { + } catch (const std::exception & exception) { const auto exception_message = Zivid::toString(exception); - RCLCPP_ERROR_STREAM( - logger, "A function threw a Zivid::Exception: \"" + exception_message + "\""); + RCLCPP_ERROR_STREAM(logger, "A function failed with exception: \"" + exception_message + "\""); throw; } } From 5ae7bf650ecd39cbdae6e1d710c08440dda4f5fc Mon Sep 17 00:00:00 2001 From: Stian Pedersen Date: Fri, 19 Jul 2024 13:21:02 +0200 Subject: [PATCH 32/33] Log param updates as early as possible also changes logging from INFO to DEBUG, since setParametersCallback is invoked for a lot of different parameters, not just the ones defined by zivid (coming from ros image_transport etc). --- .../include/zivid_camera/zivid_camera.hpp | 4 +-- zivid_camera/src/zivid_camera.cpp | 31 ++++++++++--------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/zivid_camera/include/zivid_camera/zivid_camera.hpp b/zivid_camera/include/zivid_camera/zivid_camera.hpp index 506dbe6..41d2995 100644 --- a/zivid_camera/include/zivid_camera/zivid_camera.hpp +++ b/zivid_camera/include/zivid_camera/zivid_camera.hpp @@ -78,7 +78,7 @@ class ZividCamera : public rclcpp::Node void onCameraConnectionKeepAliveTimeout(); void reconnectToCameraIfNecessary(); void setCameraStatus(CameraStatus camera_status); - + rcl_interfaces::msg::SetParametersResult setParametersCallback(const std::vector ¶meters); void cameraInfoModelNameServiceHandler( const std::shared_ptr request_header, const std::shared_ptr request, @@ -142,9 +142,9 @@ class ZividCamera : public rclcpp::Node sensor_msgs::msg::CameraInfo::ConstSharedPtr makeCameraInfo( const std_msgs::msg::Header & header, std::size_t width, std::size_t height, const Zivid::CameraIntrinsics & intrinsics); - [[noreturn]] void logErrorAndThrowRuntimeException(const std::string & message); + OnSetParametersCallbackHandle::SharedPtr set_parameters_callback_handle_; rclcpp::TimerBase::SharedPtr camera_connection_keepalive_timer_; bool use_latched_publisher_for_points_xyz_{false}; bool use_latched_publisher_for_points_xyzrgba_{false}; diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 2ab80af..7634942 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -212,14 +212,11 @@ class CaptureSettingsController explicit CaptureSettingsController(rclcpp::Node & node) : node_(node), file_path_param_{baseName() + std::string{"_file_path"}}, - yaml_string_param_{baseName() + std::string{"_yaml"}}, - param_subscriber_{std::make_shared(&node)} + yaml_string_param_{baseName() + std::string{"_yaml"}} { using namespace std::placeholders; for (const auto & param : {yaml_string_param_, file_path_param_}) { node_.declare_parameter(param, ""); - cb_handles_.push_back(param_subscriber_->add_parameter_callback( - param, std::bind(&CaptureSettingsController::parameterCallback, this, _1))); } } @@ -255,13 +252,6 @@ class CaptureSettingsController } private: - void parameterCallback(const rclcpp::Parameter & p) - { - RCLCPP_INFO( - node_.get_logger(), "Parameter '%s' was set to '%s'", p.get_name().c_str(), - p.as_string().c_str()); - } - constexpr auto baseName() const { if constexpr (std::is_same_v) { @@ -276,12 +266,12 @@ class CaptureSettingsController rclcpp::Node & node_; std::string file_path_param_; std::string yaml_string_param_; - std::shared_ptr param_subscriber_; - std::vector> cb_handles_; }; ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) : rclcpp::Node{"zivid_camera", options}, + set_parameters_callback_handle_{this->add_on_set_parameters_callback( + std::bind(&ZividCamera::setParametersCallback, this, std::placeholders::_1))}, zivid_{std::make_unique()}, camera_status_{CameraStatus::Idle}, settings_controller_{std::make_unique>(*this)}, @@ -290,7 +280,7 @@ ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) // Disable buffering on stdout setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - RCLCPP_INFO_STREAM(get_logger(), "Zivid ROS driver"); // Fix: add version + RCLCPP_INFO_STREAM(get_logger(), "Zivid ROS driver"); RCLCPP_INFO(get_logger(), "The node's namespace is '%s'", get_namespace()); RCLCPP_INFO_STREAM(get_logger(), "Running Zivid Core version " << ZIVID_CORE_VERSION); @@ -495,6 +485,19 @@ void ZividCamera::setCameraStatus(CameraStatus camera_status) } } +rcl_interfaces::msg::SetParametersResult ZividCamera::setParametersCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & param : parameters) { + RCLCPP_DEBUG_STREAM( + get_logger(), "Set parameter '" << param.get_name() << "' (" << param.get_type_name() + << ") to '" << param.value_to_string() << "'"); + } + return result; +} + void ZividCamera::cameraInfoModelNameServiceHandler( const std::shared_ptr, const std::shared_ptr, From 12835f65b36d6d149d4053b64bd09fe85a275116 Mon Sep 17 00:00:00 2001 From: Stian Pedersen Date: Fri, 19 Jul 2024 13:52:47 +0200 Subject: [PATCH 33/33] fixup! Log param updates as early as possible --- zivid_camera/include/zivid_camera/zivid_camera.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/zivid_camera/include/zivid_camera/zivid_camera.hpp b/zivid_camera/include/zivid_camera/zivid_camera.hpp index 41d2995..2f58093 100644 --- a/zivid_camera/include/zivid_camera/zivid_camera.hpp +++ b/zivid_camera/include/zivid_camera/zivid_camera.hpp @@ -78,7 +78,8 @@ class ZividCamera : public rclcpp::Node void onCameraConnectionKeepAliveTimeout(); void reconnectToCameraIfNecessary(); void setCameraStatus(CameraStatus camera_status); - rcl_interfaces::msg::SetParametersResult setParametersCallback(const std::vector ¶meters); + rcl_interfaces::msg::SetParametersResult setParametersCallback( + const std::vector & parameters); void cameraInfoModelNameServiceHandler( const std::shared_ptr request_header, const std::shared_ptr request,