Voxel grid implementation

.gitignore

@@ -3,3 +3,4 @@ __pycache__
 .vscode/
 
 watod-config.local.sh
+utils/voxel/spconv

autonomy/behaviour/octo_map/launch/octo_map.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            name='octo_map_node',
+            package='octo_map',
+            executable='octo_map_node',
+            output='screen',
+            emulate_tty=True),
+    ])

autonomy/behaviour/octo_map/octo_map/octo_map_node.py

@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+import torch
+from spconv.pytorch.utils import PointToVoxel
+import cv2
+from cv_bridge import CvBridge
+
+from sensor_msgs.msg import Image, PointCloud2, PointField, CameraInfo
+from std_msgs.msg import Header
+import sensor_msgs_py.point_cloud2 as pc2
+
+class OctoMapNode(Node):
+    def __init__(self):
+        super().__init__('octo_map_node')
+
+        # Subscribe to processed perception data
+        self.rgb_sub = self.create_subscription(
+            Image, '/perception/rgb_processed', self.rgb_callback, 10)
+        self.depth_sub = self.create_subscription(
+            Image, '/perception/depth_processed', self.depth_callback, 10)
+        self.info_sub = self.create_subscription(
+            CameraInfo, '/perception/camera_info', self.info_callback, 10)
+
+        self.octo_pub = self.create_publisher(PointCloud2, '/behaviour/octo_map', 10)
+
+        self.rgb_image = None
+        self.depth_image = None
+        self.bridge = CvBridge()
+
+        # Camera intrinsics (will be updated from camera info)
+        self.fx, self.fy, self.cx, self.cy = 525.0, 525.0, 320.0, 240.0
+
+        self.voxel_size = 0.04  
+        self.max_range = 3.5    
+
+        self.get_logger().info('OctoMap Node started - subscribing to processed perception data')
+
+    def info_callback(self, msg):
+        """Update camera intrinsics from perception node."""
+        self.fx = msg.k[0]
+        self.fy = msg.k[4]
+        self.cx = msg.k[2]
+        self.cy = msg.k[5]
+        self.get_logger().info(f'Updated camera intrinsics: fx={self.fx}, fy={self.fy}')
+
+    def rgb_callback(self, msg):
+        """Receive processed RGB image from perception node."""
+        self.rgb_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
+        self.process_rgbd_if_ready()
+        self.get_logger().info('Received processed RGB image from perception')
+
+    def depth_callback(self, msg):
+        """Receive processed depth image from perception node."""
+        self.depth_image = self.bridge.imgmsg_to_cv2(msg, 'passthrough')
+        self.process_rgbd_if_ready()
+        self.get_logger().info('Received processed depth image from perception')
+
+    # TODO: Implement point cloud generation and octomap updating
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OctoMapNode()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

autonomy/behaviour/octo_map/package.xml

@@ -0,0 +1,20 @@
+<?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>octo_map</name>
+  <version>0.0.0</version>
+  <description>OctoMap generation for 3D mapping</description>
+  <maintainer email="watonomous@uwaterloo.ca">watonomous</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

autonomy/behaviour/octo_map/resource/octo_map

@@ -0,0 +1 @@
+voxel_grid

autonomy/behaviour/octo_map/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/voxel_grid
+[install]
+install-scripts=$base/lib/voxel_grid

autonomy/behaviour/octo_map/setup.py

@@ -0,0 +1,35 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'octo_map'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        # Install marker file in the package index
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        # Include our package.xml file
+        (os.path.join('share', package_name), ['package.xml']),
+        # Include all launch files.
+        (os.path.join('share', package_name, 'launch'),
+         glob(os.path.join('launch', '*.launch.py'))),
+        # Include all config files.
+        (os.path.join('share', package_name, 'config'),
+         glob(os.path.join('config', '*.yaml'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='watonomous',
+    maintainer_email='watonomous@uwaterloo.ca',
+    description='OctoMap generation for 3D mapping',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'octo_map_node = octo_map.octo_map_node:main'
+        ],
+    },
+)

autonomy/behaviour/voxel_grid/launch/voxel_grid.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            name='voxel_grid_node',
+            package='voxel_grid',
+            executable='voxel_grid_node',
+            output='screen',
+            emulate_tty=True),
+    ])

autonomy/behaviour/voxel_grid/package.xml

@@ -0,0 +1,20 @@
+<?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>voxel_grid</name>
+  <version>0.0.0</version>
+  <description>Voxel grid generation for depth camera data</description>
+  <maintainer email="watonomous@uwaterloo.ca">watonomous</maintainer>
+  <license>Apache-2.0</license>
+
+  <depend>rclpy</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

autonomy/behaviour/voxel_grid/resource/voxel_grid

@@ -0,0 +1 @@
+voxel_grid

autonomy/behaviour/voxel_grid/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/voxel_grid
+[install]
+install-scripts=$base/lib/voxel_grid

autonomy/behaviour/voxel_grid/setup.py

@@ -0,0 +1,35 @@
+import os
+from glob import glob
+from setuptools import setup
+
+package_name = 'voxel_grid'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        # Install marker file in the package index
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        # Include our package.xml file
+        (os.path.join('share', package_name), ['package.xml']),
+        # Include all launch files.
+        (os.path.join('share', package_name, 'launch'),
+         glob(os.path.join('launch', '*.launch.py'))),
+        # Include all config files.
+        (os.path.join('share', package_name, 'config'),
+         glob(os.path.join('config', '*.yaml'))),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='watonomous',
+    maintainer_email='watonomous@uwaterloo.ca',
+    description='Voxel grid generation for depth camera data',
+    license='Apache-2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'voxel_grid_node = voxel_grid.voxel_grid_node:main'
+        ],
+    },
+)