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

rosinit

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

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

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

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

Убедитесь, что Subscribe блок подписывается на '/ptcloud_test' тема. На вкладке «Моделирование» выберите ROS Toolbox > Массивы переменных размеров и проверьте 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