How to use Chain filter?
I need to collect the point cloud data from many sensors (the number of sensors isn't fixed) together with the position of the robotic arm. Below is the information about ros message types-
In short, I want get data from single baxter_core_msgs::EndpointState
and multiple sensor_msgs::PointCloud2
simultaneously.
I am trying to use Chain filter. Below is the code snippet-
void ChainFilterTest::myCallback(const MsgConstPtr& msg)
{
ROS_INFO("Callback received");
}
ChainFilterTest::ChainFilterTest()
{
ros::NodeHandle nh("~");
message_filters::Chain c;
c.addFilter(boost::shared_ptr<message_filters::Subscriber<baxter_core_msgs::EndpointState> >(new message_filters::Subscriber<baxter_core_msgs::EndpointState>(nh, ee_topic, 1)));
std::string pc_topics[] = {"pc_topic_1", "pc_topic_2"}; // the number of elements aren't fixed
int length = sizeof(pc_topics)/sizeof(pc_topics[0]);
for(int i = 0; i < length; i++ )
c.addFilter(boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2> >(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, pc_topics[i], 1)));
c.registerCallback(&ChainFilterTest::myCallback, this);
ros::spin();
}
Above code shows following errors during complication-
error: ‘MsgConstPtr’ does not name a type
void ChainFilterTest::myCallback(const MsgConstPtr& msg)
^
error: missing template arguments before ‘c’
message_filters::Chain c;
^
error: ‘c’ was not declared in this scope
c.addFilter(boost::shared_ptr<message_filters::Subscriber<baxter_core_msgs::EndpointState> >(new message_filters::Subscriber<baxter_core_msgs::EndpointState>(nh, ee_topic, 1)));
^
The first error is related to the declaration of the message type. The second error is also similar. The message type isn't same across all filters.
I want to know that how to use Chain
filter in this situation?
It would be helpful if you posted the error you get when you try to compile.
@Airuno2L: Sorry for the inconvenience. Please see the updated question.
Is the problem you want to do time synchronization, but you have a variable number of topics to sync?
@lucasw: Exactly. You are right.
I haven't used it before but the documentation says chain filter is for chaining filters together, and only single input filters (so no time synchronization). I think you need to have a big if then block for 0-9 total topics to synchronize, though it's possible there is some C++ magic to do better.
I see. Thank you very much.