urg_stamped

Precisely and accurately stamped URG driver for ROS

Background and Algorithm

2D-LIDAR URG series provides 1ms resolution timestamp and exclusive clock synchronization mode. It was hard to compensate clock drift during measurement using them. Also, the resolution of the timestamp was not enough for a high-speed motion of the sensor.

So, urg_stamped estimates sub-millisecond by the following algorithm:

• Select the algorithms based on the sensor model
  • UTM: UTM-30LX-EW
  • UST(UUST1): UST-*LX, UST-*LC with firmware version <4.0.0
  • UST(UUST2-Unfixed): UST-*LX, UST-*LC with firmware version >=4.0.0 and <4.0.3
  • UST(UUST2-Fixed): UST-*LX, UST-*LC with firmware version >=4.0.3
• Determine sensor internal clock state (clock offset and gain) using TM command (Triggered by 30s timer by default)
  • UTM/UST(UUST1)/UST(UUST2-Fixed): (These models respond to TM command as expected in SCIP2 protocol)
    • 1. Observe sub-millisecond clock offset by finding increment of millisecond resolution sensor timestamp
    • 1. Observe clock gain from multiple observations of the clock offset
  • UST(UUST2-Unfixed): (This model responds to TM command on the next 5ms frame which breaks SCIP2’s time synchronization logic)
    • 1. Request TM command many times with different timing
    • 1. Filter responses with large delay
    • 1. Collect sensor response timings which should be synchronized to the sensor timestamp increment
    • 1. Observe sub-millisecond clock offset based on the sensor response timings and sensor timestamps
    • 1. Observe clock gain from multiple observations of the clock offset
• Determine scan origin time and interval
  • UTM: (UTM sends scan data right after the scan is finished)
    • 1. Observe scan timing based on scan data arrival time
  • UST: (UST sends scan data on the next 1ms frame after the scan is finished)
    • 1. Find sensor scan timestamp jitter
    • 1. Observe scan origin time and scan interval using scan timestamp jitter
• Calculate sub-millisecond scan timestamp based on the observed scan origin time and scan interval

LaserScan data is stopped during sensor internal clock estimation which takes at most ~100ms on UTM/UST(UUST1)/UST(UUST2-Fixed) and ~1s on UST(UUST2-Unfixed). To avoid stopping all sensors at once on multi-sensor configuration, urg_stamped automatically adjusts the timing of sensor internal clock estimation based on the messages on urg_stamped_sync_start topic.

Usages

Topics and major parameters are designed to be compatible with urg_node.

Published Topics

• scan (sensor_msgs::LaserScan)

Parameters

urg_node compatible parameters

• ip_address (string): device IP address
• ip_port (int): device TCP/IP port
• frame_id (string): frame_id of published scans
• publish_intensity (bool): fill intensity field if true
• error_limit (int): reset the sensor and exit if errors occur more than this count

urg_stamped specific parameters

• clock_estim_interval (double): sensor internal clock state estimation interval in seconds (dropping several scans during estimation)
• fallback_on_continuous_scan_drop (int): fallback to naive 1ms accuracy timestamp if failed to estimate sub-millisecond timestamp more than this value

Known Limitations

• Timestamp estimation is designed for sensors connected by ethernet interface.
  • Tested on the following sensor models:
    • UTM-30LX-EW
    • UST-05LX
    • UST-20LX
    • UST-30LC
  • UUST2 model of UST series (firmware version >=4.0.0) takes longer time to perform the time synchronization due to the sensor’s behavior.
• Some scans are dropped due to the clock synchronization and delay estimation.

Comparison with urg_node

Configurations

Three UTM-30LX-EWs are mounted on a velocity controlled turntable, as shown below, to reconstruct 3-D point cloud from 2-D scans. The accuracy of the timestamp affects offset and precision of the timestamp affects the distribution of the pointcloud.

Results

[!NOTE]
Following results are based on the previous algorithm (urg_stamped<0.2.0). They will be updated later.

The image below shows point cloud with 1 rad/s of the turntable which can be assumed as a reference. (decay time of the point cloud: 10 seconds)

urg_node has large error even if calibrate_time and synchronize_time options are enabled (captioned as urg_node (sync)). urg_stamped has better timestamp characteristics comparing with urg_node.

	10 rad/s	20 rad/s
urg_stamped
urg_node
urg_node (sync)

Changelog for package urg_stamped

0.0.8 (2021-03-10)

• Fix token name for releasing (#86)
• Fix prerelease test (#85)
• Remove unnecessary newline from log (#82)
• Migrate to GitHub Actions (#81)
• Contributors: Atsushi Watanabe

0.4.0 (2024-12-10)

• Set frame_id to status message (#180)
• Support UUST2 with fixed firmware (#177)
• CI: Use alpine 3.20 in cloner stage (#176)
• Contributors: Atsushi Watanabe, f-fl0

0.3.0 (2024-10-15)

• Avoid concurrent time sync of multiple sensors (#173)
• Support UUST2 (#172)
• Contributors: Atsushi Watanabe

0.2.1 (2024-09-10)

• Fix out-of-bound vector access in urg_sim (#169)
• Fix stamp on initial state (#168)
• Reduce log level of scan drop count if small (#167)
• Contributors: Atsushi Watanabe

0.2.0 (2024-08-03)

• Tweak E2E test timeout (#165)
• Stabilize E2E test (#163)
• Update CI actions (#161)
• New timestamp estimator (#151)
• Use C++14 (#158)
• Include cstdint (#157)
• Contributors: Atsushi Watanabe

0.1.1 (2024-05-09)

• Fix build dependency to message (#155)
• Fix handling of multiple responses in one read (#152)
• Fix urg_sim CPU usage (#150)
• Improve simulated communication delay (#149)
• Use clock_rate in urg_sim (#148)
• Fix E2E test assertion and true timestamp index (#147)
• Add URG simulator for CI (#146)
• Update codecov/codecov-action to v4 (#145)
• Contributors: Atsushi Watanabe

0.1.0 (2024-02-14)

• Add option to disable on-scan time sync (#141)
• Contributors: Atsushi Watanabe

0.0.17 (2023-09-13)

• Update CI scripts (#138)
• Reboot on reset error only if requested reset (#135)
• Drop Melodic support (#134)
• Contributors: Atsushi Watanabe

0.0.16 (2023-01-05)

• Update release scripts (#133)
• Fix multiline run in bloom-release workflow (#130)
• Detect future timestamp as error (#129)
• Unuse actions/create-release (#127)
• Unuse downloaded scripts (#128)
• Update CI workflows (#126)
• Contributors: Atsushi Watanabe, f-fl0

0.0.15 (2021-12-15)

• Fix parsing parameter containing semicolon (#123)
• Use Alpine ROS as a cloner base (#122)
• Contributors: Atsushi Watanabe

0.0.14 (2021-08-23)

• Fix continuous time sync failure (#115)
• Tweak indent of stream oprators (#119)
• Gracefully stop connection (#118)
• Improve log outputs for multi-sensor configuration (#116)
• Automatically hide old bot comments (#117)
• Contributors: Atsushi Watanabe

0.0.13 (2021-08-03)

• Remove doubled QT command request (#112)
• Contributors: Atsushi Watanabe

0.0.12 (2021-07-11)

• Fix gpg key source in prerelease test (#111)
• Reboot sensor if time sync error count exceeded limit and never succeeded (#108)
• Rotate bot token (#106)
• Drop Kinetic (#105)
• Contributors: Atsushi Watanabe

0.0.11 (2021-04-23)

• Send QT command twice to avoid being ignored (#103)
• Change TM0 status 10 error log level (#100)
• Fallback timeout during time synchronization (#97)
• Contributors: Atsushi Watanabe

0.0.10 (2021-04-07)

• Add codecov.yml (#96)
• Fix error handling during delay estimation (#92)
• Add option to enable debug log output at launch (#93)
• Send TM command after receiving QT response (#91)
• Refactor directory and namespace (#90)
• Contributors: Atsushi Watanabe

0.0.9 (2021-03-10)

• Release 0.0.8 (#84)
• Fix token name for releasing (#86)
• Fix prerelease test (#85)
• Remove unnecessary newline from log (#82)
• Migrate to GitHub Actions (#81)
• Contributors: Atsushi Watanabe, github-actions[bot]

0.0.7 (2020-08-12)

• Remove travis_retry from prerelease_test.sh (#78)
• Use downloaded gh-pr-comment binary in docker container (#77)
• Download gh-pr-comment binary instead of using pip (#75)
• Reboot lidar when it is in abnormal state (#71)
• Create GitHub Release after bloom release (#72)
• Update CI config (#69)
• Contributors: Atsushi Watanabe, Yuta Koga

0.0.6 (2020-06-23)

• Add check for device timestamp jump to node (#66)
• Add timestamp jump detector to Walltime (#65)
• Contributors: Yuta Koga

0.0.5 (2020-04-07)

• Support Noetic (#61)
• Contributors: Atsushi Watanabe

0.0.4 (2020-01-29)

• Automate bloom release (#58)
• Add error count check (#57)
• Fix response status check (#56)
• Format pointer alignment (#55)
• Contributors: Atsushi Watanabe

0.0.3 (2019-08-15)

• Run prerelease-test on release- branch (#49)
• Refactor logging (#48)
• Drop ROS Indigo and Ubuntu Trusty support (#47)
• Remove old_boost_fix.h (#42)
• Contributors: Atsushi Watanabe

0.0.2 (2018-08-23)

• Fix license in manifest (#39)
• Update README (#36)
• Estimate sub-millisecond timestamp (#35)
  • Estimate sub-millisecond timestamp by complementary filter fusing timestamp and packet arrival time
  • Add packet arrival time outlier removal
  • Add zero-delay moving average
  • Add unit tests for filters
• Add build matrix for ROS indigo/kinetic/melodic (#38)
  • Add build matrix
  • Fix workspace init
  • Fix rosdep argument
  • Fold test details
  • Fix test for latest g++
• Merge pull request #34 from seqsense/update-manifest-format
• Update manifest format
• Receive both MD and ME response by one callback (#33)
• Make some info messages debug level (#30)
• Fix step chage of estimated time origin (#28)
• Update CI settings (#26)
• Apply Apache License 2.0 (#25)
• Add periodic communication delay estimation (#23)
  • Add periodic communication delay estimation
  • Make timeSync and delayEstimation exclusive
  • Retry TM command if not responded
  • Reduce duration for delay estimation
• Fix time origin calculation (#21)
  • Fix delay check
  • Estimate time using received time and estimated delay
  • Fix time origin calculation
• Randomize time sync timing (#20)
• Tweak UTM behavior with intensity (#18)
• Fix II response parsing on UTM (#17)
• Add TCP connection watchdog (#15)
• Handle device timestamp overflow (#12)
  • Handle device timestamp overflow
  • Add test for Walltime
• Add test for Decoder (#14)
• Validate checksum (#11)
• Add publish_intensity parameter (#9)
• Fix clock gain estimation (#7)
  • Rely on sinle clock gain estimation
• Make debug outputs detailed (#6)
• Add CI (#4)
  • Add CI
  • Fix lint errors
• Estimate device clock gain (#3)
• Increase outlier removal thresholds
• Fix message header
• Fix boost placeholder namespace
• Calculate timestamp in system time
• Use urg_node compatible parameter names
• Change path and namespace to scip2
• Add communication delay estimation
• Output LaserScan messages
• Add stream data processors
• Add parameter response processors
• Add base protocol layer
• Add TCP connection layer
• Contributors: Atsushi Watanabe, So Jomura, jojo43