sbg_ros2_driver

KO | EN

gitlite — search

#python #java #python3 #arduino #golang #machine-learning #rust #html #flask #javascript #seismology #nodejs

sbg_ros2_driver

★ 54 Open GitHub ↗

ROS 2 driver for SBG Systems IMU/AHRS/INS units such as ELLIPSE or QUANTA.

// repository documentation

Was this content helpful?

★ 0 (0 ratings)

# sbg_driver
ROS2 driver package for SBG Systems IMU, AHRS and INS.  
This driver package uses the [sbgECom binary protocol](https://github.com/SBG-Systems/sbgECom) to read data and configure SBG Systems devices.

<i>Initial work has been done by [ENSTA Bretagne](https://github.com/ENSTABretagneRobotics).</i>

**Author: [SBG Systems](https://www.sbg-systems.com/)**  
**Maintainer: [SBG Systems](https://www.sbg-systems.com/)**  
**Contact:** support@sbg-systems.com

## Features
The driver supports all SBG systems sensors, IMUs, AHRS and INS, with a various degree of configurations available.

The driver supports the following features:
 - Parse IMU/AHRS/INS/GNSS using the sbgECom protocol
 - Publish standard ROS2 messages and more detailed specific SBG Systems topics
 - Subscribe and forward RTCM data to support DGPS/RTK mode with centimeters-level accuracy
 - Calibrate 2D/3D magnetic field using the on-board ELLIPSE algorithms
 - Configure ELLIPSE products using yaml files (see note below)

> [!NOTE]
> While the ROS2 drivers can be used with all SBG Systems sensors, the drivers can only be used to configure the Ellipse family.
> For other  INS such as EKINOX, APOGEE and QUANTA, please use the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/)
> For the PULSE-40, please use the sbgEcom encapsulation of the sbgInsRestApi: see our [Getting started Guide](https://support.sbg-systems.com/sc/imu/latest/getting-started)

## Installation
### Installation from Packages
User can install the sbg_ros2_driver through the standard ROS2 installation system.
* Humble ```sudo apt-get install ros-humble-sbg-driver```
* Galactic ```sudo apt-get install ros-galactic-sbg-driver```
* Foxy ```sudo apt-get install ros-foxy-sbg-driver```

### Building from sources
#### Dependencies
* [Robot Operating System (ROS2)](https://docs.ros.org/)
* [sbgECom C Library](https://github.com/SBG-Systems/sbgECom) (embeds v4.0.1987-stable - compatible with ELLIPSE firmware 2.5 and above)

#### Building
1. Clone the repository (use a Release version)
2. Build using the ROS2 colcon build system

```
cd colcon_ws/src
git clone https://github.com/SBG-Systems/sbg_ros2_driver.git
cd sbg_ros2_driver
rosdep update
rosdep install --from-path .
cd ../..
colcon build
source install/setup.bash
```

## Usage
To run the default ROS2 node with the default configuration file
```
ros2 launch sbg_driver sbg_device_launch.py
```

To run the magnetic calibration node

```
ros2 launch sbg_driver sbg_device_mag_calibration_launch.py
```

## Config files
### Default config files
Every configuration file is defined according to the same structure.

* **sbg_device_uart_default.yaml**
This config file is the default one for UART connection with the device.  
It does not configure the device through the ROS2 node, so it has to be previously configured (manually or with the ROS2 node).  
It defines a few outputs for the device:
  * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz
  * ROS2 standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz
  * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz.

* **sbg_device_udp_default.yaml**
This config file is the default one for an UDP connection with the device.  
It does not configure the device through the ROS2 node, so it has to be previously configured (manually).  
It defines a few outputs for the device:
  * `/sbg/imu_data`, `/sbg/ekf_quat` at 25Hz
  * ROS2 standard outputs `/imu/data`, `/imu/velocity`, `/imu/temp` at 25Hz
  * `/sbg/status`, `/sbg/utc_time` and `/imu/utc_ref` at 1Hz.

### Example config files
* **ellipse_A_default.yaml**
Default config file for an Ellipse-A.

* **ellipse_E_default.yaml**
Default config file for an Ellipse-E with an external NMEA GNSS.

* **ellipse_N_default.yaml**
Default config file for an Ellipse-N using internal GNSS.

* **ellipse_D_default.yaml**
Default config file for an Ellipse-D using internal GNSS.

## Launch files
### Default launch files
* **sbg_device_launch.py**
Launch the sbg_device node to handle the received data, and load the `sbg_device_uart_default.yaml` configuration.

* **sbg_device_mag_calibration_launch.py**
Launch the sbg_device_mag node to calibrate the magnetometers, and load the `ellipse_E_default.yaml` configuration.

## Nodes
### sbg_device node
The `sbg_device` node handles the communication with the connected device, publishes the SBG output to the ROS2 environment and subscribes to useful topics such as RTCM data streams.

#### Published Topics
##### SBG Systems specific topics
SBG Systems has defined proprietary ROS2 messages to report more detailed information from the AHRS/INS.  
These messages try to match as much as possible the sbgECom logs as they are output by the device.
>[!NOTE] 
> Please refer to the [firmware manual](https://support.sbg-systems.com/sc/dev/latest/firmware-documentation) to check the availability of the outputs per product.

* **`/sbg/status`** [sbg_driver/SbgStatus](http://docs.ros.org/api/sbg_driver/html/msg/SbgStatus.html)

Provides information about the general status (Communication, Aiding, etc..).
  
* **`/sbg/utc_time`** [sbg_driver/SbgUtcTime](http://docs.ros.org/api/sbg_driver/html/msg/SbgUtcTime.html)

Provides UTC time reference.

* **`/sbg/imu_data`** [sbg_driver/SbgImuData](http://docs.ros.org/api/sbg_driver/html/msg/SbgImuData.html)

IMU status, and sensors values.
  
* **`/sbg/ekf_euler`** [sbg_driver/SbgEkfEuler](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfEuler.html)

Computed orientation using Euler angles.
  
* **`/sbg/ekf_quat`** [sbg_driver/SbgEkfQuat](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfQuat.html)

Computed orientation using Quaternion.
  
* **`/sbg/ekf_nav`** [sbg_driver/SbgEkfNav](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfNav.html)

Computed navigation data.
  
* **`/sbg/ekf_vel_body`** [sbg_driver/SbgEkfVelBody](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfVelBody.html)

Computed velocity expressed in the INS body/vehicle frame.
  
* **`/sbg/ekf_rot_accel_body`** [sbg_driver/SbgEkfRotAccel](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfRotAccel.html)

Computed rotations rate and accelerations in the INS body/vehicle frame.
  
* **`/sbg/ekf_rot_accel_ned`** [sbg_driver/SbgEkfRotAccel](http://docs.ros.org/api/sbg_driver/html/msg/SbgEkfRotAccel.html)

Computed rotations rate and accelerations in North, East, Down (NED) navigation frame.
  
* **`/sbg/mag`** [sbg_driver/SbgMag](http://docs.ros.org/api/sbg_driver/html/msg/SbgMag.html)

Calibrated magnetic field measurement.
  
* **`/sbg/mag_calib`** [sbg_driver/SbgMagCalib](http://docs.ros.org/api/sbg_driver/html/msg/SbgMagCalib.html)

Magnetometer calibration data.
  
* **`/sbg/ship_motion`** [sbg_driver/SbgShipMotion](http://docs.ros.org/api/sbg_driver/html/msg/SbgShipMotion.html)

Heave, surge and sway data.
  
* **`/sbg/gps_vel`** [sbg_driver/SbgGpsVel](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsVel.html)

GPS velocities from GPS receiver.
  
* **`/sbg/gps_pos`** [sbg_driver/SbgGpsPos](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsPos.html)

GPS positions from GPS receiver.
  
* **`/sbg/gps_hdt`** [sbg_driver/SbgGpsHdt](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsHdt.html)

GPS true heading from dual antenna system.
  
* **`/sbg/gps_raw`** [sbg_driver/SbgGpsRaw](http://docs.ros.org/api/sbg_driver/html/msg/SbgGpsRaw.html)

GPS raw data for post processing.
  
* **`/sbg/odo_vel`** [sbg_driver/SbgOdoVel](http://docs.ros.org/api/sbg_driver/html/msg/SbgOdoVel.html)

Odometer velocity.
  
* **`/sbg/event[ABCDE]`** [sbg_driver/SbgEvent](http://docs.ros.org/api/sbg_driver/html/msg/SbgEvent.html)

Event on sync in the corresponding pin.
  
* **`/sbg/pressure`** [sbg_driver/SbgPressure](http://docs.ros.org/api/sbg_driver/html/msg/SbgPressure.html)

Pressure data.

##### ROS2 standard topics
In order to define ROS2 standard topics, it requires sometimes several SBG messages, to be merged.
For each ROS2 standard, you have to activate the needed SBG outputs.

* **`/imu/data`** [sensor_msgs/Imu](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html)

IMU data.
  Requires `/sbg/imu_data` or `/sbg/imu_short`.
  Optional (orientation) `/sbg/ekf_quat`.
  
* **`/imu/temp`** [sensor_msgs/Temperature](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Temperature.html)

IMU temperature data.
  Requires `/sbg/imu_data`.
  
* **`/imu/velocity`** [geometry_msgs/TwistStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/TwistStamped.html)

IMU velocity data.
  Requires `/sbg/imu_data`.
  
* **`/imu/mag`** [sensor_msgs/MagneticField](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/MagneticField.html)

IMU magnetic field.
  Requires `/sbg/mag`.
  
* **`/imu/pres`** [sensor_msgs/FluidPressure](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/FluidPressure.html)

IMU pressure data.
  Requires `/sbg/pressure`.
  
* **`/imu/pos_ecef`** [geometry_msgs/PointStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PointStamped.html)

Earth-Centered Earth-Fixed position.
  Requires `/sbg/ekf_nav`.
  
* **`/imu/utc_ref`** [sensor_msgs/TimeReference](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/TimeReference.html)

UTC time reference.
  Requires `/sbg/utc_time`.
  
* **`/imu/nav_sat_fix`** [sensor_msgs/NavSatFix](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/NavSatFix.html)

Navigation satellite fix for any Global Navigation Satellite System.
  Requires `/sbg/gps_pos`.
  
* **`/imu/odometry`** [nav_msgs/Odometry](http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/Odometry.html)

UTM projected position relative to the first valid INS position.
  Requires `/sbg/imu_data` and `/sbg/ekv_nav` and either `/sbg/ekf_euler` or `/sbg/ekf_quat`.
  Disabled by default, set `odometry.enable` in configuration file.

> [!NOTE]
> Please update the driver configuration to enable standard ROS2 messages publication. Also, the driver only publish standard ROS2 messages if the driver is setup to use ENU frame convention.

##### NMEA topics
The driver can publish NMEA GGA messages from the internal GNSS receiver. It can be used with third party [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) modules to support VRS networks providers.

Disabled by default, set `nmea.publish` to `true` in .yaml config file to use this feature.

* **`/ntrip_client/nmea`** [nmea_msgs/Sentence](http://docs.ros.org/en/api/nmea_msgs/html/msg/Sentence.html)  
  
  Data from `/sbg/gps_pos` serialized into NMEA GGA format. Requires `/sbg/gps_pos`.  
  Namespace `ntrip_client` and topic_name `nmea` can be customized in .yaml config files.

#### Subscribed Topics
##### RTCM topics
The `sbg_device` node can subscribe to RTCM topics published by third party ROS2 modules.  
Incoming RTCM data are forwarded to the INS internal GNSS receiver to enable DGPS/RTK solutions.

Disabled by default, set `rtcm.subscribe` to `true` in .yaml config file to use this feature.

* **`/ntrip_client/rtcm`** [rtcm_msgs/Message](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg)

RTCM data from `/ntrip_client/rtcm` will be forwarded to the internal INS GNSS receiver.  
  Namespace `ntrip_client` and topic_name `rtcm` can be customized in .yaml config files.

### sbg_device_mag node
The sbg_device_mag node is used to execute on board in-situ 2D or 3D magnetic field calibration.  
If you are planning to use magnetic based heading, it is mandatory to perform a magnetic field calibration in a clean magnetic environnement.

Only ELLIPSE products support magnetic based heading and feature the on-board magnetic field calibration process.

#### Services
* **`/sbg/mag_calibration`** [std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)

Service to start/stop the magnetic calibration.

* **`/sbg/mag_calibration_save`** [std_srvs/Trigger](http://docs.ros.org/api/std_srvs/html/srv/Trigger.html)

Service to save in FLASH memory the latest computed magnetic field calibration.

## HowTo
### Configure the SBG device
The SBG ROS2 driver allows the user to configure the SBG device before starting data parsing.  
To do so, set the corresponding parameter in the used config file.

```
# Configuration of the device with ROS2.
confWithRos: true
```

Then, modify the desired parameters in the config file, using the [Firmware Reference Manual](https://support.sbg-systems.com/sc/dev/latest/firmware-documentation), to see which features are configurable, and which parameter values are available.

> [!NOTE]
> The confWithRos parameter will only impact the configuration of the SBG device sensor, not the configuration of the ROS2 drivers themselves. 
> This means you can still configure RTCM corrections, reference frames, etc. when you set confWithRos to false.
> The confWithRos parameter must be disabled for HPI and pulse-40 products.

### Configure for RTK/DGPS
The `sbg_device` node can subscribe to [rtcm_msgs/Message](https://github.com/tilk/rtcm_msgs/blob/master/msg/Message.msg) topics to forward differential corrections to the INS internal GNSS receiver.

The RTCM data stream is sent through the serial/ethernet interface used by ROS2 to communicate with the INS.  
This enables simple and efficient RTK operations without requiring additional hardware or wiring.

When combined with a third party [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client), it offers a turnkey solution to access local VRS providers and get centimeter-level accuracy solutions.

The driver and the device should be properly setup:
 - Configure the INS to accept RTCM corrections on the interface used by the ROS2 driver:
   - For ELLIPSE, simply use the `sbgCenter` and in `Assignment panel`, `RTCM` should be set to `Port A`.
   - For High Performance INS, either use the configuration web interface or the [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/).
 - Install and configure a third party node that broadcast RTCM corrections such as a [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client)
 - Update the node config `yaml` file to set `rtcm.subscribe` and `nmea.publish` to `true`
 - If you use a different node to broadcast RTCM topics, you might have to update the config `yaml` file to update topics and namespaces.

### Calibrate the magnetometers
ELLIPSE products can use magnetometers to determine the heading. A calibration is then required to compensate for soft and hard iron distortions due to the vehicle the product is installed on. The magnetic calibration procedure should be held in a clean magnetic environnement (outside of buildings).

You can read more information about magnetic field calibration procedure from the SBG Systems [Support Center](https://support.sbg-systems.com/sc/kb/v3/inertial-sensors-installation/magnetic-calibration).

The ROS2 driver provides a dedicated node to easily use ELLIPSE on board magnetic field calibration algorithms.  
The ELLIPSE offers both a 2D and 3D magnetic field calibration mode.

1) Make sure you have selected the desired 2D or 3D magnetic field calibration mode (`calibration.mode` in the configuration `yaml` file).
2) Start a new magnetic calibration session once you are ready to map the magnetic field:

```
ros2 launch sbg_driver sbg_device_mag_calibration_launch.py
ros2 service call /sbg/mag_calibration std_srvs/srv/Trigger
```

> response:
> std_srvs.srv.Trigger_Response(success=True, message='Magnetometer calibration process started.')

3) Rotate as much as possible the unit to map the surrounding magnetic field (ideally, perform a 360° with X then Y then Z axis pointing downward).
4) Once you believe you have covered enough orientations, compute a magnetic field calibration:

```
ros2 service call /sbg/mag_calibration std_srvs/srv/Trigger
```

> response:
> std_srvs.srv.Trigger_Response(success=True, message='Magnetometer calibration is finished. See the output console to get calibration information.')

5) If you are happy with the results (Quality, Confidence), apply and save the new magnetic calibration parameters.  
   If not, you can continue to rotate the product and try to perform a new computation (and repeat step 4)

```
ros2 service call /sbg/mag_calibration_save std_srvs/srv/Trigger
```

> response:
> std_srvs.srv.Trigger_Response(success=True, message='Magnetometer calibration has been uploaded to the device.')

6) Reset/Power Cycle the device and you should now get an accurate magnetic based heading.

### Enable communication with the SBG device
To be able to communicate with the device, be sure that your user is part of the dialout group.  
Once added, restart your machine to save and apply the changes.

```
sudo adduser $USER dialout
```

### Create udev rules
Udev rules can be defined for communication port, in order to avoid modifying the port in configuration if it has changed.
[Udev documentation](https://wiki.debian.org/udev)

A symlink can be configured and defined to uniquely identify the connected device.  
Once it is done, configuration file could be updated `portName: "/dev/sbg"`.

See the docs folder, to see an example of rules with the corresponding screenshot using the udev functions.

### Time source & reference
ROS2 uses an internal system time to time stamp messages. This time stamp is generally gathered when the message is processed and published.
As a result, the message is not time stamped accurately due to transmission and processing delays.

SBG Systems INS however provides a very accurate timing based on GNSS time if available. The following conditions have to be met to get
absolute accurate timing information:
* The ELLIPSE-N or D should have a connected GNSS antenna with internal GNSS enabled
* The ELLIPSE-E should be connected to an external GNSS receiver with a PPS signal
* A valid GNSS position has to be available to get UTC data
* The ELLIPSE internal clock should be aligned to PPS signal (clock status)
* The ELLIPSE should be setup to send SBG_ECOM_LOG_UTC message

You can select which time source to use with the parameter `time_reference` to time stamp messages published by this driver:
* `ros`: The header.stamp member contains the current ROS2 system time when the message has been processed.
* `ins_unix`: The header.stamp member contains an absolute and accurate time referenced to UNIX epoch (00:00:00 UTC on 1 January 1970)

Configuration example to use an absolute and accurate time reference to UNIX epoch:
```
# Time reference:
time_reference: "ins_unix"
```

## Frame parameters & conventions
### Frame ID
The frame_id of the header can be set with this parameter:
```
# Frame name
frame_id: "imu_link_ned"
```

> [!NOTE]
> This parameter has not impact on the configuration and will be inserted as-is within the header field. The recommended default parameters for ros are imu_link when use_enu is activated, and imu_link_ned when use_enu is inactive. You can also use another frame_id as needed.

### GPS Frame ID
GPS position, velocity and true heading logs are expressed at the GNSS antenna phase center and not at the IMU.
The frame_id of the header of these messages (including `sensor_msgs/NavSatFix`) can be set with this parameter:
```
# GPS antenna frame name
gps_frame_id: "gps_link"
```

> [!NOTE]
> If this parameter is not set, it defaults to frame_id to keep the previous driver behavior. Set it to the frame of your GNSS antenna so consumers such as robot_localization navsat_transform_node can correctly compensate the antenna lever arm.

### Frame convention
The frame convention can be set to NED or ENU:
* The NED convention is SBG Systems native convention so no transformation is applied
* The ENU convention follows ROS2 standard [REP-103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions)

Please read the SBG Systems [Support Center article](https://support.sbg-systems.com/sc/kb/latest/underlying-maths-conventions) for more details.

You can select the frame convention to use with the following parameter:
```
# Frame convention
use_enu: true
```

> [!NOTE]
> The driver only publish standard ROS2 messages if the driver is setup to use ENU frame convention.

#### Body/Vehicle Frame:
The X axis should point the vehicle **forward** direction for both NED and ENU frame conventions. 
The table below summarizes the body/vehicle axis frame definitions for each convention:

| NED Convention | ENU Convention |
| -------------- | -------------- |
| X Forward      | X Forward      |
| Y Right        | Y Left         |
| Z Downward     | Z Upward       |

#### Navigation Frame:

The navigation frame also referred by ROS2 as the cartesian representation is defined as follow:

| NED Convention | ENU Convention |
| -------------- | -------------- |
| X North        | X East         |
| Y East         | Y North        |
| Z Down         | Z Up           |

#### Heading Example:

Based on the definitions above, when using a NED frame, if the vehicle X axis is pointing North, the INS should return a zero heading. 
When using a ENU frame, the INS should return a zero heading when the vehicle X axis is pointing East.

## Sensor fusion with state estimation packages

The ROS2 standard outputs of the driver can feed any state estimation stack consuming standard `sensor_msgs/Imu` and `sensor_msgs/NavSatFix` messages, for example to fuse the INS with wheel odometry on ground robots.

### Driver configuration
Standard ROS2 messages are disabled by default and require the ENU frame convention. In the .yaml configuration file:

```
output:
  # Publish ROS2 standard messages
  ros_standard: true
  # ROS2 standard messages require the ENU convention (REP-103)
  use_enu: true
  # Frame of the IMU body (FLU) and frame of the GNSS antenna
  frame_id: "imu_link"
  gps_frame_id: "gps_link"
  # SBG outputs required by the standard topics
  log_imu_data: 8       # /imu/data, /imu/velocity
  log_ekf_quat: 8       # /imu/data orientation
  log_gps1_pos: 10001   # /imu/nav_sat_fix
  log_utc_time: 200     # /imu/utc_ref
```

### Topics and frames

| Topic | Message | Content |
| ----- | ------- | ------- |
| `/imu/data` | sensor_msgs/Imu | Body-frame (FLU) rates and accelerations, orientation with respect to ENU |
| `/imu/nav_sat_fix` | sensor_msgs/NavSatFix | Raw GNSS position, expressed at the antenna phase center |
| `/imu/utc_ref` | sensor_msgs/TimeReference | INS UTC time reference |

State estimators rely on TF to compensate the GNSS antenna lever arm: publish static transforms from `base_link` to `frame_id` (IMU mounting) and from `base_link` to `gps_frame_id` (antenna mounting) matching the physical setup.

> [!NOTE]
> `/imu/nav_sat_fix` is not compensated for the antenna lever arm. The INS EKF fused position, expressed at the INS reference point, is available on `/sbg/ekf_nav` and `/imu/odometry`.

## Troubleshooting

If you experience higher latency than expected and have connected the IMU via an USB interface, you can enable the serial driver low latency mode:
```
/bin/setserial /dev/<device> low_latency
```

## Contributing
### Bugs and issues
Please report bugs and/or issues using the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros2_driver/issues)

### Features requests or additions
In order to contribute to the code, please use Pull requests to the `devel` branch.  
If you have some feature requests, use the [Issue Tracker](https://github.com/SBG-Systems/sbg_ros2_driver/issues) as well.

## Known limitations
> Baudrate configuration is not possible via ROS2 and can only be done in sbgCenter or using sbgECom library.
> Device information is not displayed for Pulse-40.
> GNSS and RTCM port configurations are not possible via ROS2.

sbg_ros2_driver

Explore Similar Repositories

Was this content helpful?

Recent Feedback

Download README