Synchronizing multiple topics and re-publish them
Hi, I am trying to synchronize robot joints and force/torque sensor message. So I've tried to use message_filters::sync_policies::ApproximateTime to do so, however, it does not seem to be working. I have no compile error but I can not see the new topics. The plan was to remap ft_message and joint_states once they are synchronized.
Could you please help me!!
Thanks Ali
#include <ros/ros.h>
#include <math.h>
#include<QDebug>
#include<QString>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/WrenchStamped.h>
#include <sensor_msgs/JointState.h>
using namespace sensor_msgs;
using namespace message_filters;
class Node{
public:
Node():
joints_sub_(nh_,"joint_states",100),
ft_msgs_sub_(nh_,"ft_message",100),
sync(MySyncPolicy(100), joints_sub_, ft_msgs_sub_){
sync.registerCallback(boost::bind(&Node::callback, this,_1, _2));
}
void callback(const sensor_msgs::JointStateConstPtr& joint_s, const geometry_msgs::WrenchStampedConstPtr& ft_msgs)
{
double loop_rate_;
nh_.param("loop_rate", loop_rate_, 650.0);
ros::Rate loop_rate(loop_rate_);
ros::Time::now();
sensor_msgs::JointState joint_states_sync=*joint_s;
geometry_msgs::WrenchStamped ft_msgs_sync=*ft_msgs;
ros::Publisher joint_pub=nh_.advertise<sensor_msgs::JointState>("joint_states",100);
ros::Publisher ft_msg_pub=nh_.advertise<geometry_msgs::WrenchStamped>("ft_msgs",100);
joint_pub.publish(joint_states_sync);
ft_msg_pub.publish(ft_msgs_sync);
}
private:
ros::NodeHandle nh_;
typedef message_filters::Subscriber<sensor_msgs::JointState> joints_sub;
joints_sub joints_sub_;
typedef message_filters::Subscriber<geometry_msgs::WrenchStamped> ft_msg_sub;
ft_msg_sub ft_msgs_sub_;
typedef message_filters::sync_policies::ApproximateTime sensor_msgs::JointState,geometry_msgs::WrenchStamped> MySyncPolicy;
message_filters::Synchronizer< MySyncPolicy > sync;
};
int main(int argc, char **argv)
{
ros::init (argc, argv, "synchronizer");
Node NH;
while (ros::ok())
{
ros::spin();
}
ros::shutdown();
return 0;
}