Skip to content

Commit 6fcabed

Browse files
amockCakem1x
amock
and
Cakem1x
authored
RMCL MICP-L for map-based localization (#12)
* added launch file to start MICP-L from the RMCL repo as another localization approach * better parameters * added flipped model so that the lidar is simulated on the ceiling as well * better params * Update mesh_navigation_tutorials/config/rmcl_micpl.yaml Co-authored-by: Matthias Holoch <mholoch@gmail.com> * Update mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py Co-authored-by: Matthias Holoch <mholoch@gmail.com> --------- Co-authored-by: Matthias Holoch <mholoch@gmail.com>
1 parent 81d9c97 commit 6fcabed

File tree

7 files changed

+224
-95
lines changed

7 files changed

+224
-95
lines changed

‎mesh_navigation_tutorials/config/mbf_mesh_nav.yaml

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ move_base_flex:
6161
inflation:
6262
type: 'mesh_layers/InflationLayer'
6363
factor: 1.0
64-
inflation_radius: 0.2
65-
inscribed_radius: 0.4
64+
inflation_radius: 0.5
65+
inscribed_radius: 0.7
6666
lethal_value: 1.0
67-
inscribed_value: 0.8
67+
inscribed_value: 0.6
6868
repulsive_field: false

‎mesh_navigation_tutorials/config/rmcl_micpl.yaml

Lines changed: 50 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,44 +1,62 @@
1-
micp_localization:
1+
# Parameters for point cloud conversion node
2+
# it converts a sensor_msgs/PointCloud2 to a rmcl_msgs/O1DnStamped
3+
# simple fitering can be achieved by skipping rows or cols
4+
rmcl_lidar3d_conversion:
25
ros__parameters:
6+
use_sim_time: True
7+
debug_cloud: True
8+
model:
9+
range_min: 0.3
10+
range_max: 50.0
11+
# filter / pre-processing:
12+
width:
13+
skip_begin: 0 # skip elements from begin
14+
skip_end: 0 # skip elements from end
15+
increment: 1 # increment to skip data. Simple filter to reduce scan density
16+
height:
17+
skip_begin: 0 # cutting height from begin
18+
skip_end: 0 # cutting height from end
19+
increment: 1 # increment to skip data. Simple filter to reduce scan density
320

4-
# required
21+
# Parameters for the MICP-L node
22+
rmcl_micpl:
23+
ros__parameters:
24+
25+
# required frames
526
base_frame: base_footprint
627
map_frame: map
728
odom_frame: odom
829

9-
# rate of broadcasting tf transformations
10-
tf_rate: 50.0
30+
# maximum number of correction steps per second
31+
# lower this to decrease the correction speed but save energy
32+
correction_rate_max: 50.0
33+
disable_correction: False
34+
optimization_iterations: 10
35+
tf_time_source: 1 # 0: measurement_latest, 1: correction_latest
36+
tf_rate: 100.0
37+
broadcast_tf: True
38+
publish_pose: True
39+
pose_noise: 0.01 # minimum noise of pose. can be set from sensor noise
1140

12-
micp:
13-
# merging on gpu or cpu
14-
combining_unit: cpu
15-
# maximum number of correction steps per second
16-
# lower this to decrease the correction speed but save energy
17-
corr_rate_max: 20.0
18-
19-
# adjust max distance dependend of the state of localization
20-
adaptive_max_dist: True # enable adaptive max dist
41+
# adjust max distance dependent of the state of localization
42+
adaptive_max_dist: True # enable adaptive max dist
2143

22-
# DEBUGGING
23-
# corr = correspondences
24-
viz_corr: True
25-
# corr = correction
26-
print_corr_rate: False
27-
disable_corr: False
44+
# initial offset applied to every incoming pose guess
45+
pose_guess_offset: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # translation + euler angles (6) or translation + quaternion (7)
2846

29-
# initial pose changes
30-
trans: [0.0, 0.0, 0.0]
31-
rot: [0.0, 0.0, 0.0] # euler angles (3) or quaternion (4)
47+
# very first initial pose guess
48+
initial_pose_guess: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # translation + euler angles (6) or translation + quaternion (7)
3249

3350
# describe your sensor setup here
3451
sensors: # list of range sensors - at least one is required
35-
laser3d:
36-
topic: cloud
37-
topic_type: sensor_msgs/msg/PointCloud2
38-
# normally it could also be a more memory-friendly spherical sensor model.
39-
# However, I dont trust the Gazebo sensor
40-
type: o1dn
41-
model:
42-
range_min: 0.5
43-
range_max: 130.0
44-
orig: [0.0, 0.0, 0.0]
52+
lidar3d:
53+
data_source: topic
54+
model_type: o1dn
55+
topic_name: /rmcl_inputs/cloud
56+
correspondences:
57+
backend: embree
58+
type: RC
59+
metric: P2L
60+
adaptive_max_dist_min: 0.03
61+
max_dist: 0.3
62+
visualize: True

‎mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py

Lines changed: 16 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -41,10 +41,12 @@
4141

4242

4343
def generate_launch_description():
44-
# path to this pkg
44+
# path to sim pkg
4545
pkg_mesh_navigation_tutorials_sim = get_package_share_directory(
4646
"mesh_navigation_tutorials_sim"
4747
)
48+
49+
# path to this pkg
4850
pkg_mesh_navigation_tutorials = get_package_share_directory("mesh_navigation_tutorials")
4951

5052
# Comment Alex: One can have different maps for same worlds
@@ -132,29 +134,19 @@ def generate_launch_description():
132134
)
133135

134136
# RMCL Localization
135-
map_loc_rmcl_micp = Node(
136-
package="rmcl",
137-
executable="micp_localization",
138-
name="micp_localization",
139-
output="screen",
140-
remappings=[
141-
("pose_wc", "/initialpose"),
142-
],
143-
parameters=[
144-
{
145-
"use_sim_time": True,
146-
"map_file": PathJoinSubstitution(
147-
[
148-
pkg_mesh_navigation_tutorials,
149-
"maps",
150-
PythonExpression(['"', map_name, '" + ".dae"']),
151-
]
152-
),
153-
},
154-
PathJoinSubstitution([pkg_mesh_navigation_tutorials, "config", "rmcl_micpl.yaml"]),
155-
],
137+
138+
map_loc_rmcl_micpl = IncludeLaunchDescription(
139+
PythonLaunchDescriptionSource(
140+
PathJoinSubstitution(
141+
[pkg_mesh_navigation_tutorials, "launch", "rmcl_micpl_launch.py"]
142+
)
143+
),
144+
launch_arguments={
145+
"map_name": map_name
146+
}.items(),
156147
condition=IfCondition(PythonExpression(['"', localization_type, '" == "rmcl_micpl"'])),
157-
)
148+
)
149+
158150

159151
# Move Base Flex
160152
move_base_flex = IncludeLaunchDescription(
@@ -195,7 +187,7 @@ def generate_launch_description():
195187
simulation,
196188
ekf,
197189
map_loc_gt,
198-
map_loc_rmcl_micp,
190+
map_loc_rmcl_micpl,
199191
move_base_flex,
200192
rviz,
201193
]

‎mesh_navigation_tutorials/launch/rmcl_micpl_launch.py

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
# Copyright 2024 Nature Robots GmbH
2+
#
3+
# Redistribution and use in source and binary forms, with or without
4+
# modification, are permitted provided that the following conditions are met:
5+
#
6+
# * Redistributions of source code must retain the above copyright
7+
# notice, this list of conditions and the following disclaimer.
8+
#
9+
# * Redistributions in binary form must reproduce the above copyright
10+
# notice, this list of conditions and the following disclaimer in the
11+
# documentation and/or other materials provided with the distribution.
12+
#
13+
# * Neither the name of the Nature Robots GmbH nor the names of its
14+
# contributors may be used to endorse or promote products derived from
15+
# this software without specific prior written permission.
16+
#
17+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
# POSSIBILITY OF SUCH DAMAGE.
28+
29+
30+
import os
31+
32+
from ament_index_python.packages import get_package_share_directory
33+
34+
from launch import LaunchDescription
35+
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
36+
from launch.conditions import IfCondition
37+
from launch.substitutions import PathJoinSubstitution, PythonExpression, LaunchConfiguration
38+
from launch.launch_description_sources import PythonLaunchDescriptionSource
39+
40+
from launch_ros.actions import Node
41+
42+
43+
def generate_launch_description():
44+
45+
# path to this pkg
46+
pkg_mesh_navigation_tutorials = get_package_share_directory("mesh_navigation_tutorials")
47+
48+
# Loading a map files with the following extension
49+
mesh_nav_map_ext = ".ply"
50+
51+
available_map_names = [
52+
f[:-len(mesh_nav_map_ext)]
53+
for f in os.listdir(os.path.join(pkg_mesh_navigation_tutorials, "maps"))
54+
if f.endswith(mesh_nav_map_ext)
55+
]
56+
57+
# Launch arguments
58+
launch_args = [
59+
DeclareLaunchArgument(
60+
"map_name",
61+
description="Name of the map to be used for navigation"
62+
+ '(see mesh_navigation_tutorials\' "maps" directory).',
63+
default_value=LaunchConfiguration("world_name"),
64+
choices=available_map_names,
65+
),
66+
DeclareLaunchArgument(
67+
"localization_type",
68+
description="How the robot shall localize itself",
69+
default_value="ground_truth",
70+
choices=["ground_truth", "rmcl_micpl"],
71+
),
72+
DeclareLaunchArgument(
73+
"start_rviz",
74+
description="Whether rviz shall be started.",
75+
default_value="True",
76+
choices=["True", "False"],
77+
),
78+
]
79+
80+
map_name = LaunchConfiguration("map_name")
81+
82+
rmcl_micpl_config = PathJoinSubstitution([
83+
pkg_mesh_navigation_tutorials,
84+
"config",
85+
"rmcl_micpl.yaml"])
86+
87+
mesh_map_path = PathJoinSubstitution([
88+
pkg_mesh_navigation_tutorials,
89+
"maps",
90+
PythonExpression(['"', map_name, '.ply"']),
91+
])
92+
93+
94+
# conversion
95+
pc2_to_o1dn_conversion = Node(
96+
package="rmcl_ros",
97+
executable="conv_pc2_to_o1dn_node",
98+
name="rmcl_lidar3d_conversion",
99+
output="screen",
100+
remappings=[
101+
("input", "/cloud"),
102+
("output", "/rmcl_inputs/cloud"),
103+
],
104+
parameters=[
105+
rmcl_micpl_config,
106+
{
107+
"use_sim_time": True
108+
},
109+
],
110+
)
111+
112+
113+
# MICP-L (Mesh ICP localization) from RMCL package
114+
micpl = Node(
115+
package="rmcl_ros",
116+
executable="micp_localization_node",
117+
name="rmcl_micpl",
118+
output="screen",
119+
parameters=[
120+
rmcl_micpl_config,
121+
{
122+
"use_sim_time": True,
123+
"map_file": mesh_map_path
124+
},
125+
],
126+
)
127+
128+
return LaunchDescription(
129+
launch_args
130+
+ [
131+
pc2_to_o1dn_conversion,
132+
micpl
133+
]
134+
)
135+

‎mesh_navigation_tutorials_sim/launch/ground_truth_localization_launch.xml

Lines changed: 0 additions & 34 deletions
This file was deleted.

‎mesh_navigation_tutorials_sim/models/parking_garage/model.sdf

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,5 +20,23 @@
2020
</geometry>
2121
</visual>
2222
</link>
23+
<link name="link_flipped">
24+
<collision name="collision">
25+
<geometry>
26+
<mesh>
27+
<scale>1 1 1</scale>
28+
<uri>meshes/parking_garage_flipped.dae</uri>
29+
</mesh>
30+
</geometry>
31+
</collision>
32+
<visual name="visual">
33+
<geometry>
34+
<mesh>
35+
<scale>1 1 1</scale>
36+
<uri>meshes/parking_garage_flipped.dae</uri>
37+
</mesh>
38+
</geometry>
39+
</visual>
40+
</link>
2341
</model>
2442
</sdf>

‎mesh_navigation_tutorials_sim/urdf/ceres.urdf.xacro

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -234,8 +234,8 @@
234234
<vertical>
235235
<samples>32</samples>
236236
<resolution>0.01</resolution>
237-
<min_angle>${-M_PI / 16.0}</min_angle>
238-
<max_angle>${M_PI / 4.0}</max_angle>
237+
<min_angle>${-32.0 * M_PI / 180.0}</min_angle>
238+
<max_angle>${32.0 * M_PI / 180.0}</max_angle>
239239
</vertical>
240240
</scan>
241241
<range>

0 commit comments

Comments
 (0)