Trouble with Approximate time sync
I've been trying to get approximate time sync to work on a project for a few days now. the call back just never seems to fire. I decided to start over and make a couple simple nodes to look at everything from a simpler project, as I've read things on the forums like people struggle to sync more that 2 topics.
So I made a simple publisher node that publishes three header messages on individual topics. and a simple subscriber node that tries to sync the first two topics for now. I did everything following the use guide in the message filter page.
I cant even get the simple example to build ( my original does build). The specific error is 'value' is not a member of 'ros::message_traits::TimeStamp<std_msgs::string_<std::allocator<void>>, void>'
I found a forum post that mentioned that a similar error was caused by not using a message with a header. My guess is that I'm doing something small like not initializing the headers properly or something like that. I'll post the code from both nodes and hopefully someone has a suggestion.
#include "ros/ros.h"
#include "std_msgs/Header.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "TestPublisher");
ros::NodeHandle nh;
ros::Publisher test1_pub = nh.advertise<std_msgs::Header>("test1", 1000);
ros::Publisher test2_pub = nh.advertise<std_msgs::Header>("test2", 1000);
ros::Publisher test3_pub = nh.advertise<std_msgs::Header>("test3", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
std_msgs::Header msg1, msg2, msg3;
msg1.stamp = ros::Time::now();
msg2.stamp = ros::Time::now();
msg3.stamp = ros::Time::now();
test1_pub.publish(msg1);
test2_pub.publish(msg2);
test3_pub.publish(msg3);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include "ros/ros.h"
#include "std_msgs/Header.h"
using namespace message_filters;
void testCallback(const std_msgs::Header::ConstPtr& msg1, const std_msgs::Header::ConstPtr& msg2)
{
ROS_INFO_STREAM("message 1 " << msg1);
ROS_INFO_STREAM("message 2 " << msg2);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "TestSubscriber");
ros::NodeHandle nh;
message_filters::Subscriber<std_msgs::Header> test1_sub(nh, "test1", 1);
message_filters::Subscriber<std_msgs::Header> test2_sub(nh, "test2", 1);
typedef sync_policies::ApproximateTime<std_msgs::Header, std_msgs::Header> MySyncPolicy;
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), test1_sub, test2_sub);
sync.registerCallback(boost::bind(&testCallback, _1, _2));
ros::spin();
return 0;
}