MATLAB Examples

Read A ROS Point Cloud Message In Simulink®

Read in a point cloud message from a ROS network. Calculate the center of mass of the coordinates and display the point cloud as an image.

This example requires Computer Vision System Toolbox® and Robotics System Toolbox®.

Start a ROS network.

rosinit
Initializing ROS master on http://bat7778maci:59837/.
Initializing global node /matlab_global_node_90250 with NodeURI http://bat7778maci:59841/

Load sample messages to send including a sample ipoint cloud message, ptcloud. Create a publisher to send an ROS PointCloud2 message on the '/ptcloud_test' topic. Specify the emssage type as 'sensor_msgs/PointCloud2'. Send the point cloud message.

exampleHelperROSLoadMessages
pub = rospublisher('/ptcloud_test','sensor_msgs/PointCloud2');
send(pub,ptcloud)

Open the Simulink® model for subscribing to the ROS message and reading in the point cloud from the ROS.

Ensure that the Subscribe block is subscribing to the '/ptcloud_test' topic. In the menu under Tools > Robot Operating System > Manage Array Lengths, verify the Data array has a maximum length greater than the sample image (9,830,400 points).

The model only displays the RGB values of the point cloud as an image. The XYZ output is used to calculate the center of mass (mean) of the coordinates using a MATLAB® function block. All NaN values are ignored.

open_system('read_point_cloud_example_model.slx')

Run the model. The Video Viewer shows the sample point cloud as an image. The output center of mass is [-0.2869 -0.0805 2.232] for this point cloud.

Stop the simluation and shut down the ROS network.

rosshutdown
Shutting down global node /matlab_global_node_90250 with NodeURI http://bat7778maci:59841/
Shutting down ROS master on http://bat7778maci:59837/.

The pointCloudCOM function block contains the following code for calculating the center of mass of the coordinates.

function comXYZ = pointCloudCOM(xyzPoints)
% Compute the center of mass of a point cloud based on the input NxMx3
% matrix.

% Turn matrix into vectors.
xPoints = reshape(xyzPoints(:,:,1),numel(xyzPoints(:,:,1)),1);
yPoints = reshape(xyzPoints(:,:,2),numel(xyzPoints(:,:,2)),1);
zPoints = reshape(xyzPoints(:,:,3),numel(xyzPoints(:,:,3)),1);

% Calculate the mean for each set of coordiantes.
xMean = mean(xPoints,'omitnan');
yMean = mean(yPoints,'omitnan');
zMean = mean(zPoints,'omitnan');

comXYZ = [xMean,yMean,zMean];

end