Robot Operating System and LP-Research IMUs? Simple!

NOTE: We have released a new version of our ROS / ROS 2 driver, please refer to this post.


Introduction

Robot Operating System (ROS) is a tool commonly used in the robotics community to pass data between various subsystems of a robot setup. We at LP-Research are also using it in various projects, and it is actually very familiar to our founders from the time of their PhDs. Inertial Measurement Units are not only a standard tool in robotics, the modern MEMS devices that we are using in our LPMS product line are actually the result of robotics research. So it seemed kind of odd that an important application case for our IMUs was not covered by our LpSensor software: namely, we didn’t provide a ROS driver.  We are very happy to tell you that such a driver exists, and we are happy that we don’t have to write it ourselves: the Larics laboratory at the University of Zagreb are avid users of both ROS and our LPMS-U2 sensors. So, naturally, they developed a ROS driver which they provide on their github site.  Recently, I had a chance to play with it, and the purpose of this blog post is to share my experiences with you, in order to get you started with ROS and LPMS sensors on your Ubuntu Linux system.

Installing the LpSensor Library

Please check our download page for the latest version of the library, at the time of this writing it is 1.3.5. I downloaded it, and then followed these steps to unpack and install it:

tar xvzf ~/Downloads/LpSensor-1.3.5-Linux-x86-64.tar.gz
sudo apt-get install libbluetooth-dev
sudo dpkg -i LpSensor-1.3.5-Linux-x86-64/liblpsensor-1.3.5-Linux.deb
dpkg -L liblpsensor

I also installed libbluettoth-dev, because without Bluetooth support, my LPMS-B2 would be fairly useless.

Setting up ROS and a catkin Work Space

If you don’t already have a working ROS installation, follow the ROS Installation Instructions to get started. If you already have a catkin work space you can of course skip this step, and substitute your own in what follows.  The work space is created as follows, note that you run catkin_init_workspace inside the src sub-directory of your work space.

mkdir -p ~/catkin_ws/src
cd !:2
catkin_init_workspace

Downloading and Compiling the ROS Driver for LPMS IMUs

We can now download the driver sources from github. It optionally makes use of and additional ROS module by the Larics laboratory which synchronizes time stamps between ROS and the IMU data stream.  Therefore, we have to clone two git repositories to obtain all prerequisites for building the driver.

cd ~/catkin_ws/src
git clone https://github.com/larics/timesync_ros.git
git clone https://github.com/larics/lpms_imu.git

That’s it, we are now ready to run catkin_make to get everything compiled and ready.  Building was as simple as running catkin_make, but you should setup the ROS environment before that.  If you haven’t, here’s how to do that:

cd ~/catkin_ws
source devel/setup.bash
catkin_make

This should go smoothly. Time for a test.

Not as Cool as LpmsControl, but Very Cool!

Now that we are set up, we can harness all of the power and flexibility of ROS. I’ll simply show you how to visualize the data using standard ROS tools without any further programming.  You will need two virtual terminals.  In the first start roscore, if you don’t have it running yet.  In the second, we start rqt_plot in order to see the data from our IMU, and the lpms_imu_node which provides it.  In the box you can see the command I use to connect to my IMU. You will have to replace the _sensor_model and _port strings with the values corresponding to your device.  Maybe it’s worth pointing out that the second parameter is called _port, because for a USB device it would correspond to its virtual serial port (typically /dev/ttyUSB0).

rosrun rqt_plot rqt_plot & # WordPress sometimes (but not always!) inserts "amp;" after the ampersand, if you see it, please ignore it.
rosrun lpms_imu lpms_imu_node _sensor_model:="DEVICE_LPMS_B2" _port:="00:04:3E:9E:00:8B"

Once you enter these commands, you will then see the familiar startup messages of LpSensor as in the screenshot below. As you can see the driver connected to my LPMS-B2 IMU right away. If you cannot connect, maybe Bluetooth is turned off or you didn’t enter the information needed to connect to your IMU.  Once you have verified the parameters, you can store them in your launch file or adapt the source code accordingly.

Screenshot starting LPMS ROS node

Screenshot of starting the LPMS ROS node

The lpms_imu_node uses the standard IMU and magnetic field message types provided by ROS, and it publishes them on the imu topic.  That’s all we need to actually visualize the data in realtime.  Below you can see how easy that is in rqt_plot. Not as cool as LpmsControl, but still fairly cool. Can you guess how I moved my IMU?

animation of how to display LPMS sensor data in ROS

Please get in touch with us, if you have any questions, or if you found this useful for your own projects.

Update: Martin Günther from the German Research Center for Artificial Intelligence was kind enough to teach me how to pass ROS parameters on the command line.  I’ve updated the post accordingly.

Visualization of Magnetic Field Calibration Data

One of the trickiest things for reliably measuring orientations with the LPMS is the calibration of the magnetic field sensor. The functionality of the sensor is essential for determining the yaw angle of the sensor without drift. If we used only the gyrsocope to measure the yaw angle a drift of a few angles would already occur after 10 or 20 seconds of movement.

The normally spherical shape of the environment magnetic field is, especially in the vicinity of metal or electric circuits, often distorted to an ellipsoid. Such distoritions are efficiently compensated by calibrating the LPMS. However it is hard for the user to see if the calibration was successful or what the resulting data means about the surrounding electromagnetic field. Therefore we added a visualization of this data to the control software of the sensor (LpmsControl) that is to give a better understanding of the calibration results (see image below).

We use a special algorithm to reduce the influence of a distorted magnetic environment field on the orientation measurements of the sensor. A comparison of orientation tracking without and with using this algorithm is shown below.

LPMS Head Tracking Demo

We have been playing around with several applications of the LPMS sensors and thought that an interesting one might be tracking a person’s head movements in real-time. This might be useful for virtual reality applications, medical training, tele-control systems etc. The important thing that we would like to show in the video above is that we can record head movements at high sampling frequencies (200Hz in the video) and low latency (average 1.5ms in the video). We use gyroscope information to achieve good response times and keep measurements drift-free by correcting the gyroscope data using the magnetometer (yaw axis) and accelerometer (roll and pitch axis).

This implementation is based on a great demo called San Angeles Observation by jetro (his page). We added some music of our own.

Field Distortion Compensation Algorithm

If the LPMS is operated in an environment with a partially distorted (non-homogeneous) earth magnetic field, there is the possibility of the orientation readings becoming inaccurate due to invalid data from the magnetometer unit. To prevent this we have extended our sensor fusion algorithm to detect such field distortion and automatically switch to operation without magnetometer. The switching between the two states happens seamlessly (without orientation jump) and, if the exposure to the distorted magnetic field happens for a limited amount of time, without any major orientation drift.

Please see the video below for a demonstration of the improved filter. An iron plate is used to distort the magnetic field. As soon as the sensor gets close to the metal surface the magnetic field vector starts changing direction deliberately. The color of the cube on the monitor turns red in case of the detection of a distorted magnetic field.

Better Batteries

So far we used quite a small battery (80 mAh) to power the LPMS. This worked quite well for some time, but in the end we realized that continuous runtimes of about 1h are just not enough to do practical measurements. We therefore started experimenting with batteries with higher capacity. As you can see in the image below, we were able to prolong runtimes quite a bit, with luckily not having to compromise on size and weight of the sensor too much.

We are not quite ready release the complete specifications of these new types, but basically they will be energy optimized also on the software side and the battery will be exchangeable, so that the user can decide which size he prefers. Also for longer measurements, it might be handy to have a few spare batteries. We will sell additional batteries separately at a decent price.