diff --git a/src/accelerated_image_processor_common/include/accelerated_image_processor_common/processor.hpp b/src/accelerated_image_processor_common/include/accelerated_image_processor_common/processor.hpp index 4fdee98..5034678 100644 --- a/src/accelerated_image_processor_common/include/accelerated_image_processor_common/processor.hpp +++ b/src/accelerated_image_processor_common/include/accelerated_image_processor_common/processor.hpp @@ -177,6 +177,16 @@ class BaseProcessor return std::get(parameters_.at(key)); } + /** + * @brief Set camera info, note that this function does nothing unless overridden. + */ + virtual void set_camera_info(const common::CameraInfo &) {} + + /** + * @brief Return the rectified camera information if it has value, otherwise std::nullopt. + */ + const std::optional & camera_info() const { return camera_info_; } + protected: /** * @brief Process the input image. @@ -184,5 +194,7 @@ class BaseProcessor * @return The processed image. */ virtual Image process_impl(const Image & image) = 0; + + std::optional camera_info_{std::nullopt}; //!< [maybe unused] Camera info }; } // namespace accelerated_image_processor::common diff --git a/src/accelerated_image_processor_pipeline/CMakeLists.txt b/src/accelerated_image_processor_pipeline/CMakeLists.txt index acfae3d..dea3dca 100644 --- a/src/accelerated_image_processor_pipeline/CMakeLists.txt +++ b/src/accelerated_image_processor_pipeline/CMakeLists.txt @@ -33,8 +33,8 @@ endif() add_library( ${PROJECT_NAME} SHARED - src/builder.cpp src/rectifier/cpu.cpp src/rectifier/npp.cpp - src/rectifier/opencv_cuda.cpp src/rectifier/utility.cpp) + src/builder.cpp src/sequential.cpp src/rectifier/cpu.cpp + src/rectifier/npp.cpp src/rectifier/opencv_cuda.cpp src/rectifier/utility.cpp) target_compile_definitions( ${PROJECT_NAME} @@ -55,7 +55,8 @@ target_link_libraries( $<$:CUDA::nppig> $<$:CUDA::nppisu> $<$:CUDA::cudart>) -ament_target_dependencies(${PROJECT_NAME} accelerated_image_processor_common) +ament_target_dependencies(${PROJECT_NAME} accelerated_image_processor_common + accelerated_image_processor_compression) set_property(TARGET ${PROJECT_NAME} PROPERTY INTERFACE_INCLUDE_DIRECTORIES $) @@ -64,12 +65,20 @@ install(DIRECTORY include/${PROJECT_NAME} DESTINATION include) if(BUILD_TESTING) set(test_files test/builder.cpp test/cpu_rectifier.cpp test/npp_rectifier.cpp - test/opencv_cuda_rectifier.cpp) + test/opencv_cuda_rectifier.cpp test/sequential.cpp) foreach(test_file IN LISTS test_files) get_filename_component(test_file_name ${test_file} NAME) ament_auto_add_gtest(${test_file_name}_${PROJECT_NAME} ${test_file}) target_link_libraries(${test_file_name}_${PROJECT_NAME} ${PROJECT_NAME}) endforeach() + + # Examples + set(example_files examples/example_sequential.cpp) + foreach(example_file IN LISTS example_files) + get_filename_component(example_name ${example_file} NAME_WE) + ament_auto_add_executable(${example_name} ${example_file}) + target_link_libraries(${example_name} ${PROJECT_NAME}) + endforeach() endif() ament_export_include_directories(include) diff --git a/src/accelerated_image_processor_pipeline/README.md b/src/accelerated_image_processor_pipeline/README.md index 7d38ddd..cddccd5 100644 --- a/src/accelerated_image_processor_pipeline/README.md +++ b/src/accelerated_image_processor_pipeline/README.md @@ -10,6 +10,15 @@ This package provides functionalities for image processing pipelines using vario | | `OpenCvCudaRectifier` | [OpenCV CUDA](https://opencv.org/platforms/cuda/) | GPU | | | `CpuRectifier` | [OpenCV](https://opencv.org/) | CPU | +## Sequential Processor + +`Sequential` can compose multiple processors and execute them sequentially. +By chaining `Sequential::append(...)` methods, you can add multiple processors. + +With `Sequential::register_callback(...)` methods, you can also add the postprocess callback to be executed after all processors are finished as the usual processor. + +For the example usage, see the [example_sequential.cpp](./examples/example_sequential.cpp). + ## Example Usage in ROS 2 The following code demonstrates how to use each processor in your ROS 2 codebase. diff --git a/src/accelerated_image_processor_pipeline/examples/example_sequential.cpp b/src/accelerated_image_processor_pipeline/examples/example_sequential.cpp new file mode 100644 index 0000000..1dc075c --- /dev/null +++ b/src/accelerated_image_processor_pipeline/examples/example_sequential.cpp @@ -0,0 +1,120 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "accelerated_image_processor_pipeline/rectifier.hpp" +#include "accelerated_image_processor_pipeline/sequential.hpp" + +#include +#include + +#include +#include +#include + +namespace accelerated_image_processor +{ +namespace +{ +std::string format_to_str(common::ImageFormat format) +{ + switch (format) { + case common::ImageFormat::RAW: + return "RAW"; + case common::ImageFormat::JPEG: + return "JPEG"; + default: + return "UNKNOWN"; + } +} + +void print_info(const common::Image & image) +{ + std::cout << "[INFO]:\n" + << " (width, height) = (" << image.width << ", " << image.height << ")\n" + << " format = " << format_to_str(image.format) << std::endl; +} + +void print_finish(const common::Image &) +{ + std::cout << ">>> 🎉 All processing finished!!" << std::endl; +} + +std::pair make_image_and_info(uint32_t width, uint32_t height) +{ + const std::string frame_id = "camera"; + constexpr int64_t timestamp = 123456789; + + common::Image image; + image.frame_id = frame_id; + image.timestamp = timestamp; + image.width = width; + image.height = height; + image.step = width * 3; + image.encoding = common::ImageEncoding::RGB; + image.format = common::ImageFormat::RAW; + image.data.resize(image.step * image.height); + + for (uint32_t y = 0; y < image.height; ++y) { + for (uint32_t x = 0; x < image.width; ++x) { + image.data[y * image.step + x * 3 + 0] = static_cast(x % 256); + image.data[y * image.step + x * 3 + 1] = static_cast(y % 256); + image.data[y * image.step + x * 3 + 2] = static_cast((x + y) % 256); + } + } + + common::CameraInfo camera_info; + camera_info.frame_id = frame_id; + camera_info.timestamp = timestamp; + camera_info.width = width; + camera_info.height = height; + camera_info.distortion_model = common::DistortionModel::PLUMB_BOB; + camera_info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; // (k1, k2, p1, p2, k3) + camera_info.k = {1.0, 0.0, width / 2.0, // (fx, 0, cx) + 0.0, 1.0, height / 2.0, // (0, fy, cy) + 0.0, 0.0, 1.0}; // (0, 0, 1.0) + camera_info.r = {1.0, 0.0, 0.0, // (r11, r12, r13) + 0.0, 1.0, 0.0, // (r21, r22, r23) + 0.0, 0.0, 1.0}; // (r31, r32, r33) + camera_info.p = {1.0, 0.0, width / 2.0, 0.0, // (fx', 0, cx', tx) + 0.0, 1.0, height / 2.0, 0.0, // (0, fy', cy', ty) + 0.0, 0.0, 1.0, 0.0}; // (0, 0, 1.0, tz) + camera_info.binning_x = 1; + camera_info.binning_y = 1; + camera_info.roi = {0, 0, width, height, false}; + return std::make_pair(image, camera_info); +} +} // namespace + +void run_sequential() +{ + const auto [image, camera_info] = make_image_and_info(1920, 1080); + + pipeline::Sequential sequential; + + // Pipeline: Rectification -> [Info] -> JPEG Compression -> [Info] -> [Finish] + sequential.append("rectifier", &print_info) + .append("compressor", &print_info, "jpeg") + .register_postprocess(&print_finish); + + // Set camera info for rectification + sequential.set_camera_info(camera_info); + + sequential.process(image); +} +} // namespace accelerated_image_processor + +int main() +{ + accelerated_image_processor::run_sequential(); +} diff --git a/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/rectifier.hpp b/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/rectifier.hpp index 1be3ba7..4ff6bb5 100644 --- a/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/rectifier.hpp +++ b/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/rectifier.hpp @@ -59,16 +59,11 @@ class Rectifier : public common::BaseProcessor * @brief Set camera information before rectified, and compute the rectified camera information * under the hood. */ - void set_camera_info(const common::CameraInfo & camera_info) + void set_camera_info(const common::CameraInfo & camera_info) override { camera_info_ = prepare_maps(camera_info); } - /** - * @brief Return the rectified camera information if it has value, otherwise std::nullopt. - */ - const std::optional & camera_info() const { return camera_info_; } - /** * @brief Return true if Rectifier::set_camera_info() was invoked and the rectified camera * information has been set. @@ -82,8 +77,6 @@ class Rectifier : public common::BaseProcessor */ virtual common::CameraInfo prepare_maps(const common::CameraInfo & camera_info) = 0; - std::optional camera_info_{std::nullopt}; //!< Rectified camera info. - private: const RectifierBackend backend_; //!< Rectification backend type. }; diff --git a/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/sequential.hpp b/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/sequential.hpp new file mode 100644 index 0000000..2db3b03 --- /dev/null +++ b/src/accelerated_image_processor_pipeline/include/accelerated_image_processor_pipeline/sequential.hpp @@ -0,0 +1,182 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace accelerated_image_processor::pipeline +{ +/** + * @brief Sequential processor that executes a sequence of processors in order. + */ +class Sequential final : public common::BaseProcessor +{ +public: + /** + * @brief Child processor belongs to the sequence. + */ + struct Child + { + Child(const std::string & ns, std::unique_ptr processor) + : ns(ns), processor(std::move(processor)) + { + } + + std::string ns; //!< Namespace of the processor + std::unique_ptr processor; //!< Processor instance + }; + + Sequential() : common::BaseProcessor({}) {} + + // Delete copy constructor and assignment operator because this class holds processors as + // unique_ptr. + Sequential(const Sequential &) = delete; + Sequential & operator=(const Sequential &) = delete; + Sequential(Sequential &&) = default; + Sequential & operator=(Sequential &&) = default; + + /** + * @brief Append a processor to the sequence. + * + * @tparam P Processor type + * @tparam Args Argument types + * + * @param ns Namespace of the processor + * @param args Arguments to pass to the processor constructor + * @return Sequential& Reference to the current instance + */ + template + Sequential & append(const std::string & ns, Args &&... args) + { + std::unique_ptr

processor = nullptr; + if constexpr (std::is_same_v) { + processor = create_rectifier(std::forward(args)...); + } else if constexpr (std::is_same_v) { + processor = compression::create_compressor(std::forward(args)...); + } + + if (!processor) throw std::runtime_error("Failed to create processor"); + sequence_.emplace_back(ns, std::move(processor)); + + return *this; + } + + /** + * @brief Append a processor to the sequence with a callback function. + * + * @tparam P Processor type + * @tparam F Callback function type + * @tparam Args Argument types + * + * @param ns Namespace of the processor + * @param fn Callback function to be called after processing + * @param args Arguments to pass to the processor constructor + * @return Sequential& Reference to the current instance + */ + template < + class P, class F, + std::enable_if_t, int> = 0, + class... Args> + Sequential & append(const std::string & ns, F fn, Args &&... args) + { + std::unique_ptr

processor = nullptr; + if constexpr (std::is_same_v) { + processor = create_rectifier(std::forward(args)..., fn); + } else if constexpr (std::is_same_v) { + processor = compression::create_compressor(std::forward(args)..., fn); + } + + if (!processor) throw std::runtime_error("Failed to create processor"); + sequence_.emplace_back(ns, std::move(processor)); + + return *this; + } + + /** + * @brief Append a processor to the sequence with a callback function. + * + * @tparam P Processor type + * @tparam Obj Object type + * @tparam Method Method of the object to call + * @tparam Args Argument types + * + * @param ns Namespace of the processor + * @param obj Object instance to call the method on + * @param args Arguments to pass to the processor constructor + * @return Sequential& Reference to the current instance + */ + template + Sequential & append(const std::string & ns, Obj * obj, Args &&... args) + { + std::unique_ptr

processor = nullptr; + if constexpr (std::is_same_v) { + processor = create_rectifier(std::forward(args)..., obj); + } else if constexpr (std::is_same_v) { + processor = compression::create_compressor(std::forward(args)..., obj); + } + + if (!processor) throw std::runtime_error("Failed to create processor"); + sequence_.emplace_back(ns, std::move(processor)); + + return *this; + } + + /** + * @brief Set camera info for all processors in the pipeline. + * + * @note The camera info that is held by this class will be updated with the child's last valid + * camera info. + * + * @param camera_info Camera info to set + */ + void set_camera_info(const common::CameraInfo & camera_info) override; + + /** + * @brief Return a writable reference to the sequence of child processors. + * + * @return std::vector& Reference to the sequence of child processors + */ + std::vector & items() noexcept { return sequence_; } + + /** + * @brief Return a read-only reference to the sequence of child processors. + * + * @return const std::vector& Reference to the sequence of child processors + */ + const std::vector & items() const noexcept { return sequence_; } + +private: + /** + * @brief Process an image sequentially through the pipeline. + * + * @param image Input image + * @return common::Image Processed image + */ + common::Image process_impl(const common::Image & image) override; + + std::vector sequence_; //!< Sequence of child processors +}; +} // namespace accelerated_image_processor::pipeline diff --git a/src/accelerated_image_processor_pipeline/package.xml b/src/accelerated_image_processor_pipeline/package.xml index 1d5bd43..b7c7191 100644 --- a/src/accelerated_image_processor_pipeline/package.xml +++ b/src/accelerated_image_processor_pipeline/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto accelerated_image_processor_common + accelerated_image_processor_compression libopencv-dev ament_lint_auto diff --git a/src/accelerated_image_processor_pipeline/src/sequential.cpp b/src/accelerated_image_processor_pipeline/src/sequential.cpp new file mode 100644 index 0000000..f43c959 --- /dev/null +++ b/src/accelerated_image_processor_pipeline/src/sequential.cpp @@ -0,0 +1,47 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "accelerated_image_processor_pipeline/sequential.hpp" + +namespace accelerated_image_processor::pipeline +{ +void Sequential::set_camera_info(const common::CameraInfo & camera_info) +{ + for (auto & child : sequence_) { + child.processor->set_camera_info(camera_info); + // Update camera info with child's camera info + const auto info = child.processor->camera_info(); + if (info.has_value()) { + camera_info_.emplace(info.value()); + } + } +} + +common::Image Sequential::process_impl(const common::Image & image) +{ + auto processed = image; + for (auto & child : sequence_) { + if (!child.processor) { + continue; + } + + auto result = child.processor->process(processed); + if (result.has_value()) { + processed = result.value(); + } + } + + return processed; +} +} // namespace accelerated_image_processor::pipeline diff --git a/src/accelerated_image_processor_pipeline/test/builder.cpp b/src/accelerated_image_processor_pipeline/test/builder.cpp index 8498b45..2c2ed8f 100644 --- a/src/accelerated_image_processor_pipeline/test/builder.cpp +++ b/src/accelerated_image_processor_pipeline/test/builder.cpp @@ -15,6 +15,7 @@ #include "accelerated_image_processor_pipeline/builder.hpp" #include "accelerated_image_processor_pipeline/rectifier.hpp" +#include "test_utility.hpp" #include @@ -41,24 +42,6 @@ void check_rectifier_type(const std::unique_ptr & rectifier) EXPECT_EQ(ptr->backend(), ExpectedBackend); } - -/** - * @brief Dummy class to register postprocess function. - */ -struct DummyClass -{ - /** - * @brief Dummy free function for postprocess. - */ - void dummy_function(const common::Image &) {} -}; - -/** - * @brief Dummy free function for postprocess. - */ -void dummy_function(const common::Image &) -{ -} } // namespace TEST(TestRectifierBuilder, CreateRectifier1) diff --git a/src/accelerated_image_processor_pipeline/test/sequential.cpp b/src/accelerated_image_processor_pipeline/test/sequential.cpp new file mode 100644 index 0000000..894522e --- /dev/null +++ b/src/accelerated_image_processor_pipeline/test/sequential.cpp @@ -0,0 +1,54 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "accelerated_image_processor_pipeline/sequential.hpp" + +#include "accelerated_image_processor_pipeline/rectifier.hpp" +#include "test_utility.hpp" + +#include + +#include + +namespace accelerated_image_processor::pipeline +{ +TEST(TestSequential, AppendWithoutFunction) +{ + Sequential sequential; + sequential.append("rectifier").append("compressor", "jpeg"); + + EXPECT_EQ(sequential.items().size(), 2); +} + +TEST(TestSequential, AppendWithFreeFunction) +{ + Sequential sequential; + sequential.append("rectifier", &dummy_function) + .append("compressor", &dummy_function, "jpeg"); + + EXPECT_EQ(sequential.items().size(), 2); +} + +TEST(TestSequential, AppendWithMemberFunction) +{ + DummyClass dummy; + + Sequential sequential; + sequential.append("rectifier", &dummy) + .append( + "compressor", &dummy, "jpeg"); + + EXPECT_EQ(sequential.items().size(), 2); +} +} // namespace accelerated_image_processor::pipeline diff --git a/src/accelerated_image_processor_pipeline/test/test_utility.hpp b/src/accelerated_image_processor_pipeline/test/test_utility.hpp index dcd17fb..16dbe5b 100644 --- a/src/accelerated_image_processor_pipeline/test/test_utility.hpp +++ b/src/accelerated_image_processor_pipeline/test/test_utility.hpp @@ -94,4 +94,22 @@ class TestRectifier : public ::testing::Test common::Image image_; common::CameraInfo camera_info_; }; + +/** + * @brief Dummy class to register postprocess function. + */ +struct DummyClass +{ + /** + * @brief Dummy free function for postprocess. + */ + void dummy_function(const common::Image &) {} +}; + +/** + * @brief Dummy free function for postprocess. + */ +inline void dummy_function(const common::Image &) +{ +} } // namespace accelerated_image_processor::pipeline diff --git a/src/accelerated_image_processor_ros/include/accelerated_image_processor_ros/parameter.hpp b/src/accelerated_image_processor_ros/include/accelerated_image_processor_ros/parameter.hpp index 9cacc30..9aeec63 100644 --- a/src/accelerated_image_processor_ros/include/accelerated_image_processor_ros/parameter.hpp +++ b/src/accelerated_image_processor_ros/include/accelerated_image_processor_ros/parameter.hpp @@ -15,6 +15,7 @@ #pragma once #include +#include #include #include @@ -41,4 +42,25 @@ inline void fetch_parameters(rclcpp::Node * node, common::BaseProcessor * proces { fetch_parameters(node, processor, ""); } + +/** + * @brief Fetch parameters from ROS parameter server and set them to the sequential processor. + * + * @param node The ROS node to fetch parameters from. + * @param sequential The sequential processor to set the parameters to. + * @param prefix The prefix of the parameter names. + */ +void fetch_parameters( + rclcpp::Node * node, pipeline::Sequential & sequential, const std::string & prefix); + +/** + * @brief Fetch parameters from ROS parameter server and set them to the sequential processor. + * + * @param node The ROS node to fetch parameters from. + * @param sequential The sequential processor to set the parameters to. + */ +inline void fetch_parameters(rclcpp::Node * node, pipeline::Sequential & sequential) +{ + fetch_parameters(node, sequential, ""); +} } // namespace accelerated_image_processor::ros diff --git a/src/accelerated_image_processor_ros/src/imgproc_node.cpp b/src/accelerated_image_processor_ros/src/imgproc_node.cpp index 514a3f8..d4fdb41 100644 --- a/src/accelerated_image_processor_ros/src/imgproc_node.cpp +++ b/src/accelerated_image_processor_ros/src/imgproc_node.cpp @@ -18,6 +18,9 @@ #include "accelerated_image_processor_ros/parameter.hpp" #include "accelerated_image_processor_ros/qos.hpp" +#include +#include + #include #include #include @@ -43,17 +46,14 @@ ImgProcNode::ImgProcNode(const rclcpp::NodeOptions & options) : Node("imgproc_no // rectifier (rectification & compression) if (do_rectify) { - // TODO(ktro2828): - // - Implement composable processor that embraces multiple processors - // - Enable to share a CUDA stream across multiple processors - raw_rectifier_ = - pipeline::create_rectifier(this); - rectified_compressor_ = - compression::create_compressor( - compression_type, this); - - fetch_parameters(this, raw_rectifier_.get(), "rectifier"); - fetch_parameters(this, rectified_compressor_.get(), "compressor"); + // TODO(ktro2828): Enable to share a CUDA stream across multiple processors + raw_rectifier_ + .append( + "rectifier", this) + .append( + "compressor", this, compression_type); + + fetch_parameters(this, raw_rectifier_); } qos_request_timer_ = rclcpp::create_timer( @@ -132,19 +132,14 @@ void ImgProcNode::on_image(const sensor_msgs::msg::Image::ConstSharedPtr msg) if (rectification_worker_) { // NOTE: capture `msg` by value to extend the lifetime of the shared pointer at least until the // task is completed - rectification_worker_->add_task([this, image, msg]() { - const auto rectified = raw_rectifier_->process(*image); - if (rectified) { - rectified_compressor_->process(rectified.value()); - } - }); + rectification_worker_->add_task([this, image, msg]() { raw_rectifier_.process(*image); }); } } void ImgProcNode::on_camera_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) { auto camera_info = from_ros_info(*msg); - raw_rectifier_->set_camera_info(camera_info); + raw_rectifier_.set_camera_info(camera_info); info_subscription_.reset(); } @@ -166,7 +161,7 @@ void ImgProcNode::publish_compressed(const common::Image & image) void ImgProcNode::publish_rectified_raw(const common::Image & image) { auto raw = to_ros_raw(image); - auto info = to_ros_info(raw_rectifier_->camera_info().value()); + auto info = to_ros_info(raw_rectifier_.camera_info().value()); rectified_raw_publisher_->publish(std::move(raw)); rectified_info_publisher_->publish(std::move(info)); } diff --git a/src/accelerated_image_processor_ros/src/imgproc_node.hpp b/src/accelerated_image_processor_ros/src/imgproc_node.hpp index e84502c..edc986a 100644 --- a/src/accelerated_image_processor_ros/src/imgproc_node.hpp +++ b/src/accelerated_image_processor_ros/src/imgproc_node.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -78,8 +79,7 @@ class ImgProcNode : public rclcpp::Node // --- image processors --- std::unique_ptr raw_compressor_; - std::unique_ptr raw_rectifier_; - std::unique_ptr rectified_compressor_; + pipeline::Sequential raw_rectifier_; // --- subscriptions and publishers --- rclcpp::Subscription::SharedPtr image_subscription_; diff --git a/src/accelerated_image_processor_ros/src/parameter.cpp b/src/accelerated_image_processor_ros/src/parameter.cpp index dbb92ae..8c77691 100644 --- a/src/accelerated_image_processor_ros/src/parameter.cpp +++ b/src/accelerated_image_processor_ros/src/parameter.cpp @@ -22,11 +22,19 @@ namespace accelerated_image_processor::ros { +namespace +{ +inline std::string format_prefix(const std::string & prefix, const std::string & ns) +{ + return prefix.empty() ? ns : prefix + "." + ns; +} +} // namespace + void fetch_parameters( rclcpp::Node * node, common::BaseProcessor * processor, const std::string & prefix) { for (auto & [name, value] : processor->parameters()) { - const auto & param_name = prefix.empty() ? name : prefix + "." + name; + const auto param_name = format_prefix(prefix, name); std::visit( [&](auto & v) { using T = std::decay_t; @@ -39,4 +47,13 @@ void fetch_parameters( value); } } + +void fetch_parameters( + rclcpp::Node * node, pipeline::Sequential & sequential, const std::string & prefix) +{ + for (auto & child : sequential.items()) { + const auto child_prefix = format_prefix(prefix, child.ns); + fetch_parameters(node, child.processor.get(), child_prefix); + } +} } // namespace accelerated_image_processor::ros