Write A ROS Point Cloud Message In Simulink

This example shows how to write a point cloud message and publish it to a ROS network in Simulink®.

Initialize ROS network.

rosinit

Launching ROS Core...
Creating Python virtual environment for ros1.Done.
Adding required Python packages to virtual environment.Done.
Copying include folders.Done.
Copying libraries.Done.
.....Done in 5.7445 seconds.
Initializing ROS master on http://172.19.136.75:50633.
Initializing global node /matlab_global_node_74311 with NodeURI http://vdi-wd1bgl-223:62026/ and MasterURI http://localhost:50633.

Load the 3D coordinate locations and color data for the sample point cloud. Create a subscriber to receive a ROS PointCloud2 message from the /point_cloud topic. Specify the message type as sensor_msgs/PointCloud2 and the data format as a struct.

exampleHelperROSLoadPointCloudData
sub = rossubscriber("/point_cloud","sensor_msgs/PointCloud2",DataFormat="struct");

Open the Simulink® model for writing a ROS point cloud message and publishing it to a ROS network. Ensure that the Publish block is publishing to the /point_cloud topic.

open_system("write_point_cloud_example_model.slx");

Warning: Unrecognized function or variable 'CloneDetectionUI.internal.CloneDetectionPerspective.register'.

Under the Simulation tab, select ROS Toolbox > Variable Size Arrays. This sets the maximum length for ROS message fields in the model which have variable sized arrays. For the sensor_msgs/PointCloud2 messages in the model, ensure that the maximum length of these variable size array fields are set based on the following considerations:

• Fields array — Denotes the number of fields in the sensor_msgs/PointCloud2 message. Because you are setting x, y, z and rgb fields in the sensor_msgs/PointCloud2 message, the maximum length of the Fields array must be 4.

• Data array — Denotes the point cloud data in the sensor_msgs/PointCloud2 in the form of a uint8 array. Note that there are 307200 points in the sample point cloud with single datatype. Because you are representing each point in the sensor_msgs/PointCloud2 message in the form of 4 fields, and each single element requires 4 uint8 elements for representation, the maximum length of the Data array must be 307200 x 4 x 4 = 4915200.

The model writes the color and location data to a ROS point cloud message. Using a Header Assignment block, the frame ID of the message header is set to /camera_rgb_optical_frame.

Run the model to publish the message.

sim("write_point_cloud_example_model.slx");

The point cloud message is published to the ROS network and received by the subscriber created earlier. Visualize the point cloud with the rosPlot function. rosPlot automatically extracts the x-, y-, and z- coordinates and shows them in a 3-D scatter plot. The rosPlot function ignores points with NaN values for coordinates regardless of the presence of color values at those points.

rosPlot(sub.LatestMessage);

Close the model and shut down the ROS network.

close_system("write_point_cloud_example_model.slx",0);
rosshutdown

Shutting down global node /matlab_global_node_74311 with NodeURI http://vdi-wd1bgl-223:62026/ and MasterURI http://localhost:50633.
Shutting down ROS master on http://172.19.136.75:50633.