Epson IMU ROS1 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 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 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 ROS system using the SPI interface:
  • This ROS 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

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 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

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 "epson_imu_spi_ros"

<catkin_workspace>/src/epson_imu_spi_ros/ <-- place files here

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

  • For example to set the build for G365PDF0:

set(imu_model "G365PDF0")

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 G365PDF0

guest@guest-desktop:~/catkin_ws$ catkin_make
Base path: /home/guest/catkin_ws
Source space: /home/guest/catkin_ws/src
Build space: /home/guest/catkin_ws/build
Devel space: /home/guest/catkin_ws/devel
Install space: /home/guest/catkin_ws/install
####
#### Running command: "cmake /home/guest/catkin_ws/src -DCATKIN_DEVEL_PREFIX=/home/guest/catkin_ws/devel -DCMAKE_INSTALL_PREFIX=/home/guest/catkin_ws/install -G Unix Makefiles" in "/home/guest/catkin_ws/build"
####
-- Using CATKIN_DEVEL_PREFIX: /home/guest/catkin_ws/devel
-- Using CMAKE_PREFIX_PATH: /home/guest/catkin_ws/devel;/opt/ros/melodic
-- This workspace overlays: /home/guest/catkin_ws/devel;/opt/ros/melodic
-- Found PythonInterp: /usr/bin/python2 (found suitable version "2.7.17", minimum required is "2") 
-- Using PYTHON_EXECUTABLE: /usr/bin/python2
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Call enable_testing()
-- Using CATKIN_TEST_RESULTS_DIR: /home/guest/catkin_ws/build/test_results
-- Found gtest sources under '/usr/src/googletest': gtests will be built
-- Found gmock sources under '/usr/src/googletest': gmock will be built
-- Found PythonInterp: /usr/bin/python2 (found version "2.7.17") 
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.29
-- BUILD_SHARED_LIBS is on
-- BUILD_SHARED_LIBS is on
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- ~~  traversing 1 packages in topological order:
-- ~~  - epson_imu_spi_driver
-- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- +++ processing catkin package: 'epson_imu_spi_driver'
-- ==> add_subdirectory(imu_ros_spi)
-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   system
---- Building for IMU Model: G365PDF0
---- Building for platform: RPI
---- Building for interface: SPI
-- Configuring done
-- Generating done
-- Build files have been written to: /home/guest/catkin_ws/build
####
#### Running command: "make -j4 -l4" in "/home/guest/catkin_ws/build"
####
Scanning dependencies of target epson_imu_spi_driver_lib
[ 22%] Building C object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_lib.dir/src/sensor_epsonCommon.c.o
[ 22%] Building C object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_lib.dir/src/hcl_gpio_rpi.c.o
[ 33%] Building C object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_lib.dir/src/hcl_rpi.c.o
[ 44%] Building C object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_lib.dir/src/hcl_spi_rpi.c.o
[ 66%] Building C object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_lib.dir/src/sensor_epsonG365.c.o
[ 66%] Building C object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_lib.dir/src/sensor_epsonSpi.c.o
[ 77%] Linking C shared library /home/guest/catkin_ws/devel/lib/libepson_imu_spi_driver_lib.so
[ 77%] Built target epson_imu_spi_driver_lib
Scanning dependencies of target epson_imu_spi_driver_node
[ 88%] Building CXX object imu_ros_spi/CMakeFiles/epson_imu_spi_driver_node.dir/src/epson_imu_spi_driver_node.cpp.o
In file included from /usr/include/boost/bind.hpp:22:0,
                 from /opt/ros/melodic/include/ros/publisher.h:35,
                 from /opt/ros/melodic/include/ros/node_handle.h:32,
                 from /opt/ros/melodic/include/ros/ros.h:45,
                 from /home/guest/catkin_ws/src/imu_ros_spi/src/epson_imu_spi_driver_node.cpp:61:
/usr/include/boost/bind/bind_cc.hpp: In function ‘boost::_bi::bind_t<R, R (*)(B1), typename boost::_bi::list_av_1<A1>::type> boost::bind(R (*)(B1), A1) [with R = ros::SerializedMessage; B1 = const sensor_msgs::Imu_<std::allocator<void> >&; A1 = boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >]’:
/usr/include/boost/bind/bind_cc.hpp:26:5: note: parameter passing for argument of type ‘boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >’ changed in GCC 7.1
     BOOST_BIND(BOOST_BIND_ST R (BOOST_BIND_CC *f) (B1), A1 a1)
     ^
In file included from /usr/include/boost/bind/bind.hpp:2126:0,
                 from /usr/include/boost/bind.hpp:22,
                 from /opt/ros/melodic/include/ros/publisher.h:35,
                 from /opt/ros/melodic/include/ros/node_handle.h:32,
                 from /opt/ros/melodic/include/ros/ros.h:45,
                 from /home/guest/catkin_ws/src/imu_ros_spi/src/epson_imu_spi_driver_node.cpp:61:
/usr/include/boost/bind/bind_cc.hpp:30:58: note: parameter passing for argument of type ‘boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >’ changed in GCC 7.1
     return _bi::bind_t<R, F, list_type> (f, list_type(a1));
                                                          ^
In file included from /usr/include/boost/bind.hpp:22:0,
                 from /opt/ros/melodic/include/ros/publisher.h:35,
                 from /opt/ros/melodic/include/ros/node_handle.h:32,
                 from /opt/ros/melodic/include/ros/ros.h:45,
                 from /home/guest/catkin_ws/src/imu_ros_spi/src/epson_imu_spi_driver_node.cpp:61:
/usr/include/boost/bind/bind.hpp: In constructor ‘boost::_bi::list1<A1>::list1(A1) [with A1 = boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >]’:
/usr/include/boost/bind/bind.hpp:231:14: note: parameter passing for argument of type ‘boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >’ changed in GCC 7.1
     explicit list1( A1 a1 ): base_type( a1 ) {}
              ^~~~~
/usr/include/boost/bind/bind.hpp:231:44: note: parameter passing for argument of type ‘boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >’ changed in GCC 7.1
     explicit list1( A1 a1 ): base_type( a1 ) {}
                                            ^
In file included from /usr/include/boost/bind/bind.hpp:47:0,
                 from /usr/include/boost/bind.hpp:22,
                 from /opt/ros/melodic/include/ros/publisher.h:35,
                 from /opt/ros/melodic/include/ros/node_handle.h:32,
                 from /opt/ros/melodic/include/ros/ros.h:45,
                 from /home/guest/catkin_ws/src/imu_ros_spi/src/epson_imu_spi_driver_node.cpp:61:
/usr/include/boost/bind/storage.hpp: In constructor ‘boost::_bi::storage1<A1>::storage1(A1) [with A1 = boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >]’:
/usr/include/boost/bind/storage.hpp:42:14: note: parameter passing for argument of type ‘boost::reference_wrapper<const sensor_msgs::Imu_<std::allocator<void> > >’ changed in GCC 7.1
     explicit storage1( A1 a1 ): a1_( a1 ) {}
              ^~~~~~~~
[100%] Linking CXX executable /home/guest/catkin_ws/devel/lib/epson_imu_spi_driver/epson_imu_spi_driver_node
[100%] Built target epson_imu_spi_driver_node

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 G365 IMU:

<catkin_workspace>/roslaunch epson_imu_spi_driver epson_g325_g365.launch

Example console output of launching ROS1 node for G365PDF0

guest@guest-desktop:~/catkin_ws$ roslaunch epson_imu_spi_driver epson_g325_g365.launch
... logging to /home/guest/.ros/log/4a409a9a-3126-11ec-a5ca-b827eba23167/roslaunch-guest-desktop-3305.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://guest-desktop:45909/

SUMMARY
========

PARAMETERS
 * /epson_imu_spi_driver_node/accel_bit: 1
 * /epson_imu_spi_driver_node/accel_delta_bit: 1
 * /epson_imu_spi_driver_node/accel_delta_out: 0
 * /epson_imu_spi_driver_node/accel_out: 1
 * /epson_imu_spi_driver_node/atti_bit: 1
 * /epson_imu_spi_driver_node/atti_conv: 0
 * /epson_imu_spi_driver_node/atti_mode: 1
 * /epson_imu_spi_driver_node/atti_out: 0
 * /epson_imu_spi_driver_node/atti_profile: 0
 * /epson_imu_spi_driver_node/checksum_out: 1
 * /epson_imu_spi_driver_node/count_out: 1
 * /epson_imu_spi_driver_node/dout_rate: 4
 * /epson_imu_spi_driver_node/drdy_on: 1
 * /epson_imu_spi_driver_node/drdy_pol: 1
 * /epson_imu_spi_driver_node/ext_pol: 0
 * /epson_imu_spi_driver_node/ext_sel: 1
 * /epson_imu_spi_driver_node/filter_sel: 5
 * /epson_imu_spi_driver_node/flag_out: 1
 * /epson_imu_spi_driver_node/gpio_out: 0
 * /epson_imu_spi_driver_node/gyro_bit: 1
 * /epson_imu_spi_driver_node/gyro_delta_bit: 1
 * /epson_imu_spi_driver_node/gyro_delta_out: 0
 * /epson_imu_spi_driver_node/gyro_out: 1
 * /epson_imu_spi_driver_node/invert_xaccel: 0
 * /epson_imu_spi_driver_node/invert_xgyro: 0
 * /epson_imu_spi_driver_node/invert_yaccel: 0
 * /epson_imu_spi_driver_node/invert_ygyro: 0
 * /epson_imu_spi_driver_node/invert_zaccel: 0
 * /epson_imu_spi_driver_node/invert_zgyro: 0
 * /epson_imu_spi_driver_node/qtn_bit: 1
 * /epson_imu_spi_driver_node/qtn_out: 1
 * /epson_imu_spi_driver_node/temp_bit: 1
 * /epson_imu_spi_driver_node/temp_out: 1
 * /epson_imu_spi_driver_node/time_correction: 0
 * /rosdistro: melodic
 * /rosversion: 1.14.10

NODES
  /
    epson_imu_spi_driver_node (epson_imu_spi_driver/epson_imu_spi_driver_node)

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

setting /run_id to 4a409a9a-3126-11ec-a5ca-b827eba23167
process[rosout-1]: started with pid [3327]
started core service [/rosout]
process[epson_imu_spi_driver_node-2]: started with pid [3330]
[ INFO] [1634680111.742910626]: Initializing HCL layer...

Initializing libraries......done.[ INFO] [1634680111.748499390]: Initializing GPIO interface...
[ INFO] [1634680111.748729753]: Initializing SPI interface...

...sensorDummyWrite.[ INFO] [1634680111.949504054]: Checking sensor NOT_READY status...
...done.[ INFO] [1634680112.773726640]: Initializing Sensor...
[ INFO] [1634680112.779993732]: Epson IMU initialized.
[ INFO] [1634680112.781463149]: PRODUCT ID:     G365PDF0
[ INFO] [1634680112.783040482]: SERIAL ID:      00000025

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

Technical Support


Wiki: ess_sensors/ros1spi (last edited 2022-02-22 22:00:45 by RChow)