def main(): rospy.init_node('fieldforce_tcm') pub_angle = rospy.Publisher('orientation', QuaternionStamped) path = rospy.get_param('~path', '/dev/ttyUSB0') baud = rospy.get_param('~baud', 38400) frame = rospy.get_param('~frame_id', 'frame') compass = FieldforceTCM(path, baud) compass.setDataComponents([ Component.kHeading, Component.kPAngle, Component.kRAngle, Component.kDistortion, Component.kCalStatus ]) compass.startStreaming() warn_distortion = False warn_calibration = False try: while True: datum = compass.getData() now = rospy.get_rostime() if datum.Distortion and not warn_distortion: rospy.logwarn('Magnometer has exceeded its linear range.') warn_distortion = True if not datum.CalStatus and not warn_calibration: rospy.logwarn('Compass is not calibrated.') warn_calibration = True ax = math.radians(datum.RAngle) ay = math.radians(datum.PAngle) az = math.radians(datum.Heading) quaternion = transformations.quaternion_from_euler(ax, ay, az) pub_angle.publish( header = Header(stamp=now, frame_id=frame), quaternion = Quaternion(*quaternion) ) except Exception, e: compass.stopStreaming() compass.close() raise e
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))