[SOLVED] [LIDAR- IMU Data stream decoupling] Horizon IMU rate 50hz issue

[TokyoWarfare]

EDIT: SOLUTION IN THIS LATER POST:
#54 (comment)

FORK WITH FIXED STREAMS
https://github.com/TokyoWarfare/livox_ros2_driver_Decoupled_Imu_Stream

///////////////////////////////////////////////////////////
Hello.
I'm having issue with the rate of the IMU.

(ROS2-Docker) root@PabloPCFast:/$ ros2 topic hz /livox/imu
average rate: 51.991
	min: 0.000s max: 0.100s std dev: 0.03919s window: 52
average rate: 52.374
	min: 0.000s max: 0.100s std dev: 0.03912s window: 110
average rate: 54.187
	min: 0.000s max: 0.100s std dev: 0.03859s window: 168
average rate: 53.896
	min: 0.000s max: 0.100s std dev: 0.03868s window: 221
average rate: 53.132
	min: 0.000s max: 0.100s std dev: 0.03889s window: 271
average rate: 52.946
	min: 0.000s max: 0.100s std dev: 0.03894s window: 323
average rate: 52.672
	min: 0.000s max: 0.100s std dev: 0.03902s window: 374
average rate: 51.951
	min: 0.000s max: 0.100s std dev: 0.03923s window: 426
average rate: 51.847
	min: 0.000s max: 0.100s std dev: 0.03926s window: 477
average rate: 51.862
	min: 0.000s max: 0.100s std dev: 0.03926s window: 529
average rate: 51.696
	min: 0.000s max: 0.100s std dev: 0.03930s window: 579
average rate: 51.393
	min: 0.000s max: 0.100s std dev: 0.03939s window: 627
average rate: 51.363
	min: 0.000s max: 0.100s std dev: 0.03940s window: 678
average rate: 51.337
	min: 0.000s max: 0.100s std dev: 0.03941s window: 729
average rate: 51.249
	min: 0.000s max: 0.100s std dev: 0.03943s window: 779
average rate: 51.233
	min: 0.000s max: 0.100s std dev: 0.03944s window: 830
average rate: 51.335
	min: 0.000s max: 0.100s std dev: 0.03941s window: 883
average rate: 51.427
	min: 0.000s max: 0.100s std dev: 0.03938s window: 936
average rate: 51.346
	min: 0.000s max: 0.100s std dev: 0.03940s window: 991
average rate: 51.428
	min: 0.000s max: 0.100s std dev: 0.03938s window: 1044
average rate: 51.455
	min: 0.000s max: 0.100s std dev: 0.03937s window: 1096
average rate: 51.300
	min: 0.000s max: 0.100s std dev: 0.03942s window: 1144
average rate: 51.287
	min: 0.000s max: 0.100s std dev: 0.03942s window: 1195
average rate: 51.275
	min: 0.000s max: 0.100s std dev: 0.03942s window: 1246
average rate: 51.264
	min: 0.000s max: 0.100s std dev: 0.03942s window: 1297
average rate: 51.215
	min: 0.000s max: 0.100s std dev: 0.03944s window: 1347
average rate: 51.244
	min: 0.000s max: 0.100s std dev: 0.03943s window: 1399
average rate: 51.127
	min: 0.000s max: 0.100s std dev: 0.03946s window: 1452
average rate: 51.156
	min: 0.000s max: 0.100s std dev: 0.03946s window: 1504
average rate: 51.217
	min: 0.000s max: 0.100s std dev: 0.03944s window: 1557
average rate: 51.241
	min: 0.000s max: 0.100s std dev: 0.03944s window: 1609
average rate: 51.230
	min: 0.000s max: 0.100s std dev: 0.03944s window: 1665
average rate: 51.223
	min: 0.000s max: 0.100s std dev: 0.03944s window: 1716
average rate: 51.185
	min: 0.000s max: 0.100s std dev: 0.03945s window: 1771
average rate: 51.207
	min: 0.000s max: 0.100s std dev: 0.03945s window: 1823
average rate: 51.174
	min: 0.000s max: 0.100s std dev: 0.03946s window: 1873
average rate: 51.169
	min: 0.000s max: 0.100s std dev: 0.03946s window: 1924
average rate: 51.137
	min: 0.000s max: 0.100s std dev: 0.03946s window: 1979
average rate: 51.107
	min: 0.000s max: 0.100s std dev: 0.03947s window: 2029
average rate: 51.129
	min: 0.000s max: 0.100s std dev: 0.03946s window: 2081
average rate: 51.174
	min: 0.000s max: 0.100s std dev: 0.03945s window: 2134
average rate: 51.051

As you can see it is around 50hz

Point rate is as specked:

(ROS2-Docker) root@PabloPCFast:/$ ros2 topic hz /livox/lidar
average rate: 10.004
	min: 0.100s max: 0.100s std dev: 0.00012s window: 12
average rate: 10.001
	min: 0.100s max: 0.100s std dev: 0.00013s window: 22
average rate: 10.001
	min: 0.100s max: 0.100s std dev: 0.00013s window: 32
average rate: 10.001
	min: 0.100s max: 0.100s std dev: 0.00012s window: 42
average rate: 10.000
	min: 0.100s max: 0.100s std dev: 0.00014s window: 52
average rate: 10.000
	min: 0.100s max: 0.100s std dev: 0.00014s window: 62
average rate: 10.000
	min: 0.100s max: 0.100s std dev: 0.00014s window: 72
average rate: 10.000
	min: 0.100s max: 0.100s std dev: 0.00013s window: 83
average rate: 10.000
	min: 0.099s max: 0.101s std dev: 0.00017s window: 93
average rate: 10.000
	min: 0.099s max: 0.101s std dev: 0.00016s window: 103
average rate: 10.000
	min: 0.099s max: 0.101s std dev: 0.00016s window: 113
average rate: 10.000
	min: 0.099s max: 0.101s std dev: 0.00016s window: 124
average rate: 10.000
	min: 0.099s max: 0.101s std dev: 0.00016s window: 134

My config looks like this:

{
"lidar_config": [
{
"broadcast_code": "3WEDJ540010115",
"enable_connect": true,
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 0
},

Checking again the docs it says

Push frequency of IMU sensor data
0 -- stop push
1 -- 200 Hz
Others -- undefined, it will cause unpredictable behavior
Currently only Horizon supports this, MID serials do not support it

Not sure if I've to input "Others" or set a push rate, I'll try to put 200 as value...

I know it is reading the config because when I had the default with enable in false without broadcast code etc... it did run 20hz or so and GLIM would launch err of too big gaps in between IMU registers

I'm running the driver from DOCKER, I wonder if there is overhead that could cause this.
I'm starting with SLAM & ROS so I went to use Docker so I could mess things up without trashing the whole OS . I had run GLIM from other doker and it ran nicely so my plan was to have siolated the ROS2 driver in a docker to capture in there, and run the ros2 bags from a shared volume.

Some details
ROS_DISTRO: humble
DockerOS Description: Ubuntu 22.04.5 LTS

Similar:
#10

Aparently in 2021 it was suggested to mote hte imu data to a separate thread. Has this been implemented into the driver already?

What firmware is needed for Horizon? Where can firmware be downloaded?

[TokyoWarfare]

FIXED!

(ROS2-Docker) root@PabloPCFast:/$ ros2 topic hz /livox/imu

average rate: 200.965
min: 0.000s max: 0.026s std dev: 0.00979s window: 201
average rate: 200.973
min: 0.000s max: 0.026s std dev: 0.00979s window: 402
average rate: 200.648
min: 0.000s max: 0.026s std dev: 0.00979s window: 602
average rate: 200.482
min: 0.000s max: 0.026s std dev: 0.00979s window: 802
average rate: 200.386
min: 0.000s max: 0.026s std dev: 0.00979s window: 1002
average rate: 200.025
min: 0.000s max: 0.026s std dev: 0.00980s window: 1205
average rate: 200.305
min: 0.000s max: 0.026s std dev: 0.00979s window: 1407
average rate: 200.017
min: 0.000s max: 0.026s std dev: 0.00980s window: 1605
average rate: 199.997
min: 0.000s max: 0.026s std dev: 0.00981s window: 1805
average rate: 200.014
min: 0.000s max: 0.026s std dev: 0.00981s window: 2010
average rate: 199.999
min: 0.000s max: 0.026s std dev: 0.00981s window: 2210
average rate: 199.998

Since the IMU rate is tied to teh Publish frequency and with 10hz for the point data I was capping at 50 it was as easy as to modify the config to
publish_freq = 40.0 # freqency of publish,1.0,2.0,5.0,10.0,etc

(ROS2-Docker) root@PabloPCFast:/$ ros2 topic hz /livox/lidar

average rate: 40.045
min: 0.024s max: 0.026s std dev: 0.00043s window: 42
average rate: 40.006
min: 0.024s max: 0.026s std dev: 0.00048s window: 83
average rate: 40.015
min: 0.024s max: 0.026s std dev: 0.00048s window: 124
average rate: 40.011
min: 0.024s max: 0.026s std dev: 0.00050s window: 164
average rate: 40.009
min: 0.024s max: 0.026s std dev: 0.00049s window: 205
average rate: 40.008
min: 0.024s max: 0.026s std dev: 0.00048s window: 246

for a quick test you can modify the json files in the install dir but if you rebuild that iwll be lost so don´t forget to modify the source jsons
for testing:
nano /root/ws_livox/install/livox_ros2_driver/share/livox_ros2_driver/launch/livox_lidar_launch.py
nano /root/ws_livox/install/livox_ros2_driver/share/livox_ros2_driver/launch/livox_lidar_rviz_launch.py

nano $(ros2 pkg prefix livox_ros2_driver)/share/livox_ros2_driver/launch/livox_lidar_rviz_launch.py
nano $(ros2 pkg prefix livox_ros2_driver)/share/livox_ros2_driver/launch/livox_lidar_launch.py

[TokyoWarfare]

A side effect of this is that per second the data is split into frame with smaller ammount of data. Hence GLIM, for instance with the values I was using thins some frames way too much and fails. but probably something that cna be tuned . Reducing the Imu rate to 150 may also help and finally , what should have come in the driver by default a decoupling of the imu data stream from the point publish rate could do the job but that is at another level.

[TokyoWarfare]

@Livox-SDK what's up with the support for your hardware? the software, the drivers in particular are half cooked, no commits to Github since 2021, after an epic pufblish rate feels like a nuke exploded.

[TokyoWarfare]

ok, more on this found the cause of this behaviour

On Lds.cpp

void Lds::StorageRawPacket(uint8_t handle, LivoxEthPacket* eth_packet) {
  LidarDevice *p_lidar = &lidars_[handle];
  LidarPacketStatistic *packet_statistic = &p_lidar->statistic_info;
  LdsStamp cur_timestamp;
  uint64_t timestamp;

  memcpy(cur_timestamp.stamp_bytes, eth_packet->timestamp,
         sizeof(cur_timestamp));
  timestamp = RawLdsStampToNs(cur_timestamp, eth_packet->timestamp_type);
  if (timestamp >= kRosTimeMax) {
    printf("Raw EthPacket time out of range Lidar[%d]\n", handle);
    return;
  }

  if (kImu != eth_packet->data_type) {
    UpdateLidarInfoByEthPacket(p_lidar, eth_packet);
    if (eth_packet->timestamp_type == kTimestampTypePps) {
      /** Whether a new sync frame */
      if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
          (cur_timestamp.stamp < kPacketTimeGap)) {
        auto cur_time = std::chrono::high_resolution_clock::now();
        int64_t sync_time = cur_time.time_since_epoch().count();
        /** used receive time as timebase */
        packet_statistic->timebase = sync_time;
      }
    }
    packet_statistic->last_timestamp = cur_timestamp.stamp;

    LidarDataQueue *p_queue = &p_lidar->data;
    if (nullptr == p_queue->storage_packet) {
      uint32_t queue_size = CalculatePacketQueueSize(
          buffer_time_ms_, p_lidar->info.type, eth_packet->data_type);
      InitQueue(p_queue, queue_size);
      printf("Lidar[%02d][%s] storage queue size : %d %d\n", p_lidar->handle,
             p_lidar->info.broadcast_code, queue_size, p_queue->size);
    }
    if (!QueueIsFull(p_queue)) {
      QueuePushAny(p_queue, (uint8_t *)eth_packet,
          GetEthPacketLen(eth_packet->data_type),
          packet_statistic->timebase,
          GetPointsPerPacket(eth_packet->data_type));
      if (QueueUsedSize(p_queue) > p_lidar->onetime_publish_packets) {
        if (semaphore_.GetCount() <= 0) {
          semaphore_.Signal();
        }
      }
    }
  } else {
    if (eth_packet->timestamp_type == kTimestampTypePps) {
      /** Whether a new sync frame */
      if ((cur_timestamp.stamp < packet_statistic->last_imu_timestamp) &&
          (cur_timestamp.stamp < kPacketTimeGap)) {
        auto cur_time = std::chrono::high_resolution_clock::now();
        int64_t sync_time = cur_time.time_since_epoch().count();
        /** used receive time as timebase */
        packet_statistic->imu_timebase = sync_time;
      }
    }
    packet_statistic->last_imu_timestamp = cur_timestamp.stamp;

    LidarDataQueue *p_queue = &p_lidar->imu_data;
    if (nullptr == p_queue->storage_packet) {
      uint32_t queue_size = 256;  /* fixed imu data queue size */
      InitQueue(p_queue, queue_size);
      printf("Lidar[%02d][%s] imu storage queue size : %d %d\n", p_lidar->handle,
             p_lidar->info.broadcast_code, queue_size, p_queue->size);
    }
    if (!QueueIsFull(p_queue)) {
      QueuePushAny(p_queue, (uint8_t *)eth_packet,
          GetEthPacketLen(eth_packet->data_type),
          packet_statistic->imu_timebase,
          GetPointsPerPacket(eth_packet->data_type));
    }
  }
}

as you can see, on reciving point data it checks for the queue be complete and if so semaphore.signal(), awaking the threads waiting to publish.

I would have expected that as the queue fills up at 200hz the data gets properly timestamped, and while data is received in bursts , the bag is stored with 200hz imu rates. But it is not. and the queue is 256packets size so I'm not sure why this still happens.

Hence.

since raising up the publish rate of lidar to 40 make the imu acheive the 200hz rate, the data seems to be reaching properly from the sensor, and may be a limitation or filtering of ros2 with data burst is what is causing the drop of packets form the imu. I don´t know. I'm whatever but an expert on this .

But if we know it can hancle 5 imu packets per sec with no problems, I can make the queue that short and when full signal a new imu semaphore that makes the separated threads in the driver.cpp resume publishing.

Time to create a separate semaphore for the imu data stream....

I hate to be doing this tbh... hope it works at least

[TokyoWarfare]

OK! I was looking for a fork I saw the other day fixing timesync issues and look what I found!
7d3a41f

[edit]
SO THE FIX implemented is somethign I though of that is that awake the threads when imu also arrives, he does by awaking them by timebut esentially I believe would be the esame as signal when an imu packet arrives. Will try this but this could cause some CPU overhead I beleive.

[TokyoWarfare]

ok good news
on the above posted function the data is arriving at 200hz rate from the sensor, so now it is just a matter of semaphoring properly

[livox_ros2_driver_node-1] [IMU] Packets received in last second: 206
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 205
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 202
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 202
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 204
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 203
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 204
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 206
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 203
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 203
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 204
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 202
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 202
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 204
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 206
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 204
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 202
[livox_ros2_driver_node-1] [IMU] Packets received in last second: 203

[TokyoWarfare]

ok guys, done

^C(ROS2_Driver_MOD) root@PabloPCFast:/$ ros2 topic hz /livox/imu
average rate: 201.807
min: 0.001s max: 0.005s std dev: 0.00042s window: 203
average rate: 202.688
min: 0.001s max: 0.006s std dev: 0.00051s window: 407
average rate: 203.324
min: 0.001s max: 0.006s std dev: 0.00056s window: 612
average rate: 203.196
min: 0.001s max: 0.006s std dev: 0.00055s window: 815
average rate: 203.476
min: 0.001s max: 0.006s std dev: 0.00057s window: 1020
average rate: 203.662
min: 0.001s max: 0.006s std dev: 0.00058s window: 1225
average rate: 203.250
min: 0.001s max: 0.006s std dev: 0.00055s window: 1426
average rate: 203.665
min: 0.001s max: 0.006s std dev: 0.00058s window: 1633
average rate: 203.572
min: 0.001s max: 0.006s std dev: 0.00057s window: 1836
average rate: 203.572
min: 0.001s max: 0.006s std dev: 0.00057s window: 2040
average rate: 203.664
min: 0.001s max: 0.006s std dev: 0.00058s window: 2245
average rate: 203.591
min: 0.001s max: 0.006s std dev: 0.00057s window: 2448
average rate: 203.591
min: 0.001s max: 0.006s std dev: 0.00057s window: 2652
average rate: 203.662
min: 0.001s max: 0.006s std dev: 0.00058s window: 2857
average rate: 203.539
min: 0.001s max: 0.006s std dev: 0.00057s window: 3059
^C(ROS2_Driver_MOD) root@PabloPCFast:/$ ros2 topic hz /livox/lidar
average rate: 10.009
min: 0.099s max: 0.101s std dev: 0.00057s window: 11
average rate: 10.005
min: 0.099s max: 0.101s std dev: 0.00051s window: 22
average rate: 10.004
min: 0.099s max: 0.101s std dev: 0.00047s window: 32
average rate: 10.003
min: 0.099s max: 0.101s std dev: 0.00047s window: 43
average rate: 10.002
min: 0.099s max: 0.101s std dev: 0.00048s window: 54
average rate: 10.000
min: 0.099s max: 0.101s std dev: 0.00048s window: 64
^C(ROS2_Driver_MOD) root@PabloPCFast:/$

[TokyoWarfare]

src_backup_01.tar.gz

Explanation of the Changes
This modification was carried out to properly support IMU data in the Livox ROS2 driver, ensuring that IMU packets are handled with the same architecture and efficiency as lidar packets.

What was done
Header updates (livox_ros2_driver.h):

Adjusted the signature of imuThread() to match the style of pollThread() (no parameters).

LDS class (lds.h / lds.cpp):

Added a new semaphore imu_semaphore_ alongside the existing semaphore_.

In StorageRawPacket(), when an IMU packet is received, the driver now signals imu_semaphore_ after pushing the packet into the IMU queue.

LDDC class (lddc.cpp):

Implemented DistributeIMUData() in parallel to DistributeLidarData().

This function waits on imu_semaphore_ and then polls the IMU queue (imu_data) for each connected lidar device, publishing the IMU messages.

Driver threads (livox_ros2_driver.cpp):

Added imuThread() that mirrors pollThread().

imuThread() calls DistributeIMUData() in a loop controlled by future_, ensuring clean shutdown and consistent behavior.

Results
IMU publishing rate: ~200 Hz (verified with ros2 topic hz /livox/imu).

Lidar publishing rate: ~10 Hz (verified with ros2 topic hz /livox/lidar).

Both threads now follow the same design pattern: they delegate blocking and synchronization to the DistributeXData() functions, which use semaphores internally.

The architecture is clean, symmetric, and efficient, with no duplicated logic or hacks.

Acknowledgment
This work was completed with the invaluable assistance of Microsoft Copilot, at my request. Copilot provided technical guidance step by step, while I directed the process and validated the implementation. The final solution was achieved through this collaboration.