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
