From 5f8c46df18a11186bcfa5ced852483a05e72a122 Mon Sep 17 00:00:00 2001 From: Sailor Date: Tue, 17 Sep 2024 22:27:38 +0100 Subject: [PATCH 1/4] Fix quaternion logging Co-authored-by: Harshil Co-authored-by: JacksonElia --- airbrakes/imu/imu.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/airbrakes/imu/imu.py b/airbrakes/imu/imu.py index 9d2364f8..d25ee837 100644 --- a/airbrakes/imu/imu.py +++ b/airbrakes/imu/imu.py @@ -36,7 +36,7 @@ class IMU: def __init__(self, port: str, frequency: int, upside_down: bool): # Shared Queue which contains the latest data from the IMU - self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue() + self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue(maxsize=100000) self._running = multiprocessing.Value("b", False) # Makes a boolean value that is shared between processes # Starts the process that fetches data from the IMU @@ -101,19 +101,18 @@ def _fetch_data_loop(self, port: str, frequency: int, _: bool): # This cpp file was the only place I was able to find all the channel names # https://github.com/LORD-MicroStrain/MSCL/blob/master/MSCL/source/mscl/MicroStrain/MIP/MipTypes.cpp # Check if the imu_data_packet has an attribute with the name of the channel - if hasattr(imu_data_packet, channel): + if hasattr(imu_data_packet, channel) or "Quaternion" in channel: # First checks if the data point needs special handling, if not, just set the attribute match channel: # These specific data points are matrix's rather than doubles case "estAttitudeUncertQuaternion" | "estOrientQuaternion": matrix = data_point.as_Matrix() # Converts the [4x1] matrix to the X, Y, Z, and W of the quaternion - quaternion_tuple = tuple(matrix[i, 0] for i in range(matrix.rows())) # Sets the X, Y, Z, and W of the quaternion to the data packet object - setattr(imu_data_packet, f"{channel}X", quaternion_tuple[0]) - setattr(imu_data_packet, f"{channel}Y", quaternion_tuple[1]) - setattr(imu_data_packet, f"{channel}Z", quaternion_tuple[2]) - setattr(imu_data_packet, f"{channel}W", quaternion_tuple[3]) + setattr(imu_data_packet, f"{channel}X", matrix.as_floatAt(0, 0)) + setattr(imu_data_packet, f"{channel}Y", matrix.as_floatAt(0, 1)) + setattr(imu_data_packet, f"{channel}Z", matrix.as_floatAt(0, 2)) + setattr(imu_data_packet, f"{channel}W", matrix.as_floatAt(0, 3)) case _: # Because the attribute names in our data packet classes are the same as the channel # names, we can just set the attribute to the value of the data point. From f42c33c409bc6f45e577bec64777215f8750a27d Mon Sep 17 00:00:00 2001 From: Harshil <37377066+harshil21@users.noreply.github.com> Date: Tue, 17 Sep 2024 20:10:26 -0400 Subject: [PATCH 2/4] Review: constant and comment --- airbrakes/constants.py | 3 +++ airbrakes/imu/imu.py | 9 +++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/airbrakes/constants.py b/airbrakes/constants.py index 14b8e25b..3f4c4028 100644 --- a/airbrakes/constants.py +++ b/airbrakes/constants.py @@ -29,6 +29,9 @@ ESTIMATED_DESCRIPTOR_SET = 130 RAW_DESCRIPTOR_SET = 128 +# The maximum size of the data queue for the packets, so we don't run into memory issues +MAX_QUEUE_SIZE = 100000 + # ------------------------------------------------------- # Orientation Configuration # ------------------------------------------------------- diff --git a/airbrakes/imu/imu.py b/airbrakes/imu/imu.py index d25ee837..4d27775e 100644 --- a/airbrakes/imu/imu.py +++ b/airbrakes/imu/imu.py @@ -13,7 +13,7 @@ stacklevel=2, ) -from airbrakes.constants import ESTIMATED_DESCRIPTOR_SET, RAW_DESCRIPTOR_SET +from airbrakes.constants import ESTIMATED_DESCRIPTOR_SET, RAW_DESCRIPTOR_SET, MAX_QUEUE_SIZE from airbrakes.imu.imu_data_packet import EstimatedDataPacket, IMUDataPacket, RawDataPacket @@ -35,8 +35,10 @@ class IMU: ) def __init__(self, port: str, frequency: int, upside_down: bool): - # Shared Queue which contains the latest data from the IMU - self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue(maxsize=100000) + # Shared Queue which contains the latest data from the IMU. The MAX_QUEUE_SIZE is there + # to prevent memory issues. Realistically, the queue size never exceeds 50 packets when + # it's being logged. + self._data_queue: multiprocessing.Queue[IMUDataPacket] = multiprocessing.Queue(MAX_QUEUE_SIZE) self._running = multiprocessing.Value("b", False) # Makes a boolean value that is shared between processes # Starts the process that fetches data from the IMU @@ -100,7 +102,6 @@ def _fetch_data_loop(self, port: str, frequency: int, _: bool): channel = data_point.channelName() # This cpp file was the only place I was able to find all the channel names # https://github.com/LORD-MicroStrain/MSCL/blob/master/MSCL/source/mscl/MicroStrain/MIP/MipTypes.cpp - # Check if the imu_data_packet has an attribute with the name of the channel if hasattr(imu_data_packet, channel) or "Quaternion" in channel: # First checks if the data point needs special handling, if not, just set the attribute match channel: From 69228735a23dc4cd04e7b2a622e8a7c909aac796 Mon Sep 17 00:00:00 2001 From: Harshil <37377066+harshil21@users.noreply.github.com> Date: Tue, 17 Sep 2024 20:12:49 -0400 Subject: [PATCH 3/4] Fix ruff --- airbrakes/imu/imu.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/airbrakes/imu/imu.py b/airbrakes/imu/imu.py index 4d27775e..cdc565d5 100644 --- a/airbrakes/imu/imu.py +++ b/airbrakes/imu/imu.py @@ -13,7 +13,7 @@ stacklevel=2, ) -from airbrakes.constants import ESTIMATED_DESCRIPTOR_SET, RAW_DESCRIPTOR_SET, MAX_QUEUE_SIZE +from airbrakes.constants import ESTIMATED_DESCRIPTOR_SET, MAX_QUEUE_SIZE, RAW_DESCRIPTOR_SET from airbrakes.imu.imu_data_packet import EstimatedDataPacket, IMUDataPacket, RawDataPacket From b5df4a8f4e3b03616611706b40f4cd6626d5b558 Mon Sep 17 00:00:00 2001 From: jgelia Date: Wed, 18 Sep 2024 10:53:53 -0400 Subject: [PATCH 4/4] comments --- airbrakes/imu/imu.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/airbrakes/imu/imu.py b/airbrakes/imu/imu.py index cdc565d5..105ecee6 100644 --- a/airbrakes/imu/imu.py +++ b/airbrakes/imu/imu.py @@ -102,13 +102,14 @@ def _fetch_data_loop(self, port: str, frequency: int, _: bool): channel = data_point.channelName() # This cpp file was the only place I was able to find all the channel names # https://github.com/LORD-MicroStrain/MSCL/blob/master/MSCL/source/mscl/MicroStrain/MIP/MipTypes.cpp + # Check if the channel name is one we want to save if hasattr(imu_data_packet, channel) or "Quaternion" in channel: # First checks if the data point needs special handling, if not, just set the attribute match channel: # These specific data points are matrix's rather than doubles case "estAttitudeUncertQuaternion" | "estOrientQuaternion": + # This makes a 4x1 matrix from the data point with the data as [[x], [y], [z], [w]] matrix = data_point.as_Matrix() - # Converts the [4x1] matrix to the X, Y, Z, and W of the quaternion # Sets the X, Y, Z, and W of the quaternion to the data packet object setattr(imu_data_packet, f"{channel}X", matrix.as_floatAt(0, 0)) setattr(imu_data_packet, f"{channel}Y", matrix.as_floatAt(0, 1))