Add Conversion Functions between Typical Sensor Msgs and our Tensor

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
+  $<BUILD_INTERFACE:${include_dir}>
+  $<INSTALL_INTERFACE:include>
+)
+
+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()

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.

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 <memory>
+#include <string>
+#include <vector>
+
+#include <sensor_msgs/msg/image.hpp>
+#include <std_msgs/msg/header.hpp>
+
+#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<BackendMemoryAllocator> 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<sensor_msgs::msg::Image> & images, std::shared_ptr<BackendMemoryAllocator> 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<sensor_msgs::msg::Image> & images,
+  const std::string & encoding,
+  const std_msgs::msg::Header & header = std_msgs::msg::Header());
+
+}  // namespace ros_conversions
+}  // namespace deep_ros

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 <memory>
+
+#include <sensor_msgs/msg/imu.hpp>
+
+#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<BackendMemoryAllocator> allocator = nullptr);
+
+}  // namespace ros_conversions
+}  // namespace deep_ros

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 <memory>
+
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+#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<BackendMemoryAllocator> allocator = nullptr);
+
+}  // namespace ros_conversions
+}  // namespace deep_ros

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 <memory>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#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<BackendMemoryAllocator> allocator = nullptr);
+
+}  // namespace ros_conversions
+}  // namespace deep_ros

deep_conversions/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>deep_conversions</name>
+  <version>0.1.0</version>
+  <description>Conversion utilities between ROS 2 sensor messages and tensors</description>
+  <maintainer email="hello@watonomous.ca">WATonomous</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>deep_core</depend>
+
+  <test_depend>deep_test</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>