mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
matlabbeandmathieu86
11edc01d6a
* rtabmap_odom tests and doc * opengv note * added ci checks or humble-latest flaky dep cmake errors * Added real data tests for rgbd_odom and stereo_odom * added real data for icp_odometry's deskewing test * fixing json cmake error on lyrical/rolling * test 2d icp odom deskewing branch * first review of existing OdometryROS tests * testing with imu used as guess * tested imu arrivals sync * Fixed odom reset on right pose when guess frame id is used * fixing header errors in ci >=lyrical * Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test * Added stereo odom support for features-only frames. Added multicam stereo tests. * forcing latest rtabmap version * updated OdometryROS API * ci: dont build non-latest docker in pull requests * splitting docker jobs * doc edit * Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet) * updated stereo doc * ficing rolling ci (rviz Ogre header) * Added test coverage of alll rgbd_image callbacks * fixing rolling ci * making docker ci build/run the tests on pull requests * fixing ros2 ci testing * improved sync callback coverage * improving stereo_odometry test coverage * improved icp_odometry test coverage * lyrical voxel_grid ptr error * make multicam tests working as well without opengv * removing deps of missing packages on rolling * PCL empty cloud conversion compiler errors fix * fixing icp_odometry test failure on ci witohut libpointmatcher * fixing nav2 costmap plugin build on lyrical * joining thread when exiting * updating icp test to work the same on pcl 1.15 (lyrical) * Fix parallel tests seg fault --------- Co-authored-by: mathieu86 <[email protected]>
107 lines
5.2 KiB
Markdown
107 lines
5.2 KiB
Markdown
# 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.
|
|
|
|
## Contents
|
|
|
|
- [Usage](#usage)
|
|
- [Subscribed Topics](#subscribed-topics)
|
|
- [Published Topics](#published-topics)
|
|
- [Required Transforms](#required-transforms)
|
|
- [Parameters](#parameters)
|
|
- [Requirements](#requirements)
|
|
- [Behavior when TF is missing](#behavior-when-tf-is-missing)
|
|
- [Notes](#notes)
|
|
|
|
## 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.
|