Загрузка и просмотр ППМ

Загрузите исходные данные, содержащие ППМ разной длины, которые необходимо опубликовать по одной теме, чтобы робот следовал за ними. Файл MAT wayPointSets.mat загружает два набора ППМ. Они могут использоваться для указания сообщения массива позы. ППМ имеют форму координат XYZ.

load wayPointSets.mat;

Визуализация двух наборов ППМ с помощью plot3 функция. Обратите внимание, что два набора содержат разное количество ППМ.

figure
plot3(wayPointSet1(:,1),wayPointSet1(:,2),wayPointSet1(:,3),'*-')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 1')

figure
plot3(wayPointSet2(:,1),wayPointSet2(:,2),wayPointSet2(:,3),'*-r')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 2')

Инициализация и настройка сети ROS

Использовать rosinit для создания основной записи ROS в MATLAB и запуска глобального узла, связанного с основной записью.

rosinit

Launching ROS Core...
Done in 0.91141 seconds.
Initializing ROS master on http://192.168.0.10:51650.
Initializing global node /matlab_global_node_99728 with NodeURI http://bat6315glnxa64:46377/

Использовать rospublisher создание издателя ROS для отправки сообщений типа geometry_msgs/PoseArray. Укажите имя раздела как /waypoints. Добавление подписчика ROS, подписавшегося на опубликованный раздел с помощью rossubscriber.

pub = rospublisher('/waypoints','geometry_msgs/PoseArray');
sub = rossubscriber('/waypoints');

Использовать rosmessage для создания пустого сообщения на основе темы, опубликованной издателем, pub.

poseArrayMsg = rosmessage(pub);

Заполнить сообщение и опубликовать

Укажите переменную рабочей области, соответствующую набору ППМ, который требуется опубликовать. Затем заполните массив позы geometry_msgs/Pose сообщения. Присвойте поля позиции XYZ отдельных элементов сообщения о позе из данных набора ППМ. Продолжайте добавлять новые отдельные элементы сообщения о позе до тех пор, пока сообщение массива позы не будет содержать все данные набора ППМ.

% Specify the waypoint set to publish
wayPointsToPublish = wayPointSet1;

% Populate the pose array message
for i = 1:size(wayPointsToPublish,1)
    poseMsg = rosmessage('geometry_msgs/Pose');
    poseMsg.Position.X = wayPointsToPublish(i,1);
    poseMsg.Position.Y = wayPointsToPublish(i,2);
    poseMsg.Position.Z = wayPointsToPublish(i,3);
    poseArrayMsg.Poses(i) = poseMsg;  
end

Используйте send функция для публикации сообщения массива позы в разделе /waypoints, используя объект издателя ROS, pub.

send(pub,poseArrayMsg);
pause(0.5)

Просмотр данных сообщения массива позы, полученных абонентом, с помощью LatestMessage имущества Subscriber объект. Использовать horzcat объединяют информацию положения, извлеченную из принятого сообщения, в структурный массив для целей визуализации. Использовать plot3 визуализировать ППМ, полученные абонентом. Следует отметить, что визуализация совпадает с визуализацией соответствующего набора исходных данных ППМ.

receivedPoseArrayMsg1 = sub.LatestMessage;
waypointPositions1 = horzcat(receivedPoseArrayMsg1.Poses.Position);

figure
plot3([waypointPositions1.X],[waypointPositions1.Y],[waypointPositions1.Z],'*-')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 1 Received Through ROS Topic')

Теперь опубликуйте второй ППМ с помощью той же процедуры. Заполните сообщение массива позы новым набором информации ППМ.

% Specify the waypoint set to publish
wayPointsToPublish = wayPointSet2;

% Populate the Pose Array Message
for i = 1:size(wayPointsToPublish,1)
    poseMsg = rosmessage('geometry_msgs/Pose');
    poseMsg.Position.X = wayPointsToPublish(i,1);
    poseMsg.Position.Y = wayPointsToPublish(i,2);
    poseMsg.Position.Z = wayPointsToPublish(i,3);
    poseArrayMsg.Poses(i) = poseMsg;  
end

Используйте send функция для публикации нового сообщения массива позы в том же разделе через тот же объект издателя ROS, pub.

send(pub,poseArrayMsg);
pause(0.5)

Визуализируйте данные сообщения массива позы, полученные абонентом, следуя той же процедуре, что и ранее.

receivedPoseArrayMsg2 = sub.LatestMessage;
waypointPositions2 = vertcat(receivedPoseArrayMsg2.Poses.Position);

figure
plot3([waypointPositions2.X],[waypointPositions2.Y],[waypointPositions2.Z],'*-r')
grid on
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Waypoint Set 2 Received Through ROS Topic')

Визуализация совпадает с визуализацией соответствующего набора исходных данных ППМ, указывая на успешную трансляцию двух наборов массивов позы с разной длиной по одной теме. Использовать rosshutdown для отключения сети ROS в MATLAB. При этом завершает работу хозяина ROS, инициализированного rosinit и удаляет глобальный узел. Используя rosshutdown является рекомендуемой процедурой после завершения работы с сетью ROS.

rosshutdown

Shutting down global node /matlab_global_node_99728 with NodeURI http://bat6315glnxa64:46377/
Shutting down ROS master on http://192.168.0.10:51650.

Если данные набора ППМ содержат информацию об ориентации, их можно заполнить в полях ориентации кватерниона отдельных элементов сообщения о позе перед публикацией. Публикация сообщений типа nav_msgs/Path, используйте ту же процедуру, но укажите отдельные элементы сообщения позы как geometry_msgs/PoseStamped тип. Для публикации сообщений любого другого типа укажите соответствующий вложенный тип сообщения в качестве отдельных элементов массива и убедитесь, что исходный набор данных содержит необходимую информацию, которую необходимо опубликовать.

См. также

Работа с сообщениями ROS в Simulink

Поддержка настраиваемых сообщений ROS