Increasing ELLIPSE2-N ekf_euler output frequency to 1khz

Amir-Ramezani commented 5 years ago

Hi,

I am using your ROS package to receive data from ELLIPSE2-N. Currently I am able to receive ekf_euler at about 103hz after changing the config file. I actually don't need the GPS or BARO fusion, just need the Euler angles.

Is it possible to increase the frquency to 1khz?
At this moment, even though I set the frequency to be 200hz, but it is about 103 frames per second, is it because of serial baud rate? (how can I change this?)

Thanks in advance.

ErwanCL commented 4 years ago

Hey,

Sorry for late reply. Which version of the driver are you currently using (The release or devel branch one) ? Do you have other output defined or only the ekf_euler ?

Amir-Ramezani commented 4 years ago

Hi Erwan,

I am using the version that is installed using "apt-get install ros-kinetic(melodic)-sbg-driver" (http://wiki.ros.org/sbg_driver).

Basically, I need the angular velocities that I get by subscribing to imu_data topic using ROS, and attitude that I get by subscribing to ekf_euler topic.

I am receiving only 92 fps, even after increasing the serial port speed.

Also, the frames have delay (about milliseconds), I am not sure how much, but it reduces the performance of the whole system.

While making this repository, I receive the following error: catkin_ws/src/sbg_ros_driver/src/sbg_device.cpp:16:32: fatal error: version/sbgversion.h: No such file or directory

I might be able to solve the issue by changing the codes, but does this have an actual cause?

Thanks,

ErwanCL commented 4 years ago

So you're using the master version of the driver ? (2.0.0)
Can I see your configuration file ?
How do you measure your frequency ?

And for the error, someone told me about it as well, for some reasons, my cmake version didn't catch the typo error, it is corrected now !

Amir-Ramezani commented 4 years ago

Update: Compiling this current repository (git clone https://github.com/SBG-Systems/sbg_ros_driver.git), I can receive almost 200 hz data for 'ekf_euler' and 'imu_data'. Just wondering how it automatically goes to 200 hz even though I am using the default uart setting file which sets the output to be 25 hz.

Q: can I get the data in higher frequencies? 1khz?

Some samples taken from a Generic IMU and SBG ekf_euler: Even though the secs and nsecs for some of the SBG data frames are same, but the actual data differs, so I think it should be fine, perhaps using the repository code rather than the apt-get package fixes strange issues.

------- Generic IMU
---
header: 
  seq: 762
  stamp: 
    secs: 1578627538
    nsecs: 175805091
  frame_id: "base_imu_link"
orientation: 
  x: 0.0320664008698
  y: -0.0468940424451
  z: 0.477516090741
  w: 0.876784510471

---
header: 
  seq: 763
  stamp: 
    secs: 1578627538
    nsecs: 184643983
  frame_id: "base_imu_link"
orientation: 
  x: 0.0319107255403
  y: -0.0467798463896
  z: 0.477592349393
  w: 0.876754754403

---
header: 
  seq: 764
  stamp: 
    secs: 1578627538
    nsecs: 193411111
  frame_id: "base_imu_link"
orientation: 
  x: 0.032075971639
  y: -0.046688094218
  z: 0.477830022769
  w: 0.876624106013

---
header: 
  seq: 765
  stamp: 
    secs: 1578627538
    nsecs: 202183961
  frame_id: "base_imu_link"

---
header: 
  seq: 766
  stamp: 
    secs: 1578627538
    nsecs: 207772970
  frame_id: "base_imu_link"

---
header: 
  seq: 767
  stamp: 
    secs: 1578627538
    nsecs: 216649055
  frame_id: "base_imu_link"

---
header: 
  seq: 768
  stamp: 
    secs: 1578627538
    nsecs: 225564956
  frame_id: "base_imu_link"

---
header: 
  seq: 769
  stamp: 
    secs: 1578627538
    nsecs: 234414100
  frame_id: "base_imu_link"

---
header: 
  seq: 770
  stamp: 
    secs: 1578627538
    nsecs: 243158102
  frame_id: "base_imu_link"

------ SBG /sbg/ekf_euler

---
header: 
  seq: 91099
  stamp: 
    secs: 1578627610
    nsecs: 569262416
  frame_id: "System"
time_stamp: 1289650000
angle: 
  x: -0.0120013644919
  y: 0.0402518510818
  z: -2.37693500519

---
header: 
  seq: 91100
  stamp: 
    secs: 1578627610
    nsecs: 569262416
  frame_id: "System"
time_stamp: 1289655000
angle: 
  x: -0.011998327449
  y: 0.0402572825551
  z: -2.37693595886

---
header: 
  seq: 91101
  stamp: 
    secs: 1578627610
    nsecs: 589256906
  frame_id: "System"
time_stamp: 1289660000
angle: 
  x: -0.011999306269
  y: 0.0402539893985
  z: -2.37693405151

---
header: 
  seq: 91102
  stamp: 
    secs: 1578627610
    nsecs: 589256906
  frame_id: "System"
time_stamp: 1289665000

---
header: 
  seq: 91103
  stamp: 
    secs: 1578627610
    nsecs: 589256906
  frame_id: "System"
time_stamp: 1289670000

---
header: 
  seq: 91104
  stamp: 
    secs: 1578627610
    nsecs: 589256906
  frame_id: "System"
time_stamp: 1289675000

---
header: 
  seq: 91105
  stamp: 
    secs: 1578627610
    nsecs: 589256906
  frame_id: "System"
time_stamp: 1289680000

---
header: 
  seq: 91106
  stamp: 
    secs: 1578627610
    nsecs: 609280638
  frame_id: "System"
time_stamp: 1289685000

---

About the version, it seems to me 1.1.7? :~$ apt-show-versions ros-kinetic-sbg-driver

ros-kinetic-sbg-driver:amd64/xenial 1.1.7-0xenial-20191214-110155+0000 uptodate
ros-kinetic-sbg-driver:arm64 not installed
ros-kinetic-sbg-driver:i386 not installed

About the frequency: I measure it using rostopic hz 'topic name'

That being said, I am testing our system using other IMUs also, by looking at the results, I can see SBG is more accurate, but data arrive slower and with a delay (I said that by looking at the behaviour of the system and my experience with other IMUs).

This is my configuration file:

# Configuration file for SBG Ellipse
# YAML

# Ellipse-A - Magnetic-based
# Ellipse-E - External GNSS
# Ellipse-N - GNSS-based
# Ellipse-D - Dual-antenna GNSS

uartConf:
  # Port Name
  portName: "/dev/ttyUSB0"

  # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
  baudRate: 460800

  # Port Id
  # 0 PORT_A: Main communication interface. Full duplex.
  # 1 PORT_B: Auxiliary input interface for RTCM
  # 2 PORT_C: Auxiliary communication interface. Full duplex.
  # 3 PORT_D: Auxiliary input interface
  # 4 PORT_E: Auxiliary input/output interface
  portID: 0

# Sensor Parameters
sensorParameters:
  # Initial latitude (°)
  initLat: 48.419727
  # Initial longitude (°)
  initLong: -4.472119
  # Initial altitude (above WGS84 ellipsoid) (m)
  initAlt: 100.0
  # Year at startup
  year: 2018
  # month in year at startup
  month: 03
  # day in month at startup
  day: 10

  # Montion profile ID
  # 1 GENERAL_PURPOSE Should be used as a default when other profiles do not apply
  # 2 AUTOMOTIVE      Dedicated to car applications
  # 3 MARINE          Used in marine and underwater applications
  # 4 AIRPLANE        For fixed wings aircraft
  # 5 HELICOPTER      For rotary wing aircraft
  motionProfie: 1

# IMU_ALIGNMENT_LEVER_ARM
imuAlignementLeverArm:
  # IMU X axis direction in vehicle frame
  # 0 ALIGNMENT_FORWARD   IMU Axis is turned in vehicle's forward direction
  # 1 ALIGNMENT_BACKWARD  IMU Axis is turned in vehicle's backward direction
  # 2 ALIGNMENT_LEFT      IMU Axis is turned in vehicle's left direction
  # 3 ALIGNMENT_RIGHT     IMU Axis is turned in vehicle's right direction
  # 4 ALIGNMENT_UP        IMU Axis is turned in vehicle's up direction
  # 5 ALIGNMENT_DOWN      IMU Axis is turned in vehicle's down direction
  axisDirectionX: 0
  # IMU Y axis direction in vehicle frame
  # 0 ALIGNMENT_FORWARD   IMU Axis is turned in vehicle's forward direction
  # 1 ALIGNMENT_BACKWARD  IMU Axis is turned in vehicle's backward direction
  # 2 ALIGNMENT_LEFT      IMU Axis is turned in vehicle's left direction
  # 3 ALIGNMENT_RIGHT     IMU Axis is turned in vehicle's right direction
  # 4 ALIGNMENT_UP        IMU Axis is turned in vehicle's up direction
  # 5 ALIGNMENT_DOWN      IMU Axis is turned in vehicle's down direction
  axisDirectionY: 3
  # Residual roll error after axis alignment rad
  misRoll: 0
  # Residual pitch error after axis alignment rad
  misPitch: 0
  # Residual yaw error after axis alignment rad
  misYaw: 0
  # X Primary lever arm in IMU X axis (once IMU alignment is applied) m
  leverArmX: 0
  # Y Primary lever arm in IMU Y axis (once IMU alignment is applied) m
  leverArmY: 0
  # Z Primary lever arm in IMU Z axis (once IMU alignment is applied) m
  leverArmZ: 0

# AIDING_ASSIGNMENT
# Note: GNSS1 module configuration can only be set to an external port on Ellipse-E version.
# Ellipse-N users must set this module to MODULE_INTERNAL. On the other hand, rtcmModule is only
# available for Ellipse-N users. This module must be set to MODULE_DISABLED for other users.
aidingAssignment:
  # GNSS module port assignment:
  # 255 Module is disabled
  # 1 Module connected on PORT_B
  # 2 Module connected on PORT_C
  # 3 Module connected on PORT_D
  # 5 Module is connected internally
  gnss1ModulePortAssignment: 255
  # GNSS module sync assignment:
  # 0 Module is disabled
  # 1 Synchronization is done using SYNC_IN_A pin
  # 2 Synchronization is done using SYNC_IN_B pin
  # 3 Synchronization is done using SYNC_IN_C pin
  # 4 Synchronization is done using SYNC_IN_D pin
  # 5 Synchronization is internal
  # 6 Synchronization is done using SYNC_OUT_A pin
  # 7 Synchronization is done using SYNC_OUT_B pin
  gnss1ModuleSyncAssignment: 0
  # RTCM input port assignment for Ellipse-N DGPS
  rtcmPortAssignment: 255
  # Odometer module pin assignment
  # 0 Odometer is disabled
  # 1 Odometer connected only to ODO_A (unidirectional).
  # 2 Odometer connected to both ODO_A (signal A) and ODO_B (Signal B or direction) for bidirectional odometer.
  odometerPinAssignment: 0

magnetometer:
  # Magnetometer model ID
  # 201 Should be used in most applications
  # 202 Should be used in disturbed magnetic environment
  magnetometerModel: 201
  # Magnetometer rejection mode
  # 0 Measurement is not taken into account
  # 1 Measurement is rejected if inconsistent with current estimate (depending on error model)
  # 2 Measurement is always accepted
  magnetometerRejectMode: 1

  # Theses parameters are only used for a calibration run
  calibration:
    # 1 2D Tell the device that the magnetic calibration will be performed with limited motions. 
    #     This calibration mode is only designed to be used when roll and pitch motions are less than ± 5°. 
    #     To work correctly, the device should be rotated through at least a full circle.
    # 2 3D Tell the device to start a full 3D magnetic calibration procedure. The 3D magnetic calibration offers the best accuracy
    mode: 2

    # 0 LOW_BW Use this parameter in case of strong magnetic noise during calibration. 
    #     Motion during calibration is then limited to slow rotations.
    # 1 MEDIUM_BW Tell the device that medium dynamics will be observed during the magnetic calibration process. 
    #     It can be used in case of medium magnetic noise during calibration process. Medium dynamics are used during calibration.
    # 2 HIGH_BW This parameter is suitable to most applications. It can be used when the dynamics during calibration are relatively high.
    bandwidth: 2

# GNSS configuration
# Note: Pitch and Yaw offsets as well as antenna distance parameters should only be considered in
# case of dual antenna GNSS receiver. It can be left to 0 otherwise.
gnss:
  # Gnss Model Id
  # 101 Used on Ellipse-N to setup the internal GNSS in GPS+GLONASS
  # 102 Default mode for Ellipse-E connection to external GNSS
  # 103 Used on Ellipse-N to setup the internal GNSS in GPS+BEIDOU
  # 104 Used on Ellipse-E to setup a connection to ublox in read only mode.
  # 106 Used on Ellipse-E to setup a connection to Novatel receiver in read only mode.
  # 107 Used on Ellipse-D by default
  gnss_model_id: 102

  #GNSS antenna lever arm in IMU X axis (m)
  leverArmX: 0
  #GNSS antenna lever arm in IMU Y axis (m)
  leverArmY: 0
  #GNSS antenna lever arm in IMU Z axis (m)
  leverArmZ: 0
  #Pitch offset for dual antenna GNSS (rad)
  pitchOffset: 0
  #Yaw offset for dual antenna GNSS (rad)
  yawOffset: 0
  #Distance between two GNSS antennas (m)
  antennaDistance: 0

  # Rejection mode for position
  # 0 Measurement is not taken into account
  # 1 Measurement is rejected if inconsistent with current estimate (depending on error model)
  # 2 Measurement is always accepted
  posRejectMode: 1
  # Rejection mode for velocity (see posRejectMode values)
  velRejectMode: 1
  # Rejection mode for true heading (see posRejectMode values)
  hdtRejectMode: 1

# Odometer configuration
odom:
  # Odometer's gain Pulses/m
  gain: 4800 
  # User gain average error (%)
  gain_error: 0.1 
  # Odometer's direction
  # 0: positive
  # 1: negative
  direction: 0

  # Odometer lever arm in IMU X axis (m)
  leverArmX: 0
  # Odometer lever arm in IMU Y axis (m)
  leverArmY: 0
  # Odometer lever arm in IMU Z axis (m)
  leverArmZ: 0

  # Odometer rejection mode
  # 0 Measurement is not taken into account
  # 1 Measurement is rejected if inconsistent with current estimate (depending on error model)
  # 2 Measurement is always accepted
  rejectMode: 1

# ToDo: event & CAN configuration

############################### Output configuration ###############################
# 0 Output is disabled
# 1 Output is generated at 200Hz
# 2 Output is generated at 100Hz
# 4 Output is generated at 50Hz
# 8 Output is generated at 25Hz
# 10 Output is generated at 20Hz
# 20 Output is generated at 10Hz
# 40 Output is generated at 5Hz
# 200 Output is generated at 1Hz
# 10000 Pulse Per Second. Same mode as above.
# 10001 Output sent when a new data is available.
# 10002 Output is generated when a new virtual odometer event occurs
# 10003 Output is generated on a Sync In A event
# 10004 Output is generated on a Sync In B event
# 10005 Output is generated on a Sync In C event
# 10006 Output is generated on a Sync In D event
output:
  # Time Reference
  # 0 No external time reference is used. Internal clock is used instead.
  # 1 The system will be synchronized on the clock input observed at SYNC_IN_A pin.
  # 2 The system will be synchronized GPS PPS signal, (see GPS module assignment)
  timeReference: 0

  # Status general, clock, com aiding, solution, heave
  log_status: 1
  # Includes IMU status, acc., gyro, temp delta speeds and delta angles values
  log_imu_data: 1
  # Includes roll, pitch, yaw and their accuracies on each axis
  log_ekf_euler: 1
  # Includes the 4 quaternions values
  log_ekf_quat: 0
  # Position and velocities in NED coordinates with the accuracies on each axis
  log_ekf_nav: 0
  # Heave, surge and sway and accelerations on each axis for up to 4 points
  log_ship_motion: 1
  # Provides UTC time reference
  log_utc_time: 1
  # Magnetic data with associated accelerometer on each axis
  log_mag: 1
  # Magnetometer calibration data (raw buffer)
  log_mag_calib: 0
  # GPS velocities from primary or secondary GPS receiver
  log_gps1_vel: 0
  # GPS positions from primary or secondary GPS receiver
  log_gps1_pos: 0
  # GPS true heading from dual antenna system
  log_gps1_hdt: 0
  # GPS 1 raw data for post processing.
  log_gps1_raw: 0
  # Provides odometer velocity
  log_odo_vel: 0
  # Event A/B/C/D Event markers sent when events are detected on a sync in pin
  log_event_a: 0
  log_event_b: 0
  log_event_c: 0
  log_event_d: 0
  # Barometric altimeter output
  log_pressure: 0

  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
  frequency: 200

Yes, I also changed it to capital V!

Thanks,

ErwanCL commented 4 years ago

Huges changes have been made on the current repository version (2.0.0) regarding the message handling, so I recommend you to use this version (which should be available via apt-get soon).

Just wondering how it automatically goes to 200 hz even though I am using the default uart setting file which sets the output to be 25 hz.

If you didn't reconfigure your device, even if the settings file is set to 25Hz, your Ellipse is currently sending data at 200Hz as your last configuration. A parameter is available in the config file to configure again your device with the driver.

Amir-Ramezani commented 4 years ago

Okay, great!

Can I get data higher than 200 frames per second?

ErwanCL commented 4 years ago

It is not possible to configure sbg output at more than 200Hz.

SBG-Systems / sbg_ros_driver

Increasing ELLIPSE2-N ekf_euler output frequency to 1khz #26