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('fieldforce_tcm')
pub = rospy.Publisher('compass', Imu)
mag_slot = rospy.get_param('~mag_slot', 0)
accel_slot = rospy.get_param('~accel_slot', 0)
path = rospy.get_param('~path')
baud = rospy.get_param('~baud', 38400)
frame = rospy.get_param('~frame_id', '/base_link')
cov = rospy.get_param('~covariance',
[inf, 0.0, 0.0, 0.0, inf, 0.0, 0.0, 0.0, var])
declination = rospy.get_param('~declination', 0.0)
hack_value = rospy.get_param('~hack_value', 0.0)
try:
compass = FieldforceTCM(path, baud)
ver = compass.getModelInfo()
rospy.loginfo('Found Fieldforce TCM: {0}'.format(ver))
except TimeoutException:
pass
warn_distortion = False
warn_calibration = False
is_started = False
timeout_total = 0
timeout_since_last = 0
last_yaw = 0.0
while not rospy.is_shutdown():
try:
if is_started:
datum = compass.getData(2)
else:
is_started = try_start(compass, mag_slot, accel_slot,
declination)
continue
except TimeoutException as e:
rospy.logwarn(
'Wait for data timed out. Total timeouts: {0}, timouts since last data: {1}'
.format(timeout_total, timeout_since_last))
timeout_total += 1
timeout_since_last += 1
is_started = try_start(compass, mag_slot, accel_slot, declination)
continue
timeout_since_last = 0
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
# FIXME: This should not be negated.
ax = math.radians(datum.RAngle)
ay = math.radians(datum.PAngle)
az = -math.radians(datum.Heading) + hack_value
quaternion = transformations.quaternion_from_euler(ax, ay, az)
# FIXME
#delta_raw = (az - last_yaw) % (2 * math.pi)
#delta = min(delta_raw, 2 * math.pi - delta_raw)
#last_yaw = az;
cov_new = list(cov)
#cov_new[8] += alpha * delta
pub.publish(
header=Header(stamp=now, frame_id=frame),
orientation=Quaternion(*quaternion),
orientation_covariance=cov_new,
angular_velocity=Vector3(0, 0, 0),
angular_velocity_covariance=[-1, 0, 0, 0, 0, 0, 0, 0, 0],
linear_acceleration=Vector3(0, 0, 0),
linear_acceleration_covariance=[-1, 0, 0, 0, 0, 0, 0, 0, 0])
def main():
rospy.init_node('fieldforce_tcm')
pub = rospy.Publisher('compass', Imu)
mag_slot = rospy.get_param('~mag_slot', 0)
accel_slot = rospy.get_param('~accel_slot', 0)
path = rospy.get_param('~path','/dev/ttyUSB0')
baud = rospy.get_param('~baud', 115200)
frame = rospy.get_param('~frame_id', '/base_link')
cov = rospy.get_param('~covariance', [
inf, 0.0, 0.0,
0.0, inf, 0.0,
0.0, 0.0, var
])
declination = rospy.get_param('~declination', 0.0)
UseEastAsZero = rospy.get_param('~UseEastAsZero',True)
#hack_value = rospy.get_param('~hack_value', 0.0)
try:
compass = FieldforceTCM(path, baud)
ver = compass.getModelInfo()
rospy.loginfo('Found Fieldforce TCM: {0}'.format(ver))
except TimeoutException:
pass
warn_distortion = False
warn_calibration = False
is_started = False
timeout_total = 0
timeout_since_last = 0
last_yaw = 0.0
while not rospy.is_shutdown():
try:
if is_started:
datum = compass.getData(2)
else:
is_started = try_start(compass, mag_slot, accel_slot, declination)
continue
except TimeoutException as e:
rospy.logwarn('Wait for data timed out. Total timeouts: {0}, timouts since last data: {1}'.format(timeout_total, timeout_since_last))
timeout_total += 1
timeout_since_last += 1
is_started = try_start(compass, mag_slot, accel_slot, declination)
continue
timeout_since_last = 0
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
# FIXME: This should not be negated.
ax = math.radians(datum.RAngle)
ay = math.radians(datum.PAngle)
if UseEastAsZero :
az = -math.radians(datum.Heading) +math.pi/2.- declination
else:
az = -math.radians(datum.Heading) - declination
quaternion = transformations.quaternion_from_euler(ax, ay, az)
# FIXME
#delta_raw = (az - last_yaw) % (2 * math.pi)
#delta = min(delta_raw, 2 * math.pi - delta_raw)
#last_yaw = az;
cov_new = list(cov)
#cov_new[8] += alpha * delta
pub.publish(
header = Header(stamp=now, frame_id=frame),
orientation = Quaternion(*quaternion),
orientation_covariance = cov_new,
angular_velocity = Vector3(0, 0, 0),
angular_velocity_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ],
linear_acceleration = Vector3(0, 0, 0),
linear_acceleration_covariance = [ -1, 0, 0, 0, 0, 0, 0, 0, 0 ]
)
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))
