rtabmap_util tests and doc (#1450)

* Initial tests * more tests * More in-depth deskew() testing * slightly less verbose clamping corruption warning * added tf buffer related tests * added remaining tests * Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp * cleanup doc * fixing ci * rtabmap_util tests and doc * Added db_player tests * Added MapsManager tests * Added map_assembler tests * Documenting node first draft * relative links * Fixed british->usa english style. Reviewed all md files. * added link to install ros1 * updated badges * added Iron * added ubuntu * added codecov * updated coverage ci * fixing rosdep * updated ci cov job * ci bump * fixing cov ci * small doc cleanup
2026-10-04 08:47:45 +08:00 · 2026-09-07 21:23:22 -07:00
parent f77dda2b58
commit 61edb4ee85
60 changed files with 9443 additions and 192 deletions
@@ -0,0 +1,95 @@
+# lidar_deskewing
+
+Removes the motion distortion from a lidar scan.
+
+A spinning lidar takes tens of milliseconds to complete a sweep, and on a moving robot every point in that sweep is measured from a slightly different pose. The result is a *skewed* cloud: straight walls come out bent, and registration against it drifts.
+
+This node uses TF to find where the sensor actually was when each point was taken, and moves every point into the pose at the start of the sweep. A straight wall comes back straight.
+
+## Usage
+
+```bash
+ros2 run rtabmap_util lidar_deskewing --ros-args \
+  -p fixed_frame_id:=odom \
+  -r input_cloud:=/velodyne_points
+```
+
+```python
+ComposableNode(
+    package='rtabmap_util',
+    plugin='rtabmap_util::LidarDeskewing',
+    name='lidar_deskewing',
+    parameters=[{'fixed_frame_id': 'odom'}],
+    remappings=[('input_cloud', '/velodyne_points')])
+```
+
+## Subscribed Topics
+
+Connect one of the two.
+
+| Topic | Type | Description |
+|---|---|---|
+| `input_cloud` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | Must carry a **per-point time channel**, see [Requirements](#requirements). |
+| `input_scan` | [`sensor_msgs/msg/LaserScan`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/LaserScan.html) | Per-point times come from `time_increment`. |
+
+## Published Topics
+
+Output names are derived from the **resolved** input names, so remapping the input moves the output with it. With `input_cloud` remapped to `/velodyne_points` the output is `/velodyne_points/deskewed`.
+
+| Topic | Type | Description |
+|---|---|---|
+| `<input_cloud>/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The deskewed cloud, same frame and stamp as the input. |
+| `<input_scan>/deskewed` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | A `LaserScan` cannot represent a deskewed sweep — the points no longer lie on a regular angular grid — so the scan input also produces a cloud. |
+
+## Required Transforms
+
+| Transform | Description |
+|---|---|
+| `fixed_frame_id` → sensor frame, across the sweep | Must be available for the whole span of the sweep, at both its first and last stamp. |
+
+## Parameters
+
+| Parameter | Type | Default | Description |
+|---|---|---|---|
+| `fixed_frame_id` | `string` | `""` | **Required.** Frame the motion is measured against, usually `odom`. |
+| `wait_for_transform` | `double` | `0.01` | Seconds to wait for the transforms spanning the sweep. Raise it if odometry lags the lidar. |
+| `slerp` | `bool` | `false` | Interpolate between the poses at the start and end of the sweep instead of looking up TF per point. Much cheaper, and accurate enough at constant velocity. |
+| `queue_size` | `int` | `1` | Queue depth of the input subscriptions. |
+| `qos` | `int` | `0` | Reliability of the input subscriptions: `0` system default, `1` reliable, `2` best effort. |
+
+## Requirements
+
+For `input_cloud`, the cloud **must have a per-point time field**. Without one the node cannot know when each point was taken and cannot deskew.
+
+The field has to be named `t`, `time`, `stamps` or `timestamp` — anything else is not recognized, whatever it contains. Its type decides how the value is read:
+
+| Type | Meaning |
+|---|---|
+| `uint32` | nanoseconds since the cloud's own stamp |
+| `float32` | seconds since the cloud's own stamp |
+| `float64` | an absolute timestamp; seconds, milliseconds, microseconds and nanoseconds are told apart by magnitude |
+
+Common drivers that satisfy this out of the box: **Ouster** (`t`), **Velodyne** (`time`), **RoboSense** (`timestamp`) and **Livox** (`timestamp`). Livox needs its PointCloud2 output rather than the default `CustomMsg` format, which this node cannot subscribe to at all.
+
+To check what your driver actually publishes:
+
+```bash
+ros2 topic echo /your/points --field fields --once
+```
+
+If none of the four names is in that list, look for a driver option to add per-point timestamps before anything else.
+
+The `fixed_frame_id` → sensor transform must cover the whole sweep, which means **odometry has to be at least as recent as the lidar**. If it lags, raise `wait_for_transform`.
+
+## Behavior when TF is missing
+
+The two inputs deliberately differ:
+
+- A **cloud** is republished **unchanged** with a warning. Deskewing is an improvement, not a precondition, and dropping frames would break the pipeline behind it.
+- A **scan** is **dropped**, because converting it to a cloud is only worth doing as part of deskewing.
+
+## Notes
+
+Deskewing matters most when rotating: at 1 rad/s a 100 ms sweep spans nearly 6°, and the far end of the scan is badly misplaced. Pure translation at walking speed is a few centimeters, which matters at close range.
+
+Put this node before ICP odometry or [point_cloud_assembler](point_cloud_assembler.md), not after. Anything registering against a skewed cloud has already paid for the distortion.