From d75c6958bb72601387978d5cda5b55192cd79d92 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 2 Jul 2025 22:17:25 +0200 Subject: [PATCH 1/6] added launch file to start MICP-L from the RMCL repo as another localization approach --- .../config/rmcl_micpl.yaml | 82 ++++++----- .../launch/mesh_navigation_tutorial_launch.py | 63 ++++---- .../launch/rmcl_micpl_launch.py | 135 ++++++++++++++++++ .../ground_truth_localization_launch.xml | 34 ----- 4 files changed, 224 insertions(+), 90 deletions(-) create mode 100644 mesh_navigation_tutorials/launch/rmcl_micpl_launch.py delete mode 100644 mesh_navigation_tutorials_sim/launch/ground_truth_localization_launch.xml diff --git a/mesh_navigation_tutorials/config/rmcl_micpl.yaml b/mesh_navigation_tutorials/config/rmcl_micpl.yaml index 3dc0044..38a51e9 100644 --- a/mesh_navigation_tutorials/config/rmcl_micpl.yaml +++ b/mesh_navigation_tutorials/config/rmcl_micpl.yaml @@ -1,44 +1,62 @@ -micp_localization: +# Parameters for point cloud conversion node +# it converts a sensor_msgs/PointCloud2 to a rmcl_msgs/O1DnStamped +# simple fitering can be achieved by skipping rows or cols +rmcl_lidar3d_conversion: ros__parameters: + use_sim_time: True + debug_cloud: True + model: + range_min: 0.3 + range_max: 80.0 + # filter / pre-processing: + width: + skip_begin: 0 # skip elements from begin + skip_end: 0 # skip elements from end + increment: 4 # increment to skip data. Simple filter to reduce scan density + height: + skip_begin: 0 # cutting height from begin + skip_end: 0 # cutting height from end + increment: 1 # increment to skip data. Simple filter to reduce scan density - # required +# Parameters for the MICP-L node +rmcl_micpl: + ros__parameters: + + # required frames base_frame: base_footprint map_frame: map odom_frame: odom - # rate of broadcasting tf transformations - tf_rate: 50.0 + # maximum number of correction steps per second + # lower this to decrease the correction speed but save energy + disable_correction: False + correction_rate_max: 20.0 + optimization_iterations: 5 + tf_time_source: 1 # 0: measurement_latest, 1: correction_latest + tf_rate: 100.0 + broadcast_tf: True + publish_pose: True + pose_noise: 0.01 # minimum noise of pose. can be set from sensor noise - micp: - # merging on gpu or cpu - combining_unit: cpu - # maximum number of correction steps per second - # lower this to decrease the correction speed but save energy - corr_rate_max: 20.0 - - # adjust max distance dependend of the state of localization - adaptive_max_dist: True # enable adaptive max dist + # adjust max distance dependent of the state of localization + adaptive_max_dist: True # enable adaptive max dist - # DEBUGGING - # corr = correspondences - viz_corr: True - # corr = correction - print_corr_rate: False - disable_corr: False + # initial offset applied to every incoming pose guess + pose_guess_offset: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # translation + euler angles (6) or translation + quaternion (7) - # initial pose changes - trans: [0.0, 0.0, 0.0] - rot: [0.0, 0.0, 0.0] # euler angles (3) or quaternion (4) + # very first initial pose guess + initial_pose_guess: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # translation + euler angles (6) or translation + quaternion (7) # describe your sensor setup here sensors: # list of range sensors - at least one is required - laser3d: - topic: cloud - topic_type: sensor_msgs/msg/PointCloud2 - # normally it could also be a more memory-friendly spherical sensor model. - # However, I dont trust the Gazebo sensor - type: o1dn - model: - range_min: 0.5 - range_max: 130.0 - orig: [0.0, 0.0, 0.0] \ No newline at end of file + lidar3d: + data_source: topic + model_type: o1dn + topic_name: /rmcl_inputs/cloud + correspondences: + backend: embree + type: RC + metric: P2L + adaptive_max_dist_min: 0.5 + max_dist: 2.0 + visualize: True diff --git a/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py b/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py index 6684ea7..9dcf896 100644 --- a/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py +++ b/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py @@ -41,10 +41,12 @@ def generate_launch_description(): - # path to this pkg + # path to sim pkg pkg_mesh_navigation_tutorials_sim = get_package_share_directory( "mesh_navigation_tutorials_sim" ) + + # path to this pkg pkg_mesh_navigation_tutorials = get_package_share_directory("mesh_navigation_tutorials") # Comment Alex: One can have different maps for same worlds @@ -132,29 +134,42 @@ def generate_launch_description(): ) # RMCL Localization - map_loc_rmcl_micp = Node( - package="rmcl", - executable="micp_localization", - name="micp_localization", - output="screen", - remappings=[ - ("pose_wc", "/initialpose"), - ], - parameters=[ - { - "use_sim_time": True, - "map_file": PathJoinSubstitution( - [ - pkg_mesh_navigation_tutorials, - "maps", - PythonExpression(['"', map_name, '" + ".dae"']), - ] - ), - }, - PathJoinSubstitution([pkg_mesh_navigation_tutorials, "config", "rmcl_micpl.yaml"]), - ], + + map_loc_rmcl_micpl = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [pkg_mesh_navigation_tutorials, "launch", "rmcl_micpl_launch.py"] + ) + ), + launch_arguments={ + "map_name": map_name + }.items(), condition=IfCondition(PythonExpression(['"', localization_type, '" == "rmcl_micpl"'])), - ) + ) + + # map_loc_rmcl_micpl = Node( + # package="rmcl_ros", + # executable="micp_localization", + # name="micp_localization", + # output="screen", + # remappings=[ + # ("pose_wc", "/initialpose"), + # ], + # parameters=[ + # { + # "use_sim_time": True, + # "map_file": PathJoinSubstitution( + # [ + # pkg_mesh_navigation_tutorials, + # "maps", + # PythonExpression(['"', map_name, '" + ".dae"']), + # ] + # ), + # }, + # PathJoinSubstitution([pkg_mesh_navigation_tutorials, "config", "rmcl_micpl.yaml"]), + # ], + # condition=IfCondition(PythonExpression(['"', localization_type, '" == "rmcl_micpl"'])), + # ) # Move Base Flex move_base_flex = IncludeLaunchDescription( @@ -195,7 +210,7 @@ def generate_launch_description(): simulation, ekf, map_loc_gt, - map_loc_rmcl_micp, + map_loc_rmcl_micpl, move_base_flex, rviz, ] diff --git a/mesh_navigation_tutorials/launch/rmcl_micpl_launch.py b/mesh_navigation_tutorials/launch/rmcl_micpl_launch.py new file mode 100644 index 0000000..44364a7 --- /dev/null +++ b/mesh_navigation_tutorials/launch/rmcl_micpl_launch.py @@ -0,0 +1,135 @@ +# Copyright 2024 Nature Robots GmbH +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the Nature Robots GmbH nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import PathJoinSubstitution, PythonExpression, LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node + + +def generate_launch_description(): + + # path to this pkg + pkg_mesh_navigation_tutorials = get_package_share_directory("mesh_navigation_tutorials") + + # Loading a map files with the following extension + mesh_nav_map_ext = ".ply" + + available_map_names = [ + f[:-len(mesh_nav_map_ext)] + for f in os.listdir(os.path.join(pkg_mesh_navigation_tutorials, "maps")) + if f.endswith(mesh_nav_map_ext) + ] + + # Launch arguments + launch_args = [ + DeclareLaunchArgument( + "map_name", + description="Name of the map to be used for navigation" + + '(see mesh_navigation_tutorials\' "maps" directory).', + default_value=LaunchConfiguration("world_name"), + choices=available_map_names, + ), + DeclareLaunchArgument( + "localization_type", + description="How the robot shall localize itself", + default_value="ground_truth", + choices=["ground_truth", "rmcl_micpl"], + ), + DeclareLaunchArgument( + "start_rviz", + description="Whether rviz shall be started.", + default_value="True", + choices=["True", "False"], + ), + ] + + map_name = LaunchConfiguration("map_name") + + rmcl_micpl_config = PathJoinSubstitution([ + pkg_mesh_navigation_tutorials, + "config", + "rmcl_micpl.yaml"]) + + mesh_map_path = PathJoinSubstitution([ + pkg_mesh_navigation_tutorials, + "maps", + PythonExpression(['"', map_name, '.ply"']), + ]) + + + # conversion + pc2_to_o1dn_conversion = Node( + package="rmcl_ros", + executable="conv_pc2_to_o1dn_node", + name="rmcl_lidar3d_conversion", + output="screen", + remappings=[ + ("input", "/cloud"), + ("output", "/rmcl_inputs/cloud"), + ], + parameters=[ + rmcl_micpl_config, + { + "use_sim_time": True + }, + ], + ) + + + # MICP-L (Mesh ICP localization) from RMCL package + micpl = Node( + package="rmcl_ros", + executable="micp_localization_node", + name="rmcl_micpl", + output="screen", + parameters=[ + rmcl_micpl_config, + { + "use_sim_time": True, + "map_file": mesh_map_path + }, + ], + ) + + return LaunchDescription( + launch_args + + [ + pc2_to_o1dn_conversion, + micpl + ] + ) + diff --git a/mesh_navigation_tutorials_sim/launch/ground_truth_localization_launch.xml b/mesh_navigation_tutorials_sim/launch/ground_truth_localization_launch.xml deleted file mode 100644 index 53b91ac..0000000 --- a/mesh_navigation_tutorials_sim/launch/ground_truth_localization_launch.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file From b947313622e2c354d8d6fd5e674ac181fd8392ae Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 2 Jul 2025 22:34:44 +0200 Subject: [PATCH 2/6] better parameters --- mesh_navigation_tutorials/config/rmcl_micpl.yaml | 8 ++++---- mesh_navigation_tutorials_sim/urdf/ceres.urdf.xacro | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mesh_navigation_tutorials/config/rmcl_micpl.yaml b/mesh_navigation_tutorials/config/rmcl_micpl.yaml index 38a51e9..621bd75 100644 --- a/mesh_navigation_tutorials/config/rmcl_micpl.yaml +++ b/mesh_navigation_tutorials/config/rmcl_micpl.yaml @@ -7,12 +7,12 @@ rmcl_lidar3d_conversion: debug_cloud: True model: range_min: 0.3 - range_max: 80.0 + range_max: 50.0 # filter / pre-processing: width: skip_begin: 0 # skip elements from begin skip_end: 0 # skip elements from end - increment: 4 # increment to skip data. Simple filter to reduce scan density + increment: 1 # increment to skip data. Simple filter to reduce scan density height: skip_begin: 0 # cutting height from begin skip_end: 0 # cutting height from end @@ -57,6 +57,6 @@ rmcl_micpl: backend: embree type: RC metric: P2L - adaptive_max_dist_min: 0.5 - max_dist: 2.0 + adaptive_max_dist_min: 0.03 + max_dist: 0.3 visualize: True diff --git a/mesh_navigation_tutorials_sim/urdf/ceres.urdf.xacro b/mesh_navigation_tutorials_sim/urdf/ceres.urdf.xacro index ad89f4d..48a044f 100644 --- a/mesh_navigation_tutorials_sim/urdf/ceres.urdf.xacro +++ b/mesh_navigation_tutorials_sim/urdf/ceres.urdf.xacro @@ -234,8 +234,8 @@ 32 0.01 - ${-M_PI / 16.0} - ${M_PI / 4.0} + ${-32.0 * M_PI / 180.0} + ${32.0 * M_PI / 180.0} From a7c0112ae727b420bf1304ccbebe751e4637a6e6 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 2 Jul 2025 22:53:19 +0200 Subject: [PATCH 3/6] added flipped model so that the lidar is simulated on the ceiling as well --- .../config/mbf_mesh_nav.yaml | 6 +++--- .../models/parking_garage/model.sdf | 18 ++++++++++++++++++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/mesh_navigation_tutorials/config/mbf_mesh_nav.yaml b/mesh_navigation_tutorials/config/mbf_mesh_nav.yaml index c984562..abdfd74 100644 --- a/mesh_navigation_tutorials/config/mbf_mesh_nav.yaml +++ b/mesh_navigation_tutorials/config/mbf_mesh_nav.yaml @@ -61,8 +61,8 @@ move_base_flex: inflation: type: 'mesh_layers/InflationLayer' factor: 1.0 - inflation_radius: 0.2 - inscribed_radius: 0.4 + inflation_radius: 0.5 + inscribed_radius: 0.7 lethal_value: 1.0 - inscribed_value: 0.8 + inscribed_value: 0.6 repulsive_field: false diff --git a/mesh_navigation_tutorials_sim/models/parking_garage/model.sdf b/mesh_navigation_tutorials_sim/models/parking_garage/model.sdf index d5ebcd5..3895af3 100644 --- a/mesh_navigation_tutorials_sim/models/parking_garage/model.sdf +++ b/mesh_navigation_tutorials_sim/models/parking_garage/model.sdf @@ -20,5 +20,23 @@ + + + + + 1 1 1 + meshes/parking_garage_flipped.dae + + + + + + + 1 1 1 + meshes/parking_garage_flipped.dae + + + + From e31cd1cd21197bbf0fb1626a0f544df9663c8d72 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Wed, 2 Jul 2025 23:01:12 +0200 Subject: [PATCH 4/6] better params --- mesh_navigation_tutorials/config/rmcl_micpl.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mesh_navigation_tutorials/config/rmcl_micpl.yaml b/mesh_navigation_tutorials/config/rmcl_micpl.yaml index 621bd75..21ddfab 100644 --- a/mesh_navigation_tutorials/config/rmcl_micpl.yaml +++ b/mesh_navigation_tutorials/config/rmcl_micpl.yaml @@ -30,8 +30,8 @@ rmcl_micpl: # maximum number of correction steps per second # lower this to decrease the correction speed but save energy disable_correction: False - correction_rate_max: 20.0 - optimization_iterations: 5 + correction_rate_max: 50.0 + optimization_iterations: 10 tf_time_source: 1 # 0: measurement_latest, 1: correction_latest tf_rate: 100.0 broadcast_tf: True From 3ee41ab6dc103061cfd9aefbf7f567e6e5fe21e1 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 3 Jul 2025 15:55:44 +0200 Subject: [PATCH 5/6] Update mesh_navigation_tutorials/config/rmcl_micpl.yaml Co-authored-by: Matthias Holoch --- mesh_navigation_tutorials/config/rmcl_micpl.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mesh_navigation_tutorials/config/rmcl_micpl.yaml b/mesh_navigation_tutorials/config/rmcl_micpl.yaml index 21ddfab..688e3de 100644 --- a/mesh_navigation_tutorials/config/rmcl_micpl.yaml +++ b/mesh_navigation_tutorials/config/rmcl_micpl.yaml @@ -29,8 +29,8 @@ rmcl_micpl: # maximum number of correction steps per second # lower this to decrease the correction speed but save energy - disable_correction: False correction_rate_max: 50.0 + disable_correction: False optimization_iterations: 10 tf_time_source: 1 # 0: measurement_latest, 1: correction_latest tf_rate: 100.0 From 13b8a39c4eb9787f4d69babcaeac185b9426aea0 Mon Sep 17 00:00:00 2001 From: Alexander Mock Date: Thu, 3 Jul 2025 15:55:53 +0200 Subject: [PATCH 6/6] Update mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py Co-authored-by: Matthias Holoch --- .../launch/mesh_navigation_tutorial_launch.py | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py b/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py index 9dcf896..3711586 100644 --- a/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py +++ b/mesh_navigation_tutorials/launch/mesh_navigation_tutorial_launch.py @@ -147,29 +147,6 @@ def generate_launch_description(): condition=IfCondition(PythonExpression(['"', localization_type, '" == "rmcl_micpl"'])), ) - # map_loc_rmcl_micpl = Node( - # package="rmcl_ros", - # executable="micp_localization", - # name="micp_localization", - # output="screen", - # remappings=[ - # ("pose_wc", "/initialpose"), - # ], - # parameters=[ - # { - # "use_sim_time": True, - # "map_file": PathJoinSubstitution( - # [ - # pkg_mesh_navigation_tutorials, - # "maps", - # PythonExpression(['"', map_name, '" + ".dae"']), - # ] - # ), - # }, - # PathJoinSubstitution([pkg_mesh_navigation_tutorials, "config", "rmcl_micpl.yaml"]), - # ], - # condition=IfCondition(PythonExpression(['"', localization_type, '" == "rmcl_micpl"'])), - # ) # Move Base Flex move_base_flex = IncludeLaunchDescription(