Запустите сеть ROS.

rosinit

Initializing ROS master on http://ah-rhosea:11311/.
Initializing global node /matlab_global_node_07639 with NodeURI http://ah-rhosea:51851/

Загрузите демонстрационные сообщения, чтобы отправить включая сообщение облака точки выборки, ptcloud. Создайте издателя, чтобы отправить PointCloud2 ROS обменивайтесь сообщениями на '/ptcloud_test' тема. Задайте тип сообщения как 'sensor_msgs/PointCloud2'. Отправьте сообщение облака точек.

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

Откройте модель Simulink® для подписки на сообщение ROS и чтения в облаке точек от ROS.

Убедитесь, что Subscribe блок подписывается на '/ptcloud_test' тема. Под вкладкой Simulation выберите ROS Toolbox> Variable Size Arrays и проверьте Data массив имеет максимальную длину, больше, чем демонстрационное изображение (9 830 400 точек).

Модель только отображает значения RGB облака точек как изображение. XYZ выход используется, чтобы вычислить центр массы (среднее значение) координат с помощью блока MATLAB function. Весь NaN значения проигнорированы.

open_system('read_point_cloud_example_model.slx')

Запустите модель. Video Viewer показывает облако точки выборки изображением. Выходным центром массы является [-0.2869 -0.0805 2.232] для этого облака точек.

Остановите симуляцию и закройте сеть ROS.

rosshutdown

Shutting down global node /matlab_global_node_07639 with NodeURI http://ah-rhosea:51851/
Shutting down ROS master on http://ah-rhosea:11311/.

pointCloudCOM функциональный блок содержит следующий код для вычисления центра массы координат.

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