Using three LPMS-B2 IMU sensor

[mxochicale]

Raising this one to track implementations and testing of data collection using two LPMS-B2 IMU sensors. Refer to this document for further information https://lpresearch.bitbucket.io/openzen/latest/examples.html#synchronizing-multiple-sensors

References

• https://bitbucket.org/lpresearch/openzen/issues/7/synchronization-of-multiple-imus
• ps -ef | grep "python" | awk '{print $2}' | xargs sudo kill > https://superuser.com/questions/446808/how-to-manually-stop-a-python-script-that-runs-continuously-on-linux

[mxochicale]

Logs from $ python hello-multi-sensor.py on Sat 29 Jul 21:28:16 BST 2023

-----------------------
Sensor Listing
Connecting to sensors
Sensors connected
-----------------------
Sensor sync
-----------------------
Sensor sync completed 
-----------------------
Streaming data....
printing imu data from imu_s5b
printing imu data from imu_s95
printing imu data from imu_s5b
printing imu data from imu_s95
printing imu data from imu_s5b
printing imu data from imu_s95
printing imu data from imu_s5b
printing imu data from imu_s95
printing imu data from imu_s5b
printing imu data from imu_s95
        imu_s95
        Timestamp [s]: 0.5
        Quaternion [no unit]: [0.29773610830307007, 0.053178999572992325, 0.1739906221628189, -0.937151312828064]
printing imu data from imu_s5b
printing imu data from imu_s95
        imu_s95
        Timestamp [s]: 0.505

...

printing imu data from imu_s5b
printing imu data from imu_s95
        imu_s95
        Timestamp [s]: 0.75
        Quaternion [no unit]: [0.2972484827041626, 0.05551496893167496, 0.17061834037303925, -0.9377903938293457]
printing imu data from imu_s5b
printing imu data from imu_s95
        imu_s95
        Timestamp [s]: 0.755
        Quaternion [no unit]: [0.29724547266960144, 0.05551271140575409, 0.17061471939086914, -0.9377921223640442]
Streaming of sensor data complete
-----------------------
OpenZen library was closed

[mxochicale]

Hi Miguel, Thank you for your inquiry.
I have looked at the code that you sent over and notice a few things that does not make the program works as expected. Internally openzen has a data queue that stores streamed data from the sensor. It is recommended to clear the sensor queue before starting sensor data streaming. Otherwise, when calling the wait_for_next_event() function, openzen will returned the earliest data in the queue. The synchronization between sensor is done via "software". So even after synchronization, there will be some delay between the timestamps of two sensors. When the sensor is streaming data to the PC, the order of the data is not guaranteed to alternative between sensor 1 and 2. You might get something like below:

• sensor 1 @ T1
• sensor 1 @ T2
• sensor 1 @ T3
• sensor 2 @ T1
• sensor 2 @ T2
• sensor 1 @ T4
• sensor 2 @ T3
• .... Please note that the problem mentioned above will be more obvious when sensors are streaming a lot of data at high frequency. You will notice less of a problem when you reduce the streaming frequency of the sensors

I have made some edits to your code and from the initial testing, I am able to get sensor data from both IMU. Edit made to the script are in between "# LPRESEARCH EDIT " comments. Please try and see if it works.

Thank you.

Best regards, Yap

###########################################################################
#
# hello-multi-sensor.py: OpenZen Python example
#
# References:
# https://bitbucket.org/lpresearch/openzen/src/master/examples/ExamplePython.py
# https://lpresearch.bitbucket.io/openzen/latest/examples.html#synchronizing-multiple-sensors
###########################################################################

import sys
import time

import openzen

## FOR GNU/LINUX OS
## set PYTHONPATH to find OpenZen python module
## export PYTHONPATH=$HOME/repositories/openzen/build

s1MACid = "00:04:3E:6F:37:95"
s2MACid = "00:04:3E:53:ED:5B"
print(f"-----------------------")
print(f"Sensor Listing")

openzen.set_log_level(openzen.ZenLogLevel.Warning)

error, client = openzen.make_client()
if not error == openzen.ZenError.NoError:
    print("Error while initializing OpenZen library")
    sys.exit(1)

error = client.list_sensors_async()

#############################################################
print(f'Connecting to sensors')
error, sensor_s95 = client.obtain_sensor_by_name("Bluetooth", s1MACid)
if not error == openzen.ZenError.NoError:
    print("Error connecting to sensor", s1MACid)
    sys.exit(1)
imu_s95 = sensor_s95.get_any_component_of_type(openzen.component_type_imu)

error, sensor_s5b = client.obtain_sensor_by_name("Bluetooth", s2MACid)
if not error == openzen.ZenError.NoError:
    print("Error connecting to sensor", s2MACid)
    sys.exit(1)
imu_s5b = sensor_s5b.get_any_component_of_type(openzen.component_type_imu)
print(f'Sensors connected')

# LPRESEARCH EDIT START #################################################
# Set stream frequency
streamFreq = 100 # Hz
error = imu_s5b.set_int32_property(openzen.ZenImuProperty.SamplingRate, streamFreq)
error, freq = imu_s5b.get_int32_property(openzen.ZenImuProperty.SamplingRate)
print("Sampling rate imu_s5b: {}".format(freq))

error = imu_s95.set_int32_property(openzen.ZenImuProperty.SamplingRate, streamFreq)
error, freq = imu_s95.get_int32_property(openzen.ZenImuProperty.SamplingRate)
print("Sampling rate imu_s95: {}".format(freq))
# LPRESEARCH EDIT END ###################################################

##############################################################
print(f"-----------------------")
print(f'Sensor sync')
imu_s95.execute_property(openzen.ZenImuProperty.StartSensorSync)
imu_s5b.execute_property(openzen.ZenImuProperty.StartSensorSync)

## wait for 3 seconds for the syn commands to arrive
time.sleep(3)

# LPRESEARCH EDIT START #################################################
# Clear existing imu data in event queue
while client.poll_next_event():
    pass
# LPRESEARCH EDIT END ###################################################

print(f"-----------------------")
# set both sensors back to normal mode
imu_s95.execute_property(openzen.ZenImuProperty.StopSensorSync)
imu_s5b.execute_property(openzen.ZenImuProperty.StopSensorSync)
print(f'Sensor sync completed ')

##############################################################
print(f"-----------------------")
print(f'Streaming data....')

# LPRESEARCH EDIT START #################################################
total_number_of_samples = streamFreq * 10 # Collect 10 seconds of data
imu_s5b_data_count = 0
imu_s95_data_count = 0
# LPRESEARCH EDIT END ###################################################

while True:
    zenEvent = client.wait_for_next_event()

    if zenEvent.event_type == openzen.ZenEventType.ImuData and \
            zenEvent.sensor == imu_s5b.sensor and \
            zenEvent.component.handle == imu_s5b.component.handle:
        imu_data = zenEvent.data.imu_data
        # LPRESEARCH EDIT START ###########################################
        imu_s5b_data_count = imu_s5b_data_count + 1
        print(f'printing imu data from imu_s5b {imu_data.timestamp}: {imu_data.q}')
        """
        print(f'        imu_s5b')
        print(f'        Timestamp [s]: {imu_data.timestamp}')
        print(f'        Quaternion [no unit]: {imu_data.q}')
        """
        # LPRESEARCH EDIT END  ############################################

    if zenEvent.event_type == openzen.ZenEventType.ImuData and \
            zenEvent.sensor == imu_s95.sensor and \
            zenEvent.component.handle == imu_s95.component.handle:
        imu_data = zenEvent.data.imu_data
        # LPRESEARCH EDIT START ###########################################
        imu_s95_data_count = imu_s95_data_count + 1
        print(f'printing imu data from imu_s95 {imu_data.timestamp}: {imu_data.q}')
        """
        print(f'        imu_s95')
        print(f'        Timestamp [s]: {imu_data.timestamp}')
        print(f'        Quaternion [no unit]: {imu_data.q}')
        # LPRESEARCH EDIT END  ############################################
        """

    # LPRESEARCH EDIT START ###########################################
    # Check data count of 1 sensor as loop termination condition
    # for easier comparison
    if imu_s5b_data_count >= total_number_of_samples:
        break
    # LPRESEARCH EDIT END  ############################################

print("Streaming of sensor data complete")

print(f"-----------------------")
print(f"imu_s5b_data_count: {imu_s5b_data_count}")
print(f"imu_s95_data_count: {imu_s95_data_count}")
sensor_s5b.release()
sensor_s95.release()
client.close()
print("OpenZen library was closed")

[mxochicale]

Sorted out with https://github.com/mxochicale/sentient/blob/main/dependencies/openzen/hello-multi-sensors.py see more https://github.com/mxochicale/sentient/tree/main/dependencies/openzen