Revision history [back]

I solved the problem by using the mutex lock feature to get the node from getting stuck.

The code that worked for me is below:

#include "ros/ros.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <tf/transform_broadcaster.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <boost/thread.hpp>

static boost::mutex mutex;
    void Callback(const coop_est::FeatureBArray::ConstPtr& msg, const nav_msgs::Odometry::ConstPtr& odo) /
    {

       DO A NUMBER OF MATRIX OPERATIONS ON THE GLOBAL MATRICES Q, P, Xf

    }


    int main(int argc, char** argv)
    {

      ros::init(argc, argv, "coop_node");

      ros::NodeHandle n;

      mutex.lock();   

      message_filters::Subscriber<coop_est::FeatureBArray> fea_sub(n, "Feature", 1);
      message_filters::Subscriber<nav_msgs::Odometry> odo_sub(n, "odom", 1);
      typedef sync_policies::ApproximateTime<coop_est::FeatureBArray, nav_msgs::Odometry> MySyncPolicy;
      Synchronizer<MySyncPolicy> sync(MySyncPolicy(1), fea_sub, odo_sub);
      sync.registerCallback(boost::bind(&FeaturesReceived, _1, _2));

      mutex.unlock();
      ros::spin();

      return 0;

    }

I solved the problem by using the mutex lock feature to get the node from getting stuck.

The code that worked for me is below:

#include "ros/ros.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <tf/transform_broadcaster.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <boost/thread.hpp>

static boost::mutex mutex;
    void Callback(const coop_est::FeatureBArray::ConstPtr& msg, const nav_msgs::Odometry::ConstPtr& odo) /
    {

       DO A NUMBER OF MATRIX OPERATIONS ON THE GLOBAL MATRICES Q, P, Xf

    }


    int main(int argc, char** argv)
    {

      ros::init(argc, argv, "coop_node");

      ros::NodeHandle n;

      mutex.lock();   

      message_filters::Subscriber<coop_est::FeatureBArray> fea_sub(n, "Feature", 1);
      message_filters::Subscriber<nav_msgs::Odometry> odo_sub(n, "odom", 1);
      typedef sync_policies::ApproximateTime<coop_est::FeatureBArray, nav_msgs::Odometry> MySyncPolicy;
      Synchronizer<MySyncPolicy> sync(MySyncPolicy(1), fea_sub, odo_sub);
      sync.registerCallback(boost::bind(&FeaturesReceived, sync.registerCallback(boost::bind(&Callback, _1, _2));

      mutex.unlock();
      ros::spin();

      return 0;

    }