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