Fix memory consumption issue

[siferati]

Description

• Implementation of the solution proposed in #270.

By calling openvdb::Grid::prune at the end of SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap we can free the memory being taken by expired voxels.

I measured execution time of the prune function and it was under 1ms, so I don't think it should have any major impact on performance.

const auto start = ros::WallTime::now();
_grid->pruneGrid();
ROS_ERROR_STREAM("dt: " << (ros::WallTime::now() - start).toNSec() / 1e6);

Demo

After this fix, the memory taken by the grid goes down every time the voxels are expired.

However, note that htop doesn't reflect this and the memory consumption never goes down (although it's a lot lower than before). I'm not sure why this is happening, but maybe something related with linux internal workings? Even though the process isn't using the memory anymore, linux still thinks it is? I'm not too familiar with this, but at least from the logs it's clear to see that the memory consumption of the grid does indeed go down every time voxels are expired.

You can also test this without the change in this PR:

• Let the robot move and memory consumption increase (let's say 10mb)
• Manually call clear costmap service, which will clear the grid
• Confirm that the logs do indeed show that the grid was cleared and memory was freed
• htop still shows stvl taking those 10mb of memory

Before

https://github.com/SteveMacenski/spatio_temporal_voxel_layer/assets/15384781/00b11a9d-d68e-43ca-ba9c-505d289494b7

After

https://github.com/SteveMacenski/spatio_temporal_voxel_layer/assets/15384781/cfc8e78e-6bc4-446a-9d22-4d8450c4526b

[SteveMacenski]

Seems reasonable to me! Can you just verify the complexity of the runtime of this sparsification process? I would just like to understand if this is a constant 1ms no matter the size of the grid or number of voxels expired being removed or if its proportionate to something that we may need to document if it gets really high in some situations

[siferati]

@SteveMacenski I'm not too familiar with openvdb, but since they use a b-tree as the underlying structure I guess the complexity would scale logarithmically based on the amount of voxels in the tree?

I did some quick tests on a real robot. Pruning the tree with ~100K voxels / ~1MB took less than 1ms. Pruning with ~1M voxels / \~10MB took 2\~3ms.