Gazebo Ignition recorded ros2 bag drift randomly

[BabeJoan]

Hello Community,

I am using Gazebo ignition simulator to test LIO-SAM ros2 branch now. The problem I met is, LIO-SAM runs well with simulation but when I recorded ros bag from it and play, LIO-SAM performs quite randomly. Usually it starts well but then the drift happens.

I built a simple robot and the lidar and imu both using the gazebo ignition plugins, so the extrinsic Matrix should be right. I have several guess of the possible reasons:

1. Frame transformation: I have different frame transformation from LIO-SAM's robot, the error logs are also always around 'link transformation'. But there are only imu/data and lidarpoints topics in the rosbag, so it might not be the reason.

2. Extrinsic Matrix: I'm using unit matrix for both ExtrinsicROT and ExtrinsicRPY. I built a simple robot and the lidar and imu both using the gazebo ignition plugins, so the extrinsic Matrix should be right.
   The plugin I used for imu is <plugin filename="ignition-gazebo-imu-system" name="ignition::gazebo::systems::Imu"> </plugin> But when I run LIO-SAM in some simulation world, the incremental part of the map shows a rotation trend, so does the trajectory. But in others, there is no such problem.
   

3. Time synchronization: Because LIO-SAM always starts well in the beginning and drift later. Meanwhile it ran well with simulation. I guess it's a bad synchronization increasing the error which leads to failure in alignment. But LIO-SAM runs well with ros bags which is recorded in reality with real time. And I checked the timestamp of topics, they look synchronized well.

And it always has this in log, even I decrease the rate of rosbag playing to 0.1:
Message Filter dropping message: frame 'vehicle_blue/lidar_link/gpu_lidar' at time 166.800 for reason 'discarding message because the queue is full'

Part of the /imu/data and /points is shown below:
`---
header:
stamp:
sec: 118
nanosec: 782000000
frame_id: vehicle_blue/imu_link/imu_sensor
orientation:
x: -6.693089243585746e-07
y: 3.1786958765501195e-06
z: -0.00022797704779308332
w: 0.9999999740079578
orientation_covariance:

• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
  angular_velocity:
  x: 0.00022066749092428036
  y: -7.386920544000455e-05
  z: 0.00016885955438596516
  angular_velocity_covariance:
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
  linear_acceleration:
  x: 0.01097861728531426
  y: 0.0021158993581091067
  z: 9.791787718124585
  linear_acceleration_covariance:
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0

---

header:
stamp:
sec: 118
nanosec: 785000000
frame_id: vehicle_blue/imu_link/imu_sensor
orientation:
x: -6.740578860418199e-07
y: 3.1719698767112e-06
z: -0.00022797454932045252
w: 0.9999999740085456
orientation_covariance:

• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
  angular_velocity:
  x: -4.3195727250768445e-05
  y: -6.625150169082166e-05
  z: 9.171380094776304e-05
  angular_velocity_covariance:
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
  linear_acceleration:
  x: -0.005566124718655885
  y: -0.01409705211571831
  z: 9.773846261730535
  linear_acceleration_covariance:
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0

---

header:
stamp:
sec: 120
nanosec: 0
frame_id: vehicle_blue/lidar_link/gpu_lidar
height: 16
width: 1875
fields:

• name: x
  offset: 0
  datatype: 7
  count: 1
• name: y
  offset: 4
  datatype: 7
  count: 1
• name: z
  offset: 8
  datatype: 7
  count: 1
• name: intensity
  offset: 16
  datatype: 7
  count: 1
• name: ring
  offset: 24
  datatype: 4
  count: 1
  is_bigendian: false
  point_step: 32
  row_step: 60000
  data:
• 97
• 51
• 227
• 192
• 42
• 228
• 143
• 181
• 121
• 131
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 14
• 51
• 227
• 192
• 246
• 5
• 195
• 188
• 121
• 131
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 230
• 50
• 227
• 192
• 67
• 5
• 67
• 189
• 93
• 132
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 233
• 50
• 227
• 192
• 97
• 68
• 146
• 189
• 33
• 134
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• '...'
  is_dense: false

---

header:
stamp:
sec: 120
nanosec: 102000000
frame_id: vehicle_blue/lidar_link/gpu_lidar
height: 16
width: 1875
fields:

• name: x
  offset: 0
  datatype: 7
  count: 1
• name: y
  offset: 4
  datatype: 7
  count: 1
• name: z
  offset: 8
  datatype: 7
  count: 1
• name: intensity
  offset: 16
  datatype: 7
  count: 1
• name: ring
  offset: 24
  datatype: 4
  count: 1
  is_bigendian: false
  point_step: 32
  row_step: 60000
  data:
• 58
• 51
• 227
• 192
• 17
• 228
• 143
• 181
• 79
• 131
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 231
• 50
• 227
• 192
• 212
• 5
• 195
• 188
• 79
• 131
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 190
• 50
• 227
• 192
• 33
• 5
• 67
• 189
• 49
• 132
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 195
• 50
• 227
• 192
• 72
• 68
• 146
• 189
• 248
• 133
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• '...'
  is_dense: false

---

header:
stamp:
sec: 120
nanosec: 201000000
frame_id: vehicle_blue/lidar_link/gpu_lidar
height: 16
width: 1875
fields:

• name: x
  offset: 0
  datatype: 7
  count: 1
• name: y
  offset: 4
  datatype: 7
  count: 1
• name: z
  offset: 8
  datatype: 7
  count: 1
• name: intensity
  offset: 16
  datatype: 7
  count: 1
• name: ring
  offset: 24
  datatype: 4
  count: 1
  is_bigendian: false
  point_step: 32
  row_step: 60000
  data:
• 39
• 54
• 227
• 192
• 236
• 229
• 143
• 181
• 114
• 134
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 212
• 53
• 227
• 192
• 87
• 8
• 195
• 188
• 114
• 134
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 171
• 53
• 227
• 192
• 164
• 7
• 67
• 189
• 85
• 135
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 175
• 53
• 227
• 192
• 42
• 70
• 146
• 189
• 26
• 137
• 243
• 191
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• 0
• '...'
  is_dense: false

---

`

Please leave me some thoughts if you met similar situation before or if you have any idea that I should try.

[RodrigofrBastos]

I'm facing similar issues testing LIO-SAM on a gazebo simulation