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