def main(): rospy.init_node('estimate_latency') path = rospy.get_param('~path') baud = rospy.get_param('~baud', 38400) samples = rospy.get_param('~samples', 100) compass = FieldforceTCM(path, baud) ver = compass.getModelInfo() rospy.loginfo('Found Fieldforce TCM: {0}'.format(ver)) # Configure the compass in a realistic way. compass.stopAll() compass.setConfig(Configuration.kCoeffCopySet, 0) compass.setConfig(Configuration.kAccelCoeffCopySet, 0) compass.setDataComponents([ Component.kHeading, Component.kPAngle, Component.kRAngle, Component.kDistortion, Component.kCalStatus ]) # Estimate the latency by using blocking sample requests. latencies = list() try: for i in xrange(samples): before = rospy.get_rostime() compass.readData() after = rospy.get_rostime() latency = (after - before).to_sec() latencies.append(latency) except Exception as e: compass.close() raise e avg_latency = sum(latencies) / (2 * len(latencies)) rospy.loginfo('Offset = {0} seconds'.format(avg_latency))