Reading and Publishing bag file with PointCloud2 and TimeReference topics in C++
Hi,
I have a bag file with 2 topics.
First topic has sequence of lidar data of PointCloud2 message type and the other topic has TimeReference message type.
I have read to read the bag file messages and publish the PointCloud2 messages (around 500 messages) in sync with the TimeReference messages (around 150 messages) in C++.
The below code is publishing the data without synchronization.
ros::init (argc, argv, "ros_create");
ros::NodeHandle nh;
ros_pub = nh.advertise<sensor_msgs::PointCloud2> ("ros_output", 1);
ros::Publisher ros_pub;
rosbag::Bag bag;
bag.open("mydata.bag", rosbag::bagmode::Read);
std::vector<std::string> bag_topics;
bag_topics.push_back(std::string("/points_core"));
bag_topics.push_back(std::string("/time_reference"));
rosbag::View view(bag, rosbag::TopicQuery(bag_topics));
ros::Rate loop_rate(1);
while (ros::ok())
{
foreach(rosbag::MessageInstance const m, view)
{
if ( std::strcmp( m.getTopic().c_str(), "/time_reference") == 0)
{
// Time Reference Messages...
}
if ( std::strcmp( m.getTopic().c_str(), "/points_core") == 0)
{
sensor_msgs::PointCloud2::Ptr ptcld2 = m.instantiate<sensor_msgs::PointCloud2>();
ros_pub.publish(ptcld2);
}
}// foreach
ros::spinOnce();
loop_rate.sleep();
}//while
bag.close();
How to achieve the synchronisation between these two topic messages?