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.