Re: [ros-users] Another question about policy based synchron…

>>> On Mon, Oct 18, 2010 at 9:28 AM, Aditya Gadre<agadre@vt.edu>    wrote:

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

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
 */

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;
}