diff --git a/deep_conversions/CMakeLists.txt b/deep_conversions/CMakeLists.txt new file mode 100644 index 0000000..684061e --- /dev/null +++ b/deep_conversions/CMakeLists.txt @@ -0,0 +1,83 @@ +# Copyright (c) 2025-present WATonomous. All rights reserved. +# +# 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. +cmake_minimum_required(VERSION 3.22) +project(deep_conversions) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_link_options(-Wl,-no-undefined) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(deep_core REQUIRED) + +set(include_dir ${CMAKE_CURRENT_SOURCE_DIR}/include) + +# deep_conversions library +set(DEEP_CONVERSIONS_LIB ${PROJECT_NAME}_lib) +add_library(${DEEP_CONVERSIONS_LIB} SHARED + src/image_conversions.cpp + src/imu_conversions.cpp + src/laserscan_conversions.cpp + src/pointcloud_conversions.cpp +) + +target_include_directories(${DEEP_CONVERSIONS_LIB} PUBLIC + $ + $ +) + +target_link_libraries(${DEEP_CONVERSIONS_LIB} + PUBLIC + rclcpp::rclcpp + deep_core::deep_core_lib + ${sensor_msgs_TARGETS} + ${std_msgs_TARGETS} +) + +install(TARGETS + ${DEEP_CONVERSIONS_LIB} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(EXPORT ${PROJECT_NAME}Targets + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) + +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(deep_test REQUIRED) + + add_deep_test(test_conversions test/test_conversions.cpp + LIBRARIES ${DEEP_CONVERSIONS_LIB} + ) +endif() + +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_package() diff --git a/deep_conversions/README.md b/deep_conversions/README.md new file mode 100644 index 0000000..c10ab0d --- /dev/null +++ b/deep_conversions/README.md @@ -0,0 +1,34 @@ +# deep_conversions + +ROS 2 sensor message to tensor conversion library. + +## Overview + +Provides generic conversions between ROS 2 sensor messages and `deep_core` tensors. + +## Supported Conversions + +### From ROS to Tensor +- **Images** (`sensor_msgs/msg/Image`) → Tensor with shape `[batch, height, width, channels]` or `[batch, height, width]` for grayscale +- **PointCloud2** (`sensor_msgs/msg/PointCloud2`) → Tensor with shape `[num_points, num_fields]` +- **LaserScan** (`sensor_msgs/msg/LaserScan`) → Tensor with shape `[num_ranges]` or `[num_ranges, 2]` with intensities +- **IMU** (`sensor_msgs/msg/Imu`) → Tensor with shape `[10]` containing quaternion, linear acceleration, and angular velocity + +### From Tensor to ROS +- Tensor → **Image** with specified encoding + +## Usage + +Include the appropriate header for your message type: +- `deep_conversions/image_conversions.hpp` +- `deep_conversions/pointcloud_conversions.hpp` +- `deep_conversions/laserscan_conversions.hpp` +- `deep_conversions/imu_conversions.hpp` + +All conversion functions are in the `deep_ros::ros_conversions` namespace and require a `BackendMemoryAllocator` for tensor creation. + +## License + +Copyright (c) 2025-present WATonomous. All rights reserved. + +Licensed under the Apache License, Version 2.0. diff --git a/deep_conversions/include/deep_conversions/image_conversions.hpp b/deep_conversions/include/deep_conversions/image_conversions.hpp new file mode 100644 index 0000000..e26f9fc --- /dev/null +++ b/deep_conversions/include/deep_conversions/image_conversions.hpp @@ -0,0 +1,98 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_core/plugin_interfaces/backend_memory_allocator.hpp" +#include "deep_core/types/tensor.hpp" + +namespace deep_ros +{ +namespace ros_conversions +{ + +/** + * @brief Image encoding information + */ +struct ImageEncoding +{ + DataType dtype; + size_t channels; + size_t bytes_per_channel; +}; + +/** + * @brief Get encoding information from ROS image encoding string + * @param encoding ROS image encoding string (e.g., "rgb8", "bgr8", "mono8") + * @return ImageEncoding struct with dtype, channels, and bytes_per_channel + * @throws std::runtime_error if encoding is unsupported + */ +ImageEncoding get_image_encoding_info(const std::string & encoding); + +/** + * @brief Convert sensor_msgs::msg::Image to Tensor + * @param image ROS Image message + * @param allocator Memory allocator to use (uses CPU allocator if nullptr) + * @return Tensor with shape [1, height, width, channels] or [1, height, width] + * @throws std::runtime_error if image dimensions are invalid or data size mismatches + */ +Tensor from_image(const sensor_msgs::msg::Image & image, std::shared_ptr allocator = nullptr); + +/** + * @brief Convert vector of sensor_msgs::msg::Image to batched Tensor + * @param images Vector of ROS Image messages + * @param allocator Memory allocator to use (uses CPU allocator if nullptr) + * @return Tensor with shape [batch_size, height, width, channels] or [batch_size, height, width] + * @throws std::invalid_argument if batch is empty or images have mismatched dimensions/encodings + */ +Tensor from_image( + const std::vector & images, std::shared_ptr allocator = nullptr); + +/** + * @brief Convert Tensor to sensor_msgs::msg::Image (single image from batch) + * @param tensor Tensor with shape [1, height, width] or [1, height, width, channels] + * @param image Output ROS Image message + * @param encoding Image encoding (must match tensor dtype and shape) + * @param header Optional header to set timestamp and frame_id + * @throws std::invalid_argument if tensor shape/dtype doesn't match encoding + */ +void to_image( + const Tensor & tensor, + sensor_msgs::msg::Image & image, + const std::string & encoding, + const std_msgs::msg::Header & header = std_msgs::msg::Header()); + +/** + * @brief Convert batched Tensor to vector of sensor_msgs::msg::Image + * @param tensor Tensor with shape [batch_size, height, width] or [batch_size, height, width, channels] + * @param images Output vector of ROS Image messages + * @param encoding Image encoding (must match tensor dtype and shape) + * @param header Optional header to set timestamp and frame_id + * @throws std::invalid_argument if tensor shape/dtype doesn't match encoding or batch size is 0 + */ +void to_image( + const Tensor & tensor, + std::vector & images, + const std::string & encoding, + const std_msgs::msg::Header & header = std_msgs::msg::Header()); + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/include/deep_conversions/imu_conversions.hpp b/deep_conversions/include/deep_conversions/imu_conversions.hpp new file mode 100644 index 0000000..67d7251 --- /dev/null +++ b/deep_conversions/include/deep_conversions/imu_conversions.hpp @@ -0,0 +1,46 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_core/plugin_interfaces/backend_memory_allocator.hpp" +#include "deep_core/types/tensor.hpp" + +namespace deep_ros +{ +namespace ros_conversions +{ + +/** + * @brief Convert sensor_msgs::msg::Imu to Tensor + * + * Converts a ROS IMU message to a tensor format containing orientation, + * linear acceleration, and angular velocity data. The output tensor has + * shape [10] with the following layout: + * - [0-3]: orientation quaternion (x, y, z, w) + * - [4-6]: linear acceleration (x, y, z) + * - [7-9]: angular velocity (x, y, z) + * + * @param imu ROS IMU message + * @param allocator Memory allocator to use (uses CPU allocator if nullptr) + * @return Tensor with shape [10] containing [qx,qy,qz,qw,ax,ay,az,gx,gy,gz] + */ +Tensor from_imu(const sensor_msgs::msg::Imu & imu, std::shared_ptr allocator = nullptr); + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/include/deep_conversions/laserscan_conversions.hpp b/deep_conversions/include/deep_conversions/laserscan_conversions.hpp new file mode 100644 index 0000000..6706dd4 --- /dev/null +++ b/deep_conversions/include/deep_conversions/laserscan_conversions.hpp @@ -0,0 +1,45 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_core/plugin_interfaces/backend_memory_allocator.hpp" +#include "deep_core/types/tensor.hpp" + +namespace deep_ros +{ +namespace ros_conversions +{ + +/** + * @brief Convert sensor_msgs::msg::LaserScan to Tensor + * + * Converts a ROS LaserScan message to a tensor format. If intensity data is present + * and matches the number of range readings, the output tensor will have shape + * [num_ranges, 2] with ranges and intensities. Otherwise, it will have shape + * [num_ranges] with only range data. + * + * @param scan ROS LaserScan message + * @param allocator Memory allocator to use (uses CPU allocator if nullptr) + * @return Tensor with shape [num_ranges] or [num_ranges, 2] if intensities present + */ +Tensor from_laserscan( + const sensor_msgs::msg::LaserScan & scan, std::shared_ptr allocator = nullptr); + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/include/deep_conversions/pointcloud_conversions.hpp b/deep_conversions/include/deep_conversions/pointcloud_conversions.hpp new file mode 100644 index 0000000..20b0bc2 --- /dev/null +++ b/deep_conversions/include/deep_conversions/pointcloud_conversions.hpp @@ -0,0 +1,44 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_core/plugin_interfaces/backend_memory_allocator.hpp" +#include "deep_core/types/tensor.hpp" + +namespace deep_ros +{ +namespace ros_conversions +{ + +/** + * @brief Convert sensor_msgs::msg::PointCloud2 to Tensor + * + * Converts a ROS PointCloud2 message to a tensor format suitable for deep learning. + * All field data types are converted to FLOAT32 for consistency. + * + * @param cloud ROS PointCloud2 message + * @param allocator Memory allocator to use (uses CPU allocator if nullptr) + * @return Tensor with shape [num_points, num_fields] containing point data + * @throws std::invalid_argument if pointcloud has no fields + */ +Tensor from_pointcloud2( + const sensor_msgs::msg::PointCloud2 & cloud, std::shared_ptr allocator = nullptr); + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/package.xml b/deep_conversions/package.xml new file mode 100644 index 0000000..0f89d6a --- /dev/null +++ b/deep_conversions/package.xml @@ -0,0 +1,22 @@ + + + + deep_conversions + 0.1.0 + Conversion utilities between ROS 2 sensor messages and tensors + WATonomous + Apache 2.0 + + ament_cmake + + rclcpp + sensor_msgs + std_msgs + deep_core + + deep_test + + + ament_cmake + + diff --git a/deep_conversions/src/image_conversions.cpp b/deep_conversions/src/image_conversions.cpp new file mode 100644 index 0000000..46d92cf --- /dev/null +++ b/deep_conversions/src/image_conversions.cpp @@ -0,0 +1,317 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_conversions/image_conversions.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace deep_ros +{ +namespace ros_conversions +{ + +ImageEncoding get_image_encoding_info(const std::string & encoding) +{ + static const std::unordered_map encoding_map = { + // RGB/BGR formats + {"rgb8", {DataType::UINT8, 3, 1}}, + {"rgba8", {DataType::UINT8, 4, 1}}, + {"rgb16", {DataType::UINT16, 3, 2}}, + {"rgba16", {DataType::UINT16, 4, 2}}, + {"bgr8", {DataType::UINT8, 3, 1}}, + {"bgra8", {DataType::UINT8, 4, 1}}, + {"bgr16", {DataType::UINT16, 3, 2}}, + {"bgra16", {DataType::UINT16, 4, 2}}, + + // Grayscale + {"mono8", {DataType::UINT8, 1, 1}}, + {"mono16", {DataType::UINT16, 1, 2}}, + + // Bayer patterns (treat as single channel) + {"bayer_rggb8", {DataType::UINT8, 1, 1}}, + {"bayer_bggr8", {DataType::UINT8, 1, 1}}, + {"bayer_gbrg8", {DataType::UINT8, 1, 1}}, + {"bayer_grbg8", {DataType::UINT8, 1, 1}}, + {"bayer_rggb16", {DataType::UINT16, 1, 2}}, + {"bayer_bggr16", {DataType::UINT16, 1, 2}}, + {"bayer_gbrg16", {DataType::UINT16, 1, 2}}, + {"bayer_grbg16", {DataType::UINT16, 1, 2}}, + + // OpenCV type formats + {"8UC1", {DataType::UINT8, 1, 1}}, + {"8UC2", {DataType::UINT8, 2, 1}}, + {"8UC3", {DataType::UINT8, 3, 1}}, + {"8UC4", {DataType::UINT8, 4, 1}}, + {"8SC1", {DataType::INT8, 1, 1}}, + {"8SC2", {DataType::INT8, 2, 1}}, + {"8SC3", {DataType::INT8, 3, 1}}, + {"8SC4", {DataType::INT8, 4, 1}}, + {"16UC1", {DataType::UINT16, 1, 2}}, + {"16UC2", {DataType::UINT16, 2, 2}}, + {"16UC3", {DataType::UINT16, 3, 2}}, + {"16UC4", {DataType::UINT16, 4, 2}}, + {"16SC1", {DataType::INT16, 1, 2}}, + {"16SC2", {DataType::INT16, 2, 2}}, + {"16SC3", {DataType::INT16, 3, 2}}, + {"16SC4", {DataType::INT16, 4, 2}}, + {"32SC1", {DataType::INT32, 1, 4}}, + {"32SC2", {DataType::INT32, 2, 4}}, + {"32SC3", {DataType::INT32, 3, 4}}, + {"32SC4", {DataType::INT32, 4, 4}}, + {"32FC1", {DataType::FLOAT32, 1, 4}}, + {"32FC2", {DataType::FLOAT32, 2, 4}}, + {"32FC3", {DataType::FLOAT32, 3, 4}}, + {"32FC4", {DataType::FLOAT32, 4, 4}}, + {"64FC1", {DataType::FLOAT64, 1, 8}}, + {"64FC2", {DataType::FLOAT64, 2, 8}}, + {"64FC3", {DataType::FLOAT64, 3, 8}}, + {"64FC4", {DataType::FLOAT64, 4, 8}}, + + // YUV formats - 8-bit + {"yuv422", {DataType::UINT8, 2, 1}}, + {"YUV422_YUY2", {DataType::UINT8, 2, 1}}, + {"UYVY", {DataType::UINT8, 2, 1}}, + {"YUYV", {DataType::UINT8, 2, 1}}, + {"yuv444", {DataType::UINT8, 3, 1}}, + {"YUV444", {DataType::UINT8, 3, 1}}, + + // YUV formats - 16-bit + {"yuv422_16", {DataType::UINT16, 2, 2}}, + {"yuv444_16", {DataType::UINT16, 3, 2}}, + {"YUV422_16", {DataType::UINT16, 2, 2}}, + {"YUV444_16", {DataType::UINT16, 3, 2}}, + }; + + auto it = encoding_map.find(encoding); + if (it != encoding_map.end()) { + return it->second; + } + + // Try to parse TYPE_XX(C)N format using regex + std::regex type_regex("(8U|8S|16U|16S|32S|32F|64F)C([1-4])"); + std::smatch match; + if (std::regex_match(encoding, match, type_regex)) { + std::string type_str = match[1]; + int channels = std::stoi(match[2]); + + if (type_str == "8U") return {DataType::UINT8, static_cast(channels), 1}; + if (type_str == "8S") return {DataType::INT8, static_cast(channels), 1}; + if (type_str == "16U") return {DataType::UINT16, static_cast(channels), 2}; + if (type_str == "16S") return {DataType::INT16, static_cast(channels), 2}; + if (type_str == "32S") return {DataType::INT32, static_cast(channels), 4}; + if (type_str == "32F") return {DataType::FLOAT32, static_cast(channels), 4}; + if (type_str == "64F") return {DataType::FLOAT64, static_cast(channels), 8}; + } + + throw std::runtime_error("Unsupported image encoding: " + encoding); +} + +Tensor from_image(const sensor_msgs::msg::Image & image, std::shared_ptr allocator) +{ + if (image.height == 0 || image.width == 0) { + throw std::runtime_error( + "Invalid image dimensions: height=" + std::to_string(image.height) + ", width=" + std::to_string(image.width)); + } + + auto encoding_info = get_image_encoding_info(image.encoding); + + // Create tensor with proper shape - always include batch dimension (size 1 for single image) + std::vector shape = {1, image.height, image.width}; + if (encoding_info.channels > 1) { + shape.push_back(encoding_info.channels); + } + + // Validate step size (bytes per row) + size_t expected_step = image.width * encoding_info.channels * encoding_info.bytes_per_channel; + if (image.step != expected_step) { + throw std::runtime_error( + "Image step mismatch. Expected " + std::to_string(expected_step) + " but got " + std::to_string(image.step)); + } + + // Validate data size + size_t expected_size = image.height * image.width * encoding_info.channels * encoding_info.bytes_per_channel; + if (image.data.size() != expected_size) { + throw std::runtime_error( + "Image data size mismatch. Expected " + std::to_string(expected_size) + " but got " + + std::to_string(image.data.size())); + } + + // Direct copy + Tensor tensor(shape, encoding_info.dtype, allocator); + if (allocator) { + allocator->copy_from_host(tensor.data(), image.data.data(), image.data.size()); + } else { + std::memcpy(tensor.data(), image.data.data(), image.data.size()); + } + return tensor; +} + +Tensor from_image( + const std::vector & images, std::shared_ptr allocator) +{ + if (images.empty()) { + throw std::invalid_argument("Image batch is empty"); + } + + // Get encoding info from first image + auto encoding_info = get_image_encoding_info(images[0].encoding); + + // Create batch shape: [batch_size, height, width, channels] or [batch_size, height, width] + std::vector shape = {images.size(), images[0].height, images[0].width}; + if (encoding_info.channels > 1) { + shape.push_back(encoding_info.channels); + } + + // Validate all images have same dimensions and encoding + size_t expected_size = images[0].height * images[0].width * encoding_info.channels * encoding_info.bytes_per_channel; + for (size_t i = 0; i < images.size(); ++i) { + if (images[i].height != images[0].height || images[i].width != images[0].width) { + throw std::invalid_argument("All images in batch must have same dimensions"); + } + if (images[i].encoding != images[0].encoding) { + throw std::invalid_argument("All images in batch must have same encoding"); + } + if (images[i].data.size() != expected_size) { + throw std::runtime_error( + "Image " + std::to_string(i) + " data size mismatch. Expected " + std::to_string(expected_size) + " but got " + + std::to_string(images[i].data.size())); + } + } + + // Direct copy + Tensor tensor(shape, encoding_info.dtype, allocator); + auto * dst = static_cast(tensor.data()); + + for (size_t i = 0; i < images.size(); ++i) { + if (allocator) { + allocator->copy_from_host(dst + i * images[i].data.size(), images[i].data.data(), images[i].data.size()); + } else { + std::memcpy(dst + i * images[i].data.size(), images[i].data.data(), images[i].data.size()); + } + } + return tensor; +} + +void to_image( + const Tensor & tensor, + sensor_msgs::msg::Image & image, + const std::string & encoding, + const std_msgs::msg::Header & header) +{ + if (tensor.rank() < 3 || tensor.rank() > 4) { + throw std::invalid_argument( + "Tensor must be 3D [batch, height, width] or 4D [batch, height, width, channels] for image conversion"); + } + + if (tensor.shape()[0] != 1) { + throw std::invalid_argument( + "This overload only supports single images (batch size must be 1). Use vector overload for multiple images."); + } + + image.header = header; + + // Verify encoding matches tensor + auto encoding_info = get_image_encoding_info(encoding); + size_t tensor_channels = (tensor.rank() == 4) ? tensor.shape()[3] : 1; + + if (encoding_info.channels != tensor_channels) { + throw std::invalid_argument( + "Encoding channels (" + std::to_string(encoding_info.channels) + ") doesn't match tensor channels (" + + std::to_string(tensor_channels) + ")"); + } + + if (encoding_info.dtype != tensor.dtype()) { + throw std::invalid_argument("Encoding data type doesn't match tensor data type"); + } + + image.height = tensor.shape()[1]; // Skip batch dimension + image.width = tensor.shape()[2]; // Skip batch dimension + image.encoding = encoding; + + // Calculate step (bytes per row) + image.step = image.width * encoding_info.channels * encoding_info.bytes_per_channel; + + // Set big endian flag (false for little endian) + image.is_bigendian = false; + + // Copy tensor data to image (skip batch dimension) + size_t image_size = image.height * image.width * encoding_info.channels * encoding_info.bytes_per_channel; + image.data.resize(image_size); + std::memcpy(image.data.data(), tensor.data(), image_size); +} + +void to_image( + const Tensor & tensor, + std::vector & images, + const std::string & encoding, + const std_msgs::msg::Header & header) +{ + if (tensor.rank() < 3 || tensor.rank() > 4) { + throw std::invalid_argument( + "Tensor must be 3D [batch, height, width] or 4D [batch, height, width, channels] for image conversion"); + } + + size_t batch_size = tensor.shape()[0]; + if (batch_size == 0) { + throw std::invalid_argument("Batch size cannot be 0"); + } + + // Verify encoding matches tensor + auto encoding_info = get_image_encoding_info(encoding); + size_t tensor_channels = (tensor.rank() == 4) ? tensor.shape()[3] : 1; + + if (encoding_info.channels != tensor_channels) { + throw std::invalid_argument( + "Encoding channels (" + std::to_string(encoding_info.channels) + ") doesn't match tensor channels (" + + std::to_string(tensor_channels) + ")"); + } + + if (encoding_info.dtype != tensor.dtype()) { + throw std::invalid_argument("Encoding data type doesn't match tensor data type"); + } + + size_t height = tensor.shape()[1]; + size_t width = tensor.shape()[2]; + size_t image_size = height * width * encoding_info.channels * encoding_info.bytes_per_channel; + + images.clear(); + images.reserve(batch_size); + + const auto * src = static_cast(tensor.data()); + + for (size_t i = 0; i < batch_size; ++i) { + sensor_msgs::msg::Image image; + image.header = header; + image.height = height; + image.width = width; + image.encoding = encoding; + image.step = width * encoding_info.channels * encoding_info.bytes_per_channel; + image.is_bigendian = false; + + image.data.resize(image_size); + std::memcpy(image.data.data(), src + i * image_size, image_size); + + images.push_back(std::move(image)); + } +} + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/src/imu_conversions.cpp b/deep_conversions/src/imu_conversions.cpp new file mode 100644 index 0000000..4c2f19f --- /dev/null +++ b/deep_conversions/src/imu_conversions.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_conversions/imu_conversions.hpp" + +#include +#include + +namespace deep_ros +{ +namespace ros_conversions +{ + +Tensor from_imu(const sensor_msgs::msg::Imu & imu, std::shared_ptr allocator) +{ + std::vector shape = {10}; + Tensor tensor(shape, DataType::FLOAT32, allocator); + auto * data = tensor.data_as(); + + // Orientation quaternion + data[0] = static_cast(imu.orientation.x); + data[1] = static_cast(imu.orientation.y); + data[2] = static_cast(imu.orientation.z); + data[3] = static_cast(imu.orientation.w); + + // Linear acceleration + data[4] = static_cast(imu.linear_acceleration.x); + data[5] = static_cast(imu.linear_acceleration.y); + data[6] = static_cast(imu.linear_acceleration.z); + + // Angular velocity + data[7] = static_cast(imu.angular_velocity.x); + data[8] = static_cast(imu.angular_velocity.y); + data[9] = static_cast(imu.angular_velocity.z); + + return tensor; +} + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/src/laserscan_conversions.cpp b/deep_conversions/src/laserscan_conversions.cpp new file mode 100644 index 0000000..0b4115e --- /dev/null +++ b/deep_conversions/src/laserscan_conversions.cpp @@ -0,0 +1,54 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_conversions/laserscan_conversions.hpp" + +#include +#include +#include + +namespace deep_ros +{ +namespace ros_conversions +{ + +Tensor from_laserscan(const sensor_msgs::msg::LaserScan & scan, std::shared_ptr allocator) +{ + size_t num_ranges = scan.ranges.size(); + bool has_intensities = !scan.intensities.empty() && scan.intensities.size() == num_ranges; + + if (has_intensities) { + std::vector shape = {num_ranges, 2}; + Tensor tensor(shape, DataType::FLOAT32, allocator); + auto * data = tensor.data_as(); + + for (size_t i = 0; i < num_ranges; ++i) { + data[i * 2] = scan.ranges[i]; + data[i * 2 + 1] = scan.intensities[i]; + } + return tensor; + } else { + std::vector shape = {num_ranges}; + Tensor tensor(shape, DataType::FLOAT32, allocator); + if (allocator) { + allocator->copy_from_host(tensor.data(), scan.ranges.data(), num_ranges * sizeof(float)); + } else { + std::memcpy(tensor.data(), scan.ranges.data(), num_ranges * sizeof(float)); + } + return tensor; + } +} + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/src/pointcloud_conversions.cpp b/deep_conversions/src/pointcloud_conversions.cpp new file mode 100644 index 0000000..30c85d3 --- /dev/null +++ b/deep_conversions/src/pointcloud_conversions.cpp @@ -0,0 +1,73 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 "deep_conversions/pointcloud_conversions.hpp" + +#include +#include +#include +#include + +namespace deep_ros +{ +namespace ros_conversions +{ + +Tensor from_pointcloud2(const sensor_msgs::msg::PointCloud2 & cloud, std::shared_ptr allocator) +{ + if (cloud.fields.empty()) { + throw std::invalid_argument("PointCloud2 has no fields"); + } + + size_t num_points = cloud.width * cloud.height; + size_t num_fields = cloud.fields.size(); + + // For simplicity, convert all to float32 + std::vector shape = {num_points, num_fields}; + Tensor tensor(shape, DataType::FLOAT32, allocator); + auto * data = tensor.data_as(); + + // Extract field data + for (size_t i = 0; i < num_points; ++i) { + for (size_t j = 0; j < num_fields; ++j) { + const auto & field = cloud.fields[j]; + size_t offset = i * cloud.point_step + field.offset; + + // Convert based on datatype + float value = 0.0f; + if (field.datatype == sensor_msgs::msg::PointField::FLOAT32) { + std::memcpy(&value, &cloud.data[offset], sizeof(float)); + } else if (field.datatype == sensor_msgs::msg::PointField::FLOAT64) { + double temp; + std::memcpy(&temp, &cloud.data[offset], sizeof(double)); + value = static_cast(temp); + } else if (field.datatype == sensor_msgs::msg::PointField::INT32) { + int32_t temp; + std::memcpy(&temp, &cloud.data[offset], sizeof(int32_t)); + value = static_cast(temp); + } else if (field.datatype == sensor_msgs::msg::PointField::UINT32) { + uint32_t temp; + std::memcpy(&temp, &cloud.data[offset], sizeof(uint32_t)); + value = static_cast(temp); + } + + data[i * num_fields + j] = value; + } + } + + return tensor; +} + +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_conversions/test/test_conversions.cpp b/deep_conversions/test/test_conversions.cpp new file mode 100644 index 0000000..05b2050 --- /dev/null +++ b/deep_conversions/test/test_conversions.cpp @@ -0,0 +1,445 @@ +// Copyright (c) 2025-present WATonomous. All rights reserved. +// +// 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 +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace deep_ros +{ +namespace ros_conversions +{ +namespace test +{ + +TEST_CASE("Image encoding info parsing", "[conversions][image]") +{ + SECTION("RGB8 encoding") + { + auto info = get_image_encoding_info("rgb8"); + REQUIRE(info.dtype == DataType::UINT8); + REQUIRE(info.channels == 3); + REQUIRE(info.bytes_per_channel == 1); + } + + SECTION("BGR8 encoding") + { + auto info = get_image_encoding_info("bgr8"); + REQUIRE(info.dtype == DataType::UINT8); + REQUIRE(info.channels == 3); + REQUIRE(info.bytes_per_channel == 1); + } + + SECTION("MONO8 encoding") + { + auto info = get_image_encoding_info("mono8"); + REQUIRE(info.dtype == DataType::UINT8); + REQUIRE(info.channels == 1); + REQUIRE(info.bytes_per_channel == 1); + } + + SECTION("RGBA8 encoding") + { + auto info = get_image_encoding_info("rgba8"); + REQUIRE(info.dtype == DataType::UINT8); + REQUIRE(info.channels == 4); + REQUIRE(info.bytes_per_channel == 1); + } + + SECTION("MONO16 encoding") + { + auto info = get_image_encoding_info("mono16"); + REQUIRE(info.dtype == DataType::UINT16); + REQUIRE(info.channels == 1); + REQUIRE(info.bytes_per_channel == 2); + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "Image conversion from ROS to Tensor", "[conversions][image]") +{ + auto allocator = getAllocator(); + REQUIRE(allocator != nullptr); + + SECTION("RGB8 image conversion") + { + sensor_msgs::msg::Image ros_image; + ros_image.header.stamp.sec = 123; + ros_image.header.frame_id = "camera_frame"; + ros_image.height = 100; + ros_image.width = 100; + ros_image.encoding = "rgb8"; + ros_image.is_bigendian = false; + ros_image.step = 100 * 3; // width * channels + ros_image.data.resize(100 * 100 * 3); + + // Fill with test pattern + for (size_t i = 0; i < ros_image.data.size(); ++i) { + ros_image.data[i] = static_cast(i % 256); + } + + auto tensor = from_image(ros_image, allocator); + + REQUIRE(tensor.shape().size() == 4); // [1, height, width, channels] or similar + REQUIRE(tensor.dtype() == DataType::UINT8); + REQUIRE(tensor.data() != nullptr); + REQUIRE(allocator->allocated_bytes() > 0); + } + + SECTION("Grayscale image conversion") + { + sensor_msgs::msg::Image ros_image; + ros_image.header.stamp.sec = 456; + ros_image.header.frame_id = "mono_camera_frame"; + ros_image.height = 240; + ros_image.width = 320; + ros_image.encoding = "mono8"; + ros_image.is_bigendian = false; + ros_image.step = 320; // width * 1 channel + ros_image.data.resize(240 * 320); + + // Fill with gradient test pattern + for (size_t y = 0; y < ros_image.height; ++y) { + for (size_t x = 0; x < ros_image.width; ++x) { + size_t idx = y * ros_image.step + x; + ros_image.data[idx] = static_cast((x + y) % 256); + } + } + + auto tensor = from_image(ros_image, allocator); + + // Validate tensor properties + REQUIRE(tensor.shape().size() == 3); // [1, height, width] for grayscale + REQUIRE(tensor.shape()[0] == 1); // Batch size + REQUIRE(tensor.shape()[1] == 240); // Height + REQUIRE(tensor.shape()[2] == 320); // Width + REQUIRE(tensor.dtype() == DataType::UINT8); + REQUIRE(tensor.data() != nullptr); + REQUIRE(tensor.size() == 240 * 320 * 1); + + // Verify data integrity - check a few sample points + auto tensor_data = static_cast(tensor.data()); + REQUIRE(tensor_data[0] == static_cast((0 + 0) % 256)); // Top-left corner + REQUIRE(tensor_data[319] == static_cast((319 + 0) % 256)); // Top-right corner + + // Verify memory allocation tracking + REQUIRE(allocator->allocated_bytes() > 0); + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "Batch image conversion", "[conversions][image][batch]") +{ + auto allocator = getAllocator(); + + SECTION("Multiple RGB8 images") + { + std::vector images; + + for (int i = 0; i < 3; ++i) { + sensor_msgs::msg::Image img; + img.height = 100; + img.width = 100; + img.encoding = "rgb8"; + img.step = 100 * 3; + img.data.resize(100 * 100 * 3, static_cast(i * 50)); + images.push_back(img); + } + + auto batch_tensor = from_image(images, allocator); + + // Should have batch dimension + REQUIRE(batch_tensor.shape().size() == 4); + REQUIRE(batch_tensor.shape()[0] == 3); // Batch size + REQUIRE(batch_tensor.dtype() == DataType::UINT8); + } +} + +TEST_CASE_METHOD( + deep_ros::test::MockBackendFixture, "Image conversion from Tensor to ROS", "[conversions][image][output]") +{ + auto allocator = getAllocator(); + + SECTION("Tensor to single RGB8 image") + { + // Create a test tensor + std::vector shape{1, 100, 100, 3}; // [1, height, width, channels] + Tensor tensor(shape, DataType::UINT8, allocator); + + // Fill with test data + uint8_t * data = static_cast(tensor.data()); + for (size_t i = 0; i < tensor.size(); ++i) { + data[i] = static_cast(i % 256); + } + + sensor_msgs::msg::Image output_image; + std_msgs::msg::Header header; + header.stamp.sec = 456; + header.frame_id = "test_frame"; + + to_image(tensor, output_image, "rgb8", header); + + REQUIRE(output_image.height == 100); + REQUIRE(output_image.width == 100); + REQUIRE(output_image.encoding == "rgb8"); + REQUIRE(output_image.header.stamp.sec == 456); + REQUIRE(output_image.header.frame_id == "test_frame"); + REQUIRE(output_image.data.size() == 100 * 100 * 3); + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "PointCloud2 conversion", "[conversions][pointcloud]") +{ + auto allocator = getAllocator(); + + SECTION("Basic pointcloud conversion") + { + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.stamp.sec = 789; + cloud.header.frame_id = "lidar_frame"; + cloud.height = 1; // Unorganized cloud + cloud.width = 1000; // 1000 points + cloud.is_bigendian = false; + cloud.point_step = 16; // 4 fields * 4 bytes each + cloud.row_step = cloud.point_step * cloud.width; + + // Define fields: x, y, z, intensity + sensor_msgs::msg::PointField field_x, field_y, field_z, field_i; + + field_x.name = "x"; + field_x.offset = 0; + field_x.datatype = sensor_msgs::msg::PointField::FLOAT32; + field_x.count = 1; + + field_y.name = "y"; + field_y.offset = 4; + field_y.datatype = sensor_msgs::msg::PointField::FLOAT32; + field_y.count = 1; + + field_z.name = "z"; + field_z.offset = 8; + field_z.datatype = sensor_msgs::msg::PointField::FLOAT32; + field_z.count = 1; + + field_i.name = "intensity"; + field_i.offset = 12; + field_i.datatype = sensor_msgs::msg::PointField::FLOAT32; + field_i.count = 1; + + cloud.fields = {field_x, field_y, field_z, field_i}; + cloud.data.resize(cloud.row_step); + + // Fill with test data + float * float_data = reinterpret_cast(cloud.data.data()); + for (size_t i = 0; i < cloud.width; ++i) { + float_data[i * 4 + 0] = static_cast(i); // x + float_data[i * 4 + 1] = static_cast(i * 2); // y + float_data[i * 4 + 2] = static_cast(i * 3); // z + float_data[i * 4 + 3] = static_cast(i * 4); // intensity + } + + auto tensor = from_pointcloud2(cloud, allocator); + + REQUIRE(tensor.shape().size() == 2); // [num_points, num_fields] + REQUIRE(tensor.shape()[0] == 1000); // Number of points + REQUIRE(tensor.shape()[1] == 4); // Number of fields + REQUIRE(tensor.dtype() == DataType::FLOAT32); + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "LaserScan conversion", "[conversions][laserscan]") +{ + auto allocator = getAllocator(); + + SECTION("LaserScan with ranges only") + { + sensor_msgs::msg::LaserScan scan; + scan.header.stamp.sec = 101112; + scan.header.frame_id = "laser_frame"; + scan.angle_min = -M_PI / 2; + scan.angle_max = M_PI / 2; + scan.angle_increment = M_PI / 180; // 1 degree + scan.range_min = 0.1f; + scan.range_max = 10.0f; + + // Create 180 range readings + scan.ranges.resize(180); + for (size_t i = 0; i < scan.ranges.size(); ++i) { + scan.ranges[i] = 1.0f + static_cast(i) * 0.01f; + } + + auto tensor = from_laserscan(scan, allocator); + + REQUIRE(tensor.shape().size() == 1); // [num_ranges] + REQUIRE(tensor.shape()[0] == 180); + REQUIRE(tensor.dtype() == DataType::FLOAT32); + } + + SECTION("LaserScan with ranges and intensities") + { + sensor_msgs::msg::LaserScan scan; + scan.ranges.resize(100, 5.0f); + scan.intensities.resize(100, 100.0f); // Non-empty intensities + + auto tensor = from_laserscan(scan, allocator); + + REQUIRE(tensor.shape().size() == 2); // [num_ranges, 2] for ranges + intensities + REQUIRE(tensor.shape()[0] == 100); + REQUIRE(tensor.shape()[1] == 2); + REQUIRE(tensor.dtype() == DataType::FLOAT32); + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "IMU conversion", "[conversions][imu]") +{ + auto allocator = getAllocator(); + + SECTION("Complete IMU data") + { + sensor_msgs::msg::Imu imu; + imu.header.stamp.sec = 202425; + imu.header.frame_id = "imu_frame"; + + // Set orientation (quaternion) + imu.orientation.x = 0.1; + imu.orientation.y = 0.2; + imu.orientation.z = 0.3; + imu.orientation.w = 0.9; + + // Set linear acceleration + imu.linear_acceleration.x = 1.0; + imu.linear_acceleration.y = 2.0; + imu.linear_acceleration.z = 9.8; + + // Set angular velocity + imu.angular_velocity.x = 0.01; + imu.angular_velocity.y = 0.02; + imu.angular_velocity.z = 0.03; + + auto tensor = from_imu(imu, allocator); + + REQUIRE(tensor.shape().size() == 1); // [10] for qx,qy,qz,qw,ax,ay,az,gx,gy,gz + REQUIRE(tensor.shape()[0] == 10); + REQUIRE(tensor.dtype() == DataType::FLOAT32); + + // Verify data order: [qx, qy, qz, qw, ax, ay, az, gx, gy, gz] + float * data = static_cast(tensor.data()); + REQUIRE(data[0] == Approx(0.1f)); // qx + REQUIRE(data[1] == Approx(0.2f)); // qy + REQUIRE(data[2] == Approx(0.3f)); // qz + REQUIRE(data[3] == Approx(0.9f)); // qw + REQUIRE(data[4] == Approx(1.0f)); // ax + REQUIRE(data[5] == Approx(2.0f)); // ay + REQUIRE(data[6] == Approx(9.8f)); // az + REQUIRE(data[7] == Approx(0.01f)); // gx + REQUIRE(data[8] == Approx(0.02f)); // gy + REQUIRE(data[9] == Approx(0.03f)); // gz + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "Error handling and edge cases", "[conversions][error]") +{ + auto allocator = getAllocator(); + + SECTION("Invalid image encoding") + { + REQUIRE_THROWS(get_image_encoding_info("invalid_encoding")); + } + + SECTION("Empty image data") + { + sensor_msgs::msg::Image empty_image; + empty_image.height = 0; + empty_image.width = 0; + empty_image.encoding = "rgb8"; + + // Should handle gracefully or throw appropriate exception + REQUIRE_THROWS(from_image(empty_image, allocator)); + } + + SECTION("Mismatched image step size") + { + sensor_msgs::msg::Image bad_image; + bad_image.height = 100; + bad_image.width = 100; + bad_image.encoding = "rgb8"; + bad_image.step = 100; // Should be 300 for rgb8 + bad_image.data.resize(100 * 100 * 3); + + // Should detect step size mismatch + REQUIRE_THROWS(from_image(bad_image, allocator)); + } + + SECTION("Empty batch conversion") + { + std::vector empty_batch; + REQUIRE_THROWS(from_image(empty_batch, allocator)); + } +} + +TEST_CASE_METHOD(deep_ros::test::MockBackendFixture, "Performance and memory efficiency", "[conversions][performance]") +{ + auto allocator = getAllocator(); + + SECTION("Large image processing") + { + sensor_msgs::msg::Image large_image; + large_image.height = 1080; + large_image.width = 1920; + large_image.encoding = "rgb8"; + large_image.step = 1920 * 3; + large_image.data.resize(1080 * 1920 * 3); + + auto start = std::chrono::high_resolution_clock::now(); + auto tensor = from_image(large_image, allocator); + auto end = std::chrono::high_resolution_clock::now(); + + auto duration = std::chrono::duration_cast(end - start); + + REQUIRE(tensor.data() != nullptr); + REQUIRE(duration.count() < 100); // Should complete within 100ms + } + + SECTION("Memory allocation tracking") + { + size_t initial_bytes = allocator->allocated_bytes(); + + sensor_msgs::msg::Image test_image; + test_image.height = 100; + test_image.width = 100; + test_image.encoding = "rgb8"; + test_image.step = 100 * 3; + test_image.data.resize(100 * 100 * 3); + + auto tensor = from_image(test_image, allocator); + + REQUIRE(allocator->allocated_bytes() > initial_bytes); + REQUIRE(allocator->allocated_bytes() >= 100 * 100 * 3); // At least image size + } +} + +} // namespace test +} // namespace ros_conversions +} // namespace deep_ros diff --git a/deep_core/include/deep_core/plugin_interfaces/backend_memory_allocator.hpp b/deep_core/include/deep_core/plugin_interfaces/backend_memory_allocator.hpp index 4a70094..ffe88aa 100644 --- a/deep_core/include/deep_core/plugin_interfaces/backend_memory_allocator.hpp +++ b/deep_core/include/deep_core/plugin_interfaces/backend_memory_allocator.hpp @@ -38,6 +38,14 @@ class BackendMemoryAllocator */ virtual void * allocate(size_t bytes) = 0; + /** + * @brief Allocate aligned memory + * @param bytes Number of bytes to allocate + * @param alignment Alignment requirement in bytes (must be power of 2) + * @return Pointer to allocated memory, nullptr on failure + */ + virtual void * allocate(size_t bytes, size_t alignment); + /** * @brief Deallocate memory * @param ptr Pointer to memory allocated by this allocator @@ -99,6 +107,15 @@ class BackendMemoryAllocator * @return Device name (e.g., "cpu", "cuda", "opencl") */ virtual std::string device_name() const = 0; + + /** + * @brief Get total bytes allocated by this allocator + * @return Number of bytes currently allocated (for testing/debugging) + */ + virtual size_t allocated_bytes() const + { + return 0; + } }; } // namespace deep_ros diff --git a/deep_core/src/backend_memory_allocator.cpp b/deep_core/src/backend_memory_allocator.cpp index 3b8605e..08ec053 100644 --- a/deep_core/src/backend_memory_allocator.cpp +++ b/deep_core/src/backend_memory_allocator.cpp @@ -43,4 +43,11 @@ void BackendMemoryAllocator::copy_device_to_device(void * dst, const void * src, copy_device_to_device_impl(dst, src, bytes); } +void * BackendMemoryAllocator::allocate(size_t bytes, size_t alignment) +{ + (void)bytes; // Suppress unused parameter warning + (void)alignment; // Suppress unused parameter warning + throw std::runtime_error("Aligned allocation not implemented for this backend"); +} + } // namespace deep_ros diff --git a/deep_test/include/deep_test/deep_test.hpp b/deep_test/include/deep_test/deep_test.hpp index e954dbe..f8c8bb6 100644 --- a/deep_test/include/deep_test/deep_test.hpp +++ b/deep_test/include/deep_test/deep_test.hpp @@ -18,6 +18,7 @@ // Include this to access all deep_test functionality // Test fixtures +#include "test_fixtures/mock_backend_fixture.hpp" #include "test_fixtures/test_executor_fixture.hpp" // Test nodes diff --git a/deep_test/include/test_fixtures/mock_backend_fixture.hpp b/deep_test/include/test_fixtures/mock_backend_fixture.hpp index 92f3f02..709f15c 100644 --- a/deep_test/include/test_fixtures/mock_backend_fixture.hpp +++ b/deep_test/include/test_fixtures/mock_backend_fixture.hpp @@ -40,10 +40,21 @@ class MockBackendFixture { loader_ = std::make_unique>("deep_core", "deep_ros::DeepBackendPlugin"); - mock_backend_ = loader_->createSharedInstance("mock_backend"); + mock_backend_ = loader_->createUniqueInstance("mock_backend"); allocator_ = mock_backend_->get_allocator(); } + /** + * @brief Destructor - ensures clean shutdown + */ + ~MockBackendFixture() + { + // Reset shared pointers before destroying the loader + allocator_.reset(); + mock_backend_.reset(); + loader_.reset(); + } + /** * @brief Get the mock backend allocator * @return Shared pointer to the mock backend memory allocator diff --git a/deep_test/src/mock_plugin/mock_plugin.cpp b/deep_test/src/mock_plugin/mock_plugin.cpp index ba44097..3a0a275 100644 --- a/deep_test/src/mock_plugin/mock_plugin.cpp +++ b/deep_test/src/mock_plugin/mock_plugin.cpp @@ -35,7 +35,11 @@ class MockMemoryAllocator : public BackendMemoryAllocator void * allocate(size_t bytes) override { if (bytes == 0) return nullptr; - return std::malloc(bytes); + void * ptr = std::malloc(bytes); + if (ptr) { + allocated_bytes_ += bytes; + } + return ptr; } void deallocate(void * ptr) override @@ -69,6 +73,19 @@ class MockMemoryAllocator : public BackendMemoryAllocator { return "mock_cpu"; } + + size_t allocated_bytes() const + { + return allocated_bytes_; + } + + void reset_allocation_counter() + { + allocated_bytes_ = 0; + } + +private: + size_t allocated_bytes_{0}; }; class MockInferenceExecutor : public BackendInferenceExecutor