Package Summary

The Epson IMU Driver using UART interface for ROS package

Package Summary

The Epson IMU Driver using UART interface for ROS package

Epson IMU ROS1 Node UART


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

Overview


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

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

UART Connection & Configuration


  • When connecting the Epson device to a ROS system using the UART interface:
    • direct connection to UART port must be compatible with CMOS 3.3V I/O
    • connection is also possible through a USB-UART 3.3V converter IC such as on the M-G32EV041 evaluation board

    • GPIO output for asserting RESET# pin on the IMU is optional, but recommended if possible (i.e. embedded platforms)
    • Below is an example IMU UART connection when using a Raspberry Pi:
      • Epson IMU

        Raspberry Pi

        EPSON_SIN Input

        RPI_GPIO_P1_8 (GPIO14/UART_TXD) Output

        EPSON_SOUT Output

        RPI_GPIO_P1_10 (GPIO15/UART_RXD) Input

        EPSON_RESET Input

        RPI_GPIO_P1_15 (GPIO22) Output *Optional

  • When connecting the Epson G552PR1/G552PR7 to a ROS system using a RS422 UART interface:
    • direct connection to RS422 UART port must be compatible with RS422 I/O signal levels
    • Below is an example IMU RS422 connection to a RS422 Host:
      • Epson IMU

        RS422 Host

        EPSON_TD+ Output

        HOST_RD+ Input

        EPSON_TD- Output

        HOST_RD- Input

        EPSON_RD+ Input

        HOST_TD+ Output

        EPSON_RD- Input

        HOST_TD- Output

ROS1 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

  • 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

  • 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 roslaunch/XML in the launch folder of this package is used to pass the init parameters to configure the IMU at runtime using roslaunch

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

Launch File

Comment

epson_g320_g354_g364.launch

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

epson_g325_g365.launch

launch file for G365 orientation field data is valid

epson_g325_g365_raw.launch

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

epson_g370.launch

launch file for G370 orientation field data should be ignored

epson_v340.launch

*legacy* launch file for V340 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

dout_rate

specifies the IMU output data rate in Hz

filter_sel

specifies the IMU filter setting

ext_sel

specifies the function of the GPIO2 ( GPIO2, External Trigger, External Counter Reset). External Counter Reset can be used with GPIO2 to connect to 3.3V compatible GNSS 1PPS signal for time synchronization purpose

ext_pol

specifies the polarity of the GPIO2 pin when External Trigger or External Counter Reset is selected

count_out

specifies to enable or disable counter in IMU output data (must be enabled when time_correction is enabled)

time_correction

enables time correction function using IMU counter reset function & external 1PPS connection to IMU GPIO2/EXT pin. (Must have ext_sel=1 (external counter reset). Attempts to time align the IMU message to the most resent 1 PPS pulse

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 ROS 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 ROS1 Node


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

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

<catkin_workspace>/src/ess_imu_ros1_uart_driver/

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

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

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

3. From the catkin workspace folder run "catkin_make" to build all changed ROS1 packages located in the <catkin_workspace>/src/ folder.

  • Re-run the above "catkin_make" 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

<catkin_workspace>/catkin_make

Example console output of catkin build for G370PDF1

user@user-VirtualBox:~/catkin_ws$ catkin_make
Base path: /home/user/catkin_ws
Source space: /home/user/catkin_ws/src
Build space: /home/user/catkin_ws/build
Devel space: /home/user/catkin_ws/devel
Install space: /home/user/catkin_ws/install
####
#### Running command: "cmake /home/user/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/user/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/user/catkin_ws/install -G Unix Makefiles" in "/home/user/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/user/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /home/user/catkin_ws/devel;/opt/ros/kinetic
-- This workspace overlays: /home/user/catkin_ws/devel;/opt/ros/kinetic
-- Using PYTHON_EXECUTABLE: /usr/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/user/catkin_ws/build/test_results
-- Found gmock sources under '/usr/src/gmock': gmock will be built
-- Found gtest sources under '/usr/src/gmock': gtests will be built
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.14
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~  traversing 1 packages in topological order:
-- ~~  - ess_imu_ros1_uart_driver
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'ess_imu_ros1_uart_driver'
-- ==> add_subdirectory(ess_imu_ros1_uart_driver)
-- Boost version: 1.58.0
-- Found the following Boost libraries:
--   system
---- Building for IMU Model: G370PDF1
-- Configuring done
-- Generating done
-- Build files have been written to: /home/user/catkin_ws/build
####
#### Running command: "make -j2 -l2" in "/home/user/catkin_ws/build"
####
Scanning dependencies of target ess_imu_ros1_uart_driver
[ 11%] Building C object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver.dir/src/hcl_gpio.c.o
[ 22%] Building C object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver.dir/src/hcl_linux.c.o
[ 44%] Building C object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver.dir/src/sensor_epsonCommon.c.o
[ 44%] Building C object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver.dir/src/hcl_uart.c.o
[ 55%] Building C object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver.dir/src/sensor_epsonUart.c.o
[ 66%] Building C object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver.dir/src/sensor_epsonG370.c.o
[ 77%] Linking C shared library /home/user/catkin_ws/devel/lib/libess_imu_ros1_uart_driver.so
[ 77%] Built target ess_imu_ros1_uart_driver
Scanning dependencies of target ess_imu_ros1_uart_driver_node
[ 88%] Building CXX object ess_imu_ros1_uart_driver/CMakeFiles/ess_imu_ros1_uart_driver_node.dir/src/epson_imu_uart_driver_node.cpp.o
[100%] Linking CXX executable /home/user/catkin_ws/devel/lib/ess_imu_ros1_uart_driver/ess_imu_ros1_uart_driver_node
[100%] Built target ess_imu_ros1_uart_driver_node
user@user-VirtualBox:~/catkin_ws$

4. Reload the current ROS1 environment variables that may have changed after the catkin build process.

<catkin_workspace>/source devel/setup.bash

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

  • Typically, only the dout_rate & filter_sel needs to be edited

Running the ROS1 Node


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

  • For example, for the Epson G370 IMU:

<catkin_workspace>/roslaunch ess_imu_ros1_uart_driver epson_g370.launch

Example console output of launching ROS1 node for G370PDF1

user@user-VirtualBox:~/catkin_ws$ roslaunch ess_imu_ros1_uart_driver epson_g370.launch
... logging to /home/user/.ros/log/86d27c9a-09bc-11eb-9ac8-080027cb8bd1/roslaunch-user-VirtualBox-4076.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://user-VirtualBox:35841/

SUMMARY
========

PARAMETERS
 * /ess_imu_ros1_uart_driver_node/accel_bit: 1
 * /ess_imu_ros1_uart_driver_node/accel_delta_bit: 1
 * /ess_imu_ros1_uart_driver_node/accel_delta_out: 0
 * /ess_imu_ros1_uart_driver_node/accel_out: 1
 * /ess_imu_ros1_uart_driver_node/atti_bit: 1
 * /ess_imu_ros1_uart_driver_node/atti_out: 0
 * /ess_imu_ros1_uart_driver_node/checksum_out: 1
 * /ess_imu_ros1_uart_driver_node/count_out: 1
 * /ess_imu_ros1_uart_driver_node/dout_rate: 9
 * /ess_imu_ros1_uart_driver_node/drdy_on: 0
 * /ess_imu_ros1_uart_driver_node/drdy_pol: 0
 * /ess_imu_ros1_uart_driver_node/ext_pol: 0
 * /ess_imu_ros1_uart_driver_node/ext_sel: 0
 * /ess_imu_ros1_uart_driver_node/filter_sel: 9
 * /ess_imu_ros1_uart_driver_node/flag_out: 1
 * /ess_imu_ros1_uart_driver_node/gpio_out: 0
 * /ess_imu_ros1_uart_driver_node/gyro_bit: 1
 * /ess_imu_ros1_uart_driver_node/gyro_delta_bit: 1
 * /ess_imu_ros1_uart_driver_node/gyro_delta_out: 0
 * /ess_imu_ros1_uart_driver_node/gyro_out: 1
 * /ess_imu_ros1_uart_driver_node/invert_xaccel: 0
 * /ess_imu_ros1_uart_driver_node/invert_xgyro: 0
 * /ess_imu_ros1_uart_driver_node/invert_yaccel: 0
 * /ess_imu_ros1_uart_driver_node/invert_ygyro: 0
 * /ess_imu_ros1_uart_driver_node/invert_zaccel: 0
 * /ess_imu_ros1_uart_driver_node/invert_zgyro: 0
 * /ess_imu_ros1_uart_driver_node/port: /dev/ttyUSB0
 * /ess_imu_ros1_uart_driver_node/temp_bit: 1
 * /ess_imu_ros1_uart_driver_node/temp_out: 1
 * /ess_imu_ros1_uart_driver_node/time_correction: 0
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    ess_imu_ros1_uart_driver_node (ess_imu_ros1_uart_driver/epson_imu_uart_driver_node)

auto-starting new master
process[master]: started with pid [4086]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 86d27c9a-09bc-11eb-9ac8-080027cb8bd1
process[rosout-1]: started with pid [4099]
started core service [/rosout]
process[ess_imu_ros1_uart_driver_node-2]: started with pid [4113]
[ INFO] [1602199089.860563057]: Initializing HCL layer...
[ INFO] [1602199089.860739911]: Initializing GPIO interface...
[ INFO] [1602199089.860788758]: Initializing UART interface...
Attempting to open port.../dev/ttyUSB0

...sensorDummyWrite.[ INFO] [1602199090.122510210]: Checking sensor NOT_READY status...
...done.[ INFO] [1602199091.016903361]: Initializing Sensor...
[ INFO] [1602199091.058212107]: Epson IMU initialized.
[ INFO] [1602199091.122874894]: PRODUCT ID:     G370PDF1
[ INFO] [1602199091.186292139]: SERIAL ID:      X0000002

...Sensor start.[ INFO] [1602199091.187993504]: Quaternion Output: Native.

USB-UART Latency


  • When using USB-UART bridge ICs to connect between the Linux host & Epson IMU, it is possible to experience higher than expected latencies or slower than expected IMU data rates

  • This could be caused by the USB-UART bridge device driver software which sets the default latency_timer too large (i.e. typically 16msec)
  • There are 2 recommended methods to reduce this value to 1msec to improve the latency & IMU publishing data rate

Modify latency_timer by sysfs mechanism

  • The example below reads the latency_timer setting for /dev/ttyUSB0 which returns 16msec
  • Then, the latency_timer is set to 1msec, and confirmed by readback.
  • NOTE: May require root (sudo su) access on your system to modify.

cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
16
echo 1 > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer
1

Modify low_latency flag using setserial utility

  • The example below sets the low_latency flag for /dev/ttyUSB0.
  • This will have the same effect as setting the latency_timer to 1msec.
  • This can be confirmed by running the setserial command again.
  • NOTE: You may need to install setserial with sudo apt install setserial

user@user-VirtualBox:~$ setserial /dev/ttyUSB0
/dev/ttyUSB0, UART: unknown, Port: 0x0000, IRQ: 0

user@user-VirtualBox:~$ setserial /dev/ttyUSB0 low_latency

user@user-VirtualBox:~$ setserial /dev/ttyUSB0
/dev/ttyUSB0, UART: unknown, Port: 0x0000, IRQ: 0, Flags: low_latency

Technical Support


  • Thank you for your interest and efforts in evaluating the Epson product

* For technical support or related issues using this ROS software with a supported Epson product, please email sensingsystem_support@ea.epson.com

Wiki: ess_imu_ros1_uart_driver (last edited 2022-08-03 16:49:59 by RChow)