Улучшите эффективность ROS, используя структуры сообщений

Открыть Live Script

Этот пример демонстрирует использование структур сообщений ROS, а также их преимущества и различий от объектов сообщений.

Структуры сообщений имеют лучшую эффективность по сравнению с объектами при выполнении начального создания, считывании их из файлов rosbag, доступе к вложенным свойствам и выполнении операций связи по сети ROS. Кроме того, структуры сообщений являются единственным поддерживаемым форматом сообщений при генерации кода через MATLAB Coder™.

Основы структуры сообщений

Объекты сообщений ROS являются образцами классов, заданными специально для каждого типа сообщений.

msgObj = rosmessage("nav_msgs/Path");
class(msgObj)

ans = 
'ros.msggen.nav_msgs.Path'

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

showdetails(msgObj)

  Header    
    Stamp      
      Sec  :  0
      Nsec :  0
    Seq     :  0
    FrameId :  
  Poses

Введены структуры сообщений ROS для улучшения эффективности использования сообщений ROS. Каждое сообщение является типом данных структуры MATLAB ® с теми же полями, что и свойства объектов сообщений ROS.

msgStruct = rosmessage("nav_msgs/Path","DataFormat","struct")

msgStruct = struct with fields:
    MessageType: 'nav_msgs/Path'
         Header: [1x1 struct]
          Poses: [0x1 struct]

class(msgStruct)

ans = 
'struct'

Обновление существующего кода для использования структур

Чтобы обновить существующий код, который использует объекты, предусмотрены два общих рабочих процессов с шагами, необходимыми для их обновления.

Рабочий процесс коммуникации

Этот пример кода показывает, как отправлять и получать сообщения по сети ROS.

% Setup ROS network
rosinit

Launching ROS Core...
Done in 0.77187 seconds.
Initializing ROS master on http://192.168.0.10:59565.
Initializing global node /matlab_global_node_01839 with NodeURI http://bat6315glnxa64:45457/

stringPub = rospublisher("/chatter","std_msgs/String");
stringSub = rossubscriber("/chatter","std_msgs/String");

% Set message field and send message
stringMsg = rosmessage("std_msgs/String");
stringMsg.Data = 'Hello World!';
send(stringPub,stringMsg)

% Wait for message to be received and then check the value
pause(2)
showdetails(stringSub.LatestMessage)

  Data :  Hello World!

Как обновить

Установите аргумент имя-значение формата данных издателя и подписчика.

stringPub = rospublisher("/chatter","std_msgs/String","DataFormat","struct");
stringSub = rossubscriber("/chatter","std_msgs/String","DataFormat","struct");

Обновление формата данных для rosmessage функция также.

stringMsg = rosmessage("std_msgs/String","DataFormat","struct");
stringMsg.Data = 'Hello World!';
send(stringPub,stringMsg)

Кроме того, rosmessage можно использовать функцию object для издателя. Этот синтаксис создает сообщение, соответствующее формату, заданному в издателе, и является наиболее эффективным способом обеспечения совместимости между сообщением и издателем.

stringMsg = rosmessage(stringPub);
stringMsg.Data = 'Hello World!';
send(stringPub,stringMsg)

Поскольку структуры не имеют функций объекта, предусмотрены новые функции для обработки общих задач сообщений ROS. Чтобы показать подробные сведения о структурном сообщении, используйте rosShowDetails функция. Для просмотра всех новых функций перейдите к разделу Функции обработки сообщений.

% Wait for message to be received and then check the value
pause(2)
rosShowDetails(stringSub.LatestMessage)

ans = 
    '
       MessageType :  std_msgs/String
       Data        :  Hello World!'

Рабочий процесс чтения Rosbag

Для примера, который читает сообщения от rosbag, задайте DataFormat аргумент имя-значение для readMessages функция и все издатели, которые вы используете для отправки этих сообщений.

% Extract message from rosbag
msgType = "nav_msgs/Odometry";
bag = rosbag("ex_multiple_topics.bag");
bagSelect = select(bag,"MessageType",msgType);
odomMsgs = readMessages(bagSelect,"DataFormat","struct");
odomMsg = odomMsgs{1}

odomMsg = struct with fields:
     MessageType: 'nav_msgs/Odometry'
          Header: [1x1 struct]
    ChildFrameId: 'base_footprint'
            Pose: [1x1 struct]
           Twist: [1x1 struct]

% Create publisher and send first message
odomPub = rospublisher("/odom",msgType,"DataFormat","struct");
send(odomPub,odomMsg)

Функции обработки сообщений

Поскольку функции на объектах сообщений ROS не могут использоваться со структурами сообщений, для обработки сообщений были введены новые функции. Этот список включает функции для чтения данных из или записи данных в специализированные сообщения.

Изменения поведения

Класс Handle и Structure

Важным фактором при преобразовании кода является то, что объекты сообщения ROS являются указателями, что означает, что объекты сообщения передаются путем ссылки, когда предоставляются как входные параметры функций. Если сообщение изменено в функции, изменение применяется и к сообщению в рабочей области MATLAB ®.

msgObj = rosmessage("geometry_msgs/Pose2D");
pose = [1 2 3];
exampleHelperWritePoseToMsgObj(msgObj,pose)
disp(msgObj)

  ROS Pose2D message with properties:

    MessageType: 'geometry_msgs/Pose2D'
              X: 1
              Y: 2
          Theta: 3

  Use showdetails to show the contents of the message

function exampleHelperWritePoseToMsgObj(pointMsg,pose)
pointMsg.X = pose(1);
pointMsg.Y = pose(2);
pointMsg.Theta = pose(3);
end

Структуры сообщений передают свое значение только при вводе в функции. Если структура сообщения изменена внутри функции, это изменение будет применяться только к структуре в пределах возможностей этой функции. Чтобы сделать изменение доступным вне функции, должна быть возвращена структура сообщений.

msgStruct = rosmessage("geometry_msgs/Pose2D","DataFormat","struct");
pose = [1 2 3];

% With no return, the message structure will not change
exampleHelperWritePoseToMsgObj(msgStruct, pose)
disp(msgStruct)

    MessageType: 'geometry_msgs/Pose2D'
              X: 0
              Y: 0
          Theta: 0

% When returned from the function, the message can be overwritten.
msgStruct = exampleHelperWritePoseToMsgStruct(msgStruct, pose);
disp(msgStruct)

    MessageType: 'geometry_msgs/Pose2D'
              X: 1
              Y: 2
          Theta: 3

function pointMsg = exampleHelperWritePoseToMsgStruct(pointMsg,pose)
pointMsg.X = pose(1);
pointMsg.Y = pose(2);
pointMsg.Theta = pose(3);
end

Это относится также к специализированным функциям обработки сообщений. Функции записи, которые обновляют значения сообщений, теперь имеют выходные аргументы для предоставления обновленной структуры сообщений.

image = imread('imageMap.png');

% Message object
msg = rosmessage("sensor_msgs/Image");
msg.Encoding = 'rgb8';
writeImage(msg,image)
imshow(readImage(msg))

Figure contains an axes. The axes contains an object of type image.

% Message structure
msg = rosmessage("sensor_msgs/Image","DataFormat","struct");
msg.Encoding = 'rgb8';
msg = rosWriteImage(msg,image);
imshow(rosReadImage(msg))
close

Арифматическое время и длительность

Структуры сообщений времени и длительности ROS не могут поддерживать перегрузку операторов так же, как это делают объекты времени и длительности. Операции арифметики и сравнения должны быть выполнены путем преобразования структуры времени или длительности в числовое значение секунд, выполнения операции и последующего воссоздания структуры времени или длительности при необходимости.

% Periodically update message timestamp with objects
msg = rosmessage("std_msgs/Header");
runFor = rosduration(2);
tNow = rostime("now");
tEnd = tNow + runFor;
while tNow < tEnd
    msg.Stamp = tNow;
    % Message may be sent here
    pause(1)
    tNow = rostime("now");
end
% Periodically update message timestamp with structures
msg = rosmessage("std_msgs/Header","DataFormat","struct");
runFor = 2;
tNow = rostime("now","DataFormat","struct");
tNowSec = tNow.Sec + tNow.Nsec*1e-9;
tEndSec = tNowSec + runFor;
while tNowSec < tEndSec
    msg.Stamp = tNow;
    % Message may be sent here
    pause(1)
    tNow = rostime("now","DataFormat","struct");
    tNowSec = tNow.Sec + tNow.Nsec*1e-9;
end

Принуждение поля данных

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

msg = rosmessage("std_msgs/Int8");
msg.Data = 20;
class(msg.Data)

ans = 
'int8'

Структуры сообщений ROS по своей сути принимают любой тип данных или имя поля без ошибок.

msg = rosmessage("std_msgs/Int8","DataFormat","struct");
msg.Data = 'Test'

msg = struct with fields:
    MessageType: 'std_msgs/Int8'
           Data: 'Test'

msg.Data = 20;
class(msg.Data)

ans = 
'double'

Вместо этого при попытке отправить сообщение по сети ROS произошла ошибка недопустимых типов данных.

pub = rospublisher("/int_topic","std_msgs/Int8","DataFormat","struct");
send(pub,msg)

Error using ros.Publisher/send (line 290)
Error publishing a message with type std_msgs/Int8 on topic name /int_topic.

Caused by:
    Error using ros.internal.Node/publish
    Field 'Data' is wrong type; expected a int8.

Чтобы предотвратить ошибки, убедитесь, что сообщения используют правильный тип данных из определения сообщения.

rosmsg show std_msgs/Int8

int8 Data

Другие советы по эффективности

Использование структур сообщений является хорошим первым шагом для ускорения отправки и извлечения сообщений ROS. Структуры также повышают эффективность настройки данных во вложенных мезагах и доступа к ним. Следующий код демонстрирует отправку нескольких сообщений с вложенными полями.

% Set up network (reuse publisher for all examples)
pub = rospublisher("/goal_path","nav_msgs/Path","DataFormat","struct");

% Send robot new paths to follow
nPtsOnPath = 100;
for iPaths = 1:15
    pathMsg = rosmessage(pub);
    for iPts = 1:nPtsOnPath
        pathMsg.Poses(iPts) = rosmessage("geometry_msgs/PoseStamped","DataFormat","struct");
        pathMsg.Poses(iPts).Pose.Position.X = iPaths+iPts;
        pathMsg.Poses(iPts).Pose.Position.Y = iPaths-iPts;
        pathMsg.Poses(iPts).Pose.Position.Z = (iPaths+iPts)/10;
    end
    send(pub,pathMsg)
end

Повторно используйте сообщения

Если сообщения создаются и изменяются в цикле, и те же поля данных устанавливаются каждую итерацию, быстрее создать сообщение только один раз. Переместите создание сообщений за пределы цикла и повторно используйте те же сообщения внутри цикла для каждой итерации.

% Set up messages for use
pathMsg = rosmessage(pub);
poseMsg = rosmessage("geometry_msgs/PoseStamped","DataFormat","struct");

% Send robot new paths to follow
nPtsOnPath = 100;
for iPaths = 1:15
    for iPts = 1:nPtsOnPath
        pathMsg.Poses(iPts) = poseMsg;
        pathMsg.Poses(iPts).Pose.Position.X = iPaths+iPts;
        pathMsg.Poses(iPts).Pose.Position.Y = iPaths-iPts;
        pathMsg.Poses(iPts).Pose.Position.Z = (iPaths+iPts)/10;
    end
    send(pub,pathMsg)
end

Извлечение вложенных сообщений для манипуляции

При чтении или установке нескольких полей во вложенное сообщение извлеките вложенное сообщение перед чтением или настройкой полей.

% Set up messages for use
pathMsg = rosmessage(pub);
poseMsg = rosmessage("geometry_msgs/PoseStamped","DataFormat","struct");
ptMsg = poseMsg.Pose.Position;  % Extract nested message

% Send robot new paths to follow
nPtsOnPath = 100;
for iPaths = 1:15
    for iPts = 1:nPtsOnPath
        % Set fields before setting nested message
        ptMsg.X = iPaths+iPts;
        ptMsg.Y = iPaths-iPts;
        ptMsg.Z = (iPaths+iPts)/10;
        poseMsg.Pose.Position = ptMsg;
        pathMsg.Poses(iPts) = poseMsg;
    end
    send(pub,pathMsg)
end

Предварительное выделение массивов структур сообщений

Для относительно больших массивов сообщений предварительное выделение массива структур может улучшить эффективность при установке значений в цикле. Используйте этот метод, когда массив является фиксированной длиной каждую итерацию.

% Set up messages for use
pathMsg = rosmessage(pub);
poseMsg = rosmessage("geometry_msgs/PoseStamped","DataFormat","struct");
ptMsg = poseMsg.Pose.Position;  % Extract nested message

% Preallocate path array
nPtsOnPath = 100;
pathMsg.Poses(nPtsOnPath) = poseMsg;

% Send robot new paths to follow
for iPaths = 1:15
    for iPts = 1:nPtsOnPath
        % Set fields before setting nested message
        ptMsg.X = iPaths+iPts;
        ptMsg.Y = iPaths-iPts;
        ptMsg.Z = (iPaths+iPts)/10;
        poseMsg.Pose.Position = ptMsg;
        pathMsg.Poses(iPts) = poseMsg;
    end
    send(pub,pathMsg)
end

Теперь сеть ROS может быть отключена.

rosshutdown

Shutting down global node /matlab_global_node_01839 with NodeURI http://bat6315glnxa64:45457/
Shutting down ROS master on http://192.168.0.10:59565.

Документация