Publish an occupancy grid

Author: nhewitt99

kiss_slam/ros/Dockerfile

@@ -0,0 +1,16 @@
+# KISS-SLAM ROS 2 Wrapper
+This directory provides a ROS 2 wrapper for some of KISS-SLAM's functionality.
+The kiss_slam_node receives point cloud messages and publishes odometry and a transform between the map frame and laser frame.
+An occupancy grid is also published, but this is currently incomplete and does not incorporate loop closures.
+
+Currently tested on Humble.
+
+### How to build
+Copy this directory into your workspace, then build, source, and run as normal provided that `kiss-slam` is already installed in your environment.
+
+### How to run
+By default, the node will listen for the topic `/points`.
+This can be changed at runtime with a parameter:
+```
+ros2 run kiss_slam_ros kiss_slam_node --ros-args -p points_topic:=your_pointcloud_topic
+```

kiss_slam/ros/README.md

@@ -8,7 +8,9 @@
 from geometry_msgs.msg import Pose, TransformStamped
 from sensor_msgs_py import point_cloud2
 from sensor_msgs.msg import PointCloud2
-from nav_msgs.msg import Odometry
+from nav_msgs.msg import Odometry, OccupancyGrid
+
+from kiss_slam.voxel_map import VoxelMap
 
 
 def pc2_to_numpy(msg: PointCloud2):
@@ -96,4 +98,34 @@ def build_odometry(header: Header, pose: Pose, position_cov: float, orientation_
     odom.pose.covariance[21] = orientation_cov
     odom.pose.covariance[28] = orientation_cov
     odom.pose.covariance[35] = orientation_cov
-    return odom
+    return odom
+
+def build_map(header: Header, occupancy_2d: np.ndarray, min_voxel_idx: tuple, resolution: float):
+    """Convenience function for packing a map message.
+
+    :param header: A header to provide timestamp and frame_id
+    :type header: Header
+    :param occupancy_2d: An occupancy grid supplied by an OccupancyGridMapper
+    :type occupancy_2d: np.ndarray
+    :param min_voxel_idx: The lowest active voxel index, used to calculate map origin
+    :type min_voxel_idx: tuple
+    :param resolution: Size of a grid cell in m
+    :type resolution: float
+    :return: An occupancy grid message containing input information
+    :rtype: OccupancyGrid
+    """
+    map_msg = OccupancyGrid()
+    map_msg.header = header
+    map_msg.info.resolution = resolution
+    map_msg.info.width = occupancy_2d.shape[0]
+    map_msg.info.height = occupancy_2d.shape[1]
+    map_msg.info.origin.position.x = min_voxel_idx[0] * resolution
+    map_msg.info.origin.position.y = min_voxel_idx[1] * resolution
+
+    # Rotate, invert probabilities, and scale to [0, 100]
+    occupancy_image = np.rint(100 * (1 - occupancy_2d)).astype(int)
+    occupancy_image = np.rot90(occupancy_image)
+    occupancy_image = np.flip(occupancy_image, axis=0)
+    map_msg.data = occupancy_image.ravel().tolist()
+
+    return map_msg

kiss_slam/ros/docker-compose.yaml

@@ -9,14 +9,16 @@
 
 from kiss_slam.slam import KissSLAM
 from kiss_slam.config import load_config
+from kiss_slam.occupancy_mapper import OccupancyGridMapper
+from map_closures import map_closures
 
-from kiss_slam_ros.conversions import pc2_to_numpy, build_transform, build_odometry, matrix_to_pose
+from kiss_slam_ros.conversions import pc2_to_numpy, build_transform, build_odometry, build_map, matrix_to_pose
 from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult
 from rclpy.parameter import Parameter
 from std_msgs.msg import Header
 from tf2_ros import TransformBroadcaster
 from sensor_msgs.msg import PointCloud2
-from nav_msgs.msg import Odometry
+from nav_msgs.msg import Odometry, OccupancyGrid
 
 class KissSLAMNode(Node):
 
@@ -52,9 +54,15 @@ def __init__(self):
 
         # Publishers
         self.odom_publisher = self.create_publisher(Odometry, 'odom', 10)
+        self.map_publisher = self.create_publisher(OccupancyGrid, 'map', 10)
         self.tf_broadcaster = TransformBroadcaster(self)
 
-        self.slam = KissSLAM(load_config(None))
+        self.slam_config = load_config(None)
+        self.slam = KissSLAM(self.slam_config)
+        self.mapper = OccupancyGridMapper(self.slam_config.occupancy_mapper)
+        self.ref_ground_alignment = None
+        self.min_voxel_idx = (0, 0)
+
         self.add_on_set_parameters_callback(self.parameter_callback)
 
     def parameter_callback(self, params: list):
@@ -81,7 +89,8 @@ def cloud_callback(self, in_msg: PointCloud2):
         :param in_msg: New points to injest
         :type in_msg: PointCloud2
         """
-        self.slam.process_scan(pc2_to_numpy(in_msg), np.empty((0,)))
+        pcd = pc2_to_numpy(in_msg)
+        self.slam.process_scan(pcd, np.empty((0,)))
 
         pose = matrix_to_pose(self.slam.poses[-1])
         header = Header()
@@ -93,6 +102,31 @@ def cloud_callback(self, in_msg: PointCloud2):
 
         odom = build_odometry(header, pose, self.position_covariance, self.orientation_covariance)
         self.odom_publisher.publish(odom)
+        
+        self.update_mapper(pcd)
+        map_msg = build_map(header, self.occupancy_2d, self.min_voxel_idx, self.slam_config.occupancy_mapper.resolution)
+        self.map_publisher.publish(map_msg)
+
+    def update_mapper(self, pcd: np.ndarray):
+        """Use a point cloud to update the node's OccupancyGridMapper and store the new 2d grid.
+        Also update the minimum active voxel xy idx so the map origin can be placed.
+
+        :param pcd: Latest point cloud received
+        :type pcd: np.ndarray
+        """
+        if self.ref_ground_alignment is None:
+            self.ref_ground_alignment = map_closures.align_map_to_local_ground(
+                self.slam.voxel_grid.open3d_pcd_with_normals().point.positions.cpu().numpy(),
+                self.slam_config.odometry.mapping.voxel_size,
+            )
+        
+        self.mapper.integrate_frame(
+            pcd, self.ref_ground_alignment @ self.slam.poses[-1]
+        )
+        self.mapper.compute_3d_occupancy_information()
+        self.mapper.compute_2d_occupancy_information()
+        self.occupancy_2d = self.mapper.occupancy_grid
+        self.min_voxel_idx = (self.mapper.lower_bound[0], self.mapper.lower_bound[1])
 
 
 def main(args=None):
@@ -102,9 +136,6 @@ def main(args=None):
 
     rclpy.spin(kiss)
 
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
     kiss.destroy_node()
     rclpy.shutdown()