using PTP for Ouster LiDAR

[JJuhyeon-Kim]

Hi! Thank you so much for your interesting research.

I want to use Ouster LiDAR, but I am struggling with handling it for months.
I know we have to sync the IMU and lidar and we should change timestamp mode to TIME_FROM_PTP_1588 for Ouster LiDAR.
However, I don't have hardware PTP enabled NIC.
Is there any way to use PTP based timestamp?

[px4Li]

First run ouster then try this command and waiting for more 140 seconds.
sudo ptpd -i enp3s0 -M

[JJuhyeon-Kim]

Thank you @px4Li I can confirm it's working, here's a msg timestamp example with the internal clock:

LiDAR (t) timestamp: 		1614616844904756332 - March 1, 2021 4:40:44.904 PM
LiDAR (msg.header) timestamp: 	7403340742010 	    - August 8, 2204 7:32:22.010 PM

And here's one with the sensor set to TIME_FROM_PTP_1588 and running the command you provided above:

LiDAR (t) timestamp: 		1614616639310819517 - March 1, 2021 4:37:19.310 PM
LiDAR (msg.header) timestamp: 	1614616639254676480 - March 1, 2021 4:37:19.254 PM

msg.header.timestamps are now properly timestamped!

Excerpt from ptpd's manual page:

Even without hardware assistance, PTPd is able to achieve and maintain sub-microsecond level timing precision and is able to withstand PTP Grandmaster failovers, link failures and restarts with minimal impact to timing performance.

You sir deserve a trophy! :)

@hummblebee, Thank you very much for your good reply.

By the way, can I know how you mapped it?
I already succeeded in experimenting with velodyne lidar, and I also succeeded in experimenting with the sample data of the ouster (rooftop_ouster_dataset.bag) that the author uploaded.
But strangely, my ouster data (OS1-128) fails to map.
I modified the parts to be modified when using the ouster lidar (type of sensor, N_SCAN, topic name) , but strangely enough, the sample data and my velodyne data are mapped well, but my ouster data does not work.
Also I use the same sensors (IMU&LiDAR) with the author.

Below is the param file when using ouster data

lio_sam:

  # Topics
#  pointCloudTopic: "os1_node_"               # Point cloud data
#  imuTopic: "imu_raw"                         # IMU data

  pointCloudTopic : "os_cloud_node/points"
  imuTopic : "imu/data"

  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  gpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file

  # Frames
  lidarFrame: "base_link"
  baselinkFrame: "base_link"
  odometryFrame: "odom"
  mapFrame: "map"

  # GPS Settings
  useImuHeadingInitialization: true           # if using GPS data, set to "true"
  useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  
  # Export settings
  savePCD: false                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/Downloads/LOAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

  # Sensor Settings
  sensor: ouster                            # lidar sensor type, either 'velodyne' or 'ouster'
  N_SCAN: 128                                  # number of lidar channel (i.e., 16, 32, 64, 128)
  Horizon_SCAN: 1024                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used

  # IMU Settings
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03
  imuAccBiasN: 6.4356659353532566e-05
  imuGyrBiasN: 3.5640318696367613e-05
  imuGravity: 9.80511
  imuRPYWeight: 0.01

  # Extrinsics (lidar -> IMU)
  extrinsicTrans: [0.0, 0.0, 0.0]
  extrinsicRot: [-1, 0, 0,
                  0, 1, 0,
                  0, 0, -1]
  extrinsicRPY: [0,  1, 0,
                 -1, 0, 0,
                  0, 0, 1]
  # extrinsicRot: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]
  # extrinsicRPY: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]

  # LOAM feature threshold
  edgeThreshold: 1.0
  surfThreshold: 0.1
  edgeFeatureMinValidNum: 10
  surfFeatureMinValidNum: 100

  # voxel filter paprams
  odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
  mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
  mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor

  # robot motion constraint (in case you are using a 2D robot)
  z_tollerance: 1000                            # meters
  rotation_tollerance: 1000                     # radians

  # CPU Params
  numberOfCores: 4                              # number of cores for mapping optimization
  mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency

  # Surrounding map
  surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
  surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
  surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
  surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)

  # Loop closure
  loopClosureEnableFlag: true
  loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
  historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
  historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
  historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
  historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

  # Visualization
  globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
  globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
  globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density




# Navsat (convert GPS coordinates to Cartesian)
navsat:
  frequency: 50
  wait_for_datum: false
  delay: 0.0
  magnetic_declination_radians: 0
  yaw_offset: 0
  zero_altitude: true
  broadcast_utm_transform: false
  broadcast_utm_transform_as_parent_frame: false
  publish_filtered_gps: false

# EKF for Navsat
ekf_gps:
  publish_tf: false
  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  frequency: 50
  two_d_mode: false
  sensor_timeout: 0.01
  # -------------------------------------
  # External IMU:
  # -------------------------------------
  imu0: imu_correct
  # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
  imu0_config: [false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, true,
                true,  true,  true]
  imu0_differential: false
  imu0_queue_size: 50 
  imu0_remove_gravitational_acceleration: true
  # -------------------------------------
  # Odometry (From Navsat):
  # -------------------------------------
  odom0: odometry/gps
  odom0_config: [true,  true,  true,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_differential: false
  odom0_queue_size: 10

  #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
  process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

I've been thinking about this problem for months.
I'd appreciate it if you could give me some advice.

[JJuhyeon-Kim]

First run ouster then try this command and waiting for more 140 seconds.
sudo ptpd -i enp3s0 -M

@px4Li , Thank you so much for sharing.

However, the way you taught me doesn't work well for me.
The two pictures below are the header/stamp of imu and the header/stamp.

After I enter the command 'sudo ptpd -i enp4s0 -M -V', I can't get the header/stamp info.

Do you know the cause of this phenomenon?
I'd really appreciate it if you could give me some advice.

[px4Li]

@JJuhyeon-Kim
What is your roslaunch command for ouster_ros? Seems ouster_ros does not publish data?

[rcabg]

Hey @JJuhyeon-Kim

I struggled with ouster and ptp for so long until I got it working. I modified the ouster driver as you can see here https://github.com/fada-catec/ouster_ros in order to make it simpler.

It includes instructions in order to make PTP and the driver installed and working. I hope it helps!

[rcabg]

@rcabg Thanks for the driver, although a newer driver is released for the new firmware 2.0 compatibility but I went ahead and tried yours after running sudo ptpd -i enp3s0 -M and it printed an error saying "OS clock is not sync with host. Delay is: 9 secs" !

9 secs is a lot, but when I print the ros messages the LiDAR and the IMU are milliseconds apart!

Hey @hummblebee , thanks for trying!

The driver is compatible with the 2.0 version, I'm going to update the README for clarification. I don't pretend to replace the official driver, but certainly there is some features that I missed in the official, so this is why we made this. More info in the "Motivation/Features" section of the README.

If the Delay error does not appear continuously, it means that timestamps are synchronized. It is normal that the firsts messages have a stamp offset until ptp syncs everything. This max admissible offset can be tuned in the launch file so the driver warns you if it not working correctly.

Again, if the error message stops, everything is working normally.