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