Python example: hwdriver-tao-imu-usb.py

This example shows how to read a Taobotics USB IMU with the pymrpt hwdrivers module.

 1#!/usr/bin/env python3
 2
 3# ---------------------------------------------------------------------
 4# Install python3-pymrpt, ros-$ROS_DISTRO-mrpt2, or test with a local build with:
 5# export PYTHONPATH=$HOME/code/mrpt/build-Release/:$PYTHONPATH
 6# ---------------------------------------------------------------------
 7
 8from mrpt.pymrpt import mrpt
 9
10imu = mrpt.hwdrivers.CTaoboticsIMU()
11
12imu.setSerialPort('/dev/ttyUSB0')
13
14# Opens the serial port and start listening
15imu.initialize()
16
17while True:
18    # Process serial port data queue:
19    imu.doProcess()
20
21    # Get observations:
22    obsList = imu.getObservations()
23
24    # Process them:
25    if not obsList.empty():
26        print('Read {} observations'.format(obsList.size()))
27        for t, obs in obsList:
28            timestamp = mrpt.Clock.toDouble(t)
29            print('Time={} obs={}'.format(timestamp, obs.asString()))
30
31            # You can access individual IMU readings with:
32            # o = mrpt.obs.CObservationIMU(obs)
33            # angVel_z = o.get(mrpt.obs.TIMUDataIndex.IMU_WZ)
34            #
35            # See: https://docs.mrpt.org/reference/latest/enum_mrpt_obs_TIMUDataIndex.html