def __init__(self, init_state=Quaternion(1, 0, 0, 0), target_freq=30.0):
"""Initialize IMU bus connection, filter and threading.
Initialize a bus connection to the ICM20948 IMU, a MadgwickAHRS
filter for filtering noisy sensor data, and a deamon thread
for background updating.
Args:
init_state (madgwick_py.quaternion, optional): Initial state of the filter. Defaults to Quaternion(1, 0, 0, 0).
target_freq (float, optional): Target update frequency. Defaults to 30.0.
"""
self.lock = threading.Lock()
self.target_freq = target_freq
self.imu = ICM20948(i2c_bus=SMBus(0))
self.filter = MadgwickAHRS(
sampleperiod=1.0 / target_freq,
quaternion=init_state,
beta=0.05,
)
thread = threading.Thread(target=self.__update, args=())
thread.daemon = True
thread.start()
class ImuUpdater:
"""Asynchronously get IMU state and pass to filter
"""
def __init__(self, init_state=Quaternion(1, 0, 0, 0), target_freq=30.0):
"""Initialize IMU bus connection, filter and threading.
Initialize a bus connection to the ICM20948 IMU, a MadgwickAHRS
filter for filtering noisy sensor data, and a deamon thread
for background updating.
Args:
init_state (madgwick_py.quaternion, optional): Initial state of the filter. Defaults to Quaternion(1, 0, 0, 0).
target_freq (float, optional): Target update frequency. Defaults to 30.0.
"""
self.lock = threading.Lock()
self.target_freq = target_freq
self.imu = ICM20948(i2c_bus=SMBus(0))
self.filter = MadgwickAHRS(
sampleperiod=1.0 / target_freq,
quaternion=init_state,
beta=0.05,
)
thread = threading.Thread(target=self.__update, args=())
thread.daemon = True
thread.start()
def __update(self):
"""Background update function.
Asynchronously called in the background by a thread, this function
obtains IMU data and passes it to the filter.
"""
while True:
start = time.time()
x, y, z = self.imu.read_magnetometer_data()
ax, ay, az, gx, gy, gz = self.imu.read_accelerometer_gyro_data()
magnetometer = [x, y, z]
accelerometer = [ax, ay, az]
gyroscope = [math.radians(gx), math.radians(gy), math.radians(gz)]
with self.lock:
self.filter.update(gyroscope, accelerometer, magnetometer)
time.sleep(max(1.0 / self.target_freq - (time.time() - start), 0))
def get_data(self):
"""Get filtered IMU data
Returns:
madgwick_py.quaternion: Filtered orientation quaternion
"""
with self.lock:
return copy.deepcopy(self.filter.quaternion)
def _sample_thread(self):
"""Run sampling thread, to be run in a separate thread to handle sampling of sensors."""
while not rospy.is_shutdown():
# Acceleration is already in m/s^2 so no conversion needed,
# just unpacking
acceleration_result = self._sensor.acceleration
acceleration_x, acceleration_y, acceleration_z = acceleration_result
acceleration_x, acceleration_y, acceleration_z = acceleration_x - self._calibration[
"accelerometer"]["x"], acceleration_y - self._calibration["accelerometer"][
"y"], acceleration_z - self._calibration["accelerometer"]["z"]
# Gyro is in degrees/second so conversion to rads/sec is needed,
# as well as unpacking
gyro_result = [deg * (math.pi / 180.0) for deg in self._sensor.gyro]
gyro_x, gyro_y, gyro_z = gyro_result
gyro_x, gyro_y, gyro_z = gyro_x - self._calibration["gyro"][
"x"], gyro_y - self._calibration["gyro"]["y"], gyro_z - self._calibration["gyro"][
"z"]
# Magnetometer values can be used to calculate our orientation rotation about the z
# axis (yaw).
# Futher work is needed to calculate pitch and roll from other available sensors.
magnetometer_result = self._sensor.magnetic
# magnetometer_quaternion = self.calculate_heading_quaternion(magnetometer_result)
# Apply calibration to magnetometer result
magnetometer_result = (magnetometer_result[0] - self._calibration["magnetometer"]["x"],
magnetometer_result[1] - self._calibration["magnetometer"]["y"],
magnetometer_result[2] - self._calibration["magnetometer"]["z"])
if self._calculate_full_orientation:
previous_orientation_quaternion = self._orientation_filter.quaternion
self._orientation_filter.update(gyro_result, acceleration_result,
magnetometer_result)
if any(map(math.isnan, self._orientation_filter.quaternion)):
self._orientation_filter = MadgwickAHRS(
sampleperiod=1.0 / float(self._raw_sample_rate),
quaternion=previous_orientation_quaternion,
beta=AHRS_BETA)
filter_w, filter_x, filter_y, filter_z = self._orientation_filter.quaternion
self._orientation_quaternion = Quaternion(w=filter_w,
x=filter_x,
y=filter_y,
z=filter_z)
else:
self._orientation_quaternion = PiPuckImuServer.calculate_heading_quaternion(
magnetometer_result)
self._gyro_result = (gyro_x, gyro_y, gyro_z)
if self._remove_gravity:
pass # Not yet implemented
self._acceleration_result = (acceleration_x, acceleration_y, acceleration_z)
self._magnetometer_result = magnetometer_result
self._sample_rate.sleep()
(1 / samplePeriod), 'lowpass')
accMagnitudeFiltered = signal.filtfilt(
butterFilterB,
butterFilterA, [accMagnitudeFiltered, accMagnitudeFiltered],
padlen=1)
# Are we actually stationary?
stationary = accMagnitudeFiltered < ACCELEROMETER_DRIFT_WHEN_STATIONARY
print(accMagnitudeFiltered)
# If we are stationary, don't bother doing anything
if stationary.any():
continue
# Compute orientation
ahrsAlgorithm = MadgwickAHRS()
ahrsAlgorithm.update([gyro['x'], gyro['y'], gyro['z']],
[acc['x'], acc['y'], acc['z']],
[mag['x'], mag['y'], mag['z']])
quaternion = ahrsAlgorithm.quaternion
# Rotate body accelerations to earth frame
trueAcceleration = qv_mult(quaternion.conj(),
[acc['x'], acc['y'], acc['z']])
# Convert acceleration to m/s/s
accX = acc['x'] * 9.81
accY = acc['y'] * 9.81
accZ = acc['z'] * 9.81
# Compute translational velocities
import math
def rad2deg(a):
return list(map(lambda x: x * RAD2DEG, a))
def show3(v):
print('x:{:+10.5f}'.format(v[0]), end=' ')
print('y:{:+10.5f}'.format(v[1]), end=' ')
print('z:{:+10.5f}'.format(v[2]))
try:
imu = mpu9250()
mw = MadgwickAHRS(0.02, Quaternion(1, 0, 0, 0))
dcc = DynamicCompassCalibrator()
while True:
a = imu.accel
g = list(map(lambda x: x * DEG2RAD, imu.gyro))
#m = list(imu.mag)
#m = dcc.correct(imu.mag)
mw.update_imu(g, a)
#mw.update(g, a, m)
#print('w:{:.3f} x:{:.3f} y:{:.3f} z:{:.3f}'.format(*mw.quaternion))
ea = mw.quaternion.to_euler_angles()
show3(rad2deg(ea))
sleep(0.02)
except KeyboardInterrupt:
print('bye ...')
def __init__(self):
"""Initialise IMU server node."""
rospy.on_shutdown(self.close_sensor)
rospy.init_node("imu")
# Switch to determine if full orientation is calculated or just rotation around z axis.
self._calculate_full_orientation = bool(rospy.get_param("~calculate_full_orientation",
True))
# Unimplemented switch to control removal of gravity.
self._remove_gravity = bool(rospy.get_param("~remove_gravity", False))
# Handle transformation tree prefix.
tf_prefix_key = rospy.search_param("tf_prefix")
if tf_prefix_key:
self._tf_prefix = rospy.get_param(tf_prefix_key, None)
else:
self._tf_prefix = None
self._tf_prefix = rospy.get_param(tf_prefix_key, None)
if self._tf_prefix is not None and not self._tf_prefix.endswith("/"):
self._tf_prefix += "/"
# Rate of sensor data transmission
self._rate = rospy.Rate(rospy.get_param('~rate', 10))
# Rate of collection of sample data
self._raw_sample_rate = int(rospy.get_param('~sample_rate', 128))
self._sample_rate = rospy.Rate(self._raw_sample_rate)
self._sensor_imu_publisher = rospy.Publisher('imu/imu', Imu, queue_size=10)
self._sensor_temperature_publisher = rospy.Publisher('imu/temperature',
Temperature,
queue_size=10)
self._sensor_magnetic_publisher = rospy.Publisher('imu/magnetic',
MagneticField,
queue_size=10)
self._sensor = None
# Handle IMU calibration
calibration_file = None
if path.isfile("calibration.json"):
calibration_file = "calibration.json"
elif path.isfile(path.join(path.dirname(__file__), "calibration.json")):
calibration_file = path.join(path.dirname(__file__), "calibration.json")
if calibration_file:
with open(calibration_file, "rb") as calibration_file_handle:
self._calibration = load(calibration_file_handle)
else:
self._calibration = CALIBRATION_DATA_DEFAULT
# Setup data storage variables.
if self._calculate_full_orientation:
self._orientation_filter = MadgwickAHRS(sampleperiod=1.0 / float(self._raw_sample_rate),
beta=AHRS_BETA)
self._acceleration_result = (0, 0, 0)
self._orientation_quaternion = Quaternion(w=1, x=0, y=0, z=0)
self._gyro_result = (0, 0, 0)
self._magnetometer_result = (0, 0, 0)
class PiPuckImuServer(object): # pylint: disable=too-many-instance-attributes
"""ROS Node to publish data from the Pi-puck's IMU."""
def __init__(self):
"""Initialise IMU server node."""
rospy.on_shutdown(self.close_sensor)
rospy.init_node("imu")
# Switch to determine if full orientation is calculated or just rotation around z axis.
self._calculate_full_orientation = bool(rospy.get_param("~calculate_full_orientation",
True))
# Unimplemented switch to control removal of gravity.
self._remove_gravity = bool(rospy.get_param("~remove_gravity", False))
# Handle transformation tree prefix.
tf_prefix_key = rospy.search_param("tf_prefix")
if tf_prefix_key:
self._tf_prefix = rospy.get_param(tf_prefix_key, None)
else:
self._tf_prefix = None
self._tf_prefix = rospy.get_param(tf_prefix_key, None)
if self._tf_prefix is not None and not self._tf_prefix.endswith("/"):
self._tf_prefix += "/"
# Rate of sensor data transmission
self._rate = rospy.Rate(rospy.get_param('~rate', 10))
# Rate of collection of sample data
self._raw_sample_rate = int(rospy.get_param('~sample_rate', 128))
self._sample_rate = rospy.Rate(self._raw_sample_rate)
self._sensor_imu_publisher = rospy.Publisher('imu/imu', Imu, queue_size=10)
self._sensor_temperature_publisher = rospy.Publisher('imu/temperature',
Temperature,
queue_size=10)
self._sensor_magnetic_publisher = rospy.Publisher('imu/magnetic',
MagneticField,
queue_size=10)
self._sensor = None
# Handle IMU calibration
calibration_file = None
if path.isfile("calibration.json"):
calibration_file = "calibration.json"
elif path.isfile(path.join(path.dirname(__file__), "calibration.json")):
calibration_file = path.join(path.dirname(__file__), "calibration.json")
if calibration_file:
with open(calibration_file, "rb") as calibration_file_handle:
self._calibration = load(calibration_file_handle)
else:
self._calibration = CALIBRATION_DATA_DEFAULT
# Setup data storage variables.
if self._calculate_full_orientation:
self._orientation_filter = MadgwickAHRS(sampleperiod=1.0 / float(self._raw_sample_rate),
beta=AHRS_BETA)
self._acceleration_result = (0, 0, 0)
self._orientation_quaternion = Quaternion(w=1, x=0, y=0, z=0)
self._gyro_result = (0, 0, 0)
self._magnetometer_result = (0, 0, 0)
def close_sensor(self):
"""Close the sensor after the ROS Node is shutdown."""
self._sensor.close()
def open_sensor(self):
"""Open the sensor."""
self._sensor = LSM9DS1()
@staticmethod
def euler_to_quaternion(yaw, pitch, roll):
"""Convert euler angles of pitch, roll, and yaw to a quaternion.
Based on code from
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles.
We don't use tf.transforms here to reduce dependence on tf and increase performance on this
simple task.
"""
c_y = math.cos(yaw * 0.5)
s_y = math.sin(yaw * 0.5)
c_p = math.cos(pitch * 0.5)
s_p = math.sin(pitch * 0.5)
c_r = math.cos(roll * 0.5)
s_r = math.sin(roll * 0.5)
q_w = c_y * c_p * c_r + s_y * s_p * s_r
q_x = c_y * c_p * s_r - s_y * s_p * c_r
q_y = s_y * c_p * s_r + c_y * s_p * c_r
q_z = s_y * c_p * c_r - c_y * s_p * s_r
return Quaternion(x=q_x, y=q_y, z=q_z, w=q_w)
@staticmethod
def calculate_heading(magnetometer_reading):
"""Calculate the current orientation using magnetometer.
Currently only supports yaw calculation e.g. rotation about the z axis.
"""
mag_x, mag_y, _ = magnetometer_reading
if mag_y == 0:
if mag_x > 0:
x_y_direction = math.radians(180)
else: # x <= 0
x_y_direction = math.radians(0)
elif mag_y < 0:
x_y_direction = math.radians(90) - math.atan(mag_x / mag_y)
else: # y > 0
x_y_direction = math.radians(270) - math.atan(mag_x / mag_y)
return x_y_direction
@staticmethod
def calculate_heading_quaternion(magnetometer_reading):
"""Calculate the current orientation as a quaternion using magnetometer.
Currently only supports yaw calculation e.g. rotation about the z axis.
"""
return PiPuckImuServer.euler_to_quaternion(
yaw=PiPuckImuServer.calculate_heading(magnetometer_reading), pitch=0, roll=0)
def _sample_thread(self):
"""Run sampling thread, to be run in a separate thread to handle sampling of sensors."""
while not rospy.is_shutdown():
# Acceleration is already in m/s^2 so no conversion needed,
# just unpacking
acceleration_result = self._sensor.acceleration
acceleration_x, acceleration_y, acceleration_z = acceleration_result
acceleration_x, acceleration_y, acceleration_z = acceleration_x - self._calibration[
"accelerometer"]["x"], acceleration_y - self._calibration["accelerometer"][
"y"], acceleration_z - self._calibration["accelerometer"]["z"]
# Gyro is in degrees/second so conversion to rads/sec is needed,
# as well as unpacking
gyro_result = [deg * (math.pi / 180.0) for deg in self._sensor.gyro]
gyro_x, gyro_y, gyro_z = gyro_result
gyro_x, gyro_y, gyro_z = gyro_x - self._calibration["gyro"][
"x"], gyro_y - self._calibration["gyro"]["y"], gyro_z - self._calibration["gyro"][
"z"]
# Magnetometer values can be used to calculate our orientation rotation about the z
# axis (yaw).
# Futher work is needed to calculate pitch and roll from other available sensors.
magnetometer_result = self._sensor.magnetic
# magnetometer_quaternion = self.calculate_heading_quaternion(magnetometer_result)
# Apply calibration to magnetometer result
magnetometer_result = (magnetometer_result[0] - self._calibration["magnetometer"]["x"],
magnetometer_result[1] - self._calibration["magnetometer"]["y"],
magnetometer_result[2] - self._calibration["magnetometer"]["z"])
if self._calculate_full_orientation:
previous_orientation_quaternion = self._orientation_filter.quaternion
self._orientation_filter.update(gyro_result, acceleration_result,
magnetometer_result)
if any(map(math.isnan, self._orientation_filter.quaternion)):
self._orientation_filter = MadgwickAHRS(
sampleperiod=1.0 / float(self._raw_sample_rate),
quaternion=previous_orientation_quaternion,
beta=AHRS_BETA)
filter_w, filter_x, filter_y, filter_z = self._orientation_filter.quaternion
self._orientation_quaternion = Quaternion(w=filter_w,
x=filter_x,
y=filter_y,
z=filter_z)
else:
self._orientation_quaternion = PiPuckImuServer.calculate_heading_quaternion(
magnetometer_result)
self._gyro_result = (gyro_x, gyro_y, gyro_z)
if self._remove_gravity:
pass # Not yet implemented
self._acceleration_result = (acceleration_x, acceleration_y, acceleration_z)
self._magnetometer_result = magnetometer_result
self._sample_rate.sleep()
def run(self):
"""Run the sensor server."""
self.open_sensor()
# We need a separate sensor reading thread as the sample rate must be relatively high
# (> 64 hrz), ideally close to 256 hrz. If we sent IMU data at this rate the overheads of
# sending would slow the sensor reading rate, hence we use a separate thread.
sensor_sample_thread = Thread(target=self._sample_thread)
sensor_sample_thread.start()
while not rospy.is_shutdown():
if self._tf_prefix:
tf_reference_frame = self._tf_prefix + REFERENCE_FRAME_ID
else:
tf_reference_frame = REFERENCE_FRAME_ID
# Temperature is in degrees C so no conversion needed
temperature_message = Temperature(temperature=self._sensor.temperature,
variance=UNKNOWN_VARIANCE)
temperature_message.header.frame_id = tf_reference_frame
self._sensor_temperature_publisher.publish(temperature_message)
acceleration_x, acceleration_y, acceleration_z = self._acceleration_result
gyro_x, gyro_y, gyro_z = self._gyro_result
magnetometer_x, magnetometer_y, magnetometer_z = self._magnetometer_result
orientation_quaternion = self._orientation_quaternion
imu_message = Imu(linear_acceleration_covariance=LINEAR_ACCELERATION_COVARIANCE,
linear_acceleration=Vector3(x=acceleration_x,
y=acceleration_y,
z=acceleration_z),
angular_velocity_covariance=ANGULAR_VELOCITY_COVARIANCE,
angular_velocity=Vector3(x=gyro_x, y=gyro_y, z=gyro_z),
orientation=orientation_quaternion,
orientation_covariance=ORIENTATION_COVARIANCE)
imu_message.header.frame_id = tf_reference_frame
self._sensor_imu_publisher.publish(imu_message)
magnetic_message = MagneticField(
magnetic_field=Vector3(x=magnetometer_x, y=magnetometer_y, z=magnetometer_z))
magnetic_message.header.frame_id = tf_reference_frame
self._sensor_magnetic_publisher.publish(magnetic_message)
self._rate.sleep()
sensor_sample_thread.join()