Загрузка и просмотр путевых точек

Загрузите исходные данные, которые содержат путевые точки разной длины, которые необходимо опубликовать в одной теме, чтобы робот следовал. Файл 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