Trim CuPy memory pool during elevation mapping

Author: Idate96

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping.py

@@ -699,12 +699,18 @@ def copy_to_cpu(self, array, data, stream=None):
             stream (Union[None, cupy.cuda.stream.Stream, None, None, None, None, None, None, None]):
         """
         if type(array) == np.ndarray:
-            data[...] = array.astype(np.float32)
+            data[...] = array.astype(np.float32, copy=False)
         elif type(array) == cp.ndarray:
+            source = array.astype(np.float32, copy=False)
             if stream is not None:
-                data[...] = cp.asnumpy(array.astype(np.float32), stream=stream)
+                cp.asnumpy(source, stream=stream, out=data)
             else:
-                data[...] = cp.asnumpy(array.astype(np.float32))
+                cp.asnumpy(source, out=data)
+
+    def trim_memory_pool(self):
+        """Release cached CuPy allocator blocks that are not currently in use."""
+        pool.free_all_blocks()
+        cp.get_default_pinned_memory_pool().free_all_blocks()
 
     def exists_layer(self, name):
         """Check if the layer exists in elevation map or in the semantic map.

elevation_mapping_cupy/elevation_mapping_cupy/elevation_mapping_node.py

@@ -152,6 +152,11 @@ def get_ros_params(self) -> None:
         self.update_variance_fps = self.get_parameter('update_variance_fps').get_parameter_value().double_value
         self.time_interval = self.get_parameter('time_interval').get_parameter_value().double_value
         self.update_pose_fps = self.get_parameter('update_pose_fps').get_parameter_value().double_value
+        if not self.has_parameter('cupy_memory_pool_trim_interval_s'):
+            self.declare_parameter('cupy_memory_pool_trim_interval_s', 5.0)
+        self.cupy_memory_pool_trim_interval_s = float(
+            self.get_parameter('cupy_memory_pool_trim_interval_s').value
+        )
         self.initialize_tf_grid_size = self.get_parameter('initialize_tf_grid_size').get_parameter_value().double_value
         self.map_acquire_fps = self.get_parameter('map_acquire_fps').get_parameter_value().double_value
         self.publish_statistics_fps = self.get_parameter('publish_statistics_fps').get_parameter_value().double_value
@@ -406,6 +411,12 @@ def register_timers(self) -> None:
             self.time_interval,
             self.update_time
         )
+        self.timer_cupy_memory_pool = None
+        if self.cupy_memory_pool_trim_interval_s > 0.0:
+            self.timer_cupy_memory_pool = self.create_timer(
+                self.cupy_memory_pool_trim_interval_s,
+                self.trim_cupy_memory_pool
+            )
 
     def register_services(self) -> None:
         service_masked = self._resolve_service_name('masked_replace')
@@ -990,6 +1001,9 @@ def update_variance(self) -> None:
     def update_time(self) -> None:
         self._map.update_time()
 
+    def trim_cupy_memory_pool(self) -> None:
+        self._map.trim_memory_pool()
+
     def destroy_node(self) -> None:
         super().destroy_node()