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®.

Start a ROS network.

rosinit

Launching ROS Core...
Done in 0.42494 seconds.
Initializing ROS master on http://172.29.206.40:51907.
Initializing global node /matlab_global_node_91215 with NodeURI http://dcc689387glnxa64:41661/ and MasterURI http://localhost:51907.

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.

open_system("write_point_cloud_example_model.slx");

Ensure that the Publish block is publishing to the /point_cloud topic. Under the Simulation tab, select ROS Toolbox > Variable Size Arrays and verify the Data array has a maximum length greater than the length of the Data field in ptcloud (9,830,400).

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_91215 with NodeURI http://dcc689387glnxa64:41661/ and MasterURI http://localhost:51907.
Shutting down ROS master on http://172.29.206.40:51907.