Merge pull request #14 from naturerobots/feature/dynamic-cost-updates

Reaction to MeshNav v3 + Real World Maps

Author: amock

.gitattributes

@@ -0,0 +1,30 @@
+# Physics Campus UOS 
+mesh_navigation_pluto_sim/worlds/physics_campus_uos.sdf filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_pluto_sim/models/physics_campus_uos/** filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_pluto/maps/physics_campus_uos.ply filter=lfs diff=lfs merge=lfs -text
+
+# Botanical Garden Osnabrueck
+mesh_navigation_pluto_sim/worlds/botanical_garden_osnabrueck.sdf filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_pluto_sim/models/botanical_garden_osnabrueck/** filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_pluto/maps/botanical_garden_osnabrueck.ply filter=lfs diff=lfs merge=lfs -text
+
+# Stone Quarry Brockum
+mesh_navigation_pluto_sim/worlds/stone_quarry_brockum.sdf filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_pluto_sim/models/stone_quarry_brockum/** filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_pluto/maps/stone_quarry_brockum.ply filter=lfs diff=lfs merge=lfs -text
+
+
+# Coppenrath Innovation Centre (CIC) Outdoor 
+mesh_navigation_ceres_sim/worlds/cic_outdoor.sdf filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_ceres_sim/models/cic_outdoor/** filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_ceres/maps/cic_outdoor.ply filter=lfs diff=lfs merge=lfs -text
+
+# Agro-Technicum Osnabrueck
+mesh_navigation_ceres_sim/worlds/agrotechnicum.sdf filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_ceres_sim/models/agrotechnicum/** filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_ceres/maps/agrotechnicum.ply filter=lfs diff=lfs merge=lfs -text
+
+# FH Aachen Campus (Provided by MASKOR)
+mesh_navigation_ceres_sim/worlds/fh_aachen.sdf filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_ceres_sim/models/fh_aachen/** filter=lfs diff=lfs merge=lfs -text
+mesh_navigation_ceres/maps/fh_aachen.ply filter=lfs diff=lfs merge=lfs -text

README.md

@@ -32,19 +32,31 @@ With the hand-modelled examples we particularly aim to support low-end computers
 
 ## Requirements and Installation
 
-* You need a working ROS 2 installation. We target `humble` at the moment.
-* Go into a ROS 2 workspace's source directory `cd $YOUR_ROS_WS/src`.
-* Clone the tutorial code `git clone [email protected]:naturerobots/mesh_navigation_tutorials.git`
-* Get 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: Go to workspace root `cd $YOUR_ROS_WS` and run `colcon build --packages-up-to mesh_navigation_tutorials`.
+You need a working ROS 2 installation. We target `humble` at the moment. Go into a ROS 2 workspace's source directory `cd $YOUR_ROS_WS/src`. Then clone the tutorial code 
+
+```bash
+GIT_LFS_SKIP_SMUDGE=1 git clone [email protected]:naturerobots/mesh_navigation_tutorials.git
+```
+
+> [!NOTE] 
+> If you forget to add `GIT_LFS_SKIP_SMUDGE=1` before `git clone` and you have Git LFS installed, all available maps will be downloaded during the clone. This may take a while. But than you skip all the Git LFS commands later.
+
+Get 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: Go to workspace root `cd $YOUR_ROS_WS` and run 
+
+```bash
+colcon build --packages-up-to mesh_navigation_tutorials
+```
 
 ## Run the Examples
 
 ### Launch
-```console
-ros2 launch mesh_navigation_tutorials mesh_navigation_tutorial_launch.py world_name:=floor_is_lava
+
+```bash
+ros2 launch mesh_navigation_tutorials mesh_navigation_tutorials_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: 
@@ -57,7 +69,8 @@ You change `floor_is_lava` by any world name that is available with this reposit
 
 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
+### RViz GUI
+
 In rviz, you should be able to see the mesh map.
 This map is being used for navigation.
 
@@ -69,15 +82,33 @@ The MbfGoalActions rviz plugin contains a very tiny state machine that performs
 * get a path to that pose
 * execute that path
 
-## Detailed Instructions 
+## Detailed Instructions
+
+For more detailed instructions on how to parameterize things or what things can be changed see the [wiki](https://naturerobots.github.io/mesh_navigation_docs/tutorials/)
 
-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)
+## Extended Examples
 
+Additionally, we offer larger maps that better resemble real-world scales:
+
+* [Pluto Maps](./mesh_navigation_pluto/)
+* [Ceres Maps](./mesh_navigation_ceres/)
+
+All requires Git LFS to be installed. Under Ubuntu you can simply type:
+
+```bash
+sudo apt install git-lfs
+```
 
 ## 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))
-- [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)
+
+* [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))
+* [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_ceres/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+project(mesh_navigation_ceres)
+
+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_ceres/README.md

@@ -0,0 +1,45 @@
+# Mesh Navigation Tutorials on Real World Ceres maps
+
+This repository contains the "ceres" environments. A group of newer maps recorded in reality during the benchmarking of the MeshMPPI controller.
+
+## Download GIT LFS
+
+The following maps are available within this package:
+
+- `cic_outdoor`
+- `agro_technicum`
+- `fh_aachen`
+
+While being in the source directory of this repository, download all Ceres maps by entering
+
+```bash
+git lfs pull --include="mesh_navigation_ceres*"
+```
+
+or specific ones by calling
+
+```bash
+git lfs pull --include="mesh_navigation_ceres*/**/cic_outdoor*"
+```
+
+## Install Ceres Maps
+
+Compile your ROS workspace to install the downloaded Ceres maps
+
+```bash
+colcon build
+```
+
+## Quick-Start
+
+```bash
+ros2 launch mesh_navigation_ceres mesh_navigation_ceres_launch.py world_name:=cic_outdoor
+```
+
+> [!NOTE]
+> Since the maps are quite large the startup might take some time to load.
+
+
+## Acknowledgements
+
+We thank the [MASKOR](https://maskor.fh-aachen.de/) institute for supplying the TLS data of the `fh_aachen` environment for public use.

mesh_navigation_ceres/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]