I am using the Ouster OS1 LIDAR with ROS Melodic. The ROS package is publishing the LIDAR data and I am able to view it in RViz. I want to calibrate and fuse the LIDAR data with data from a USB camera, so I have implemented the cv_camera ROS package for the camera and created a new package called calib.
However, I am facing an issue where the message_filter.ApproximateTimeSynchronizer method is not able to invoke the callback function. The topics I am subscribing to are
When I try to echo the ouster/points topic, the timestamp is not showing.
How can I synchronize the time between the data streams ? I have given below the code used.
import rospy
import message_filters
from sensor_msgs.msg import Image, CameraInfo, PointCloud2
def callback(image_msg, camera_info_msg, lidar_msg):
# Process synchronized data here
image_sub = message_filters.Subscriber('/cv_camera/image_raw', Image)
info_sub = message_filters.Subscriber('/cv_camera/camera_info', CameraInfo)
lidar_sub = message_filters.Subscriber('/ouster/points', PointCloud2)
ts = message_filters.ApproximateTimeSynchronizer([image_sub, info_sub, lidar_sub], 10, 0.1)
ts.registerCallback(callback)
rospy.init_node('sync_node')
rospy.spin()
Syncronising topics with an ApproximateTimeSyncroniser, perhaps unsurprisingly, needs timestamp information for each topic it needs to syncronise. If your OS1 isn't providing that information, you'll need to get it some other way or add it yourself (this is the simpler option).
One way is to subscribe to the raw OS1 topic stream, append timestamp information to the message and republish it on a new topic, which you then feed into your syncroniser. Consider also adding the frame ID to the header if Ouster doesn't do that either to avoid frame transform issues down the line.
For example,
import rospy
import message_filters
from sensor_msgs.msg import Image, CameraInfo, PointCloud2
lidar_republisher = rospy.Publisher('/ouster/points_with_timestamp', PointCloud2, queue_size=1)
def add_stamp_to_lidar(lidar_msg):
stamped_msg = lidar_msg
stamped_msg.header.stamp = rospy.Time.now()
stamped_msg.header.frame_id = 'base_link'
lidar_republisher.pub(stamped_msg)
def callback(image_msg, camera_info_msg, stamped_lidar_msg):
# Do your thing here
pass
lidar_sub = rospy.Subscriber('/ouster/points', PointCloud2, add_stamp_to_lidar)
# Process synchronized data here
image_sub = message_filters.Subscriber('/cv_camera/image_raw', Image)
info_sub = message_filters.Subscriber('/cv_camera/camera_info', CameraInfo)
stamped_lidar_sub = message_filters.Subscriber('/ouster/points_with_timestamp', PointCloud2)
ts = message_filters.ApproximateTimeSynchronizer([image_sub, info_sub, stamped_lidar_sub], 10, 0.1)
ts.registerCallback(callback)
rospy.init_node('sync_node')
rospy.spin()
One danger here is that, since your hardware is out of sync, you might be syncronising out-of-date data alongside up-to-date data, meaning your camera's might render a newer frame than the LiDAR and show things that the LiDAR hasn't seen yet and vice-versa. The best solution is the get the topic timestamp from the hardware itself so it might be worthwhile to see if you can update your OS1's firmware or drivers