Initial commit of the mesh navigation tutorials: three synthetic worl…

…ds and one robot.

Co-authored-by: Matthias Holoch <[email protected]>
Co-authored-by: Justus Braun <[email protected]>

.github/workflows/humble.yaml

@@ -0,0 +1,15 @@
+name: Humble CI - Build and Test
+
+on:
+  push:
+    branches:
+      - 'humble'
+  pull_request:
+  workflow_dispatch:
+    branches:
+      - '*'
+
+jobs:
+  build_and_test:
+    uses: naturerobots/github_automation_public/.github/workflows/humble_ci.yaml@main
+    secrets: inherit

README.md

@@ -1,17 +1,78 @@
-# Mesh Navigation Tutorials
-Placeholder repository for a potential [ROSCon 2024](https://roscon.ros.org/2024/) talk.
+<div align="center">
+<h1>
+Mesh Navigation Tutorials
+</h1>
+</div>
 
-Here, we will release tutorials that provide entrypoints for new users interested in 3D navigation on meshes.
-Besides documentation, it includes a range of real-world and artificial environments that are ready to use: Complete with simulation, launch files, and configuration files.
+<div align="center">
+  <a href="https://github.com/naturerobots/mesh_navigation">Mesh Navigation</a>
+  <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
+  <a href="https://github.com/naturerobots/mesh_navigation_tutorials/wiki">Documentation</a>
+  <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
+  <a href="https://www.youtube.com/@nature-robots">Videos</a>
+  <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
+  <a href="https://github.com/naturerobots/mesh_navigation_tutorials/issues">Issues</a>
+  <br />
+</div>
 
-![Mesh Navigation on simple tray dataset](docs/images/mesh_nav_on_tray.png "Mesh Navigation on simple tray dataset")
 
-![Mesh Navigation on real world dataset: Botanical garden Osnabrück](docs/images/mesh_nav_on_botanical_garden.png "Mesh Navigation on real world dataset: Botanical garden Osnabrück")
+<br/>
 
-# Related Repositories
-- [Move Base Flex](https://github.com/magazino/move_base_flex) ([IROS 2018 paper](https://doi.org/10.1109/IROS.2018.8593829))
-- [Mesh Tools](https://github.com/naturerobots/mesh_tools) ([RAS 2021 paper](https://doi.org/10.1016/j.robot.2020.103688))
+This repository contains a set of examples to quickly and easily start with [mesh_navigation](https://github.com/naturerobots/mesh_navigation). 
+We provide different scenarios where our approach excels over state-of-the art 2D or 2.5D approaches.
+We will explain different parameter sets in more detail and show how to fine-tune [mesh_navigation](https://github.com/naturerobots/mesh_navigation) in various scenarios.
+Our example worlds consists of both real-world and hand-modelled scenarios.
+With the hand-modelled examples we particularly aim to support low-end computers or laptops.
+
+
+*Note*: Because of an great interest of people we talked to, we decided to release this repository in an unfinished state. It is still under construction and will be extended by more synthetic and real-world recorded worlds and detailed docs. It's open-source: Feel free to contribute.  
+
+## Requirements and Installation
+
+* A working ROS 2 installation (we target `humble` at the moment).
+* The tutorial's ROS 2 dependencies
+  * Clone source dependencies: Run `vcs import --input mesh_navigation_tutorials/source_dependencies.yaml` in your ROS 2 workspace source directory.
+  * Get packaged dependencies: Run `rosdep install --from-paths . --ignore-src -r -y` from within your ROS 2 workspace source directory.
+* Build via `colcon build --packages-up-to mesh_navigation_tutorials`, from within your ROS 2 workspace root directory.
+
+## Run the Examples
+
+### Launch
+```console
+ros2 launch mesh_navigation_tutorials mesh_navigation_tutorial_launch.py world_name:=floor_is_lava
+```
+
+You change `floor_is_lava` by any world name that is available with this repository (see all by calling launch file with `--show-args`). Those are: 
+
+| Name | World | Default Map | Description |
+|------|-------|-----|-------------|
+| tray | ![tray_world](.resources/tray_world.png) | ![tray_map](.resources/tray_map.png)| This world is a rectangular area with a wall around the perimeter. |
+| floor_is_lava | ![floor_is_lava_world](.resources/floor_is_lava_world.png) | ![floor_is_lava_map](.resources/floor_is_lava_map.png)| This world contains a square area with with two pits and a connecting section at a slightly higher elevation.
+| parking_garage | ![parking_garage_world](.resources/parking_garage_world.png) | ![parking_garage_map](.resources/parking_garage_map.png)| This world represents a parking garage with multiple floors connected by ramps. |
+
+When running a simulated world, you can save some resources by not running the gazebo GUI: Add the `start_gazebo_gui:=False` launch argument.
+
+### Rviz GUI
+In rviz, you should be able to see the mesh map.
+This map is being used for navigation.
+
+In order to make the robot move, find the "Mesh Goal" tool at the top.
+With it, you can click on any part of the mesh.
+Click and hold to set a goal pose.
+The MbfGoalActions rviz plugin contains a very tiny state machine that performs the following actions:
+* subscribe to that goal pose
+* get a path to that pose
+* execute that path
+
+## Detailed Instructions 
+
+For more detailed instructions on how to parameterize things or what things can be changed see the [wiki](https://github.com/naturerobots/mesh_navigation_tutorials/wiki)
+
+
+## Related Repositories
+- [Move Base Flex](https://github.com/magazino/move_base_flex) ([IROS 2018](https://doi.org/10.1109/IROS.2018.8593829))
+- [Mesh Tools](https://github.com/naturerobots/mesh_tools) ([RAS 2021](https://doi.org/10.1016/j.robot.2020.103688))
 - [Mesh Navigation](https://github.com/naturerobots/mesh_navigation) ([ICRA 2021 paper for Continuous Vector Field Planner, CVP](https://doi.org/10.1109/ICRA48506.2021.9560981))
-- [MICP-L](https://github.com/uos/rmcl) ([paper on arxiv](https://arxiv.org/abs/2210.13904))
-- [Rmagine](https://github.com/uos/rmagine) ([ICRA 2023 paper](https://doi.org/10.1109/ICRA48891.2023.10161388))
+- [Rmagine](https://github.com/uos/rmagine) ([ICRA 2023](https://doi.org/10.1109/ICRA48891.2023.10161388))
+- [MICP-L](https://github.com/uos/rmcl) ([IROS 2024](https://arxiv.org/abs/2210.13904))
 - [RMCL](https://github.com/uos/rmcl)

mesh_navigation_tutorials/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+project(mesh_navigation_tutorials)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+# install
+install(DIRECTORY config launch rviz maps
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# test
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

mesh_navigation_tutorials/config/ekf.yaml

@@ -0,0 +1,81 @@
+ekf_filter_node:
+  ros__parameters:
+    frequency: 50.0
+    two_d_mode: true
+
+    debug: false
+
+    map_frame: map              # Defaults to "map" if unspecified
+    odom_frame: odom            # Defaults to "odom" if unspecified
+    base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
+    world_frame: odom           # Defaults to the value of odom_frame if unspecified
+
+    odom0: /odom
+    odom0_config: [false, false, false, #xyz
+                    false, false, false, # rpy
+                    true, false, false, #vxyz
+                    false, false, true, #vrpy
+                    false, false, false] #axyz 
+
+    odom0_differential: false
+    # odom0_relative: true
+
+    imu0: /imu/data
+    imu0_config: [false,  false,  false, #xyz
+                    false, false, false, # rpy
+                    false, false, false, #vxyz
+                    true, true, true, #vrpy
+                    false, false, false] #axyz 
+    imu0_differential: false
+
+    use_control: false
+
+    # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+    # false.
+    stamped_control: true
+
+    # The last issued control command will be used in prediction for this period. Defaults to 0.2.
+    control_timeout: 0.2
+
+    # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+    control_config: [true, false, false, false, false, true]
+
+    # Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+    acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+
+    # Acceleration and deceleration limits are not always the same for robots.
+    deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+
+    # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+    # gains
+    acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+
+    # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+    # gains
+    deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+    # imu0_relative: true
+
+    # imu0_remove_gravitational_acceleration: true
+
+    # process_noise_covariance: [0.01, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.2, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.01, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.01, 0.0,    0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.1, 0.0,    0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.1, 0.0,
+    #                            0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.1]
+
+    # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+    # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+    # question. Users should take care not to use large values for variables that will not be measured directly. The values
+    # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below
+    # if unspecified. In this example, we specify only the diagonal of the matrix.
+    # initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]

mesh_navigation_tutorials/config/mbf_mesh_nav.yaml

@@ -0,0 +1,60 @@
+move_base_flex:
+  ros__parameters:
+    global_frame: 'map'
+    robot_frame: 'base_footprint'
+    odom_topic: 'odom'
+
+    use_sim_time: true
+    force_stop_at_goal: true
+    force_stop_on_cancel: true
+
+    planners: ['mesh_planner']
+    mesh_planner:
+      type: 'cvp_mesh_planner/CVPMeshPlanner'
+      cost_limit: 0.8
+      publish_vector_field: true
+    planner_patience: 10.0
+    planner_max_retries: 2
+    project_path_onto_mesh: false
+
+    controllers: ['mesh_controller']
+    mesh_controller:
+      type: 'mesh_controller/MeshController'
+      ang_vel_factor: 7.0
+      lin_vel_factor: 1.0
+
+    controller_patience: 2.0
+    controller_max_retries: 4
+    dist_tolerance: 0.2
+    angle_tolerance: 0.8
+    cmd_vel_ignored_tolerance: 10.0
+
+    mesh_map:
+      mesh_part: 'mesh'
+
+      layers: ['border', 'height_diff', 'roughness', 'inflation']
+
+      height_diff:
+        type: 'mesh_layers/HeightDiffLayer'
+        factor: 1.0
+        threshold: 0.8
+
+      border:
+        type: 'mesh_layers/BorderLayer'
+        factor: 1.0
+        border_cost: 1.0
+        threshold: 0.2
+
+      roughness:
+        type: 'mesh_layers/RoughnessLayer'
+        factor: 1.0
+        threshold: 0.8
+
+      inflation:
+        type: 'mesh_layers/InflationLayer'
+        factor: 1.0
+        inflation_radius: 0.4
+        inscribed_radius: 0.2
+        lethal_value: 1.0
+        inscribed_value: 0.8
+        repulsive_field: false

mesh_navigation_tutorials/config/rmcl_micpl.yaml

@@ -0,0 +1,44 @@
+micp_localization:
+  ros__parameters:
+
+    # required
+    base_frame: base_footprint
+    map_frame: map
+    odom_frame: odom
+
+    # rate of broadcasting tf transformations
+    tf_rate: 50.0
+
+    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
+
+      # DEBUGGING   
+      # corr = correspondences
+      viz_corr: True
+      # corr = correction
+      print_corr_rate: False
+      disable_corr: False
+
+      # initial pose changes
+      trans: [0.0, 0.0, 0.0]
+      rot: [0.0, 0.0, 0.0] # euler angles (3) or quaternion (4) 
+
+    # 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]

mesh_navigation_tutorials/launch/mbf_mesh_navigation_server_launch.py

@@ -0,0 +1,41 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+def generate_launch_description():
+  launch_args = [
+    DeclareLaunchArgument("mesh_map_path", 
+                          description='Path to the mesh file that defines the map.',
+    ),
+  ]
+  mesh_map_path = LaunchConfiguration('mesh_map_path')
+
+  mbf_mesh_nav_config = os.path.join(
+    get_package_share_directory('mesh_navigation_tutorials'),
+    'config',
+    'mbf_mesh_nav.yaml'
+    )
+
+  mesh_nav_server = Node(
+    name='move_base_flex',
+    package='mbf_mesh_nav',
+    executable='mbf_mesh_nav',
+    remappings=[
+      ('/move_base_flex/cmd_vel', '/cmd_vel'),
+    ],
+    parameters=[
+      mbf_mesh_nav_config,
+      {'mesh_map.mesh_file': mesh_map_path},
+    ],
+  )
+
+  return LaunchDescription(
+      launch_args + [
+      mesh_nav_server,
+  ])