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/config/rmcl_micpl.yaml b/mesh_navigation_tutorials/config/rmcl_micpl.yaml index 3dc0044..688e3de 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: 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 + 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 + 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.03 + max_dist: 0.3 + 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..3711586 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,19 @@ 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"'])), - ) + ) + # Move Base Flex move_base_flex = IncludeLaunchDescription( @@ -195,7 +187,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 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 + + + + 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}