ellipsoid calibration, parameter conversion script and more documentation

.gitignore

@@ -0,0 +1,2 @@
+# Specify filepatterns you want git to ignore.
+*.vscode

README.md

@@ -1,5 +1,4 @@
-i2c_imu
-=======
+# i2c_imu
 
 ROS driver for several 9-DOF IMUs.  This node is a ROS interface for [RTIMULib2](https://github.com/RTIMULib/RTIMULib2) by [richards-tech](http://richardstechnotes.wordpress.com/), a C++ library of drivers & filters of operating I2C IMUs in linux.  I take no credit for RTIMULib (the hard part), I simply created a ROS interface to it (the easy part).
 
@@ -11,4 +10,73 @@ The list of IMUs supported by RTIMULib is as follows:
 - [Adafruit 9-DOF IMU Breakout - L3GD20 + LSM303](http://www.adafruit.com/product/1714)
 - [MinIMU-9 v3 Gyro, Accelerometer, and Compass (L3GD20H and LSM303D Carrier) by Polulu](http://www.pololu.com/product/2468)
 
+## i2c_imu_node
+### Published topics
+* data (sensor_msgs/Imu): Data from the IMU.
+* mag (sensor_msgs/MagneticField): Data from the magnetometer, only published if the `publish_magnetometer` parameter is set to true.
+* euler (geometry_msgs/Vector3): Orientation as euler angles, only published if the `publish_euler` parameter is set to true.
+
+### Parameters
+* ~frame_id (string, default: "imu_link"): Name of the IMU's frame.
+* ~publish_magnetometer (bool, default: false): Flag to publish the magnetometer data as a sensor_msgs/MagneticField message.
+* ~publish_euler (bool, default: false): Flag to publish the orientation data as euler angles in a geometry_msgs/Vector3 message.
+* ~orientation_covariance (double[9], default: [0] * 9): Orientation covariance matrix.
+* ~angular_velocity_covariance (double[9], default: [0] * 9): Angular velocity covariance matrix.
+* ~linear_acceleration_covariance (double[9], default: [0] * 9): Linear acceleration covariance matrix.
+* ~magnetic_declination (double, default: 0): Magnetic declaration. You can find it using a [magnetic declination estimator](https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml).
+
+**IMU specific parameters are not listed here**
+
+## Installation
+1. Install RTIMULib2 dependencies. Qt4 is necessary to build the GUI apps included in the library and Octave is used for ellipsoid calibration of the magnetometer.
+    ```bash
+    sudo apt update
+    sudo apt install qt4-default liboctave-dev
+    ```
+
+2. Build and install RTIMULib2 from source
+    ```bash
+    mkdir ~/software && cd ~/software
+    git clone https://github.com/RTIMULib/RTIMULib2.git
+    cd RTIMULib2/Linux
+    mkdir build && cd build
+    cmake ..
+    make -j4
+    sudo make install
+    ```
+
+3. Build the package
+    ```bash
+    cd ~/catkin_ws
+    catkin_make
+    ```
+
+## Usage
+The simplest way to use this package is with
+```bash
+roslaunch i2c_imu i2c_imu_auto.launch
+```
+This will automatically detect the IMU and use the default parameters.
+
+If you want to use custom parameters, such as calibration data, you can specify them directly in the launch file or in a yaml file. Look at `mpu_9150_param.launch` or `mpu_9150.launch` for an example.
+
+## How to calibrate
+Calibration can be done using the tools from RTIMULib2. Instructions on how to do so can be found in [this guide](https://github.com/RTIMULib/RTIMULib2/blob/master/Calibration.pdf).
+
+**Important**: to avoid an error while doing ellipsoid calibration, you'll want to run `RTIMULibCal` from the `RTEllipsoidFit` folder located where you cloned the RTIMULib2 repo. For example:
+```bash
+cd ~/dev/RTIMULib2/RTEllipsoidFit/
+RTIMULibCal
+```
+
+By default, the calibration data will be written to a file named `RTIMULib.ini` located in the directory where you ran the calibration tool. In order for the `i2c_imu_node` to use this data, it needs to be converted into a yaml file. A script has been made to automatically do the conversion. It can be found in the `scripts` folder.
+
+**Note**: right now, it will only convert the data for the mpu9250 imu.
+
+### Usage
+```bash
+cd scripts
+./convert_params path/to/RTIMULib.ini path/to/params.yaml
+```
+
 

scripts/convert_params.py

@@ -0,0 +1,227 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+from pathlib import Path
+
+source_path = sys.argv[1]
+target_path = sys.argv[2]
+
+# Make sure source path exists and create target directory if needed
+if os.path.exists(source_path):
+    print("Source path: '" + str(source_path) + "'")
+else:
+    print("Error: source path: '" + str(source_path) + "' does not exist")
+    exit()
+
+Path(os.path.dirname(target_path)).mkdir(parents=True, exist_ok=True)
+print("Target path: '" + str(target_path) + "'")
+
+
+imu_types = {
+    2: "mpu9150",
+    3: "GD20HM303D",
+    4: "GD20M303DLHC",
+    5: "LSM9DS0",
+    7: "mpu9250",
+    8: "GD20HM303DLHC"
+}
+
+imu_name = None
+general_params = {}
+imu_params = {}
+calibration = {
+    "compass_min": [0] * 3,
+    "compass_max": [0] * 3,
+    "compass_cal_offset": [0] * 3,
+    "compass_cal_corr": [0] * 9,
+    "accel_min": [0] * 3,
+    "accel_max": [0] * 3
+}
+
+
+def get_value_from_line(line, key, dict, type, index=-1):
+    line = line.strip()
+    split_line = line.split("=")
+    value = type(split_line[1])
+    if index >= 0:
+        dict[key][index] = value
+    else:
+        dict[key] = value
+
+
+# Parse file
+with open(source_path, "r") as f:
+    lines = f.readlines()
+
+    for line in lines:
+        # Get general parameters
+        if "IMUType" in line:
+            get_value_from_line(line, "imu_type", general_params, int)
+            imu_name = imu_types[general_params["imu_type"]]
+        elif "I2CBus" in line:
+            get_value_from_line(line, "i2c_bus", general_params, int)
+        elif "I2CSlaveAddress" in line:
+            get_value_from_line(line, "i2c_slave_address", general_params, int)
+        elif "FusionType" in line:
+            get_value_from_line(line, "fusion_type", general_params, int)
+        elif "compassAdjDeclination" in line:
+            get_value_from_line(line, "magnetic_declination", general_params, float)
+
+        # Get calibration data
+        elif "CompassCalMinX" in line:
+            get_value_from_line(line, "compass_min", calibration, float, index=0)
+        elif "CompassCalMinY" in line:
+            get_value_from_line(line, "compass_min", calibration, float, index=1)
+        elif "CompassCalMinZ" in line:
+            get_value_from_line(line, "compass_min", calibration, float, index=2)
+        elif "CompassCalMaxX" in line:
+            get_value_from_line(line, "compass_max", calibration, float, index=0)
+        elif "CompassCalMaxY" in line:
+            get_value_from_line(line, "compass_max", calibration, float, index=1)
+        elif "CompassCalMaxZ" in line:
+            get_value_from_line(line, "compass_max", calibration, float, index=2)
+        elif "compassCalOffsetX" in line:
+            get_value_from_line(line, "compass_cal_offset", calibration, float, index=0)
+        elif "compassCalOffsetY" in line:
+            get_value_from_line(line, "compass_cal_offset", calibration, float, index=1)
+        elif "compassCalOffsetZ" in line:
+            get_value_from_line(line, "compass_cal_offset", calibration, float, index=2)
+        elif "compassCalCorr" in line:
+            split_line = line.split("=")
+            row = int((split_line[0])[-2])
+            col = int((split_line[0])[-1])
+            index = (row - 1) * 3 + (col - 1)
+            get_value_from_line(line, "compass_cal_corr", calibration, float, index=index)
+        elif "AccelCalMinX" in line:
+            get_value_from_line(line, "accel_min", calibration, float, index=0)
+        elif "AccelCalMinY" in line:
+            get_value_from_line(line, "accel_min", calibration, float, index=1)
+        elif "AccelCalMinZ" in line:
+            get_value_from_line(line, "accel_min", calibration, float, index=2)
+        elif "AccelCalMaxX" in line:
+            get_value_from_line(line, "accel_max", calibration, float, index=0)
+        elif "AccelCalMaxY" in line:
+            get_value_from_line(line, "accel_max", calibration, float, index=1)
+        elif "AccelCalMaxZ" in line:
+            get_value_from_line(line, "accel_max", calibration, float, index=2)
+
+        # Get imu parameters
+        if imu_name == "mpu9150":
+            if "MPU9150GyroAccelSampleRate" in line:
+                get_value_from_line(line, "gyro_accel_sample_rate", imu_params, int)
+            elif "MPU9150CompassSampleRate" in line:
+                get_value_from_line(line, "compass_sample_rate", imu_params, int)
+            elif "MPU9150GyroAccelLpf" in line:
+                get_value_from_line(line, "gyro_accel_low_pass_filter", imu_params, int)
+            elif "MPU9150GyroFSR" in line:
+                get_value_from_line(line, "gyro_full_scale_range", imu_params, int)
+            elif "MPU9150AccelFSR" in line:
+                get_value_from_line(line, "accel_full_scale_range", imu_params, int)
+        elif imu_name == "mpu9250":
+            if "MPU9250GyroAccelSampleRate" in line:
+                get_value_from_line(line, "gyro_accel_sample_rate", imu_params, int)
+            elif "MPU9250CompassSampleRate" in line:
+                get_value_from_line(line, "compass_sample_rate", imu_params, int)
+            elif "MPU9250GyroLpf" in line:
+                get_value_from_line(line, "gyro_low_pass_filter", imu_params, int)
+            elif "MPU9250AccelLpf" in line:
+                get_value_from_line(line, "accel_low_pass_filter", imu_params, int)
+            elif "MPU9250GyroFSR" in line:
+                get_value_from_line(line, "gyro_full_scale_range", imu_params, int)
+            elif "MPU9250AccelFSR" in line:
+                get_value_from_line(line, "accel_full_scale_range", imu_params, int)
+        elif imu_name == "GD20HM303D":
+            if "GD20HM303DGyroSampleRate" in line:
+                get_value_from_line(line, "gyro_sample_rate", imu_params, int)
+            elif "GD20HM303DGyroFsr" in line:
+                get_value_from_line(line, "gyro_full_scale_range", imu_params, int)
+            elif "GD20HM303DGyroHpf" in line:
+                get_value_from_line(line, "gyro_high_pass_filter", imu_params, int)
+            elif "GD20HM303DGyroBW" in line:
+                get_value_from_line(line, "gyro_bandwidth", imu_params, int)
+            elif "GD20HM303DAccelSampleRate" in line:
+                get_value_from_line(line, "accel_sample_rate", imu_params, int)
+            elif "GD20HM303DAccelFsr" in line:
+                get_value_from_line(line, "accel_full_scale_range", imu_params, int)
+            elif "GD20HM303DAccelLpf" in line:
+                get_value_from_line(line, "accel_low_pass_filter", imu_params, int)
+            elif "GD20HM303DCompassSampleRate" in line:
+                get_value_from_line(line, "compass_sample_rate", imu_params, int)
+            elif "GD20HM303DCompassFsr" in line:
+                get_value_from_line(line, "compass_full_scale_range", imu_params, int)
+        elif imu_name == "GD20M303DLHC":
+            if "GD20M303DLHCGyroSampleRate" in line:
+                get_value_from_line(line, "gyro_sample_rate", imu_params, int)
+            elif "GD20M303DLHCGyroFsr" in line:
+                get_value_from_line(line, "gyro_full_scale_range", imu_params, int)
+            elif "GD20M303DLHCGyroHpf" in line:
+                get_value_from_line(line, "gyro_high_pass_filter", imu_params, int)
+            elif "GD20M303DLHCGyroBW" in line:
+                get_value_from_line(line, "gyro_bandwidth", imu_params, int)
+            elif "GD20M303DLHCAccelSampleRate" in line:
+                get_value_from_line(line, "accel_sample_rate", imu_params, int)
+            elif "GD20M303DLHCAccelFsr" in line:
+                get_value_from_line(line, "accel_full_scale_range", imu_params, int)
+            elif "GD20M303DLHCCompassSampleRate" in line:
+                get_value_from_line(line, "compass_sample_rate", imu_params, int)
+            elif "GD20M303DLHCCompassFsr" in line:
+                get_value_from_line(line, "compass_full_scale_range", imu_params, int)
+        elif imu_name == "GD20HM303DLHC":
+            if "GD20HM303DLHCGyroSampleRate" in line:
+                get_value_from_line(line, "gyro_sample_rate", imu_params, int)
+            elif "GD20HM303DLHCGyroFsr" in line:
+                get_value_from_line(line, "gyro_full_scale_range", imu_params, int)
+            elif "GD20HM303DLHCGyroHpf" in line:
+                get_value_from_line(line, "gyro_high_pass_filter", imu_params, int)
+            elif "GD20HM303DLHCGyroBW" in line:
+                get_value_from_line(line, "gyro_bandwidth", imu_params, int)
+            elif "GD20HM303DLHCAccelSampleRate" in line:
+                get_value_from_line(line, "accel_sample_rate", imu_params, int)
+            elif "GD20HM303DLHCAccelFsr" in line:
+                get_value_from_line(line, "accel_full_scale_range", imu_params, int)
+            elif "GD20HM303DLHCCompassSampleRate" in line:
+                get_value_from_line(line, "compass_sample_rate", imu_params, int)
+            elif "GD20HM303DLHCCompassFsr" in line:
+                get_value_from_line(line, "compass_full_scale_range", imu_params, int)
+        elif imu_name == "LSM9DS0":
+            if "LSM9DS0GyroSampleRate" in line:
+                get_value_from_line(line, "gyro_sample_rate", imu_params, int)
+            elif "LSM9DS0GyroFsr" in line:
+                get_value_from_line(line, "gyro_full_scale_range", imu_params, int)
+            elif "LSM9DS0GyroHpf" in line:
+                get_value_from_line(line, "gyro_high_pass_filter", imu_params, int)
+            elif "LSM9DS0GyroBW" in line:
+                get_value_from_line(line, "gyro_bandwidth", imu_params, int)
+            elif "LSM9DS0AccelSampleRate" in line:
+                get_value_from_line(line, "accel_sample_rate", imu_params, int)
+            elif "LSM9DS0AccelFsr" in line:
+                get_value_from_line(line, "accel_full_scale_range", imu_params, int)
+            elif "LSM9DS0AccelLpf" in line:
+                get_value_from_line(line, "accel_low_pass_filter", imu_params, int)
+            elif "LSM9DS0CompassSampleRate" in line:
+                get_value_from_line(line, "compass_sample_rate", imu_params, int)
+            elif "LSM9DS0CompassFsr" in line:
+                get_value_from_line(line, "compass_full_scale_range", imu_params, int)
+
+
+# Write yaml file
+with open(target_path, "w") as f:
+    # General parameters
+    general_param_keys = list(general_params.keys())
+    for param in general_param_keys:
+        f.write(param + ": " + str(general_params[param]) + "\n")
+
+    # IMU parameters
+    f.write("\n" + imu_name + ": \n")
+    imu_param_keys = list(imu_params.keys())
+    for param in imu_param_keys:
+        f.write("  " + param + ": " + str(imu_params[param]) + "\n")
+
+    # Calibration data
+    f.write("\ncalib: \n")
+    calibration_keys = list(calibration.keys())
+    for key in calibration_keys:
+        f.write("  " + key + ": " + str(calibration[key]) + "\n")
+
+print("Conversion completed successfully")

src/i2c_imu_node.cpp

@@ -291,6 +291,26 @@ bool I2cImu::ImuSettings::loadSettings()
 	{
 	  	ROS_INFO("No Calibration for Compass");
 	}
+
+	std::vector<double> compass_cal_offset, compass_cal_corr;
+	if (settings_nh_->getParam("calib/compass_cal_offset", compass_cal_offset) &&
+		settings_nh_->getParam("calib/compass_cal_corr", compass_cal_corr))
+	{
+		m_compassCalEllipsoidOffset = RTVector3(compass_cal_offset[0],compass_cal_offset[1], compass_cal_offset[2]);
+		for (int i=0; i<3; i++)
+		{
+			for (int j=0; j<3; j++)
+			{
+				m_compassCalEllipsoidCorr[i][j] = compass_cal_corr[i*3 + j];
+			}
+		}
+		m_compassCalEllipsoidValid = true;
+		ROS_INFO("Got Ellipsoid Calibration for Compass");
+	}
+	else
+	{
+		ROS_INFO("No Ellipsoid Calibration for Compass");
+	}
 
 	std::vector<double> accel_max, accel_min;
 	if (settings_nh_->getParam("calib/accel_min", accel_min)