Epson IMU ROS2 Node SPI


ess_sensors/g552.jpg ess_sensors/g370.jpg ess_sensors/g365.jpg

Overview


  • The Epson IMU ROS software is C++ wrapper around a C Linux driver for communicating on a ROS2 system

  • This code assumes that the user is familiar with building ROS2 packages using the colcon build process
  • This is *NOT* detailed step by step instructions describing how to build and install this ROS2 driver
  • Refer to the README.md inside the package for more detailed up-to-date technical info on usage

SPI Connection & Configuration


  • Since SPI interfaces are non-standard on PCs, typically IMU SPI interfacing is applicable only to embedded Linux platforms
  • When connecting the Epson device to a ROS2 system using the SPI interface:
  • This ROS2 driver uses the external library Unofficial wiringPi library to generate low level SPI communication & accurate time delays

    • Other libraries can be used to generate timer delays and SPI messages but requires minor changes to redirect the low-level function calls (i.e. bcm2835 C library, etc)
  • Accurate timing delays are necessary to optimize throughput for high dout_rate when using the SPI interface (refer to IMU datasheets for timing specifications)

Below is an example IMU SPI connection when using a Raspberry Pi:

  • Epson IMU

    Raspberry Pi

    EPSON_RESET Input

    RPI_GPIO_P1_15 (GPIO22) Output

    EPSON_DRDY Output

    RPI_GPIO_P1_18 (GPIO24) Input

    EPSON_CS Input

    RPI_GPIO_P1_16 (GPIO23) Output

    EPSON_SCLK Input

    RPI_GPIO_P1_23 (GPIO11) Output

    EPSON_DOUT Output

    RPI_GPIO_P1_19 (GPIO10) Input

    EPSON_DIN Input

    RPI_GPIO_P1_21 (GPIO9) Output

Support for Timer Delays

  • There are wrapper functions for time delays in millisecond and microseconds using seDelayMS() and seDelayMicroSecs(), respectively.
  • On embedded Linux platforms, these may need to be redirected to platform-specific HW delay routines if not using a RaspberryPi

  • For example on RaspberryPi, the time delay functions for millisecond and microseconds is redirected to WiringPi library delay() and delayMicroseconds() function calls, respectively.

  • If a hardware delay is not available from a library, then a software delay loop is possible, but not preferred

Support for GPIO Control

  • 3 GPIO pins on the host are designated used and should be connected to the IMU
    • GPIO output for asserting CS# on the IMU (NOTE: Not using pin SPI0_CE0)
    • GPIO output for asserting RESET# on the IMU
    • GPIO input for reading the logic status on the IMU DRDY (GPIO1) pin
  • The use of GPIO pins for connecting to the IMU CS# and DRDY is mandatory
  • RESET# is recommended to force Hardware Reset during every IMU initialization for better robustness.
  • This code is structured to easily redirect to low-level hardware GPIO function calls for easy implementation such as RaspberryPi.

  • There is no standard method to implement GPIO connections on an embedded Linux platform, but the following files in the package are an example for RaspberryPi (Refer to the README.md inside the package for a detailed description)

  src/hcl_rpi.c
  src/hcl_gpio.c
  src/hcl_gpio.h

ROS2 Node


Published Topics

  • For all IMU models (except for G365 when quaternion output function is enabled), the orientation field in sensor_msgs/Imu will not contain valid data and should be ignored

  • epson_imu/data_raw - only header, angular velocity, and linear acceleration are valid. Orientation field should be ignored

---
header:
  stamp:
    sec: 1602020116
    nanosec: 892343523
  frame_id: imu_link
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: 0.0008243194897659123
  y: 9.91016931948252e-05
  z: -0.00020670934463851154
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
  x: -1.4295457601547241
  y: -2.184906244277954
  z: 9.49894905090332
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
  • For IMU model G365 when quaternion output function is enabled, the orientation field in sensor_msgs/Imu will contain valid data

  • epson_imu/data - header, orientation (quaternion), angular velocity, and linear acceleration

---
header:
  stamp:
    sec: 1602020004
    nanosec: 667342534
  frame_id: imu_link
orientation:
  x: -0.11234959959983826
  y: 0.07211977988481522
  z: 0.007866359315812588
  w: 0.9910168647766113
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: -0.0018992983968928456
  y: 0.0007579181110486388
  z: 0.0010170674649998546
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
  x: -1.4219565391540527
  y: -2.176177740097046
  z: 9.500624656677246
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

Launch File

  • The launch file in the launch folder of this package is used to pass the init parameters to configure the IMU at runtime using ros2 launch

  • Changes to init parameters by editing the launch file does not require rebuilding with colcon
  • The following is a brief description of sample launch files:

Launch File

Comment

epson_g320_g354_g364_launch.py

launch file for G354, G364 orientation field data should be ignored

epson_g325_g365_launch.py

launch file for G365 without quaternion enabled orientation field data should be ignored

epson_g325_g365q_launch.py

launch file for G365 with quaternion enabled orientation field data is valid

epson_g370_launch.py

launch file for G370 orientation field data should be ignored

Init Parameters

  • The following is a list of typical init parameter settings in a launch file (refer to the individual launch file for full listing and embedded comments)
  • For more technical descriptions of register settings please refer to the specific IMU datasheet
  • Typically, the user only needs to modify dout_rate & filter_sel as needed based on system requirements

Init Parameter

Comment

imu_topic

specifies the topic name for publishing IMU messages

imu_dout_rate

specifies the IMU output data rate in Hz

imu_filter_sel

specifies the IMU filter setting

quaternion_output_en

enables or disables quaternion output (supported only for G365)

atti_profile

specifies the attitude motion profile (supported only for G365)

ext_trigger_en

enables triggering of IMU samples using IMU external trigger function on IMU GPIO2/EXT pin. Cannot be enabled when time correction enabled (uses the counter reset function)

time_correction_en

enables time correction function using IMU counter reset function & external 1PPS connection to IMU GPIO2/EXT pin to time align the IMU message to the most resent 1 PPS pulse. Cannot be enabled when external trigger enabled

qtn_out

specifies to enable or disable Quaternion in IMU output data (only support for G365)

atti_mode

specifies the attitude mode as 0=inclination or 1=euler. Typically, set to euler when used (only support for G365)

atti_profile

specifies the attitude motion profile (supported only for G365PDF1)

Timestamping With EXT Signal

  • The G3xx series Epson IMU has 3.3V I/O EXT (GPIO2) pin which can be connected to a cyclic external sync signal such as a GNSS 1PPS signal
  • When this pin is enabled with external counter reset function (refer to the IMU datasheet for more details) a free running internal 16-bit up-counter value is sent out with every sensor sample
  • Every active edge that is detected on the EXT pin causes the free-running internal 16-bit up-counter to increment starting from 0
  • The 16-bit reset counter value in each sensor sample provides a mechanism to determine the relative time delay from the most recent GNSS 1PPS pulse
  • When the init parameter time_correction is enabled (including ext_sel set to reset counter & count_out enabled)

    • The ROS2 driver attempts to correct the time stamp field in sensor_msgs/Imu based on the reset counter value (measured delay since latest GNSS 1PPS)

    • This should give a more accurate timestamp of the inertial data by minimizing the effects of link delays or host processing overhead delays

Building & Installing ROS2 Node


1. Place this package (including folders) into a new folder within your colcon workspace "src" folder.

  • For example, we recommend using the folder name "epson_imu_spi_ros2"

<colcon_workspace>/src/epson_imu_spi_ros2/ <-- place files here

2. Modify the CMakeLists.txt to select the desired Epson IMU model that is being used in the ROS2 system.

  • Refer to the comment lines inside the CMakeLists.txt for additional info.

  • NOTE: You *MUST* re-build using colcon when changing IMU models or any changes in the CMakeLists.txt

3. From the colcon workspace folder run "colcon build" to build all changed ROS2 packages located in the <colcon_workspace>/src/ folder.

  • To build this specific package type the following:

   <colcon_workspace>/colcon build --packages-select epson_imu_uart_ros2 --symlink-install
  • Re-run the above "colcon build" command to rebuild the driver after making any changes to the CMakeLists.txt (or any of the .c or .cpp or .h source files)

  • *NOTE:* It is recommended to change IMU settings by editing the parameters in the launch file, wherever possible, instead of modifying the .c or .cpp source files directly
  • It is not necessary to "colcon build" if changes are only made to the launch.py files

Example console output of colcon build for G370PDF1

guest@guest-desktop:~/dev_ws$ colcon build --packages-select epson_imu_spi_ros2 --symlink-install
Starting >>> epson_imu_spi_ros2
[Processing: epson_imu_spi_ros2]
--- stderr: epson_imu_spi_ros2
[STATUS]---- Building for IMU Model: G370PDF1
[STATUS]---- Building for platform: RPI
[STATUS]---- Building for interface: SPI
---
Finished <<< epson_imu_spi_ros2 [52.5s]

Summary: 1 package finished [54.1s]
  1 package had stderr output: epson_imu_spi_ros2
guest@guest-desktop:~/dev_ws$

4. Reload the current ROS2 environment variables that may have changed after the colcon build process.

From the <colcon_workspace>: . install/setup.bash

5. Modify the appropriate launch file to set your desired IMU init parameters for the specific IMU model in the launch folder that you selected and built

  • Typically, only the imu_dout_rate & imu_filter_sel needs to be edited

Running the ROS2 Node


  • To start the Epson IMU ROS2 driver use the appropriate launch file (located in launch/) with ros2 launch

  • For example, for the Epson G370 IMU:

<colcon_workspace>/roslaunch epson_imu_spi_driver epson_g325_g365.launch

Example console output of launching ROS2 node for G365PDF1

guest@guest-desktop:~/dev_ws$ ros2 launch epson_imu_spi_ros2 g325_g365q_launch.py
[INFO] [launch]: All log files can be found below /home/guest/.ros/log/2020-10-06-14-32-59-842954-guest-desktop-20304
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [imu_node-1]: process started with pid [20314]
[imu_node-1] [INFO] [epson_node]: frame_id:             imu_link
[imu_node-1] [INFO] [epson_node]: time_correction_en:   0
[imu_node-1] [INFO] [epson_node]: ext_trigger_en:       0
[imu_node-1] [INFO] [epson_node]: burst_polling_rate:   4000.0
[imu_node-1] [INFO] [epson_node]: imu_dout_rate:        4
[imu_node-1] [INFO] [epson_node]: imu_filter_sel:       6
[imu_node-1] [INFO] [epson_node]: quaternion_output_en: 1
[imu_node-1] [INFO] [epson_node]: imu_topic:            /epson_imu/data
[imu_node-1] [INFO] [epson_node]: output_32bit_en:      1
[imu_node-1] [INFO] [epson_node]: atti_profile:         0
[imu_node-1]
[imu_node-1] Initializing libraries......done.[INFO] [epson_node]: Checking sensor power on status...
[imu_node-1] [INFO] [epson_node]: Initializing Sensor...
[imu_node-1] [INFO] [epson_node]: Epson IMU initialized.
[imu_node-1] [INFO] [epson_node]: PRODUCT ID:   G365PDF1
[imu_node-1] [INFO] [epson_node]: SERIAL ID:    X0000013
[imu_node-1]

Technical Support


Wiki: ess_sensors/ros2spi (last edited 2022-02-22 22:09:15 by RChow)