Загрузите и просмотрите Waypoints

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

load wayPointSets.mat;

Визуализируйте два набора waypoints использование plot3 функция. Обратите внимание на то, что два набора содержат различные количества waypoints.

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.63044 seconds.
Initializing ROS master on http://192.168.0.10:51894.
Initializing global node /matlab_global_node_87080 with NodeURI http://bat6312glnxa64:45099/

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

pub = rospublisher("/waypoints","geometry_msgs/PoseArray","DataFormat","struct");
sub = rossubscriber("/waypoints","DataFormat","struct");

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

poseArrayMsg = rosmessage(pub);

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

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

% Specify the waypoint set to publish
wayPointsToPublish = wayPointSet1;

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

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

send(pub,poseArrayMsg);
pause(0.5)

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

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")

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

% Specify the waypoint set to publish
wayPointsToPublish = wayPointSet2;

% Populate the Pose Array Message
poseMsg = rosmessage("geometry_msgs/Pose","DataFormat","struct");
for k = 1:size(wayPointsToPublish,1)
    poseMsg.Position.X = wayPointsToPublish(k,1);
    poseMsg.Position.Y = wayPointsToPublish(k,2);
    poseMsg.Position.Z = wayPointsToPublish(k,3);
    poseArrayMsg.Poses(k) = 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")

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

rosshutdown

Shutting down global node /matlab_global_node_87080 with NodeURI http://bat6312glnxa64:45099/
Shutting down ROS master on http://192.168.0.10:51894.

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

Смотрите также

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

ROS пользовательская поддержка сообщения