synchronize

This example shows how to use the ros2topicsynchronizer object to get sets of time-synchronized messages from multiple ROS 2 topics publishing at different frequencies.

Read all the messages from the ROS 2 bag file. The bag file contains odometry and laser scan messages which are published at different rates. The odometry messages are published at 25 Hz and the laser scan messages are published at 5 Hz.

unzip("ros2_netwrk_bag.zip");
bagReader = ros2bagreader(fullfile(pwd,"ros2_netwrk_bag"));
odomMsgs = readMessages(select(bagReader,Topic="/odom"));
scanMsgs = readMessages(select(bagReader,Topic="/scan"));

Create two publishers /odom and /scan, which can publish odometry and laserscan messages.

publishNode = ros2node("publisherNode");
odomPub = ros2publisher(publishNode,"/odom","nav_msgs/Odometry",Durability="transientlocal",Depth=10);
scanPub = ros2publisher(publishNode,"/scan","sensor_msgs/LaserScan",Durability="transientlocal",Depth=10);

Create a ros2topicsynchronizer object which uses approximate epsilon time policy to time-synchronize messages. This means that sets of messages with timestamps between an epsilon tolerance are considered to be time-synchronized. Also, to ensure that messages from topics published at higher frequencies are not lost while waiting for those published at lower frequencies, specify the queue size as at least twice the highest frequency.

Use the syncPolicySettings structure to specify:

Specify the syncOdomScan function as the callback to execute each time a set of time-synchronized messages become available. The syncOdomScan function prints the individual timestamps of odometry and laser scan messages in each time-synchrnized set.

syncPolicySettings = struct("MessageQueueSize",uint32(50),"Epsilon",0.05);
syncObj = ros2topicsynchronizer("ApproximateEpsilonTime",SyncCallback=@syncOdomScan,PolicySettings=syncPolicySettings);

Add the two topics /odom and /scan for time-synchronization using the addTopic object function.

addTopic(syncObj,"/odom")
addTopic(syncObj,"/scan")

Initiate time-synchronization using the synchronize object function.

synchronize(syncObj)

Before publishing messages, create separate ros2rate objects for each topic to control publishing messages at different frequencies.

odomRate = ros2rate(publishNode,25);
scanRate = ros2rate(publishNode,5);

Start publishing messages on the /odom and /scan topics.

j = 1;
i = 1;

nextOdomPubTime = odomRate.TotalElapsedTime;
nextScanPubTime = scanRate.TotalElapsedTime;

while i <= numel(odomMsgs) || j <= numel(scanMsgs)
    currentTime = odomRate.TotalElapsedTime;
    if currentTime >= nextOdomPubTime && i <= numel(odomMsgs)
        send(odomPub,odomMsgs{i});
        i = i+1;
        nextOdomPubTime = nextOdomPubTime + odomRate.DesiredPeriod;
    end

    if currentTime >= nextScanPubTime && j <= numel(scanMsgs)
        send(scanPub,scanMsgs{j});
        j = j+1;
        nextScanPubTime = nextScanPubTime + scanRate.DesiredPeriod;
    end

    waitfor(odomRate);
end

You can also view the latest set of time-synchronized odometry and laser scan messages using the LatestSynchronizedMessages property.

syncMsgs = syncObj.LatestSynchronizedMessages;
syncMsgs{1}

ans = struct with fields:
       MessageType: 'nav_msgs/Odometry'
            header: [1×1 struct]
    child_frame_id: 'base_footprint'
              pose: [1×1 struct]
             twist: [1×1 struct]

syncMsgs{2}

ans = struct with fields:
        MessageType: 'sensor_msgs/LaserScan'
             header: [1×1 struct]
          angle_min: 0
          angle_max: 6.2832
    angle_increment: 0.0175
     time_increment: 0
          scan_time: 0
          range_min: 0.1200
          range_max: 3.5000
             ranges: [360×1 single]
        intensities: [360×1 single]

The syncOdomScan function receives two input arguments odomMsg and scanMsg. These input arguments correspond to a time-synchronized set of odometry and scan messages.

function syncOdomScan(odomMsg,scanMsg)
    t1 = double(odomMsg.header.stamp.sec) + 1e-9*double(odomMsg.header.stamp.nanosec);
    t2 = double(scanMsg.header.stamp.sec) + 1e-9*double(scanMsg.header.stamp.nanosec);
    fprintf("OdomMsgTime: %.3f ScanMsgTime: %.3f\n",t1,t2);
end