search

SBG-Systems / sbg_ros_driver

ROS 1 driver for SBG Systems IMU/AHRS/INS units such as ELLIPSE or QUANTA.
https://www.sbg-systems.com
MIT License
75 stars 43 forks source link

Why the change of frequency wont work #37

Closed YushengWHU closed 3 years ago

YushengWHU commented 3 years ago

Hello here, I want a 200Hz IMU output and better higher GPS pos output with my ellipse 2N. Unfortunately, no matter how I change the frequency with respect to description, the ros standard topic output frequency seems not changed at all. Which is to say, I want the /imu/data to be at 200Hz, and /imu/nav_sat_fix better higher. In my case, the maximum /imu/data is 50Hz, and /imu/nav_sat_fix is 5Hz. Below is my yaml setting

 timeReference: 0
  ros_standard: true

  # Status general, clock, com aiding, solution, heave
  log_status: 8
  # 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: 8
  # Position and velocities in NED coordinates with the accuracies on each axis
  log_ekf_nav: 8
  # Heave, surge and sway and accelerations on each axis for up to 4 points
  log_ship_motion: 8
  # Provides UTC time reference
  log_utc_time: 8
  # Magnetic data with associated accelerometer on each axis
  log_mag: 8
  # Magnetometer calibration data (raw buffer)
  log_mag_calib: 0
  # GPS velocities from primary or secondary GPS receiver
  log_gps1_vel: 10001
  # GPS positions from primary or secondary GPS receiver
  log_gps1_pos: 10001
  # GPS true heading from dual antenna system
  log_gps1_hdt: 10001
  # GPS 1 raw data for post processing.
  log_gps1_raw: 10001
  # 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
  # Air data
  log_air_data: 0
  # Short IMU data
  log_imu_short: 0

  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
  frequency: 0
YushengWHU commented 3 years ago

Besides, the hardware and firmware version is shown below

[ INFO] [1604924437.331166299]: SBG_DRIVER - productCode = ELLIPSE2-N-G5A4-B1
[ INFO] [1604924437.331196295]: SBG_DRIVER - serialNumber = 45001165
[ INFO] [1604924437.331216171]: SBG_DRIVER - calibationRev = 1.1.0.0
[ INFO] [1604924437.331233361]: SBG_DRIVER - calibrationDate = 0 / 0 / 2000
[ INFO] [1604924437.331254505]: SBG_DRIVER - hardwareRev = 1.1.0.0
[ INFO] [1604924437.331270259]: SBG_DRIVER - firmwareRev = 1.3.97-stable
rsiryani commented 3 years ago

Hi Yusheng, Could you please confirm the serial baud rate first ? Secondly, you can try to setup the unit using the sbgCenter and then just use the driver in no conf mode: confWithRos: false

Please let me know if it fixes your issue so we can further dig if it comes from the ROS configuration code.

YushengWHU commented 3 years ago

Thanks for the quick response

Could you please confirm the serial baud rate first ?

I have done that before

try to setup the unit using the sbgCenter and then just use the driver in no conf mode

Also tried, but the output frequency cant reach 200Hz

I found that may be the reason of reaching transmission limit? Cause I noticed a increasing frequency of 165Hz now, but cant get to 200Hz, below is my setting.

 timeReference: 0
  ros_standard: true

  # Status general, clock, com aiding, solution, heave
  log_status: 8
  # 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: 1
  # Position and velocities in NED coordinates with the accuracies on each axis
  log_ekf_nav: 1
  # Heave, surge and sway and accelerations on each axis for up to 4 points
  log_ship_motion: 0
  # Provides UTC time reference
  log_utc_time: 1
  # Magnetic data with associated accelerometer on each axis
  log_mag: 0
  # Magnetometer calibration data (raw buffer)
  log_mag_calib: 0
  # GPS velocities from primary or secondary GPS receiver
  log_gps1_vel: 10001
  # GPS positions from primary or secondary GPS receiver
  log_gps1_pos: 10001
  # GPS true heading from dual antenna system
  log_gps1_hdt: 10001
  # GPS 1 raw data for post processing.
  log_gps1_raw: 10001
  # 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
  # Air data
  log_air_data: 0
  # Short IMU data
  log_imu_short: 0

  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
  frequency: 0
YushengWHU commented 3 years ago

I am just a little confused now, is that the reason my port cant hold that much data or internal calculation needs space for initialization, cause I noticed a step by step output frequency increasing now. Below is from my terminal:

dji@dji-MANIFOLD-2-C:~$ rostopic hz /imu/data
subscribed to [/imu/data]
average rate: 162.156
    min: 0.000s max: 0.013s std dev: 0.00521s window: 158
average rate: 166.427
    min: 0.000s max: 0.013s std dev: 0.00522s window: 329
average rate: 170.790
    min: 0.000s max: 0.013s std dev: 0.00522s window: 510
average rate: 173.199
    min: 0.000s max: 0.013s std dev: 0.00522s window: 690
average rate: 174.758
    min: 0.000s max: 0.013s std dev: 0.00522s window: 871
average rate: 175.644
    min: 0.000s max: 0.013s std dev: 0.00522s window: 1051
average rate: 176.291
    min: 0.000s max: 0.013s std dev: 0.00522s window: 1233
average rate: 177.007
    min: 0.000s max: 0.013s std dev: 0.00522s window: 1415
average rate: 177.118
    min: 0.000s max: 0.013s std dev: 0.00522s window: 1593
average rate: 177.351
    min: 0.000s max: 0.013s std dev: 0.00522s window: 1772
average rate: 177.431
    min: 0.000s max: 0.013s std dev: 0.00522s window: 1952
average rate: 177.559
    min: 0.000s max: 0.013s std dev: 0.00522s window: 2131
average rate: 177.673
    min: 0.000s max: 0.013s std dev: 0.00522s window: 2310
average rate: 177.643
    min: 0.000s max: 0.013s std dev: 0.00522s window: 2489
average rate: 177.799
    min: 0.000s max: 0.020s std dev: 0.00523s window: 2669
average rate: 177.871
    min: 0.000s max: 0.020s std dev: 0.00523s window: 2848
average rate: 177.900
    min: 0.000s max: 0.020s std dev: 0.00523s window: 3028
average rate: 178.039
    min: 0.000s max: 0.020s std dev: 0.00523s window: 3208
average rate: 178.247
    min: 0.000s max: 0.020s std dev: 0.00522s window: 3390
average rate: 178.344
    min: 0.000s max: 0.020s std dev: 0.00522s window: 3572
average rate: 178.518
    min: 0.000s max: 0.020s std dev: 0.00522s window: 3754
average rate: 178.642
    min: 0.000s max: 0.020s std dev: 0.00522s window: 3937
average rate: 178.614
    min: 0.000s max: 0.020s std dev: 0.00522s window: 4115
average rate: 178.555
    min: 0.000s max: 0.020s std dev: 0.00522s window: 4294
average rate: 178.614
    min: 0.000s max: 0.020s std dev: 0.00522s window: 4474
average rate: 178.681
    min: 0.000s max: 0.020s std dev: 0.00522s window: 4654
average rate: 178.650
    min: 0.000s max: 0.020s std dev: 0.00522s window: 4834
average rate: 178.606
    min: 0.000s max: 0.022s std dev: 0.00523s window: 5011
average rate: 178.629
    min: 0.000s max: 0.022s std dev: 0.00523s window: 5192
average rate: 178.680
    min: 0.000s max: 0.022s std dev: 0.00523s window: 5374
average rate: 178.659
    min: 0.000s max: 0.022s std dev: 0.00523s window: 5552
average rate: 178.705
    min: 0.000s max: 0.022s std dev: 0.00523s window: 5734
average rate: 178.745
    min: 0.000s max: 0.022s std dev: 0.00523s window: 5914
average rate: 178.792
    min: 0.000s max: 0.022s std dev: 0.00523s window: 6096
average rate: 178.796
    min: 0.000s max: 0.022s std dev: 0.00523s window: 6275
average rate: 178.805
    min: 0.000s max: 0.022s std dev: 0.00523s window: 6456
average rate: 178.812
    min: 0.000s max: 0.022s std dev: 0.00523s window: 6635
average rate: 178.821
    min: 0.000s max: 0.022s std dev: 0.00523s window: 6816
average rate: 178.832
    min: 0.000s max: 0.022s std dev: 0.00523s window: 6997
average rate: 178.811
    min: 0.000s max: 0.022s std dev: 0.00523s window: 7175
average rate: 178.725
    min: 0.000s max: 0.022s std dev: 0.00523s window: 7352
average rate: 178.764
    min: 0.000s max: 0.022s std dev: 0.00523s window: 7532
average rate: 178.774
    min: 0.000s max: 0.022s std dev: 0.00523s window: 7713
average rate: 178.819
    min: 0.000s max: 0.022s std dev: 0.00523s window: 7896
average rate: 178.807
    min: 0.000s max: 0.022s std dev: 0.00523s window: 8076
average rate: 178.821
    min: 0.000s max: 0.022s std dev: 0.00523s window: 8255
average rate: 178.828
    min: 0.000s max: 0.022s std dev: 0.00523s window: 8436
average rate: 178.836
    min: 0.000s max: 0.022s std dev: 0.00523s window: 8617
average rate: 178.793
    min: 0.000s max: 0.022s std dev: 0.00523s window: 8796
average rate: 178.866
    min: 0.000s max: 0.022s std dev: 0.00523s window: 8978
average rate: 178.854
    min: 0.000s max: 0.022s std dev: 0.00523s window: 9158
average rate: 178.861
    min: 0.000s max: 0.022s std dev: 0.00523s window: 9339
average rate: 178.906
    min: 0.000s max: 0.022s std dev: 0.00523s window: 9522
average rate: 178.847
    min: 0.000s max: 0.022s std dev: 0.00523s window: 9700
average rate: 178.813
    min: 0.000s max: 0.022s std dev: 0.00523s window: 9877
average rate: 178.892
    min: 0.000s max: 0.022s std dev: 0.00523s window: 10062
average rate: 178.897
    min: 0.000s max: 0.022s std dev: 0.00523s window: 10243
YushengWHU commented 3 years ago

I also recorded a bag shown below, which denotes the IMU frequency is around 179Hz

dji@dji-MANIFOLD-2-C:~$ rosbag info 2020-11-09-22-06-53.bag 
path:        2020-11-09-22-06-53.bag
version:     2.0
duration:    1:05s (65s)
start:       Nov 09 2020 22:06:54.19 (1604930814.19)
end:         Nov 09 2020 22:07:59.30 (1604930879.30)
size:        11.0 MB
messages:    66640
compression: none [14/14 chunks]
types:       geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
             geometry_msgs/TwistStamped [98d34b0043a2093cf9d9345ab6eef12e]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/NavSatFix      [2d3a8cd499b9b4a0249fb98fd05cfa48]
             sensor_msgs/TimeReference  [fded64a0265108ba86c3d38fb11c0c16]
topics:      /imu/data          11670 msgs    : sensor_msgs/Imu           
             /imu/nav_sat_fix      14 msgs    : sensor_msgs/NavSatFix     
             /imu/pos_ecef       7885 msgs    : geometry_msgs/PointStamped
             /imu/utc_ref        1617 msgs    : sensor_msgs/TimeReference 
             /imu/velocity      45454 msgs    : geometry_msgs/TwistStamped
rsiryani commented 3 years ago

Hi,

The unit could not output at 179 Hz so it's clear you have a saturation on the serial port. According to the data you would like to output I guess a baudrate of 46800 bps or 921600 bps is required.

You can check if there are saturations on the Port A by checking the status bit in the SBG_ECOM_LOG_STATUS or using the sbgCenter directly (interfaces tab in the device status panel).

If you could just give it a try at 921600 bps it should be fine but make sure y our hardware is capable of achieving this speed and also make sure your ROS platform is correctly setup.

We have validated 921600 bps continuous operations with no issues on ROS platforms but some HAL are not always correctly setup so you can miss some bytes at high speed.

If it's the case you will see gaps in the data and this can be checked easily using the IMU returned time stamp value.

Please let me know if a higher baudrate fixes your issue.

Regards,

YushengWHU commented 3 years ago

921600 solved the problem, thanks!

learnhard22 commented 2 years ago

Hey guys,

I'am facing the same problem as @YushengWHU but in my system it is not even possible to nearly achieve the area of 200Hz or even 165Hz. I modified the yaml file the exact same way that you did and I changed the baud rate to 921600 in the yaml file and the terminal, respectively. Everytime when I'm trying to build I get an error and the process is shutting down.

This is the output I received when launching the ellipseD.

NODES / sbg_ellipseD (sbg_driver/sbg_device)

ROS_MASTER_URI=http://localhost:11...

process[sbg_ellipseD-1]: started with pid [278694] [ INFO] [1661358191.500712130]: SBG DRIVER - Init node, load params and connect to the device. [ERROR] [1661358193.091465839]: Unable to get the device Info : SBG_TIME_OUT [ INFO] [1661358193.091700007]: SBG DRIVER - Initialize device for receiving data [ WARN] [1661358193.113370597]: SBG_DRIVER - [Publisher] SBG Mag data output are not configured, the standard Magnetic publisher can not be defined. [ WARN] [1661358193.116504582]: SBG_DRIVER - [Publisher] SBG AirData output are not configured, the standard FluidPressure publisher can not be defined. [ERROR] [1661358194.653739402]: SBG_DRIVER - [Config] Unable to get the Init conditions configuration : SBG_TIME_OUT [sbg_ellipseD-1] process has finished cleanly log file: /home/stapler1/.ros/log/d6000b94-239a-11ed-9cca-cd7dd0b9b063/sbg_ellipseD-1*.log all processes on machine have died, roslaunch will exit shutting down processing monitor... ... shutting down processing monitor complete done

The settings I did in ellipse_D_default.yaml file:

baud rate:

portName: "/dev/ttyUSB0"

Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600) #set with: stty speed 921600 < /dev/ttyUSB0 > /dev/ttyUSB0 #check it with: stty -a < /dev/ttyUSB0 baudRate: 921600 . . . # Status general, clock, com aiding, solution, heave log_status: 8 # 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: 1 # Position and velocities in NED coordinates with the accuracies on each axis log_ekf_nav: 1 # Heave, surge and sway and accelerations on each axis for up to 4 points log_ship_motion: 0 # Provides UTC time reference log_utc_time: 1 # Magnetic data with associated accelerometer on each axis log_mag: 0 # Magnetometer calibration data (raw buffer) log_mag_calib: 0 # GPS velocities from primary or secondary GPS receiver log_gps1_vel: 10001 # GPS positions from primary or secondary GPS receiver log_gps1_pos: 10001 # GPS true heading from dual antenna system log_gps1_hdt: 10001 # GPS 1 raw data for post processing. log_gps1_raw: 10001 # 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 # Air data log_air_data: 0 # Short IMU data log_imu_short: 0

Before I was changing the settings I received at max. 80Hz with baud rate of 115200. Do you have any advices how I can solve this problem? If you need some more information please let me know, I'm very new into that all.

Thank you in advantage.

Best regards, Lennard

hoangvietdo commented 1 year ago

@learnhard22 Have you solved the problem? I got the same issue.

learnhard22 commented 1 year ago

Hey @hoangvietdo, I tried to find some helpful answers in my documentations from last year and this is all I got: I did the following changes in my ellipse_D_default.yaml file:

• baudRate: 921600 • log_imu_data: 1 • log_ekf_euler: 1 • log_ekf_quat: 1 • log_ekf_nav: 1

But before I think it was necessary to ensure that your interface will not be saturated by the data so if I remember right I did a manual change of the interface's baudrate to 921600. However, I am not 100% sure, because it's been a while since I have dealt with this topic.

hoangvietdo commented 1 year ago

Hi, @learnhard22.

Thanks for your effort.

For some reason, I managed to obtain 100 - 130 Hz IMU data (I checked it using rostopic hz command). However, when I checked the data quality using rqt_bag or Matlab after making a trial record, it seemed the data was only generated at 25 Hz. It is so weird.

I hope you can give me some advice! Thank you so much!

learnhard22 commented 1 year ago

Hi @hoangvietdo

I am sorry, I am at a loss. I hope someone else can help you quickly. Good luck!