ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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;
}
2 | No.2 Revision |
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;
}