Issue with timestamp handling in kinematic_icp for 2D LiDAR

[MikeDegany]

Description:
When using simulated data with Kinematic_ICP, the 2D lidar messages include a "stamps" field that is populated, but the timestamp values are different from those of a physical LiDAR. For instance, real data shows:

stamp:
  sec: 1724403965
  nanosec: 338063112

while simulation data shows:

stamp:
  sec: 13059
  nanosec: 441000000

when extracting timestamps using a sensor_msgs::PointCloud2ConstIterator on the "stamps" field, the iterator does not seem to advance properly—it always returns zero for every point.

Steps to Reproduce:

Run the ClearPath simulation that publishes 2D laser scanner messages and you can move the robot around by sending commands to /cmd_vel.

In Kinematic_ICP:
[namespace] is your namespace while simulation.

• set the argument base_frame to "base_link"
• In online_node.launch.py add remappings:

remappings=[
            ("lidar_odometry", LaunchConfiguration("lidar_odometry_topic")),
             ('/tf', '/[namespace]/tf'),
             ('/tf_static', '/[namespace]/tf_static'),        
],

Launch the Kinematic_ICP node (e.g., using ros2 launch kinematic_icp online_node.launch.py lidar_topic:=/[namespace]/sensors/lidar2d_0/scan use_2d_lidar:=true use_sim_time:=True ).

Make sure Kinematic_ICP starts without error. It should print outputs like

Started in 2D scanner mode with topic: /[namespace]/sensors/lidar2d_0/scan
It may also reset Kiss-ICP pose
KISS-ICP ROS 2 odometry node initialized

If you set the arg visualize:=true you should see the odom frame tf on RVIZ.

Observed Behavior:

When you start moving the robot, Kinematic_ICP crashes with error code -6

I tried to debug with GDB,
gdb --args /home/user/kinematicicp_ws/install/kinematic_icp/lib/kinematic_icp/kinematic_icp_online_node --ros-args -r __node:=online_node -r __ns:=/kinematic_icp --params-file /home/user/kinematicicp_ws/install/kinematic_icp/share/kinematic_icp/config/kinematic_icp_ros.yaml --params-file /tmp/launch_params_[] -r lidar_odometry:=lidar_odometry -r /tf:=/[namespace]/tf -r /tf_static:=/[namespace]/tf_static
Then type 'run' and move the robot to crash the program.
use backtrace and list to find out the problem. I got this:

Sophus ensure failed in function 'static Sophus::SO3<Scalar_> Sophus::SO3<Scalar_, Options>::expAndTheta(const Tangent&, Sophus::SO3<Scalar_, Options>::Scalar*) [with Scalar_ = double; int Options = 0; Sophus::SO3<Scalar_, Options>::Tangent = Eigen::Matrix<double, 3, 1>; Sophus::SO3<Scalar_, Options>::Scalar = double]', file '/opt/ros/humble/include/sophus/so3.hpp', line 725.
SO3::exp failed! omega: -nan -nan -nan, real: -nan, img: -nan

Thread 1 "kinematic_icp_o" received signal SIGABRT, Aborted.
__pthread_kill_implementation (no_tid=0, signo=6, threadid=[]) at ./nptl/pthread_kill.c:44
44	./nptl/pthread_kill.c: No such file or directory.

I traced back the error to get that in here in the TimeStampHandler.cpp file the iterator it does not advance. Although, the simulation does populate the "stamps" field (its name, offset, datatype, and count are correctly defined). However, when iterating over the "stamps" field with a sensor_msgs::PointCloud2ConstIterator, the iterator always returns zero. This behavior prevents the timestamps from being normalized and used further in the de-skewing process, ultimately causing downstream errors.

The program works fine with 3D Lidar which is weird. Could the be about laser_geometry projection?!! I don't know!

In the TimeStampHandler.cpp file, observe that the debug output for the "stamps" field (extracted using a PointCloud2ConstIterator) shows zero values for all points.
Note that while the simulation appears to populate the "stamps" field (as seen in the message fields printout), the iterator used in ExtractTimestampsFromMsg always yields 0.

Expected Behavior:

The "stamps" field should be read correctly by the iterator, yielding nonzero values that reflect the simulation’s timestamp (even if these values differ in magnitude from real LiDAR data).
The iterator should advance correctly through all points in the PointCloud2 message.

Environment:

ROS2 Distribution:  Humble
Simulator: Gazebo Fortress
Operating System: Ubuntu 22.04

Additional Context:
I have verified the available fields in the PointCloud2 message, which are:

        x, offset: 0, datatype: 7, count: 1
        y, offset: 4, datatype: 7, count: 1
        z, offset: 8, datatype: 7, count: 1
        stamps, offset: 12, datatype: 7, count: 1

[MikeDegany]

I’m sharing a ROS2 bag file to make it easier to reproduce.

📥 Download the bag file.

Steps to Run the Bag:

1. Unzip the downloaded file.
2. Run the following commands:

ros2 bag play KICP_Issue_31

ros2 launch kinematic_icp online_node.launch.py lidar_topic:=/JK5/sensors/lidar2d_0/scan use_2d_lidar:=true visualize:=true use_sim_time:=True

After a few seconds, the robot will begin moving. Shortly after, the program crashes.
Let me know if you need any additional details!

[benemer]

I just looked into this, and it turns out to be a bug in our TimeStampHandler. It looks like your 2D Laserscan does not have a timestamp for each point, so the timestamps in the PointCloud2 message will contain only zeros. This leads to an issue during the normalization.

Can you checkout at the branch benedikt/31_fix_TimeStampHandler and test if it solves your issue?

[MikeDegany]

@benemer
I noticed that your PR partially addresses the issue, but I found that some aspects were still incomplete. It crashes on my 3D simulated data for empty timestamps! To ensure a fully functional fix, I created a separate PR (#32 ) incorporating the necessary changes.
I've thoroughly tested my implementation, and it runs perfectly without any issues. I’d appreciate it if you could take a look and let me know your thoughts. If you'd prefer, I'm happy to collaborate to merge relevant improvements into your branch instead.

Thanks for your time and effort in maintaining this project! Looking forward to your feedback.