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))
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))
