Synchronizing multiple topics and re-publish them

edit

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