RMCL MICP-L for map-based localization

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

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: 50.0
+    # filter / pre-processing:
+    width:
+      skip_begin: 0 # skip elements from begin  
+      skip_end:   0 # skip elements from end
+      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
+      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: 50.0
+    optimization_iterations: 10
+    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]
+      lidar3d:
+        data_source: topic
+        model_type: o1dn
+        topic_name: /rmcl_inputs/cloud
+        correspondences:
+          backend: embree
+          type: RC
+          metric: P2L
+          adaptive_max_dist_min: 0.03
+          max_dist: 0.3
+          visualize: True

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,
         ]

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
+        ]
+    )
+

mesh_navigation_tutorials_sim/models/parking_garage/model.sdf

@@ -20,5 +20,23 @@
       </geometry>
     </visual>
   </link>
+  <link name="link_flipped">
+    <collision name="collision">
+      <geometry>
+        <mesh>
+          <scale>1 1 1</scale>
+          <uri>meshes/parking_garage_flipped.dae</uri>
+        </mesh>
+      </geometry>
+    </collision>
+    <visual name="visual">
+      <geometry>
+        <mesh>
+          <scale>1 1 1</scale>
+          <uri>meshes/parking_garage_flipped.dae</uri>
+        </mesh>
+      </geometry>
+    </visual>
+  </link>
 </model>
 </sdf>