%% Read A ROS Point Cloud Message In Simulink(R)
% 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 Toolbox(R) and Robotics 
% System Toolbox(R). 


%%
% Start a ROS network.
rosinit

%%
% Load sample messages to send including a sample point cloud message, |ptcloud|. Create a 
% publisher to send an ROS |PointCloud2| message on the |'/ptcloud_test'| topic.
% Specify the message type as |'sensor_msgs/PointCloud2'|. Send the point
% cloud message.
exampleHelperROSLoadPCloud
pub = rospublisher('/ptcloud_test','sensor_msgs/PointCloud2');
send(pub,ptcloud)
%%
% Open the Simulink(R) 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. Under the
% *Simulation* tab, select *ROS Toolbox* > *Variable Size Arrays* and 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.
%%
%
% <<../read_point_cloud_video_viewer.png>>
%
%%
% Stop the simulation and shut down the ROS network.
rosshutdown

%% 
% 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 coordinates.
xMean = mean(xPoints,'omitnan');
yMean = mean(yPoints,'omitnan');
zMean = mean(zPoints,'omitnan');

comXYZ = [xMean,yMean,zMean];

end

%% 
% Copyright 2012 The MathWorks, Inc.