[ros-users] Another question about policy based synchronizer

I have a working test code for policy based (approximate time)
synchronizer in which one node publishes two data messages and the other
node runs a synchronizer. One message is published at 1Hz and the other
at 0.5Hz. For each message, I increment the relevant sequence number in
the header. However, when these messages are received by the node
running the policy based synchronizer, the sequence numbers are off by
1, although the time stamps match perfectly.

For example, following output shows a simple run -

Publisher log:
[ INFO] [1287418571.114515026]: Starting
test_policy_synchronizer_publisher node.
[ INFO] [1287418571.122496486]: Publishing AHRS data: time =
1287418571.122447, seq = 1
[ INFO] [1287418572.122643642]: Publishing AHRS data: time =
1287418572.122516, seq = 2
[ INFO] [1287418572.122956042]: Publishing Alasca data: time =
1287418572.122883, seq = 1
[ INFO] [1287418573.122685477]: Publishing AHRS data: time =
1287418573.122559, seq = 3
[ INFO] [1287418574.122935718]: Publishing AHRS data: time =
1287418574.122809, seq = 4
[ INFO] [1287418574.123219204]: Publishing Alasca data: time =
1287418574.123158, seq = 2
[ INFO] [1287418581.122753362]: Shutting down
test_policy_synchronizer_publisher node.

Synchronizer node log:
[ INFO] [1287418573.123472169]: ----------
[ INFO] [1287418573.123744969]: Received message set
[ INFO] [1287418573.123947997]: AHRS: timestamp = 1287418572.122516, seq = 1
[ INFO] [1287418573.124106258]: Alasca: timestamp = 1287418572.122883,
seq = 0
[ INFO] [1287418575.122839197]: ----------
[ INFO] [1287418575.122900518]: Received message set
[ INFO] [1287418575.122951782]: AHRS: timestamp = 1287418574.122809, seq = 3
[ INFO] [1287418575.123003883]: Alasca: timestamp = 1287418574.123158,
seq = 1

Comparing time stamps of sent and received messages, I see that the
sequence numbers in the synchronizer node are off by 1 (if sent message
seq = 3 then received message seq = 2). I am not sure why this
discrepancy is popping up. I would appreciate any help in this regard! I
have attached source code for both nodes.

Thank you.
- Aditya
/*

 * test_policy_synchronizer.cpp
 *
 *  Created on: Oct 17, 2010
 *      Author: agadre
 */

#include "riverine_msgs/AhrsEuler.h"
#include "riverine_msgs/AlascaScan.h"

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

void callback(const riverine_msgs::AhrsEuler::ConstPtr& ahrsEuler,
        const riverine_msgs::AlascaScan::ConstPtr& alascaScan)
{
    ROS_INFO_STREAM("----------");
    ROS_INFO_STREAM("Received message set");
    ROS_INFO_STREAM("AHRS: timestamp = " << std::fixed << ahrsEuler->header.stamp.toSec()
            << ", seq = " << ahrsEuler->header.seq);
    ROS_INFO_STREAM("Alasca: timestamp = " << std::fixed << alascaScan->header.stamp.toSec()
            << ", seq = " << alascaScan->header.seq);
}

int main(int argc, char* argv[])
{
    // init ros
    ros::init(argc, argv, "test_policy_synchronizer_node");

    ros::NodeHandle nh;
    message_filters::Subscriber<riverine_msgs::AhrsEuler>
        subAhrsEuler(nh, "ahrs_euler", 10);
    message_filters::Subscriber<riverine_msgs::AlascaScan>
        subAlascaScan(nh, "alasca_scan", 10);

    typedef message_filters::sync_policies::ApproximateTime<riverine_msgs::AhrsEuler, riverine_msgs::AlascaScan>
        MyApproxTimeSyncPolicy;
    message_filters::Synchronizer<MyApproxTimeSyncPolicy> sync(MyApproxTimeSyncPolicy(10), subAhrsEuler, subAlascaScan);
    sync.registerCallback(boost::bind(&callback, _1, _2));

    ros::spin();

    // done
    return 0;
}

/*

 * test_policy_synchronizer.cpp
 *
 *  Created on: Oct 17, 2010
 *      Author: agadre
 */

#include <ros/ros.h>
#include "riverine_msgs/AhrsEuler.h"
#include "riverine_msgs/AlascaScan.h"

int main(int argc, char* argv[])
{
    ////////////////////
    // Initialize ROS
    ROS_INFO_STREAM("Starting test_policy_synchronizer_publisher node.");
    ros::init(argc, argv, "test_policy_synchronizer_publisher_node");

    // Initialize communications
    ros::NodeHandle nodeHandle;

    // Create publisher
    ros::Publisher pubAhrsEuler =
            nodeHandle.advertise<riverine_msgs::AhrsEuler>("ahrs_euler", 1);
    ros::Publisher pubAlascaScan =
            nodeHandle.advertise<riverine_msgs::AlascaScan>("alasca_scan", 1);

    // Poll rate
    ros::Rate pollRate(1);

    ////////////////////
    // Run main loop
    riverine_msgs::AhrsEuler msgAhrsEuler;
    riverine_msgs::AlascaScan msgAlascaScan;
    bool publishAlasca(false);
    while (ros::ok())
    {
        // publish ahrs
        msgAhrsEuler.header.stamp = ros::Time::now();
        ++msgAhrsEuler.header.seq;
        ROS_INFO_STREAM("Publishing AHRS data: time = "
                << std::fixed << msgAhrsEuler.header.stamp.toSec()
                << ", seq = "<< msgAhrsEuler.header.seq);
        pubAhrsEuler.publish(msgAhrsEuler);

        // publish alasca
        if(publishAlasca)
        {
            msgAlascaScan.header.stamp = ros::Time::now();
            ++msgAlascaScan.header.seq;
            ROS_INFO_STREAM("Publishing Alasca data: time = "
                    << std::fixed << msgAlascaScan.header.stamp.toSec()
                    << ", seq = "<< msgAlascaScan.header.seq);
            pubAlascaScan.publish(msgAlascaScan);
        }
        publishAlasca = !publishAlasca;

        // Run event loop
        ros::spinOnce();

        // Make sure we try to maintain the requested poll rate
        pollRate.sleep();
    }

    ROS_INFO_STREAM("Shutting down test_policy_synchronizer_publisher node.");

    ////////////////////
    // done
    return 0;
}