Subscriber support for PointCloud2

[LauraConnolly]

We have explored the integration of a PointCloud2 subscriber on this branch: https://github.com/rosmed/slicer_ros2_module/tree/feature_pointcloud2

Here is a brief description of the data structure:
A PointCloud2 is a data structure for storing arrays of points such that the points can have multiple attributes like x,y,z,intensity or R,A,S,label. The PointCloud2 is structured like a table with a width and height - this is how we can determine the size of the point cloud. In each of the elements of the table, or points in the cloud, there is a field that tells you the name of the attribute (ie. "x"), the byte offset of that field in the point for accessing, the type of data (eg. FLOAT), and the number of elements in the field (usually one). You can also determine whether or not the PointCloud is big or little endian, the number of bytes per element and per row, whether or not the point cloud contains naan points or not, and finally, the data as an array.

• In this commit: 164c388 - I tried to iterate through the point cloud and append it to a vtkPoints object. This implementation is not feasible for large point clouds because it is too slow.
• In this commit: 633bb9d - I updated the implementation to directly copy the point data into vtkPoints instead. This improved the speed but there are still issues with the display on the Slicer side because there are too many points to visualize with the standard tools available in 3D Slicer.

We need to decide what the data structure on the Slicer side should be which will impact the final implementation (ie. visualization, computation). Also we should decide what attributes we want to support (ie. in a point cloud with x,y,z,rgba - should we send rgba and how do we handle it?)

[adeguet1]

Ideally we should provide a Subscriber Node that converts directly into a vtkPolyData object instead of vtkPoints. Then users wouldn't have to convert the vtkPoints to poly data to display it. The following code from Xinyuan Huang uses the existing point cloud subscriber and has to convert to poly data in callback:

def pointCloudUpdated(caller, event):
    global modelNode
    print("point cloud updated")
    points = subscriber.GetLastMessage()
    polyData = vtk.vtkPolyData()
    polyData.SetPoints(points)
    sphere = vtk.vtkSphereSource()
    sphere.SetRadius(0.001)
    glyphFilter = vtk.vtkGlyph3D()
    glyphFilter.SetInputData(polyData)
    glyphFilter.SetSourceConnection(sphere.GetOutputPort())
    modelNode.SetPolyDataConnection(glyphFilter.GetOutputPort())

if __name__ == '__main__':
    rosLogic = slicer.util.getModuleLogic('ROS2')
    rosNode = rosLogic.GetDefaultROS2Node()
    subscriber = rosNode.CreateAndAddSubscriberNode('vtkMRMLROS2SubscriberPointCloud2Node', 'points/xyzrgba')
    observerId = subscriber.AddObserver('ModifiedEvent', pointCloudUpdated)
    modelNode = slicer.mrmlScene.AddNewNodeByClass("vtkMRMLModelNode", "PointCloudModel")
    modelNode.CreateDefaultDisplayNodes()

If we provide a vtkROS2Slicer conversion method from the PointCloud2 to vtkPolyData, that would save the conversion to and from vtkPoints. This conversion is likely using random iterators and some kind of memory allocation which might be the bottleneck we run into.

Note: It might be possible to use the pcl library mesh2vtk but I don't know how efficient this is.