How to Synchronize two topics ?
Hi I am new to ROS, I am trying to synchronize two subscribed topics from last few days. I have tried almost all solutions mentioned in this forum but still struggling to get it synchronized. The callback function is never called. Below is my code as well CMakeLists.txt. Can anyone plz suggest where I am going wrong or what changes I should make.
Here is my code:
#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 <sensor_msgs/Image.h>
#include <boost/bind.hpp>
using namespace sensor_msgs;
using namespace message_filters;
void callback(const ImageConstPtr& image1, const ImageConstPtr& image2)
{
ROS_INFO_STREAM("Hi I am in callback");
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_messge_filter");
ros::NodeHandle nh;
message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);
typedef sync_policies::ApproximateTime<Image, Image> MySyncPolicy;
// ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
return 0;
}
CMakeLists.txt:
find_package(catkin REQUIRED COMPONENTS
pcl_ros
roscpp
sensor_msgs
image_transport
message_filters
std_msgs
message_generation
cv_bridge
)
find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED COMPONENTS
signals
)
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
)
add_executable(pub_sub_image_pointcloud src/pub_sub_image_pointcloud.cpp)
target_link_libraries(pub_sub_image_pointcloud
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${Boost_LIBRARIES}
)
Please use the Preformatted Text button (the one with
101010
on it) to properly format code and console copy-pastes in your question text.Sorry for inconvenience, I guess it is in correct format now
Can you try to put a larger queue size for both your subscribers ? I believe that 1 can be a little problematic for the algorithm wich is trying to match the timestamps. I never tried but it appears that there 3 optionals parameters to approx time http://wiki.ros.org/message_filters/A...
@TTDM I tried giving larger values it doesn't work
So i tested your code here and it works. Can you check that both your topics exists and that they have a time stamp ? ( rostopic echo image1 --noarr ) Btw, I believe every topic name has to start with /
Initially I was trying to run only this code, so it was not calling callback. But when I wrote a Publisher file separately & tried to subscribe it, It worked. So now I have merged both the files & this is what I found when I first wrote publisher & then subscribe it dint work, but vice-versa worked
Is it necessary to subscribe first & then publish?? May actual intension is to call a callback & publish from the callback function. Is this possible using Approximate Time policy??