保存pcd

.gitignore

@@ -49,7 +49,6 @@ build/
 CMakeLists.txt.user
 
 srv/_*.py
-*.pcd
 *.pyc
 qtcreator-*
 *.user

mapping.sh

@@ -2,7 +2,7 @@ source install/setup.bash
 
 commands=(
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RC2025 \
+    world:=RC2026 \
     mode:=mapping \
     lio:=fastlio \
     localization:=icp \

nav.sh

@@ -3,14 +3,14 @@ source install/setup.bash
 commands=(
    "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
     "ros2 launch rm_nav_bringup bringup_real.launch.py \
-    world:=RC2025 \
+    world:=RC2026 \
     mode:=nav \
     lio:=fastlio \
     localization:=icp \
     lio_rviz:=false \
     nav_rviz:=true"
     "ros2 launch rm_simpal_move simple_move.launch.py"
-    "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.56, y: 3.960, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
+    "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.60, y: 3.995, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
 )
 
 for cmd in "${commands[@]}"; do

src/rm_nav_bringup/config/reality/measurement_params_real.yaml

@@ -1,3 +1,3 @@
 base_link2livox_frame:
-  xyz: "\"0.013 -0.354 0.41\""
+  xyz: "\"0.037 -0.354 0.41\""
   rpy: "\"0.0  0.0  0.0\""

src/rm_nav_bringup/config/rm_navigation/costmap_converter/.gitlab-ci.yml

@@ -0,0 +1,23 @@
+build-kinetic:
+  variables:
+    ROSDISTRO: "kinetic"
+    CATKIN_WS: "/root/catkin_ws"
+  stage: build
+  image: osrf/ros:$ROSDISTRO-desktop-full
+  before_script: 
+    - apt-get -qq update
+    - apt-get -qq install git-core python-catkin-tools curl
+    - mkdir -p $CATKIN_WS/src
+    - cp -a . $CATKIN_WS/src/
+    - cd $CATKIN_WS
+    - rosdep update
+    - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
+  script: 
+    - source /ros_entrypoint.sh
+    - cd $CATKIN_WS
+    - catkin build -i -s --no-status
+  only:
+    - master
+  tags:
+    - ubuntu
+    - docker

src/rm_nav_bringup/config/rm_navigation/costmap_converter/.travis.yml

@@ -0,0 +1,109 @@
+# Generic .travis.yml file for running continuous integration on Travis-CI with
+# any ROS package.
+#
+# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
+# workspace, resolves all listed dependencies, and sets environment variables
+# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
+# no compilation errors), and runs all the tests. If any of the compilation/test
+# phases fail, the build is marked as a failure.
+#
+# We handle two types of package dependencies:
+#   - packages (ros and otherwise) available through apt-get. These are installed
+#     using rosdep, based on the information in the ROS package.xml.
+#   - dependencies that must be checked out from source. These are handled by
+#     'wstool', and should be listed in a file named dependencies.rosinstall.
+#
+# There are two variables you may want to change:
+#   - ROS_DISTRO (default is indigo). Note that packages must be available for
+#     ubuntu 14.04 trusty.
+#   - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
+#     root). This should list all necessary repositories in wstool format (see
+#     the ros wiki). If the file does not exists then nothing happens.
+#
+# See the README.md for more information.
+#
+# Author: Felix Duvallet <felixd@gmail.com>
+
+# NOTE: The build lifecycle on Travis.ci is something like this:
+#    before_install
+#    install
+#    before_script
+#    script
+#    after_success or after_failure
+#    after_script
+#    OPTIONAL before_deploy
+#    OPTIONAL deploy
+#    OPTIONAL after_deploy
+
+################################################################################
+
+# Use ubuntu trusty (14.04) with sudo privileges.
+dist: trusty
+sudo: required
+language:
+  - generic
+cache:
+  - apt
+
+# Configuration variables. All variables are global now, but this can be used to
+# trigger a build matrix for different ROS distributions if desired.
+env:
+  global:
+    - ROS_CI_DESKTOP="`lsb_release -cs`"  # e.g. [precise|trusty|...]
+    - CI_SOURCE_PATH=$(pwd)
+    - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
+    - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
+    - ROS_PARALLEL_JOBS='-j8 -l6'
+  matrix:
+    - ROS_DISTRO=indigo
+    - ROS_DISTRO=jade
+
+
+################################################################################
+
+# Install system dependencies, namely a very barebones ROS setup.
+before_install:
+  - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+  - sudo apt-get update -qq
+  - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
+  - source /opt/ros/$ROS_DISTRO/setup.bash
+  # Prepare rosdep to install dependencies.
+  - sudo rosdep init
+  - rosdep update
+
+# Create a catkin workspace with the package under integration.
+install:
+  - mkdir -p ~/catkin_ws/src
+  - cd ~/catkin_ws/src
+  - catkin_init_workspace
+  # Create the devel/setup.bash (run catkin_make with an empty workspace) and
+  # source it to set the path variables.
+  - cd ~/catkin_ws
+  - catkin_make
+  - source devel/setup.bash
+  # Add the package under integration to the workspace using a symlink.
+  - cd ~/catkin_ws/src
+  - ln -s $CI_SOURCE_PATH .
+
+# Install all dependencies, using wstool and rosdep.
+# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
+before_script:
+  # source dependencies: install using wstool.
+  - cd ~/catkin_ws/src
+  - wstool init
+  - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
+  - wstool up
+  # package depdencies: install using rosdep.
+  - cd ~/catkin_ws
+  - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+
+# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to
+# catkin_make.
+script:
+  - cd ~/catkin_ws
+  - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
+  # Testing: Use both run_tests (to see the output) and test (to error out).
+  - catkin_make run_tests  # This always returns 0, but looks pretty.
+  - catkin_make test  # This will return non-zero if a test fails.
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/README.md

@@ -0,0 +1,30 @@
+costmap_converter ROS Package
+=============================
+
+A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types
+
+Build status of the *master* branch:
+- ROS Buildfarm Kinetic: [![Kinetic Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__costmap_converter__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__costmap_converter__ubuntu_xenial_amd64/)
+- ROS Buildfarm Indigo: [![Indigo Build Status](http://build.ros.org/buildStatus/icon?job=Idev__costmap_converter__ubuntu_trusty_amd64)](http://build.ros.org/job/Idev__costmap_converter__ubuntu_trusty_amd64/)
+
+
+### Contributors
+
+- Christoph Rösmann
+- Franz Albers (*CostmapToDynamicObstacles* plugin)
+- Otniel Rinaldo
+
+
+### License
+
+The *costmap_converter* package is licensed under the BSD license.
+It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
+
+Some third-party dependencies are included that are licensed under different terms:
+ - *MultitargetTracker*, GNU GPLv3, https://github.com/Smorodov/Multitarget-tracker
+   (partially required for the *CostmapToDynamicObstacles* plugin)
+
+All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
+
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CHANGELOG.rst

@@ -0,0 +1,105 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package costmap_converter
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.2 (2020-06-22)
+------------------
+* OpenCV 4 compatibility fix
+* Contributors: daviddudas
+
+0.1.1 (2020-01-25)
+------------------
+* Fixed ament plugin export
+* Revert release-mode for cmake build
+* Contributors: Christoph Rösmann
+
+0.1.0 (2020-01-23)
+------------------
+* Port to ROS2 (thanks to Vinnam Kim and stevemacenski)
+* Messages moved to a separate package
+* Contributors: Christoph Rösmann, Vinnam Kim, stevemacenski
+
+0.0.12 (2019-12-02)
+-------------------
+* CostmapToPolygons: Simplification of the polygon by Douglas-Peucker algorithm (reduces the density of points in the polygon).
+* Bugfixes
+* Contributors: Rainer Kümmerle
+
+0.0.11 (2019-10-26)
+-------------------
+* rostest integration to avoid running a roscore separately for unit testing
+* Contributors: Christoph Rösmann
+
+0.0.10 (2019-10-26)
+-------------------
+* Runtime improvements for CostmapToPolygonsDBSMCCH (`#12 <https://github.com/rst-tu-dortmund/costmap_converter/issues/12>`_)
+  * Grid lookup for regionQuery
+  * use a grid structure for looking up nearest neighbors
+  * parameters in a struct
+  * guard the parameters by drawing a copy from dynamic reconfigure
+  * Adding some test cases for regionQuery and dbScan
+  * Avoid computing sqrt at the end of convexHull2
+  * Add doxygen comments for the neighbor lookup
+  * Change the param read to one liners
+  * Add test on empty map for dbScan
+* Contributors: Rainer Kümmerle
+
+0.0.9 (2018-05-28)
+------------------
+* Moved plugin loader for static costmap conversion to BaseCostmapToDynamicObstacles.
+  The corresponding ROS parameter `static_converter_plugin` is now defined in the CostmapToDynamicObstacles namespace. 
+* Contributors: Christoph Rösmann
+
+0.0.8 (2018-05-17)
+------------------
+* Standalone converter subscribes now to costmap updates. Fixes `#1 <https://github.com/rst-tu-dortmund/costmap_converter/issues/1>`_
+* Adds radius field for circular obstacles to ObstacleMsg
+* Stacked costmap conversion (`#7 <https://github.com/rst-tu-dortmund/costmap_converter/issues/7>`_).
+  E.g., it is now possible combine a dynamic obstacle and static obstacle converter plugin.
+* Contributors: Christoph Rösmann, Franz Albers
+
+0.0.7 (2017-09-20)
+------------------
+* Fixed some compilation issues (C++11 compiler flags and opencv2 on indigo/jade).
+* Dynamic obstacle plugin: obstacle velocity is now published for both x and y coordinates rather than the absolute value
+
+0.0.6 (2017-09-18)
+------------------
+* This pull request adds the costmap to dynamic obstacles plugin (written by Franz Albers).
+  It detects the dynamic foreground of the costmap (based on the temporal evolution of the costmap)
+  including blobs representing the obstacles. Furthermore, Kalman-based tracking is applied to estimate
+  the current velocity for each obstacle.
+  **Note, this plugin is still experimental.**
+* New message types are introduced: costmap\_converter::ObstacleMsg and costmap\_converter::ObstacleArrayMsg.
+  These types extend the previous polygon representation by additional velocity, orientation and id information.
+* The API has been extended to provide obstacles via the new ObstacleArrayMsg type instead of vector of polygons.
+* Contributors: Franz Albers, Christoph Rösmann
+
+0.0.5 (2016-02-01)
+------------------
+* Major changes regarding the line detection based on the convex hull
+  (it should be much more robust now).
+* Concave hull plugin added.
+* The cluster size can now be limited from above using a specific parameter.
+  This implicitly avoids large clusters forming a 'L' or 'U'.
+* All parameters can now be adjusted using dynamic_reconfigure (rqt_reconfigure).
+* Some parameter names changed.
+* Line plugin based on ransac: line inliers must now be placed inbetween the start and end of a line.
+
+0.0.4 (2016-01-11)
+------------------
+* Fixed conversion from map to world coordinates if the costmap is not quadratic.
+
+0.0.3 (2015-12-23)
+------------------
+* The argument list of the initialize method requires a nodehandle from now on. This facilitates the handling of parameter namespaces for multiple instantiations of the plugin.
+* This change is pushed immediately as a single release to avoid API breaks (since version 0.0.2 is not on the official repos up to now).
+
+0.0.2 (2015-12-22)
+------------------
+* Added a plugin for converting the costmap to lines using ransac
+
+0.0.1 (2015-12-21)
+------------------
+* First release of the package including a pluginlib interface, two plugins (costmap to polygons and costmap to lines) and a standalone conversion node.
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.5)
+project(costmap_converter)
+
+# Set to Release in order to speed up the program significantly
+set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(costmap_converter_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+
+set(dependencies
+    class_loader
+    costmap_converter_msgs
+    cv_bridge
+    geometry_msgs
+    nav2_costmap_2d
+    tf2
+    tf2_geometry_msgs
+    tf2_ros
+    OpenCV
+    pluginlib
+    rclcpp
+)
+
+include_directories(
+  include
+)
+
+add_library(costmap_converter SHARED
+  src/costmap_to_polygons.cpp 
+  src/costmap_to_polygons_concave.cpp
+  src/costmap_to_lines_convex_hull.cpp
+  src/costmap_to_lines_ransac.cpp
+  src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp
+  src/costmap_to_dynamic_obstacles/background_subtractor.cpp
+  src/costmap_to_dynamic_obstacles/blob_detector.cpp
+  src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp
+  src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp
+  src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp
+)
+
+ament_target_dependencies(costmap_converter 
+    ${dependencies}
+)
+
+add_executable(standalone_converter src/costmap_converter_node.cpp)
+
+target_link_libraries(standalone_converter
+    costmap_converter
+)
+ament_target_dependencies(standalone_converter
+  ${dependencies}
+)
+
+install(TARGETS costmap_converter
+   DESTINATION lib
+)
+install(TARGETS standalone_converter
+   DESTINATION bin
+)
+
+install(DIRECTORY include/
+   DESTINATION include/
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(costmap_converter)
+ament_export_dependencies(${dependencies})
+pluginlib_export_plugin_description_file(costmap_converter costmap_converter_plugins.xml)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_costmap_polygons test/costmap_polygons.cpp)
+  if(TARGET test_costmap_polygons)
+    target_link_libraries(test_costmap_polygons costmap_converter)  
+  endif()
+  ament_target_dependencies(test_costmap_polygons ${dependencies})
+endif()
+
+ament_package()

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg

@@ -0,0 +1,136 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+##################################################################
+###################### Foreground detection ######################
+gen.add("alpha_slow", double_t, 0,
+	"Foreground detection: Learning rate of the slow filter",
+	0.3, 0.0, 1.0)
+
+gen.add("alpha_fast", double_t, 0,
+	"Foreground detection: Learning rate of the fast filter",
+	0.85, 0.0, 1.0)
+
+gen.add("beta", double_t, 0,
+	"Foreground detection: Weighting coefficient between a pixels value and the mean of its nearest neighbors",
+	0.85, 0.0, 1.0)
+
+gen.add("min_sep_between_slow_and_fast_filter", int_t, 0,
+	"Foreground detection: Minimal difference between the fast and the slow filter to recognize a obstacle as dynamic",
+	80, 0, 255)
+
+gen.add("min_occupancy_probability", int_t, 0,
+	"Foreground detection: Minimal value of the fast filter to recognize a obstacle as dynamic",
+	180, 0, 255)
+
+gen.add("max_occupancy_neighbors", int_t, 0,
+	"Foreground detection: Maximal mean value of the nearest neighbors of a pixel in the slow filter",
+	80, 0, 255)
+
+gen.add("morph_size", int_t, 0,
+	"Foreground detection: Size of the structuring element for the closing operation",
+	1, 0, 10)
+
+gen.add("publish_static_obstacles", bool_t, 0,
+        "Include static obstacles as single-point polygons",
+        True)
+
+############################################################
+###################### Blob detection ######################
+
+# These parameters are commented out, because the input image for the blob detection is already binary -> irrelevant
+#gen.add("threshold_step", double_t, 0,
+#	"Blob detection: Distance between neighboring thresholds",
+#	256.0, 0.0, 256.0)
+#
+#gen.add("min_threshold", double_t, 0,
+#	"Blob detection: Convert the source image to binary images by applying several thresholds, starting at min_threshold",
+#	1, 0, 255)
+#
+#gen.add("max_threshold", double_t, 0,
+#	"Blob detection: Convert the source image to binary images by applying several thresholds, ending at max_threshold",
+#	255, 0, 255)
+#
+#gen.add("min_repeatability", int_t, 0,
+#	"Blob detection: Minimal number of detections of a blob in the several thresholds to be considered as real blob",
+#	1, 1, 10)
+#
+gen.add("min_distance_between_blobs", double_t, 0,
+	"Blob detection: Minimal distance between centers of two blobs to be considered as seperate blobs",
+	10, 0.0, 300.0)
+
+gen.add("filter_by_area", bool_t, 0,
+	"Blob detection: Filter blobs based on number of pixels",
+	True)
+
+gen.add("min_area", int_t, 0,
+	"Blob detection: Minimal number of pixels a blob consists of",
+	3, 0, 300)
+
+gen.add("max_area", int_t, 0,
+	"Blob detection: Maximal number of pixels a blob consists of",
+	300, 0, 300)
+
+gen.add("filter_by_circularity", bool_t, 0,
+	"Blob detection: Filter blobs based on their circularity",
+	True)
+
+gen.add("min_circularity", double_t, 0,
+	"Blob detection: Minimal circularity value (0 in case of a line)",
+	0.2, 0.0, 1.0)
+
+gen.add("max_circularity", double_t, 0,
+	"Blob detection: Maximal circularity value (1 in case of a circle)",
+	1.0, 0.0, 1.0)
+
+gen.add("filter_by_inertia", bool_t, 0,
+	"Blob detection: Filter blobs based on their inertia ratio",
+	True)
+
+gen.add("min_inertia_ratio", double_t, 0,
+	"Blob detection: Minimal inertia ratio",
+	0.2, 0.0, 1.0)
+
+gen.add("max_inertia_ratio", double_t, 0,
+	"Blob detection: Maximal inertia ratio",
+	1.0, 0.0, 1.0)
+
+gen.add("filter_by_convexity", bool_t, 0,
+	"Blob detection: Filter blobs based on their convexity (Blob area / area of its convex hull)",
+	False)
+
+gen.add("min_convexity", double_t, 0,
+	"Blob detection: Minimum convexity ratio",
+	0.0, 0.0, 1.0)
+
+gen.add("max_convexity", double_t, 0,
+	"Blob detection: Maximal convexity ratio",
+	1.0, 0.0, 1.0)
+
+################################################################
+#################### Tracking ##################################
+gen.add("dt", double_t, 0,
+	"Tracking: Time for one timestep of the kalman filter",
+	0.2, 0.1, 3.0)
+
+gen.add("dist_thresh", double_t, 0,
+	"Tracking: Maximum distance between two points to be considered in the assignment problem",
+	20.0, 0.0, 150.0)
+
+gen.add("max_allowed_skipped_frames", int_t, 0,
+	"Tracking: Maximum number of frames a object is tracked while it is not seen",
+	3, 0, 10)
+
+gen.add("max_trace_length", int_t, 0,
+	"Tracking: Maximum number of Points in a objects trace",
+	10, 1, 100)
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToDynamicObstacles"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg

@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+gen.add("support_pts_max_dist",   double_t,   0,
+	"Minimum distance from a point to the line to be counted as support point",
+	0.3, 0.0, 10.0)
+
+gen.add("support_pts_max_dist_inbetween",   double_t,   0,
+  "A line is only defined, if the distance between two consecutive support points is less than this treshold. Set to 0 in order to deactivate this check.",
+  1.0, 0.0, 10.0)
+
+gen.add("min_support_pts", int_t, 0, 
+	"Minimum number of support points to represent a line",
+	2, 0, 50)	
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSMCCH"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("ransac_inlier_distance", double_t, 0, 
+	"Maximum distance to the line segment for inliers",
+	0.2, 0.0, 10.0)
+
+gen.add("ransac_min_inliers", int_t, 0, 
+	"Minimum numer of inliers required to form a line",
+	10, 0, 100)
+
+gen.add("ransac_no_iterations", int_t, 0, 
+	"Number of ransac iterations",
+	2000, 1, 10000)
+
+gen.add("ransac_remainig_outliers", int_t, 0, 
+	"Repeat ransac until the number of outliers is as specified here",
+	3, 0, 50)
+
+gen.add("ransac_convert_outlier_pts",   bool_t,   0, 
+	"Convert remaining outliers to single points.",
+	True)
+
+gen.add("ransac_filter_remaining_outlier_pts",   bool_t,   0, 
+	"Filter the interior of remaining outliers and keep only keypoints of their convex hull",
+	False)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSRANSAC"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+gen.add("concave_hull_depth",   double_t,   0,
+  "Smaller depth: sharper surface, depth -> high value: convex hull",
+  2.0, 0.0, 100.0) 
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSConcaveHull"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+gen.add("cluster_max_distance", double_t, 0, 
+	"Parameter for DB_Scan, maximum distance to neighbors [m]",
+	0.4, 0.0, 10.0)
+
+gen.add("cluster_min_pts", int_t, 0, 
+	"Parameter for DB_Scan: minimum number of points that define a cluster",
+	2, 1, 20)
+
+gen.add("cluster_max_pts", int_t, 0, 
+  "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)",
+  30, 2, 200)
+
+gen.add("convex_hull_min_pt_separation",   double_t,   0,
+	"Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)",
+	0.1, 0.0, 10.0)	
+
+exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSMCCH"))

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/costmap_converter_plugins.xml

@@ -0,0 +1,38 @@
+<library path="costmap_converter">
+  
+  <class type="costmap_converter::CostmapToPolygonsDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of convex polygons. 
+      Clustering is performed with DBScan. Convex polygons are detected using the Monotone Chain Convex Hull algorithm.
+  </description>
+  </class>
+  
+  <class type="costmap_converter::CostmapToLinesDBSMCCH" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). 
+      Clustering is performed with DBScan. Clusters are transformed into convex polygons using the Monotone Chain Convex Hull algorithm.
+      The resulting keypoints are used for possible line candidates.
+      A line is only defined if there exist a specified number of support points between each keypoint pair.
+  </description>
+  </class>
+  
+  <class type="costmap_converter::CostmapToLinesDBSRANSAC" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). 
+      Clustering is performed with DBScan. For each cluster RANSAC is applied sequentally to fit multiple line models.
+  </description>
+  </class>
+  
+  <class type="costmap_converter::CostmapToPolygonsDBSConcaveHull" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class converts costmap2d obstacles into a vector of non-convex (concave) polygons. 
+  </description>
+  </class>
+
+  <class type="costmap_converter::CostmapToDynamicObstacles" base_class_type="costmap_converter::BaseCostmapToPolygons">
+  <description>
+      This class detects and tracks obstacles from a sequence of costmaps. 
+  </description>
+  </class>
+  
+</library>

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_converter_interface.h

@@ -0,0 +1,389 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#ifndef COSTMAP_CONVERTER_INTERFACE_H_
+#define COSTMAP_CONVERTER_INTERFACE_H_
+
+//#include <costmap_2d/costmap_2d_ros.h>
+#include <mutex>
+#include <memory>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <nav2_costmap_2d/costmap_2d.hpp>
+#include <nav2_costmap_2d/costmap_2d_ros.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+#include <costmap_converter_msgs/msg/obstacle_array_msg.hpp>
+
+namespace costmap_converter
+{
+  
+//! Typedef for a shared dynamic obstacle container
+typedef costmap_converter_msgs::msg::ObstacleArrayMsg::SharedPtr ObstacleArrayPtr;
+//! Typedef for a shared dynamic obstacle container (read-only access)
+typedef costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr ObstacleArrayConstPtr;
+
+//! Typedef for a shared polygon container 
+typedef std::shared_ptr<std::vector<geometry_msgs::msg::Polygon>> PolygonContainerPtr;
+//! Typedef for a shared polygon container (read-only access)
+typedef std::shared_ptr<const std::vector<geometry_msgs::msg::Polygon>> PolygonContainerConstPtr;
+  
+
+/**
+ * @class BaseCostmapToPolygons
+ * @brief This abstract class defines the interface for plugins that convert the costmap into polygon types
+ * 
+ * Plugins must accept a nav2_costmap_2d::Costmap2D datatype as information source.
+ * The interface provides two different use cases:
+ * 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons() 
+ *    (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D())
+ * 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()):
+ *    Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner).
+ *    Costmaps can be obtained by invoking getPolygons().
+ */
+class BaseCostmapToPolygons
+{
+public: 
+  
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(rclcpp::Node::SharedPtr nh) {
+      nh_ = nh;
+    }
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~BaseCostmapToPolygons() 
+    {
+      stopWorker();
+    }
+
+    
+    /**
+     * @brief Pass a pointer to the costap to the plugin.
+     * @warning The plugin should handle the costmap's mutex locking.
+     * @sa updateCostmap2D
+     * @param costmap Pointer to the costmap2d source
+     */
+    virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap) = 0;
+    
+    /**
+     * @brief Get updated data from the previously set Costmap2D
+     * @warning The plugin should handle the costmap's mutex locking.
+     * @sa setCostmap2D
+     */
+    virtual void updateCostmap2D() = 0;
+    
+     /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute() = 0;
+    
+    /**
+     * @brief Get a shared instance of the current polygon container
+     *
+     * If this method is not implemented by any subclass (plugin) the returned shared
+     * pointer is empty.
+     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
+     * @warning The underlying plugin must ensure that this method is thread safe.
+     * @return Shared instance of the current polygon container
+     */
+    virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();}
+
+  /**
+   * @brief Get a shared instance of the current obstacle container
+   * If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons().
+   * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
+   * @warning The underlying plugin must ensure that this method is thread safe.
+   * @return Shared instance of the current obstacle container
+   * @sa getPolygons
+   */
+    virtual ObstacleArrayConstPtr getObstacles()
+    {
+      ObstacleArrayPtr obstacles = std::make_shared<costmap_converter_msgs::msg::ObstacleArrayMsg>();
+      PolygonContainerConstPtr polygons = getPolygons();
+      if (polygons)
+      {
+        for (const geometry_msgs::msg::Polygon& polygon : *polygons)
+        {
+          obstacles->obstacles.emplace_back();
+          obstacles->obstacles.back().polygon = polygon;
+        }
+      }
+      return obstacles;
+    }
+
+    /**
+     * @brief Set name of robot's odometry topic
+     *
+     * Some plugins might require odometry information
+     * to compensate the robot's ego motion
+     * @param odom_topic topic name
+     */
+    virtual void setOdomTopic(const std::string& odom_topic) { (void)odom_topic; }
+
+    /**
+     * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
+     *
+     * @return false, since all plugins for static costmap conversion are independent
+     */
+    virtual bool stackedCostmapConversion() {return false;}
+
+     /**
+      * @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons.
+      * The worker is implemented as a timer event that is invoked at a specific \c rate.
+      * The passed \c costmap pointer must be valid at the complete time and must be lockable.
+      * By specifying the argument \c spin_thread the timer event is invoked in a separate
+      * thread and callback queue or otherwise it is called from the global callback queue (of the
+      * node in which the plugin is used).
+      * @param rate The rate that specifies how often the costmap should be updated
+      * @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active
+      * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue)
+     */
+    void startWorker(rclcpp::Rate::SharedPtr rate, nav2_costmap_2d::Costmap2D* costmap, bool spin_thread = false)
+    {
+      setCostmap2D(costmap);
+      
+      if (spin_thread_)
+      {
+        {
+          std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
+          need_to_terminate_ = true;
+        }
+        spin_thread_->join();
+        delete spin_thread_;
+      }
+      
+      if (spin_thread)
+      {
+        RCLCPP_DEBUG(nh_->get_logger(), "Spinning up a thread for the CostmapToPolygons plugin");
+        need_to_terminate_ = false;
+        
+        worker_timer_ = nh_->create_wall_timer(
+                    rate->period(),
+                    std::bind(&BaseCostmapToPolygons::workerCallback, this));
+        spin_thread_ = new std::thread(std::bind(&BaseCostmapToPolygons::spinThread, this));
+      }
+      else
+      {
+        worker_timer_ = nh_->create_wall_timer(
+                    rate->period(),
+                    std::bind(&BaseCostmapToPolygons::workerCallback, this));
+        spin_thread_ = nullptr;
+      }
+    }
+    
+    /**
+     * @brief Stop the worker that repeatedly converts the costmap to polygons
+     */
+    void stopWorker()
+    {
+      if (worker_timer_) worker_timer_->cancel();
+      if (spin_thread_)
+      {
+        {
+          std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
+          need_to_terminate_ = true;
+        }
+        spin_thread_->join();
+        delete spin_thread_;
+      }
+    }
+
+protected:
+  
+    /**
+     * @brief Protected constructor that should be called by subclasses
+     */
+    BaseCostmapToPolygons() : //nh_("~costmap_to_polygons"),
+        nh_(nullptr),
+        spin_thread_(nullptr), need_to_terminate_(false) {}
+    
+    /**
+     * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) 
+     */
+    void spinThread()
+    {
+      rclcpp::WallRate r(10);
+      while (rclcpp::ok())
+      {
+        {
+          std::lock_guard<std::mutex> terminate_lock(terminate_mutex_);
+          if (need_to_terminate_)
+            break;
+          rclcpp::spin_some(nh_);
+          r.sleep();
+        }
+      }
+    }
+    
+    /**
+     * @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons)
+     */
+    void workerCallback()
+    {
+      updateCostmap2D();
+      compute();
+    }
+
+    rclcpp::Logger getLogger() const
+    {
+        return nh_->get_logger();
+    }
+
+    rclcpp::Time now() const
+    {
+        return nh_->now();
+    }
+    
+private:
+  rclcpp::TimerBase::SharedPtr worker_timer_;
+  rclcpp::Node::SharedPtr nh_;
+  std::thread* spin_thread_;
+  std::mutex terminate_mutex_;
+  bool need_to_terminate_;
+};    
+
+
+/**
+ * @class BaseCostmapToDynamicPolygons
+ * @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins in order to incorporate a subsequent costmap converter plugin for static obstacles
+ *
+ * This class should not be instantiated.
+ */
+class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons
+{
+public:
+
+  /**
+   * @brief Load underlying static costmap conversion plugin via plugin-loader
+   * @param plugin_name Exact class name of the plugin to be loaded, e.g.
+   *                    costmap_converter::CostmapToPolygonsDBSMCCH
+   * @param nh_parent   NodeHandle which is extended by the namespace of the static conversion plugin
+   */
+  void loadStaticCostmapConverterPlugin(const std::string& plugin_name, rclcpp::Node::SharedPtr nh_parent)
+  {
+    try
+    {
+      static_costmap_converter_ = static_converter_loader_.createSharedInstance(plugin_name);
+
+      if(std::dynamic_pointer_cast<BaseCostmapToDynamicObstacles>(static_costmap_converter_))
+      {
+        throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin.");
+      }
+//      std::string raw_plugin_name = static_converter_loader_.getName(plugin_name);
+      static_costmap_converter_->initialize(nh_parent);
+      setStaticCostmapConverterPlugin(static_costmap_converter_);
+      RCLCPP_INFO(getLogger(), "CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles %s loaded.", plugin_name.c_str());
+    }
+    catch(const pluginlib::PluginlibException& ex)
+    {
+      RCLCPP_WARN(getLogger(), "CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. "
+                               "Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what());
+      static_costmap_converter_.reset();
+    }
+  }
+
+  /**
+   * @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap
+   * @param static_costmap_converter shared pointer to the static costmap conversion plugin
+   */
+  void setStaticCostmapConverterPlugin(std::shared_ptr<BaseCostmapToPolygons> static_costmap_converter)
+  {
+    static_costmap_converter_ = static_costmap_converter;
+  }
+
+  /**
+   * @brief Set the costmap for the underlying plugin
+   * @param static_costmap Costmap2D, which contains the static part of the original costmap
+   */
+  void setStaticCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> static_costmap)
+  {
+    static_costmap_converter_->setCostmap2D(static_costmap.get());
+  }
+
+  /**
+   * @brief Apply the underlying plugin for static costmap conversion
+   */
+  void convertStaticObstacles()
+  {
+    static_costmap_converter_->compute();
+  }
+
+  /**
+   * @brief Get the converted polygons from the underlying plugin
+   * @return Shared instance of the underlying plugins polygon container
+   */
+  PolygonContainerConstPtr getStaticPolygons()
+  {
+    return static_costmap_converter_->getPolygons();
+  }
+
+  /**
+   * @brief Determines whether an additional plugin for subsequent costmap conversion is specified
+   *
+   * @return true, if a plugin for subsequent costmap conversion is specified
+   */
+  bool stackedCostmapConversion()
+  {
+    if(static_costmap_converter_)
+      return true;
+    else
+      return false;
+  }
+
+protected:
+  /**
+   * @brief Protected constructor that should be called by subclasses
+   */
+  BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {}
+
+private:
+  pluginlib::ClassLoader<BaseCostmapToPolygons> static_converter_loader_;
+  std::shared_ptr<BaseCostmapToPolygons> static_costmap_converter_;
+};
+
+
+}
+
+
+
+#endif // end COSTMAP_CONVERTER_INTERFACE_H_

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h

@@ -0,0 +1,115 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Notes:
+ * The following code makes use of the OpenCV library.
+ * OpenCV is licensed under the terms of the 3-clause BSD License.
+ *
+ * Authors: Franz Albers, Christoph Rösmann
+ *********************************************************************/
+
+#ifndef BACKGROUNDSUBTRACTOR_H_
+#define BACKGROUNDSUBTRACTOR_H_
+
+#include <cv_bridge/cv_bridge.h>
+
+/**
+ * @class BackgroundSubtractor
+ * @brief Perform background subtraction to extract the "moving" foreground
+ *
+ * This class is based on OpenCV's background subtraction class cv::BackgroundSubtractor.
+ * It has been modified in order to incorporate a specialized bandpass filter.
+ *
+ * See http://docs.opencv.org/3.2.0/d7/df6/classcv_1_1BackgroundSubtractor.html for the original class.
+ */
+class BackgroundSubtractor
+{
+public:
+  struct Params
+  {
+    double alpha_slow; //!< Filter constant (learning rate) of the slow filter part
+    double alpha_fast; //!< Filter constant (learning rate) of the fast filter part
+    double beta;
+    double min_sep_between_fast_and_slow_filter;
+    double min_occupancy_probability;
+    double max_occupancy_neighbors;
+    int morph_size;
+  };
+
+  //! Constructor that accepts a specific parameter configuration
+  BackgroundSubtractor(const Params& parameters);
+
+  /**
+   * @brief Computes a foreground mask
+   * @param[in]  image    Next video frame
+   * @param[out] fg_mask  Foreground mask as an 8-bit binary image
+   * @param[in]  shift_x  Translation along the x axis between the current and previous image
+   * @param[in]  shift_y  Translation along the y axis between the current and previous image
+   */
+  void apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x = 0, int shift_y = 0);
+
+  /**
+   * @brief OpenCV Visualization
+   * @param name  Id/name of the opencv window
+   * @param image Image to be visualized
+   */
+  void visualize(const std::string& name, const cv::Mat& image);
+
+  /**
+   * @brief Export vector of matrices to yaml file
+   * @remarks This method is intended for debugging purposes
+   * @param filename   Desired filename including path and excluding file suffix
+   * @param mat_vec    Vector of cv::Mat to be exported
+   */
+  void writeMatToYAML(const std::string& filename, const std::vector<cv::Mat>& mat_vec);
+
+  //! Update internal parameters
+  void updateParameters(const Params& parameters);
+
+private:
+  //! Transform/shift all internal matrices/grids according to a given translation vector
+  void transformToCurrentFrame(int shift_x, int shift_y);
+
+  cv::Mat occupancy_grid_fast_;
+  cv::Mat occupancy_grid_slow_;
+  cv::Mat current_frame_;
+
+  int previous_shift_x_;
+  int previous_shift_y_;
+
+  Params params_;
+};
+
+#endif // BACKGROUNDSUBTRACTOR_H_

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h

@@ -0,0 +1,111 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Notes:
+ * The following code makes use of the OpenCV library.
+ * OpenCV is licensed under the terms of the 3-clause BSD License.
+ *
+ * Authors: Franz Albers, Christoph Rösmann
+ *********************************************************************/
+
+#ifndef BLOBDETECTOR_H_
+#define BLOBDETECTOR_H_
+
+// Basically the OpenCV SimpleBlobDetector, extended with getContours()
+
+#include <opencv2/features2d/features2d.hpp>
+
+/**
+ * @class BlobDetector
+ * @brief Detect blobs in image (specialized for dynamic obstacles in the costmap)
+ *
+ * This class is based on OpenCV's blob detector cv::SimpleBlobDetector.
+ * It has been modified and specialized for dynamic obstacle tracking in the costmap:
+ * -> The modified version also returns contours of the blob.
+ *
+ * See http://docs.opencv.org/trunk/d0/d7a/classcv_1_1SimpleBlobDetector.html for the original class.
+ */
+class BlobDetector : public cv::SimpleBlobDetector
+{
+public:
+  //! Default constructor which optionally accepts custom parameters
+  BlobDetector(const cv::SimpleBlobDetector::Params& parameters = cv::SimpleBlobDetector::Params());
+
+  //! Create shared instance of the blob detector with given parameters
+  static cv::Ptr<BlobDetector> create(const BlobDetector::Params& params);
+
+  /**
+   * @brief Detects keypoints in an image and extracts contours
+   *
+   * In contrast to the original detect method, this extended version
+   * also extracts contours. Contours can be accessed by getContours()
+   * after invoking this method.
+   *
+   * @todo The mask option is currently not implemented.
+   *
+   * @param image     image
+   * @param keypoints The detected keypoints.
+   * @param mask      Mask specifying where to look for keypoints (optional). It must be a 8-bit integer
+   *                  matrix with non-zero values in the region of interest.
+   */
+  virtual void detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,
+                      const cv::Mat& mask = cv::Mat());
+
+  /**
+   * @brief Access contours extracted during detection stage
+   * @return Read-only reference to the contours set of the previous detect() run
+   */
+  const std::vector<std::vector<cv::Point>>& getContours() { return contours_; }
+
+  //! Update internal parameters
+  void updateParameters(const cv::SimpleBlobDetector::Params& parameters);
+
+protected:
+  struct Center
+  {
+    cv::Point2d location;
+    double radius;
+    double confidence;
+  };
+
+  virtual void findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
+                         std::vector<std::vector<cv::Point>>& cur_contours) const;
+
+  std::vector<std::vector<cv::Point>> contours_;
+
+  Params params_;
+};
+
+#endif // BLOBDETECTOR_H_

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h

@@ -0,0 +1,212 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Notes:
+ * The following code makes use of the OpenCV library.
+ * OpenCV is licensed under the terms of the 3-clause BSD License.
+ *
+ * Authors: Franz Albers, Christoph Rösmann
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_
+#define COSTMAP_TO_DYNAMIC_OBSTACLES_H_
+
+// ROS
+#include <costmap_converter/costmap_converter_interface.h>
+#include <nav_msgs/msg/odometry.hpp>
+#include <pluginlib/class_loader.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+// OpenCV
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/video/tracking.hpp>
+
+// dynamic reconfigure
+//#include <costmap_converter/CostmapToDynamicObstaclesConfig.h>
+//#include <dynamic_reconfigure/server.h>
+
+// Own includes
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
+#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
+#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
+
+// STL
+#include <memory>
+
+namespace costmap_converter {
+
+/**
+ * @class CostmapToDynamicObstacles
+ * @brief This class converts the costmap_2d into dynamic obstacles.
+ *
+ * Static obstacles are treated as point obstacles.
+ */
+class CostmapToDynamicObstacles : public BaseCostmapToDynamicObstacles
+{
+public:
+  /**
+   * @brief Constructor
+   */
+  CostmapToDynamicObstacles();
+
+  /**
+   * @brief Destructor
+   */
+  virtual ~CostmapToDynamicObstacles();
+
+  /**
+   * @brief Initialize the plugin
+   * @param nh Nodehandle that defines the namespace for parameters
+   */
+  virtual void initialize(rclcpp::Node::SharedPtr nh);
+
+  /**
+   * @brief This method performs the actual work (conversion of the costmap to
+   * obstacles)
+   */
+  virtual void compute();
+
+  /**
+   * @brief Pass a pointer to the costmap to the plugin.
+   * @sa updateCostmap2D
+   * @param costmap Pointer to the costmap2d source
+   */
+  virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap);
+
+  /**
+   * @brief Get updated data from the previously set Costmap2D
+   * @sa setCostmap2D
+   */
+  virtual void updateCostmap2D();
+
+  /**
+   * @brief Get a shared instance of the current obstacle container
+   * @remarks If compute() or startWorker() has not been called before, this
+   * method returns an empty instance!
+   * @return Shared instance of the current obstacle container
+   */
+  ObstacleArrayConstPtr getObstacles();
+
+  /**
+   * @brief Set name of robot's odometry topic
+   *
+   * @warning The method must be called before initialize()
+   *
+   * Some plugins might require odometry information
+   * to compensate the robot's ego motion
+   * @param odom_topic topic name
+   */
+  virtual void setOdomTopic(const std::string& odom_topic)
+  {
+    odom_topic_ = odom_topic;
+  }
+
+  /**
+   * @brief OpenCV Visualization
+   * @param name  Id/name of the opencv window
+   * @param image Image to be visualized
+   */
+  void visualize(const std::string& name, const cv::Mat& image);
+
+protected:
+  /**
+   * @brief Converts the estimated velocity of a tracked object to m/s and
+   * returns it
+   * @param idx Index of the tracked object in the tracker
+   * @return Point_t, which represents velocity in [m/s] of object(idx) in x,y,z
+   * coordinates
+   */
+  Point_t getEstimatedVelocityOfObject(unsigned int idx);
+
+  /**
+   * @brief Gets the last observed contour of a object and converts it from [px]
+   * to [m]
+   * @param[in]  idx Index of the tracked object in the tracker
+   * @param[out] contour vector of Point_t, which represents the last observed contour in [m]
+   *             in x,y,z coordinates
+   */
+  void getContour(unsigned int idx, std::vector<Point_t>& contour);
+
+  /**
+   * @brief Thread-safe update of the internal obstacle container (that is
+   * shared using getObstacles() from outside this
+   * class)
+   * @param obstacles Updated obstacle container
+   */
+  void updateObstacleContainer(ObstacleArrayPtr obstacles);
+
+private:
+  std::mutex mutex_;
+  nav2_costmap_2d::Costmap2D* costmap_;
+  cv::Mat costmap_mat_;
+  ObstacleArrayPtr obstacles_;
+  cv::Mat fg_mask_;
+  std::unique_ptr<BackgroundSubtractor> bg_sub_;
+  cv::Ptr<BlobDetector> blob_det_;
+  std::vector<cv::KeyPoint> keypoints_;
+  std::unique_ptr<CTracker> tracker_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  Point_t ego_vel_;
+
+  std::string odom_topic_ = "/odom";
+  bool publish_static_obstacles_ = true;
+
+//  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>*
+//      dynamic_recfg_; //!< Dynamic reconfigure server to allow config
+//                       //! modifications at runtime
+
+  /**
+   * @brief Callback for the odometry messages of the observing robot.
+   *
+   * Used to convert the velocity of obstacles to the /map frame.
+   * @param msg The Pointer to the nav_msgs::Odometry of the observing robot
+   */
+  void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
+
+  /**
+   * @brief Callback for the dynamic_reconfigure node.
+   *
+   * This callback allows to modify parameters dynamically at runtime without
+   * restarting the node
+   * @param config Reference to the dynamic reconfigure config
+   * @param level Dynamic reconfigure level
+   */
+//  void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level);
+};
+
+} // end namespace costmap_converter
+
+#endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h

@@ -0,0 +1,96 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#pragma once
+#include "Kalman.h"
+#include "HungarianAlg.h"
+#include "defines.h"
+#include <iostream>
+#include <vector>
+#include <memory>
+#include <array>
+
+// --------------------------------------------------------------------------
+class CTrack
+{
+public:
+  CTrack(const Point_t& p, const std::vector<cv::Point>& contour, track_t dt, size_t trackID)
+      : track_id(trackID),
+        skipped_frames(0),
+        prediction(p),
+        lastContour(contour),
+        KF(p, dt)
+  {
+  }
+
+  track_t CalcDist(const Point_t& p)
+  {
+    Point_t diff = prediction - p;
+    return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
+  }
+
+  void Update(const Point_t& p, const std::vector<cv::Point>& contour, bool dataCorrect, size_t max_trace_length)
+  {
+    KF.Prediction();
+    prediction = KF.Update(p, dataCorrect);
+
+    if (dataCorrect)
+    {
+      lastContour = contour;
+    }
+
+    if (trace.size() > max_trace_length)
+    {
+      trace.erase(trace.begin(), trace.end() - max_trace_length);
+    }
+
+    trace.push_back(prediction);
+  }
+
+  // Returns contour in [px], not in [m]!
+  std::vector<cv::Point> getLastContour() const
+  {
+    return lastContour;
+  }
+
+  // Returns velocity in [px/s], not in [m/s]!
+  Point_t getEstimatedVelocity() const
+  {
+    return KF.LastVelocity;
+  }
+
+  std::vector<Point_t> trace;
+  size_t track_id;
+  size_t skipped_frames;
+
+private:
+  Point_t prediction;
+  std::vector<cv::Point> lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt
+  TKalmanFilter KF;
+};
+
+// --------------------------------------------------------------------------
+class CTracker
+{
+public:
+  struct Params{
+    track_t dt; // time for one step of the filter
+    track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem
+    int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data
+    int max_trace_length; // Maximum trace length
+  };
+
+  CTracker(const Params& parameters);
+  ~CTracker(void);
+
+  std::vector<std::unique_ptr<CTrack>> tracks;
+  void Update(const std::vector<Point_t>& detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);
+
+  void updateParameters(const Params &parameters);
+
+private:
+  // Aggregated parameters for the tracker object
+  Params params;
+  // ID for the upcoming CTrack object
+  size_t NextTrackID;
+};

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h

@@ -0,0 +1,60 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <vector>
+#include <iostream>
+#include <limits>
+#include <time.h>
+#include "defines.h"
+
+// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm
+
+typedef std::vector<int> assignments_t;
+typedef std::vector<track_t> distMatrix_t;
+
+class AssignmentProblemSolver
+{
+private:
+  // --------------------------------------------------------------------------
+  // Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
+  // --------------------------------------------------------------------------
+  void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
+                         size_t nOfColumns);
+  void buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, size_t nOfColumns);
+  void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn,
+                             size_t nOfRows);
+  void step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  void step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+              bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  void step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  void step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row,
+             size_t col);
+  void step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix,
+             bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim);
+  // --------------------------------------------------------------------------
+  // Computes a suboptimal solution. Good for cases with many forbidden assignments.
+  // --------------------------------------------------------------------------
+  void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
+                             size_t nOfColumns);
+  // --------------------------------------------------------------------------
+  // Computes a suboptimal solution. Good for cases with many forbidden assignments.
+  // --------------------------------------------------------------------------
+  void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows,
+                             size_t nOfColumns);
+
+public:
+  enum TMethod
+  {
+    optimal,
+    many_forbidden_assignments,
+    without_forbidden_assignments
+  };
+
+  AssignmentProblemSolver();
+  ~AssignmentProblemSolver();
+  track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment,
+                TMethod Method = optimal);
+};

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h

@@ -0,0 +1,20 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#pragma once
+#include "defines.h"
+#include <opencv2/opencv.hpp>
+
+// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/
+class TKalmanFilter
+{
+public:
+  TKalmanFilter(Point_t p, track_t deltatime = 0.2);
+  ~TKalmanFilter();
+  void Prediction();
+  Point_t Update(Point_t p, bool DataCorrect);
+  cv::KalmanFilter* kalman;
+  track_t dt;
+  Point_t LastPosition; // contour in [px]
+  Point_t LastVelocity; // velocity in [px/s]
+};

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md

@@ -0,0 +1,13 @@
+Multitarget Tracker
+===================
+
+This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
+It is utilized for the *CostmapToDynamicObstacles* plugin.
+
+The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
+The package itself depends on other third party packages with different licenses (refer to the package repository).
+
+TODO: Include the whole package as external CMake project.
+
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h

@@ -0,0 +1,9 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#pragma once
+#include <opencv2/opencv.hpp>
+
+typedef float track_t;
+typedef cv::Point3_<track_t> Point_t;
+#define Mat_t CV_32FC

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h

@@ -0,0 +1,137 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_LINES_CONVEX_HULL_H_
+#define COSTMAP_TO_LINES_CONVEX_HULL_H_
+
+#include <costmap_converter/costmap_converter_interface.h>
+#include <costmap_converter/costmap_to_polygons.h>
+
+// dynamic reconfigure
+//#include <costmap_converter/CostmapToLinesDBSMCCHConfig.h>
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToLinesDBSMCCH
+ * @brief This class converts the costmap_2d into a set of lines (and points)
+ * 
+ * The conversion is performed in three stages:
+ * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+ *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+ *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+ *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+ *    
+ * 2. In the subsequent stage, the convex hull of each cluster is determined using the monotone chain algorithm aka Andrew's algorithm:
+ *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
+ *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
+ * 
+ * 3. In the third step extract lines from each polygon (convex hull) if there exist at least a predefined number of support points.
+ * 
+ * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
+ */
+  class CostmapToLinesDBSMCCH : public CostmapToPolygonsDBSMCCH
+  {
+  public:
+   
+    /**
+     * @brief Constructor
+     */
+    CostmapToLinesDBSMCCH();
+       
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToLinesDBSMCCH();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(rclcpp::Node::SharedPtr nh);
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();   
+    
+    
+  protected:
+    
+    /**
+     * @brief Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points
+     * @param cluster list of points in the cluster
+     * @param polygon convex hull of the cluster \c cluster
+     * @param[out] lines back_inserter object to a sequence of polygon msgs (new lines will be pushed back)
+     */
+    void extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::msg::Polygon& polygon, std::back_insert_iterator< std::vector<geometry_msgs::msg::Polygon> > lines);
+
+    
+    
+  protected:
+      
+    double support_pts_max_dist_inbetween_;
+    double support_pts_max_dist_;
+    int min_support_pts_;
+   
+  private:
+    
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+//    void reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level);
+    
+//    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
+ 
+  };  
+  
+  
+  
+  
+
+
+  
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_LINES_CONVEX_HULL_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h

@@ -0,0 +1,187 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_LINES_RANSAC_H_
+#define COSTMAP_TO_LINES_RANSAC_H_
+
+#include <costmap_converter/costmap_converter_interface.h>
+#include <costmap_converter/costmap_to_polygons.h>
+#include <costmap_converter/misc.h>
+
+#include <random>
+
+//#include <costmap_converter/CostmapToLinesDBSRANSACConfig.h>
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToLinesDBSRANSAC
+ * @brief This class converts the costmap_2d into a set of lines (and points)
+ * 
+ * The conversion is performed in two stages:
+ * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+ *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+ *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+ *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+ *    
+ * 2. The RANSAC algorithm is used to find straight line segment models (https://en.wikipedia.org/wiki/RANSAC)
+ *    RANSAC is called repeatedly to find multiple lines per cluster until the number of inliners is below a specific threshold.
+ *    In that case the remaining outliers are used as points or keypoints of their convex hull are used as points (depending on a paramter).
+ *    The latter one applies as a filter. The convex assumption is not strict in practice, since the remaining regions/cluster (line inliers are removed)
+ *    should be dense and small. For details about the convex hull algorithm, refer to costmap_converter::CostmapToPolygonsDBSMCCH.
+ *    Resulting lines of RANSAC are currently not refined by linear regression.
+ * 
+ * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line)
+ */
+  class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH
+  {
+  public:
+   
+    /**
+     * @brief Constructor
+     */
+    CostmapToLinesDBSRANSAC();
+       
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToLinesDBSRANSAC();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(rclcpp::Node::SharedPtr nh);
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();   
+        
+     
+    /**
+     * @brief Check if the candidate point is an inlier.
+     * @param point generic 2D point type defining the reference point
+     * @param line_start generic 2D point type defining the start of the line
+     * @param line_end generic 2D point type defining the end of the line
+     * @param min_distance minimum distance allowed
+     * @tparam Point generic point type that should provide (writable) x and y member fiels.
+     * @tparam LinePoint generic point type that should provide (writable) x and y member fiels.
+     * @return \c true if inlier, \c false otherwise
+     */
+    template <typename Point, typename LinePoint>
+    static bool isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance);
+    
+  protected:
+    
+    double ransac_inlier_distance_; //!< Maximum distance to the line segment for inliers
+    int ransac_min_inliers_; //!< Minimum numer of inliers required to form a line
+    int ransac_no_iterations_; //!< Number of ransac iterations
+    int ransac_remainig_outliers_; //!< Repeat ransac until the number of outliers is as specified here
+    bool ransac_convert_outlier_pts_; //!< If \c true, convert remaining outliers to single points.
+    bool ransac_filter_remaining_outlier_pts_; //!< If \c true, filter the interior of remaining outliers and keep only keypoints of their convex hull
+   
+   
+   std::mt19937 rnd_generator_; //!< Random number generator for ransac with default seed
+    
+    
+    /**
+     * @brief Find a straight line segment in a point cloud with RANSAC (without linear regression).
+     * @param data set of 2D data points
+     * @param inlier_distance maximum distance that inliers must satisfy
+     * @param no_iterations number of RANSAC iterations
+     * @param min_inliers minimum number of inliers to return true
+     * @param[out] best_model start and end of the best straight line segment 
+     * @param[out] inliers inlier keypoints are written to this container [optional]
+     * @param[out] outliers outlier keypoints are written to this container [optional]
+     * @return \c false, if \c min_inliers is not satisfied, \c true otherwise
+     */
+    bool lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations, int min_inliers,
+                    std::pair<KeyPoint, KeyPoint>& best_model, std::vector<KeyPoint>* inliers = NULL,
+                     std::vector<KeyPoint>* outliers = NULL);
+    
+    /**
+     * @brief Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept'
+     * @param data set of 2D data points
+     * @param[out] slope The slope of the fitted line 
+     * @param[out] intercept The intercept / offset of the line
+     * @param[out] mean_x_out mean of the x-values of the data [optional]
+     * @param[out] mean_y_out mean of the y-values of the data [optional]
+     * @return \c true, if a valid line has been fitted, \c false otherwise.
+     */
+    bool linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept,
+                          double* mean_x_out = NULL, double* mean_y_out = NULL);
+    
+    
+    
+//     void adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
+//                           KeyPoint& line_start, KeyPoint& line_end);
+    
+    private:
+    
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+//    void reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level);
+    
+//    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
+  };  
+  
+  
+
+template <typename Point, typename LinePoint>
+bool CostmapToLinesDBSRANSAC::isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance)
+{
+  bool is_inbetween = false;
+  double distance = computeDistanceToLineSegment(point, line_start, line_end, &is_inbetween);
+  if (!is_inbetween)
+    return false;
+  if (distance <= min_distance)
+    return true;
+  return false;
+}
+     
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_LINES_RANSAC_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons.h

@@ -0,0 +1,335 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_POLYGONS_H_
+#define COSTMAP_TO_POLYGONS_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <costmap_converter/costmap_converter_interface.h>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+#include <vector>
+#include <algorithm>
+#include <mutex>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+// dynamic reconfigure
+//#include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>
+//#include <dynamic_reconfigure/server.h>
+
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToPolygonsDBSMCCH
+ * @brief This class converts the costmap_2d into a set of convex polygons (and points)
+ * 
+ * The conversion is performed in two stages:
+ * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+ *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+ *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+ *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+ *    
+ * 2. In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
+ *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
+ *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
+ *  
+ * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
+ */
+class CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons
+{
+  public:
+    
+    /**
+     * @struct KeyPoint
+     * @brief Defines a keypoint in metric coordinates of the map 
+     */
+    struct KeyPoint
+    {
+      //! Default constructor
+      KeyPoint() : x(0.0), y(0.0) {}
+      //! Constructor with point initialization
+      KeyPoint(double x_, double y_) : x(x_), y(y_) {}
+      double x; //!< x coordinate [m]
+      double y; //!< y coordinate [m]
+      
+      //! Convert keypoint to geometry_msgs::msg::Point message type
+      void toPointMsg(geometry_msgs::msg::Point& point) const {point.x=x; point.y=y; point.z=0;}
+      //! Convert keypoint to geometry_msgs::msg::Point32 message type
+      void toPointMsg(geometry_msgs::msg::Point32& point) const {point.x=x; point.y=y; point.z=0;}
+    };
+
+    /**
+     * @struct Parameters
+     * @brief Defines the parameters of the algorithm
+     */
+    struct Parameters
+    {
+      Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {}
+      // DBSCAN parameters
+      double max_distance_; //!< Parameter for DB_Scan, maximum distance to neighbors [m]
+      int min_pts_; //!< Parameter for DB_Scan: minimum number of points that define a cluster
+      int max_pts_; //!< Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes)
+      
+      // convex hull parameters
+      double min_keypoint_separation_; //!< Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)
+    };
+    
+    /**
+     * @brief Constructor
+     */
+    CostmapToPolygonsDBSMCCH();
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToPolygonsDBSMCCH();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(rclcpp::Node::SharedPtr nh) override;
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();
+        
+    /**
+     * @brief Pass a pointer to the costap to the plugin.
+     * @sa updateCostmap2D
+     * @param costmap Pointer to the costmap2d source
+     */
+    virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap);
+    
+    /**
+     * @brief Get updated data from the previously set Costmap2D
+     * @sa setCostmap2D
+     */
+    virtual void updateCostmap2D();
+    
+    
+    /**
+     * @brief Convert a generi point type to a geometry_msgs::msg::Polygon
+     * @param point generic 2D point type
+     * @param[out] polygon already instantiated polygon that will be filled with a single point
+     * @tparam Point generic point type that should provide (writable) x and y member fiels.
+     */
+    template< typename Point>
+    static void convertPointToPolygon(const Point& point, geometry_msgs::msg::Polygon& polygon)
+    {
+      polygon.points.resize(1);
+      polygon.points.front().x = point.x;
+      polygon.points.front().y = point.y;
+      polygon.points.front().z = 0;
+    }
+    
+    /**
+     * @brief Get a shared instance of the current polygon container
+     * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance!
+     * @return Shared instance of the current polygon container
+     */
+    PolygonContainerConstPtr getPolygons();
+
+    
+    
+  protected:
+    
+    /**
+     * @brief DBSCAN algorithm for clustering 
+     * 
+     * Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN)
+     *    Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei,
+     *               A density-based algorithm for discovering clusters in large spatial databases with noise. 
+     *               Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. 
+     * 
+     * @param[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster)
+     *                      the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition 
+     */
+    void dbScan(std::vector< std::vector<KeyPoint> >& clusters);
+    
+    /**
+     * @brief Helper function for dbScan to search for neighboring points 
+     * 
+     * @param curr_index index to the current item in \c occupied_cells
+     * @param[out] neighbor_indices list of neighbors (indices of \c occupied cells)
+     */
+    void regionQuery(int curr_index, std::vector<int>& neighbor_indices);
+
+    /**
+     * @brief helper function for adding a point to the lookup data structures
+     */
+    void addPoint(double x, double y)
+    {
+      int idx = occupied_cells_.size();
+      occupied_cells_.emplace_back(x, y);
+      int cx, cy;
+      pointToNeighborCells(occupied_cells_.back(), cx, cy);
+      int nidx = neighborCellsToIndex(cx, cy);
+      if (nidx >= 0)
+        neighbor_lookup_[nidx].push_back(idx);
+    }
+    
+    /**
+     * @brief Compute the convex hull for a single cluster (monotone chain algorithm)
+     * 
+     * Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm:
+     *    C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 )
+     *    Reference:  A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979).
+     * @remarks The algorithm seems to cut edges, thus we give a try to convexHull2().
+     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
+     * @remarks The last point is the same as the first one
+     * @param cluster list of keypoints that should be converted into a polygon
+     * @param[out] polygon the resulting convex polygon
+     */  
+    void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon);
+    
+    /**
+     * @brief Compute the convex hull for a single cluster
+     * 
+     * Clusters are converted into convex polgons using an algorithm provided here:
+     *  https://bitbucket.org/vostreltsov/concave-hull/overview
+     * The convex hull algorithm is part of the concave hull algorithm.
+     * The license is WTFPL 2.0 and permits any usage.
+     * 
+     * @remarks The last point is the same as the first one
+     * @param cluster list of keypoints that should be converted into a polygon
+     * @param[out] polygon the resulting convex polygon
+     * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...)
+     */  
+    void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon);
+
+    /**
+     * @brief Simplify a polygon by removing points.
+     * 
+     * We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon.
+     * Internally, the parameter min_keypoint_separation is used for deciding whether
+     * a point is considered close to an edge and to be merged into the line.
+     */
+    void simplifyPolygon(geometry_msgs::msg::Polygon& polygon);
+    
+   /**
+    * @brief 2D Cross product of two vectors defined by two points and a common origin
+    * @param O Origin
+    * @param A First point
+    * @param B Second point
+    * @tparam P1 2D Point type with x and y member fields
+    * @tparam P2 2D Point type with x and y member fields
+    * @tparam P3 2D Point type with x and y member fields
+    */    
+    template <typename P1, typename P2, typename P3>
+    long double cross(const P1& O, const P2& A, const P3& B)
+    { 
+        return (long double)(A.x - O.x) * (long double)(B.y - O.y) - (long double)(A.y - O.y) * (long double)(B.x - O.x);
+    }
+    
+      
+   /**
+    * @brief Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class)
+    * @param polygons Updated polygon container
+    */
+   void updatePolygonContainer(PolygonContainerPtr polygons);   
+          
+   
+   std::vector<KeyPoint> occupied_cells_; //!< List of occupied cells in the current map (updated by updateCostmap2D())
+   
+    std::vector<std::vector<int> > neighbor_lookup_; //! array of cells for neighbor lookup
+    int neighbor_size_x_; //! size of the neighbour lookup in x (number of cells)
+    int neighbor_size_y_; //! size of the neighbour lookup in y (number of cells)
+    double offset_x_;     //! offset [meters] in x for the lookup grid
+    double offset_y_;     //! offset [meters] in y for the lookup grid
+
+    /**
+      * @brief convert a 2d cell coordinate into the 1D index of the array
+      * @param cx the x index of the cell
+      * @param cy the y index of the cell
+      */
+    int neighborCellsToIndex(int cx, int cy)
+    {
+      if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_)
+        return -1;
+      return cy * neighbor_size_x_ + cx;
+    }
+
+    /**
+      * @brief compute the cell indices of a keypoint
+      * @param kp key point given in world coordinates [m, m]
+      * @param cx output cell index in x direction
+      * @param cy output cell index in y direction
+      */
+    void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)
+    {
+      cx = int((kp.x - offset_x_) / parameter_.max_distance_);
+      cy = int((kp.y - offset_y_) / parameter_.max_distance_);
+    }
+
+
+    Parameters parameter_;          //< active parameters throughout computation
+    Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure
+    std::mutex parameter_mutex_;  //!< Mutex that keeps track about the ownership of the shared polygon instance
+   
+  private:
+       
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+    //void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);
+    
+    
+    PolygonContainerPtr polygons_; //!< Current shared container of polygons
+    std::mutex mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance
+    
+    //dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
+   
+    nav2_costmap_2d::Costmap2D *costmap_; //!< Pointer to the costmap2d
+   
+}; 
+
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_POLYGONS_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h

@@ -0,0 +1,202 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef COSTMAP_TO_POLYGONS_CONCAVE_H_
+#define COSTMAP_TO_POLYGONS_CONCAVE_H_
+
+#include <costmap_converter/costmap_to_polygons.h>
+#include <costmap_converter/misc.h>
+
+// dynamic reconfigure
+//#include <costmap_converter/CostmapToPolygonsDBSConcaveHullConfig.h>
+//#include <dynamic_reconfigure/server.h>
+
+
+namespace costmap_converter
+{
+  
+/**
+ * @class CostmapToPolygonsDBSConcaveHull
+ * @brief This class converts the costmap_2d into a set of non-convex polygons (and points)
+ *  
+ * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex)
+ * @todo change the class hierarchy to a clearer and more generic one
+ */
+class CostmapToPolygonsDBSConcaveHull : public CostmapToPolygonsDBSMCCH
+{
+  public:
+    
+    
+    
+    /**
+     * @brief Constructor
+     */
+    CostmapToPolygonsDBSConcaveHull();
+    
+    /**
+     * @brief Destructor
+     */
+    virtual ~CostmapToPolygonsDBSConcaveHull();
+    
+    /**
+     * @brief Initialize the plugin
+     * @param nh Nodehandle that defines the namespace for parameters
+     */
+    virtual void initialize(rclcpp::Node::SharedPtr nh);
+   
+    
+    /**
+     * @brief This method performs the actual work (conversion of the costmap to polygons)
+     */
+    virtual void compute();
+    
+  protected:
+ 
+   
+    /**
+     * @brief Compute the concave hull for a single cluster
+     * 
+     * @remarks The last point is the same as the first one
+     * @param cluster list of keypoints that should be converted into a polygon
+     * @param depth Smaller depth: sharper surface, depth -> high value: convex hull
+     * @param[out] polygon the resulting convex polygon
+     */  
+    void concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon);
+        
+    void concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon);
+    
+    template <typename PointLine, typename PointCluster, typename PointHull>
+    std::size_t findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found);
+        
+    
+    template <typename Point1, typename Point2, typename Point3, typename Point4>
+    bool checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end);
+    
+    template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
+    bool checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, 
+                               const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end);
+    
+    double concave_hull_depth_;
+
+  private:
+       
+    /**
+     * @brief Callback for the dynamic_reconfigure node.
+     * 
+     * This callback allows to modify parameters dynamically at runtime without restarting the node
+     * @param config Reference to the dynamic reconfigure config
+     * @param level Dynamic reconfigure level
+     */
+//    void reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level);
+    
+//    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
+   
+   
+}; 
+
+
+template <typename PointLine, typename PointCluster, typename PointHull>
+std::size_t CostmapToPolygonsDBSConcaveHull::findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector<PointCluster>& cluster, const std::vector<PointHull>& hull, bool* found)
+{
+    std::size_t nearsest_idx = 0;
+    double distance = 0;
+    *found = false;
+
+    for (std::size_t i = 0; i < cluster.size(); ++i)
+    {
+        // Skip points that are already in the hull
+        if (std::find_if( hull.begin(), hull.end(), std::bind(isApprox2d<PointHull, PointCluster>, std::placeholders::_1, std::cref(cluster[i]), 1e-5) ) != hull.end() )
+            continue;
+
+        double dist = computeDistanceToLineSegment(cluster[i], line_start, line_end);
+        bool skip = false;
+        for (int j = 0; !skip && j < (int)hull.size() - 1; ++j) 
+        {
+            double dist_temp = computeDistanceToLineSegment(cluster[i], hull[j], hull[j + 1]);
+            skip |= dist_temp < dist;
+        }
+        if (skip) 
+            continue;
+
+        if (!(*found) || dist < distance) 
+        {
+            nearsest_idx = i;
+            distance = dist;
+            *found = true;
+        }
+    }
+    return nearsest_idx;
+}
+
+
+template <typename Point1, typename Point2, typename Point3, typename Point4>
+bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end)
+{
+    double dx1 = line1_end.x - line1_start.x;
+    double dy1 = line1_end.y - line1_start.y;
+    double dx2 = line2_end.x - line2_start.x;
+    double dy2 = line2_end.y - line2_start.y;
+    double s = (-dy1 * (line1_start.x - line2_start.x) + dx1 * (line1_start.y - line2_start.y)) / (-dx2 * dy1 + dx1 * dy2);
+    double t = ( dx2 * (line1_start.y - line2_start.y) - dy2 * (line1_start.x - line2_start.x)) / (-dx2 * dy1 + dx1 * dy2);
+    return (s > 0 && s < 1 && t > 0 && t < 1);
+}
+
+
+template <typename PointHull, typename Point1, typename Point2, typename Point3, typename Point4>
+bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const std::vector<PointHull>& hull, const Point1& current_line_start, 
+                                                            const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end)
+{
+    for (int i = 0; i < (int)hull.size() - 2; i++) 
+    {
+        if (isApprox2d(current_line_start, hull[i], 1e-5) && isApprox2d(current_line_end, hull[i+1], 1e-5))
+        {
+            continue;
+        }
+
+        if (checkLineIntersection(test_line_start, test_line_end, hull[i], hull[i+1])) 
+        {
+            return true;
+        }
+    }
+    return false;
+} 
+  
+  
+} //end namespace teb_local_planner
+
+#endif /* COSTMAP_TO_POLYGONS_CONCAVE_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/misc.h

@@ -0,0 +1,157 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef MISC_H_
+#define MISC_H_
+
+#include <algorithm>
+#include <cmath>
+
+namespace costmap_converter
+{
+
+/**
+  * @brief Calculate the distance between a point and a straight line (with infinite length)
+  * @param point generic 2D point type defining the reference point
+  * @param line_pt1 generic 2D point as part of the line
+  * @param line_pt2 generic 2D point as part of the line
+  * @tparam Point generic point type that should provide x and y member fiels.
+  * @tparam LinePoint generic point type that should provide x and y member fiels.
+  * @return (minimum) euclidean distance to the line segment
+  */
+template <typename Point, typename LinePoint>
+inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)
+{
+    double dx = line_pt2.x - line_pt1.x;
+    double dy = line_pt2.y - line_pt1.y;
+    
+    double length = std::sqrt(dx*dx + dy*dy);
+    
+    if (length>0)
+      return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;
+  
+    return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));  
+}
+
+
+/**
+  * @brief Calculate the squared distance between a point and a straight line segment
+  * @param point generic 2D point type defining the reference point
+  * @param line_start generic 2D point type defining the start of the line
+  * @param line_end generic 2D point type defining the end of the line
+  * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
+  * @tparam Point generic point type that should provide x and y member fiels.
+  * @tparam LinePoint generic point type that should provide x and y member fiels.
+  * @return (minimum) squared euclidean distance to the line segment
+  */
+template <typename Point, typename LinePoint>
+inline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
+{
+    double dx = line_end.x - line_start.x;
+    double dy = line_end.y - line_start.y;
+    
+    double length_sqr = dx*dx + dy*dy;
+
+    double u = 0;
+
+    if (length_sqr > 0)
+      u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr;
+
+    if (is_inbetween)
+      *is_inbetween = (u>=0 && u<=1);
+  
+    if (u <= 0)
+      return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2);
+    
+    if (u >= 1)
+      return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2);
+    
+    return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2);
+}
+  
+/**
+  * @brief Calculate the distance between a point and a straight line segment
+  * @param point generic 2D point type defining the reference point
+  * @param line_start generic 2D point type defining the start of the line
+  * @param line_end generic 2D point type defining the end of the line
+  * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional]
+  * @tparam Point generic point type that should provide x and y member fiels.
+  * @tparam LinePoint generic point type that should provide x and y member fiels.
+  * @return (minimum) euclidean distance to the line segment
+  */
+template <typename Point, typename LinePoint>
+inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
+{
+  return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween));
+}
+  
+
+/**
+  * @brief Calculate the distance between two 2d points
+  * @param pt1 generic 2D point
+  * @param pt2 generic 2D point
+  * @tparam Point1 generic point type that should provide x and y member fiels.
+  * @tparam Point2 generic point type that should provide x and y member fiels.
+  * @return euclidean distance to the line segment
+  */
+template <typename Point1, typename Point2>  
+inline double norm2d(const Point1& pt1, const Point2& pt2)
+{
+  return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2)  );
+}
+
+/**
+  * @brief Check if two points are approximately defining the same one
+  * @param pt1 generic 2D point
+  * @param pt2 generic 2D point
+  * @param threshold define the minimum threshold |pt1.x-pt2.y| < tresh && |pt1.y-pt2.y| < tresh
+  * @tparam Point1 generic point type that should provide x and y member fiels.
+  * @tparam Point2 generic point type that should provide x and y member fiels.
+  * @return \c true, if similar, \c false otherwise
+  */
+template <typename Point1, typename Point2>  
+inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
+{
+  return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );
+}
+
+
+  
+} //end namespace teb_local_planner
+
+#endif /* MISC_H_ */

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>costmap_converter</name>
+  <version>0.1.2</version>
+  <description>
+    A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types.
+  </description>
+
+  <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
+
+  <author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
+  <author email="franz.albers@tu-dortmund.de">Franz Albers</author>
+  <author email="otniel.rinaldo@tu-dortmund.de">Otniel Rinaldo</author> 
+
+  <license>BSD</license>
+
+  <url type="website">http://wiki.ros.org/costmap_converter</url>
+  
+  <buildtool_depend>ament_cmake</buildtool_depend>
+    
+  <depend>class_loader</depend>
+  <depend>costmap_converter_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>tf2</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <!-- <depend>dynamic_reconfigure</depend> -->
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_converter_node.cpp

@@ -0,0 +1,264 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/polygon_stamped.hpp>
+#include <nav2_costmap_2d/costmap_2d.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+
+#include <costmap_converter/costmap_converter_interface.h>
+#include <pluginlib/class_loader.hpp>
+
+class CostmapStandaloneConversion : public rclcpp::Node {
+ public:
+  CostmapStandaloneConversion(const std::string node_name)
+      : rclcpp::Node(node_name),
+        converter_loader_("costmap_converter",
+                          "costmap_converter::BaseCostmapToPolygons") {
+    costmap_ros_ =
+        std::make_shared<nav2_costmap_2d::Costmap2DROS>("converter_costmap");
+    costmap_thread_ = std::make_unique<std::thread>(
+        [](rclcpp_lifecycle::LifecycleNode::SharedPtr node) {
+          rclcpp::spin(node->get_node_base_interface());
+        },
+        costmap_ros_);
+    rclcpp_lifecycle::State state;
+    costmap_ros_->on_configure(state);
+    costmap_ros_->on_activate(state);
+
+    n_ = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
+    // load converter plugin from parameter server, otherwise set default
+
+    std::string converter_plugin =
+        "costmap_converter::CostmapToPolygonsDBSMCCH";
+
+    declare_parameter("converter_plugin",
+                      rclcpp::ParameterValue(converter_plugin));
+
+    get_parameter_or<std::string>("converter_plugin", converter_plugin,
+                                  converter_plugin);
+
+    try {
+      converter_ = converter_loader_.createSharedInstance(converter_plugin);
+    } catch (const pluginlib::PluginlibException &ex) {
+      RCLCPP_ERROR(get_logger(),
+                   "The plugin failed to load for some reason. Error: %s",
+                   ex.what());
+      rclcpp::shutdown();
+      return;
+    }
+
+    RCLCPP_INFO(get_logger(), "Standalone costmap converter: %s loaded.",
+                converter_plugin.c_str());
+
+    std::string obstacles_topic = "costmap_obstacles";
+    declare_parameter("obstacles_topic",
+                      rclcpp::ParameterValue(obstacles_topic));
+    get_parameter_or<std::string>("obstacles_topic", obstacles_topic,
+                                  obstacles_topic);
+
+    std::string polygon_marker_topic = "costmap_polygon_markers";
+    declare_parameter("polygon_marker_topic",
+                      rclcpp::ParameterValue(polygon_marker_topic));
+    get_parameter_or<std::string>("polygon_marker_topic", polygon_marker_topic,
+                                  polygon_marker_topic);
+
+    obstacle_pub_ =
+        create_publisher<costmap_converter_msgs::msg::ObstacleArrayMsg>(
+            obstacles_topic, 1000);
+    marker_pub_ = create_publisher<visualization_msgs::msg::Marker>(
+        polygon_marker_topic, 10);
+
+    occupied_min_value_ = 100;
+    declare_parameter("occupied_min_value",
+                      rclcpp::ParameterValue(occupied_min_value_));
+    get_parameter_or<int>("occupied_min_value", occupied_min_value_,
+                          occupied_min_value_);
+
+    std::string odom_topic = "/odom";
+    declare_parameter("odom_topic", rclcpp::ParameterValue(odom_topic));
+    get_parameter_or<std::string>("odom_topic", odom_topic, odom_topic);
+
+    if (converter_) {
+      converter_->setOdomTopic(odom_topic);
+      converter_->initialize(shared_from_this());
+      converter_->startWorker(std::make_shared<rclcpp::Rate>(5),
+                              costmap_ros_->getCostmap(), false);
+    }
+
+    pub_timer_ = n_->create_wall_timer(
+        std::chrono::milliseconds(200),
+        std::bind(&CostmapStandaloneConversion::publishCallback, this));
+  }
+
+  void publishCallback() {
+    costmap_converter::ObstacleArrayConstPtr obstacles =
+        converter_->getObstacles();
+
+    if (!obstacles) return;
+
+    obstacle_pub_->publish(*obstacles);
+
+    frame_id_ = costmap_ros_->getGlobalFrameID();
+
+    publishAsMarker(frame_id_, *obstacles);
+  }
+
+  void publishAsMarker(
+      const std::string &frame_id,
+      const std::vector<geometry_msgs::msg::PolygonStamped> &polygonStamped) {
+    visualization_msgs::msg::Marker line_list;
+    line_list.header.frame_id = frame_id;
+    line_list.header.stamp = now();
+    line_list.ns = "Polygons";
+    line_list.action = visualization_msgs::msg::Marker::ADD;
+    line_list.pose.orientation.w = 1.0;
+
+    line_list.id = 0;
+    line_list.type = visualization_msgs::msg::Marker::LINE_LIST;
+
+    line_list.scale.x = 0.1;
+    line_list.color.g = 1.0;
+    line_list.color.a = 1.0;
+
+    for (std::size_t i = 0; i < polygonStamped.size(); ++i) {
+      for (int j = 0; j < (int)polygonStamped[i].polygon.points.size() - 1;
+           ++j) {
+        geometry_msgs::msg::Point line_start;
+        line_start.x = polygonStamped[i].polygon.points[j].x;
+        line_start.y = polygonStamped[i].polygon.points[j].y;
+        line_list.points.push_back(line_start);
+        geometry_msgs::msg::Point line_end;
+        line_end.x = polygonStamped[i].polygon.points[j + 1].x;
+        line_end.y = polygonStamped[i].polygon.points[j + 1].y;
+        line_list.points.push_back(line_end);
+      }
+      // close loop for current polygon
+      if (!polygonStamped[i].polygon.points.empty() &&
+          polygonStamped[i].polygon.points.size() != 2) {
+        geometry_msgs::msg::Point line_start;
+        line_start.x = polygonStamped[i].polygon.points.back().x;
+        line_start.y = polygonStamped[i].polygon.points.back().y;
+        line_list.points.push_back(line_start);
+        if (line_list.points.size() % 2 != 0) {
+          geometry_msgs::msg::Point line_end;
+          line_end.x = polygonStamped[i].polygon.points.front().x;
+          line_end.y = polygonStamped[i].polygon.points.front().y;
+          line_list.points.push_back(line_end);
+        }
+      }
+    }
+    marker_pub_->publish(line_list);
+  }
+
+  void publishAsMarker(
+      const std::string &frame_id,
+      const costmap_converter_msgs::msg::ObstacleArrayMsg &obstacles) {
+    visualization_msgs::msg::Marker line_list;
+    line_list.header.frame_id = frame_id;
+    line_list.header.stamp = now();
+    line_list.ns = "Polygons";
+    line_list.action = visualization_msgs::msg::Marker::ADD;
+    line_list.pose.orientation.w = 1.0;
+
+    line_list.id = 0;
+    line_list.type = visualization_msgs::msg::Marker::LINE_LIST;
+
+    line_list.scale.x = 0.1;
+    line_list.color.g = 1.0;
+    line_list.color.a = 1.0;
+
+    for (const auto &obstacle : obstacles.obstacles) {
+      for (int j = 0; j < (int)obstacle.polygon.points.size() - 1; ++j) {
+        geometry_msgs::msg::Point line_start;
+        line_start.x = obstacle.polygon.points[j].x;
+        line_start.y = obstacle.polygon.points[j].y;
+        line_list.points.push_back(line_start);
+        geometry_msgs::msg::Point line_end;
+        line_end.x = obstacle.polygon.points[j + 1].x;
+        line_end.y = obstacle.polygon.points[j + 1].y;
+        line_list.points.push_back(line_end);
+      }
+      // close loop for current polygon
+      if (!obstacle.polygon.points.empty() &&
+          obstacle.polygon.points.size() != 2) {
+        geometry_msgs::msg::Point line_start;
+        line_start.x = obstacle.polygon.points.back().x;
+        line_start.y = obstacle.polygon.points.back().y;
+        line_list.points.push_back(line_start);
+        if (line_list.points.size() % 2 != 0) {
+          geometry_msgs::msg::Point line_end;
+          line_end.x = obstacle.polygon.points.front().x;
+          line_end.y = obstacle.polygon.points.front().y;
+          line_list.points.push_back(line_end);
+        }
+      }
+    }
+    marker_pub_->publish(line_list);
+  }
+
+ private:
+  pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons>
+      converter_loader_;
+  std::shared_ptr<costmap_converter::BaseCostmapToPolygons> converter_;
+
+  rclcpp::Node::SharedPtr n_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  std::unique_ptr<std::thread> costmap_thread_;
+  rclcpp::Publisher<costmap_converter_msgs::msg::ObstacleArrayMsg>::SharedPtr
+      obstacle_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
+  rclcpp::TimerBase::SharedPtr pub_timer_;
+
+  std::string frame_id_;
+  int occupied_min_value_;
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+
+  auto convert_process =
+      std::make_shared<CostmapStandaloneConversion>("costmap_converter");
+
+  rclcpp::spin(convert_process);
+
+  return 0;
+}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp

@@ -0,0 +1,115 @@
+#include <opencv2/highgui/highgui.hpp>
+//#include <opencv2/cvv/cvv.hpp>
+#include <costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h>
+
+BackgroundSubtractor::BackgroundSubtractor(const Params &parameters): params_(parameters)
+{
+}
+
+void BackgroundSubtractor::apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x, int shift_y)
+{
+  current_frame_ = image;
+
+  // occupancy grids are empty only once in the beginning -> initialize variables
+  if (occupancy_grid_fast_.empty() && occupancy_grid_slow_.empty())
+  {
+    occupancy_grid_fast_ = current_frame_;
+    occupancy_grid_slow_ = current_frame_;
+    previous_shift_x_ = shift_x;
+    previous_shift_y_ = shift_y;
+    return;
+  }
+
+  // Shift previous occupancy grid to new location (match current_frame_)
+  int shift_relative_to_previous_pos_x_ = shift_x - previous_shift_x_;
+  int shift_relative_to_previous_pos_y_ = shift_y - previous_shift_y_;
+  previous_shift_x_ = shift_x;
+  previous_shift_y_ = shift_y;
+
+  // if(shift_relative_to_previous_pos_x_ != 0 || shift_relative_to_previous_pos_y_ != 0)
+  transformToCurrentFrame(shift_relative_to_previous_pos_x_, shift_relative_to_previous_pos_y_);
+
+  // cvv::debugFilter(occupancy_grid_fast_, currentFrame_, CVVISUAL_LOCATION);
+
+  // Calculate normalized sum (mean) of nearest neighbors
+  int size = 3; // 3, 5, 7, ....
+  cv::Mat nearest_neighbor_mean_fast(occupancy_grid_fast_.size(), CV_8UC1);
+  cv::Mat nearest_neighbor_mean_slow(occupancy_grid_slow_.size(), CV_8UC1);
+  cv::boxFilter(occupancy_grid_fast_, nearest_neighbor_mean_fast, -1, cv::Size(size, size), cv::Point(-1, -1), true,
+                cv::BORDER_REPLICATE);
+  cv::boxFilter(occupancy_grid_slow_, nearest_neighbor_mean_slow, -1, cv::Size(size, size), cv::Point(-1, -1), true,
+                cv::BORDER_REPLICATE);
+  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
+  //  cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE);
+
+  // compute time mean value for each pixel according to learningrate alpha
+  // occupancy_grid_fast_ = beta*(alpha_fast*current_frame_ + (1.0-alpha_fast)*occupancy_grid_fast_) + (1-beta)*nearest_neighbor_mean_fast;
+  cv::addWeighted(current_frame_, params_.alpha_fast, occupancy_grid_fast_, (1 - params_.alpha_fast), 0, occupancy_grid_fast_);
+  cv::addWeighted(occupancy_grid_fast_, params_.beta, nearest_neighbor_mean_fast, (1 - params_.beta), 0, occupancy_grid_fast_);
+  // occupancy_grid_slow_ = beta*(alpha_slow*current_frame_ + (1.0-alpha_slow)*occupancy_grid_slow) + (1-beta)*nearest_neighbor_mean_slow;
+  cv::addWeighted(current_frame_, params_.alpha_slow, occupancy_grid_slow_, (1 - params_.alpha_slow), 0, occupancy_grid_slow_);
+  cv::addWeighted(occupancy_grid_slow_, params_.beta, nearest_neighbor_mean_slow, (1 - params_.beta), 0, occupancy_grid_slow_);
+
+  // 1) Obstacles should be detected after a minimum response of the fast filter
+  //    occupancy_grid_fast_ > min_occupancy_probability
+  cv::threshold(occupancy_grid_fast_, occupancy_grid_fast_, params_.min_occupancy_probability, 0 /*unused*/, cv::THRESH_TOZERO);
+  // 2) Moving obstacles have a minimum difference between the responses of the slow and fast filter
+  //    occupancy_grid_fast_-occupancy_grid_slow_ > min_sep_between_fast_and_slow_filter
+  cv::threshold(occupancy_grid_fast_ - occupancy_grid_slow_, fg_mask, params_.min_sep_between_fast_and_slow_filter, 255,
+                cv::THRESH_BINARY);
+  // 3) Dismiss static obstacles
+  //    nearest_neighbor_mean_slow < max_occupancy_neighbors
+  cv::threshold(nearest_neighbor_mean_slow, nearest_neighbor_mean_slow, params_.max_occupancy_neighbors, 255, cv::THRESH_BINARY_INV);
+  cv::bitwise_and(nearest_neighbor_mean_slow, fg_mask, fg_mask);
+
+  //visualize("Current frame", currentFrame_);
+  cv::Mat setBorderToZero = cv::Mat(current_frame_.size(), CV_8UC1, 0.0);
+  int border = 5;
+  setBorderToZero(cv::Rect(border, border, current_frame_.cols-2*border, current_frame_.rows-2*border)) = 255;
+
+  cv::bitwise_and(setBorderToZero, fg_mask, fg_mask);
+
+  // cv::imwrite("/home/albers/Desktop/currentFrame.png", currentFrame_);
+  // visualize("Foreground mask", fgMask);
+
+  // Closing Operation
+  cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE,
+                                              cv::Size(2*params_.morph_size + 1, 2*params_.morph_size + 1),
+                                              cv::Point(params_.morph_size, params_.morph_size));
+  cv::dilate(fg_mask, fg_mask, element);
+  cv::dilate(fg_mask, fg_mask, element);
+  cv::erode(fg_mask, fg_mask, element);
+}
+
+void BackgroundSubtractor::transformToCurrentFrame(int shift_x, int shift_y)
+{
+  // TODO: initialize with current_frame (first observed image) rather than zeros
+
+  // Translate (shift) image in costmap coordinates
+  // in cv::Mat: shift X -> to the left; shift y -> to the top
+  cv::Mat temp_fast, temp_slow;
+  cv::Mat translation_mat = (cv::Mat_<double>(2, 3, CV_64F) << 1, 0, -shift_x, 0, 1, -shift_y);
+  cv::warpAffine(occupancy_grid_fast_, temp_fast, translation_mat, occupancy_grid_fast_.size()); // can't operate in-place
+  cv::warpAffine(occupancy_grid_slow_, temp_slow, translation_mat, occupancy_grid_slow_.size()); // can't operate in-place
+
+  // cvv::debugFilter(occupancy_grid_fast_, temp_fast);
+
+  occupancy_grid_fast_ = temp_fast;
+  occupancy_grid_slow_ = temp_slow;
+}
+
+void BackgroundSubtractor::visualize(const std::string& name, const cv::Mat& image)
+{
+  if (!image.empty())
+  {
+    cv::Mat im = image.clone();
+    cv::flip(im, im, 0);
+    cv::imshow(name, im);
+    cv::waitKey(1);
+  }
+}
+
+void BackgroundSubtractor::updateParameters(const Params &parameters)
+{
+  params_ = parameters;
+}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp

@@ -0,0 +1,193 @@
+#include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
+#include <opencv2/opencv.hpp>
+#include <iostream>
+
+BlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {}
+
+cv::Ptr<BlobDetector> BlobDetector::create(const cv::SimpleBlobDetector::Params& params)
+{
+  return cv::Ptr<BlobDetector> (new BlobDetector(params)); // compatibility with older versions
+  //return cv::makePtr<BlobDetector>(params);
+}
+
+void BlobDetector::detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat&)
+{
+  // TODO: support mask
+  contours_.clear();
+
+  keypoints.clear();
+  cv::Mat grayscale_image;
+  if (image.channels() == 3)
+    cv::cvtColor(image, grayscale_image, cv::COLOR_BGR2GRAY);
+  else
+    grayscale_image = image;
+
+  if (grayscale_image.type() != CV_8UC1)
+  {
+    //CV_Error(cv::Error::StsUnsupportedFormat, "Blob detector only supports 8-bit images!");
+    std::cerr << "Blob detector only supports 8-bit images!\n";
+  }
+
+  std::vector<std::vector<Center>> centers;
+  std::vector<std::vector<cv::Point>> contours;
+  for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep)
+  {
+    cv::Mat binarized_image;
+    cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY);
+
+    std::vector<Center> cur_centers;
+    std::vector<std::vector<cv::Point>> cur_contours, new_contours;
+    findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours);
+    std::vector<std::vector<Center>> new_centers;
+    for (std::size_t i = 0; i < cur_centers.size(); ++i)
+    {
+      bool isNew = true;
+      for (std::size_t j = 0; j < centers.size(); ++j)
+      {
+        double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location);
+        isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius &&
+                dist >= cur_centers[i].radius;
+        if (!isNew)
+        {
+          centers[j].push_back(cur_centers[i]);
+
+          size_t k = centers[j].size() - 1;
+          while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius)
+          {
+            centers[j][k] = centers[j][k - 1];
+            k--;
+          }
+          centers[j][k] = cur_centers[i];
+
+          break;
+        }
+      }
+      if (isNew)
+      {
+        new_centers.push_back(std::vector<Center>(1, cur_centers[i]));
+        new_contours.push_back(cur_contours[i]);
+      }
+    }
+    std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers));
+    std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours));
+  }
+
+  for (size_t i = 0; i < centers.size(); ++i)
+  {
+    if (centers[i].size() < params_.minRepeatability)
+      continue;
+    cv::Point2d sum_point(0, 0);
+    double normalizer = 0;
+    for (std::size_t j = 0; j < centers[i].size(); ++j)
+    {
+      sum_point += centers[i][j].confidence * centers[i][j].location;
+      normalizer += centers[i][j].confidence;
+    }
+    sum_point *= (1. / normalizer);
+    cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius));
+    keypoints.push_back(kpt);
+    contours_.push_back(contours[i]);
+  }
+}
+
+void BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
+                             std::vector<std::vector<cv::Point>>& cur_contours) const
+{
+  (void)image;
+  centers.clear();
+  cur_contours.clear();
+
+  std::vector<std::vector<cv::Point>> contours;
+  cv::Mat tmp_binary_image = binary_image.clone();
+  cv::findContours(tmp_binary_image, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE);
+
+  for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx)
+  {
+    Center center;
+    center.confidence = 1;
+    cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx]));
+    if (params_.filterByArea)
+    {
+      double area = moms.m00;
+      if (area < params_.minArea || area >= params_.maxArea)
+        continue;
+    }
+
+    if (params_.filterByCircularity)
+    {
+      double area = moms.m00;
+      double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true);
+      double ratio = 4 * CV_PI * area / (perimeter * perimeter);
+      if (ratio < params_.minCircularity || ratio >= params_.maxCircularity)
+        continue;
+    }
+
+    if (params_.filterByInertia)
+    {
+      double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2));
+      const double eps = 1e-2;
+      double ratio;
+      if (denominator > eps)
+      {
+        double cosmin = (moms.mu20 - moms.mu02) / denominator;
+        double sinmin = 2 * moms.mu11 / denominator;
+        double cosmax = -cosmin;
+        double sinmax = -sinmin;
+
+        double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;
+        double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;
+        ratio = imin / imax;
+      }
+      else
+      {
+        ratio = 1;
+      }
+
+      if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio)
+        continue;
+
+      center.confidence = ratio * ratio;
+    }
+
+    if (params_.filterByConvexity)
+    {
+      std::vector<cv::Point> hull;
+      cv::convexHull(cv::Mat(contours[contour_idx]), hull);
+      double area = cv::contourArea(cv::Mat(contours[contour_idx]));
+      double hullArea = cv::contourArea(cv::Mat(hull));
+      double ratio = area / hullArea;
+      if (ratio < params_.minConvexity || ratio >= params_.maxConvexity)
+        continue;
+    }
+
+    if (moms.m00 == 0.0)
+      continue;
+    center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);
+
+    if (params_.filterByColor)
+    {
+      if (binary_image.at<uchar>(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor)
+        continue;
+    }
+
+    // compute blob radius
+    {
+      std::vector<double> dists;
+      for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx)
+      {
+        cv::Point2d pt = contours[contour_idx][point_idx];
+        dists.push_back(cv::norm(center.location - pt));
+      }
+      std::sort(dists.begin(), dists.end());
+      center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;
+    }
+
+    centers.push_back(center);
+    cur_contours.push_back(contours[contour_idx]);
+  }
+}
+
+void BlobDetector::updateParameters(const Params& parameters)
+{
+  params_ = parameters;
+}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp

@@ -0,0 +1,489 @@
+#include <costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h>
+
+#include <pluginlib/class_list_macros.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+
+CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles()
+{
+  ego_vel_.x = ego_vel_.y = ego_vel_.z = 0;
+  costmap_ = nullptr;
+//  dynamic_recfg_ = nullptr;
+}
+
+CostmapToDynamicObstacles::~CostmapToDynamicObstacles()
+{
+//  if(dynamic_recfg_ != nullptr)
+//    delete dynamic_recfg_;
+}
+
+void CostmapToDynamicObstacles::initialize(rclcpp::Node::SharedPtr nh)
+{
+  BaseCostmapToPolygons::initialize(nh);
+  
+  costmap_ = nullptr;
+
+  // We need the odometry from the robot to compensate the ego motion
+//  rclcpp::Node::SharedPtr gn; // create odom topic w.r.t. global node handle
+  odom_sub_ = nh->create_subscription<nav_msgs::msg::Odometry>(
+              odom_topic_,
+              rclcpp::SystemDefaultsQoS(),
+              std::bind(&CostmapToDynamicObstacles::odomCallback, this, std::placeholders::_1));
+
+  nh->get_parameter_or<bool>("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_);
+
+  //////////////////////////////////
+  // Foreground detection parameters
+  BackgroundSubtractor::Params bg_sub_params;
+
+  bg_sub_params.alpha_slow = 0.3;
+  nh->get_parameter_or<double>("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow);
+
+  bg_sub_params.alpha_fast = 0.85;
+  nh->get_parameter_or<double>("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast);
+
+  bg_sub_params.beta = 0.85;
+  nh->get_parameter_or<double>("beta", bg_sub_params.beta, bg_sub_params.beta);
+
+  bg_sub_params.min_occupancy_probability = 180;
+  nh->get_parameter_or<double>("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability);
+
+  bg_sub_params.min_sep_between_fast_and_slow_filter = 80;
+  nh->get_parameter_or<double>("min_sep_between_slow_and_fast_filter", bg_sub_params.min_sep_between_fast_and_slow_filter, bg_sub_params.min_sep_between_fast_and_slow_filter);
+
+  bg_sub_params.max_occupancy_neighbors = 100;
+  nh->get_parameter_or<double>("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors);
+
+  bg_sub_params.morph_size = 1;
+  nh->get_parameter_or<int>("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size);
+
+  bg_sub_ = std::unique_ptr<BackgroundSubtractor>(new BackgroundSubtractor(bg_sub_params));
+
+  ////////////////////////////
+  // Blob detection parameters
+  BlobDetector::Params blob_det_params;
+
+  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
+  blob_det_params.blobColor = 255;      // Extract light blobs
+  blob_det_params.thresholdStep = 256;  // Input for blob detection is already a binary image
+  blob_det_params.minThreshold = 127;
+  blob_det_params.maxThreshold = 255;
+  blob_det_params.minRepeatability = 1;
+
+  blob_det_params.minDistBetweenBlobs = 10;
+  nh->get_parameter_or<float>("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs);
+
+  blob_det_params.filterByArea = true;
+  nh->get_parameter_or<bool>("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea);
+
+  blob_det_params.minArea = 3; // Filter out blobs with less pixels
+  nh->get_parameter_or<float>("min_area", blob_det_params.minArea, blob_det_params.minArea);
+
+  blob_det_params.maxArea = 300;
+  nh->get_parameter_or<float>("max_area", blob_det_params.maxArea, blob_det_params.maxArea);
+
+  blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2
+  nh->get_parameter_or<bool>("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity);
+
+  blob_det_params.minCircularity = 0.2;
+  nh->get_parameter_or<float>("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity);
+
+  blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle)
+  nh->get_parameter_or<float>("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity);
+
+  blob_det_params.filterByInertia = true; // Filter blobs based on their elongation
+  nh->get_parameter_or<bool>("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia);
+
+  blob_det_params.minInertiaRatio = 0.2;  // minimal 0 (in case of a line)
+  nh->get_parameter_or<float>("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio);
+
+  blob_det_params.maxInertiaRatio = 1;    // maximal 1 (in case of a circle)
+  nh->get_parameter_or<float>("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio);
+
+  blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull
+  nh->get_parameter_or<bool>("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity);
+
+  blob_det_params.minConvexity = 0;          // minimal 0
+  nh->get_parameter_or<float>("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity);
+
+  blob_det_params.maxConvexity = 1;          // maximal 1
+  nh->get_parameter_or<float>("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity);
+
+  blob_det_ = BlobDetector::create(blob_det_params);
+
+  ////////////////////////////////////
+  // Tracking parameters
+  CTracker::Params tracker_params;
+  tracker_params.dt = 0.2;
+  nh->get_parameter_or<float>("dt", tracker_params.dt, tracker_params.dt);
+
+  tracker_params.dist_thresh = 60.0;
+  nh->get_parameter_or<float>("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh);
+
+  tracker_params.max_allowed_skipped_frames = 3;
+  nh->get_parameter_or<int>("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames);
+
+  tracker_params.max_trace_length = 10;
+  nh->get_parameter_or<int>("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length);
+
+  tracker_ = std::unique_ptr<CTracker>(new CTracker(tracker_params));
+
+
+  ////////////////////////////////////
+  // Static costmap conversion parameters
+  std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH";
+  nh->get_parameter_or<std::string>("static_converter_plugin", static_converter_plugin, static_converter_plugin);
+  loadStaticCostmapConverterPlugin(static_converter_plugin, nh);
+
+
+// setup dynamic reconfigure
+//  dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>(nh);
+//  dynamic_reconfigure::Server<CostmapToDynamicObstaclesConfig>::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2);
+//  dynamic_recfg_->setCallback(cb);
+}
+
+void CostmapToDynamicObstacles::compute()
+{
+  if (costmap_mat_.empty())
+    return;
+
+  /////////////////////////// Foreground detection ////////////////////////////////////
+  // Dynamic obstacles are separated from static obstacles
+  int origin_x = round(costmap_->getOriginX() / costmap_->getResolution());
+  int origin_y = round(costmap_->getOriginY() / costmap_->getResolution());
+  // ROS_INFO("Origin x  [m]: %f    Origin_y  [m]: %f", costmap_->getOriginX(), costmap_->getOriginY());
+  // ROS_INFO("Origin x [px]: %d \t Origin_y [px]: %d", originX, originY);
+
+  bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y);
+
+  // if no foreground object is detected, no ObstacleMsgs need to be published
+  if (fg_mask_.empty())
+    return;
+
+  cv::Mat bg_mat;
+  if (publish_static_obstacles_)
+  {
+    // Get static obstacles
+    bg_mat = costmap_mat_ - fg_mask_;
+    // visualize("bg_mat", bg_mat);
+  }
+
+
+  /////////////////////////////// Blob detection /////////////////////////////////////
+  // Centers and contours of Blobs are detected
+  blob_det_->detect(fg_mask_, keypoints_);
+  std::vector<std::vector<cv::Point>> contours = blob_det_->getContours();
+
+
+  ////////////////////////////// Tracking ////////////////////////////////////////////
+  // Objects are assigned to objects from previous frame based on Hungarian Algorithm
+  // Object velocities are estimated using a Kalman Filter
+  std::vector<Point_t> detected_centers(keypoints_.size());
+  for (size_t i = 0; i < keypoints_.size(); i++)
+  {
+    detected_centers.at(i).x = keypoints_.at(i).pt.x;
+    detected_centers.at(i).y = keypoints_.at(i).pt.y;
+    detected_centers.at(i).z = 0; // Currently unused!
+  }
+
+  tracker_->Update(detected_centers, contours);
+
+
+  ///////////////////////////////////// Output ///////////////////////////////////////
+  /*
+  cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3);
+  cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR);
+
+  for (int i = 0; i < (int)tracker_->tracks.size(); ++i)
+    cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()),
+                  cv::Scalar(0, 0, 255), 1);
+
+  //visualize("fgMaskWithKeyPoints", fgMaskWithKeypoints);
+  //cv::imwrite("/home/albers/Desktop/fgMask.png", fgMask);
+  //cv::imwrite("/home/albers/Desktop/fgMaskWithKeyPoints.png", fgMaskWithKeypoints);
+  */
+
+  //////////////////////////// Fill ObstacleContainerPtr /////////////////////////////
+  ObstacleArrayPtr obstacles(new costmap_converter_msgs::msg::ObstacleArrayMsg);
+  // header.seq is automatically filled
+  obstacles->header.stamp = now();
+  obstacles->header.frame_id = "/map"; //Global frame /map
+
+  // For all tracked objects
+  for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i)
+  {
+    geometry_msgs::msg::Polygon polygon;
+
+    // TODO directly create polygon inside getContour and avoid copy
+    std::vector<Point_t> contour;
+    getContour(i, contour); // this method also transforms map to world coordinates
+
+    // convert contour to polygon
+    for (const Point_t& pt : contour)
+    {
+      polygon.points.emplace_back();
+      polygon.points.back().x = pt.x;
+      polygon.points.back().y = pt.y;
+      polygon.points.back().z = 0;
+    }
+
+    obstacles->obstacles.emplace_back();
+    obstacles->obstacles.back().polygon = polygon;
+
+    // Set obstacle ID
+    obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id;
+
+    // Set orientation
+    geometry_msgs::msg::QuaternionStamped orientation;
+
+    Point_t vel = getEstimatedVelocityOfObject(i);
+    double yaw = std::atan2(vel.y, vel.x);
+    //ROS_INFO("yaw: %f", yaw);
+    tf2::Quaternion q;
+    q.setRPY(0, 0, yaw);
+    obstacles->obstacles.back().orientation = tf2::toMsg(q);
+
+    // Set velocity
+    geometry_msgs::msg::TwistWithCovariance velocities;
+    //velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y);
+    //velocities.twist.linear.y = 0;
+    velocities.twist.linear.x = vel.x;
+    velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames?
+    velocities.twist.linear.z = 0;
+    velocities.twist.angular.x = 0;
+    velocities.twist.angular.y = 0;
+    velocities.twist.angular.z = 0;
+
+    // TODO: use correct covariance matrix
+    velocities.covariance = {1, 0, 0, 0, 0, 0,
+                             0, 1, 0, 0, 0, 0,
+                             0, 0, 1, 0, 0, 0,
+                             0, 0, 0, 1, 0, 0,
+                             0, 0, 0, 0, 1, 0,
+                             0, 0, 0, 0, 0, 1};
+
+    obstacles->obstacles.back().velocities = velocities;
+  }
+
+  ////////////////////////// Static obstacles ////////////////////////////
+  if (publish_static_obstacles_)
+  {
+    uchar* img_data = bg_mat.data;
+    int width = bg_mat.cols;
+    int height = bg_mat.rows;
+    int stride = bg_mat.step;
+
+    if (stackedCostmapConversion())
+    {
+      // Create new costmap with static obstacles (background)
+      std::shared_ptr<nav2_costmap_2d::Costmap2D> static_costmap(new nav2_costmap_2d::Costmap2D(costmap_->getSizeInCellsX(),
+                                                                                        costmap_->getSizeInCellsY(),
+                                                                                        costmap_->getResolution(),
+                                                                                        costmap_->getOriginX(),
+                                                                                        costmap_->getOriginY()));
+      for(int i = 0; i < height; i++)
+      {
+        for(int j = 0; j < width; j++)
+        {
+          static_costmap->setCost(j, i, img_data[i * stride + j]);
+        }
+      }
+
+      // Apply static obstacle conversion plugin
+      setStaticCostmap(static_costmap);
+      convertStaticObstacles();
+
+      // Store converted static obstacles for publishing
+      auto static_polygons = getStaticPolygons();
+      for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it)
+      {
+        obstacles->obstacles.emplace_back();
+        obstacles->obstacles.back().polygon = *it;
+        obstacles->obstacles.back().velocities.twist.linear.x = 0;
+        obstacles->obstacles.back().velocities.twist.linear.y = 0;
+        obstacles->obstacles.back().id = -1;
+      }
+    }
+    // Otherwise leave static obstacles point-shaped
+    else
+    {
+      for(int i = 0; i < height; i++)
+      {
+        for(int j = 0; j < width; j++)
+        {
+            uchar value = img_data[i * stride + j];
+            if (value > 0)
+            {
+              // obstacle found
+              obstacles->obstacles.emplace_back();
+              geometry_msgs::msg::Point32 pt;
+              pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX();
+              pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY();
+              obstacles->obstacles.back().polygon.points.push_back(pt);
+              obstacles->obstacles.back().velocities.twist.linear.x = 0;
+              obstacles->obstacles.back().velocities.twist.linear.y = 0;
+              obstacles->obstacles.back().id = -1;
+            }
+        }
+      }
+    }
+  }
+
+  updateObstacleContainer(obstacles);
+}
+
+void CostmapToDynamicObstacles::setCostmap2D(nav2_costmap_2d::Costmap2D* costmap)
+{
+  if (!costmap)
+    return;
+
+  costmap_ = costmap;
+
+  updateCostmap2D();
+}
+
+void CostmapToDynamicObstacles::updateCostmap2D()
+{
+  if (!costmap_->getMutex())
+  {
+    RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null");
+    return;
+  }
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap_->getMutex());
+
+  // Initialize costmapMat_ directly with the unsigned char array of costmap_
+  //costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
+  //                      costmap_->getCharMap()).clone(); // Deep copy: TODO
+  costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1,
+                        costmap_->getCharMap());
+}
+
+ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  return obstacles_;
+}
+
+void CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  obstacles_ = obstacles;
+}
+
+Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx)
+{
+  // vel [px/s] * costmapResolution [m/px] = vel [m/s]
+  Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_;
+
+  //ROS_INFO("vel x: %f, vel y: %f, vel z: %f", vel.x, vel.y, vel.z);
+  // velocity in /map frame
+  return vel;
+}
+
+void CostmapToDynamicObstacles::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
+{
+  RCLCPP_INFO_ONCE(getLogger(), "CostmapToDynamicObstacles: odom received.");
+
+  tf2::Quaternion pose;
+  tf2::fromMsg(msg->pose.pose.orientation, pose);
+
+  tf2::Vector3 twistLinear;
+  // tf2::fromMsg(msg->twist.twist.linear, twistLinear); // not available in tf2
+  twistLinear.setX(msg->twist.twist.linear.x);
+  twistLinear.setY(msg->twist.twist.linear.y);
+  twistLinear.setZ(msg->twist.twist.linear.z);
+
+
+  // velocity of the robot in x, y and z coordinates
+  tf2::Vector3 vel = tf2::quatRotate(pose, twistLinear);
+  ego_vel_.x = vel.x();
+  ego_vel_.y = vel.y();
+  ego_vel_.z = vel.z();
+}
+
+//void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level)
+//{
+//  publish_static_obstacles_ = config.publish_static_obstacles;
+
+//  // BackgroundSubtraction Parameters
+//  BackgroundSubtractor::Params bg_sub_params;
+//  bg_sub_params.alpha_slow = config.alpha_slow;
+//  bg_sub_params.alpha_fast = config.alpha_fast;
+//  bg_sub_params.beta = config.beta;
+//  bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter;
+//  bg_sub_params.min_occupancy_probability = config.min_occupancy_probability;
+//  bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors;
+//  bg_sub_params.morph_size = config.morph_size;
+//  bg_sub_->updateParameters(bg_sub_params);
+
+//  // BlobDetector Parameters
+//  BlobDetector::Params blob_det_params;
+//  // necessary, because blobDetParams are otherwise initialized with default values for dark blobs
+//  blob_det_params.filterByColor = true; // actually filterByIntensity, always true
+//  blob_det_params.blobColor = 255;      // Extract light blobs
+//  blob_det_params.thresholdStep = 256;  // Input for blobDetection is already a binary image
+//  blob_det_params.minThreshold = 127;
+//  blob_det_params.maxThreshold = 255;
+//  blob_det_params.minRepeatability = 1;
+//  blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected
+//  blob_det_params.filterByArea = config.filter_by_area;
+//  blob_det_params.minArea = config.min_area;
+//  blob_det_params.maxArea = config.max_area;
+//  blob_det_params.filterByCircularity = config.filter_by_circularity;
+//  blob_det_params.minCircularity = config.min_circularity;
+//  blob_det_params.maxCircularity = config.max_circularity;
+//  blob_det_params.filterByInertia = config.filter_by_inertia;
+//  blob_det_params.minInertiaRatio = config.min_inertia_ratio;
+//  blob_det_params.maxInertiaRatio = config.max_inertia_ratio;
+//  blob_det_params.filterByConvexity = config.filter_by_convexity;
+//  blob_det_params.minConvexity = config.min_convexity;
+//  blob_det_params.maxConvexity = config.max_convexity;
+//  blob_det_->updateParameters(blob_det_params);
+
+//  // Tracking Parameters
+//  CTracker::Params tracking_params;
+//  tracking_params.dt = config.dt;
+//  tracking_params.dist_thresh = config.dist_thresh;
+//  tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames;
+//  tracking_params.max_trace_length = config.max_trace_length;
+//  tracker_->updateParameters(tracking_params);
+//}
+
+void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector<Point_t>& contour)
+{
+  assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size());
+
+  contour.clear();
+
+  // contour [px] * costmapResolution [m/px] = contour [m]
+  std::vector<cv::Point> contour2i = tracker_->tracks.at(idx)->getLastContour();
+
+  contour.reserve(contour2i.size());
+
+  Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0);
+
+  for (std::size_t i = 0; i < contour2i.size(); ++i)
+  {
+    contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution())
+                        + costmap_origin); // Shift to /map
+  }
+
+}
+
+void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image)
+{
+  if (!image.empty())
+  {
+    cv::Mat im = image.clone();
+    cv::flip(im, im, 0);
+    cv::imshow(name, im);
+    cv::waitKey(1);
+  }
+}
+}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp

@@ -0,0 +1,130 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
+
+// ---------------------------------------------------------------------------
+// Tracker. Manage tracks. Create, remove, update.
+// ---------------------------------------------------------------------------
+CTracker::CTracker(const Params &parameters)
+    : params(parameters),
+      NextTrackID(0)
+{
+}
+// ---------------------------------------------------------------------------
+//
+// ---------------------------------------------------------------------------
+void CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)
+{
+  // Each contour has a centroid
+  assert(detectedCentroid.size() == contours.size());
+
+  // -----------------------------------
+  // If there is no tracks yet, then every cv::Point begins its own track.
+  // -----------------------------------
+  if (tracks.size() == 0)
+  {
+    // If no tracks yet
+    for (size_t i = 0; i < detectedCentroid.size(); ++i)
+    {
+      tracks.push_back(
+          std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
+    }
+  }
+
+  size_t N = tracks.size();
+  size_t M = detectedCentroid.size();
+
+  assignments_t assignment;
+
+  if (!tracks.empty())
+  {
+    // Distance matrix of N-th Track to the M-th detectedCentroid
+    distMatrix_t Cost(N * M);
+
+    // calculate distance between the blobs centroids
+    for (size_t i = 0; i < tracks.size(); i++)
+    {
+      for (size_t j = 0; j < detectedCentroid.size(); j++)
+      {
+        Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
+      }
+    }
+
+    // -----------------------------------
+    // Solving assignment problem (tracks and predictions of Kalman filter)
+    // -----------------------------------
+    AssignmentProblemSolver APS;
+    APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
+
+    // -----------------------------------
+    // clean assignment from pairs with large distance
+    // -----------------------------------
+    for (size_t i = 0; i < assignment.size(); i++)
+    {
+      if (assignment[i] != -1)
+      {
+        if (Cost[i + assignment[i] * N] > params.dist_thresh)
+        {
+          assignment[i] = -1;
+          tracks[i]->skipped_frames = 1;
+        }
+      }
+      else
+      {
+        // If track have no assigned detect, then increment skipped frames counter.
+        tracks[i]->skipped_frames++;
+      }
+    }
+
+    // -----------------------------------
+    // If track didn't get detects long time, remove it.
+    // -----------------------------------
+    for (int i = 0; i < static_cast<int>(tracks.size()); i++)
+    {
+      if ((int)tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
+      {
+        tracks.erase(tracks.begin() + i);
+        assignment.erase(assignment.begin() + i);
+        i--;
+      }
+    }
+  }
+
+  // -----------------------------------
+  // Search for unassigned detects and start new tracks for them.
+  // -----------------------------------
+  for (size_t i = 0; i < detectedCentroid.size(); ++i)
+  {
+    if (find(assignment.begin(), assignment.end(), i) == assignment.end())
+    {
+      tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
+    }
+  }
+
+  // Update Kalman Filters state
+
+  for (size_t i = 0; i < assignment.size(); i++)
+  {
+    // If track updated less than one time, than filter state is not correct.
+
+    if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
+    {
+      tracks[i]->skipped_frames = 0;
+      tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
+    }
+    else // if not continue using predictions
+    {
+      tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
+    }
+  }
+}
+
+void CTracker::updateParameters(const Params &parameters)
+{
+  params = parameters;
+}
+// ---------------------------------------------------------------------------
+//
+// ---------------------------------------------------------------------------
+CTracker::~CTracker(void) {}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp

@@ -0,0 +1,732 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h>
+#include <limits>
+
+AssignmentProblemSolver::AssignmentProblemSolver() {}
+
+AssignmentProblemSolver::~AssignmentProblemSolver() {}
+
+track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns,
+                                       std::vector<int>& assignment, TMethod Method)
+{
+  assignment.resize(nOfRows, -1);
+
+  track_t cost = 0;
+
+  switch (Method)
+  {
+  case optimal:
+    assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+    break;
+
+  case many_forbidden_assignments:
+    assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+    break;
+
+  case without_forbidden_assignments:
+    assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns);
+    break;
+  }
+
+  return cost;
+}
+// --------------------------------------------------------------------------
+// Computes the optimal assignment (minimum overall costs) using Munkres algorithm.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost,
+                                                const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+  // Generate distance cv::Matrix
+  // and check cv::Matrix elements positiveness :)
+
+  // Total elements number
+  size_t nOfElements = nOfRows * nOfColumns;
+  // Memory allocation
+  track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t));
+  // Pointer to last element
+  track_t* distMatrixEnd = distMatrix + nOfElements;
+
+  for (size_t row = 0; row < nOfElements; row++)
+  {
+    track_t value = distMatrixIn[row];
+    assert(value >= 0);
+    distMatrix[row] = value;
+  }
+
+  // Memory allocation
+  bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool));
+  bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool));
+  bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool));
+  bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool));
+  bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */
+
+  /* preliminary steps */
+  if (nOfRows <= nOfColumns)
+  {
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      /* find the smallest element in the row */
+      track_t* distMatrixTemp = distMatrix + row;
+      track_t minValue = *distMatrixTemp;
+      distMatrixTemp += nOfRows;
+      while (distMatrixTemp < distMatrixEnd)
+      {
+        track_t value = *distMatrixTemp;
+        if (value < minValue)
+        {
+          minValue = value;
+        }
+        distMatrixTemp += nOfRows;
+      }
+      /* subtract the smallest element from each element of the row */
+      distMatrixTemp = distMatrix + row;
+      while (distMatrixTemp < distMatrixEnd)
+      {
+        *distMatrixTemp -= minValue;
+        distMatrixTemp += nOfRows;
+      }
+    }
+    /* Steps 1 and 2a */
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        if (distMatrix[row + nOfRows * col] == 0)
+        {
+          if (!coveredColumns[col])
+          {
+            starMatrix[row + nOfRows * col] = true;
+            coveredColumns[col] = true;
+            break;
+          }
+        }
+      }
+    }
+  }
+  else /* if(nOfRows > nOfColumns) */
+  {
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      /* find the smallest element in the column */
+      track_t* distMatrixTemp = distMatrix + nOfRows * col;
+      track_t* columnEnd = distMatrixTemp + nOfRows;
+      track_t minValue = *distMatrixTemp++;
+      while (distMatrixTemp < columnEnd)
+      {
+        track_t value = *distMatrixTemp++;
+        if (value < minValue)
+        {
+          minValue = value;
+        }
+      }
+      /* subtract the smallest element from each element of the column */
+      distMatrixTemp = distMatrix + nOfRows * col;
+      while (distMatrixTemp < columnEnd)
+      {
+        *distMatrixTemp++ -= minValue;
+      }
+    }
+    /* Steps 1 and 2a */
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      for (size_t row = 0; row < nOfRows; row++)
+      {
+        if (distMatrix[row + nOfRows * col] == 0)
+        {
+          if (!coveredRows[row])
+          {
+            starMatrix[row + nOfRows * col] = true;
+            coveredColumns[col] = true;
+            coveredRows[row] = true;
+            break;
+          }
+        }
+      }
+    }
+
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      coveredRows[row] = false;
+    }
+  }
+  /* move to step 2b */
+  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+         nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns);
+  /* compute cost and remove invalid assignments */
+  computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
+  /* free allocated memory */
+  free(distMatrix);
+  free(coveredColumns);
+  free(coveredRows);
+  free(starMatrix);
+  free(primeMatrix);
+  free(newStarMatrix);
+  return;
+}
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows,
+                                                    size_t nOfColumns)
+{
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (starMatrix[row + nOfRows * col])
+      {
+        assignment[row] = static_cast<int>(col);
+        break;
+      }
+    }
+  }
+}
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost,
+                                                    const distMatrix_t& distMatrixIn, size_t nOfRows)
+{
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    const int col = assignment[row];
+    if (col >= 0)
+    {
+      cost += distMatrixIn[row + nOfRows * col];
+    }
+  }
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  bool* starMatrixTemp, *columnEnd;
+  /* cover every column containing a starred zero */
+  for (size_t col = 0; col < nOfColumns; col++)
+  {
+    starMatrixTemp = starMatrix + nOfRows * col;
+    columnEnd = starMatrixTemp + nOfRows;
+    while (starMatrixTemp < columnEnd)
+    {
+      if (*starMatrixTemp++)
+      {
+        coveredColumns[col] = true;
+        break;
+      }
+    }
+  }
+  /* move to step 3 */
+  step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+         nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                     bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                     size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  /* count covered columns */
+  size_t nOfCoveredColumns = 0;
+  for (size_t col = 0; col < nOfColumns; col++)
+  {
+    if (coveredColumns[col])
+    {
+      nOfCoveredColumns++;
+    }
+  }
+  if (nOfCoveredColumns == minDim)
+  {
+    /* algorithm finished */
+    buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
+  }
+  else
+  {
+    /* move to step 3 */
+    step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+          nOfColumns, minDim);
+  }
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                    size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  bool zerosFound = true;
+  while (zerosFound)
+  {
+    zerosFound = false;
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (!coveredColumns[col])
+      {
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0))
+          {
+            /* prime zero */
+            primeMatrix[row + nOfRows * col] = true;
+            /* find starred zero in current row */
+            size_t starCol = 0;
+            for (; starCol < nOfColumns; starCol++)
+            {
+              if (starMatrix[row + nOfRows * starCol])
+              {
+                break;
+              }
+            }
+            if (starCol == nOfColumns) /* no starred zero found */
+            {
+              /* move to step 4 */
+              step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows,
+                    nOfRows, nOfColumns, minDim, row, col);
+              return;
+            }
+            else
+            {
+              coveredRows[row] = true;
+              coveredColumns[starCol] = false;
+              zerosFound = true;
+              break;
+            }
+          }
+        }
+      }
+    }
+  }
+  /* move to step 5 */
+  step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+        nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                    size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col)
+{
+  const size_t nOfElements = nOfRows * nOfColumns;
+  /* generate temporary copy of starMatrix */
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    newStarMatrix[n] = starMatrix[n];
+  }
+  /* star current zero */
+  newStarMatrix[row + nOfRows * col] = true;
+  /* find starred zero in current column */
+  size_t starCol = col;
+  size_t starRow = 0;
+  for (; starRow < nOfRows; starRow++)
+  {
+    if (starMatrix[starRow + nOfRows * starCol])
+    {
+      break;
+    }
+  }
+  while (starRow < nOfRows)
+  {
+    /* unstar the starred zero */
+    newStarMatrix[starRow + nOfRows * starCol] = false;
+    /* find primed zero in current row */
+    size_t primeRow = starRow;
+    size_t primeCol = 0;
+    for (; primeCol < nOfColumns; primeCol++)
+    {
+      if (primeMatrix[primeRow + nOfRows * primeCol])
+      {
+        break;
+      }
+    }
+    /* star the primed zero */
+    newStarMatrix[primeRow + nOfRows * primeCol] = true;
+    /* find starred zero in current column */
+    starCol = primeCol;
+    for (starRow = 0; starRow < nOfRows; starRow++)
+    {
+      if (starMatrix[starRow + nOfRows * starCol])
+      {
+        break;
+      }
+    }
+  }
+  /* use temporary copy as new starMatrix */
+  /* delete all primes, uncover all rows */
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    primeMatrix[n] = false;
+    starMatrix[n] = newStarMatrix[n];
+  }
+  for (size_t n = 0; n < nOfRows; n++)
+  {
+    coveredRows[n] = false;
+  }
+  /* move to step 2a */
+  step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+         nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+//
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix,
+                                    bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows,
+                                    size_t nOfRows, size_t nOfColumns, size_t minDim)
+{
+  /* find smallest uncovered element h */
+  float h = std::numeric_limits<track_t>::max();
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    if (!coveredRows[row])
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        if (!coveredColumns[col])
+        {
+          const float value = distMatrix[row + nOfRows * col];
+          if (value < h)
+          {
+            h = value;
+          }
+        }
+      }
+    }
+  }
+  /* add h to each covered row */
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    if (coveredRows[row])
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        distMatrix[row + nOfRows * col] += h;
+      }
+    }
+  }
+  /* subtract h from each uncovered column */
+  for (size_t col = 0; col < nOfColumns; col++)
+  {
+    if (!coveredColumns[col])
+    {
+      for (size_t row = 0; row < nOfRows; row++)
+      {
+        distMatrix[row + nOfRows * col] -= h;
+      }
+    }
+  }
+  /* move to step 3 */
+  step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows,
+        nOfColumns, minDim);
+}
+
+// --------------------------------------------------------------------------
+// Computes a suboptimal solution. Good for cases without forbidden assignments.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost,
+                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+  /* make working copy of distance Matrix */
+  const size_t nOfElements = nOfRows * nOfColumns;
+  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    distMatrix[n] = distMatrixIn[n];
+  }
+
+  /* recursively search for the minimum element and do the assignment */
+  for (;;)
+  {
+    /* find minimum distance observation-to-track pair */
+    float minValue = std::numeric_limits<track_t>::max();
+    size_t tmpRow = 0;
+    size_t tmpCol = 0;
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        const float value = distMatrix[row + nOfRows * col];
+        if (value != std::numeric_limits<track_t>::max() && (value < minValue))
+        {
+          minValue = value;
+          tmpRow = row;
+          tmpCol = col;
+        }
+      }
+    }
+
+    if (minValue != std::numeric_limits<track_t>::max())
+    {
+      assignment[tmpRow] = static_cast<int>(tmpCol);
+      cost += minValue;
+      for (size_t n = 0; n < nOfRows; n++)
+      {
+        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
+      }
+      for (size_t n = 0; n < nOfColumns; n++)
+      {
+        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
+      }
+    }
+    else
+    {
+      break;
+    }
+  }
+
+  free(distMatrix);
+}
+// --------------------------------------------------------------------------
+// Computes a suboptimal solution. Good for cases with many forbidden assignments.
+// --------------------------------------------------------------------------
+void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost,
+                                                    const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns)
+{
+  /* make working copy of distance Matrix */
+  const size_t nOfElements = nOfRows * nOfColumns;
+  float* distMatrix = (float*)malloc(nOfElements * sizeof(float));
+  for (size_t n = 0; n < nOfElements; n++)
+  {
+    distMatrix[n] = distMatrixIn[n];
+  }
+
+  /* allocate memory */
+  int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int));
+  int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int));
+
+  /* compute number of validations */
+  bool infiniteValueFound = false;
+  bool finiteValueFound = false;
+  for (size_t row = 0; row < nOfRows; row++)
+  {
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
+      {
+        nOfValidTracks[col] += 1;
+        nOfValidObservations[row] += 1;
+        finiteValueFound = true;
+      }
+      else
+      {
+        infiniteValueFound = true;
+      }
+    }
+  }
+
+  if (infiniteValueFound)
+  {
+    if (!finiteValueFound)
+    {
+      free(nOfValidTracks);
+      free(nOfValidObservations);
+      free(distMatrix);
+      return;
+    }
+    bool repeatSteps = true;
+
+    while (repeatSteps)
+    {
+      repeatSteps = false;
+
+      /* step 1: reject assignments of multiply validated tracks to singly validated observations		 */
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        bool singleValidationFound = false;
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() &&
+              (nOfValidObservations[row] == 1))
+          {
+            singleValidationFound = true;
+            break;
+          }
+
+          if (singleValidationFound)
+          {
+            for (size_t row = 0; row < nOfRows; row++)
+              if ((nOfValidObservations[row] > 1) &&
+                  distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
+              {
+                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
+                nOfValidObservations[row] -= 1;
+                nOfValidTracks[col] -= 1;
+                repeatSteps = true;
+              }
+          }
+        }
+      }
+
+      /* step 2: reject assignments of multiply validated observations to singly validated tracks */
+      if (nOfColumns > 1)
+      {
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          bool singleValidationFound = false;
+          for (size_t col = 0; col < nOfColumns; col++)
+          {
+            if (distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max() && (nOfValidTracks[col] == 1))
+            {
+              singleValidationFound = true;
+              break;
+            }
+          }
+
+          if (singleValidationFound)
+          {
+            for (size_t col = 0; col < nOfColumns; col++)
+            {
+              if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits<track_t>::max())
+              {
+                distMatrix[row + nOfRows * col] = std::numeric_limits<track_t>::max();
+                nOfValidObservations[row] -= 1;
+                nOfValidTracks[col] -= 1;
+                repeatSteps = true;
+              }
+            }
+          }
+        }
+      }
+    } /* while(repeatSteps) */
+
+    /* for each multiply validated track that validates only with singly validated  */
+    /* observations, choose the observation with minimum distance */
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      if (nOfValidObservations[row] > 1)
+      {
+        bool allSinglyValidated = true;
+        float minValue = std::numeric_limits<track_t>::max();
+        size_t tmpCol = 0;
+        for (size_t col = 0; col < nOfColumns; col++)
+        {
+          const float value = distMatrix[row + nOfRows * col];
+          if (value != std::numeric_limits<track_t>::max())
+          {
+            if (nOfValidTracks[col] > 1)
+            {
+              allSinglyValidated = false;
+              break;
+            }
+            else if ((nOfValidTracks[col] == 1) && (value < minValue))
+            {
+              tmpCol = col;
+              minValue = value;
+            }
+          }
+        }
+
+        if (allSinglyValidated)
+        {
+          assignment[row] = static_cast<int>(tmpCol);
+          cost += minValue;
+          for (size_t n = 0; n < nOfRows; n++)
+          {
+            distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
+          }
+          for (size_t n = 0; n < nOfColumns; n++)
+          {
+            distMatrix[row + nOfRows * n] = std::numeric_limits<track_t>::max();
+          }
+        }
+      }
+    }
+
+    // for each multiply validated observation that validates only with singly validated  track, choose the track with
+    // minimum distance
+    for (size_t col = 0; col < nOfColumns; col++)
+    {
+      if (nOfValidTracks[col] > 1)
+      {
+        bool allSinglyValidated = true;
+        float minValue = std::numeric_limits<track_t>::max();
+        size_t tmpRow = 0;
+        for (size_t row = 0; row < nOfRows; row++)
+        {
+          const float value = distMatrix[row + nOfRows * col];
+          if (value != std::numeric_limits<track_t>::max())
+          {
+            if (nOfValidObservations[row] > 1)
+            {
+              allSinglyValidated = false;
+              break;
+            }
+            else if ((nOfValidObservations[row] == 1) && (value < minValue))
+            {
+              tmpRow = row;
+              minValue = value;
+            }
+          }
+        }
+
+        if (allSinglyValidated)
+        {
+          assignment[tmpRow] = static_cast<int>(col);
+          cost += minValue;
+          for (size_t n = 0; n < nOfRows; n++)
+          {
+            distMatrix[n + nOfRows * col] = std::numeric_limits<track_t>::max();
+          }
+          for (size_t n = 0; n < nOfColumns; n++)
+          {
+            distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
+          }
+        }
+      }
+    }
+  } /* if(infiniteValueFound) */
+
+  /* now, recursively search for the minimum element and do the assignment */
+  for (;;)
+  {
+    /* find minimum distance observation-to-track pair */
+    float minValue = std::numeric_limits<track_t>::max();
+    size_t tmpRow = 0;
+    size_t tmpCol = 0;
+    for (size_t row = 0; row < nOfRows; row++)
+    {
+      for (size_t col = 0; col < nOfColumns; col++)
+      {
+        const float value = distMatrix[row + nOfRows * col];
+        if (value != std::numeric_limits<track_t>::max() && (value < minValue))
+        {
+          minValue = value;
+          tmpRow = row;
+          tmpCol = col;
+        }
+      }
+    }
+
+    if (minValue != std::numeric_limits<track_t>::max())
+    {
+      assignment[tmpRow] = static_cast<int>(tmpCol);
+      cost += minValue;
+      for (size_t n = 0; n < nOfRows; n++)
+      {
+        distMatrix[n + nOfRows * tmpCol] = std::numeric_limits<track_t>::max();
+      }
+      for (size_t n = 0; n < nOfColumns; n++)
+      {
+        distMatrix[tmpRow + nOfRows * n] = std::numeric_limits<track_t>::max();
+      }
+    }
+    else
+    {
+      break;
+    }
+  }
+
+  /* free allocated memory */
+  free(nOfValidObservations);
+  free(nOfValidTracks);
+  free(distMatrix);
+}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp

@@ -0,0 +1,99 @@
+// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
+// Refer to README.md in this directory.
+
+#include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h>
+#include "opencv2/opencv.hpp"
+#include <iostream>
+#include <vector>
+
+//---------------------------------------------------------------------------
+//---------------------------------------------------------------------------
+TKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime)
+{
+  // time increment (lower values makes target more "massive")
+  dt = deltatime;
+
+  // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z]
+  kalman = new cv::KalmanFilter(6, 3, 0);
+  // Transition cv::Matrix
+  kalman->transitionMatrix = (cv::Mat_<track_t>(6, 6) <<
+                              1, 0, 0, dt,  0,  0,
+                              0, 1, 0,  0, dt,  0,
+                              0, 0, 1,  0,  0, dt,
+                              0, 0, 0,  1,  0,  0,
+                              0, 0, 0,  0,  1,  0,
+                              0, 0, 0,  0,  0,  1);
+  // init...
+  LastPosition = pt;
+  kalman->statePre.at<track_t>(0) = pt.x;
+  kalman->statePre.at<track_t>(1) = pt.y;
+  kalman->statePre.at<track_t>(2) = pt.z;
+
+  kalman->statePre.at<track_t>(3) = 0;
+  kalman->statePre.at<track_t>(4) = 0;
+  kalman->statePre.at<track_t>(5) = 0;
+
+  kalman->statePost.at<track_t>(0) = pt.x;
+  kalman->statePost.at<track_t>(1) = pt.y;
+  kalman->statePost.at<track_t>(2) = pt.z;
+
+  // Nur x, y und z Koordinaten messbar!
+  kalman->measurementMatrix = (cv::Mat_<track_t>(3, 6) <<
+                               1, 0, 0, 0, 0, 0,
+                               0, 1, 0, 0, 0, 0,
+                               0, 0, 1, 0, 0, 0);
+
+  float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure
+  float c1 = pow(dt,4.0)/4.0;
+  float c2 = pow(dt,2.0);
+  float c3 = pow(dt,3.0)/2.0;
+  kalman->processNoiseCov = sigma*sigma*(cv::Mat_<float>(6, 6) <<
+                             c1,  0,  0, c3,  0,  0,
+                              0, c1,  0,  0, c3,  0,
+                              0,  0, c1,  0,  0, c3,
+                             c3,  0,  0, c2,  0,  0,
+                              0, c3,  0,  0, c2,  0,
+                              0,  0, c3,  0,  0, c2);
+
+  cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2));
+
+  cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000));
+}
+//---------------------------------------------------------------------------
+TKalmanFilter::~TKalmanFilter() { delete kalman; }
+
+//---------------------------------------------------------------------------
+void TKalmanFilter::Prediction()
+{
+  cv::Mat prediction = kalman->predict();
+  LastPosition = Point_t(prediction.at<track_t>(0), prediction.at<track_t>(1), prediction.at<track_t>(2));
+  LastVelocity = Point_t(prediction.at<track_t>(3), prediction.at<track_t>(4), prediction.at<track_t>(5));
+}
+
+//---------------------------------------------------------------------------
+Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect)
+{
+  cv::Mat measurement(3, 1, Mat_t(1));
+  if (!DataCorrect)
+  {
+    measurement.at<track_t>(0) = LastPosition.x; // update using prediction
+    measurement.at<track_t>(1) = LastPosition.y;
+    measurement.at<track_t>(2) = LastPosition.z;
+  }
+  else
+  {
+    measurement.at<track_t>(0) = p.x; // update using measurements
+    measurement.at<track_t>(1) = p.y;
+    measurement.at<track_t>(2) = p.z;
+  }
+  // Correction
+  cv::Mat estimated = kalman->correct(measurement);
+  LastPosition.x = estimated.at<track_t>(0); // update using measurements
+  LastPosition.y = estimated.at<track_t>(1);
+  LastPosition.z = estimated.at<track_t>(2);
+  LastVelocity.x = estimated.at<track_t>(3);
+  LastVelocity.y = estimated.at<track_t>(4);
+  LastVelocity.z = estimated.at<track_t>(5);
+  return LastPosition;
+}
+//---------------------------------------------------------------------------

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md

@@ -0,0 +1,13 @@
+Multitarget Tracker
+===================
+
+This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker].
+It is utilized for the *CostmapToDynamicObstacles* plugin.
+
+The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt].
+The package itself depends on other third party packages with different licenses (refer to the package repository).
+
+TODO: Include the whole package as external CMake project.
+
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_convex_hull.cpp

@@ -0,0 +1,303 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_lines_convex_hull.h>
+#include <costmap_converter/misc.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+
+CostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH() 
+{
+//    dynamic_recfg_ = NULL;
+}
+  
+CostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH() 
+{
+//  if (dynamic_recfg_ != NULL)
+//    delete dynamic_recfg_;
+}
+  
+void CostmapToLinesDBSMCCH::initialize(rclcpp::Node::SharedPtr nh)
+{ 
+    BaseCostmapToPolygons::initialize(nh);
+    
+    // DB SCAN
+    parameter_.max_distance_ = 0.4;
+    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
+    
+    parameter_.min_pts_ = 2;
+    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
+    
+    parameter_.max_pts_ = 30;
+    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
+    
+    // convex hull
+    parameter_.min_keypoint_separation_ = 0.1;
+    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
+    
+    parameter_buffered_ = parameter_;
+
+    // Line extraction
+    support_pts_max_dist_ = 0.3;
+    nh->get_parameter_or<double>("support_pts_max_dist", support_pts_max_dist_, support_pts_max_dist_);
+    
+    support_pts_max_dist_inbetween_ = 1.0;
+    nh->get_parameter_or<double>("support_pts_max_dist_inbetween", support_pts_max_dist_inbetween_, support_pts_max_dist_inbetween_);
+    
+    min_support_pts_ = 2;
+    nh->get_parameter_or<int>("min_support_pts", min_support_pts_, min_support_pts_);
+    
+    // setup dynamic reconfigure
+//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>(nh);
+//    dynamic_reconfigure::Server<CostmapToLinesDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2);
+//    dynamic_recfg_->setCallback(cb);
+    
+    // deprecated
+    rclcpp::Parameter dummy;
+    if (nh->get_parameter("support_pts_min_dist_", dummy) || nh->get_parameter("support_pts_min_dist", dummy))
+      RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore.");
+    if (nh->get_parameter("min_support_pts_", dummy))
+      RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore.");
+}  
+  
+void CostmapToLinesDBSMCCH::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());  
+    
+    
+    // add convex hulls to polygon container
+    for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    {
+      geometry_msgs::msg::Polygon polygon;
+      convexHull2(clusters[i], polygon );
+      
+      // now extract lines of the polygon (by searching for support points in the cluster)  
+      // and add them to the polygon container
+      extractPointsAndLines(clusters[i], polygon, std::back_inserter(*polygons));
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (size_t i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::msg::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+        
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+typedef CostmapToLinesDBSMCCH CL;
+bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
+{ return (cluster[i].x<cluster[j].x) || (cluster[i].x == cluster[j].x && cluster[i].y < cluster[j].y); }
+bool sort_keypoint_y(const std::size_t& i, const std::size_t& j, const std::vector<CL::KeyPoint>& cluster) 
+{ return (cluster[i].y<cluster[j].y) || (cluster[i].y == cluster[j].y && cluster[i].x < cluster[j].x); }
+
+void CostmapToLinesDBSMCCH::extractPointsAndLines(std::vector<KeyPoint>& cluster, const geometry_msgs::msg::Polygon& polygon,
+                                                  std::back_insert_iterator< std::vector<geometry_msgs::msg::Polygon> > lines)
+{
+   if (polygon.points.empty())
+     return;
+  
+   if (polygon.points.size() < 2)
+   {
+     lines = polygon; // our polygon is just a point, push back onto the output sequence
+     return;
+   }
+   int n = (int)polygon.points.size();
+     
+   for (int i=1; i<n; ++i) // this implemenation requires an explicitly closed polygon (start vertex = end vertex)
+   {
+        const geometry_msgs::msg::Point32* vertex1 = &polygon.points[i-1];
+        const geometry_msgs::msg::Point32* vertex2 = &polygon.points[i];
+
+        // check how many vertices belong to the line (sometimes the convex hull algorithm finds multiple vertices on a line,
+        // in case of small coordinate deviations)
+        double dx = vertex2->x - vertex1->x;
+        double dy = vertex2->y - vertex1->y;
+//         double norm = std::sqrt(dx*dx + dy*dy);
+//         dx /= norm;
+//         dy /= norm;
+//         for (int j=i; j<(int)polygon.points.size() - 2; ++j)
+//         {
+//           const geometry_msgs::msg::Point32* vertex_jp2 = &polygon.points[j+2];
+//           double dx2 = vertex_jp2->x - vertex2->x;
+//           double dy2 = vertex_jp2->y - vertex2->y;
+//           double norm2 = std::sqrt(dx2*dx2 + dy2*dy2);
+//           dx2 /= norm2;
+//           dy2 /= norm2;
+//           if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree
+//           {
+//             vertex2 = &polygon.points[j+2];
+//             i = j; // DO NOT USE "i" afterwards
+//           }
+//           else break;
+//         }
+       
+        //Search support points
+        std::vector<std::size_t> support_points; // store indices of cluster
+        
+        for (std::size_t k = 0; k < cluster.size(); ++k)
+        {
+            bool is_inbetween = false;
+            double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween );
+
+            if(is_inbetween && dist_line_to_point <= support_pts_max_dist_)
+            {
+              support_points.push_back(k);
+              continue;
+            }
+        }
+
+        // now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps)
+        // and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0
+        bool is_line=true;
+        if (support_pts_max_dist_inbetween_!=0)
+        {
+          if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included
+          {          
+            // sort points
+            if (std::abs(dx) >= std::abs(dy))
+              std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_x, std::placeholders::_1, std::placeholders::_2, std::cref(cluster)));
+            else 
+              std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_y, std::placeholders::_1, std::placeholders::_2, std::cref(cluster)));
+            
+            // now calculate distances
+            for (int k = 1; k < int(support_points.size()); ++k)
+            {
+              double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x;
+              double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y;
+              double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y);
+              if (dist > support_pts_max_dist_inbetween_)
+              {
+                is_line = false;
+                break;
+              }
+            }
+            
+          }
+          else
+            is_line = false;
+        }
+        
+        if (is_line)
+        {
+          // line found:
+          geometry_msgs::msg::Polygon line;
+          line.points.push_back(*vertex1);
+          line.points.push_back(*vertex2);
+          lines = line; // back insertion
+          
+          // remove inlier from list
+          // iterate in reverse order, otherwise indices are not valid after erasing elements
+          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
+          for (; support_it != support_points.rend(); ++support_it)
+          {
+            cluster.erase(cluster.begin() + *support_it);
+          }
+        }
+        else
+        {
+            // remove goal, since it will be added in the subsequent iteration
+            //support_points.pop_back();
+          // old:
+//             // add vertex 1 as single point
+//             geometry_msgs::msg::Polygon point;
+//             point.points.push_back(*vertex1);
+//             lines = point; // back insertion
+
+           // add complete inlier set as points 
+//            for (int k = 0; k < int(support_points.size()); ++k)
+//            {
+//               geometry_msgs::msg::Polygon polygon;
+//               convertPointToPolygon(cluster[support_points[k]], polygon);
+//               lines = polygon; // back insertion
+//            }
+           
+          // remove inlier from list and add them as point obstacles
+          // iterate in reverse order, otherwise indices are not valid after erasing elements
+          std::vector<std::size_t>::reverse_iterator support_it = support_points.rbegin();
+          for (; support_it != support_points.rend(); ++support_it)
+          {
+            geometry_msgs::msg::Polygon polygon;
+            convertPointToPolygon(cluster[*support_it], polygon);
+            lines = polygon; // back insertion
+            
+            cluster.erase(cluster.begin() + *support_it);
+          }
+        }
+    }
+ 
+    // add all remaining cluster points, that do not belong to a line
+    for (int i=0; i<(int)cluster.size();++i)
+    {
+        geometry_msgs::msg::Polygon polygon;
+        convertPointToPolygon(cluster[i], polygon);
+        lines = polygon; // back insertion
+    }
+
+}
+
+//void CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level)
+//{
+//    boost::mutex::scoped_lock lock(parameter_mutex_);
+//    parameter_buffered_.max_distance_ = config.cluster_max_distance;
+//    parameter_buffered_.min_pts_ = config.cluster_min_pts;
+//    parameter_buffered_.max_pts_ = config.cluster_max_pts;
+//    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
+//    support_pts_max_dist_ = config.support_pts_max_dist;
+//    support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween;
+//    min_support_pts_ = config.min_support_pts;
+//}
+
+
+
+}//end namespace costmap_converter
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_ransac.cpp

@@ -0,0 +1,361 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_lines_ransac.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+
+CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH() 
+{
+//  dynamic_recfg_ = NULL;
+}
+
+CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC() 
+{
+//  if (dynamic_recfg_ != NULL)
+//    delete dynamic_recfg_;
+}
+  
+void CostmapToLinesDBSRANSAC::initialize(rclcpp::Node::SharedPtr nh)
+{ 
+    BaseCostmapToPolygons::initialize(nh);
+    
+    // DB SCAN
+    parameter_.max_distance_ = 0.4;
+    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
+    
+    parameter_.min_pts_ = 2;
+    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
+    
+    parameter_.max_pts_ = 30;
+    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
+    
+    // convex hull (only necessary if outlier filtering is enabled)
+    parameter_.min_keypoint_separation_ = 0.1;
+    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
+
+    parameter_buffered_ = parameter_;
+
+    // ransac
+    ransac_inlier_distance_ = 0.2;
+    nh->get_parameter_or<double>("ransac_inlier_distance", ransac_inlier_distance_, ransac_inlier_distance_);
+    
+    ransac_min_inliers_ = 10;
+    nh->get_parameter_or<int>("ransac_min_inliers", ransac_min_inliers_, ransac_min_inliers_);
+    
+    ransac_no_iterations_ = 2000;
+    nh->get_parameter_or<int>("ransac_no_iterations", ransac_no_iterations_, ransac_no_iterations_);
+   
+    ransac_remainig_outliers_ = 3;
+    nh->get_parameter_or<int>("ransac_remainig_outliers", ransac_remainig_outliers_, ransac_remainig_outliers_);
+    
+    ransac_convert_outlier_pts_ = true;
+    nh->get_parameter_or<bool>("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, ransac_convert_outlier_pts_);
+    
+    ransac_filter_remaining_outlier_pts_ = false;
+    nh->get_parameter_or<bool>("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, ransac_filter_remaining_outlier_pts_);
+    
+    // setup dynamic reconfigure
+//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>(nh);
+//    dynamic_reconfigure::Server<CostmapToLinesDBSRANSACConfig>::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2);
+//    dynamic_recfg_->setCallback(cb);
+}  
+  
+void CostmapToLinesDBSRANSAC::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());  
+    
+    
+    // fit lines using ransac for each cluster
+    for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    { 
+      
+      while ((int)clusters[i].size() > ransac_remainig_outliers_)
+      {
+//         std::vector<KeyPoint> inliers;
+        std::vector<KeyPoint> outliers;
+        std::pair<KeyPoint,KeyPoint> model;
+        if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) )
+          break;
+        
+        // add to polygon container
+        geometry_msgs::msg::Polygon line;
+        line.points.resize(2);
+        model.first.toPointMsg(line.points.front());
+        model.second.toPointMsg(line.points.back());
+        polygons->push_back(line);
+        
+        clusters[i] = outliers;
+      }
+      // create point polygons for remaining outliers
+      if (ransac_convert_outlier_pts_)
+      {
+        if (ransac_filter_remaining_outlier_pts_)
+        {
+          // take edge points of a convex polygon
+          // these points define a cluster and since all lines are extracted,
+          // we remove points from the interior...
+          geometry_msgs::msg::Polygon polygon;
+          convexHull2(clusters[i], polygon);
+          for (int j=0; j < (int)polygon.points.size(); ++j)
+          {
+            polygons->push_back(geometry_msgs::msg::Polygon());
+            convertPointToPolygon(polygon.points[j], polygons->back());
+          }
+        }
+        else
+        {
+          for (int j = 0; j < (int)clusters[i].size(); ++j)
+          {
+            polygons->push_back(geometry_msgs::msg::Polygon());
+            convertPointToPolygon(clusters[i][j], polygons->back());           
+          }
+        }
+
+      }
+      
+      
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (size_t i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::msg::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+        
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+
+bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector<KeyPoint>& data, double inlier_distance, int no_iterations,
+                                         int min_inliers, std::pair<KeyPoint, KeyPoint>& best_model, 
+                                         std::vector<KeyPoint>* inliers, std::vector<KeyPoint>* outliers)
+{
+  if ((int)data.size() < 2 ||  (int)data.size() < min_inliers)
+  {
+    return false;
+  }
+  
+  std::uniform_int_distribution<> distribution(0, data.size()-1);
+  
+  std::pair<int, int> best_model_idx;
+  int best_no_inliers = -1;
+  
+  
+  for (int i=0; i < no_iterations; ++i)
+  {
+    // choose random points to define a line candidate
+    int start_idx = distribution(rnd_generator_);
+    int end_idx = start_idx;
+    while (end_idx == start_idx)
+      end_idx = distribution(rnd_generator_);
+    
+    
+    // compute inliers
+    int no_inliers = 0;
+    for (int j=0; j<(int)data.size(); ++j)
+    {
+      if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) )
+        ++no_inliers;
+    }
+    
+    if (no_inliers > best_no_inliers)
+    {
+      best_no_inliers = no_inliers;
+      best_model_idx.first = start_idx;
+      best_model_idx.second = end_idx;
+    }
+  }
+  
+  best_model.first = data[best_model_idx.first];
+  best_model.second = data[best_model_idx.second];
+  
+  if (best_no_inliers < min_inliers)
+    return false;
+  
+  // Now repeat the calculation for the best model in order to obtain the inliers and outliers set
+  // This might be faster if no_iterations is large, but TEST
+  if (inliers || outliers)
+  {
+    if (inliers)
+      inliers->clear();
+    if (outliers)
+      outliers->clear();
+    
+//    int no_inliers = 0;
+    for (int i=0; i<(int)data.size(); ++i)
+    {
+        if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) )
+        {
+          if (inliers)
+            inliers->push_back( data[i] );
+        }
+        else
+        {
+          if (outliers)
+            outliers->push_back( data[i] );
+        }
+    }
+  }
+  
+  return true;
+}
+
+bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector<KeyPoint>& data, double& slope, double& intercept, 
+                                               double* mean_x_out, double* mean_y_out)
+{
+  if (data.size() < 2)
+  {
+    RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression");
+    return false;
+  }
+  
+  double mean_x = 0;
+  double mean_y = 0;
+  for (int i=0; i<(int)data.size(); ++i)
+  {
+    mean_x += data[i].x;
+    mean_y += data[i].y;
+  }
+  mean_x /= double(data.size());
+  mean_y /= double(data.size());
+  
+  if (mean_x_out)
+    *mean_x_out = mean_x;
+ 
+  if (mean_y_out)
+    *mean_y_out = mean_y;
+  
+  double numerator = 0.0;
+  double denominator = 0.0;
+
+  for(int i=0; i<(int)data.size(); ++i)
+  {
+      double dx = data[i].x - mean_x;
+      numerator += dx * (data[i].y - mean_y);
+      denominator += dx*dx;
+  }
+  
+  if (denominator == 0)
+  {
+    RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: linear regression failed, denominator 0");
+    return false;
+  }
+  else
+    slope = numerator / denominator;
+  
+  intercept = mean_y - slope * mean_x;
+  return true;
+}
+
+
+//void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level)
+//{
+//    boost::mutex::scoped_lock lock(parameter_mutex_);
+//    parameter_buffered_.max_distance_ = config.cluster_max_distance;
+//    parameter_buffered_.min_pts_ = config.cluster_min_pts;
+//    parameter_buffered_.max_pts_ = config.cluster_max_pts;
+//    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
+//    ransac_inlier_distance_ = config.ransac_inlier_distance;
+//    ransac_min_inliers_ = config.ransac_min_inliers;
+//    ransac_no_iterations_ = config.ransac_no_iterations;
+//    ransac_remainig_outliers_ = config.ransac_remainig_outliers;
+//    ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts;
+//    ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts;
+//}
+
+
+/*
+void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector<KeyPoint>& data, const KeyPoint& linept1, const KeyPoint& linept2, 
+                                               KeyPoint& line_start, KeyPoint& line_end)
+{
+  line_start = linept1; 
+  line_end = linept2;
+
+  // infinite line is defined by linept1 and linept2
+  double dir_x = line_end.x - line_start.x;
+  double dir_y = line_end.y - line_start.y;
+  double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y);
+  dir_x /= norm;
+  dir_y /= norm;
+  
+  // project data onto the line and check if the distance is increased in both directions
+  for (int i=0; i < (int) data.size(); ++i)
+  {
+    double dx = data[i].x - line_start.x;
+    double dy = data[i].y - line_start.y;
+    // check scalar product at start
+    double extension = dx*dir_x + dy*dir_y;
+    if (extension<0)
+    {
+      line_start.x -=  dir_x*extension;
+      line_start.y -=  dir_y*extension;
+    }
+    else
+    {
+      dx = data[i].x - line_end.x;
+      dy = data[i].y - line_end.y;
+      // check scalar product at end
+      double extension = dx*dir_x + dy*dir_y;
+      if (extension>0)
+      {
+        line_end.x +=  dir_x*extension;
+        line_end.y +=  dir_y*extension;
+      }
+    }
+  }
+  
+}*/
+
+
+}//end namespace costmap_converter
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons.cpp

@@ -0,0 +1,514 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_polygons.h>
+#include <costmap_converter/misc.h>
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)
+
+namespace
+{
+
+/**
+ * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
+ *
+ * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
+ *
+ * @param begin iterator pointing to the begin of the range of points
+ * @param end interator pointing to the end of the range of points
+ * @param epsilon distance criteria for removing points if it is closer to the line segment than this
+ * @param result the simplified polygon
+ */
+std::vector<geometry_msgs::msg::Point32> douglasPeucker(std::vector<geometry_msgs::msg::Point32>::iterator begin,
+  std::vector<geometry_msgs::msg::Point32>::iterator end, double epsilon)
+{
+  if (std::distance(begin, end) <= 2)
+  {
+    return std::vector<geometry_msgs::msg::Point32>(begin, end);
+  }
+
+  // Find the point with the maximum distance from the line [begin, end)
+  double dmax = std::numeric_limits<double>::lowest();
+  std::vector<geometry_msgs::msg::Point32>::iterator max_dist_it;
+  std::vector<geometry_msgs::msg::Point32>::iterator last = std::prev(end);
+  for (auto it = std::next(begin); it != last; ++it)
+  {
+    double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
+    if (d > dmax)
+    {
+      max_dist_it = it;
+      dmax = d;
+    }
+  }
+
+  if (dmax < epsilon * epsilon)
+  { // termination criterion reached, line is good enough
+    std::vector<geometry_msgs::msg::Point32> result;
+    result.push_back(*begin);
+    result.push_back(*last);
+    return result;
+  }
+
+  // Recursive calls for the two splitted parts
+  auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
+  auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);
+
+  // Combine the two lines into one line and return the merged line.
+  // Note that we have to skip the first point of the second line, as it is duplicated above.
+  firstLineSimplified.insert(firstLineSimplified.end(),
+    std::make_move_iterator(std::next(secondLineSimplified.begin())),
+    std::make_move_iterator(secondLineSimplified.end()));
+  return firstLineSimplified;
+}
+
+} // end namespace
+
+namespace costmap_converter
+{
+
+CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
+{
+  costmap_ = NULL;
+  // dynamic_recfg_ = NULL;
+  neighbor_size_x_ = neighbor_size_y_ = -1;
+  offset_x_ = offset_y_ = 0.;
+}
+
+CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH()
+{
+//  if (dynamic_recfg_ != NULL)
+//    delete dynamic_recfg_;
+}
+
+void CostmapToPolygonsDBSMCCH::initialize(rclcpp::Node::SharedPtr nh)
+{
+    BaseCostmapToPolygons::initialize(nh);
+
+    costmap_ = NULL;
+
+    parameter_.max_distance_ = 0.4;
+    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
+
+    parameter_.min_pts_ = 2;
+    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
+
+    parameter_.max_pts_ = 30;
+    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
+
+    parameter_.min_keypoint_separation_ = 0.1;
+    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
+
+    parameter_buffered_ = parameter_;
+
+    // setup dynamic reconfigure
+//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
+//    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
+//    dynamic_recfg_->setCallback(cb);
+}
+
+
+void CostmapToPolygonsDBSMCCH::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
+
+
+    // add convex hulls to polygon container
+    for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    {
+      polygons->push_back( geometry_msgs::msg::Polygon() );
+      convexHull2(clusters[i], polygons->back() );
+    }
+
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (std::size_t i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::msg::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+void CostmapToPolygonsDBSMCCH::setCostmap2D(nav2_costmap_2d::Costmap2D *costmap)
+{
+    if (!costmap)
+      return;
+
+    costmap_ = costmap;
+
+    updateCostmap2D();
+}
+
+void CostmapToPolygonsDBSMCCH::updateCostmap2D()
+{
+      occupied_cells_.clear();
+
+      if (!costmap_->getMutex())
+      {
+        RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null");
+        return;
+      }
+
+      // TODO: currently dynamic reconigure is not supported in ros2
+      { // get a copy of our parameters from dynamic reconfigure
+        std::lock_guard<std::mutex> lock(parameter_mutex_);
+        parameter_ = parameter_buffered_;
+      }
+
+      std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*costmap_->getMutex());
+
+      // allocate neighbor lookup
+      int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
+      int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;
+
+      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {
+        neighbor_size_x_ = cells_x;
+        neighbor_size_y_ = cells_y;
+        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
+      }
+      offset_x_ = costmap_->getOriginX();
+      offset_y_ = costmap_->getOriginY();
+      for (auto& n : neighbor_lookup_)
+        n.clear();
+      
+      auto size_x = costmap_->getSizeInCellsX();
+      auto size_y = costmap_->getSizeInCellsY();
+
+      // get indices of obstacle cells
+      for(std::size_t i = 0; i < size_x; i++)
+      {
+        for(std::size_t j = 0; j < size_y; j++)
+        {
+          int value = costmap_->getCost(i,j);
+          if(value >= nav2_costmap_2d::LETHAL_OBSTACLE)
+          {
+            double x, y;
+            costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
+            addPoint(x, y);
+          }
+        }
+      }
+}
+
+
+void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)
+{
+  std::vector<bool> visited(occupied_cells_.size(), false);
+
+  clusters.clear();
+
+  //DB Scan Algorithm
+  int cluster_id = 0; // current cluster_id
+  clusters.push_back(std::vector<KeyPoint>());
+  for(int i = 0; i< (int)occupied_cells_.size(); i++)
+  {
+    if(!visited[i]) //keypoint has not been visited before
+    {
+      visited[i] = true; // mark as visited
+      std::vector<int> neighbors;
+      regionQuery(i, neighbors); //Find neighbors around the keypoint
+      if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise
+      {
+        clusters[0].push_back(occupied_cells_[i]);
+      }
+      else
+      {
+        ++cluster_id; // increment current cluster_id
+        clusters.push_back(std::vector<KeyPoint>());
+
+        // Expand the cluster
+        clusters[cluster_id].push_back(occupied_cells_[i]);
+        for(int j = 0; j<(int)neighbors.size(); j++)
+        {
+          if ((int)clusters[cluster_id].size() == parameter_.max_pts_)
+            break;
+
+          if(!visited[neighbors[j]]) //keypoint has not been visited before
+          {
+            visited[neighbors[j]] = true;  // mark as visited
+            std::vector<int> further_neighbors;
+            regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
+//             if(further_neighbors.size() < min_pts_)
+//             {
+//               clusters[0].push_back(occupied_cells[neighbors[j]]);
+//             }
+//             else
+            if ((int)further_neighbors.size() >= parameter_.min_pts_)
+            {
+              // neighbors found
+              neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
+              clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
+void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)
+{
+    neighbors.clear();
+
+    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
+    const KeyPoint& kp = occupied_cells_[curr_index];
+    int cx, cy;
+    pointToNeighborCells(kp, cx,cy);
+
+    // loop over the neighboring cells for looking up the points
+    const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},
+                               {-1,  0}, {0,  0}, {1,  0},
+                               {-1,  1}, {0,  1}, {1,  1}};
+    for (int i = 0; i < 9; ++i)
+    {
+      int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);
+      if (idx < 0 || idx >= int(neighbor_lookup_.size()))
+        continue;
+      const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];
+      for (int point_idx : pointIndicesToCheck) {
+        if (point_idx == curr_index) // point is not a neighbor to itself
+          continue;
+        const KeyPoint& other = occupied_cells_[point_idx];
+        double dx = other.x - kp.x;
+        double dy = other.y - kp.y;
+        double dist_sqr = dx*dx + dy*dy;
+        if (dist_sqr <= dist_sqr_threshold)
+          neighbors.push_back(point_idx);
+      }
+    }
+}
+
+bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
+{
+  return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
+}
+
+void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon)
+{
+    //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull
+
+    int k = 0;
+    int n = cluster.size();
+
+    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
+    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);
+
+    polygon.points.resize(2*n);
+
+    // lower hull
+    for (int i = 0; i < n; ++i)
+    {
+      while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
+      {
+        --k;
+      }
+      cluster[i].toPointMsg(polygon.points[k]);
+      ++k;
+    }
+
+    // upper hull
+    for (int i = n-2, t = k+1; i >= 0; --i)
+    {
+      while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
+      {
+        --k;
+      }
+      cluster[i].toPointMsg(polygon.points[k]);
+      ++k;
+    }
+
+
+    polygon.points.resize(k); // original
+    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
+//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost
+
+    simplifyPolygon(polygon);
+}
+
+
+
+void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::msg::Polygon& polygon)
+{
+    std::vector<KeyPoint>& P = cluster;
+    std::vector<geometry_msgs::msg::Point32>& points = polygon.points;
+
+    // Sort P by x and y
+    std::sort(P.begin(), P.end(), isXCoordinateSmaller);
+
+    // the output array H[] will be used as the stack
+    int i;                 // array scan index
+
+    // Get the indices of points with min x-coord and min|max y-coord
+    int minmin = 0, minmax;
+    double xmin = P[0].x;
+    for (i = 1; i < (int)P.size(); i++)
+        if (P[i].x != xmin) break;
+    minmax = i - 1;
+    if (minmax == (int)P.size() - 1)
+    {   // degenerate case: all x-coords == xmin
+        points.push_back(geometry_msgs::msg::Point32());
+        P[minmin].toPointMsg(points.back());
+        if (P[minmax].y != P[minmin].y) // a  nontrivial segment
+        {
+            points.push_back(geometry_msgs::msg::Point32());
+            P[minmax].toPointMsg(points.back());
+        }
+        // add polygon endpoint
+        points.push_back(geometry_msgs::msg::Point32());
+        P[minmin].toPointMsg(points.back());
+        return;
+    }
+
+    // Get the indices of points with max x-coord and min|max y-coord
+    int maxmin, maxmax = (int)P.size() - 1;
+    double xmax = P.back().x;
+    for (i = P.size() - 2; i >= 0; i--)
+        if (P[i].x != xmax) break;
+    maxmin = i+1;
+
+    // Compute the lower hull on the stack H
+    // push  minmin point onto stack
+    points.push_back(geometry_msgs::msg::Point32());
+    P[minmin].toPointMsg(points.back());
+    i = minmax;
+    while (++i <= maxmin)
+    {
+        // the lower line joins P[minmin]  with P[maxmin]
+        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
+            continue;           // ignore P[i] above or on the lower line
+
+        while (points.size() > 1)         // there are at least 2 points on the stack
+        {
+            // test if  P[i] is left of the line at the stack top
+            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
+                break;         // P[i] is a new hull  vertex
+            points.pop_back();         // pop top point off  stack
+        }
+        // push P[i] onto stack
+        points.push_back(geometry_msgs::msg::Point32());
+        P[i].toPointMsg(points.back());
+    }
+
+    // Next, compute the upper hull on the stack H above  the bottom hull
+    if (maxmax != maxmin)      // if  distinct xmax points
+    {
+         // push maxmax point onto stack
+         points.push_back(geometry_msgs::msg::Point32());
+         P[maxmax].toPointMsg(points.back());
+    }
+    int bot = (int)points.size();                  // the bottom point of the upper hull stack
+    i = maxmin;
+    while (--i >= minmax)
+    {
+        // the upper line joins P[maxmax]  with P[minmax]
+        if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)
+            continue;           // ignore P[i] below or on the upper line
+
+        while ((int)points.size() > bot)     // at least 2 points on the upper stack
+        {
+            // test if  P[i] is left of the line at the stack top
+            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
+                break;         // P[i] is a new hull  vertex
+            points.pop_back();         // pop top point off stack
+        }
+        // push P[i] onto stack
+        points.push_back(geometry_msgs::msg::Point32());
+        P[i].toPointMsg(points.back());
+    }
+    if (minmax != minmin)
+    {
+        // push  joining endpoint onto stack
+        points.push_back(geometry_msgs::msg::Point32());
+        P[minmin].toPointMsg(points.back());
+    }
+
+    simplifyPolygon(polygon);
+}
+
+void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::msg::Polygon& polygon)
+{
+  size_t triangleThreshold = 3;
+  // check if first and last point are the same. If yes, a triangle has 4 points
+  if (polygon.points.size() > 1
+      && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5
+      && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
+  {
+    triangleThreshold = 4;
+  }
+  if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines
+    return;
+  // TODO Reason about better start conditions for splitting lines, e.g., by
+  // https://en.wikipedia.org/wiki/Rotating_calipers
+  polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;
+}
+
+void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  polygons_ = polygons;
+}
+
+
+PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  PolygonContainerConstPtr polygons = polygons_;
+  return polygons;
+}
+
+//void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
+//{
+  //boost::mutex::scoped_lock lock(parameter_mutex_);
+  //parameter_buffered_.max_distance_ = config.cluster_max_distance;
+  //parameter_buffered_.min_pts_ = config.cluster_min_pts;
+  //parameter_buffered_.max_pts_ = config.cluster_max_pts;
+  //parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
+//}
+
+}//end namespace costmap_converter
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons_concave.cpp

@@ -0,0 +1,231 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann, Otniel Rinaldo
+ *********************************************************************/
+
+#include <costmap_converter/costmap_to_polygons_concave.h>
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons)
+
+namespace costmap_converter
+{
+    
+CostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH()
+{
+//  dynamic_recfg_ = NULL;
+}
+
+CostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull() 
+{
+//  if (dynamic_recfg_ != NULL)
+//    delete dynamic_recfg_;
+}
+
+void CostmapToPolygonsDBSConcaveHull::initialize(rclcpp::Node::SharedPtr nh)
+{ 
+    BaseCostmapToPolygons::initialize(nh);
+    
+    parameter_.max_distance_ = 0.4; 
+    nh->get_parameter_or<double>("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_);
+    
+    parameter_.min_pts_ = 2;
+    nh->get_parameter_or<int>("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_);
+    
+    parameter_.max_pts_ = 30;
+    nh->get_parameter_or<int>("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_);
+    
+    parameter_.min_keypoint_separation_ = 0.1;
+    nh->get_parameter_or<double>("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_);
+    
+    parameter_buffered_ = parameter_;
+
+    concave_hull_depth_ = 2.0;
+    nh->get_parameter_or<double>("concave_hull_depth", concave_hull_depth_, concave_hull_depth_);
+    
+    // setup dynamic reconfigure
+//    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>(nh);
+//    dynamic_reconfigure::Server<CostmapToPolygonsDBSConcaveHullConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2);
+//    dynamic_recfg_->setCallback(cb);
+}
+
+
+void CostmapToPolygonsDBSConcaveHull::compute()
+{
+    std::vector< std::vector<KeyPoint> > clusters;
+    dbScan(clusters);
+    
+    // Create new polygon container
+    PolygonContainerPtr polygons(new std::vector<geometry_msgs::msg::Polygon>());
+    
+    
+    // add convex hulls to polygon container
+    for (size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
+    {
+      polygons->push_back( geometry_msgs::msg::Polygon() );
+      concaveHull(clusters[i], concave_hull_depth_, polygons->back() );
+    }
+    
+    // add our non-cluster points to the polygon container (as single points)
+    if (!clusters.empty())
+    {
+      for (size_t i=0; i < clusters.front().size(); ++i)
+      {
+        polygons->push_back( geometry_msgs::msg::Polygon() );
+        convertPointToPolygon(clusters.front()[i], polygons->back());
+      }
+    }
+    
+    // replace shared polygon container
+    updatePolygonContainer(polygons);
+}
+
+
+void CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon)
+{
+    // start with convex hull
+    convexHull2(cluster, polygon);
+
+    std::vector<geometry_msgs::msg::Point32>& concave_list = polygon.points;
+
+    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
+    {
+      
+        // find nearest inner point pk from line (vertex1 -> vertex2)
+        const geometry_msgs::msg::Point32& vertex1 = concave_list[i];
+        const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1];
+
+        bool found;
+        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
+        if (!found) 
+          continue;  
+        
+        double line_length = norm2d(vertex1, vertex2);
+                
+        double dst1 = norm2d(cluster[nearest_idx], vertex1);
+        double dst2 = norm2d(cluster[nearest_idx], vertex2);
+        double dd = std::min(dst1, dst2);
+        if (dd<1e-8)
+          continue;
+
+        if (line_length / dd > depth)
+        {
+            // Check that new candidate edge will not intersect existing edges.
+            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
+            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
+            if (!intersects) 
+            {
+              geometry_msgs::msg::Point32 new_point;
+              cluster[nearest_idx].toPointMsg(new_point);
+              concave_list.insert(concave_list.begin() + i + 1, new_point);
+              i--;
+            }
+        }
+    }
+}
+
+
+void CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector<KeyPoint>& cluster, double depth, geometry_msgs::msg::Polygon& polygon)
+{
+    // start with convex hull
+    convexHull2(cluster, polygon);
+
+    std::vector<geometry_msgs::msg::Point32>& concave_list = polygon.points;
+    
+    // get line length
+    double mean_length = 0;
+    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
+    {
+      mean_length += norm2d(concave_list[i],concave_list[i+1]);
+    }
+    mean_length /= double(concave_list.size());
+
+    for (int i = 0; i < (int)concave_list.size() - 1; ++i)
+    {
+      
+        // find nearest inner point pk from line (vertex1 -> vertex2)
+        const geometry_msgs::msg::Point32& vertex1 = concave_list[i];
+        const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1];
+
+        double line_length = norm2d(vertex1, vertex2);
+        
+        bool found;
+        size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found);
+        if (!found) 
+        {
+          continue;  
+        }
+        
+                
+        double dst1 = norm2d(cluster[nearest_idx], vertex1);
+        double dst2 = norm2d(cluster[nearest_idx], vertex2);
+        double dd = std::min(dst1, dst2);
+        if (dd<1e-8)
+          continue;
+
+        if (line_length / dd > depth)
+        {
+            // Check that new candidate edge will not intersect existing edges.
+            bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]);
+            intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2);
+            if (!intersects) 
+            {
+              geometry_msgs::msg::Point32 new_point;
+              cluster[nearest_idx].toPointMsg(new_point);
+              concave_list.insert(concave_list.begin() + i + 1, new_point);
+              i--;
+            }
+        }
+    }
+}
+
+
+
+
+//void CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level)
+//{
+//    boost::mutex::scoped_lock lock(parameter_mutex_);
+//    parameter_buffered_.max_distance_ = config.cluster_max_distance;
+//    parameter_buffered_.min_pts_ = config.cluster_min_pts;
+//    parameter_buffered_.max_pts_ = config.cluster_max_pts;
+//    parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts;
+//    concave_hull_depth_ = config.concave_hull_depth;
+//}
+
+}//end namespace costmap_converter
+
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.cpp

@@ -0,0 +1,230 @@
+#include <random>
+#include <memory>
+#include <gtest/gtest.h>
+
+#include <costmap_converter/costmap_to_polygons.h>
+
+namespace {
+geometry_msgs::msg::Point32 create_point(double x, double y)
+{
+  geometry_msgs::msg::Point32 result;
+  result.x = x;
+  result.y = y;
+  result.z = 0.;
+  return result;
+}
+} // end namespace
+
+// make things accesible in the test
+class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH
+{
+  public:
+    const std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint>& points() const {return occupied_cells_;}
+    costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;}
+    using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2;
+    using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon;
+};
+
+class CostmapToPolygonsDBSMCCHTest : public ::testing::Test
+{
+  protected:
+    void SetUp() override {
+      // parameters
+      costmap_to_polygons.parameters().max_distance_ = 0.5;
+      costmap_to_polygons.parameters().max_pts_ = 100;
+      costmap_to_polygons.parameters().min_pts_ = 2;
+      costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1;
+
+      costmap.reset(new nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
+      costmap_to_polygons.setCostmap2D(costmap.get());
+
+      std::random_device rand_dev;
+      std::mt19937 generator(rand_dev());
+      std::uniform_real_distribution<> random_angle(-M_PI, M_PI);
+      std::uniform_real_distribution<> random_dist(0.,   costmap_to_polygons.parameters().max_distance_);
+
+      costmap_to_polygons.addPoint(0., 0.);
+      costmap_to_polygons.addPoint(1.3, 1.3);
+
+      // adding random points
+      double center_x = costmap_to_polygons.points()[0].x;
+      double center_y = costmap_to_polygons.points()[0].y;
+      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i)
+      {
+        double alpha = random_angle(generator);
+        double dist  = random_dist(generator);
+        double wx = center_x + std::cos(alpha) * dist;
+        double wy = center_y + std::sin(alpha) * dist;
+        costmap_to_polygons.addPoint(wx, wy);
+      }
+
+      // some noisy points not belonging to a cluster
+      costmap_to_polygons.addPoint(-1, -1);
+      costmap_to_polygons.addPoint(-2, -2);
+      
+      // adding random points
+      center_x = costmap_to_polygons.points()[1].x;
+      center_y = costmap_to_polygons.points()[1].y;
+      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i)
+      {
+        double alpha = random_angle(generator);
+        double dist  = random_dist(generator);
+        double wx = center_x + std::cos(alpha) * dist;
+        double wy = center_y + std::sin(alpha) * dist;
+        costmap_to_polygons.addPoint(wx, wy);
+      }
+    }
+
+    void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_indices)
+    {
+      neighbor_indices.clear();
+      const auto& query_point = costmap_to_polygons.points()[curr_index];
+      for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i)
+      {
+        if (i == curr_index)
+          continue;
+        const auto& kp = costmap_to_polygons.points()[i];
+        double dx = query_point.x - kp.x;
+        double dy = query_point.y - kp.y;
+        double dist = sqrt(dx*dx + dy*dy);
+        if (dist < costmap_to_polygons.parameters().max_distance_)
+          neighbor_indices.push_back(i);
+      }
+    }
+
+    CostmapToPolygons costmap_to_polygons;
+    std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap;
+};
+
+TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)
+{
+  std::vector<int> neighbors, neighbors_trivial;
+  costmap_to_polygons.regionQuery(0, neighbors);
+  regionQueryTrivial(0, neighbors_trivial);
+  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size()));
+  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
+  std::sort(neighbors.begin(), neighbors.end());
+  ASSERT_EQ(neighbors_trivial, neighbors);
+
+  costmap_to_polygons.regionQuery(1, neighbors);
+  regionQueryTrivial(1, neighbors_trivial);
+  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size()));
+  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
+  std::sort(neighbors.begin(), neighbors.end());
+  ASSERT_EQ(neighbors_trivial, neighbors);
+}
+
+TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)
+{
+  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
+  costmap_to_polygons.dbScan(clusters);
+  
+  ASSERT_EQ((unsigned)3, clusters.size());
+  ASSERT_EQ((unsigned)2, clusters[0].size()); // noisy points not belonging to a cluster
+  ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0)
+  ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1)
+}
+
+TEST(CostmapToPolygonsDBSMCCH, EmptyMap)
+{
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap =
+    std::make_shared<nav2_costmap_2d::Costmap2D>(nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
+  CostmapToPolygons costmap_to_polygons;
+  costmap_to_polygons.setCostmap2D(costmap.get());
+
+  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
+  costmap_to_polygons.dbScan(clusters);
+  ASSERT_EQ((unsigned)1, clusters.size());    // noise cluster exists
+  ASSERT_EQ((unsigned)0, clusters[0].size()); // noise clsuter is empty
+}
+
+TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)
+{
+  const double simplification_threshold = 0.1;
+  CostmapToPolygons costmap_to_polygons;
+  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
+  
+  geometry_msgs::msg::Polygon polygon;
+  polygon.points.push_back(create_point(0., 0.));
+  polygon.points.push_back(create_point(1., 0.));
+
+  // degenerate case with just two points
+  geometry_msgs::msg::Polygon original_polygon = polygon;
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ((unsigned)2, polygon.points.size());
+  for (size_t i = 0; i < polygon.points.size(); ++i)
+  {
+    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
+    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
+  }
+
+  // degenerate case with three points
+  polygon.points.push_back(create_point(1., simplification_threshold / 2.));
+  original_polygon = polygon;
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ((unsigned)3, polygon.points.size());
+  for (size_t i = 0; i < polygon.points.size(); ++i)
+  {
+    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
+    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
+  }
+
+  polygon.points.push_back(create_point(1., 1.));
+  original_polygon = polygon;
+  // remove the point that has to be removed by the simplification
+  original_polygon.points.erase(original_polygon.points.begin()+2);
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ((unsigned)3, polygon.points.size());
+  for (size_t i = 0; i < polygon.points.size(); ++i)
+  {
+    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
+    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
+  }
+}
+
+TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)
+{
+  const double simplification_threshold = 0.1;
+  CostmapToPolygons costmap_to_polygons;
+  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
+  
+  geometry_msgs::msg::Polygon polygon;
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(i*1., 0.));
+  geometry_msgs::msg::Point32 lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
+  lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
+  lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
+  lastPoint = polygon.points.back();
+  for (int i = 0; i <= 100; ++i)
+    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
+
+  costmap_to_polygons.simplifyPolygon(polygon);
+  ASSERT_EQ((unsigned)6, polygon.points.size());
+  ASSERT_FLOAT_EQ(  0., polygon.points[0].x);
+  ASSERT_FLOAT_EQ(  0., polygon.points[0].y);
+  ASSERT_FLOAT_EQ(100., polygon.points[1].x);
+  ASSERT_FLOAT_EQ(  0., polygon.points[1].y);
+  ASSERT_FLOAT_EQ(100., polygon.points[2].x);
+  ASSERT_FLOAT_EQ(100., polygon.points[2].y);
+  ASSERT_FLOAT_EQ(200., polygon.points[3].x);
+  ASSERT_FLOAT_EQ(100., polygon.points[3].y);
+  ASSERT_FLOAT_EQ(200., polygon.points[4].x);
+  ASSERT_FLOAT_EQ(200., polygon.points[4].y);
+  ASSERT_FLOAT_EQ(300., polygon.points[5].x);
+  ASSERT_FLOAT_EQ(200., polygon.points[5].y);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.test

@@ -0,0 +1,3 @@
+<launch>
+  <test test-name="costmap_polygons" pkg="costmap_converter" type="test_costmap_polygons" />
+</launch>

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CHANGELOG.rst

@@ -0,0 +1,41 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package costmap_converter_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.2 (2020-06-22)
+------------------
+
+0.1.1 (2020-01-25)
+------------------
+
+0.1.0 (2020-01-23)
+------------------
+* Messages moved to a separate package
+* Contributors: Vinnam Kim
+
+0.0.9 (2018-05-28)
+------------------
+
+0.0.8 (2018-05-17)
+------------------
+
+0.0.7 (2017-09-20)
+------------------
+
+0.0.6 (2017-09-18)
+------------------
+
+0.0.5 (2016-02-01)
+------------------
+
+0.0.4 (2016-01-11)
+------------------
+
+0.0.3 (2015-12-23)
+------------------
+
+0.0.2 (2015-12-22)
+------------------
+
+0.0.1 (2015-12-21)
+------------------

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(costmap_converter_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(costmap_converter_msgs
+  "msg/ObstacleMsg.msg"
+  "msg/ObstacleArrayMsg.msg"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+)
+
+ament_package()

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleArrayMsg.msg

@@ -0,0 +1,10 @@
+# Message that contains a list of polygon shaped obstacles.
+# Special types:
+# Polygon with 1 vertex: Point obstacle
+# Polygon with 2 vertices: Line obstacle
+# Polygon with more than 2 vertices: First and last points are assumed to be connected
+
+std_msgs/Header header
+
+costmap_converter_msgs/ObstacleMsg[] obstacles
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleMsg.msg

@@ -0,0 +1,24 @@
+# Special types:
+# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius)
+# Polygon with 2 vertices: Line obstacle
+# Polygon with more than 2 vertices: First and last points are assumed to be connected
+
+std_msgs/Header header
+
+# Obstacle footprint (polygon descriptions)
+geometry_msgs/Polygon polygon
+
+# Specify the radius for circular/point obstacles
+float64 radius
+
+# Obstacle ID
+# Specify IDs in order to provide (temporal) relationships
+# between obstacles among multiple messages.
+int64 id
+
+# Individual orientation (centroid)
+geometry_msgs/Quaternion orientation
+
+# Individual velocities (centroid)
+geometry_msgs/TwistWithCovariance velocities
+

src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>costmap_converter_msgs</name>
+  <version>0.1.2</version>
+  <description>Package containing message types for costmap conversion</description>
+  <maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
+  
+  <license>BSD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.10)
+project(fake_vel_transform)
+
+## Use C++14
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## By adding -Wall and -Werror, the compiler does not ignore warnings anymore,
+## enforcing cleaner code.
+add_definitions(-Wall -Werror)
+
+## Export compile commands for clangd
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+#######################
+## Find dependencies ##
+#######################
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+###########
+## Build ##
+###########
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/fake_vel_transform.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN fake_vel_transform::FakeVelTransform
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_copyright
+    ament_cmake_uncrustify
+  )
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+#############
+## Install ##
+#############
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/README.md

@@ -0,0 +1,29 @@
+# fake_vel_transform
+
+该包用于实现雷达和云台共用的 yaw 轴时的兼容性，即使云台处于旋转扫描状态，速度变换后，仍然可以实现较稳定的轨迹跟踪效果。
+
+## 实现方式
+
+创建 XYZ 轴基于 `base_link`, RPY 轴基于局部路径规划朝向的 `base_link_fake` 坐标系，使得 nav2 局部规划器能够将机器人的方向视为与当前路径规划方向一致。
+
+nav2 发布的速度也是基于 `base_link_fake` 坐标系的，通过 tf2 将速度转换到 `base_link` 坐标系，使机器人能够正常运动。在仿真中表现即底盘小陀螺时仍能跟随轨迹。
+
+电控端执行底盘控制命令时，机器人运动正方向为云台枪管朝向。
+
+## fake_vel_transform_node
+
+订阅：
+
+- nav2 发布的基于 base_link_fake 坐标系的速度指令 `/cmd_vel`
+- nav2 controller 发布的局部路径朝向 `/local_path`
+- `odom` 到 `base_link` 的 tf 变换
+
+发布：
+
+- 转换到 base_link 坐标系的速度 `/cmd_vel_chassis`
+
+静态参数：
+
+- 底盘固定旋转速度 `spin_speed`
+
+  搭配电控固定小陀螺速度，将 spin_speed 设为负，可实现移动时小陀螺减慢。

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp

@@ -0,0 +1,56 @@
+#ifndef FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
+#define FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_
+
+#include <message_filters/subscriber.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/create_timer_ros.h>
+#include <tf2_ros/message_filter.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/subscription.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+namespace fake_vel_transform
+{
+class FakeVelTransform : public rclcpp::Node
+{
+public:
+  explicit FakeVelTransform(const rclcpp::NodeOptions & options);
+
+private:
+  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+  void localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg);
+
+  void publishTransform();
+
+  // Subscriber with tf2 message_filter
+  std::string target_frame_;
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr local_pose_sub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_chassis_pub_;
+
+  // Broadcast tf from base_link to base_link_fake
+  rclcpp::TimerBase::SharedPtr tf_timer_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  geometry_msgs::msg::PoseStamped planner_local_pose_;
+  double current_angle_;
+  double base_link_angle_;
+  float spin_speed_;
+};
+
+}  // namespace fake_vel_transform
+
+#endif  // FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/launch/fake_vel_transform.launch.py

@@ -0,0 +1,20 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+
+    fake_vel_transform_node = Node(
+        package='fake_vel_transform',
+        executable='fake_vel_transform_node',
+        output='screen',
+        parameters=[
+            {'use_sim_time': use_sim_time }
+        ]
+    )
+
+    return LaunchDescription([fake_vel_transform_node])

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model
+  href="http://download.ros.org/schema/package_format3.xsd"
+  schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>fake_vel_transform</name>
+  <version>0.1.0</version>
+  <description>A template for ROS packages.</description>
+  <license>MIT</license>
+  <author email="lihanchen2004@163.com">Lihan Chen</author>
+  <maintainer email="lihanchen2004@163.com">Lihan Chen</maintainer>
+
+  <!-- buildtool_depend: dependencies of the build process -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- depend: build, export, and execution dependency -->
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_clang_format</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp

@@ -0,0 +1,103 @@
+#include "fake_vel_transform/fake_vel_transform.hpp"
+
+#include <tf2/utils.h>
+
+#include <rclcpp/logging.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/utilities.hpp>
+
+namespace fake_vel_transform
+{
+const std::string CMD_VEL_TOPIC = "/cmd_vel";
+const std::string AFTER_TF_CMD_VEL = "/cmd_vel_chassis";
+const std::string TRAJECTORY_TOPIC = "/local_plan";
+const int TF_PUBLISH_FREQUENCY = 20;  // base_link to base_link_fake. Frequency in Hz.
+
+FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options)
+: Node("fake_vel_transform", options)
+{
+  RCLCPP_INFO(get_logger(), "Start FakeVelTransform!");
+
+  // Declare and get the spin speed parameter
+  this->declare_parameter<float>("spin_speed", -6.0);
+  this->get_parameter("spin_speed", spin_speed_);
+
+  // TF broadcaster
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+  tf2_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
+
+  // Create Publisher and Subscriber
+  cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+    CMD_VEL_TOPIC, 1, std::bind(&FakeVelTransform::cmdVelCallback, this, std::placeholders::_1));
+  cmd_vel_chassis_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
+    AFTER_TF_CMD_VEL, rclcpp::QoS(rclcpp::KeepLast(1)));
+  local_pose_sub_ = this->create_subscription<nav_msgs::msg::Path>(
+    TRAJECTORY_TOPIC, 1,
+    std::bind(&FakeVelTransform::localPoseCallback, this, std::placeholders::_1));
+
+  // Create a timer to publish the transform regularly
+  tf_timer_ = this->create_wall_timer(
+    std::chrono::milliseconds(1000 / TF_PUBLISH_FREQUENCY),
+    std::bind(&FakeVelTransform::publishTransform, this));
+}
+
+// Get the local pose from planner
+void FakeVelTransform::localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg)
+{
+  if (!msg || msg->poses.empty()) {
+    RCLCPP_WARN(get_logger(), "Received empty or invalid PoseArray message");
+    return;
+  }
+
+  // Choose the pose based on the size of the poses array
+  size_t index = std::min(msg->poses.size() / 4, msg->poses.size() - 1);
+  const geometry_msgs::msg::Pose & selected_pose = msg->poses[index].pose;
+
+  // Update current angle based on the difference between teb_angle and base_link_angle_
+  double teb_angle = tf2::getYaw(selected_pose.orientation);
+  current_angle_ = teb_angle - base_link_angle_;
+}
+
+// Transform the velocity from base_link to base_link_fake
+void FakeVelTransform::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  try {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped = tf2_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero);
+    base_link_angle_ = tf2::getYaw(transform_stamped.transform.rotation);
+
+    double angle_diff = -current_angle_;
+
+    geometry_msgs::msg::Twist aft_tf_vel;
+    aft_tf_vel.angular.z = (msg->angular.z != 0) ? spin_speed_ : 0;
+    aft_tf_vel.linear.x = msg->linear.x * cos(angle_diff) + msg->linear.y * sin(angle_diff);
+    aft_tf_vel.linear.y = -msg->linear.x * sin(angle_diff) + msg->linear.y * cos(angle_diff);
+
+    cmd_vel_chassis_pub_->publish(aft_tf_vel);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_INFO(this->get_logger(), "Could not transform odom to base_link: %s", ex.what());
+  }
+}
+
+// Publish transform from base_link to base_link_fake
+void FakeVelTransform::publishTransform()
+{
+  geometry_msgs::msg::TransformStamped t;
+  t.header.stamp = get_clock()->now();
+  t.header.frame_id = "base_link";
+  t.child_frame_id = "base_link_fake";
+  tf2::Quaternion q;
+  q.setRPY(0, 0, current_angle_);
+  t.transform.rotation = tf2::toMsg(q);
+  tf_broadcaster_->sendTransform(t);
+}
+
+}  // namespace fake_vel_transform
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(fake_vel_transform::FakeVelTransform)

src/rm_nav_bringup/config/rm_navigation/rm_navigation/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.8)
+project(rm_navigation)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(CMAKE_EXPORT_COMPILE_COMMANDS)
+  set(CMAKE_EXPORT_COMPILE_COMMANDS ${CMAKE_EXPORT_COMPILE_COMMANDS})
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+    params
+    rviz
+    launch
+  )

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/bringup_rm_navigation.py

@@ -0,0 +1,156 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.actions import PushRosNamespace
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('rm_navigation')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
+    use_namespace = LaunchConfiguration('use_namespace')
+    map_yaml_file = LaunchConfiguration('map')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    params_file = LaunchConfiguration('params_file')
+    autostart = LaunchConfiguration('autostart')
+    use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+    use_nav_rviz = LaunchConfiguration('nav_rviz')
+
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'yaml_filename': map_yaml_file}
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key=namespace,
+        param_rewrites=param_substitutions,
+        convert_types=True)
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        'use_namespace',
+        default_value='false',
+        description='Whether to apply a namespace to the navigation stack')
+
+    declare_use_slam_cmd = DeclareLaunchArgument(
+        'use_slam',
+        default_value='True',
+        description='Whether run a SLAM')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        default_value= os.path.join(bringup_dir,'map', 'RMUL.yaml'),
+        description='Full path to map yaml file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='True',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='True',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True',
+        description='Whether to use composed bringup')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='True',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info',
+        description='log level')
+    
+    declare_nav_rviz_cmd = DeclareLaunchArgument(
+        'nav_rviz',
+        default_value='True',
+        description='Visualize navigation2 if true')
+
+    # Specify the actions
+    bringup_cmd_group = GroupAction([
+        PushRosNamespace(
+            condition=IfCondition(use_namespace),
+            namespace=namespace),
+
+        Node(
+            condition=IfCondition(use_composition),
+            name='nav2_container',
+            package='rclcpp_components',
+            executable='component_container_mt',
+            parameters=[configured_params, {'autostart': autostart}],
+            arguments=['--ros-args', '--log-level', log_level],
+            remappings=remappings,
+            output='screen'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
+            launch_arguments={'namespace': namespace,
+                              'use_sim_time': use_sim_time,
+                              'autostart': autostart,
+                              'params_file': params_file,
+                              'use_composition': use_composition,
+                              'use_respawn': use_respawn,
+                              'container_name': 'nav2_container'}.items()),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')),
+            condition=IfCondition(use_nav_rviz)
+        ),
+    ])
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_use_slam_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    ld.add_action(declare_nav_rviz_cmd)
+
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(bringup_cmd_group)
+
+    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/localization_amcl_launch.py

@@ -0,0 +1,149 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    namespace = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    lifecycle_nodes = ['amcl']
+
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {
+        'use_sim_time': use_sim_time}
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True),
+        allow_substs=True)
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='False',
+        description='Use composed bringup if True')
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='nav2_container',
+        description='the name of conatiner that nodes will load in if use composition')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info',
+        description='log level')
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='nav2_amcl',
+                executable='amcl',
+                name='amcl',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_localization',
+                output='screen',
+                arguments=['--ros-args', '--log-level', log_level],
+                parameters=[{'use_sim_time': use_sim_time},
+                            {'autostart': autostart},
+                            {'node_names': lifecycle_nodes}])
+        ]
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name_full,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='nav2_amcl',
+                plugin='nav2_amcl::AmclNode',
+                name='amcl',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_lifecycle_manager',
+                plugin='nav2_lifecycle_manager::LifecycleManager',
+                name='lifecycle_manager_localization',
+                parameters=[{'use_sim_time': use_sim_time,
+                             'autostart': autostart,
+                             'node_names': lifecycle_nodes}]),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+
+    # Add the actions to launch all of the localiztion nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/map_server_launch.py

@@ -0,0 +1,158 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode, ParameterFile
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('nav2_bringup')
+
+    namespace = LaunchConfiguration('namespace')
+    map_yaml_file = LaunchConfiguration('map')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    lifecycle_nodes = ['map_server']
+
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'yaml_filename': map_yaml_file}
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True),
+        allow_substs=True)
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_map_yaml_cmd = DeclareLaunchArgument(
+        'map',
+        description='Full path to map yaml file to load')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='False',
+        description='Use composed bringup if True')
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='nav2_container',
+        description='the name of conatiner that nodes will load in if use composition')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info',
+        description='log level')
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='nav2_map_server',
+                executable='map_server',
+                name='map_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_localization',
+                output='screen',
+                arguments=['--ros-args', '--log-level', log_level],
+                parameters=[{'use_sim_time': use_sim_time},
+                            {'autostart': autostart},
+                            {'node_names': lifecycle_nodes}])
+        ]
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name_full,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='nav2_map_server',
+                plugin='nav2_map_server::MapServer',
+                name='map_server',
+                parameters=[configured_params],
+                remappings=remappings),
+
+            ComposableNode(
+                package='nav2_lifecycle_manager',
+                plugin='nav2_lifecycle_manager::LifecycleManager',
+                name='lifecycle_manager_localization',
+                parameters=[{'use_sim_time': use_sim_time,
+                             'autostart': autostart,
+                             'node_names': lifecycle_nodes}]),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_map_yaml_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+
+    # Add the actions to launch all of the localiztion nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/navigation_launch.py

@@ -0,0 +1,270 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('rm_navigation')
+
+    namespace = LaunchConfiguration('namespace')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    lifecycle_nodes = ['controller_server',
+                       'smoother_server',
+                       'planner_server',
+                       'behavior_server',
+                       'bt_navigator',
+                       'waypoint_follower',
+                       'velocity_smoother']
+
+    # Map fully qualified names to relative ones so the node's namespace can be prepended.
+    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
+    # https://github.com/ros/geometry2/issues/32
+    # https://github.com/ros/robot_state_publisher/pull/30
+    # TODO(orduno) Substitute with `PushNodeRemapping`
+    #              https://github.com/ros2/launch_ros/issues/56
+    remappings = [('/tf', 'tf'),
+                  ('/tf_static', 'tf_static')]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {
+        'use_sim_time': use_sim_time,
+        'autostart': autostart}
+
+    configured_params = RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True)
+
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace')
+
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='true',
+        description='Use simulation (Gazebo) clock if true')
+
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
+        description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart', default_value='true',
+        description='Automatically startup the nav2 stack')
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='False',
+        description='Use composed bringup if True')
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='nav2_container',
+        description='the name of conatiner that nodes will load in if use composition')
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info',
+        description='log level')
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='nav2_controller',
+                executable='controller_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
+            Node(
+                package='nav2_smoother',
+                executable='smoother_server',
+                name='smoother_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+            Node(
+                package='nav2_planner',
+                executable='planner_server',
+                name='planner_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+            Node(
+                package='nav2_behaviors',
+                executable='behavior_server',
+                name='behavior_server',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+            Node(
+                package='nav2_bt_navigator',
+                executable='bt_navigator',
+                name='bt_navigator',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+            Node(
+                package='nav2_waypoint_follower',
+                executable='waypoint_follower',
+                name='waypoint_follower',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings),
+            Node(
+                package='nav2_velocity_smoother',
+                executable='velocity_smoother',
+                name='velocity_smoother',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level],
+                remappings=remappings +
+                        [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_navigation',
+                output='screen',
+                arguments=['--ros-args', '--log-level', log_level],
+                parameters=[{'use_sim_time': use_sim_time},
+                            {'autostart': autostart},
+                            {'node_names': lifecycle_nodes}]),
+        ]
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name_full,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='nav2_controller',
+                plugin='nav2_controller::ControllerServer',
+                name='controller_server',
+                parameters=[configured_params],
+                remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
+            ComposableNode(
+                package='nav2_smoother',
+                plugin='nav2_smoother::SmootherServer',
+                name='smoother_server',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_planner',
+                plugin='nav2_planner::PlannerServer',
+                name='planner_server',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_behaviors',
+                plugin='behavior_server::BehaviorServer',
+                name='behavior_server',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_bt_navigator',
+                plugin='nav2_bt_navigator::BtNavigator',
+                name='bt_navigator',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_waypoint_follower',
+                plugin='nav2_waypoint_follower::WaypointFollower',
+                name='waypoint_follower',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_velocity_smoother',
+                plugin='nav2_velocity_smoother::VelocitySmoother',
+                name='velocity_smoother',
+                parameters=[configured_params],
+                remappings=remappings +
+                           [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+            ComposableNode(
+                package='nav2_lifecycle_manager',
+                plugin='nav2_lifecycle_manager::LifecycleManager',
+                name='lifecycle_manager_navigation',
+                parameters=[{'use_sim_time': use_sim_time,
+                             'autostart': autostart,
+                             'node_names': lifecycle_nodes}]),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_sim_time_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/rviz_launch.py

@@ -0,0 +1,109 @@
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from nav2_common.launch import ReplaceString
+
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('rm_navigation')
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration('namespace')
+    use_namespace = LaunchConfiguration('use_namespace')
+    rviz_config_file = LaunchConfiguration('rviz_config')
+
+    # Declare the launch arguments
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='navigation',
+        description=('Top-level namespace. The value will be used to replace the '
+                     '<robot_namespace> keyword on the rviz config file.'))
+
+    declare_use_namespace_cmd = DeclareLaunchArgument(
+        'use_namespace',
+        default_value='false',
+        description='Whether to apply a namespace to the navigation stack')
+
+    declare_rviz_config_file_cmd = DeclareLaunchArgument(
+        'rviz_config',
+        default_value=os.path.join(bringup_dir, 'rviz', 'nav2.rviz'),
+        description='Full path to the RVIZ config file to use')
+
+    # Launch rviz
+    start_rviz_cmd = Node(
+        condition=UnlessCondition(use_namespace),
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', rviz_config_file],
+        output='screen')
+
+    namespaced_rviz_config_file = ReplaceString(
+            source_file=rviz_config_file,
+            replacements={'<robot_namespace>': ('/', namespace)})
+
+    start_namespaced_rviz_cmd = Node(
+        condition=IfCondition(use_namespace),
+        package='rviz2',
+        executable='rviz2',
+        namespace=namespace,
+        arguments=['-d', namespaced_rviz_config_file],
+        output='screen',
+        remappings=[('/map', 'map'),
+                    ('/tf', 'tf'),
+                    ('/tf_static', 'tf_static'),
+                    ('/goal_pose', 'goal_pose'),
+                    ('/clicked_point', 'clicked_point'),
+                    ('/initialpose', 'initialpose')])
+
+    exit_event_handler = RegisterEventHandler(
+        condition=UnlessCondition(use_namespace),
+        event_handler=OnProcessExit(
+            target_action=start_rviz_cmd,
+            on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
+
+    exit_event_handler_namespaced = RegisterEventHandler(
+        condition=IfCondition(use_namespace),
+        event_handler=OnProcessExit(
+            target_action=start_namespaced_rviz_cmd,
+            on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_namespace_cmd)
+    ld.add_action(declare_rviz_config_file_cmd)
+
+    # Add any conditioned actions
+    ld.add_action(start_rviz_cmd)
+    ld.add_action(start_namespaced_rviz_cmd)
+
+    # Add other nodes and processes we need
+    ld.add_action(exit_event_handler)
+    ld.add_action(exit_event_handler_namespaced)
+
+    return ld

src/rm_nav_bringup/config/rm_navigation/rm_navigation/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rm_navigation</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="fyt@todo.todo">fyt</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>spatio_temporal_voxel_layer</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/mapper_params_online_async.yaml

@@ -0,0 +1,73 @@
+slam_toolbox:
+  ros__parameters:
+
+    # Plugin params
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS Parameters
+    odom_frame: odom 
+    map_frame: map
+    base_frame: livox_frame 
+    scan_topic: /scan
+    mode: mapping #localization
+
+    # if you'd like to immediately start continuing a map at a given pose
+    # or at the dock, but they are mutually exclusive, if pose is given
+    # will use pose
+    #map_file_name: test_steve
+    # map_start_pose: [0.0, 0.0, 0.0]
+    #map_start_at_dock: true
+
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02 #if 0 never publishes odometry
+    map_update_interval: 5.0
+    resolution: 0.05
+    max_laser_range: 20.0 #for rastering images
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.
+    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
+    enable_interactive_mode: true
+
+    # General Parameters
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.5
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1  
+    link_scan_maximum_distance: 1.5
+    loop_search_maximum_distance: 3.0
+    do_loop_closing: true 
+    loop_match_minimum_chain_size: 10           
+    loop_match_maximum_variance_coarse: 3.0  
+    loop_match_minimum_response_coarse: 0.35    
+    loop_match_minimum_response_fine: 0.45
+
+    # Correlation Parameters - Correlation Parameters
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1 
+
+    # Correlation Parameters - Loop Closure Parameters
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Scan Matcher Parameters
+    distance_variance_penalty: 0.5      
+    angle_variance_penalty: 1.0    
+
+    fine_search_angle_offset: 0.00349     
+    coarse_search_angle_offset: 0.349   
+    coarse_angle_resolution: 0.0349        
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true

src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/nav2_params.yaml

@@ -0,0 +1,371 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_link"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::OmniMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /Odometry
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 5.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    # Goal checker parameters
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: False
+      min_vel_x: -2.5
+      min_vel_y: -2.5
+      max_vel_x: 2.5
+      max_vel_y: 2.5
+      max_vel_theta: 12.0
+      min_speed_xy: -8.0
+      max_speed_xy: 8.0
+      min_speed_theta: -12.0
+      acc_lim_x: 5.0
+      acc_lim_y: 5.0
+      acc_lim_theta: 15.0
+      decel_lim_x: -5.0
+      decel_lim_y: -5.0
+      decel_lim_theta: -15.0
+      vx_samples: 20
+      vy_samples: 20
+      vtheta_samples: 20
+      sim_time: 0.51
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 0.2
+      xy_goal_tolerance: 0.25
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+      
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 20.0
+      publish_frequency: 10.0
+      # global_frame: odom
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 5
+      height: 5
+      resolution: 0.05
+      robot_radius: 0.2
+      plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 8.0
+        inflation_radius: 0.7
+      voxel2d_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: False
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan 
+        scan:
+          topic: /scan
+          raytrace_min_range: 0.0
+          raytrace_max_range: 50.0
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      voxel3d_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: False
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        mark_threshold: 1
+        observation_sources: livox
+        min_obstacle_height: 0.00
+        max_obstacle_height: 2.0 
+        livox:
+          topic: /livox/lidar/pointcloud
+          raytrace_min_range: 0.0
+          raytrace_max_range: 50.0
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0 
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 20.0
+      publish_frequency: 10.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: False
+      robot_radius: 0.2
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"]
+      voxel2d_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          raytrace_min_range: 0.0
+          raytrace_max_range: 50.0
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      voxel3d_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: False
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        mark_threshold: 1
+        observation_sources: livox
+        min_obstacle_height: 0.00
+        max_obstacle_height: 2.0 
+        livox:
+          topic: /livox/lidar/pointcloud
+          raytrace_min_range: 0.0
+          raytrace_max_range: 50.0
+          min_obstacle_height: 0.0
+          max_obstacle_height: 2.0 
+          clearing: True
+          marking: True
+          data_type: "PointCloud2"
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 8.0
+        inflation_radius: 0.7
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    yaml_filename: ""
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+# planner_server_rclcpp_node:
+#   ros__parameters:
+#     use_sim_time: False
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 20.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 10.0
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 5.0
+    scale_velocities: False
+    feedback: "CLOSED_LOOP"
+    max_velocity: [2.5, 2.5, 12.0]
+    min_velocity: [-2.5, -2.5, -12.0]
+    max_accel: [5.0, 5.0, 15.0]
+    max_decel: [-5.0, -5.0, -15.0]
+    odom_topic: "Odometry"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0

src/rm_nav_bringup/config/rm_navigation/rm_navigation/rviz/nav2.rviz

@@ -0,0 +1,665 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Tree1
+      Splitter Ratio: 0.5833333134651184
+    Tree Height: 462
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: nav2_rviz_plugins/Navigation 2
+    Name: Navigation 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        dummy:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        livox_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wheel_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        wheel_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: true
+        base_link_fake:
+          Value: true
+        dummy:
+          Value: true
+        imu_link:
+          Value: true
+        lidar_odom:
+          Value: true
+        livox_frame:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        wheel_1:
+          Value: true
+        wheel_2:
+          Value: true
+        wheel_3:
+          Value: true
+        wheel_4:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        map:
+          odom:
+            lidar_odom:
+              base_link:
+                base_link_fake:
+                  {}
+                imu_link:
+                  {}
+                livox_frame:
+                  {}
+                wheel_1:
+                  {}
+                wheel_2:
+                  {}
+                wheel_3:
+                  {}
+                wheel_4:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /scan
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: voxel_grid
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05000000074505806
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /global_costmap/voxel_grid
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Bumper Hit
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.07999999821186066
+      Style: Spheres
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /mobile_base/sensors/bumper_pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.6000000238418579
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: true
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 1
+        Durability Policy: Transient Local
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /map_updates
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: nav2_rviz_plugins/ParticleCloud
+      Color: 0; 180; 0
+      Enabled: true
+      Max Arrow Length: 0.30000001192092896
+      Min Arrow Length: 0.019999999552965164
+      Name: Amcl Particle Swarm
+      Shape: Arrow (Flat)
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /particle_cloud
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.30000001192092896
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Global Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/costmap
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/costmap_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 0.30000001192092896
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Downsampled Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /downsampled_costmap
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /downsampled_costmap_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 255; 0; 0
+          Enabled: true
+          Head Diameter: 0.019999999552965164
+          Head Length: 0.019999999552965164
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: Arrows
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.004999999888241291
+          Shaft Length: 0.019999999552965164
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /plan
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 125; 125; 125
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: VoxelGrid
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.05000000074505806
+          Style: Boxes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/voxel_marked_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Class: rviz_default_plugins/Polygon
+          Color: 25; 255; 0
+          Enabled: false
+          Name: Polygon
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /global_costmap/published_footprint
+          Value: false
+      Enabled: true
+      Name: Global Planner
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.699999988079071
+          Class: rviz_default_plugins/Map
+          Color Scheme: costmap
+          Draw Behind: false
+          Enabled: true
+          Name: Local Costmap
+          Topic:
+            Depth: 1
+            Durability Policy: Transient Local
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/costmap
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/costmap_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 0; 12; 255
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Local Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_plan
+          Value: true
+        - Class: rviz_default_plugins/MarkerArray
+          Enabled: false
+          Name: Trajectories
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /marker
+          Value: false
+        - Alpha: 1
+          Class: rviz_default_plugins/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Polygon
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/published_footprint
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: VoxelGrid
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /local_costmap/voxel_marked_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: Controller
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /waypoints
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.30000001192092896
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz_default_plugins/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.07000000029802322
+      Head Radius: 0.029999999329447746
+      Name: PoseArray
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.009999999776482582
+      Shape: Arrow (Flat)
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /teb_poses
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Marker
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /teb_markers
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 0.5
+      Name: base_link_fake
+      Radius: 0.07999999821186066
+      Reference Frame: base_link_fake
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+    - Class: nav2_rviz_plugins/GoalTool
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 14.485743522644043
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 3.056168556213379
+        Y: 0.4070683419704437
+        Z: 0.4124468266963959
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 3.1423678398132324
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 932
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Navigation 2:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000438000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000020b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001390000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ad0000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1545
+  X: 1585
+  Y: 228

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitignore

@@ -0,0 +1,3 @@
+.svn
+.kdev4
+*.kdev4

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitlab-ci.yml

@@ -0,0 +1,23 @@
+build-kinetic:
+  variables:
+    ROSDISTRO: "kinetic"
+    CATKIN_WS: "/root/catkin_ws"
+  stage: build
+  image: osrf/ros:$ROSDISTRO-desktop-full
+  before_script: 
+    - apt-get -qq update
+    - apt-get -qq install git-core python-catkin-tools curl
+    - mkdir -p $CATKIN_WS/src
+    - cp -a . $CATKIN_WS/src/
+    - cd $CATKIN_WS
+    - rosdep update
+    - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false
+  script: 
+    - source /ros_entrypoint.sh
+    - cd $CATKIN_WS
+    - catkin build -i -s --no-status
+  only:
+    - kinetic-devel
+  tags:
+    - ubuntu
+    - docker

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.travis.yml

@@ -0,0 +1,108 @@
+# Generic .travis.yml file for running continuous integration on Travis-CI with
+# any ROS package.
+#
+# This installs ROS on a clean Travis-CI virtual machine, creates a ROS
+# workspace, resolves all listed dependencies, and sets environment variables
+# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are
+# no compilation errors), and runs all the tests. If any of the compilation/test
+# phases fail, the build is marked as a failure.
+#
+# We handle two types of package dependencies:
+#   - packages (ros and otherwise) available through apt-get. These are installed
+#     using rosdep, based on the information in the ROS package.xml.
+#   - dependencies that must be checked out from source. These are handled by
+#     'wstool', and should be listed in a file named dependencies.rosinstall.
+#
+# There are two variables you may want to change:
+#   - ROS_DISTRO (default is indigo). Note that packages must be available for
+#     ubuntu 14.04 trusty.
+#   - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo
+#     root). This should list all necessary repositories in wstool format (see
+#     the ros wiki). If the file does not exists then nothing happens.
+#
+# See the README.md for more information.
+#
+# Author: Felix Duvallet <felixd@gmail.com>
+
+# NOTE: The build lifecycle on Travis.ci is something like this:
+#    before_install
+#    install
+#    before_script
+#    script
+#    after_success or after_failure
+#    after_script
+#    OPTIONAL before_deploy
+#    OPTIONAL deploy
+#    OPTIONAL after_deploy
+
+################################################################################
+
+# Use ubuntu trusty (14.04) with sudo privileges.
+dist: trusty
+sudo: required
+language:
+  - generic
+cache:
+  - apt
+
+# Configuration variables. All variables are global now, but this can be used to
+# trigger a build matrix for different ROS distributions if desired.
+env:
+  global:
+    - ROS_CI_DESKTOP="`lsb_release -cs`"  # e.g. [precise|trusty|...]
+    - CI_SOURCE_PATH=$(pwd)
+    - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
+    - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
+    - ROS_PARALLEL_JOBS='-j8 -l6'
+  matrix:
+    - ROS_DISTRO=jade
+
+
+################################################################################
+
+# Install system dependencies, namely a very barebones ROS setup.
+before_install:
+  - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+  - sudo apt-get update -qq
+  - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin
+  - source /opt/ros/$ROS_DISTRO/setup.bash
+  # Prepare rosdep to install dependencies.
+  - sudo rosdep init
+  - rosdep update
+
+# Create a catkin workspace with the package under integration.
+install:
+  - mkdir -p ~/catkin_ws/src
+  - cd ~/catkin_ws/src
+  - catkin_init_workspace
+  # Create the devel/setup.bash (run catkin_make with an empty workspace) and
+  # source it to set the path variables.
+  - cd ~/catkin_ws
+  - catkin_make
+  - source devel/setup.bash
+  # Add the package under integration to the workspace using a symlink.
+  - cd ~/catkin_ws/src
+  - ln -s $CI_SOURCE_PATH .
+
+# Install all dependencies, using wstool and rosdep.
+# wstool looks for a ROSINSTALL_FILE defined in the environment variables.
+before_script:
+  # source dependencies: install using wstool.
+  - cd ~/catkin_ws/src
+  - wstool init
+  - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
+  - wstool up
+  # package depdencies: install using rosdep.
+  - cd ~/catkin_ws
+  - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+
+# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to
+# catkin_make.
+script:
+  - cd ~/catkin_ws
+  - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS )
+  # Testing: Use both run_tests (to see the output) and test (to error out).
+  - catkin_make run_tests  # This always returns 0, but looks pretty.
+  - catkin_make test  # This will return non-zero if a test fails.
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/README.md

@@ -0,0 +1,61 @@
+teb_local_planner ROS Package
+=============================
+
+The teb_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. 
+The underlying method called Timed Elastic Band locally optimizes the robot's trajectory with respect to trajectory execution time, 
+separation from obstacles and compliance with kinodynamic constraints at runtime.
+
+Refer to http://wiki.ros.org/teb_local_planner for more information and tutorials.
+
+Build status of the *melodic-devel* branch:
+- ROS Buildfarm (Melodic): [![Melodic Status](http://build.ros.org/buildStatus/icon?job=Mdev__teb_local_planner__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__teb_local_planner__ubuntu_bionic_amd64/)
+
+### Port to ROS2
+This branch is the teb_local_planner package ported to ROS2(Dashing Diademata). Currently, it is currently compatible with [Navigation2(master branch)](https://github.com/ros-planning/navigation2/tree/master)([226f06c](https://github.com/ros-planning/navigation2/commit/226f06ce282c727ca240ce8be0cb4b093e26343b)). You can test teb_local_planner with Navigation2 and TurtleBot3 simulation by launching the following command.
+```
+ros2 launch teb_local_planner teb_tb3_simulation_launch.py
+```
+
+## Citing the Software
+
+*Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the planner for your own research:*
+
+- C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.
+- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79.
+- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143.
+- C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015.
+- C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017.
+
+<a href="https://www.buymeacoffee.com/croesmann" target="_blank"><img src="https://cdn.buymeacoffee.com/buttons/lato-black.png" alt="Buy Me A Coffee" height="31px" width="132px" ></a>
+
+## Videos
+
+The left of the following videos presents features of the package and shows examples from simulation and real robot situations.
+Some spoken explanations are included in the audio track of the video. 
+The right one demonstrates features introduced in version 0.2 (supporting car-like robots and costmap conversion). Please watch the left one first.
+
+<a href="http://www.youtube.com/watch?feature=player_embedded&v=e1Bw6JOgHME" target="_blank"><img src="http://img.youtube.com/vi/e1Bw6JOgHME/0.jpg" 
+alt="teb_local_planner - An Optimal Trajectory Planner for Mobile Robots" width="240" height="180" border="10" /></a>
+<a href="http://www.youtube.com/watch?feature=player_embedded&v=o5wnRCzdUMo" target="_blank"><img src="http://img.youtube.com/vi/o5wnRCzdUMo/0.jpg" 
+alt="teb_local_planner - Car-like Robots and Costmap Conversion" width="240" height="180" border="10" /></a>
+
+## License
+
+The *teb_local_planner* package is licensed under the BSD license.
+It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed.
+
+Some third-party dependencies are included that are licensed under different terms:
+ - *Eigen*, MPL2 license, http://eigen.tuxfamily.org
+ - *libg2o* / *g2o* itself is licensed under BSD, but the enabled *csparse_extension* is licensed under LGPL3+, 
+   https://github.com/RainerKuemmerle/g2o. [*CSparse*](http://www.cise.ufl.edu/research/sparse/CSparse/) is included as part of the *SuiteSparse* collection, http://www.suitesparse.com. 
+ - *Boost*, Boost Software License, http://www.boost.org
+
+All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details.
+
+## Requirements
+
+Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*:
+
+    rosdep install teb_local_planner
+
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CHANGELOG.rst

@@ -0,0 +1,407 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package teb_local_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.9.1 (2020-05-29)
+------------------
+* Fixed RobotFootprintModel visualization bug (thanks to Anson Wang)
+* Reserve the size of the relevant obstacles vector to avoid excessive memory allocations (thanks to João Monteiro)
+* CMake: Removed system include to avoid compiling issues on some platforms
+* Contributors: Anson Wang, Christoph Rösmann, João Carlos Espiúca Monteiro
+
+0.9.0 (2020-05-26)
+------------------
+* Added pill resp. stadium-shaped obstacle
+* Changed minimum CMake version to 3.1
+* Improved efficiency of 3d h-signature computation
+* Changed default value for parameter penalty_epsilon to 0.05
+* Improved efficiency of findClosedTrajectoryPose()
+* Removed obsolete method isHorizonReductionAppropriate()
+* Contributors: Christoph Rösmann, XinyuKhan
+
+0.8.4 (2019-12-02)
+------------------
+* Fixed TEB autoResize if last TimeDiff is small
+* Add a rotational threshold for identifying a warm start goal
+* Contributors: Rainer Kümmerle
+
+0.8.3 (2019-10-25)
+------------------
+* Limiting the control look-ahead pose to the first that execeeds the expected look-ahead time (thanks to Marco Bassa)
+* test_optim_node fix circular obstacles (thanks to dtaranta)
+* Fix shadow variable warning (thanks to Victor Lopez)
+* Use SYSTEM when including external dependencies headers (thanks to Victor Lopez)
+* Robustify initTrajectoryToGoal if a plan is given (thanks to Rainer Kuemmerle)
+* Adding the option to shift ahead the target pose used to extract the velocity command (thanks to Marco Bassa)
+* Fixed segfault in optimal_planner.cpp when clearing graph with unallocated optimizer.
+  Fixes `#158 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/158>`_.
+* On footprintCost, fail only if footprint is in collision, not outside the map or on unknown space (thanks to corot)
+* Native MoveBaseFlex support added: Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces (thanks to corot)
+* added warning if parameter optimal_time is <= 0
+* Nonlinear obstacle cost from EdgeInflatedObstacle also added to EdgeObstacle.
+  See `#140 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/140>`_.
+* Fixed proper initialization of parameter obstacle_cost_exponent in case it is not loaded from the parameter server
+* Contributors: Christoph Rösmann, Marco Bassa, Rainer Kuemmerle, Victor Lopez, corot, dtaranta
+
+0.8.2 (2019-07-02)
+------------------
+* Allow scripts to be executable and usable by rosrun after catkin_make install and through the catkin release process (thanks to Devon Ash)
+* Add nonlinear part to obstacle cost to improve narrow gap behavior.
+  Parameter `obstacle_cost_exponent` defines the exponent of the nonlinear cost term.
+  The default linear behavior is achieved by setting this parameter to 1 (default).
+  A value of 4 performed well in some tests and experiments (thanks to Howard Cochran).
+* Parameter `global_plan_prune_distance` added via ros parameter server.
+* Fixed SIGSEGV in optimizeAllTEBs() if main thread is interrupted by boost (thanks to Howard Cochran)
+* Fixed SIGSEGV crash in deleteTebDetours() (thanks to Howard Cochran)
+* On footprint visualization, avoid overshadowing by obstacles (thanks to corot)
+* Do not ignore robot model on the association stage.
+  Important mostly for polygon footprint model (thanks to corot).
+* Adjustable color for footprint visualization
+* Showing (detected) infeasible robot poses in a separate marker namespace and color
+* Added edge for minimizing Euclidean path length (parameter: `weight_shortest_path`)
+* Ackermann steering conversion (python script): fixed direction inversion in backwards mode when `cmd_angle_instead_rotvel` is true (thanks to Tobi Loew)
+* Fixed wrong skipping condition in AddEdgesKinematicsCarlike() (thanks to ShiquLIU)
+* Never discarding the previous best teb in renewAndAnalyzeOldTebs (thanks to Marco Bassa)
+* Allowing for the fallback to a different trajectory when the costmap check fails. This prevents the switch to unfeasible trajectories (thanks to Marco Bassa).
+* Skipping the generation of the homotopy exploration graph in case the maximum number of allowed classes is reached (thanks to Marco Bassa)
+* Changed isTrajectoryFeasible function to allow for a more accurate linear and angular discretization (thanks to Marco Bassa)
+* Function TebOptimalPlanner::computeError() considers now the actual optimizer weights. 
+  As a result, the default value of `selection_obst_cost_scale` is reduced (thanks to Howard Cochran).
+* update to use non deprecated pluginlib macro (thanks to Mikael Arguedas)
+* Avoiding h signature interpolation between coincident poses (thanks to Marco Bassa)
+* New strategy for the deletion of detours: Detours are now determined w.r.t. the least-cost alternative and not w.r.t. just the goal heading.
+  Deletion of additional alternatives applies if either an initial backward motion is detected, if the transition time is much bigger than the duration of the best teb
+  and if a teb cannot be optimized (thanks to Marco Bassa).
+  Optionally allowing the usage of the initial plan orientation when initializing new tebs.
+* Contributors: Christoph Rösmann, Mikael Arguedas, Devon Ash, Howard Cochran, Marco Bassa, ShiquLIU, Tobi Loew, corot
+
+0.8.1 (2018-08-14)
+------------------
+* bugfix in calculateHSignature. Fixes `#90 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/90>`_.
+* fixed centroid computation in a special case of polygon-obstacles
+* Contributors: Christoph Rösmann
+
+0.8.0 (2018-08-06)
+------------------
+* First melodic release
+* Updated to new g2o API
+* Migration to tf2
+* Contributors: Christoph Rösmann
+
+0.7.3 (2018-07-05)
+------------------
+* Parameter `switching_blocking_period` added to homotopy class planner parameter group.
+  Values greater than zero enforce the homotopy class planner to only switch to new equivalence classes as soon
+  as the given period is expired (this might reduce oscillations in some scenarios). The value is set to zero seconds
+  by default in order to not change the behavior of existing configurations.
+* Fixed unconsistent naming of parameter `global_plan_viapoint_sep`.
+  The parameter retrieved at startup was `global_plan_via_point_sep` and via dynamic_reconfigure it was `global_plan_viapoint_sep`.
+  `global_plan_via_point_sep` has now been replaced by `global_plan_viapoint_sep` since this is more consistent with the variable name
+  in the code as well as `weight_viapoint` and the ros wiki description.
+  In order to not break things, the old parameter name can still be used. However, a deprecated warning is printed.
+* transformGlobalPlan searches now for the closest point within the complete subset of the global plan in the local costmap:
+  In every sampling interval, the global plan is processed in order to find the closest pose to the robot (as reference start) 
+  and the current end pose (either local at costmap boundary or max_global_plan_lookahead_dist).
+  Previously, the search algorithm stopped as soon as the distance to the robot increased once. 
+  This caused troubles with more complex global plans, hence the new strategy checks the complete subset
+  of the global plan in the local costmap for the closest distance to the robot.
+* via-points that are very close to the current robot pose or behind the robot are now skipped (in non-ordered mode)
+* Edge creation: minor performance improvement for dynamic obstacle edges
+* dynamic_reconfigure: parameter visualize_with_time_as_z_axis_scale moved to group trajectory
+* Contributors: Christoph Rösmann
+
+0.7.2 (2018-06-08)
+------------------
+* Adds the possibility to provide via-points via a topic. 
+  Currently, the user needs to decide whether to receive via-points from topic or to obtain them from the global reference plan 
+  (e.g., activate the latter by setting global_plan_viapoint_sep>0 as before).
+  A small test script publish_viapoints.py is provided to demonstrate the feature within test_optim_node.
+* Contributors: Christoph Rösmann
+
+0.7.1 (2018-06-05)
+------------------
+* Fixed a crucial bug (from 0.6.6): A cost function for prefering a clockwise resp. anti-clockwise turn was enabled by default.
+  This cost function was only intended to be active only for recovering from an oscillating robot. 
+  This cost led to a penalty for one of the turning directions and hence the maximum turning rate for the penalized direction could not be reached.
+  Furthermore, which is more crucial: since the penalty applied only to a small (initial) subset of the trajectory, the overall control performance was poor
+  (huge gap between planned motion and closed-loop trajectories led to frequent corrections of the robot pose and hence many motion reversals).
+* Adds support for circular obstacle types. This includes support for the radius field in costmap_converter::ObstacleMsg
+* rqt reconfigure: parameters are now grouped in tabs (robot, trajectory, viapoints, ...)
+* Update to use non deprecated pluginlib macro
+* Python scripts updated to new obstacle message definition.
+* Fixed issue when start and end are at the same location (PR #43)
+* Normalize marker quaternions in *test_optim_node*
+* Contributors: Christoph Rösmann, Alexander Reimann, Mikael Arguedas, wollip
+
+0.7.0 (2017-09-23)
+------------------
+* This update introduces support for dynamic obstacles (thanks to Franz Albers, who implemented and tested the code).
+  Dynamic obstacle support requires parameter *include\_dynamic\_obstacles* to be activated.
+  Note, this feature is still experimental and subject to testing.
+  Motion prediction is performed using a constant velocity model.
+  Dynamic obstacles might be incorporated as follows:
+  * via a custom message provided on topic ~/obstacles (warning: we changed the message type from teb_local_planner/ObstacleMsg to costmap_converter/ObstacleArrayMsg).
+  * via the CostmapToDynamicObstacles plugin as part of the costmap\_converter package (still experimental).
+  A tutorial is going to be provided soon.
+* FeedbackMsg includes a ObstacleMsg instead of a polygon
+* ObstacleMsg removed from package since it is now part of the costmap\_converter package.
+* Homotopy class planer code update: graph search methods and equivalence classes (h-signatures) are now 
+  implemented as subclasses of more general interfaces.
+* TEB trajectory initialization now uses a max\_vel\_x argument instead of the desired time difference in order to give the optimizer a better warm start. 
+  Old methods are marked as deprecated. This change does not affect users settings.
+* Inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer
+* teb\_local\_planner::ObstacleMsg removed in favor of costmap\_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format
+* the "new" trajectory resizing method is only activated, if "include_dynamic_obstacles" is set to true.
+  We introduced the non-fast mode with the support of dynamic obstacles
+  (which leads to better results in terms of x-y-t homotopy planning).
+  However, we have not yet tested this mode intensively, so we keep
+  the previous mode as default until we finish our tests.
+* added parameter and code to update costmap footprint if it is dynamic (#49)
+* Contributors: Franz Albers, Christoph Rösmann, procopiostein
+
+0.6.6 (2016-12-23)
+------------------
+* Strategy for recovering from oscillating local plans added (see new parameters)
+* Horizon reduction for resolving infeasible trajectories is not activated anymore if the global goal is already selected
+  (to avoid oscillations due to changing final orientations)
+* Global plan orientations are now taken for TEB initialization if lobal_plan_overwrite_orientation==true
+* Parameter max_samples added
+* Further fixes (thanks to Matthias Füller and Daniel Neumann for providing patches)
+
+0.6.5 (2016-11-15)
+------------------
+* The trajectory is now initialized backwards for goals close to and behind the robot.
+  Parameter 'allow_init_with_backwards_motion' added.
+* Updated the TEB selection in the HomotopyClassPlanner.
+  * A new parameter is introduced to prefer the equivalence class of the initial plan
+  * Fixed some bugs related to the deletion of candidates and for keeping the equivalence class of the initial plan.
+* Weight adaptation added for obstacles edges.
+  Added parameter 'weight_adapt_factor'.
+  Obstacle weights are repeatedly scaled by this factor in each outer TEB iteration.
+  Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions.
+* Added a warning if the optim footprint + min_obstacle_dist is smaller than the costmap footprint.
+  Validation is performed by only comparing the inscribed radii of the footprints.
+* Revision/extension of the reduced-horizon backup mode which is triggered in case infeasible trajectories are detected.
+* Changed HSignature to a generic equivalence class
+* Minor changes
+
+0.6.4 (2016-10-23)
+------------------
+* New default obstacle association strategy:
+  During optimization graph creation, for each pose of the trajectory a
+  relevance detection is performed before considering the obstacle
+  during optimization. New parameters are introduced. The
+  old strategy is kept as 'legacy' strategy (see parameters).
+* Computation of velocities, acceleration and turning radii extended:
+  Added an option to compute the actual arc length
+  instead of using the Euclidean distance approximation (see parameter `exact_arc_length`.
+* Added intermediate edge layer for unary, binary and multi edges in order to reduce code redundancy.
+* Script for visualizing velocity profile updated to accept the feedback topic name via rosparam server
+* Removed TebConfig dependency in TebVisualization
+* PolygonObstacle can now be constructed using a vertices container
+* HomotopyClassPlanner public interface extended
+* Changed H-Signature computation to work 'again' with few obstacles such like 1 or 2
+* Removed inline flags in visualization.cpp
+* Removed inline flags in timed_elastic_band.cpp.
+  Fixes `#15 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/15>`_.
+* Increased bounds of many variables in dynamic_reconfigure. 
+  Resolves `#14 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/14>`_.
+  The particular variables are maximum velocities, maximum accelerations,
+  minimum turning radius,...
+  Note: optimization weights and dt_ref as well as dt_hyst are not
+  tuned for velocities and accelerations beyond
+  the default values (e.g. >1 m/s). Just increasing the maximum velocity
+  bounds without adjusting the other parameters leads to an insufficient behavior.
+* Default parameter value update: 'costmap_obstacles_behind_robot_dist'
+* Additional minor fixes.
+
+0.6.3 (2016-08-17)
+------------------
+* Changed the f0 function for calculating the H-Signature.
+  The new one seems to be more robust for a much larger number of obstacles
+  after some testing.
+* HomotopyClassPlanner: vertex collision check removed since collisions will be determined in the edge collision check again
+* Fixed distance calculation polygon-to-polygon-obstacle
+* cmake config exports now *include directories* of external packages for dependent projects
+* Enlarged upper bounds on goal position and orientation tolerances in *dynamic_reconfigure*. Fixes #13.
+
+
+0.6.2 (2016-06-15)
+------------------
+* Fixed bug causing the goal to disappear in case the robot arrives with non-zero orientation error.
+* Inflation mode for obstacles added.
+* The homotopy class of the global plan is now always forced to be initialized as trajectory.
+* The initial velocity of the robot is now taken into account correctly for
+  all candidate trajectories.
+* Removed a check in which the last remaining candidate trajectory was rejected if it was close to an obstacle.
+  This fix addresses issue `#7 <https://github.com/rst-tu-dortmund/teb_local_planner/issues/7>`_
+
+0.6.1 (2016-05-23)
+------------------
+* Debian ARM64 library path added to SuiteSparse cmake find-script (resolves ARM compilation issue)
+
+
+0.6.0 (2016-05-22)
+------------------
+* Extended support to holonomic robots
+* Wrong parameter namespace for *costmap_converter* plugins fixed
+* Added the option to scale the length of the hcp sampling area
+* Compiler warnings fixed.
+* Workaround for compilation issues that are caused by a bug in boost 1.58
+  concerning the graph library (missing move constructor/assignment operator
+  in boost source).
+* Using *tf_listener* from *move_base* now.
+* Via-point support improved.
+  Added the possibility to take the actual order of via-points into account.
+  Additionally, via-points beyond start and goal are now included.
+* Obsolete include of the angles package header removed
+* Update to package.xml version 2
+* Some other minor fixes.
+
+
+0.4.0 (2016-04-19)
+------------------
+* The teb_local_planner supports a path-following mode (w.r.t. the global plan) and via-points now.
+  This allows the user to adapt the tradeoff between time-optimality and path-following.
+  Check out the new tutorial: "Following the Global Plan (Via-Points)".
+* All external configuration and launch files are removed, since they are part
+  of the new teb_local_planner_tutorials package.
+
+
+0.3.1 (2016-04-14)
+------------------
+* Fixed wrong coordinate transformation in 'line' and 'polygon' footprint models.
+* Trajectory selection strategy in case of multiple topologies updated:
+  * The obstacle costs for selection can now be scaling separately.
+  * The cost regarding time optimality can now be replaced by the actual transition time.
+  * Added a hysteresis to cost comparison between a new and the previously selected trajectory.
+  * In the default parameter setting the strategy is similar to release 0.3.0.
+* Warning message removed that occured if an odom message with only zeros was received.
+
+
+0.3.0 (2016-04-08)
+------------------
+* Different/custom robot footprints are now supported and subject to optimization (refer to the new tutorial!).
+* The new robot footprint is also visualized using the common marker topic.
+* The strategy of taking occupied costmap cells behind the robot into account has been improved.
+  These changes significantly improve navigation close to walls.
+* Parameter 'max_global_plan_lookahead_dist' added.
+  Previously, the complete subset of the global plan contained in the local costmap
+  was taken into account for choosing the current intermediate goal point. With this parameter, the maximum
+  length of the reference global plan can be limited. The actual global plan subset
+  is now computed using the logical conjunction of both local costmap size and 'max_global_plan_lookahead_dist'.
+* Bug fixes:
+  * Fixed a compilation issue on ARM architectures
+  * If custom obstacles are used, the container with old obstacles is now cleared properly. 
+* Parameter cleanup: 
+  * "weight_X_obstacle" parameters combined to single parameter "weight_obstacle".
+  * "X_obstacle_poses_affected" parameters combined to single parameter "obstacle_poses_affected". 
+  * Deprecated parameter 'costmap_emergency_stop_dist' removed.
+* Code cleanup
+
+
+0.2.3 (2016-02-01)
+------------------
+* Marker lifetime changed
+* In case the local planner detects an infeasible trajectory it does now try to
+  reduce the horizon to 50 percent of the length. The trajectory is only reduced
+  if some predefined cases are detected.
+  This mechanism constitutes a backup behavior.
+* Improved carlike robot support.
+  Instead of commanding the robot using translational and rotational velocities,
+  the robot might also be commanded using the transl. velocity and steering angle.
+  Appropriate parameters are added to the config.
+* Changed default parameter for 'h_signature_threshold' from 0.01 to 0.1 to better match the actual precision.
+* Some python scripts for data conversion added
+* Minor other changes
+
+0.2.2 (2016-01-11)
+------------------
+* Carlike robots (ackermann steering) are supported from now on (at least experimentally) 
+  by specifying a minimum bound on the turning radius.
+  Currently, the output of the planner in carlike mode is still (v,omega).
+  Since I don't have any real carlike robot, I would be really happy if someone could provide me with
+  some feedback to further improve/extend the support.
+* Obstacle cost function modified to avoid undesired jerks in the trajectory.
+* Added a feedback message that contains current trajectory information (poses, velocities and temporal information).
+  This is useful for analyzing and debugging the velocity profile e.g. at runtime.
+  The message will be published only if it's activated (rosparam).
+  A small python script is added to plot the velocity profile (while *test_optim_node* runs).
+* Cost functions are now taking the direction/sign of the translational velocity into account:
+  Specifying a maximum backwards velocity other than forward velocity works now.
+  Additionally, the change in acceleration is now computed correctly if the robot switches directions.
+* The global plan is now pruned such that already passed posses are cut off
+  (relevant for global planners with *planning_rate=0*).
+* Fixed issue#1: If a global planner with *planning_rate=0* was used, 
+  a TF timing/extrapolation issue appeared after some time.
+* The planner resets now properly if the velocity command cannot be computed due to invalid optimization results.
+
+
+0.2.1 (2015-12-30)
+------------------
+* This is an important bugfix release.
+* Fixed a major issue concerning the stability and performance of the optimization process. Each time the global planner was updating the global plan, the local planner was resetted completely even if
+  the updated global plan did not differ from the previous one. This led to stupid reinitializations and a slighly jerky behavior if the update rate of the global planner was high (each 0.5-2s).
+  From now on the local planner is able to utilize the global plan as a warm start and determine automatically whether to reinitialize or not.
+* Support for polygon obstacles extended and improved (e.g. the homotopy class planner does now compute actual distances to the polygon rather than utilizing the distance to the centroid).
+
+0.2.0 (2015-12-23)
+------------------
+* The teb_local_planner supports costmap_converter plugins (pluginlib) from now on. Those plugins convert occupied costmap2d cells into polygon shapes.
+  The costmap_converter is disabled by default, since the extension still needs to be tested (parameter choices, computation time advantages, etc.). 
+  A tutorial will explain how to activate the converter using the ros-param server.
+
+0.1.11 (2015-12-12)
+-------------------
+* This is a bugfix release (it fixes a lot of issues which occured frequently when the robot was close to the goal)
+
+0.1.10 (2015-08-13)
+-------------------
+* The optimizer copies the global plan as initialization now instead of using a simple straight line approximation.
+* Some bugfixes and improvements
+
+0.1.9 (2015-06-24)
+------------------
+* Fixed a segmentation fault issue. This minor update is crucial for stability.
+
+0.1.8 (2015-06-08)
+------------------
+* Custom obstacles can be included via publishing dedicated messages
+* Goal-reached-condition also checks orientation error (desired yaw) now
+* Numerical improvements of the h-signature calculation
+* Minor bugfixes
+
+0.1.7 (2015-05-22)
+------------------
+* Finally fixed saucy compilation issue by retaining compatiblity to newer distros
+  (my "new" 13.10 VM helps me to stop spamming new releases for testing).
+
+0.1.6 (2015-05-22)
+------------------
+* Fixed compilation errors on ubuntu saucy caused by different FindEigen.cmake scripts.
+  I am not able to test releasing on saucy, forcing me to release again and again. Sorry.
+
+0.1.5 (2015-05-21)
+------------------
+* Added possibility to dynamically change parameters of test_optim_node using dynamic reconfigure.
+* Fixed a wrong default-min-max tuple in the dynamic reconfigure config.
+* Useful config and launch files are now added to cmake install.
+* Added install target for the test_optim_node executable.
+
+0.1.4 (2015-05-20)
+------------------
+* Fixed compilation errors on ROS Jade
+
+0.1.3 (2015-05-20)
+------------------
+* Fixed compilation errors on ubuntu saucy
+
+0.1.2 (2015-05-19)
+------------------
+* Removed unused include that could break compilation.
+
+0.1.1 (2015-05-19)
+------------------
+* All files added to the indigo-devel branch
+* Initial commit
+* Contributors: Christoph Rösmann

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CMakeLists.txt

@@ -0,0 +1,162 @@
+cmake_minimum_required(VERSION 3.5)
+project(teb_local_planner)
+
+# Set to Release in order to speed up the program significantly
+set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+#  add_compile_options(-Wall -Wextra -Wpedantic)
+#endif()
+
+## Find catkin macros and libraries
+find_package(ament_cmake REQUIRED)
+find_package(costmap_converter REQUIRED)
+find_package(dwb_critics REQUIRED)
+find_package(nav_2d_utils REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+#find_package(interactive_markers REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(teb_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+message(STATUS "System: ${CMAKE_SYSTEM}")
+## System dependencies are found with CMake's conventions
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules)
+message(STATUS "${CMAKE_MODULE_PATH}")
+find_package(SUITESPARSE REQUIRED)
+find_package(G2O REQUIRED)
+find_package(Boost REQUIRED)
+
+# Eigen3 FindScript Backward compatibility (ubuntu saucy)
+# Since FindEigen.cmake is deprecated starting from jade.
+if (EXISTS "FindEigen3.cmake")
+  find_package(Eigen3 REQUIRED)
+  set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS})
+elseif (EXISTS "FindEigen.cmake")
+  find_package(Eigen REQUIRED)
+elseif (EXISTS "FindEigen.cmake")
+  message(WARNING "No findEigen cmake script found. You must provde one of them,
+  e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.")
+endif (EXISTS "FindEigen3.cmake")
+
+set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR})
+set(EXTERNAL_LIBS ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES})
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${Boost_INCLUDE_DIRS}
+  ${EXTERNAL_INCLUDE_DIRS}
+)
+
+## Build the teb_local_planner library
+
+add_library(teb_local_planner SHARED
+   src/timed_elastic_band.cpp
+   src/optimal_planner.cpp
+   src/obstacles.cpp
+   src/visualization.cpp
+   src/recovery_behaviors.cpp
+   src/teb_config.cpp
+   src/homotopy_class_planner.cpp
+   src/teb_local_planner_ros.cpp
+   src/graph_search.cpp
+)
+
+set(ament_dependencies
+    costmap_converter
+    dwb_critics
+    nav_2d_utils
+    nav2_core
+    nav2_costmap_2d
+    nav2_util
+    geometry_msgs
+    nav_msgs
+    rclcpp
+    rclcpp_action
+    rclcpp_lifecycle
+    std_msgs
+    pluginlib
+    teb_msgs
+    tf2
+    tf2_eigen
+    tf2_geometry_msgs
+    tf2_ros
+    visualization_msgs
+    builtin_interfaces
+)
+ament_target_dependencies(teb_local_planner
+   ${ament_dependencies}
+)
+target_link_libraries(teb_local_planner
+   ${EXTERNAL_LIBS}
+)
+
+target_compile_definitions(teb_local_planner PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+
+#############
+## Install ##
+#############
+
+## Mark executables and/or libraries for installation
+install(TARGETS teb_local_planner
+   DESTINATION lib
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/
+   DESTINATION include/
+)
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(FILES
+  teb_local_planner_plugin.xml
+  DESTINATION share
+  )
+
+install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
+
+ament_export_include_directories(include)
+ament_export_libraries(teb_local_planner)
+ament_export_dependencies(${ament_dependencies})
+pluginlib_export_plugin_description_file(nav2_core teb_local_planner_plugin.xml)
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  #ament_add_gtest(homotopy_class_planner_test
+  #    test/homotopy_class_planner_test.cpp
+  #)
+  #target_link_libraries(homotopy_class_planner_test
+  #    teb_local_planner
+  #)
+endif()
+
+ament_package()

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/LICENSE

@@ -0,0 +1,28 @@
+Copyright (c) 2016, TU Dortmund - Lehrstuhl RST
+All rights reserved.
+
+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 teb_local_planner 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.
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg

@@ -0,0 +1,420 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+#from local_planner_limits import add_generic_localplanner_params
+
+gen = ParameterGenerator()
+
+# This unusual line allows to reuse existing parameter definitions
+# that concern all localplanners
+#add_generic_localplanner_params(gen)
+
+# For integers and doubles:
+#       Name                    Type      Reconfiguration level
+#       Description
+#       Default  Min  Max
+
+
+grp_trajectory = gen.add_group("Trajectory", type="tab")
+
+# Trajectory
+grp_trajectory.add("teb_autosize",   bool_t,   0, 
+	"Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)",
+	True)
+
+grp_trajectory.add("dt_ref", double_t, 0, 
+	"Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)",
+	0.3, 0.01,  1)
+
+grp_trajectory.add("dt_hysteresis", double_t, 0, 
+	"Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref",
+	0.1, 0.002,  0.5)
+
+grp_trajectory.add("global_plan_overwrite_orientation",   bool_t,   0,
+	"Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically",
+	True)
+
+grp_trajectory.add("allow_init_with_backwards_motion",   bool_t,   0,
+	"If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)",
+	False)
+	
+grp_trajectory.add("max_global_plan_lookahead_dist",   double_t,   0,
+  "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]",
+  3.0, 0, 50.0) 
+	
+grp_trajectory.add("force_reinit_new_goal_dist",   double_t,   0,
+	"Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)",
+	1.0, 0.0, 10.0)
+
+grp_trajectory.add("force_reinit_new_goal_angular",   double_t,   0,
+	"Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)",
+	0.78, 0.0, 4.0)	
+	
+grp_trajectory.add("feasibility_check_no_poses",   int_t,   0,
+  "Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.",
+  5, -1, 50)
+
+grp_trajectory.add("feasibility_check_lookahead_distance",   double_t,   0,
+  "Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.",
+  -1, -1, 20)
+
+grp_trajectory.add("exact_arc_length",   bool_t,   0,
+  "If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.",
+  False)    
+   
+grp_trajectory.add("publish_feedback",   bool_t,   0,
+  "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)",
+  False)    
+
+grp_trajectory.add("visualize_with_time_as_z_axis_scale",    double_t,   0,
+  "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.",
+  0, 0, 1)
+        
+# ViaPoints
+grp_viapoints = gen.add_group("ViaPoints", type="tab")
+
+grp_viapoints.add("global_plan_viapoint_sep",   double_t,   0,
+  "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]",
+  -0.1, -0.1, 5.0) 
+
+grp_viapoints.add("via_points_ordered",   bool_t,   0,
+  "If true, the planner adheres to the order of via-points in the storage container",
+  False)     
+
+# Robot
+grp_robot = gen.add_group("Robot", type="tab")
+
+grp_robot.add("max_vel_x", double_t, 0, 
+	"Maximum translational velocity of the robot",
+	0.4, 0.01, 100)   
+
+grp_robot.add("max_vel_x_backwards", double_t, 0, 
+	"Maximum translational velocity of the robot for driving backwards",
+	0.2, 0.01, 100)   
+	    
+grp_robot.add("max_vel_theta", double_t, 0,
+	"Maximum angular velocity of the robot", 
+	0.3, 0.01, 100) 
+
+grp_robot.add("acc_lim_x", double_t, 0, 
+	"Maximum translational acceleration of the robot",
+	0.5, 0.01, 100)   
+	    
+grp_robot.add("acc_lim_theta", double_t, 0,
+	"Maximum angular acceleration of the robot", 
+	0.5, 0.01, 100)  
+ 
+grp_robot.add("is_footprint_dynamic",   bool_t,   0,
+  "If true, updated the footprint before checking trajectory feasibility",
+  False)
+
+grp_robot.add("use_proportional_saturation", bool_t, 0,
+	"If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually",
+	False)
+grp_robot.add("transform_tolerance", double_t, 0,
+	"Tolerance when querying the TF Tree for a transformation (seconds)",
+	0.5, 0.001, 20)
+
+# Robot/Carlike
+
+grp_robot_carlike = grp_robot.add_group("Carlike", type="hide")
+
+grp_robot_carlike.add("min_turning_radius", double_t, 0,
+  "Minimum turning radius of a carlike robot (diff-drive robot: zero)", 
+  0.0, 0.0, 50.0)   
+
+grp_robot_carlike.add("wheelbase", double_t, 0,
+  "The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!", 
+  1.0, -10.0, 10.0)  
+  
+grp_robot_carlike.add("cmd_angle_instead_rotvel",   bool_t,   0, 
+  "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", 
+  False)  
+
+# Robot/Omni
+
+grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide")
+
+grp_robot_omni.add("max_vel_y", double_t, 0, 
+  "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)",
+  0.0, 0.0, 100) 
+
+grp_robot_omni.add("acc_lim_y", double_t, 0, 
+  "Maximum strafing acceleration of the robot",
+  0.5, 0.01, 100) 
+
+# GoalTolerance
+grp_goal = gen.add_group("GoalTolerance", type="tab")
+    
+grp_goal.add("free_goal_vel",   bool_t,   0, 
+	"Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", 
+	False)
+
+# Obstacles
+grp_obstacles = gen.add_group("Obstacles", type="tab")
+
+grp_obstacles.add("min_obstacle_dist", double_t, 0, 
+	"Minimum desired separation from obstacles", 
+	0.5, 0, 10)     
+
+grp_obstacles.add("inflation_dist", double_t, 0,
+        "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
+        0.6, 0, 15)
+
+grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0,
+        "Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)",
+        0.6, 0, 15)
+
+grp_obstacles.add("include_dynamic_obstacles", bool_t, 0,
+        "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.",
+        False)
+
+grp_obstacles.add("include_costmap_obstacles",   bool_t,   0, 
+	"Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", 
+	True)
+	
+grp_obstacles.add("legacy_obstacle_association",  bool_t,   0, 
+	"If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).", 
+	False)
+	
+grp_obstacles.add("obstacle_association_force_inclusion_factor",   double_t,   0,
+  "The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.", 
+  1.5, 0.0, 100.0) 
+
+grp_obstacles.add("obstacle_association_cutoff_factor",   double_t,   0,
+  "See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.", 
+  5.0, 1.0, 100.0)   
+  
+grp_obstacles.add("costmap_obstacles_behind_robot_dist",   double_t,   0,
+  "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", 
+  1.5, 0.0, 20.0)  
+
+grp_obstacles.add("obstacle_poses_affected",    int_t,    0, 
+	"The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well", 
+	30, 0, 200)
+
+# Obstacle - Velocity ratio parameters
+grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles")
+
+grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0,
+        "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles",
+        1, 0, 1)
+
+grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0,
+        "Distance to a static obstacle for which the velocity should be lower",
+        0, 0, 10)
+
+grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0,
+        "Distance to a static obstacle for which the velocity should be higher",
+        0.5, 0, 10)
+
+# Optimization
+grp_optimization = gen.add_group("Optimization", type="tab")    
+
+grp_optimization.add("no_inner_iterations",    int_t,    0,
+	"Number of solver iterations called in each outerloop iteration", 
+	5, 1, 100)
+
+grp_optimization.add("no_outer_iterations",    int_t,    0, 
+	"Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", 
+	4, 1, 100)
+
+grp_optimization.add("optimization_activate",   bool_t,   0,
+	"Activate the optimization",
+	True)
+
+grp_optimization.add("optimization_verbose",   bool_t,   0, 
+	"Print verbose information", 
+	False)
+    
+grp_optimization.add("penalty_epsilon", double_t, 0, 
+	"Add a small safty margin to penalty functions for hard-constraint approximations",
+        0.05, 0, 1.0)
+
+grp_optimization.add("weight_max_vel_x", double_t, 0, 
+	"Optimization weight for satisfying the maximum allowed translational velocity", 
+	2, 0, 1000) 
+	
+grp_optimization.add("weight_max_vel_y", double_t, 0, 
+	"Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", 
+	2, 0, 1000) 
+
+grp_optimization.add("weight_max_vel_theta", double_t, 0, 
+	"Optimization weight for satisfying the maximum allowed angular velocity", 
+	1, 0, 1000) 
+
+grp_optimization.add("weight_acc_lim_x", double_t, 0, 
+	"Optimization weight for satisfying the maximum allowed translational acceleration", 
+	1, 0, 1000)
+	
+grp_optimization.add("weight_acc_lim_y", double_t, 0, 
+	"Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", 
+	1, 0, 1000)
+ 
+grp_optimization.add("weight_acc_lim_theta", double_t, 0, 
+	"Optimization weight for satisfying the maximum allowed angular acceleration",
+	1, 0, 1000) 
+
+grp_optimization.add("weight_kinematics_nh", double_t, 0,
+	"Optimization weight for satisfying the non-holonomic kinematics", 
+	1000 , 0, 10000) 
+
+grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, 
+  "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)",
+  1, 0, 10000)
+	
+grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, 
+  "Optimization weight for enforcing a minimum turning radius (carlike robots)", 
+  1, 0, 1000)
+
+grp_optimization.add("weight_optimaltime", double_t, 0, 
+        "Optimization weight for contracting the trajectory w.r.t. transition time",
+	1, 0, 1000) 
+
+grp_optimization.add("weight_shortest_path", double_t, 0,
+        "Optimization weight for contracting the trajectory w.r.t. path length",
+        0, 0, 100)
+
+grp_optimization.add("weight_obstacle", double_t, 0, 
+	"Optimization weight for satisfying a minimum seperation from obstacles",
+	50, 0, 1000) 
+	
+grp_optimization.add("weight_inflation", double_t, 0, 
+        "Optimization weight for the inflation penalty (should be small)",
+        0.1, 0, 10) 
+
+grp_optimization.add("weight_dynamic_obstacle", double_t, 0, 
+	"Optimization weight for satisfying a minimum seperation from dynamic obstacles", 
+	50, 0, 1000) 
+
+grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0,
+        "Optimization weight for the inflation penalty of dynamic obstacles (should be small)",
+        0.1, 0, 10)
+
+grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0,
+        "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle",
+        0, 0, 1000)
+
+grp_optimization.add("weight_viapoint", double_t, 0, 
+  "Optimization weight for minimizing the distance to via-points", 
+  1, 0, 1000) 
+
+grp_optimization.add("weight_adapt_factor", double_t, 0, 
+  "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.", 
+  2, 1, 100) 
+
+grp_optimization.add("obstacle_cost_exponent", double_t, 0,
+	"Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)",
+	1, 0.01, 100)
+
+  
+  
+# Homotopy Class Planner
+grp_hcp = gen.add_group("HCPlanning", type="tab")    
+    
+grp_hcp.add("enable_multithreading",    bool_t,    0,
+	"Activate multiple threading for planning multiple trajectories in parallel", 
+	True)
+
+grp_hcp.add("max_number_classes",    int_t,    0,
+	"Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", 
+	5, 1, 100)
+
+grp_hcp.add("max_number_plans_in_current_class",    int_t,    0,
+	"Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes",
+	1, 1, 10)
+
+grp_hcp.add("selection_cost_hysteresis", double_t, 0, 
+  "Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)", 
+  1.0, 0, 2) 
+
+
+grp_hcp.add("selection_prefer_initial_plan", double_t, 0, 
+  "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", 
+  0.95, 0, 1) 
+
+grp_hcp.add("selection_obst_cost_scale", double_t, 0, 
+  "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", 
+  2.0, 0, 1000) 
+
+grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, 
+  "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", 
+  1.0, 0, 100) 
+
+grp_hcp.add("selection_alternative_time_cost",   bool_t,   0, 
+  "If true, time cost is replaced by the total transition time.", 
+  False)
+
+grp_hcp.add("selection_dropping_probability", double_t, 0,
+  "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", 
+  0.0, 0.0, 1.0)
+
+grp_hcp.add("switching_blocking_period",   double_t,   0,
+  "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed",
+  0.0, 0.0, 60)
+
+grp_hcp.add("roadmap_graph_no_samples",    int_t,    0,
+	"Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", 
+	15, 1, 100)
+
+grp_hcp.add("roadmap_graph_area_width", double_t, 0, 
+	"Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", 
+	5, 0.1, 20) 
+	
+grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, 
+        "The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)", 
+        1.0, 0.5, 2) 
+
+grp_hcp.add("h_signature_prescaler", double_t, 0, 
+	"Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2<H<=1)", 
+	1, 0.2, 1) 
+
+grp_hcp.add("h_signature_threshold", double_t, 0, 
+	"Two h-signuteres are assumed to be equal, if both the difference of real parts and complex parts are below the specified threshold", 
+	0.1, 0, 1) 
+
+grp_hcp.add("obstacle_heading_threshold", double_t, 0, 
+	"Specify the value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account for exploration)", 
+	0.45, 0, 1) 
+
+grp_hcp.add("viapoints_all_candidates",    bool_t,    0,
+  "If true, all trajectories of different topologies are attached to the set of via-points, otherwise only the trajectory sharing the same one as the initial/global plan is attached (no effect in test_optim_node).", 
+  True)
+
+grp_hcp.add("visualize_hc_graph",    bool_t,    0,
+	"Visualize the graph that is created for exploring new homotopy classes", 
+	False)
+
+
+# Recovery
+grp_recovery = gen.add_group("Recovery", type="tab")   
+
+grp_recovery.add("shrink_horizon_backup",   bool_t,   0,
+  "Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues.",
+  True) 
+
+grp_recovery.add("oscillation_recovery",   bool_t,   0,
+  "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
+  True) 
+
+grp_recovery_divergence = grp_recovery.add_group("Divergence Detection", type="hide")
+
+grp_recovery_divergence.add(
+    "divergence_detection_enable",
+    bool_t,
+    0,
+    "True to enable divergence detection.",
+    False
+)
+
+grp_recovery_divergence.add(
+    "divergence_detection_max_chi_squared",
+    double_t,
+    0,
+    "Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged.",
+    10,
+    0,
+    100
+)
+
+exit(gen.generate("teb_local_planner", "teb_local_planner", "TebLocalPlannerReconfigure"))

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/rviz_test_optim.rviz

@@ -0,0 +1,183 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 81
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 562
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.100000001
+      Cell Size: 0.100000001
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Fine Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /test_optim_node/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.300000012
+      Axes Radius: 0.00999999978
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: PoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Arrow (Flat)
+      Topic: /test_optim_node/teb_poses
+      Unreliable: false
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /test_optim_node/teb_markers
+      Name: Marker
+      Namespaces:
+        PointObstacles: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /marker_obstacles/update
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 7.77247
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.56979632
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.71043873
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1354
+  X: 137
+  Y: 50

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindG2O.cmake

@@ -0,0 +1,97 @@
+# Locate the g2o libraries
+# A general framework for graph optimization.
+#
+# This module defines
+# G2O_FOUND, if false, do not try to link against g2o
+# G2O_LIBRARIES, path to the libg2o
+# G2O_INCLUDE_DIR, where to find the g2o header files
+#
+# Niko Suenderhauf <niko@etit.tu-chemnitz.de>
+# Adapted by Felix Endres <endres@informatik.uni-freiburg.de>
+
+IF(UNIX)
+
+  #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
+    # in cache already
+    #  SET(G2O_FIND_QUIETLY TRUE)
+    #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES)
+
+  MESSAGE(STATUS "Searching for g2o ...")
+  FIND_PATH(G2O_INCLUDE_DIR
+    NAMES core math_groups types
+    PATHS /usr/local /usr
+    PATH_SUFFIXES include/g2o include)
+
+  IF (G2O_INCLUDE_DIR)
+    MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}")
+  ENDIF (G2O_INCLUDE_DIR)
+
+  FIND_LIBRARY(G2O_CORE_LIB             
+    NAMES g2o_core g2o_core_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_STUFF_LIB            
+    NAMES g2o_stuff g2o_stuff_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB     
+    NAMES g2o_types_slam2d g2o_types_slam2d_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB     
+    NAMES g2o_types_slam3d g2o_types_slam3d_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB   
+    NAMES g2o_solver_cholmod g2o_solver_cholmod_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_SOLVER_PCG_LIB       
+    NAMES g2o_solver_pcg g2o_solver_pcg_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB   
+    NAMES g2o_solver_csparse g2o_solver_csparse_rd
+    PATHS /usr/local /usr 
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_INCREMENTAL_LIB      
+    NAMES g2o_incremental g2o_incremental_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+  FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB
+    NAMES g2o_csparse_extension g2o_csparse_extension_rd
+    PATHS /usr/local /usr ${CMAKE_PREFIX_PATH}
+    PATH_SUFFIXES lib)
+
+  SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB}
+                    ${G2O_CORE_LIB}           
+                    ${G2O_STUFF_LIB}          
+                    ${G2O_TYPES_SLAM2D_LIB}   
+                    ${G2O_TYPES_SLAM3D_LIB}   
+                    ${G2O_SOLVER_CHOLMOD_LIB} 
+                    ${G2O_SOLVER_PCG_LIB}     
+                    ${G2O_SOLVER_CSPARSE_LIB} 
+                    ${G2O_INCREMENTAL_LIB}                        
+                    )
+
+  IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+    SET(G2O_FOUND "YES")
+    IF(NOT G2O_FIND_QUIETLY)
+      MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}")
+    ENDIF(NOT G2O_FIND_QUIETLY)
+  ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+    IF(NOT G2O_LIBRARIES)
+      IF(G2O_FIND_REQUIRED)
+        message(FATAL_ERROR "Could not find libg2o!")
+      ENDIF(G2O_FIND_REQUIRED)
+    ENDIF(NOT G2O_LIBRARIES)
+
+    IF(NOT G2O_INCLUDE_DIR)
+      IF(G2O_FIND_REQUIRED)
+        message(FATAL_ERROR "Could not find g2o include directory!")
+      ENDIF(G2O_FIND_REQUIRED)
+    ENDIF(NOT G2O_INCLUDE_DIR)
+  ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR)
+
+ENDIF(UNIX)
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake

@@ -0,0 +1,133 @@
+# - Try to find SUITESPARSE
+# Once done this will define
+#  
+#  SUITESPARSE_FOUND            - system has SUITESPARSE
+#  SUITESPARSE_INCLUDE_DIRS     - the SUITESPARSE include directory
+#  SUITESPARSE_LIBRARIES        - Link these to use SUITESPARSE
+#  SUITESPARSE_SPQR_LIBRARY     - name of spqr library (necessary due to error in debian package)
+#  SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package)
+#  SUITESPARSE_LIBRARY_DIR      - Library main directory containing suitesparse libs
+#  SUITESPARSE_LIBRARY_DIRS     - all Library directories containing suitesparse libs
+#  SUITESPARSE_SPQR_VALID       - automatic identification whether or not spqr package is installed correctly
+
+IF (SUITESPARSE_INCLUDE_DIRS)
+  # Already in cache, be silent
+  SET(SUITESPARSE_FIND_QUIETLY TRUE)
+ENDIF (SUITESPARSE_INCLUDE_DIRS)
+
+if( WIN32 )
+   # Find cholmod part of the suitesparse library collection
+
+   FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+              PATHS "C:\\libs\\win32\\SuiteSparse\\Include"  )
+
+   # Add cholmod include directory to collection include directories
+   IF ( CHOLMOD_INCLUDE_DIR )
+	list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
+   ENDIF( CHOLMOD_INCLUDE_DIR )
+
+
+   # find path suitesparse library
+   FIND_PATH( SUITESPARSE_LIBRARY_DIRS 
+	         amd.lib
+               PATHS "C:\\libs\\win32\\SuiteSparse\\libs" )
+
+   # if we found the library, add it to the defined libraries
+   IF ( SUITESPARSE_LIBRARY_DIRS )
+	list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd )
+   ENDIF( SUITESPARSE_LIBRARY_DIRS )  
+
+else( WIN32 )
+   IF(APPLE)
+	   FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+        	      PATHS /opt/local/include/ufsparse
+			    /usr/local/include )
+
+           FIND_PATH( SUITESPARSE_LIBRARY_DIR
+                      NAMES libcholmod.a
+                      PATHS /opt/local/lib
+			    /usr/local/lib )
+   ELSE(APPLE)
+	   FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h
+        	      PATHS /usr/local/include 
+        	            /usr/include 
+        	            /usr/include/suitesparse/ 
+        	            ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod
+              	      PATH_SUFFIXES cholmod/ CHOLMOD/ )
+   	
+           FIND_PATH( SUITESPARSE_LIBRARY_DIR
+                      NAMES libcholmod.so libcholmod.a
+                      PATHS /usr/lib 
+                            /usr/lib64
+                            /usr/lib/x86_64-linux-gnu
+                            /usr/lib/i386-linux-gnu
+                            /usr/local/lib
+                            /usr/lib/arm-linux-gnueabihf/
+                            /usr/lib/aarch64-linux-gnu/
+                            /usr/lib/arm-linux-gnueabi/
+                            /usr/lib/arm-linux-gnu)
+   ENDIF(APPLE)
+
+   # Add cholmod include directory to collection include directories
+   IF ( CHOLMOD_INCLUDE_DIR )
+	list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} )
+   ENDIF( CHOLMOD_INCLUDE_DIR )
+
+   # if we found the library, add it to the defined libraries
+   IF ( SUITESPARSE_LIBRARY_DIR )
+
+       list ( APPEND SUITESPARSE_LIBRARIES amd)
+       list ( APPEND SUITESPARSE_LIBRARIES btf)
+       list ( APPEND SUITESPARSE_LIBRARIES camd)
+       list ( APPEND SUITESPARSE_LIBRARIES ccolamd)
+       list ( APPEND SUITESPARSE_LIBRARIES cholmod)
+       list ( APPEND SUITESPARSE_LIBRARIES colamd)
+ #     list ( APPEND SUITESPARSE_LIBRARIES csparse)
+       list ( APPEND SUITESPARSE_LIBRARIES cxsparse)
+       list ( APPEND SUITESPARSE_LIBRARIES klu)
+ #     list ( APPEND SUITESPARSE_LIBRARIES spqr)
+       list ( APPEND SUITESPARSE_LIBRARIES umfpack)
+
+       IF (APPLE)
+           list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig)
+       ENDIF (APPLE)
+
+       # Metis and spqr are optional
+       FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY
+                     NAMES metis
+                     PATHS ${SUITESPARSE_LIBRARY_DIR} )
+       IF (SUITESPARSE_METIS_LIBRARY)			
+	  list ( APPEND SUITESPARSE_LIBRARIES metis)
+       ENDIF(SUITESPARSE_METIS_LIBRARY)
+
+       if(EXISTS  "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp")
+	  SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid")
+       else()
+	  SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid")
+       endif()
+
+       if(SUITESPARSE_SPQR_VALID)
+	  FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY
+		      NAMES spqr
+		      PATHS ${SUITESPARSE_LIBRARY_DIR} )
+	  IF (SUITESPARSE_SPQR_LIBRARY)			
+	    list ( APPEND SUITESPARSE_LIBRARIES spqr)
+	  ENDIF (SUITESPARSE_SPQR_LIBRARY)
+       endif()
+       
+    ENDIF( SUITESPARSE_LIBRARY_DIR )  
+   
+endif( WIN32 )
+
+
+IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+   IF(WIN32)
+    list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig )
+   ENDIF(WIN32)
+   SET(SUITESPARSE_FOUND TRUE)
+   MESSAGE(STATUS "Found SuiteSparse")
+ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+   SET( SUITESPARSE_FOUND FALSE )
+   MESSAGE(FATAL_ERROR "Unable to find SuiteSparse")
+ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES)
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/distance_calculations.h

@@ -0,0 +1,464 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef DISTANCE_CALCULATIONS_H
+#define DISTANCE_CALCULATIONS_H
+
+#include <Eigen/Core>
+#include "teb_local_planner/misc.h"
+
+
+namespace teb_local_planner
+{
+  
+//! Abbrev. for a container storing 2d points
+typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > Point2dContainer;
+
+  
+/**
+ * @brief Helper function to obtain the closest point on a line segment w.r.t. a reference point
+ * @param point 2D point
+ * @param line_start 2D point representing the start of the line segment
+ * @param line_end 2D point representing the end of the line segment
+ * @return Closest point on the line segment
+ */
+inline Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
+{
+  Eigen::Vector2d diff = line_end - line_start;
+  double sq_norm = diff.squaredNorm();
+  
+  if (sq_norm == 0)
+    return line_start;
+
+  double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y())*diff.y()) / sq_norm;
+  
+  if (u <= 0) return line_start;
+  else if (u >= 1) return line_end;
+  
+  return line_start + u*diff;
+}
+  
+/**
+ * @brief Helper function to calculate the distance between a line segment and a point
+ * @param point 2D point
+ * @param line_start 2D point representing the start of the line segment
+ * @param line_end 2D point representing the end of the line segment
+ * @return minimum distance to a given line segment
+ */
+inline double distance_point_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& point, const Eigen::Ref<const Eigen::Vector2d>& line_start, const Eigen::Ref<const Eigen::Vector2d>& line_end)
+{
+  return  (point - closest_point_on_line_segment_2d(point, line_start, line_end)).norm(); 
+}
+  
+/**
+ * @brief Helper function to check whether two line segments intersects
+ * @param line1_start 2D point representing the start of the first line segment
+ * @param line1_end 2D point representing the end of the first line segment
+ * @param line2_start 2D point representing the start of the second line segment
+ * @param line2_end 2D point representing the end of the second line segment
+ * @param[out] intersection [optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns \c true)
+ * @return \c true if both line segments intersect
+ */  
+inline bool check_line_segments_intersection_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end, 
+                                                const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end, Eigen::Vector2d* intersection = NULL)
+{
+  // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
+  double s_numer, t_numer, denom, t;
+  Eigen::Vector2d line1 = line1_end - line1_start;
+  Eigen::Vector2d line2 = line2_end - line2_start;
+  
+  denom = line1.x() * line2.y() - line2.x() * line1.y();
+  if (denom == 0) return false; // Collinear
+  bool denomPositive = denom > 0;
+
+  Eigen::Vector2d aux = line1_start - line2_start;
+  
+  s_numer = line1.x() * aux.y() - line1.y() * aux.x();
+  if ((s_numer < 0) == denomPositive)  return false; // No collision
+
+  t_numer = line2.x() * aux.y() - line2.y() * aux.x();
+  if ((t_numer < 0) == denomPositive)  return false; // No collision
+
+  if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) return false; // No collision
+  
+  // Otherwise collision detected
+  t = t_numer / denom;
+  if (intersection)
+  {
+    *intersection = line1_start + t * line1;
+  }
+
+  return true;
+}
+  
+  
+/**
+ * @brief Helper function to calculate the smallest distance between two line segments
+ * @param line1_start 2D point representing the start of the first line segment
+ * @param line1_end 2D point representing the end of the first line segment
+ * @param line2_start 2D point representing the start of the second line segment
+ * @param line2_end 2D point representing the end of the second line segment
+ * @return smallest distance between both segments
+*/  
+inline double distance_segment_to_segment_2d(const Eigen::Ref<const Eigen::Vector2d>& line1_start, const Eigen::Ref<const Eigen::Vector2d>& line1_end, 
+                                             const Eigen::Ref<const Eigen::Vector2d>& line2_start, const Eigen::Ref<const Eigen::Vector2d>& line2_end)
+{
+  // TODO more efficient implementation
+  
+  // check if segments intersect
+  if (check_line_segments_intersection_2d(line1_start, line1_end, line2_start, line2_end))
+    return 0;
+  
+  // check all 4 combinations
+  std::array<double,4> distances;
+  
+  distances[0] = distance_point_to_segment_2d(line1_start, line2_start, line2_end);
+  distances[1] = distance_point_to_segment_2d(line1_end, line2_start, line2_end);
+  distances[2] = distance_point_to_segment_2d(line2_start, line1_start, line1_end);
+  distances[3] = distance_point_to_segment_2d(line2_end, line1_start, line1_end);
+  
+  return *std::min_element(distances.begin(), distances.end());
+}
+  
+  
+/**
+ * @brief Helper function to calculate the smallest distance between a point and a closed polygon
+ * @param point 2D point
+ * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
+ * @return smallest distance between point and polygon
+*/    
+inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices)
+{
+  double dist = HUGE_VAL;
+    
+  // the polygon is a point
+  if (vertices.size() == 1)
+  {
+    return (point - vertices.front()).norm();
+  }
+    
+  // check each polygon edge
+  for (int i=0; i<(int)vertices.size()-1; ++i)
+  {
+      double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1));
+//       double new_dist = calc_distance_point_to_segment( position,  vertices.at(i), vertices.at(i+1));
+      if (new_dist < dist)
+        dist = new_dist;
+  }
+
+  if (vertices.size()>2) // if not a line close polygon
+  {
+    double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge
+    if (new_dist < dist)
+      return new_dist;
+  }
+  
+  return dist;
+}  
+
+/**
+ * @brief Helper function to calculate the smallest distance between a line segment and a closed polygon
+ * @param line_start 2D point representing the start of the line segment
+ * @param line_end 2D point representing the end of the line segment
+ * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end)
+ * @return smallest distance between point and polygon
+*/    
+inline double distance_segment_to_polygon_2d(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const Point2dContainer& vertices)
+{
+  double dist = HUGE_VAL;
+    
+  // the polygon is a point
+  if (vertices.size() == 1)
+  {
+    return distance_point_to_segment_2d(vertices.front(), line_start, line_end);
+  }
+    
+  // check each polygon edge
+  for (int i=0; i<(int)vertices.size()-1; ++i)
+  {
+      double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.at(i), vertices.at(i+1));
+//       double new_dist = calc_distance_point_to_segment( position,  vertices.at(i), vertices.at(i+1));
+      if (new_dist < dist)
+        dist = new_dist;
+  }
+
+  if (vertices.size()>2) // if not a line close polygon
+  {
+    double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.back(), vertices.front()); // check last edge
+    if (new_dist < dist)
+      return new_dist;
+  }
+  
+  return dist;
+}
+
+/**
+ * @brief Helper function to calculate the smallest distance between two closed polygons
+ * @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end)
+ * @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end)
+ * @return smallest distance between point and polygon
+*/    
+inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2)
+{
+  double dist = HUGE_VAL;
+    
+  // the polygon1 is a point
+  if (vertices1.size() == 1)
+  {
+    return distance_point_to_polygon_2d(vertices1.front(), vertices2);
+  }
+    
+  // check each edge of polygon1
+  for (int i=0; i<(int)vertices1.size()-1; ++i)
+  {
+      double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2);
+      if (new_dist < dist)
+        dist = new_dist;
+  }
+
+  if (vertices1.size()>2) // if not a line close polygon1
+  {
+    double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge
+    if (new_dist < dist)
+      return new_dist;
+  }
+
+  return dist;
+}
+  
+  
+  
+  
+// Further distance calculations:
+
+
+// The Distance Calculations are mainly copied from http://geomalgorithms.com/a07-_distance.html
+// Copyright 2001 softSurfer, 2012 Dan Sunday
+// This code may be freely used and modified for any purpose
+// providing that this copyright notice is included with it.
+// SoftSurfer makes no warranty for this code, and cannot be held
+// liable for any real or imagined damage resulting from its use.
+// Users of this code must verify correctness for their application.
+
+inline double calc_distance_line_to_line_3d(const Eigen::Ref<const Eigen::Vector3d>& x1, Eigen::Ref<const Eigen::Vector3d>& u,
+              const Eigen::Ref<const Eigen::Vector3d>& x2, Eigen::Ref<const Eigen::Vector3d>& v)
+{
+  Eigen::Vector3d   w = x2 - x1;
+  double a = u.squaredNorm();         // dot(u,u) always >= 0
+  double b = u.dot(v);
+  double c = v.squaredNorm();         // dot(v,v) always >= 0
+  double d = u.dot(w);
+  double e = v.dot(w);
+  double D = a*c - b*b;        // always >= 0
+  double sc, tc;
+
+  // compute the line parameters of the two closest points
+  if (D < SMALL_NUM) {          // the lines are almost parallel
+    sc = 0.0;
+    tc = (b>c ? d/b : e/c);    // use the largest denominator
+  }
+  else {
+    sc = (b*e - c*d) / D;
+    tc = (a*e - b*d) / D;
+  }
+
+  // get the difference of the two closest points
+  Eigen::Vector3d dP = w + (sc * u) - (tc * v);  // =  L1(sc) - L2(tc)
+
+  return dP.norm();   // return the closest distance
+}
+
+
+
+
+
+inline double calc_distance_segment_to_segment3D(const Eigen::Ref<const Eigen::Vector3d>& line1_start, Eigen::Ref<const Eigen::Vector3d>& line1_end,
+             const Eigen::Ref<const Eigen::Vector3d>& line2_start, Eigen::Ref<const Eigen::Vector3d>& line2_end)
+{
+  Eigen::Vector3d u = line1_end - line1_start;
+  Eigen::Vector3d v = line2_end - line2_start;
+  Eigen::Vector3d w = line2_start - line1_start;
+  double a = u.squaredNorm();         // dot(u,u) always >= 0
+  double b = u.dot(v);
+  double c = v.squaredNorm();         // dot(v,v) always >= 0
+  double d = u.dot(w);
+  double e = v.dot(w);
+  double D = a*c - b*b;        // always >= 0
+  double sc, sN, sD = D;       // sc = sN / sD, default sD = D >= 0
+  double tc, tN, tD = D;       // tc = tN / tD, default tD = D >= 0
+
+  // compute the line parameters of the two closest points
+  if (D < SMALL_NUM) 
+  { // the lines are almost parallel
+    sN = 0.0;         // force using point P0 on segment S1
+    sD = 1.0;         // to prevent possible division by 0.0 later
+    tN = e;
+    tD = c;
+  }
+  else 
+  {       // get the closest points on the infinite lines
+    sN = (b*e - c*d);
+    tN = (a*e - b*d);
+    if (sN < 0.0)
+    {        // sc < 0 => the s=0 edge is visible
+      sN = 0.0;
+      tN = e;
+      tD = c;
+    }
+    else if (sN > sD)
+    {  // sc > 1  => the s=1 edge is visible
+      sN = sD;
+      tN = e + b;
+      tD = c;
+    }
+  }
+
+  if (tN < 0.0) 
+  {   // tc < 0 => the t=0 edge is visible
+    tN = 0.0;
+    // recompute sc for this edge
+    if (-d < 0.0)
+      sN = 0.0;
+    else if (-d > a)
+      sN = sD;
+    else 
+    {
+      sN = -d;
+      sD = a;
+    }
+  }
+  else if (tN > tD)
+  {      // tc > 1  => the t=1 edge is visible
+    tN = tD;
+    // recompute sc for this edge
+    if ((-d + b) < 0.0)
+      sN = 0;
+    else if ((-d + b) > a)
+      sN = sD;
+    else
+    {
+      sN = (-d +  b);
+      sD = a;
+    }
+  }
+  // finally do the division to get sc and tc
+  sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
+  tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
+
+  // get the difference of the two closest points
+  Eigen::Vector3d dP = w + (sc * u) - (tc * v);  // =  S1(sc) - S2(tc)
+
+  return dP.norm();   // return the closest distance
+}
+
+
+
+
+template <typename VectorType>
+double calc_closest_point_to_approach_time(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2)
+{
+  VectorType dv = vel1 - vel2;
+
+  double dv2 = dv.squaredNorm(); // dot(v,v)
+  if (dv2 < SMALL_NUM)      // the  tracks are almost parallel
+    return 0.0;             // any time is ok.  Use time 0.
+
+  VectorType w0 = x1 - x2;
+  double cpatime = -w0.dot(dv) / dv2;
+
+  return cpatime;             // time of CPA
+}
+
+template <typename VectorType>
+double calc_closest_point_to_approach_distance(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2, double bound_cpa_time = 0)
+{
+  double ctime = calc_closest_point_to_approach_time<VectorType>(x1, vel1, x2, vel2);
+  if (bound_cpa_time!=0 && ctime > bound_cpa_time) ctime = bound_cpa_time;
+  VectorType P1 = x1 + (ctime * vel1);
+  VectorType P2 = x2 + (ctime * vel2);
+
+  return (P2-P1).norm(); // distance at CPA
+}
+
+
+
+// dist_Point_to_Line(): get the distance of a point to a line
+//     Input:  a Point P and a Line L (in any dimension)
+//     Return: the shortest distance from P to L
+template <typename VectorType>
+double calc_distance_point_to_line( const VectorType& point, const VectorType& line_base, const VectorType& line_dir)
+{
+  VectorType w = point - line_base;
+
+  double c1 = w.dot(line_dir);
+  double c2 = line_dir.dot(line_dir);
+  double b = c1 / c2;
+
+  VectorType Pb = line_base + b * line_dir;
+  return (point-Pb).norm();
+}
+//===================================================================
+
+
+// dist_Point_to_Segment(): get the distance of a point to a segment
+//     Input:  a Point P and a Segment S (in any dimension)
+//     Return: the shortest distance from P to S
+template <typename VectorType>
+double calc_distance_point_to_segment( const VectorType& point, const VectorType& line_start, const VectorType& line_end)
+{
+  VectorType v = line_end - line_start;
+  VectorType w = point - line_start;
+
+  double c1 = w.dot(v);
+  if ( c1 <= 0 )
+    return w.norm();
+
+  double c2 = v.dot(v);
+  if ( c2 <= c1 )
+    return (point-line_end).norm();
+
+  double b = c1 / c2;
+  VectorType Pb = line_start + b * v;
+  return (point-Pb).norm();
+}
+
+  
+  
+} // namespace teb_local_planner
+
+#endif /* DISTANCE_CALCULATIONS_H */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/equivalence_relations.h

@@ -0,0 +1,103 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EQUIVALENCE_RELATIONS_H_
+#define EQUIVALENCE_RELATIONS_H_
+
+#include <memory>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EquivalenceClass
+ * @brief Abstract class that defines an interface for computing and comparing equivalence classes
+ *
+ * Equivalence relations are utilized in order to test if two trajectories are belonging to the same
+ * equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is
+ * the concept of homotopy classes. All trajectories belonging to the same homotopy class
+ * can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely
+ * share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation
+ * is defined by the concept of homology classes (e.g. refer to HSignature).
+ *
+ * Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object.
+ *
+ * @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass.
+ *          Call the "compute"-methods directly on the subclass.
+ */
+class EquivalenceClass
+{
+public:
+
+   /**
+    * @brief Default constructor
+    */
+   EquivalenceClass() {}
+
+   /**
+    * @brief virtual destructor
+    */
+   virtual ~EquivalenceClass() {}
+
+   /**
+    * @brief Check if two candidate classes are equivalent
+    * @param other The other equivalence class to test with
+    */
+   virtual bool isEqual(const EquivalenceClass& other) const = 0;
+
+   /**
+    * @brief Check if the equivalence value is detected correctly
+    * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
+    */
+   virtual bool isValid() const = 0;
+
+   /**
+    * @brief Check if the trajectory is non-looping around an obstacle
+    * @return Returns false, if the trajectory loops around an obstacle
+    */
+   virtual bool isReasonable() const = 0;
+
+};
+
+using EquivalenceClassPtr = std::shared_ptr<EquivalenceClass>;
+
+
+} // namespace teb_local_planner
+
+
+#endif /* EQUIVALENCE_RELATIONS_H_ */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h

@@ -0,0 +1,278 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef _BASE_TEB_EDGES_H_
+#define _BASE_TEB_EDGES_H_
+
+#include "teb_local_planner/teb_config.h"
+
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/core/base_multi_edge.h>
+
+namespace teb_local_planner
+{
+    
+    
+/**
+ * @class BaseTebUnaryEdge
+ * @brief Base edge connecting a single vertex in the TEB optimization problem
+ * 
+ * This edge defines a base edge type for the TEB optimization problem.
+ * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
+ * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
+ * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
+ * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
+ */   
+template <int D, typename E, typename VertexXi>
+class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
+{
+public:
+            
+  using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
+  using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
+    
+  /**
+  * @brief Compute and return error / cost value.
+  * 
+  * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
+  * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
+  */     
+  ErrorVector& getError()
+  {
+    computeError();
+    return _error;
+  }
+  
+  /**
+   * @brief Read values from input stream
+   */  	
+  virtual bool read(std::istream& is)
+  {
+    // TODO generic read
+    return true;
+  }
+
+  /**
+   * @brief Write values to an output stream
+   */    
+  virtual bool write(std::ostream& os) const
+  {
+    // TODO generic write
+    return os.good();
+  }
+
+  /**
+   * @brief Assign the TebConfig class for parameters.
+   * @param cfg TebConfig class
+   */ 
+  void setTebConfig(const TebConfig& cfg)
+  {
+    cfg_ = &cfg;
+  }
+    
+protected:
+    
+  using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
+  using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
+  
+  const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
+  
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
+};
+
+/**
+ * @class BaseTebBinaryEdge
+ * @brief Base edge connecting two vertices in the TEB optimization problem
+ * 
+ * This edge defines a base edge type for the TEB optimization problem.
+ * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
+ * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
+ * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
+ * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
+ */    
+template <int D, typename E, typename VertexXi, typename VertexXj>
+class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
+{
+public:
+    
+  using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
+  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
+
+  /**
+  * @brief Compute and return error / cost value.
+  * 
+  * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
+  * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
+  */     
+  ErrorVector& getError()
+  {
+    computeError();
+    return _error;
+  }
+  
+  /**
+   * @brief Read values from input stream
+   */  	
+  virtual bool read(std::istream& is)
+  {
+    // TODO generic read
+    return true;
+  }
+
+  /**
+   * @brief Write values to an output stream
+   */    
+  virtual bool write(std::ostream& os) const
+  {
+    // TODO generic write
+    return os.good();
+  }
+
+  /**
+   * @brief Assign the TebConfig class for parameters.
+   * @param cfg TebConfig class
+   */ 
+  void setTebConfig(const TebConfig& cfg)
+  {
+    cfg_ = &cfg;
+  }
+  
+protected:
+  
+  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
+  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
+    
+  const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
+  
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
+};
+
+
+/**
+ * @class BaseTebMultiEdge
+ * @brief Base edge connecting multiple vertices in the TEB optimization problem
+ * 
+ * This edge defines a base edge type for the TEB optimization problem.
+ * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config).
+ * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls.
+ * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class.
+ * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge
+ */    
+template <int D, typename E>
+class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
+{
+public:
+  
+  using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
+  using g2o::BaseMultiEdge<D, E>::computeError;
+    
+  // Overwrites resize() from the parent class
+  virtual void resize(size_t size)
+  {
+      g2o::BaseMultiEdge<D, E>::resize(size);
+      
+      for(std::size_t i=0; i<_vertices.size(); ++i)
+        _vertices[i] = NULL;
+  }
+
+  /**
+  * @brief Compute and return error / cost value.
+  * 
+  * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost.
+  * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T
+  */     
+  ErrorVector& getError()
+  {
+    computeError();
+    return _error;
+  }
+  
+  /**
+   * @brief Read values from input stream
+   */  	
+  virtual bool read(std::istream& is)
+  {
+    // TODO generic read
+    return true;
+  }
+
+  /**
+   * @brief Write values to an output stream
+   */    
+  virtual bool write(std::ostream& os) const
+  {
+    // TODO generic write
+    return os.good();
+  }
+
+  /**
+   * @brief Assign the TebConfig class for parameters.
+   * @param cfg TebConfig class
+   */ 
+  void setTebConfig(const TebConfig& cfg)
+  {
+    cfg_ = &cfg;
+  }
+  
+protected:
+    
+  using g2o::BaseMultiEdge<D, E>::_error;
+  using g2o::BaseMultiEdge<D, E>::_vertices;
+  
+  const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters
+  
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
+};
+
+
+
+
+
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h

@@ -0,0 +1,732 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_ACCELERATION_H_
+#define EDGE_ACCELERATION_H_
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/misc.h"
+
+#include <geometry_msgs/msg/twist.hpp>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeAcceleration
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration.
+ * 
+ * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
+ * \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n
+ * \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n 
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
+ * the second one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationStart
+ * @see EdgeAccelerationGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values!
+ */    
+class EdgeAcceleration : public BaseTebMultiEdge<2, double>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */ 
+  EdgeAcceleration()
+  {
+    this->resize(5);
+  }
+    
+  /**
+   * @brief Actual cost function
+   */   
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
+    const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
+    const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+    const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+
+    // VELOCITY & ACCELERATION
+    const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
+    const Eigen::Vector2d diff2 = pose3->position() - pose2->position();
+        
+    double dist1 = diff1.norm();
+    double dist2 = diff2.norm();
+    const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta());
+    const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta());
+    
+    if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation
+    {
+        if (angle_diff1 != 0)
+        {
+            const double radius =  dist1/(2*sin(angle_diff1/2));
+            dist1 = fabs( angle_diff1 * radius ); // actual arg length!
+        }
+        if (angle_diff2 != 0)
+        {
+            const double radius =  dist2/(2*sin(angle_diff2/2));
+            dist2 = fabs( angle_diff2 * radius ); // actual arg length!
+        }
+    }
+    
+    double vel1 = dist1 / dt1->dt();
+    double vel2 = dist2 / dt2->dt();
+    
+    
+    // consider directions
+//     vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); 
+//     vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); 
+    vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); 
+    vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); 
+    
+    const double acc_lin  = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
+   
+
+    _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+    
+    // ANGULAR ACCELERATION
+    const double omega1 = angle_diff1 / dt1->dt();
+    const double omega2 = angle_diff2 / dt2->dt();
+    const double acc_rot  = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
+      
+    _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
+    TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
+  }
+
+
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 0
+  /*
+   * @brief Jacobi matrix of the cost function specified in computeError().
+   */
+  void linearizeOplus()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
+    const VertexPointXY* conf1 = static_cast<const VertexPointXY*>(_vertices[0]);
+    const VertexPointXY* conf2 = static_cast<const VertexPointXY*>(_vertices[1]);
+    const VertexPointXY* conf3 = static_cast<const VertexPointXY*>(_vertices[2]);
+    const VertexTimeDiff* deltaT1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+    const VertexTimeDiff* deltaT2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+    const VertexOrientation* angle1 = static_cast<const VertexOrientation*>(_vertices[5]);
+    const VertexOrientation* angle2 = static_cast<const VertexOrientation*>(_vertices[6]);
+    const VertexOrientation* angle3 = static_cast<const VertexOrientation*>(_vertices[7]);
+
+    Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate();
+    Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate();
+    double dist1 = deltaS1.norm();
+    double dist2 = deltaS2.norm();
+    
+    double sum_time = deltaT1->estimate() + deltaT2->estimate();
+    double sum_time_inv = 1 / sum_time;
+    double dt1_inv = 1/deltaT1->estimate();
+    double dt2_inv = 1/deltaT2->estimate();
+    double aux0 = 2/sum_time_inv;
+    double aux1 = dist1 * deltaT1->estimate();
+    double aux2 = dist2 * deltaT2->estimate();
+
+    double vel1 = dist1 * dt1_inv;
+    double vel2 = dist2 * dt2_inv;
+    double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv;
+    double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv;
+    double acc = (vel2 - vel1) * aux0;
+    double omegadot = (omega2 - omega1) * aux0;
+    double aux3 = -acc/2;
+    double aux4 = -omegadot/2;
+    
+    double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
+    double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
+    
+    _jacobianOplus[0].resize(2,2); // conf1
+    _jacobianOplus[1].resize(2,2); // conf2
+    _jacobianOplus[2].resize(2,2); // conf3
+    _jacobianOplus[3].resize(2,1); // deltaT1
+    _jacobianOplus[4].resize(2,1); // deltaT2
+    _jacobianOplus[5].resize(2,1); // angle1
+    _jacobianOplus[6].resize(2,1); // angle2
+    _jacobianOplus[7].resize(2,1); // angle3
+    
+    if (aux1==0) aux1=1e-20;
+    if (aux2==0) aux2=1e-20;
+  
+    if (dev_border_acc!=0)
+    {
+      // TODO: double aux = aux0 * dev_border_acc;
+      // double aux123 = aux / aux1;
+      _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1
+      _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1
+      _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2
+      _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2
+      _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3
+      _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3	
+      _jacobianOplus[2](0,0) = 0;
+      _jacobianOplus[2](0,1) = 0;
+      _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1
+      _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2
+    }
+    else
+    {
+      _jacobianOplus[0](0,0) = 0; // acc x1
+      _jacobianOplus[0](0,1) = 0; // acc y1	
+      _jacobianOplus[1](0,0) = 0; // acc x2
+      _jacobianOplus[1](0,1) = 0; // acc y2
+      _jacobianOplus[2](0,0) = 0; // acc x3
+      _jacobianOplus[2](0,1) = 0; // acc y3	
+      _jacobianOplus[3](0,0) = 0; // acc deltaT1
+      _jacobianOplus[4](0,0) = 0; // acc deltaT2
+    }
+    
+    if (dev_border_omegadot!=0)
+    {
+      _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1
+      _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2
+      _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1
+      _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2
+      _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3
+    }
+    else
+    {
+      _jacobianOplus[3](1,0) = 0; // omegadot deltaT1
+      _jacobianOplus[4](1,0) = 0; // omegadot deltaT2
+      _jacobianOplus[5](1,0) = 0; // omegadot angle1
+      _jacobianOplus[6](1,0) = 0; // omegadot angle2
+      _jacobianOplus[7](1,0) = 0; // omegadot angle3			
+    }
+
+    _jacobianOplus[0](1,0) = 0; // omegadot x1
+    _jacobianOplus[0](1,1) = 0; // omegadot y1
+    _jacobianOplus[1](1,0) = 0; // omegadot x2
+    _jacobianOplus[1](1,1) = 0; // omegadot y2
+    _jacobianOplus[2](1,0) = 0; // omegadot x3
+    _jacobianOplus[2](1,1) = 0; // omegadot y3
+    _jacobianOplus[5](0,0) = 0; // acc angle1
+    _jacobianOplus[6](0,0) = 0; // acc angle2
+    _jacobianOplus[7](0,0) = 0; // acc angle3
+    }
+#endif
+#endif
+
+      
+public: 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+   
+};
+    
+    
+/**
+ * @class EdgeAccelerationStart
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
+ * \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi].  \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
+ * the second one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAcceleration
+ * @see EdgeAccelerationGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory!
+ */      
+class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */	  
+  EdgeAccelerationStart()
+  {
+    _measurement = NULL;
+    this->resize(3);
+  }
+  
+  
+  /**
+   * @brief Actual cost function
+   */   
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
+    const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    // VELOCITY & ACCELERATION
+    const Eigen::Vector2d diff = pose2->position() - pose1->position();
+    double dist = diff.norm();
+    const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta());
+    if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+    {
+        const double radius =  dist/(2*sin(angle_diff/2));
+        dist = fabs( angle_diff * radius ); // actual arg length!
+    }
+    
+    const double vel1 = _measurement->linear.x;
+    double vel2 = dist / dt->dt();
+
+    // consider directions
+    //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta())); 
+    vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) ); 
+    
+    const double acc_lin  = (vel2 - vel1) / dt->dt();
+    
+    _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+    
+    // ANGULAR ACCELERATION
+    const double omega1 = _measurement->angular.z;
+    const double omega2 = angle_diff / dt->dt();
+    const double acc_rot  = (omega2 - omega1) / dt->dt();
+      
+    _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
+    TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
+  }
+  
+  /**
+   * @brief Set the initial velocity that is taken into account for calculating the acceleration
+   * @param vel_start twist message containing the translational and rotational velocity
+   */    
+  void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start)
+  {
+    _measurement = &vel_start;
+  }
+  
+public:       
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};    
+    
+    
+    
+
+/**
+ * @class EdgeAccelerationGoal
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
+ * 
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n
+ * \e a is calculated using the difference quotient (twice) and the position parts of the poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi].  \n
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and
+ * the second one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAcceleration
+ * @see EdgeAccelerationStart
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory
+ */  
+class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */  
+  EdgeAccelerationGoal()
+  {
+    _measurement = NULL;
+    this->resize(3);
+  }
+  
+
+  /**
+   * @brief Actual cost function
+   */ 
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
+    const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    // VELOCITY & ACCELERATION
+
+    const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();  
+    double dist = diff.norm();
+    const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta());
+    if (cfg_->trajectory.exact_arc_length  && angle_diff != 0)
+    {
+        double radius =  dist/(2*sin(angle_diff/2));
+        dist = fabs( angle_diff * radius ); // actual arg length!
+    }
+    
+    double vel1 = dist / dt->dt();
+    const double vel2 = _measurement->linear.x;
+    
+    // consider directions
+    //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta())); 
+    vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) ); 
+    
+    const double acc_lin  = (vel2 - vel1) / dt->dt();
+
+    _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+    
+    // ANGULAR ACCELERATION
+    const double omega1 = angle_diff / dt->dt();
+    const double omega2 = _measurement->angular.z;
+    const double acc_rot  = (omega2 - omega1) / dt->dt();
+      
+    _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
+    TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
+  }
+    
+  /**
+   * @brief Set the goal / final velocity that is taken into account for calculating the acceleration
+   * @param vel_goal twist message containing the translational and rotational velocity
+   */    
+  void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal)
+  {
+    _measurement = &vel_goal;
+  }
+  
+public: 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+}; 
+    
+
+
+
+/**
+ * @class EdgeAccelerationHolonomic
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration.
+ * 
+ * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes:
+ * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n
+ * \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n
+ * \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n 
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir), 
+ * the second one the strafing acceleration and the third one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationHolonomicStart
+ * @see EdgeAccelerationHolonomicGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values!
+ */    
+class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */    
+  EdgeAccelerationHolonomic()
+  {
+    this->resize(5);
+  }
+    
+  /**
+   * @brief Actual cost function
+   */   
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
+    const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
+    const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+    const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+
+    // VELOCITY & ACCELERATION
+    Eigen::Vector2d diff1 = pose2->position() - pose1->position();
+    Eigen::Vector2d diff2 = pose3->position() - pose2->position();
+    
+    double cos_theta1 = std::cos(pose1->theta());
+    double sin_theta1 = std::sin(pose1->theta()); 
+    double cos_theta2 = std::cos(pose2->theta());
+    double sin_theta2 = std::sin(pose2->theta()); 
+    
+    // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
+    double p1_dx =  cos_theta1*diff1.x() + sin_theta1*diff1.y();
+    double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y();
+    // transform pose3 into robot frame pose2 (inverse 2d rotation matrix)
+    double p2_dx =  cos_theta2*diff2.x() + sin_theta2*diff2.y();
+    double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y();
+    
+    double vel1_x = p1_dx / dt1->dt();
+    double vel1_y = p1_dy / dt1->dt();
+    double vel2_x = p2_dx / dt2->dt();
+    double vel2_y = p2_dy / dt2->dt();
+    
+    double dt12 = dt1->dt() + dt2->dt();
+    
+    double acc_x  = (vel2_x - vel1_x)*2 / dt12;
+    double acc_y  = (vel2_y - vel1_y)*2 / dt12;
+   
+    _error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
+    
+    // ANGULAR ACCELERATION
+    double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
+    double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
+    double acc_rot  = (omega2 - omega1)*2 / dt12;
+      
+    _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
+    TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]);
+    TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]);
+  }
+
+public: 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+   
+};
+
+
+/**
+ * @class EdgeAccelerationHolonomicStart
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory.
+ *
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
+ * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n
+ * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi].  \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
+ * the second one the strafing acceleration and the third one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationHolonomic
+ * @see EdgeAccelerationHolonomicGoal
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory!
+ */      
+class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */   
+  EdgeAccelerationHolonomicStart()
+  {
+    this->resize(3);
+    _measurement = NULL;
+  }
+    
+  /**
+   * @brief Actual cost function
+   */   
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
+    const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    // VELOCITY & ACCELERATION
+    Eigen::Vector2d diff = pose2->position() - pose1->position();
+            
+    double cos_theta1 = std::cos(pose1->theta());
+    double sin_theta1 = std::sin(pose1->theta()); 
+    
+    // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
+    double p1_dx =  cos_theta1*diff.x() + sin_theta1*diff.y();
+    double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
+    
+    double vel1_x = _measurement->linear.x;
+    double vel1_y = _measurement->linear.y;
+    double vel2_x = p1_dx / dt->dt();
+    double vel2_y = p1_dy / dt->dt();
+
+    double acc_lin_x  = (vel2_x - vel1_x) / dt->dt();
+    double acc_lin_y  = (vel2_y - vel1_y) / dt->dt();
+    
+    _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
+    
+    // ANGULAR ACCELERATION
+    double omega1 = _measurement->angular.z;
+    double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
+    double acc_rot  = (omega2 - omega1) / dt->dt();
+      
+    _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
+    TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
+    TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]);
+  }
+  
+  /**
+   * @brief Set the initial velocity that is taken into account for calculating the acceleration
+   * @param vel_start twist message containing the translational and rotational velocity
+   */    
+  void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start)
+  {
+    _measurement = &vel_start;
+  }
+        
+public:       
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};    
+    
+       
+
+/**
+ * @class EdgeAccelerationHolonomicGoal
+ * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory.
+ * 
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity()
+ * and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n
+ * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n
+ * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n
+ * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi].  \n
+ * \e weight can be set using setInformation() \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational acceleration,
+ * the second one is the strafing velocity and the third one the rotational acceleration.
+ * @see TebOptimalPlanner::AddEdgesAcceleration
+ * @see EdgeAccelerationHolonomic
+ * @see EdgeAccelerationHolonomicStart
+ * @remarks Do not forget to call setTebConfig()
+ * @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory
+ */  
+class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */  
+  EdgeAccelerationHolonomicGoal()
+  {
+    _measurement = NULL;
+    this->resize(3);
+  }
+  
+  /**
+   * @brief Actual cost function
+   */ 
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
+    const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    // VELOCITY & ACCELERATION
+
+    Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();    
+    
+    double cos_theta1 = std::cos(pose_pre_goal->theta());
+    double sin_theta1 = std::sin(pose_pre_goal->theta()); 
+    
+    // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
+    double p1_dx =  cos_theta1*diff.x() + sin_theta1*diff.y();
+    double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
+   
+    double vel1_x = p1_dx / dt->dt();
+    double vel1_y = p1_dy / dt->dt();
+    double vel2_x = _measurement->linear.x;
+    double vel2_y = _measurement->linear.y;
+    
+    double acc_lin_x  = (vel2_x - vel1_x) / dt->dt();
+    double acc_lin_y  = (vel2_y - vel1_y) / dt->dt();
+
+    _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon);
+    
+    // ANGULAR ACCELERATION
+    double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
+    double omega2 = _measurement->angular.z;
+    double acc_rot  = (omega2 - omega1) / dt->dt();
+      
+    _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
+    TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);
+    TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]);
+  }
+  
+  
+  /**
+   * @brief Set the goal / final velocity that is taken into account for calculating the acceleration
+   * @param vel_goal twist message containing the translational and rotational velocity
+   */    
+  void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal)
+  {
+    _measurement = &vel_goal;
+  }
+  
+
+public: 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+}; 
+    
+
+
+
+}; // end namespace
+
+#endif /* EDGE_ACCELERATION_H_ */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h

@@ -0,0 +1,154 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#ifndef EDGE_DYNAMICOBSTACLE_H
+#define EDGE_DYNAMICOBSTACLE_H
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/robot_footprint_model.h"
+#include "teb_local_planner/misc.h"
+
+namespace teb_local_planner
+{
+  
+/**
+ * @class EdgeDynamicObstacle
+ * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles.
+ * 
+ * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n
+ * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n
+ * @see TebOptimalPlanner::AddEdgesDynamicObstacles
+ * @remarks Do not forget to call setTebConfig(), setVertexIdx() and 
+ * @warning Experimental
+ */  
+class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
+{
+public:
+  
+  /**
+   * @brief Construct edge.
+   */    
+  EdgeDynamicObstacle() : t_(0)
+  {
+  }
+  
+  /**
+   * @brief Construct edge and specify the time for its associated pose (neccessary for computeError).
+   * @param t_ Estimated time until current pose is reached
+   */      
+  EdgeDynamicObstacle(double t) : t_(t)
+  {
+  }
+  
+  /**
+   * @brief Actual cost function
+   */   
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()");
+    const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+    
+    double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
+
+    _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]);
+  }
+  
+  
+  /**
+   * @brief Set Obstacle for the underlying cost function
+   * @param obstacle Const pointer to an Obstacle or derived Obstacle
+   */     
+  void setObstacle(const Obstacle* obstacle)
+  {
+    _measurement = obstacle;
+  }
+  
+  /**
+   * @brief Set pointer to the robot model
+   * @param robot_model Robot model required for distance calculation
+   */
+  void setRobotModel(const BaseRobotFootprintModel* robot_model)
+  {
+    robot_model_ = robot_model;
+  }
+
+  /**
+   * @brief Set all parameters at once
+   * @param cfg TebConfig class
+   * @param robot_model Robot model required for distance calculation
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */
+  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  {
+    cfg_ = &cfg;
+    robot_model_ = robot_model;
+    _measurement = obstacle;
+  }
+
+protected:
+  
+  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+  double t_; //!< Estimated time until current pose is reached
+  
+public: 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+    
+ 
+    
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h

@@ -0,0 +1,231 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef _EDGE_KINEMATICS_H
+#define _EDGE_KINEMATICS_H
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+#include <cmath>
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeKinematicsDiffDrive
+ * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot.
+ * 
+ * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
+ * of the non-holonomic constraint: 
+ * 	- C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
+ * 
+ * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
+ * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n
+ * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n
+ * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost, 
+ * the second one backward-drive cost.
+ * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike
+ * @remarks Do not forget to call setTebConfig()
+ */    
+class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
+{
+public:
+  
+  /**
+   * @brief Construct edge.
+   */  
+  EdgeKinematicsDiffDrive()
+  {
+      this->setMeasurement(0.);
+  }
+  
+  /**
+   * @brief Actual cost function
+   */    
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    
+    Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+
+    // non holonomic constraint
+    _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+
+    // positive-drive-direction constraint
+    Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );	   
+    _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
+    // epsilon=0, otherwise it pushes the first bandpoints away from start
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
+  }
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 1
+  /**
+   * @brief Jacobi matrix of the cost function specified in computeError().
+   */
+  void linearizeOplus()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    
+    Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+	    
+    double cos1 = cos(conf1->theta());
+    double cos2 = cos(conf2->theta());
+    double sin1 = sin(conf1->theta());
+    double sin2 = sin(conf2->theta());
+    double aux1 = sin1 + sin2;
+    double aux2 = cos1 + cos2;
+    
+    double dd_error_1 = deltaS[0]*cos1;
+    double dd_error_2 = deltaS[1]*sin1;
+    double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
+    
+    double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - 
+	      ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+	    
+    // conf1
+    _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1
+    _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1
+    _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1
+    _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1
+    _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle
+    _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1
+    
+    // conf2
+    _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2
+    _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2
+    _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2
+    _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2
+    _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle
+    _jacobianOplusXj(1,2) = 0; // drive-dir angle1					
+  }
+#endif
+#endif
+      
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
+};
+
+
+
+
+/**
+ * @class EdgeKinematicsCarlike
+ * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot.
+ * 
+ * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation
+ * of the non-holonomic constraint: 
+ *  - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012.
+ * 
+ * The definition is identically to the one of the differential drive robot.
+ * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots.
+ * The turning radius is defined by \f$ r=v/omega \f$.
+ * 
+ * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n
+ * The second equation enforces a minimum turning radius.
+ * The \e weight can be set using setInformation(): Matrix element 2,2. \n
+ * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost, 
+ * the second one backward-drive cost and the third one the minimum turning radius
+ * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive
+ * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter, 
+ *          the user might add an extra margin to the min_turning_radius param.
+ * @remarks Do not forget to call setTebConfig()
+ */    
+class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
+{
+public:
+  
+  /**
+   * @brief Construct edge.
+   */  
+  EdgeKinematicsCarlike()
+  {
+      this->setMeasurement(0.);
+  }
+  
+  /**
+   * @brief Actual cost function
+   */    
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    
+    Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+
+    // non holonomic constraint
+    _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
+
+    // limit minimum turning radius
+    double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
+    if (angle_diff == 0)
+      _error[1] = 0; // straight line motion
+    else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius
+      _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0);
+    else
+      _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0); 
+    // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter.
+    
+    TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
+  }
+  
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
+};
+
+
+
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h

@@ -0,0 +1,295 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+#ifndef EDGE_OBSTACLE_H_
+#define EDGE_OBSTACLE_H_
+
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/robot_footprint_model.h"
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeObstacle
+ * @brief Edge defining the cost function for keeping a minimum distance from obstacles.
+ * 
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
+ * \e dist2point denotes the minimum distance to the point obstacle. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
+ * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
+ * @remarks Do not forget to call setTebConfig() and setObstacle()
+ */     
+class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose>
+{
+public:
+    
+  /**
+   * @brief Construct edge.
+   */    
+  EdgeObstacle() 
+  {
+    _measurement = NULL;
+  }
+ 
+  /**
+   * @brief Actual cost function
+   */    
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()");
+    const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+    double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
+
+    // Original obstacle cost.
+    _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
+
+    if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
+    {
+      // Optional non-linear cost. Note the max cost (before weighting) is
+      // the same as the straight line version and that all other costs are
+      // below the straight line (for positive exponent), so it may be
+      // necessary to increase weight_obstacle and/or the inflation_weight
+      // when using larger exponents.
+      _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
+    }
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]);
+  }
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 0
+
+  /**
+   * @brief Jacobi matrix of the cost function specified in computeError().
+   */
+  void linearizeOplus()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()");
+    const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+    
+    Eigen::Vector2d deltaS = *_measurement - bandpt->position(); 
+    double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta();
+    
+    double dist_squared = deltaS.squaredNorm();
+    double dist = sqrt(dist_squared);
+    
+    double aux0 = sin(angdiff);
+    double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon);
+
+    if (dev_left_border==0)
+    {
+      _jacobianOplusXi( 0 , 0 ) = 0;
+      _jacobianOplusXi( 0 , 1 ) = 0;
+      _jacobianOplusXi( 0 , 2 ) = 0;
+      return;
+    }
+    
+    double aux1 = -fabs(aux0) / dist;
+    double dev_norm_x = deltaS[0]*aux1;
+    double dev_norm_y = deltaS[1]*aux1;
+    
+    double aux2 = cos(angdiff) * g2o::sign(aux0);
+    double aux3 = aux2 / dist_squared;
+    double dev_proj_x = aux3 * deltaS[1] * dist;
+    double dev_proj_y = -aux3 * deltaS[0] * dist;
+    double dev_proj_angle = -aux2;
+    
+    _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x );
+    _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y );
+    _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle;
+  }
+#endif
+#endif
+  
+  /**
+   * @brief Set pointer to associated obstacle for the underlying cost function 
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */ 
+  void setObstacle(const Obstacle* obstacle)
+  {
+    _measurement = obstacle;
+  }
+    
+  /**
+   * @brief Set pointer to the robot model 
+   * @param robot_model Robot model required for distance calculation
+   */ 
+  void setRobotModel(const BaseRobotFootprintModel* robot_model)
+  {
+    robot_model_ = robot_model;
+  }
+    
+  /**
+   * @brief Set all parameters at once
+   * @param cfg TebConfig class
+   * @param robot_model Robot model required for distance calculation
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */ 
+  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  {
+    cfg_ = &cfg;
+    robot_model_ = robot_model;
+    _measurement = obstacle;
+  }
+  
+protected:
+
+  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+  
+public: 	
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+  
+
+
+/**
+ * @class EdgeInflatedObstacle
+ * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles.
+ * 
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n
+ * Additional, a second penalty is provided with \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$.
+ * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation.
+ * \e dist2point denotes the minimum distance to the point obstacle. \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
+ * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle
+ * @remarks Do not forget to call setTebConfig() and setObstacle()
+ */     
+class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
+{
+public:
+    
+  /**
+   * @brief Construct edge.
+   */    
+  EdgeInflatedObstacle() 
+  {
+    _measurement = NULL;
+  }
+ 
+  /**
+   * @brief Actual cost function
+   */    
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeInflatedObstacle()");
+    const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+    double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement);
+
+    // Original "straight line" obstacle cost. The max possible value
+    // before weighting is min_obstacle_dist
+    _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon);
+
+    if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0)
+    {
+      // Optional non-linear cost. Note the max cost (before weighting) is
+      // the same as the straight line version and that all other costs are
+      // below the straight line (for positive exponent), so it may be
+      // necessary to increase weight_obstacle and/or the inflation_weight
+      // when using larger exponents.
+      _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent);
+    }
+
+    // Additional linear inflation cost
+    _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0);
+
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]);
+  }
+
+  /**
+   * @brief Set pointer to associated obstacle for the underlying cost function 
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */ 
+  void setObstacle(const Obstacle* obstacle)
+  {
+    _measurement = obstacle;
+  }
+    
+  /**
+   * @brief Set pointer to the robot model 
+   * @param robot_model Robot model required for distance calculation
+   */ 
+  void setRobotModel(const BaseRobotFootprintModel* robot_model)
+  {
+    robot_model_ = robot_model;
+  }
+    
+  /**
+   * @brief Set all parameters at once
+   * @param cfg TebConfig class
+   * @param robot_model Robot model required for distance calculation
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */ 
+  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  {
+    cfg_ = &cfg;
+    robot_model_ = robot_model;
+    _measurement = obstacle;
+  }
+  
+protected:
+
+  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+  
+public:         
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+    
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h

@@ -0,0 +1,115 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+#ifndef EDGE_PREFER_ROTDIR_H_
+#define EDGE_PREFER_ROTDIR_H_
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "g2o/core/base_unary_edge.h"
+#include "teb_local_planner/misc.h"
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgePreferRotDir
+ * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns
+ * 
+ * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction
+ * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$)
+ * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations  \n
+ * \e weight can be set using setInformation(). \n
+ * @see TebOptimalPlanner::AddEdgePreferRotDir
+ */     
+class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose>
+{
+public:
+    
+  /**
+   * @brief Construct edge.
+   */    
+  EdgePreferRotDir() 
+  {
+    _measurement = 1;
+  }
+ 
+  /**
+   * @brief Actual cost function
+   */    
+  void computeError()
+  {
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    
+    _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]);
+  }
+
+  /**
+   * @brief Specify the prefered direction of rotation
+   * @param dir +1 to prefer the left side, -1 to prefer the right side
+   */ 
+  void setRotDir(double dir)
+  {
+    _measurement = dir;
+  }
+  
+  /** Prefer rotations to the right */
+  void preferRight() {_measurement = -1;}
+    
+  /** Prefer rotations to the right */
+  void preferLeft() {_measurement = 1;}  
+    
+  
+public: 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+  
+    
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h

@@ -0,0 +1,88 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_SHORTEST_PATH_H_
+#define EDGE_SHORTEST_PATH_H_
+
+#include <float.h>
+
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/misc.h"
+
+#include <Eigen/Core>
+
+namespace teb_local_planner {
+
+/**
+ * @class EdgeShortestPath
+ * @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses.
+ *
+ * @see TebOptimalPlanner::AddEdgesShortestPath
+ */
+class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> {
+public:
+  /**
+   * @brief Construct edge.
+   */
+  EdgeShortestPath() { this->setMeasurement(0.); }
+
+  /**
+   * @brief Actual cost function
+   */
+  void computeError() {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()");
+    const VertexPose *pose1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose *pose2 = static_cast<const VertexPose*>(_vertices[1]);
+    _error[0] = (pose2->position() - pose1->position()).norm();
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]);
+  }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // end namespace
+
+#endif /* EDGE_SHORTEST_PATH_H_ */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h

@@ -0,0 +1,117 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_TIMEOPTIMAL_H_
+#define EDGE_TIMEOPTIMAL_H_
+
+#include <float.h>
+
+//#include <base_local_planner/BaseLocalPlannerConfig.h>
+
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+#include <Eigen/Core>
+
+namespace teb_local_planner
+{
+
+  
+/**
+ * @class EdgeTimeOptimal
+ * @brief Edge defining the cost function for minimizing transition time of the trajectory.
+ * 
+ * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n
+ * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n
+ * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n
+ * @see TebOptimalPlanner::AddEdgesTimeOptimal
+ * @remarks Do not forget to call setTebConfig()
+ */
+class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff>
+{
+public:
+    
+  /**
+   * @brief Construct edge.
+   */
+  EdgeTimeOptimal()
+  {
+    this->setMeasurement(0.);
+  }
+  
+  /**
+   * @brief Actual cost function
+   */
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
+    const VertexTimeDiff* timediff = static_cast<const VertexTimeDiff*>(_vertices[0]);
+
+   _error[0] = timediff->dt();
+  
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]);
+  }
+
+#ifdef USE_ANALYTIC_JACOBI
+  /**
+   * @brief Jacobi matrix of the cost function specified in computeError().
+   */
+  void linearizeOplus()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
+    _jacobianOplusXi( 0 , 0 ) = 1;
+  }
+#endif
+  
+  
+public:        
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}; // end namespace
+
+#endif /* EDGE_TIMEOPTIMAL_H_ */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h

@@ -0,0 +1,275 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef EDGE_VELOCITY_H
+#define EDGE_VELOCITY_H
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/vertex_timediff.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/g2o_types/penalties.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/misc.h"
+
+#include <rclcpp/logging.hpp>
+
+#include <exception>
+
+namespace teb_local_planner
+{
+
+  
+/**
+ * @class EdgeVelocity
+ * @brief Edge defining the cost function for limiting the translational and rotational velocity.
+ * 
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n
+ * \e v is calculated using the difference quotient and the position parts of both poses. \n
+ * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 2: the first component represents the translational velocity and
+ * the second one the rotational velocity.
+ * @see TebOptimalPlanner::AddEdgesVelocity
+ * @remarks Do not forget to call setTebConfig()
+ */  
+class EdgeVelocity : public BaseTebMultiEdge<2, double>
+{
+public:
+  
+  /**
+   * @brief Construct edge.
+   */	      
+  EdgeVelocity()
+  {
+    this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+  }
+  
+  /**
+   * @brief Actual cost function
+   */  
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+    
+    const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
+    
+    double dist = deltaS.norm();
+    const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
+    if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+    {
+        double radius =  dist/(2*sin(angle_diff/2));
+        dist = fabs( angle_diff * radius ); // actual arg length!
+    }
+    double vel = dist / deltaT->estimate();
+    
+//     vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
+    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+    
+    const double omega = angle_diff / deltaT->estimate();
+  
+    _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
+  }
+
+#ifdef USE_ANALYTIC_JACOBI
+#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value)
+      // Change accordingly...
+
+  /**
+   * @brief Jacobi matrix of the cost function specified in computeError().
+   */
+  void linearizeOplus()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+    
+    Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+    double dist = deltaS.norm();
+    double aux1 = dist*deltaT->estimate();
+    double aux2 = 1/deltaT->estimate();
+    
+    double vel = dist * aux2;
+    double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2;
+    
+    double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
+    double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+    
+    _jacobianOplus[0].resize(2,3); // conf1
+    _jacobianOplus[1].resize(2,3); // conf2
+    _jacobianOplus[2].resize(2,1); // deltaT
+    
+//  if (aux1==0) aux1=1e-6;
+//  if (aux2==0) aux2=1e-6;
+    
+    if (dev_border_vel!=0)
+    {
+      double aux3 = dev_border_vel / aux1;
+      _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1
+      _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1	
+      _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2
+      _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2
+      _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT
+    }
+    else
+    {
+      _jacobianOplus[0](0,0) = 0; // vel x1
+      _jacobianOplus[0](0,1) = 0; // vel y1	
+      _jacobianOplus[1](0,0) = 0; // vel x2
+      _jacobianOplus[1](0,1) = 0; // vel y2	
+      _jacobianOplus[2](0,0) = 0; // vel deltaT
+    }
+    
+    if (dev_border_omega!=0)
+    {
+      double aux4 = aux2 * dev_border_omega;
+      _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT
+      _jacobianOplus[0](1,2) = -aux4; // omega angle1
+      _jacobianOplus[1](1,2) = aux4; // omega angle2
+    }
+    else
+    {
+      _jacobianOplus[2](1,0) = 0; // omega deltaT
+      _jacobianOplus[0](1,2) = 0; // omega angle1
+      _jacobianOplus[1](1,2) = 0; // omega angle2			
+    }
+
+    _jacobianOplus[0](1,0) = 0; // omega x1
+    _jacobianOplus[0](1,1) = 0; // omega y1
+    _jacobianOplus[1](1,0) = 0; // omega x2
+    _jacobianOplus[1](1,1) = 0; // omega y2
+    _jacobianOplus[0](0,2) = 0; // vel angle1
+    _jacobianOplus[1](0,2) = 0; // vel angle2
+  }
+#endif
+#endif
+ 
+  
+public:
+  
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+
+
+
+
+/**
+ * @class EdgeVelocityHolonomic
+ * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta.
+ * 
+ * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n
+ * \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n
+ * \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n
+ * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n
+ * The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis,
+ * the second one w.r.t. the y-axis and the third one the rotational velocity.
+ * @see TebOptimalPlanner::AddEdgesVelocity
+ * @remarks Do not forget to call setTebConfig()
+ */  
+class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
+{
+public:
+  
+  /**
+   * @brief Construct edge.
+   */       
+  EdgeVelocityHolonomic()
+  {
+    this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+  }
+  
+  /**
+   * @brief Actual cost function
+   */  
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+    Eigen::Vector2d deltaS = conf2->position() - conf1->position();
+    
+    double cos_theta1 = std::cos(conf1->theta());
+    double sin_theta1 = std::sin(conf1->theta()); 
+    
+    // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix)
+    double r_dx =  cos_theta1*deltaS.x() + sin_theta1*deltaS.y();
+    double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y();
+    
+    double vx = r_dx / deltaT->estimate();
+    double vy = r_dy / deltaT->estimate();
+    double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
+    
+    _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon);
+    _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero
+    _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]),
+                   "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);
+  }
+ 
+  
+public:
+  
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h

@@ -0,0 +1,127 @@
+#pragma once
+
+
+#include <teb_local_planner/optimal_planner.h>
+#include <teb_local_planner/g2o_types/base_teb_edges.h>
+#include <teb_local_planner/g2o_types/vertex_timediff.h>
+#include <teb_local_planner/g2o_types/vertex_pose.h>
+#include <teb_local_planner/robot_footprint_model.h>
+
+namespace teb_local_planner
+{
+
+
+/**
+ * @class EdgeVelocityObstacleRatio
+ * @brief Edge defining the cost function for keeping a minimum distance from obstacles.
+ *
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n
+ * \e dist2point denotes the minimum distance to the point obstacle. \n
+ * \e weight can be set using setInformation(). \n
+ * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n
+ * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle
+ * @remarks Do not forget to call setTebConfig() and setObstacle()
+ */
+class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */
+  EdgeVelocityObstacleRatio() :
+    robot_model_(nullptr)
+  {
+    // The three vertices are two poses and one time difference
+    this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+  }
+
+  /**
+   * @brief Actual cost function
+   */
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
+
+    double dist = deltaS.norm();
+    const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta());
+    if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
+    {
+        double radius =  dist/(2*sin(angle_diff/2));
+        dist = fabs( angle_diff * radius ); // actual arg length!
+    }
+    double vel = dist / deltaT->estimate();
+
+    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+
+    const double omega = angle_diff / deltaT->estimate();
+
+    double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement);
+
+    double ratio;
+    if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound)
+      ratio = 0;
+    else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound)
+      ratio = 1;
+    else
+      ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) /
+      (cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound);
+    ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel;
+
+    const double max_vel_fwd = ratio * cfg_->robot.max_vel_x;
+    const double max_omega = ratio * cfg_->robot.max_vel_theta;
+    _error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0);
+    _error[1] = penaltyBoundToInterval(omega, max_omega, 0);
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::computeError() _error[0]=%f , _error[1]=%f\n",_error[0],_error[1]);
+  }
+
+  /**
+   * @brief Set pointer to associated obstacle for the underlying cost function
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */
+  void setObstacle(const Obstacle* obstacle)
+  {
+    _measurement = obstacle;
+  }
+
+  /**
+   * @brief Set pointer to the robot model
+   * @param robot_model Robot model required for distance calculation
+   */
+  void setRobotModel(const BaseRobotFootprintModel* robot_model)
+  {
+    robot_model_ = robot_model;
+  }
+
+  /**
+   * @brief Set all parameters at once
+   * @param cfg TebConfig class
+   * @param robot_model Robot model required for distance calculation
+   * @param obstacle 2D position vector containing the position of the obstacle
+   */
+  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
+  {
+    cfg_ = &cfg;
+    robot_model_ = robot_model;
+    _measurement = obstacle;
+  }
+
+protected:
+
+  const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+
+} // end namespace
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h

@@ -0,0 +1,121 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+#ifndef EDGE_VIA_POINT_H_
+#define EDGE_VIA_POINT_H_
+
+#include "teb_local_planner/g2o_types/vertex_pose.h"
+#include "teb_local_planner/g2o_types/base_teb_edges.h"
+#include "teb_local_planner/misc.h"
+
+#include "g2o/core/base_unary_edge.h"
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @class EdgeViaPoint
+ * @brief Edge defining the cost function for pushing a configuration towards a via point
+ * 
+ * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n
+ * \f$ \min  dist2point \cdot weight \f$. \n
+ * \e dist2point denotes the distance to the via point. \n
+ * \e weight can be set using setInformation(). \n
+ * @see TebOptimalPlanner::AddEdgesViaPoints
+ * @remarks Do not forget to call setTebConfig() and setViaPoint()
+ */     
+class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose>
+{
+public:
+    
+  /**
+   * @brief Construct edge.
+   */    
+  EdgeViaPoint() 
+  {
+    _measurement = NULL;
+  }
+ 
+  /**
+   * @brief Actual cost function
+   */    
+  void computeError()
+  {
+    TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()");
+    const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
+
+    _error[0] = (bandpt->position() - *_measurement).norm();
+
+    TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]);
+  }
+
+  /**
+   * @brief Set pointer to associated via point for the underlying cost function 
+   * @param via_point 2D position vector containing the position of the via point
+   */ 
+  void setViaPoint(const Eigen::Vector2d* via_point)
+  {
+    _measurement = via_point;
+  }
+    
+  /**
+   * @brief Set all parameters at once
+   * @param cfg TebConfig class
+   * @param via_point 2D position vector containing the position of the via point
+   */ 
+  void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point)
+  {
+    cfg_ = &cfg;
+    _measurement = via_point;
+  }
+  
+public: 	
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+  
+    
+
+} // end namespace
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h

@@ -0,0 +1,193 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef PENALTIES_H
+#define PENALTIES_H
+
+#include <cmath>
+#include <Eigen/Core>
+#include <g2o/stuff/misc.h>
+
+namespace teb_local_planner
+{
+
+/**
+ * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
+ * @param var The scalar that should be bounded
+ * @param a lower and upper absolute bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToIntervalDerivative
+ * @return Penalty / cost value that is nonzero if the constraint is not satisfied
+ */
+inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon)
+{
+  if (var < -a+epsilon)
+  {
+    return (-var - (a - epsilon));
+  }
+  if (var <= a-epsilon)
+  {
+    return 0.;
+  }
+  else
+  {
+    return (var - (a - epsilon));
+  }
+}
+
+/**
+ * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param b upper bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToIntervalDerivative
+ * @return Penalty / cost value that is nonzero if the constraint is not satisfied
+ */
+inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon)
+{
+  if (var < a+epsilon)
+  {
+    return (-var + (a + epsilon));
+  }
+  if (var <= b-epsilon)
+  {
+    return 0.;
+  }
+  else
+  {
+    return (var - (b - epsilon));
+  }
+}
+
+
+/**
+ * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundFromBelowDerivative
+ * @return Penalty / cost value that is nonzero if the constraint is not satisfied
+ */
+inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon)
+{
+  if (var >= a+epsilon)
+  {
+    return 0.;
+  }
+  else
+  {
+    return (-var + (a+epsilon));
+  }
+}
+
+/**
+ * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$
+ * @param var The scalar that should be bounded
+ * @param a lower and upper absolute bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToInterval
+ * @return Derivative of the penalty function w.r.t. \c var
+ */
+inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon)
+{
+  if (var < -a+epsilon)
+  {
+    return -1;
+  }
+  if (var <= a-epsilon)
+  {
+    return 0.;
+  }
+  else
+  {
+    return 1;		
+  }
+}
+
+/**
+ * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param b upper bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundToInterval
+ * @return Derivative of the penalty function w.r.t. \c var
+ */
+inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon)
+{
+  if (var < a+epsilon)
+  {
+    return -1;
+  }
+  if (var <= b-epsilon)
+  {
+    return 0.;
+  }
+  else
+  {
+    return 1;		
+  }
+}
+
+
+/**
+ * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$
+ * @param var The scalar that should be bounded
+ * @param a lower bound
+ * @param epsilon safty margin (move bound to the interior of the interval)
+ * @see penaltyBoundFromBelow
+ * @return Derivative of the penalty function w.r.t. \c var
+ */
+inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon)
+{
+  if (var >= a+epsilon)
+  {
+    return 0.;
+  }
+  else
+  {
+    return -1;
+  }
+}
+
+
+} // namespace teb_local_planner
+
+
+#endif // PENALTIES_H

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h

@@ -0,0 +1,229 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef VERTEX_POSE_H_
+#define VERTEX_POSE_H_
+
+#include <g2o/config.h>
+#include <g2o/core/base_vertex.h>
+#include <g2o/core/hyper_graph_action.h>
+#include <g2o/stuff/misc.h>
+
+#include "teb_local_planner/pose_se2.h"
+
+namespace teb_local_planner
+{
+
+/**
+  * @class VertexPose
+  * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o
+  * @see PoseSE2
+  * @see VertexTimeDiff
+  */
+class VertexPose : public g2o::BaseVertex<3, PoseSE2 >
+{
+public:
+
+  /**
+    * @brief Default constructor
+    * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+    */ 
+  VertexPose(bool fixed = false)
+  {
+    setToOriginImpl();
+    setFixed(fixed);
+  }
+  
+  /**
+    * @brief Construct pose using a given PoseSE2
+    * @param pose PoseSE2 defining the pose [x, y, angle_rad]
+    * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+    */ 
+  VertexPose(const PoseSE2& pose, bool fixed = false)
+  {
+    _estimate = pose;
+    setFixed(fixed);
+  }
+  
+  /**
+    * @brief Construct pose using a given 2D position vector and orientation
+    * @param position Eigen::Vector2d containing x and y coordinates
+    * @param theta yaw-angle
+    * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+    */ 
+  VertexPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed = false)
+  {
+    _estimate.position() = position;
+    _estimate.theta() = theta;
+    setFixed(fixed);
+  }
+  
+  /**
+    * @brief Construct pose using single components x, y, and the yaw angle
+    * @param x x-coordinate
+    * @param y y-coordinate
+    * @param theta yaw angle in rad
+    * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+    */ 
+  VertexPose(double x, double y, double theta, bool fixed = false)
+  {
+    _estimate.x() = x;
+    _estimate.y() = y;
+    _estimate.theta() = theta;
+    setFixed(fixed);
+  }
+
+  /**
+    * @brief Access the pose
+    * @see estimate
+    * @return reference to the PoseSE2 estimate
+    */ 
+  inline PoseSE2& pose() {return _estimate;}
+  
+  /**
+    * @brief Access the pose (read-only)
+    * @see estimate
+    * @return const reference to the PoseSE2 estimate
+    */ 
+  inline const PoseSE2& pose() const {return _estimate;}
+	  
+  
+  /**
+    * @brief Access the 2D position part
+    * @see estimate
+    * @return reference to the 2D position part
+    */ 
+  inline Eigen::Vector2d& position() {return _estimate.position();}
+
+  /**
+    * @brief Access the 2D position part (read-only)
+    * @see estimate
+    * @return const reference to the 2D position part
+    */ 
+  inline const Eigen::Vector2d& position() const {return _estimate.position();}
+  
+  /**
+    * @brief Access the x-coordinate the pose
+    * @return reference to the x-coordinate
+    */ 
+  inline double& x() {return _estimate.x();}
+  
+  /**
+    * @brief Access the x-coordinate the pose (read-only)
+    * @return const reference to the x-coordinate
+    */ 
+  inline const double& x() const {return _estimate.x();}
+  
+  /**
+    * @brief Access the y-coordinate the pose
+    * @return reference to the y-coordinate
+    */ 
+  inline double& y() {return _estimate.y();}
+  
+  /**
+    * @brief Access the y-coordinate the pose (read-only)
+    * @return const reference to the y-coordinate
+    */ 
+  inline const double& y() const {return _estimate.y();}
+  
+  /**
+    * @brief Access the orientation part (yaw angle) of the pose
+    * @return reference to the yaw angle
+    */ 
+  inline double& theta() {return _estimate.theta();}
+  
+  /**
+    * @brief Access the orientation part (yaw angle) of the pose (read-only)
+    * @return const reference to the yaw angle
+    */ 
+  inline const double& theta() const {return _estimate.theta();}
+  
+  /**
+    * @brief Set the underlying estimate (2D vector) to zero.
+    */ 
+  virtual void setToOriginImpl() override
+  {
+    _estimate.setZero();
+  }
+
+  /**
+    * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$.
+    * A simple addition for the position.
+    * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$
+    * @param update increment that should be added to the previous esimate
+    */ 
+  virtual void oplusImpl(const double* update) override
+  {
+    _estimate.plus(update);
+  }
+
+  /**
+    * @brief Read an estimate from an input stream.
+    * First the x-coordinate followed by y and the yaw angle.
+    * @param is input stream
+    * @return always \c true
+    */ 
+  virtual bool read(std::istream& is) override
+  {
+    is >> _estimate.x() >> _estimate.y() >> _estimate.theta();
+    return true;
+  }
+
+  /**
+    * @brief Write the estimate to an output stream
+    * First the x-coordinate followed by y and the yaw angle.
+    * @param os output stream
+    * @return \c true if the export was successful, otherwise \c false
+    */ 
+  virtual bool write(std::ostream& os) const override
+  {
+    os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta();
+    return os.good();
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  
+};
+
+}
+
+#endif // VERTEX_POSE_H_

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h

@@ -0,0 +1,145 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ * 
+ * Notes:
+ * The following class is derived from a class defined by the
+ * g2o-framework. g2o is licensed under the terms of the BSD License.
+ * Refer to the base class source for detailed licensing information.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef VERTEX_TIMEDIFF_H
+#define VERTEX_TIMEDIFF_H
+
+
+#include "g2o/config.h"
+#include "g2o/core/base_vertex.h"
+#include "g2o/core/hyper_graph_action.h"
+
+#include <Eigen/Core>
+
+namespace teb_local_planner
+{
+
+/**
+  * @class VertexTimeDiff
+  * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o
+  */
+class VertexTimeDiff : public g2o::BaseVertex<1, double>
+{
+public:
+
+  /**
+    * @brief Default constructor
+    * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+    */  
+  VertexTimeDiff(bool fixed = false)
+  {
+    setToOriginImpl();
+    setFixed(fixed);
+  }
+  
+  /**
+    * @brief Construct the TimeDiff vertex with a value
+    * @param dt time difference value of the vertex
+    * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false]
+    */  
+  VertexTimeDiff(double dt, bool fixed = false)
+  {
+    _estimate = dt;
+    setFixed(fixed);
+  }
+
+  /**
+    * @brief Access the timediff value of the vertex
+    * @see estimate
+    * @return reference to dt
+    */ 
+  inline double& dt() {return _estimate;}
+  
+  /**
+    * @brief Access the timediff value of the vertex (read-only)
+    * @see estimate
+    * @return const reference to dt
+    */ 
+  inline const double& dt() const {return _estimate;}
+  
+  /**
+    * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default.
+    */ 
+  virtual void setToOriginImpl() override
+  {
+    _estimate = 0.1;
+  }
+
+  /**
+    * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$.
+    * A simple addition implements what we want.
+    * @param update increment that should be added to the previous esimate
+    */ 
+  virtual void oplusImpl(const double* update) override
+  {
+      _estimate += *update;
+  }
+
+  /**
+    * @brief Read an estimate of \f$ \Delta T \f$ from an input stream
+    * @param is input stream
+    * @return always \c true
+    */ 
+  virtual bool read(std::istream& is) override
+  {
+    is >> _estimate;
+    return true;
+  }
+
+  /**
+    * @brief Write the estimate \f$ \Delta T \f$ to an output stream
+    * @param os output stream
+    * @return \c true if the export was successful, otherwise \c false
+    */ 
+  virtual bool write(std::ostream& os) const override
+  {
+    os << estimate();
+    return os.good();
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+}
+
+#endif

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/graph_search.h

@@ -0,0 +1,215 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Authors: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#ifndef GRAPH_SEARCH_INTERFACE_H
+#define GRAPH_SEARCH_INTERFACE_H
+
+#ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
+  #include <boost/graph/adjacency_list.hpp>
+#else
+  // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48:
+  // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated,
+  // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now.
+  #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
+  #include <boost/graph/adjacency_list.hpp>
+  #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS
+#endif
+
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/depth_first_search.hpp>
+#include <boost/utility.hpp>
+#include <boost/random.hpp>
+
+#include <Eigen/Core>
+
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "teb_local_planner/equivalence_relations.h"
+#include "teb_local_planner/pose_se2.h"
+#include "teb_local_planner/teb_config.h"
+
+namespace teb_local_planner
+{
+
+class HomotopyClassPlanner; // Forward declaration
+
+//! Vertex in the graph that is used to find homotopy classes (only stores 2D positions)
+struct HcGraphVertex
+{
+public:
+  Eigen::Vector2d pos; // position of vertices in the map
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//! Abbrev. for the homotopy class search-graph type @see HcGraphVertex
+typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph;
+//! Abbrev. for vertex type descriptors in the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::vertex_descriptor HcGraphVertexType;
+//! Abbrev. for edge type descriptors in the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::edge_descriptor HcGraphEdgeType;
+//! Abbrev. for the vertices iterator of the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::vertex_iterator HcGraphVertexIterator;
+//! Abbrev. for the edges iterator of the homotopy class search-graph
+typedef boost::graph_traits<HcGraph>::edge_iterator HcGraphEdgeIterator;
+//! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one
+typedef boost::graph_traits<HcGraph>::adjacency_iterator HcGraphAdjecencyIterator;
+
+//!< Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors
+inline std::complex<long double> getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
+{
+  return std::complex<long double>(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y());
+}
+
+//!< Inline function used for initializing the TEB in combination with HCP graph vertex descriptors
+inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph)
+{
+  return graph[vert_descriptor].pos;
+}
+
+/**
+ * @brief Base class for graph based path planning / homotopy class sampling
+ */
+class GraphSearchInterface
+{
+public:
+
+  virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false) = 0;
+
+  /**
+   * @brief Clear any existing graph of the homotopy class search
+   */
+  void clearGraph() {graph_.clear();}
+
+  // HcGraph graph() const {return graph_;}
+  // Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above.
+  HcGraph graph_; //!< Store the graph that is utilized to find alternative homotopy classes.
+
+protected:
+  /**
+   * @brief Protected constructor that should be called by subclasses
+   */
+  GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){}
+
+  /**
+   * @brief Depth First Search implementation to find all paths between the start and the specified goal vertex.
+   *
+   * Complete paths are stored to the internal path container.
+   * @sa http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/
+   * @param g Graph on which the depth first should be performed
+   * @param visited A container that stores visited vertices (pass an empty container, it will be filled inside during recursion).
+   * @param goal Desired goal vertex
+   * @param start_orientation Orientation of the first trajectory pose, required to initialize the trajectory/TEB
+   * @param goal_orientation Orientation of the goal trajectory pose, required to initialize the trajectory/TEB
+   * @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   */
+
+  void DepthFirst(HcGraph& g, std::vector<HcGraphVertexType>& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+protected:
+    const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
+    HomotopyClassPlanner* const hcp_; //!< Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the graph search class it is holding.
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+
+class lrKeyPointGraph : public GraphSearchInterface
+{
+public:
+  lrKeyPointGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
+
+  virtual ~lrKeyPointGraph(){}
+
+  /**
+   * @brief Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal.
+   *
+   * This version of the graph creation places a keypoint on the left and right side of each obstacle w.r.t to the goal heading. \n
+   * All feasible paths between start and goal point are extracted using a Depth First Search afterwards. \n
+   * This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph()
+   * method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths.
+   *
+   * @see createProbRoadmapGraph
+   * @param start Start pose from wich to start on (e.g. the current robot pose).
+   * @param goal Goal pose to find paths to (e.g. the robot's goal).
+   * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
+   * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
+   * @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   */
+  virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+};
+
+
+
+
+class ProbRoadmapGraph : public GraphSearchInterface
+{
+public:
+  ProbRoadmapGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){}
+
+  virtual ~ProbRoadmapGraph(){}
+
+
+  /**
+   * @brief Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal.
+   *
+   * This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal. \n
+   * Afterwards all feasible paths between start and goal point are extracted using a Depth First Search. \n
+   * Use the sampling method for complex, non-point or huge obstacles. \n
+   * You may call createGraph() instead.
+   *
+   * @see createGraph
+   * @param start Start pose from wich to start on (e.g. the current robot pose).
+   * @param goal Goal pose to find paths to (e.g. the robot's goal).
+   * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
+   * @param no_samples number of random samples
+   * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1]
+   * @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   */
+  virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+private:
+    boost::random::mt19937 rnd_generator_; //!< Random number generator used by createProbRoadmapGraph to sample graph keypoints.
+};
+} // end namespace
+
+#endif // GRAPH_SEARCH_INTERFACE_H

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/h_signature.h

@@ -0,0 +1,432 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Authors: Christoph Rösmann, Franz Albers
+ *********************************************************************/
+
+#ifndef H_SIGNATURE_H_
+#define H_SIGNATURE_H_
+
+#include <boost/math/special_functions.hpp>
+
+#include "teb_local_planner/equivalence_relations.h"
+#include "teb_local_planner/misc.h"
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/timed_elastic_band.h"
+
+#include <rclcpp/rclcpp.hpp>
+#include <math.h>
+#include <algorithm>
+#include <functional>
+#include <vector>
+#include <iterator>
+
+
+namespace teb_local_planner
+{
+
+/**
+ * @brief The H-signature defines an equivalence relation based on homology in terms of complex calculus.
+ *
+ * The H-Signature depends on the obstacle configuration and can be utilized
+ * to check whether two trajectores belong to the same homology class.
+ * Refer to: \n
+ * 	- S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
+ */
+class HSignature : public EquivalenceClass
+{
+
+public:
+
+    /**
+    * @brief Constructor accepting a TebConfig
+    * @param cfg TebConfig storing some user configuration options
+    */
+    HSignature(const TebConfig& cfg) : cfg_(&cfg) {}
+
+
+   /**
+    * @brief Calculate the H-Signature of a path
+    *
+    * The implemented function accepts generic path descriptions that are restricted to the following structure: \n
+    * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
+    * Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
+    * that returns a complex value for the position (Re(*)=x, Im(*)=y).
+     *
+    * T could also be a pointer type, if the passed function also accepts a const T* point_Type.
+    *
+    * @param path_start Iterator to the first element in the path
+    * @param path_end Iterator to the last element in the path
+    * @param obstacles obstacle container
+    * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
+    * @tparam BidirIter Bidirectional iterator type
+    * @tparam Fun function of the form std::complex< long double > (const T& point_type)
+    */
+    template<typename BidirIter, typename Fun>
+    void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles)
+    {
+        if (obstacles->empty())
+        {
+            hsignature_ = std::complex<double>(0,0);
+            return;
+        }
+
+
+        TEB_ASSERT_MSG(cfg_->hcp.h_signature_prescaler>0.1 && cfg_->hcp.h_signature_prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed.");
+
+        // guess values for f0
+        // paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles
+        int m = std::max( (int)obstacles->size()-1, 5 );  // for only a few obstacles we need a min threshold in order to get significantly high H-Signatures
+
+        int a = (int) std::ceil(double(m)/2.0);
+        int b = m-a;
+
+        std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points
+
+        typedef std::complex<long double> cplx;
+        // guess map size (only a really really coarse guess is required
+        // use distance from start to goal as distance to each direction
+        // TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval
+        cplx start = fun_cplx_point(*path_start);
+        cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before
+        cplx delta = end-start;
+        cplx normal(-delta.imag(), delta.real());
+        cplx map_bottom_left;
+        cplx map_top_right;
+        if (std::abs(delta) < 3.0)
+        { // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine...
+            map_bottom_left = start + cplx(0, -3);
+            map_top_right = start + cplx(3, 3);
+        }
+        else
+        {
+            map_bottom_left = start - normal;
+            map_top_right = start + delta + normal;
+        }
+
+        hsignature_ = 0; // reset local signature
+
+        std::vector<double> imag_proposals(5);
+
+        // iterate path
+        while(path_start != path_end)
+        {
+            cplx z1 = fun_cplx_point(*path_start);
+            cplx z2 = fun_cplx_point(*std::next(path_start));
+
+            for (std::size_t l=0; l<obstacles->size(); ++l) // iterate all obstacles
+            {
+                cplx obst_l = obstacles->at(l)->getCentroidCplx();
+                //cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b);
+                cplx f0 = (long double) cfg_->hcp.h_signature_prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right);
+
+                // denum contains product with all obstacles exepct j==l
+                cplx Al = f0;
+                for (std::size_t j=0; j<obstacles->size(); ++j)
+                {
+                    if (j==l)
+                        continue;
+                    cplx obst_j = obstacles->at(j)->getCentroidCplx();
+                    cplx diff = obst_l - obst_j;
+                    //if (diff.real()!=0 || diff.imag()!=0)
+                    if (std::abs(diff)<0.05) // skip really close obstacles
+                        continue;
+                     else
+                        Al /= diff;
+                }
+                // compute log value
+                double diff2 = std::abs(z2-obst_l);
+                double diff1 = std::abs(z1-obst_l);
+                if (diff2 == 0 || diff1 == 0)
+                    continue;
+                double log_real = std::log(diff2)-std::log(diff1);
+                // complex ln has more than one solution -> choose minimum abs angle -> paper
+                double arg_diff = std::arg(z2-obst_l)-std::arg(z1-obst_l);
+                imag_proposals.at(0) = arg_diff;
+                imag_proposals.at(1) = arg_diff+2*M_PI;
+                imag_proposals.at(2) = arg_diff-2*M_PI;
+                imag_proposals.at(3) = arg_diff+4*M_PI;
+                imag_proposals.at(4) = arg_diff-4*M_PI;
+                double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs);
+                cplx log_value(log_real,log_imag);
+                //cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work
+                hsignature_ += Al*log_value;
+            }
+            ++path_start;
+        }
+    }
+
+
+   /**
+    * @brief Check if two candidate classes are equivalent
+    * @param other The other equivalence class to test with
+    */
+    virtual bool isEqual(const EquivalenceClass& other) const
+    {
+        const HSignature* hother = dynamic_cast<const HSignature*>(&other); // TODO: better architecture without dynamic_cast
+        if (hother)
+        {
+            double diff_real = std::abs(hother->hsignature_.real() - hsignature_.real());
+            double diff_imag = std::abs(hother->hsignature_.imag() - hsignature_.imag());
+            if (diff_real<=cfg_->hcp.h_signature_threshold && diff_imag<=cfg_->hcp.h_signature_threshold)
+                return true; // Found! Homotopy class already exists, therefore nothing added
+        }
+        else
+            RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature equivalence classes with types other than HSignature.");
+
+        return false;
+    }
+
+   /**
+    * @brief Check if the equivalence value is detected correctly
+    * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
+    */
+    virtual bool isValid() const
+    {
+        return std::isfinite(hsignature_.real()) && std::isfinite(hsignature_.imag());
+    }
+
+    /**
+     * @brief Check if the trajectory is non-looping around an obstacle.
+     * @return Returns always true, as this cannot be detected by this kind of H-Signature.
+     */
+    virtual bool isReasonable() const
+    {
+      return true;
+    }
+
+    /**
+     * @brief Get the current value of the h-signature (read-only)
+     * @return h-signature in complex-number format
+     */
+     const std::complex<long double>& value() const {return hsignature_;}
+
+
+private:
+
+    const TebConfig* cfg_;
+    std::complex<long double> hsignature_;
+};
+
+
+
+
+
+/**
+ * @brief The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology using theorems from electro magnetism.
+ *
+ * The H-Signature depends on the obstacle configuration and can be utilized
+ * to check whether two trajectores belong to the same homology class.
+ * Refer to: \n
+ * 	- S. Bhattacharya et al.: Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D, 2011
+ */
+class HSignature3d : public EquivalenceClass
+{
+public:
+    /**
+    * @brief Constructor accepting a TebConfig
+    * @param cfg TebConfig storing some user configuration options
+    */
+    HSignature3d(const TebConfig& cfg) : cfg_(&cfg) {}
+
+
+   /**
+    * @brief Calculate the H-Signature of a path
+    *
+    * The implemented function accepts generic path descriptions that are restricted to the following structure: \n
+    * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n
+    * Provide a unary function with the following signature <c> std::complex< long double > (const T& point_type) </c>
+    * that returns a complex value for the position (Re(*)=x, Im(*)=y).
+    *
+    * T could also be a pointer type, if the passed function also accepts a const T* point_Type.
+    *
+    * @param path_start Iterator to the first element in the path
+    * @param path_end Iterator to the last element in the path
+    * @param obstacles obstacle container
+    * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
+    * @tparam BidirIter Bidirectional iterator type
+    * @tparam Fun function of the form std::complex< long double > (const T& point_type)
+    */
+    template<typename BidirIter, typename Fun>
+    void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
+                             boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
+    {
+      hsignature3d_.resize(obstacles->size());
+
+      std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points)
+
+      constexpr int num_int_steps_per_segment = 10;
+
+      for (std::size_t l = 0; l < obstacles->size(); ++l) // iterate all obstacles
+      {
+        double H = 0;
+        double transition_time = 0;
+        double next_transition_time = 0;
+        BidirIter path_iter;
+        TimeDiffSequence::iterator timediff_iter;
+
+        Eigen::Vector3d s1 (obstacles->at(l)->getCentroid()(0), obstacles->at(l)->getCentroid()(1), 0);
+        double t = 120; // some large value for defining the end point of the obstacle/"conductor" model
+        Eigen::Vector3d s2;
+        obstacles->at(l)->predictCentroidConstantVelocity(t, s2.head(2));
+        s2[2] = t;
+        Eigen::Vector3d ds = s2 - s1;
+        double ds_sq_norm = ds.squaredNorm(); // by definition not zero as t > 0 (3rd component)
+
+        // iterate path
+        for (path_iter = path_start, timediff_iter = timediff_start.get(); path_iter != path_end; ++path_iter, ++timediff_iter)
+        {
+          std::complex<long double> z1 = fun_cplx_point(*path_iter);
+          std::complex<long double> z2 = fun_cplx_point(*std::next(path_iter));
+
+          transition_time = next_transition_time;
+          if (timediff_start == boost::none || timediff_end == boost::none) // if no time information is provided yet, approximate transition time
+            next_transition_time += std::abs(z2 - z1) / cfg_->robot.max_vel_x; // Approximate the time, if no time is known
+          else // otherwise use the time information from the teb trajectory
+          {
+            if (std::distance(path_iter, path_end) != std::distance(timediff_iter, timediff_end.get())) {
+              RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Size of poses and timediff vectors does not match. This is a bug.");
+            }
+            next_transition_time += (*timediff_iter)->dt();
+          }
+
+          Eigen::Vector3d direction_vec;
+          direction_vec[0] = z2.real() - z1.real();
+          direction_vec[1] = z2.imag() - z1.imag();
+          direction_vec[2] = next_transition_time - transition_time;
+
+          if(direction_vec.norm() < 1e-15)  // Coincident poses
+            continue;
+
+          Eigen::Vector3d r(z1.real(), z1.imag(), transition_time);
+          Eigen::Vector3d dl = 1.0/static_cast<double>(num_int_steps_per_segment) * direction_vec; // Integrate with multiple steps between each pose
+          Eigen::Vector3d p1, p2, d, phi;
+          for (int i = 0; i < num_int_steps_per_segment; ++i, r += dl)
+          {
+            p1 = s1 - r;
+            p2 = s2 - r;
+            d = (ds.cross(p1.cross(p2))) / ds_sq_norm;
+            phi = 1.0 / d.squaredNorm() * ((d.cross(p2) / p2.norm()) - (d.cross(p1) / p1.norm()));
+            H += phi.dot(dl);
+          }
+        }
+
+        // normalize to 1
+        hsignature3d_.at(l) = H/(4.0*M_PI);
+      }
+    }
+
+    /**
+     * @brief Check if two candidate classes are equivalent
+     *
+     * If the absolute value of the H-Signature is equal or greater than 1, a loop (in x-y) around the obstacle is indicated.
+     * Positive H-Signature: Obstacle lies on the left hand side of the planned trajectory
+     * Negative H-Signature: Obstacle lies on the right hand side of the planned trajectory
+     * H-Signature equals zero: Obstacle lies in infinity, has no influence on trajectory
+     *
+     * @param other The other equivalence class to test with
+     */
+    virtual bool isEqual(const EquivalenceClass& other) const
+    {
+      const HSignature3d* hother = dynamic_cast<const HSignature3d*>(&other); // TODO: better architecture without dynamic_cast
+      if (hother)
+      {
+        if (hsignature3d_.size() == hother->hsignature3d_.size())
+        {
+          for(size_t i = 0; i < hsignature3d_.size(); ++i)
+          {
+            // If the H-Signature for one obstacle is below this threshold, that obstacle is far away from the planned trajectory,
+            // and therefore ignored in the homotopy class planning
+            if (std::abs(hother->hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold || std::abs(hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold)
+              continue;
+
+            if (boost::math::sign(hother->hsignature3d_.at(i)) != boost::math::sign(hsignature3d_.at(i)))
+              return false; // Signatures are not equal, new homotopy class
+          }
+        return true; // Found! Homotopy class already exists, therefore nothing added
+        }
+      }
+      else {
+        RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature3d equivalence classes with types other than HSignature3d.");
+      }
+
+      return false;
+    }
+
+    /**
+     * @brief Check if the equivalence value is detected correctly
+     * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur.
+     */
+     virtual bool isValid() const
+     {
+        for(const double& value : hsignature3d_)
+        {
+          if (!std::isfinite(value))
+            return false;
+        }
+        return true;
+     }
+
+    /**
+     * @brief Check if the trajectory is non-looping around any obstacle. Values greater than 1 indicate a looping trajectory.
+     * @return Returns false, if the trajectory loops around an obstacle
+     */
+    virtual bool isReasonable() const
+    {
+      for(const double& value : hsignature3d_)
+      {
+        if (value > 1.0)
+          return false;
+      }
+      return true;
+    }
+
+    /**
+     * @brief Get the current h-signature (read-only)
+     * @return h-signature in complex-number format
+     */
+     const std::vector<double>& values() const {return hsignature3d_;}
+
+private:
+    const TebConfig* cfg_;
+    std::vector<double> hsignature3d_;
+};
+
+
+} // namespace teb_local_planner
+
+
+#endif /* H_SIGNATURE_H_ */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h

@@ -0,0 +1,591 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef HOMOTOPY_CLASS_PLANNER_H_
+#define HOMOTOPY_CLASS_PLANNER_H_
+
+#include <math.h>
+
+#include <algorithm>
+#include <functional>
+#include <iterator>
+#include <memory>
+#include <vector>
+#include <iterator>
+#include <random>
+
+#include <visualization_msgs/msg/marker.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "teb_local_planner/planner_interface.h"
+#include "teb_local_planner/teb_config.h"
+#include "teb_local_planner/obstacles.h"
+#include "teb_local_planner/optimal_planner.h"
+#include "teb_local_planner/visualization.h"
+#include "teb_local_planner/robot_footprint_model.h"
+#include "teb_local_planner/equivalence_relations.h"
+#include "teb_local_planner/graph_search.h"
+
+
+namespace teb_local_planner
+{
+
+//!< Inline function used for calculateHSignature() in combination with VertexPose pointers
+inline std::complex<long double> getCplxFromVertexPosePtr(const VertexPose* pose)
+{
+  return std::complex<long double>(pose->x(), pose->y());
+};
+
+
+//!< Inline function used for calculateHSignature() in combination with geometry_msgs::msg::PoseStamped
+inline std::complex<long double> getCplxFromMsgPoseStamped(const geometry_msgs::msg::PoseStamped& pose)
+{
+  return std::complex<long double>(pose.pose.position.x, pose.pose.position.y);
+};
+
+/**
+ * @class HomotopyClassPlanner
+ * @brief Local planner that explores alternative homotopy classes, create a plan for each alternative
+ *	  and finally return the robot controls for the current best path (repeated in each sampling interval)
+ *
+ * Equivalence classes (e.g. homotopy) are explored using the help of a search-graph. \n
+ * A couple of possible candidates are sampled / generated and filtered afterwards such that only a single candidate
+ * per homotopy class remain. Filtering is applied using the H-Signature, a homotopy (resp. homology) invariant: \n
+ *      - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010
+ *      - C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015.
+ *
+ * Followed by the homotopy class search, each candidate is used as an initialization for the underlying trajectory
+ * optimization (in this case utilizing the TebOptimalPlanner class with the TimedElasticBand). \n
+ * Depending on the config parameters, the optimization is performed in parallel. \n
+ * After the optimization is completed, the best optimized candidate is selected w.r.t. to trajectory cost, since the
+ * cost already contains important features like clearance from obstacles and transition time. \n
+ *
+ * Everyhting is performed by calling one of the overloaded plan() methods. \n
+ * Afterwards the velocity command to control the robot is obtained from the "best" candidate
+ * via getVelocityCommand(). \n
+ *
+ * All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories
+ * are preferred against new path initilizations in order to improve the hot-starting capability.
+ */
+class HomotopyClassPlanner : public PlannerInterface
+{
+public:
+
+  using EquivalenceClassContainer = std::vector< std::pair<EquivalenceClassPtr, bool> >;
+
+  /**
+   * @brief Default constructor
+   */
+  HomotopyClassPlanner();
+
+  /**
+   * @brief Construct and initialize the HomotopyClassPlanner
+   * @param node Shared pointer for rclcpp::Node
+   * @param cfg Const reference to the TebConfig class for internal parameters
+   * @param obstacles Container storing all relevant obstacles (see Obstacle)
+   * @param robot_model Shared pointer to the robot shape model used for optimization (optional)
+   * @param visualization Shared pointer to the TebVisualization class (optional)
+   * @param via_points Container storing via-points (optional)
+   */
+  HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
+                       TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
+
+  /**
+   * @brief Destruct the HomotopyClassPlanner.
+   */
+  virtual ~HomotopyClassPlanner();
+
+  /**
+   * @brief Initialize the HomotopyClassPlanner
+   * @param node Shared pointer for rclcpp::Node
+   * @param cfg Const reference to the TebConfig class for internal parameters
+   * @param obstacles Container storing all relevant obstacles (see Obstacle)
+   * @param visualization Shared pointer to the TebVisualization class (optional)
+   * @param via_points Container storing via-points (optional)
+   */
+  void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL,
+                  TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL);
+
+  /** @name Plan a trajectory */
+  //@{
+
+  /**
+   * @brief Plan a trajectory based on an initial reference plan.
+   *
+   * Provide this method to create and optimize a trajectory that is initialized
+   * according to an initial reference plan (given as a container of poses).
+   * @warning The current implementation extracts only the start and goal pose and calls the overloaded plan()
+   * @param initial_plan vector of geometry_msgs::msg::PoseStamped (must be valid until clearPlanner() is called!)
+   * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+   *		      otherwise the final velocity will be zero (default: false)
+   * @return \c true if planning was successful, \c false otherwise
+   */
+  virtual bool plan(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+  /**
+   * @brief Plan a trajectory between a given start and goal pose (tf::Pose version).
+   *
+   * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
+   * @param start tf::Pose containing the start pose of the trajectory
+   * @param goal tf::Pose containing the goal pose of the trajectory
+   * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+   *		      otherwise the final velocity will be zero (default: false)
+   * @return \c true if planning was successful, \c false otherwise
+   */
+  // tf2 doesn't have tf::Pose
+//  virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+  /**
+   * @brief Plan a trajectory between a given start and goal pose.
+   *
+   * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose.
+   * @param start PoseSE2 containing the start pose of the trajectory
+   * @param goal PoseSE2 containing the goal pose of the trajectory
+   * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity).
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed,
+   *		      otherwise the final velocity will be zero (default: false)
+   * @return \c true if planning was successful, \c false otherwise
+   */
+  virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false);
+
+  /**
+   * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval.
+   * @warning Call plan() first and check if the generated plan is feasible.
+   * @param[out] vx translational velocity [m/s]
+   * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s]
+   * @param[out] omega rotational velocity [rad/s]
+   * @param[in] look_ahead_poses index of the final pose used to compute the velocity command.
+   * @return \c true if command is valid, \c false otherwise
+   */
+  virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const;
+
+  /**
+   * @brief Access current best trajectory candidate (that relates to the "best" homotopy class).
+   *
+   * If no trajectory is available, the pointer will be empty.
+   * If only a single trajectory is available, return it.
+   * Otherwise return the best one, but call selectBestTeb() before to perform the actual selection (part of the plan() methods).
+   * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
+   */
+  TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;}
+
+  /**
+   * @brief Check whether the planned trajectory is feasible or not.
+   *
+   * This method currently checks only that the trajectory, or a part of the trajectory is collision free.
+   * Obstacles are here represented as costmap instead of the internal ObstacleContainer.
+   * @param costmap_model Pointer to the costmap model
+   * @param footprint_spec The specification of the footprint of the robot in world coordinates
+   * @param inscribed_radius The radius of the inscribed circle of the robot
+   * @param circumscribed_radius The radius of the circumscribed circle of the robot
+   * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked.
+   * @return \c true, if the robot footprint along the first part of the trajectory intersects with
+   *         any obstacle in the costmap, \c false otherwise.
+   */
+  virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector<geometry_msgs::msg::Point>& footprint_spec,
+                                    double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0);
+
+  /**
+   * @brief In case of empty best teb, scores again the available plans to find the best one.
+   *        The best_teb_ variable is updated consequently.
+   * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
+   *         An empty pointer is returned if no plan is available.
+   */
+  TebOptimalPlannerPtr findBestTeb();
+
+  /**
+   * @brief Removes the specified teb and the corresponding homotopy class from the list of available ones.
+   * @param pointer to the teb Band to be removed
+   * @return Iterator to the next valid teb if available, else to the end of the tebs container.
+   */
+  TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb);
+
+  //@}
+
+  /** @name Visualization */
+  //@{
+
+  /**
+   * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence)
+   * @param visualization shared pointer to a TebVisualization instance
+   * @see visualizeTeb
+   */
+  void setVisualization(const TebVisualizationPtr & visualization) override;
+
+   /**
+    * @brief Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz).
+    *
+    * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor.
+    * @see setVisualization
+    */
+  virtual void visualize();
+
+  //@}
+
+  /** @name Important steps that are called during planning */
+  //@{
+
+
+  /**
+   * @brief Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them.
+   *
+   * This "all-in-one" method creates a graph with position keypoints from which
+   * feasible paths (with clearance from obstacles) are extracted. \n
+   * All obtained paths are filted to only keep a single path for each equivalence class. \n
+   * Each time a new equivalence class is explored (by means of \b no previous trajectory/TEB remain in that equivalence class),
+   * a new trajectory/TEB will be initialized. \n
+   *
+   * Everything is prepared now for the optimization step: see optimizeAllTEBs().
+   * @param start Current start pose (e.g. pose of the robot)
+   * @param goal Goal pose (e.g. robot's goal)
+   * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization).
+   * @param @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   */
+  void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel = false);
+
+  /**
+   * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path
+   *
+   * Refer to TimedElasticBand::initTEBtoGoal() for more details about the template parameters.
+   * @param path_start start iterator of a generic 2d path
+   * @param path_end end iterator of a generic 2d path
+   * @param fun_position unary function that returns the Eigen::Vector2d object
+   * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading)
+   * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading)
+   * @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   * @tparam BidirIter Bidirectional iterator type
+   * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d
+   * @return Shared pointer to the newly created teb optimal planner
+   */
+  template<typename BidirIter, typename Fun>
+  TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+  /**
+   * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal
+   * @param start start pose
+   * @param goal goal pose
+   * @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   * @return Shared pointer to the newly created teb optimal planner
+   */
+  TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+  /**
+   * @brief Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container
+   * @param initial_plan container of poses (start and goal orientation should be valid!)
+   * @param start_velocity start velocity (optional)
+   * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false)
+   * @return Shared pointer to the newly created teb optimal planner
+   */
+  TebOptimalPlannerPtr addAndInitNewTeb(const std::vector<geometry_msgs::msg::PoseStamped>& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false);
+
+  /**
+   * @brief Update TEBs with new pose, goal and current velocity.
+   * @param start New start pose (optional)
+   * @param goal New goal pose (optional)
+   * @param start_velocity start velocity (optional)
+   */
+  void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity);
+
+
+  /**
+   * @brief Optimize all available trajectories by invoking the optimizer on each one.
+   *
+   * Depending on the configuration parameters, the optimization is performed either single or multi threaded.
+   * @param iter_innerloop Number of inner iterations (see TebOptimalPlanner::optimizeTEB())
+   * @param iter_outerloop Number of outer iterations (see TebOptimalPlanner::optimizeTEB())
+   */
+  void optimizeAllTEBs(int iter_innerloop, int iter_outerloop);
+
+  /**
+   * @brief Returns a shared pointer to the TEB related to the initial plan
+   * @return A non-empty shared ptr is returned if a match was found; Otherwise the shared ptr is empty.
+   */
+  TebOptimalPlannerPtr getInitialPlanTEB();
+
+  /**
+   * @brief In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values.
+   *
+   * The trajectory cost includes features such as transition time and clearance from obstacles. \n
+   * The best trajectory can be accessed later by bestTeb() within the current sampling interval in order to avoid unessary recalculations.
+   * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand).
+   */
+  TebOptimalPlannerPtr selectBestTeb();
+
+  //@}
+
+   /**
+    * @brief Reset the planner.
+    *
+    * Clear all previously found H-signatures, paths, tebs and the hcgraph.
+    */
+  virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;}
+
+
+  /**
+   * @brief Prefer a desired initial turning direction (by penalizing the opposing one)
+   *
+   * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two
+   * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty.
+   * Initial means that the penalty is applied only to the first few poses of the trajectory.
+   * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none)
+   */
+  virtual void setPreferredTurningDir(RotType dir);
+
+  /**
+   * @brief Calculate the equivalence class of a path
+   *
+   * Currently, only the H-signature (refer to HSignature) is implemented.
+   *
+   * @param path_start Iterator to the first element in the path
+   * @param path_end Iterator to the last element in the path
+   * @param obstacles obstacle container
+   * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number.
+   * @tparam BidirIter Bidirectional iterator type
+   * @tparam Fun function of the form std::complex< long double > (const T& point_type)
+   * @return pointer to the equivalence class base type
+   */
+  template<typename BidirIter, typename Fun>
+  EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL,
+                                                boost::optional<TimeDiffSequence::iterator> timediff_start = boost::none, boost::optional<TimeDiffSequence::iterator> timediff_end = boost::none);
+
+  /**
+   * @brief Read-only access to the internal trajectory container.
+   * @return read-only reference to the teb container.
+   */
+  const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;}
+
+  bool hasDiverged() const override;
+
+  /**
+   * Compute and return the cost of the current optimization graph (supports multiple trajectories)
+   * @param[out] cost current cost value for each trajectory
+   *                  [for a planner with just a single trajectory: size=1, vector will not be cleared]
+   * @param obst_cost_scale Specify extra scaling for obstacle costs
+   * @param viapoint_cost_scale Specify extra scaling for via points.
+   * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time
+   */
+  virtual void computeCurrentCost(std::vector<double>& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false);
+
+  /**
+   * @brief Check if two h-signatures are similar (w.r.t. a certain threshold)
+   * @param h1 first h-signature
+   * @param h2 second h-signature
+   * @return \c true if both h-signatures are similar, false otherwise.
+   */
+  inline static bool isHSignatureSimilar(const std::complex<long double>& h1, const std::complex<long double>& h2, double threshold)
+  {
+      double diff_real = std::abs(h2.real() - h1.real());
+      double diff_imag = std::abs(h2.imag() - h1.imag());
+      if (diff_real<=threshold && diff_imag<=threshold)
+        return true; // Found! Homotopy class already exists, therefore nothing added
+      return false;
+  }
+  /**
+   * @brief Checks if the orientation of the computed trajectories differs from that of the best plan of more than the
+   *  specified threshold and eventually deletes them.
+   *  Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration).
+   * @param orient_threshold: Threshold paramter for allowed orientation changes in radians
+   * @param len_orientation_vector: length of the vector used to compute the start orientation
+   */
+  void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector);
+  /**
+   * @brief Given a plan, computes its start orientation using a vector of length >= len_orientation_vector
+   *        starting from the initial pose.
+   * @param plan: Teb to be analyzed
+   * @param len_orientation_vector: min length of the vector used to compute the start orientation
+   * @param orientation: computed start orientation
+   * @return: Could the vector for the orientation check be computed? (False if the plan has no pose with a distance
+   *          > len_orientation_vector from the start poseq)
+   */
+  bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation);
+
+
+  /**
+   * @brief Access config (read-only)
+   * @return const pointer to the config instance
+   */
+  const TebConfig* config() const {return cfg_;}
+
+  /**
+   * @brief Access current obstacle container (read-only)
+   * @return const pointer to the obstacle container instance
+   */
+  const ObstContainer* obstacles() const {return obstacles_;}
+
+  /**
+   * @brief Returns true if the planner is initialized
+   */
+  bool isInitialized() const {return initialized_;}
+
+  /**
+   * @brief Clear any existing graph of the homotopy class search
+   */
+  void clearGraph() {if(graph_search_) graph_search_->clearGraph();}
+
+  /**
+   * @brief find the index of the currently best TEB in the container
+   * @remarks bestTeb() should be preferred whenever possible
+   * @return index of the best TEB obtained with bestTEB(), if no TEB is avaiable, it returns -1.
+   */
+  int bestTebIdx() const;
+
+
+  /**
+   * @brief Internal helper function that adds a new equivalence class to the list of known classes only if it is unique.
+   * @param eq_class equivalence class that should be tested
+   * @param lock if \c true, exclude the H-signature from deletion.
+   * @return \c true if the h-signature was added and no duplicate was found, \c false otherwise
+   */
+  bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false);
+
+  /**
+   * @brief Return the current set of equivalence erelations (read-only)
+   * @return reference to the internal set of currently tracked equivalence relations
+   */
+  const EquivalenceClassContainer& getEquivalenceClassRef() const  {return equivalence_classes_;}
+
+  bool isInBestTebClass(const EquivalenceClassPtr& eq_class) const;
+
+  int numTebsInClass(const EquivalenceClassPtr& eq_class) const;
+
+  int numTebsInBestTebClass() const;
+
+  /**
+   * @brief Randomly drop non-optimal TEBs to so we can explore other alternatives
+   *
+   * The HCP has a tendency to become "fixated" once its tebs_ list becomes
+   * fully populated, repeatedly refining and evaluating paths from the same
+   * few homotopy classes until the robot moves far enough for a teb to become
+   * invalid. As a result, it can fail to discover a more optimal path. This
+   * function alleviates this problem by randomly dropping TEBs other than the
+   * current "best" one with a probability controlled by
+   * selection_dropping_probability parameter.
+   */
+  void randomlyDropTebs();
+
+protected:
+
+  /** @name Explore new paths and keep only a single one for each homotopy class */
+  //@{
+
+  /**
+   * @brief Check if a h-signature exists already.
+   * @param eq_class equivalence class that should be tested
+   * @return \c true if the h-signature is found, \c false otherwise
+   */
+  bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const;
+
+
+  /**
+   * @brief Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded.
+   *
+   * Calling this method in each new planning interval is really important.
+   * First all old h-signatures are deleted, since they could be invalid for this planning step (obstacle position may changed).
+   * Afterwards the h-signatures are calculated for each existing TEB/trajectory and is inserted to the list of known h-signatures.
+   * Doing this is important to prefer already optimized trajectories in contrast to initialize newly explored coarse paths.
+   * @param delete_detours if this param is \c true, all existing TEBs are cleared from detour-candidates calling deletePlansGoingBackwards().
+   */
+  void renewAndAnalyzeOldTebs(bool delete_detours);
+
+  /**
+   * @brief Associate trajectories with via-points
+   *
+   * If \c all_trajectories is true, all trajectory candidates are connected with the set of via_points,
+   * otherwise only the trajectory sharing the homotopy class of the initial/global plan (and all via-points from alternative trajectories are removed)
+   * @remarks Requires that the plan method is called with an initial plan provided and that via-points are enabled (config)
+   * @param all_trajectories see method description
+   */
+  void updateReferenceTrajectoryViaPoints(bool all_trajectories);
+
+  //@}
+
+  // external objects (store weak pointers)
+  const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
+  ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning
+  const ViaPointContainer* via_points_; //!< Store the current list of via-points
+
+  // internal objects (memory management owned)
+  TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...)
+  TebOptimalPlannerPtr best_teb_; //!< Store the current best teb.
+  EquivalenceClassPtr best_teb_eq_class_; //!< Store the equivalence class of the current best teb
+  RobotFootprintModelPtr robot_model_; //!< Robot model shared instance
+
+  const std::vector<geometry_msgs::msg::PoseStamped>* initial_plan_; //!< Store the initial plan if available for a better trajectory initialization
+  EquivalenceClassPtr initial_plan_eq_class_; //!< Store the equivalence class of the initial plan
+  TebOptimalPlannerPtr initial_plan_teb_; //!< Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.)
+
+  TebOptPlannerContainer tebs_; //!< Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs
+
+  EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.
+                                                                            //   The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).
+
+  std::shared_ptr<GraphSearchInterface> graph_search_;
+
+  rclcpp::Time last_eq_class_switching_time_; //!< Store the time at which the equivalence class changed recently
+
+  std::default_random_engine random_;
+  bool initialized_; //!< Keeps track about the correct initialization of this class
+
+  TebOptimalPlannerPtr last_best_teb_;  //!< Points to the plan used in the previous control cycle
+
+
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+
+};
+
+//! Abbrev. for a shared pointer of a HomotopyClassPlanner instance.
+typedef std::shared_ptr<HomotopyClassPlanner> HomotopyClassPlannerPtr;
+
+
+} // namespace teb_local_planner
+
+// include template implementations / definitions
+#include "teb_local_planner/homotopy_class_planner.hpp"
+
+#endif /* HOMOTOPY_CLASS_PLANNER_H_ */

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp

@@ -0,0 +1,95 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#include "teb_local_planner/homotopy_class_planner.h"
+#include "teb_local_planner/h_signature.h"
+
+namespace teb_local_planner
+{
+  
+  
+template<typename BidirIter, typename Fun>
+EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles,
+                                                                    boost::optional<TimeDiffSequence::iterator> timediff_start, boost::optional<TimeDiffSequence::iterator> timediff_end)
+{
+  if(cfg_->obstacles.include_dynamic_obstacles)
+  {
+    HSignature3d* H = new HSignature3d(*cfg_);
+    H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end);
+    return EquivalenceClassPtr(H);
+  }
+  else
+  {
+    HSignature* H = new HSignature(*cfg_);
+    H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles);
+    return EquivalenceClassPtr(H);
+  }
+}
+
+
+template<typename BidirIter, typename Fun>
+TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel)
+{
+  TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_));
+
+  candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta,
+                                 cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples,
+                                 cfg_->trajectory.allow_init_with_backwards_motion);
+
+  if (start_velocity)
+    candidate->setVelocityStart(*start_velocity);
+
+  EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_,
+                                                    candidate->teb().timediffs().begin(), candidate->teb().timediffs().end());
+
+  
+  if (free_goal_vel)
+    candidate->setVelocityGoalFree();
+
+  if(addEquivalenceClassIfNew(H))
+  {
+    tebs_.push_back(candidate);
+    return tebs_.back();
+  }
+
+  // If the candidate constitutes no new equivalence class, return a null pointer
+  return TebOptimalPlannerPtr();
+}
+  
+} // namespace teb_local_planner
+

src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/misc.h

@@ -0,0 +1,210 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2016,
+ *  TU Dortmund - Institute of Control Theory and Systems Engineering.
+ *  All rights reserved.
+ *
+ *  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 institute 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 OWNER 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.
+ *
+ * Author: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef MISC_H
+#define MISC_H
+
+#include <builtin_interfaces/msg/duration.hpp>
+
+#include <Eigen/Core>
+
+#include <exception>
+#include <type_traits>
+
+#include <rclcpp/logging.hpp>
+
+namespace teb_local_planner
+{
+#define SMALL_NUM 0.00000001
+
+//! Symbols for left/none/right rotations      
+enum class RotType { left, none, right };
+
+/** 
+ * @brief Check whether two variables (double) are close to each other
+ * @param a the first value to compare
+ * @param b the second value to compare
+ * @param epsilon precision threshold
+ * @return \c true if |a-b| < epsilon, false otherwise
+ */
+inline bool is_close(double a, double b, double epsilon = 1e-4) 
+{ 
+  return std::fabs(a - b) < epsilon; 
+}
+
+/** 
+ * @brief Return the average angle of an arbitrary number of given angles [rad]
+ * @param angles vector containing all angles
+ * @return average / mean angle, that is normalized to [-pi, pi]
+ */
+inline double average_angles(const std::vector<double>& angles)
+{
+  double x=0, y=0;
+  for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
+  {
+      x += cos(*it);
+      y += sin(*it);
+  }
+  if(x == 0 && y == 0)
+      return 0;
+  else
+      return std::atan2(y, x);
+}
+
+/** @brief Small helper function: check if |a|<|b| */
+inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
+
+
+/**
+ * @brief Calculate a fast approximation of a sigmoid function
+ * @details The following function is implemented: \f$ x / (1 + |x|) \f$
+ * @param x the argument of the function
+*/
+inline double fast_sigmoid(double x)
+{
+  return x / (1 + fabs(x));
+}
+
+/**
+ * @brief Calculate Euclidean distance between two 2D point datatypes
+ * @param point1 object containing fields x and y
+ * @param point2 object containing fields x and y
+ * @return Euclidean distance: ||point2-point1||
+*/
+template <typename P1, typename P2>
+inline double distance_points2d(const P1& point1, const P2& point2)
+{
+  return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
+}
+
+
+/**
+ * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)
+ * @param v1 object containing public methods x() and y()
+ * @param v2 object containing fields x() and y()
+ * @return magnitude that would result in the 3D case (along the z-axis)
+*/
+template <typename V1, typename V2>
+inline double cross2d(const V1& v1, const V2& v2)
+{
+     return v1.x()*v2.y() - v2.x()*v1.y();
+}
+
+/** 
+ * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
+ * 
+ * Return a constant reference for boths input variants (pointer or reference).
+ * @remarks Makes only sense in combination with the overload getConstReference(const T& val).
+ * @param ptr pointer of type T
+ * @tparam T arbitrary type 
+ * @return  If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
+ */
+template<typename T>
+inline const T& get_const_reference(const T* ptr) {return *ptr;}
+
+/** 
+ * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference.
+ * 
+ * Return a constant reference for boths input variants (pointer or reference).
+ * @remarks Makes only sense in combination with the overload getConstReference(const T* val).
+ * @param val
+ * @param dummy SFINAE helper variable
+ * @tparam T arbitrary type 
+ * @return  If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion
+ */
+template<typename T>
+inline const T& get_const_reference(const T& val, typename std::enable_if_t<!std::is_pointer<T>::value, T>* dummy = nullptr) {return val;}
+
+inline builtin_interfaces::msg::Duration durationFromSec(double t_sec)
+{
+  int32_t sec;
+  uint32_t nsec;
+  sec = static_cast<int32_t>(floor(t_sec));
+  nsec = static_cast<int32_t>(std::round((t_sec - sec) * 1e9));
+  // avoid rounding errors
+  sec += (nsec / 1000000000l);
+  nsec %= 1000000000l;
+
+  builtin_interfaces::msg::Duration duration;
+  duration.sec = sec;
+  duration.nanosec = nsec;
+  return duration;
+}
+
+struct TebAssertionFailureException : public std::runtime_error
+{
+    TebAssertionFailureException(const std::string &msg)
+        : std::runtime_error(msg)
+    {
+        RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), msg.c_str());
+    }
+};
+
+#define TEB_ASSERT_MSG_IMPL(...) \
+    { \
+        char arg_string[1024]; \
+        std::sprintf(arg_string, __VA_ARGS__); \
+        const std::string msg(arg_string); \
+        throw TebAssertionFailureException(msg); \
+    }
+
+template<typename T, typename ...ARGS, typename std::enable_if_t<std::is_arithmetic<T>::value>* = nullptr>
+void teb_assert_msg_impl(const T expression, ARGS ...args) {
+    if(expression == 0) {
+        char arg_string[1024];
+        std::sprintf(arg_string, args..., "");
+        const std::string msg(arg_string);
+        throw TebAssertionFailureException(msg);
+    }
+}
+
+template<typename T, typename ...ARGS, typename std::enable_if_t<std::is_pointer<T>::value>* = nullptr>
+void teb_assert_msg_impl(const T expression, ARGS ...args) {
+    if(expression == nullptr) {
+        char arg_string[1024];
+        std::sprintf(arg_string, args..., "");
+        const std::string msg(arg_string);
+        throw TebAssertionFailureException(msg);
+    }
+}
+
+#define TEB_ASSERT_MSG(expression, ...) teb_assert_msg_impl(expression, __VA_ARGS__)
+
+} // namespace teb_local_planner
+
+#endif /* MISC_H */