Skip to content

Commit ff8f79b

Browse files
amockamock
authored
Merge pull request #8 from uos/develop
ROS 2-ify MICP-L Node & Bug Fixes
2 parents ec14c2a + d2bae05 commit ff8f79b

125 files changed

Lines changed: 6681 additions & 17797 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎README.md‎

Lines changed: 76 additions & 213 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626

2727
This repository contains algorithms designed for map-based robot localization, specifically when dealing with maps composed of triangle meshes or complete scene graphs. These maps may be provided by architects who have designed the building in which the robot operates, or they can be autonomously generated by the robot through Simultaneous Localization and Mapping (SLAM) methods. It's crucial to note that map-based localization differs from SLAM; it focuses on estimating the robot's pose within a potentially large map, whether the initial pose is roughly known (tracking) or entirely unknown from the start aka kidnapped robot problem. Map-based localization is essential for precisely planning the robot's missions on a given map.
2828

29-
## MICP-L
29+
## MICP-L - Pose Tracking in Meshes
3030

3131
MICP-L: Mesh-based ICP for Robot Localization Using Hardware-Accelerated Ray Casting.
3232
An approach to directly register range sensor data to a mesh in order to localize a mobile robot using hardware-accelerated ray casting correspondences (See publications).
@@ -59,252 +59,104 @@ Please reference the following paper when using the MICP-L method in your scient
5959
}
6060
```
6161

62-
The paper is available on [IEEE Xplore](https://ieeexplore.ieee.org/document/10802360) and as preprint on [arXiv](https://arxiv.org/abs/2210.13904). The experiments are available at https://github.com/amock/micp_experiments (mostly for ROS 1).
62+
The paper is available on [IEEE Xplore](https://ieeexplore.ieee.org/document/10802360) and as preprint on [arXiv](https://arxiv.org/abs/2210.13904). The experiments are available at [https://github.com/amock/micp_experiments](https://github.com/amock/micp_experiments), but they are primarily compatible with the ROS 1 version.
63+
See the older branches or commits for reference.
6364

64-
### Usage
6565

66-
The `micp_localization` Node starts the process of localizing the robot in a mesh using MICP and a given pose estimate.
67-
It is usually started through a launch file since it requires a large set of parameters.
66+
### Running `micp_localization_node` (Theoretical Usage)
6867

69-
### Launch
68+
> **Note:** The following is a theoretical usage example meant to illustrate the general setup.
69+
> For actual working examples and detailed instructions, please refer to:
70+
> [https://github.com/amock/rmcl_examples](https://github.com/amock/rmcl_examples)
7071
71-
The following Launch-File
72+
73+
The `micp_localization_node` starts the process of localizing the robot within a triangle mesh using MICP, based on a given pose estimate. It is typically launched via a **launch file**:
7274

7375
```xml
7476
<launch>
75-
76-
<node pkg="rmcl_ros" exec="micp_localization" name="micp_localization" output="screen">
77+
<node pkg="rmcl_ros" exec="micp_localization_node" name="rmcl_micpl" output="screen">
7778
<param name="map_file" value="/path/to/mesh/map.dae" />
7879
<param from="/path/to/config/file.yaml" />
79-
<remap from="pose_wc" to="/initialpose" />
80-
</node>
81-
80+
</node>
8281
</launch>
8382
```
8483

85-
runs the MICP localization node. After that a pose has to be given, e.g. by the RViz "2D Pose Estimate" Tool that publishes the results on the `/initialpose` topic.
86-
Doing that, make sure to set the fixed frame to the map coordinate system.
87-
RMCL itself doesn't provide any tools to visualize the maps (triangle meshes).
88-
If you want to see the map in RViz, use for example the `rviz_mesh_tools_plugins` of the [mesh_tools](https://github.com/naturerobots/mesh_tools).
89-
9084
<details>
9185
<summary>Once the launch file is started, the output in Terminal should look as follows:</summary>
9286

9387
```console
94-
Combining Unit: CPU
95-
MICP initiailized
96-
97-
-------------------------
98-
--- BACKENDS ---
99-
-------------------------
100-
Available computing units:
101-
- CPU
102-
- GPU
103-
Available raytracing backends:
104-
- Embree (CPU)
105-
- Optix (GPU)
106-
MICP load params
107-
108-
-------------------------
109-
--- FRAMES ---
110-
-------------------------
111-
- base: base_footprint
112-
- odom: odom
113-
- base -> odom: yes
114-
- map: map
115-
Estimating: base_footprint -> map
116-
Providing: odom -> map
117-
118-
-------------------------
119-
--- SENSORS ---
120-
-------------------------
121-
- velodyne
122-
- data: Topic
123-
- topic: /velodyne_points
124-
- msg: sensor_msgs/PointCloud2
125-
- data: yes
126-
- frame: velodyne
127-
- model: Params
128-
- type: spherical - loaded
129-
- micp:
130-
- backend: embree
131-
MICP load params - done. Valid Sensors: 1
132-
Waiting for pose guess...
88+
[micp_localization_node-2] -------------------------
89+
[micp_localization_node-2] --- MAP ---
90+
[micp_localization_node-2] -------------------------
91+
[micp_localization_node-2] - file: /home/amock/rmcl_ws/install/rmcl_examples_maps/share/rmcl_examples_maps/maps/tray.dae
92+
[micp_localization_node-2] - meshes: 1
93+
[micp_localization_node-2] Cube-mesh
94+
[micp_localization_node-2] - vertices, faces: 30, 10
95+
[micp_localization_node-2] For more infos enter in terminal:
96+
[micp_localization_node-2] $ rmagine_map_info /home/amock/rmcl_ws/install/rmcl_examples_maps/share/rmcl_examples_maps/maps/tray.dae
97+
[micp_localization_node-2]
98+
[micp_localization_node-2] --------------------------
99+
[micp_localization_node-2] --- BACKENDS ---
100+
[micp_localization_node-2] --------------------------
101+
[micp_localization_node-2] Available combining units:
102+
[micp_localization_node-2] - CPU
103+
[micp_localization_node-2] Available raytracing backends:
104+
[micp_localization_node-2] - Embree (CPU)
105+
[micp_localization_node-2]
106+
[micp_localization_node-2] -------------------------
107+
[micp_localization_node-2] --- FRAMES ---
108+
[micp_localization_node-2] -------------------------
109+
[micp_localization_node-2] - base: base_footprint
110+
[micp_localization_node-2] - odom: odom
111+
[micp_localization_node-2] - map: map
112+
[micp_localization_node-2] Estimating: base_footprint -> map
113+
[micp_localization_node-2] Providing: odom -> map
114+
[micp_localization_node-2]
115+
[micp_localization_node-2] -------------------------
116+
[micp_localization_node-2] --- SENSORS ---
117+
[micp_localization_node-2] -------------------------
118+
[micp_localization_node-2] - lidar3d
119+
[micp_localization_node-2] - data: topic
120+
[micp_localization_node-2] - topic: /rmcl_inputs/lidar3d
121+
[micp_localization_node-2] - frame: velodyne
122+
[micp_localization_node-2] - model: o1dn
123+
[micp_localization_node-2] - correspondences:
124+
[micp_localization_node-2] - backend: embree
125+
[micp_localization_node-2] - type: RC
126+
[micp_localization_node-2] - metric: P2L
127+
[micp_localization_node-2] MICP load params - done. Valid Sensors: 1
128+
[micp_localization_node-2] [INFO] [1747438141.203392843] [rmcl_micpl]: Waiting for 'odom' frame to become available ...
129+
[micp_localization_node-2] Waiting for pose...
133130
```
134-
135131
</details>
136132

137-
At startup, MICP does a few sanity checks for the input parameters.
138-
Every possible mistake in configuration can then be inferred by this output.
139-
For example, once there is no data available on the given `PointCloud2`-Topic it will print `data: no` instead.
140-
141-
142-
### Parameters
143-
144-
The following sections describe example configuration files.
145-
More example files for configuration are placed in the `rmcl_ros/config/examples`.
146-
147-
### Parameters - 3D LiDAR only - CPU
148-
149-
MICP Localization using a 3D LiDAR and doing the MICP steps completely on the CPU.
150-
Here the 3D LiDAR is a Velodyne VLP-16 with 16 scan lines.
151-
The horizontal number of points is reduced to 440 and might be adjusted for your own Velodyne.
152-
153-
<details>
154-
<summary>File: `rmcl_ros/config/examples/micp_velodyne_cpu.yaml` </summary>
155-
156-
```yaml
157-
# required
158-
base_frame: base_footprint
159-
map_frame: map
160-
odom_frame: odom
161-
162-
# rate of broadcasting tf transformations
163-
tf_rate: 50.0
164-
invert_tf: False
165-
166-
micp:
167-
# merging on gpu or cpu
168-
combining_unit: cpu
169-
# maximum number of correction steps per second
170-
# lower this to decrease the correction speed but save energy
171-
corr_rate_max: 1000.0
172-
print_corr_rate: False
173-
174-
# adjust max distance dependend of the state of localization
175-
# helps to continuously disregard objects that not exist in the map
176-
adaptive_max_dist: True # enable adaptive max dist
177-
178-
# offset added to inital pose guess
179-
trans: [0.0, 0.0, 0.0]
180-
rot: [0.0, 0.0, 0.0] # euler angles (3) or quaternion (4)
181-
182-
# describe your sensor setup here
183-
sensors: # list of range sensors - at least one is required
184-
velodyne:
185-
topic: velodyne_points
186-
# spherical is comparable to sensor_msgs::msg::LaserScan
187-
# but in 3D
188-
type: spherical
189-
model:
190-
range_min: 0.5
191-
range_max: 130.0
192-
phi_min: -0.261799067259
193-
phi_inc: 0.03490658503988659
194-
phi_n: 16
195-
theta_min: -3.14159011841
196-
theta_inc: 0.01431249500496489
197-
theta_n: 440
198-
micp:
199-
max_dist: 1.0
200-
# Once adaptive_max_dist is set to true:
201-
#
202-
# If the localization is perfect, the max
203-
# distance for finding SPCs is reduced to
204-
# `adaptive_max_dist_min`.
205-
# If the localization is bad, the max
206-
# distance for finding SPCs is raised to
207-
# `max_dist`
208-
adaptive_max_dist_min: 0.15
209-
backend: embree
210-
```
211-
212-
</details>
213133

214-
### Parameters - 2D LiDAR + Wheels - GPU
134+
After the node has been started, an initial pose estimate must be provided, eg, using the "2D Pose Estimate" tool in RViz, which publishes to the `/initialpose` topic.
135+
Make sure the fixed frame in RViz is set to match the map coordinate system.
215136

216-
MICP also supports to localize a robot only equipped with a 2D LiDAR in a 3D map.
217-
To correct the third dimension the wheels can be used to pull the robot towards the map's ground plane.
218-
Thus, you should only run it on a robot that always drives on the ground and e.g. cannot fly.
219-
In this example, all MICP steps are computed on GPU.
220-
The robot has four wheels of which the highest points are located relative to `base_footprint` as listed in `origs`.
221-
By setting a virtual scanner to the wheel positions scanning downwards with a constant scanning range equal to the wheel diameter it is possible to pull the robot to the mesh.
137+
> Note: RMCL does not provide tools to visualize triangle mesh maps in RViz.
138+
> To view mesh maps, consider using the rviz_mesh_tools_plugins from the
139+
[mesh_tools](https://github.com/naturerobots/mesh_tools) repository.
222140

223-
<details>
224-
<summary>File: `rmcl_ros/config/examples/micp_sick_gpu.yaml`</summary>
225-
226-
```yaml
227-
# required
228-
base_frame: base_footprint
229-
map_frame: map
230-
odom_frame: odom
231-
232-
# rate of broadcasting tf transformations
233-
tf_rate: 50.0
234-
235-
micp:
236-
# merging on gpu or cpu
237-
combining_unit: gpu
238-
# maximum number of correction steps per second
239-
# lower this to decrease the correction speed but save energy
240-
corr_rate_max: 1000.0
241-
print_corr_rate: False
242-
243-
# adjust max distance dependend of the state of localization
244-
adaptive_max_dist: True # enable adaptive max dist
245-
246-
# offset added to initial pose guess
247-
trans: [0.0, 0.0, 0.0]
248-
rot: [0.0, 0.0, 0.0] # euler angles (3) or quaternion (4)
249-
250-
# describe your sensor setup here
251-
sensors: # list of range sensors - at least one is required
252-
sick:
253-
topic: scan
254-
micp:
255-
weight: 1.0
256-
backend: optix
257-
wheels: # pull robot to the mesh
258-
ranges: [0.2, 0.2, 0.2, 0.2]
259-
type: ondn
260-
frame: base_footprint
261-
model:
262-
width: 4
263-
height: 1
264-
range_min: 0.0
265-
range_max: 10.0
266-
origs: [[ 0.2, 0.15, 0.2], # front left
267-
[ 0.2, -0.15, 0.2], # front right
268-
[-0.2, 0.15, 0.2], # rear left
269-
[-0.2, -0.15, 0.2]] # rear right
270-
dirs: [[ 0.0, 0.0, -1.0],
271-
[ 0.0, 0.0, -1.0],
272-
[ 0.0, 0.0, -1.0],
273-
[ 0.0, 0.0, -1.0]]
274-
micp:
275-
max_dist: 1.0
276-
adaptive_max_dist_min: 0.2
277-
weight: 1.0
278-
backend: optix
279-
```
280-
</details>
141+
--> For an actual quick start, go to: [https://github.com/amock/rmcl_examples](https://github.com/amock/rmcl_examples)
281142

282143

283144
# RMCL - Project
284145

285146
## Installation
286147

287148
Dependencies:
288-
- Download and install [Rmagine](https://github.com/uos/rmagine) (v > 2.2.9): Compile from source (not debian packages).
289-
- Recommended: Install OptiX backend if NVIDIA GPU is available
290-
- For rmagine version >= 2.2.9 it is possible to put rmagine into your ROS workspace for easier compilation
291-
- ROS2 (check compatible branches)
149+
- ROS 2 (check compatible branches)
150+
- Download and put [Rmagine](https://github.com/uos/rmagine) (v >= 2.3.0) into your ROS workspace.
151+
- Recommended: Install OptiX backend if NVIDIA GPU is available.
152+
- Optional for functionality, but required for visualizations: [mesh_tools](https://github.com/naturerobots/mesh_tools).
292153

293154
Clone this repository into your ROS workspace and build it.
294155

295-
```bash
296-
$ colcon build
156+
```console
157+
colcon build
297158
```
298159

299-
Optional, but recommended: RMCL itself doesn't provide any tools to visualize the triangle mesh maps.
300-
If you want to see the map in RViz, use for example the `rviz_mesh_tools_plugins` of the [mesh_tools](https://github.com/naturerobots/mesh_tools).
301-
302-
## Examples
303-
304-
To learn how to use RMCL ROS nodes in your project, visit https://github.com/amock/rmcl_examples.
305-
306-
To learn how to use RMCL library in your Node: `src/nodes/examples`.
307-
308160
## Mesh Navigation
309161

310162
To navigate a robot automatically and safely through uneven terrain, the combination RMCL + Mesh Navigation Stack is very suitable: [https://github.com/naturerobots/mesh_navigation](https://github.com/naturerobots/mesh_navigation). As we presented on [ROSCon 2023](https://vimeo.com/879000775):
@@ -321,6 +173,17 @@ The planned Roadmap is as follows:
321173

322174
## News
323175

176+
### 2025-05-17: ROS 2-ify MICP-L - v2.2.0
177+
178+
After conducting real-world tests, we refactored the **MICP-L** node to better integrate it into the **ROS 2** ecosystem and added several new features:
179+
- Limited the possible inputs to *only* `rmcl_msgs`. Instead, we provide nodes and instructions to convert commonly used range sensor messages into `rmcl_msgs`.
180+
- MICP-L can now be launched as a composable node.
181+
- Separated correspondence search from optimization without losing much efficiency. This allowed us to add classic closest-point correspondences (CP), in addition to ray-casting correspondences (RC) (only available for embree backend).
182+
- Improved time synchronization between combinations of sensors and odometry.
183+
- Added many new examples and small demos for a quick start: [https://github.com/amock/rmcl_examples](https://github.com/amock/rmcl_examples)
184+
185+
> For the old version, download v2.1.0
186+
324187
### 2024-11-25: Restructuring - ROS 1 + ROS 2
325188

326189
We had to do minor structural changes to the repository in order to better integrate new features into RMCL. This repository is now devided into

0 commit comments

Comments
 (0)