Skip to content

Commit 084ffdf

Browse files
glpugaglpuga-ekumen
glpuga
authored and
glpuga-ekumen
committed
Update main testbench to run more sensor models and provide confidence intervals
Signed-off-by: Gerardo Puga <glpuga@gmail.com>
1 parent c560d6a commit 084ffdf

File tree

11 files changed

+1163
-251
lines changed

11 files changed

+1163
-251
lines changed

‎src/benchmarks/beluga_vs_nav2_multi_dataset/config/display.rviz‎

Lines changed: 458 additions & 0 deletions
Large diffs are not rendered by default.

‎src/benchmarks/beluga_vs_nav2_multi_dataset/launch/beluga_vs_nav2_multi_dataset.launch‎

Lines changed: 199 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -1,80 +1,227 @@
11
<launch>
2-
<arg name="map_filename"/>
2+
<arg name="map_filename" />
33
<arg name="global_frame_id" />
44
<arg name="odom_frame_id" />
55
<arg name="base_frame_id" />
66
<arg name="scan_topic" />
77

8-
<arg name="max_particles" default="2000"/>
9-
<arg name="min_particles" default="500"/>
10-
<arg name="laser_model_type" default="beam"/>
11-
<arg name="execution_policy" default="seq"/>
12-
<arg name="use_sim_time" default="true"/>
8+
<arg name="use_sim_time" default="true" />
139

14-
<arg name="initial_pose_x" default="0.0"/>
15-
<arg name="initial_pose_y" default="0.0"/>
16-
<arg name="initial_pose_yaw" default="0.0"/>
10+
<arg name="initial_pose_x" default="0.0" />
11+
<arg name="initial_pose_y" default="0.0" />
12+
<arg name="initial_pose_yaw" default="0.0" />
1713

1814
<arg name="robot_model_type" />
1915

20-
<!-- don't change this because it's hardcoded in the robotframework stuff and you too will waste time -->
16+
<!-- don't change this because it's hardcoded in the robotframework stuff -->
2117
<arg name="map_topic" default="map" />
2218

23-
<set_parameter name="use_sim_time" value="$(var use_sim_time)"/>
19+
<set_parameter name="use_sim_time" value="$(var use_sim_time)" />
2420

25-
<node pkg="nav2_amcl" exec="amcl" name="nav2_amcl" launch-prefix="$(env nav2_amcl_PREFIX '')" >
26-
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml"/>
27-
<param name="laser_model_type" value="$(var laser_model_type)"/>
28-
<param name="max_particles" value="$(var max_particles)"/>
29-
<param name="min_particles" value="$(var min_particles)"/>
21+
<node pkg="nav2_map_server" exec="map_server" name="map_server">
22+
<param name="yaml_filename" value="$(var map_filename)" />
23+
<remap from="map" to="$(var map_topic)" />
24+
</node>
25+
26+
<node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager">
27+
<param name="autostart" value="true" />
28+
<param name="node_names"
29+
value="[map_server, nav2_amcl_beam, beluga_amcl_beam, nav2_amcl_likelihood, nav2_amcl_likelihood_prob, nav2_amcl_likelihood_beam_skip, beluga_amcl_likelihood, beluga_amcl_likelihood_prob]" />
30+
</node>
31+
32+
<!-- nav2_amcl_beam -->
33+
34+
<node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_beam"
35+
launch-prefix="$(env nav2_amcl_beam_PREFIX '')">
36+
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
3037

31-
<param name="global_frame_id" value="$(var global_frame_id)"/>
32-
<param name="odom_frame_id" value="$(var odom_frame_id)"/>
33-
<param name="base_frame_id" value="$(var base_frame_id)"/>
34-
<param name="map_topic" value="$(var map_topic)"/>
35-
<param name="scan_topic" value="$(var scan_topic)"/>
38+
<param name="global_frame_id" value="$(var global_frame_id)" />
39+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
40+
<param name="base_frame_id" value="$(var base_frame_id)" />
41+
<param name="map_topic" value="$(var map_topic)" />
42+
<param name="scan_topic" value="$(var scan_topic)" />
3643

37-
<param name="initial_pose.x" value="$(var initial_pose_x)"/>
38-
<param name="initial_pose.y" value="$(var initial_pose_y)"/>
39-
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)"/>
44+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
45+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
46+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
4047

41-
<param name="robot_model_type" value="$(var robot_model_type)"/>
48+
<param name="robot_model_type" value="$(var robot_model_type)" />
4249

43-
<remap from="amcl_pose" to="/nav2_amcl/pose"/>
44-
<remap from="particle_cloud" to="/nav2_amcl/particle_cloud"/>
50+
<!-- we use this one for reference in case of failures -->
51+
<param name="tf_broadcast" value="true" />
52+
53+
<param name="laser_model_type" value="beam" />
54+
<param name="do_beamskip" value="false" />
55+
56+
<remap from="amcl_pose" to="/nav2_amcl_beam/pose" />
57+
<remap from="particle_cloud" to="/nav2_amcl_beam/particle_cloud" />
4558
</node>
4659

47-
<node pkg="beluga_amcl" exec="amcl_node" name="beluga_amcl" launch-prefix="$(env beluga_amcl_PREFIX '')" >
48-
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml"/>
49-
<param name="laser_model_type" value="$(var laser_model_type)"/>
50-
<param name="max_particles" value="$(var max_particles)"/>
51-
<param name="min_particles" value="$(var min_particles)"/>
52-
<param name="execution_policy" value="$(var execution_policy)"/>
60+
<!-- nav2_amcl_likelihood -->
61+
62+
<node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_likelihood"
63+
launch-prefix="$(env nav2_amcl_likelihood_PREFIX '')">
64+
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
65+
66+
<param name="global_frame_id" value="$(var global_frame_id)" />
67+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
68+
<param name="base_frame_id" value="$(var base_frame_id)" />
69+
<param name="map_topic" value="$(var map_topic)" />
70+
<param name="scan_topic" value="$(var scan_topic)" />
5371

54-
<param name="global_frame_id" value="$(var global_frame_id)"/>
55-
<param name="odom_frame_id" value="$(var odom_frame_id)"/>
56-
<param name="base_frame_id" value="$(var base_frame_id)"/>
57-
<param name="map_topic" value="$(var map_topic)"/>
58-
<param name="scan_topic" value="$(var scan_topic)"/>
72+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
73+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
74+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
5975

60-
<param name="initial_pose.x" value="$(var initial_pose_x)"/>
61-
<param name="initial_pose.y" value="$(var initial_pose_y)"/>
62-
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)"/>
76+
<param name="robot_model_type" value="$(var robot_model_type)" />
6377

64-
<param name="robot_model_type" value="$(var robot_model_type)"/>
78+
<!-- we use this one for reference in case of failures -->
79+
<param name="tf_broadcast" value="true" />
6580

66-
<remap from="pose" to="/beluga_amcl/pose"/>
67-
<remap from="particle_cloud" to="/beluga_amcl/particle_cloud"/>
68-
<remap from="particle_markers" to="/beluga_amcl/particle_markers"/>
81+
<param name="laser_model_type" value="likelihood_field" />
82+
<param name="do_beamskip" value="false" />
83+
84+
<remap from="amcl_pose" to="/nav2_amcl_likelihood/pose" />
85+
<remap from="particle_cloud" to="/nav2_amcl_likelihood/particle_cloud" />
6986
</node>
7087

71-
<node pkg="nav2_map_server" exec="map_server" name="map_server">
72-
<param name="yaml_filename" value="$(var map_filename)"/>
73-
<remap from="map" to="$(var map_topic)"/>
88+
<!-- nav2_amcl_likelihood_prob -->
89+
90+
<node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_likelihood_prob"
91+
launch-prefix="$(env nav2_amcl_likelihood_prob_PREFIX '')">
92+
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
93+
94+
<param name="global_frame_id" value="$(var global_frame_id)" />
95+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
96+
<param name="base_frame_id" value="$(var base_frame_id)" />
97+
<param name="map_topic" value="$(var map_topic)" />
98+
<param name="scan_topic" value="$(var scan_topic)" />
99+
100+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
101+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
102+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
103+
104+
<param name="robot_model_type" value="$(var robot_model_type)" />
105+
106+
<param name="laser_model_type" value="likelihood_field_prob" />
107+
<param name="do_beamskip" value="false" />
108+
109+
<remap from="amcl_pose" to="/nav2_amcl_likelihood_prob/pose" />
110+
<remap from="particle_cloud" to="/nav2_amcl_likelihood_prob/particle_cloud" />
74111
</node>
75112

76-
<node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager">
77-
<param name="autostart" value="true"/>
78-
<param name="node_names" value="[beluga_amcl, nav2_amcl, map_server]"/>
113+
<!-- nav2_amcl_likelihood_beam_skip -->
114+
115+
<node pkg="nav2_amcl" exec="amcl" name="nav2_amcl_likelihood_beam_skip"
116+
launch-prefix="$(env nav2_amcl_likelihood_beam_skip_PREFIX '')">
117+
<param
118+
from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
119+
120+
<param name="global_frame_id" value="$(var global_frame_id)" />
121+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
122+
<param name="base_frame_id" value="$(var base_frame_id)" />
123+
<param name="map_topic" value="$(var map_topic)" />
124+
<param name="scan_topic" value="$(var scan_topic)" />
125+
126+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
127+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
128+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
129+
130+
<param name="robot_model_type" value="$(var robot_model_type)" />
131+
132+
<param name="laser_model_type" value="likelihood_field_prob" />
133+
<param name="do_beamskip" value="true" />
134+
135+
<remap from="amcl_pose" to="/nav2_amcl_likelihood_beam_skip/pose" />
136+
<remap from="particle_cloud" to="/nav2_amcl_likelihood_beam_skip/particle_cloud" />
137+
</node>
138+
139+
<!-- beluga_amcl_beam -->
140+
141+
<node pkg="beluga_amcl" exec="amcl_node" name="beluga_amcl_beam"
142+
launch-prefix="$(env beluga_amcl_beam_PREFIX '')">
143+
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
144+
145+
<param name="global_frame_id" value="$(var global_frame_id)" />
146+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
147+
<param name="base_frame_id" value="$(var base_frame_id)" />
148+
<param name="map_topic" value="$(var map_topic)" />
149+
<param name="scan_topic" value="$(var scan_topic)" />
150+
151+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
152+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
153+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
154+
155+
<param name="robot_model_type" value="$(var robot_model_type)" />
156+
157+
<!-- we use this one for reference in case of failures -->
158+
<param name="tf_broadcast" value="true" />
159+
160+
<param name="laser_model_type" value="beam" />
161+
<param name="do_beamskip" value="false" />
162+
163+
<remap from="pose" to="/beluga_amcl_beam/pose"/>
164+
<remap from="particle_cloud" to="/beluga_amcl_beam/particle_cloud"/>
165+
<remap from="particle_markers" to="/beluga_amcl_beam/particle_markers"/>
79166
</node>
80-
</launch>
167+
168+
<!-- beluga_amcl_likelihood -->
169+
170+
<node pkg="beluga_amcl" exec="amcl_node" name="beluga_amcl_likelihood"
171+
launch-prefix="$(env beluga_amcl_likelihood_PREFIX '')">
172+
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
173+
174+
<param name="global_frame_id" value="$(var global_frame_id)" />
175+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
176+
<param name="base_frame_id" value="$(var base_frame_id)" />
177+
<param name="map_topic" value="$(var map_topic)" />
178+
<param name="scan_topic" value="$(var scan_topic)" />
179+
180+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
181+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
182+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
183+
184+
<param name="robot_model_type" value="$(var robot_model_type)" />
185+
186+
<param name="laser_model_type" value="likelihood_field" />
187+
<param name="do_beamskip" value="false" />
188+
189+
<remap from="pose" to="/beluga_amcl_likelihood/pose"/>
190+
<remap from="particle_cloud" to="/beluga_amcl_likelihood/particle_cloud"/>
191+
<remap from="particle_markers" to="/beluga_amcl_likelihood/particle_markers"/>
192+
</node>
193+
194+
<!-- beluga_amcl_likelihood_prob -->
195+
196+
<node pkg="beluga_amcl" exec="amcl_node" name="beluga_amcl_likelihood_prob"
197+
launch-prefix="$(env beluga_amcl_likelihood_prob_PREFIX '')">
198+
<param from="$(find-pkg-share beluga_vs_nav2_multi_dataset)/params/amcl.yaml" />
199+
200+
<param name="global_frame_id" value="$(var global_frame_id)" />
201+
<param name="odom_frame_id" value="$(var odom_frame_id)" />
202+
<param name="base_frame_id" value="$(var base_frame_id)" />
203+
<param name="map_topic" value="$(var map_topic)" />
204+
<param name="scan_topic" value="$(var scan_topic)" />
205+
206+
<param name="initial_pose.x" value="$(var initial_pose_x)" />
207+
<param name="initial_pose.y" value="$(var initial_pose_y)" />
208+
<param name="initial_pose.yaw" value="$(var initial_pose_yaw)" />
209+
210+
<param name="robot_model_type" value="$(var robot_model_type)" />
211+
212+
<param name="laser_model_type" value="likelihood_field_prob" />
213+
<param name="do_beamskip" value="false" />
214+
215+
<remap from="pose" to="/beluga_amcl_likelihood_prob/pose"/>
216+
<remap from="particle_cloud" to="/beluga_amcl_likelihood_prob/particle_cloud"/>
217+
<remap from="particle_markers" to="/beluga_amcl_likelihood_prob/particle_markers"/>
218+
</node>
219+
220+
<!-- launch rviz -->
221+
222+
<!-- <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen"
223+
args="-d $(find-pkg-share beluga_vs_nav2_multi_dataset)/config/display.rviz">
224+
<param name="use_sim_time" value="$(var use_sim_time)" />
225+
</node> -->
226+
227+
</launch>

‎src/benchmarks/beluga_vs_nav2_multi_dataset/package.xml‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,14 +20,16 @@
2020
<exec_depend>python3-tk</exec_depend>
2121

2222
<exec_depend>nav2_amcl</exec_depend>
23+
<exec_depend>beluga_amcl</exec_depend>
2324
<exec_depend>nav2_lifecycle_manager</exec_depend>
2425
<exec_depend>nav2_map_server</exec_depend>
2526

26-
<exec_depend>beluga_amcl</exec_depend>
27-
2827
<exec_depend>rosbag2_storage_default_plugins</exec_depend>
2928
<exec_depend>rosbag2_storage_mcap</exec_depend>
3029

30+
<exec_depend>rviz2</exec_depend>
31+
<exec_depend>nav2_rviz_plugins</exec_depend>
32+
3133
<export>
3234
<build_type>ament_cmake</build_type>
3335
</export>

‎src/benchmarks/beluga_vs_nav2_multi_dataset/reports/nominal_report/sections/datasets/diff_drive_sim_60m_description.inc‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
Dataset information
2+
^^^^^^^^^^^^^^^^^^^
3+
4+
This is a synthetic dataset created recording a simulated robot wandering around a
5+
:math:`450 \mathrm{m}^2` office environment for 24 hours.
6+
7+
The simulated diff-drive robot is modelled after a `Kobuki platform <https://iclebo-kobuki.readthedocs.io/en/latest/about.html>`_ featuring an `RPLidar A1 <https://www.slamtec.com/en/Lidar/A1Spec>`_, an 8m range 2D lidar. The
8+
simulation was performed using Gazebo Classic.
9+
10+
The dataset was recorded in a single run, and it consists of a single continuous bagfile with a duration of 24 hours.
11+
During this time the robot is periodically sent navigation goals to reachable locations to ensure it keeps moving
12+
around the environment.
13+
14+
The robot covers approximately 24 km during those 24 hours, with an average speed of about 0.28 m/s.
15+
16+
The robot model was configured to introduce small imperfections in the odometry to cause a small amount of drift and
17+
emulate conditions closer to real-world.
18+
19+
The simulated environment is populated with static obstacles (furniture), but the localization map intentionally
20+
excludes them to evaluate the systems under test in a condition where unmapped obstacles perturb the estimation.
21+
22+
The localization map used to evaluate the localization performance with this dataset is the one shown below.
23+
24+
.. figure:: assets/representative_diff_drive_sim_24hs_map.png
25+
:scale: 99 %
26+
27+
Localization map used to evaluate the localization performance with the Diff Drive Sim 60m dataset.
28+
29+
The simulated environment used to record the Diff Drive Sim 60m dataset is shown below.
30+
31+
.. figure:: assets/hq_simulated_environment.jpg
32+
:scale: 99 %
33+
34+
The simulated environment used to record the Diff Drive Sim 60m dataset.

‎src/benchmarks/beluga_vs_nav2_multi_dataset/reports/nominal_report/sections/datasets/omni_drive_sim_30m_description.inc‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
Dataset information
2+
^^^^^^^^^^^^^^^^^^^
3+
4+
This is a synthetic dataset created recording a simulated omni-drive robot wandering around the
5+
`AWS Robomaker Bookstore World <https://github.com/aws-robotics/aws-robomaker-bookstore-world>`_ for 24 hours.
6+
7+
The simulated omni-drive robot is modelled after a customized `Robomaster EP <https://www.dji.com/global/robomaster-ep>`_ featuring an `RPLidar A2 <https://www.slamtec.com/en/Lidar/A2Spec>`_, a 12m range 2D lidar.
8+
The simulation was performed using Gazebo Sim.
9+
10+
The dataset was recorded in a single run, and it consists of a single continuous bagfile with a duration of 24 hours.
11+
During this time the robot is periodically sent navigation goals to reachable locations within the map to ensure it
12+
keeps moving.
13+
14+
The robot covers approximately 45 km during those 24 hours, with an average speed of about 0.5 m/s.
15+
16+
The robot model was configured to introduce small imperfections in the odometry to cause a small amount of drift and
17+
emulate conditions closer to real-world.
18+
19+
Most but not all obstacles and furniture are mapped, and one of the walls is windowed and therefore
20+
invisible to the simulated lidar.
21+
22+
The localization world used to evaluate the localization performance with this dataset is the one shown below.
23+
24+
.. figure:: assets/representative_omni_drive_sim_24hs_map.png
25+
:scale: 99 %
26+
27+
Localization world used to evaluate the localization performance with the Omni Drive Sim 30m dataset.
28+
29+
The simulated world used to record the Omni Drive Sim 30m dataset is shown below. It differs from the localization world in that it is populated with obstacles and furniture.
30+
in that a few of the obstacles and furniture present in the simulated world are not present.
31+
32+
.. figure:: assets/bookstore_simulated_world.png
33+
:scale: 99 %
34+
35+
The simulated environment used to record the Omni Drive Sim 30m dataset.

0 commit comments

Comments
 (0)