diff --git a/.circleci/config.yml b/.circleci/config.yml
index e0a3aab16..6101fe431 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -1,8 +1,13 @@
 version: 2
+
 jobs:
   build_and_test:
     docker:
       - image: ros:foxy
+    environment:
+      ROS_WS: "/opt/ros"
+      UNDERLAY_WS: "/opt/underlay_ws"
+      OVERLAY_WS: "/opt/overlay_ws"
     steps:
       - checkout
       - run:
@@ -10,19 +15,31 @@ jobs:
           command: |
             apt update -qq && apt install -y build-essential cmake python3-colcon-common-extensions python3-rosdep libeigen3-dev
             apt upgrade -y
-            source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
-            mkdir -p src/grid_map && mv `find -maxdepth 1 -not -name . -not -name src` src/grid_map/
+            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
             rosdep update
+            mkdir -p $OVERLAY_WS/src/grid_map && mv `find -maxdepth 1 -not -name . -not -name src` $OVERLAY_WS/src/grid_map/
+            mkdir -p $UNDERLAY_WS/src && cp $OVERLAY_WS/src/grid_map/tools/ros2_dependencies.repos \
+              $UNDERLAY_WS/ros2_dependencies.repos
+            cd $UNDERLAY_WS && vcs import src < ros2_dependencies.repos
+            cp $UNDERLAY_WS/src/ros-planning/navigation2/tools/ros2_dependencies.repos \
+              $UNDERLAY_WS/ros2_dependencies.repos
+            vcs import src < ros2_dependencies.repos
             rosdep install -y --ignore-src --from-paths src
+            colcon build --symlink-install --packages-up-to nav2_costmap_2d
+            source $UNDERLAY_WS/install/local_setup.bash
+            cd $OVERLAY_WS && rosdep install -y --ignore-src --from-paths src
       - run:
           name: Debug Build
           command: |
-            source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
-            colcon build --parallel-workers 1
+            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
+            source $UNDERLAY_WS/install/local_setup.bash
+            cd $OVERLAY_WS && colcon build --parallel-workers 1
       - run:
           name: Run Tests
           command: |
-            source `find /opt/ros -maxdepth 2 -name local_setup.bash | sort | head -1`
+            source `find $ROS_WS -maxdepth 2 -name local_setup.bash | sort | head -1`
+            source $UNDERLAY_WS/install/local_setup.bash
+            cd $OVERLAY_WS
             colcon test --parallel-workers 1
             colcon test-result --verbose
 workflows:
diff --git a/grid_map_costmap_2d/CMakeLists.txt b/grid_map_costmap_2d/CMakeLists.txt
index 38fcdac5f..68b38a161 100644
--- a/grid_map_costmap_2d/CMakeLists.txt
+++ b/grid_map_costmap_2d/CMakeLists.txt
@@ -1,47 +1,34 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(grid_map_costmap_2d)
 
-set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+## Find ament_cmake macros and libraries
+find_package(ament_cmake REQUIRED)
+find_package(grid_map_cmake_helpers REQUIRED)
+find_package(grid_map_core REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
-## Find catkin macros and libraries
-find_package(catkin REQUIRED COMPONENTS
-  grid_map_core
-  costmap_2d
-  tf
-)
+find_package(Eigen3 REQUIRED)
 
-## System dependencies are found with CMake's conventions
-#find_package(Eigen3 REQUIRED)
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-  CATKIN_DEPENDS
-    grid_map_core
-    costmap_2d
-    tf
-  DEPENDS
-)
-
-###########
-## Build ##
-###########
+grid_map_package()
 
 ## Specify additional locations of header files
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
+  ${grid_map_core_INCLUDE_DIRS}
+  ${nav2_costmap_2d_INCLUDE_DIRS}
+  ${geometry_msgs_INCLUDE_DIRS}
+)
+
+set(dependencies
+  grid_map_core
+  geometry_msgs
+  nav2_costmap_2d
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 #############
@@ -51,7 +38,7 @@ include_directories(
 # Mark cpp header files for installation
 install(
   DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  DESTINATION include/${PROJECT_NAME}
   FILES_MATCHING PATTERN "*.hpp"
 )
 
@@ -59,17 +46,36 @@ install(
 ## Testing ##
 #############
 
-if(CATKIN_ENABLE_TESTING)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
-  ## Add gtest based cpp test target and link libraries
-  catkin_add_gtest(
-  	${PROJECT_NAME}-test
-  	test/test_grid_map_costmap_2d.cpp
-  	test/Costmap2DConverterTest.cpp
-  )
-  add_subdirectory(rostest)
-endif()
+if(BUILD_TESTING)
+  # Linting is setup this way to add a filter
+  # to ament_cpplint to ignore the lack of
+  # copyright messages at the top of files.
+  # Copyright messages are being checked for by both
+  # ament_cmake_cpplint & ament_cmake_copyright.
+
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_lint_auto QUIET)
+  if(ament_lint_auto_FOUND)
+    # exclude copyright checks
+    list(APPEND AMENT_LINT_AUTO_EXCLUDE
+      ament_cmake_cpplint
+      ament_cmake_copyright
+    )
+    ament_lint_auto_find_test_dependencies()
 
-if(TARGET ${PROJECT_NAME}-test)
-  target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES})
+    # run cpplint without copyright filter
+    find_package(ament_cmake_cpplint)
+    ament_cpplint(
+      FILTERS -legal/copyright
+    )
+  endif()
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+
+  add_subdirectory(test)
 endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+ament_package()
diff --git a/grid_map_costmap_2d/COLCON_IGNORE b/grid_map_costmap_2d/COLCON_IGNORE
deleted file mode 100644
index e69de29bb..000000000
diff --git a/grid_map_costmap_2d/include/grid_map_costmap_2d/Costmap2DConverter.hpp b/grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp
similarity index 59%
rename from grid_map_costmap_2d/include/grid_map_costmap_2d/Costmap2DConverter.hpp
rename to grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp
index ce3995065..b3e30ca52 100644
--- a/grid_map_costmap_2d/include/grid_map_costmap_2d/Costmap2DConverter.hpp
+++ b/grid_map_costmap_2d/include/grid_map_costmap_2d/costmap_2d_converter.hpp
@@ -1,5 +1,5 @@
 /*
- * CostmapConverter.hpp
+ * costmap_2d_converter.hpp
  *
  *  Created on: Nov 23, 2016
  *      Author: Peter Fankhauser, ETH Zurich
@@ -8,44 +8,55 @@
  *              Gabriel Hottiger, ANYbotics
  */
 
-#pragma once
+#ifndef GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_
+#define GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_
 
-#include <grid_map_core/grid_map_core.hpp>
+// STD
+#include <cstdint>
+#include <vector>
+#include <string>
 
 // ROS
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/costmap_2d_ros.h>
-#include <tf/tf.h>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
-// STD
-#include <stdint.h>
-#include <vector>
+#include "grid_map_core/grid_map_core.hpp"
 
-namespace grid_map {
+namespace grid_map
+{
 
 /**
  * Defines the conversion between grid_map and costmap_2d.
  * -> There is only one distinct value for no information in the grid map.
  * -> All values smaller or equal to freeSpace are considered free space (except noInformation).
- * -> All values bigger or equal to lethalObstacle are considered a lethal obstacle (except noInformation).
- * -> All values between inscribedInflatedObstacle and lethalObstacle are considered an inscribed inflated obstacle (except noInformation).
+ * -> All values bigger or equal to lethalObstacle are
+ * considered a lethal obstacle (except noInformation).
+ * -> All values between inscribedInflatedObstacle and
+ * lethalObstacle are considered an inscribed inflated obstacle (except noInformation).
  * -> All other values are interpolated between freeSpace and inscribedInflatedObstacle
- * @tparam noInformation                Gridmap value that represents no information.
- * @tparam lethalObstacle               Gridmap value (lower bound) for lethal obstacles.
- * @tparam inscribedInflatedObstacle    Gridmap value (lower bound) for inscribed inflated obstacles.
- * @tparam freeSpace                    Gridmap value (upper bound) for free space.
+ * @tparam noInformation               Gridmap value that represents no information.
+ * @tparam lethalObstacle              Gridmap value (lower bound) for lethal obstacles.
+ * @tparam inscribedInflatedObstacle   Gridmap value (lower bound) for inscribed inflated obstacles.
+ * @tparam freeSpace                   Gridmap value (upper bound) for free space.
  */
-template <int64_t noInformation, int64_t lethalObstacle, int64_t inscribedInflatedObstacle, int64_t freeSpace>
-class Costmap2DTranslationTable {
+template<int64_t noInformation, int64_t lethalObstacle, int64_t inscribedInflatedObstacle,
+  int64_t freeSpace>
+class Costmap2DTranslationTable
+{
   // Check required pre-conditions of template arguments
-  static_assert(freeSpace < inscribedInflatedObstacle,
-                "[Costmap2DTranslationTable] Condition violated: freeSpace < inscribedInflatedObstacle.");
-  static_assert(inscribedInflatedObstacle < lethalObstacle,
-                "[Costmap2DTranslationTable] Condition violated: inscribedInflatedObstacle < lethalObstacle.");
-  static_assert(noInformation < freeSpace || noInformation > lethalObstacle,
-                "[Costmap2DTranslationTable] Condition violated: noInformation < freeSpace || noInformation > lethalObstacle.");
-
- public:
+  static_assert(
+    freeSpace < inscribedInflatedObstacle,
+    "[Costmap2DTranslationTable] Condition violated: freeSpace < inscribedInflatedObstacle.");
+  static_assert(
+    inscribedInflatedObstacle < lethalObstacle,
+    "[Costmap2DTranslationTable] Condition violated: inscribedInflatedObstacle < lethalObstacle.");
+  static_assert(
+    (noInformation < freeSpace) || (noInformation > lethalObstacle),
+    "[Costmap2DTranslationTable] Condition violated: "
+    "noInformation < freeSpace || noInformation > lethalObstacle.");
+
+public:
   // Only static methods -> delete constructor.
   Costmap2DTranslationTable() = delete;
 
@@ -54,8 +65,9 @@ class Costmap2DTranslationTable {
    * @tparam DataType Data type of the grid map.
    * @param costTranslationTable Translation table mapping from costmap value to grid map value.
    */
-  template <typename DataType>
-  static void create(std::vector<DataType>& costTranslationTable) {
+  template<typename DataType>
+  static void create(std::vector<DataType> & costTranslationTable)
+  {
     costTranslationTable.resize(256);
     for (unsigned int i = 0; i < costTranslationTable.size(); ++i) {
       costTranslationTable[i] = fromCostmap<DataType>(static_cast<uint8_t>(i));
@@ -68,26 +80,29 @@ class Costmap2DTranslationTable {
    * @param costmapValue Cost map value.
    * @return Equivalent grid map value.
    */
-  template <typename DataType>
-  static DataType fromCostmap(const uint8_t costmapValue) {
+  template<typename DataType>
+  static DataType fromCostmap(const uint8_t costmapValue)
+  {
     // Check special cost map values.
-    if (costmapValue == costmap_2d::FREE_SPACE) {
+    if (costmapValue == nav2_costmap_2d::FREE_SPACE) {
       return freeSpace;
-    } else if (costmapValue == costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+    } else if (costmapValue == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
       return inscribedInflatedObstacle;
-    } else if (costmapValue == costmap_2d::LETHAL_OBSTACLE) {
+    } else if (costmapValue == nav2_costmap_2d::LETHAL_OBSTACLE) {
       return lethalObstacle;
-    } else if (costmapValue == costmap_2d::NO_INFORMATION) {
+    } else if (costmapValue == nav2_costmap_2d::NO_INFORMATION) {
       return noInformation;
     }
 
-    // Map costmap map interval to gridmap interval for values between free space and inflated obstacle
-    constexpr DataType costmapIntervalStart = costmap_2d::FREE_SPACE;
-    constexpr DataType costmapIntervalWidth = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
+    // Map costmap map interval to gridmap interval
+    // for values between free space and inflated obstacle
+    constexpr DataType costmapIntervalStart = nav2_costmap_2d::FREE_SPACE;
+    constexpr DataType
+      costmapIntervalWidth = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
     constexpr DataType gridmapIntervalStart = freeSpace;
     constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
-    const DataType interpolatedValue =
-        gridmapIntervalStart + (costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth;
+    const DataType interpolatedValue = gridmapIntervalStart + (
+      costmapValue - costmapIntervalStart) * gridmapIntervalWidth / costmapIntervalWidth;
     return interpolatedValue;
   }
 
@@ -97,26 +112,28 @@ class Costmap2DTranslationTable {
    * @param gridmapValue Grid map value.
    * @return Equivalent cost map value.
    */
-  template <typename DataType>
-  static uint8_t toCostmap(const DataType gridmapValue) {
+  template<typename DataType>
+  static uint8_t toCostmap(const DataType gridmapValue)
+  {
     // Check special grid map values.
     if (gridmapValue == static_cast<DataType>(noInformation)) {
-      return costmap_2d::NO_INFORMATION;
+      return nav2_costmap_2d::NO_INFORMATION;
     } else if (gridmapValue <= static_cast<DataType>(freeSpace)) {
-      return costmap_2d::FREE_SPACE;
+      return nav2_costmap_2d::FREE_SPACE;
     } else if (gridmapValue >= static_cast<DataType>(lethalObstacle)) {
-      return costmap_2d::LETHAL_OBSTACLE;
+      return nav2_costmap_2d::LETHAL_OBSTACLE;
     } else if (gridmapValue >= static_cast<DataType>(inscribedInflatedObstacle)) {
-      return costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+      return nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
     }
 
     // Map gridmap interval to costmap interval for values between free space and inflated obstacle
-    constexpr DataType costmapIntervalStart = costmap_2d::FREE_SPACE;
-    constexpr DataType costmapIntervalWidth = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
+    constexpr DataType costmapIntervalStart = nav2_costmap_2d::FREE_SPACE;
+    constexpr DataType
+      costmapIntervalWidth = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - costmapIntervalStart;
     constexpr DataType gridmapIntervalStart = freeSpace;
     constexpr DataType gridmapIntervalWidth = inscribedInflatedObstacle - gridmapIntervalStart;
-    const DataType interpolatedValue =
-        costmapIntervalStart + (gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth;
+    const DataType interpolatedValue = costmapIntervalStart + (
+      gridmapValue - gridmapIntervalStart) * costmapIntervalWidth / gridmapIntervalWidth;
 
     return std::round(interpolatedValue);
   }
@@ -124,22 +141,33 @@ class Costmap2DTranslationTable {
 
 /*!
  * @brief Direct cost translations.
- * This maps the cost directly, simply casting between the underlying unsigned char and float fields.
+ * This maps the cost directly, simply casting between
+ * the underlying unsigned char and float fields.
  */
-using Costmap2DDirectTranslationTable = Costmap2DTranslationTable<costmap_2d::NO_INFORMATION, costmap_2d::LETHAL_OBSTACLE,
-                                                                  costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap_2d::FREE_SPACE>;
+using Costmap2DDirectTranslationTable = Costmap2DTranslationTable<nav2_costmap_2d::NO_INFORMATION,
+    nav2_costmap_2d::LETHAL_OBSTACLE,
+    nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+    nav2_costmap_2d::FREE_SPACE>;
 
 /**
  * @brief Century cost translations to [0, 100] for costmap_2d -> grid map.
- * This maps the cost onto float fields between 0.0 and 100.0 with reserved values allocated for the costmap_2d.
+ * This maps the cost onto float fields between 0.0 and 100.0
+ * with reserved values allocated for the costmap_2d.
  */
-using Costmap2DCenturyTranslationTable = Costmap2DTranslationTable<-1, 100, 99, costmap_2d::FREE_SPACE>;
+using Costmap2DCenturyTranslationTable =
+  Costmap2DTranslationTable<-1, 100, 99, nav2_costmap_2d::FREE_SPACE>;
 
-template <typename DataType>
-class Costmap2DDefaultTranslationTable : public Costmap2DDirectTranslationTable {};
+template<typename DataType>
+class Costmap2DDefaultTranslationTable
+  : public Costmap2DDirectTranslationTable
+{
+};
 
-template <>
-class Costmap2DDefaultTranslationTable<float> : public Costmap2DCenturyTranslationTable {};
+template<>
+class Costmap2DDefaultTranslationTable<float>
+  : public Costmap2DCenturyTranslationTable
+{
+};
 
 /*****************************************************************************
 ** Converter
@@ -153,9 +181,11 @@ class Costmap2DDefaultTranslationTable<float> : public Costmap2DCenturyTranslati
  *
  * @sa Costmap2DDirectTranslationTable, Costmap2DCenturyTranslationTable
  */
-template <typename MapType, typename TranslationTable = Costmap2DDefaultTranslationTable<typename MapType::DataType>>
-class Costmap2DConverter {
- public:
+template<typename MapType,
+  typename TranslationTable = Costmap2DDefaultTranslationTable<typename MapType::DataType>>
+class Costmap2DConverter
+{
+public:
   Costmap2DConverter() = default;
   virtual ~Costmap2DConverter() = default;
 
@@ -164,11 +194,17 @@ class Costmap2DConverter {
    * @param gridMap Grid map containing size info and properties.
    * @param outputCostmap Resized costmap.
    */
-  void initializeFromGridMap(const MapType& gridMap, costmap_2d::Costmap2D& outputCostmap) {
+  void initializeFromGridMap(const MapType & gridMap, nav2_costmap_2d::Costmap2D & outputCostmap)
+  {
     // Different origin position
     const Position position = gridMap.getPosition() - Position(0.5 * gridMap.getLength());
     const Size sizeXY = gridMap.getSize();
-    outputCostmap.resizeMap(sizeXY(0), sizeXY(1), gridMap.getResolution(), position(0), position(1));
+    outputCostmap.resizeMap(
+      sizeXY(0),
+      sizeXY(1),
+      gridMap.getResolution(),
+      position(0),
+      position(1));
   }
 
   /*!
@@ -178,7 +214,11 @@ class Costmap2DConverter {
    * @param outputCostmap to this costmap type object.
    * @return true if successful, false otherwise.
    */
-  bool setCostmap2DFromGridMap(const MapType& gridMap, const std::string& layer, costmap_2d::Costmap2D& outputCostmap) {
+  bool setCostmap2DFromGridMap(
+    const MapType & gridMap,
+    const std::string & layer,
+    nav2_costmap_2d::Costmap2D & outputCostmap)
+  {
     // Check compliance.
     Size size(outputCostmap.getSizeInCellsX(), outputCostmap.getSizeInCellsY());
     if ((gridMap.getSize() != size).any()) {
@@ -194,19 +234,23 @@ class Costmap2DConverter {
     // between Costmap2D and grid map.
     const size_t nCells = gridMap.getSize().prod();
     for (size_t i = 0, j = nCells - 1; i < nCells; ++i, --j) {
-      outputCostmap.getCharMap()[j] = TranslationTable::template toCostmap<DataType>(gridMap.get(layer).data()[i]);
+      outputCostmap.getCharMap()[j] =
+        TranslationTable::template toCostmap<DataType>(gridMap.get(layer).data()[i]);
     }
     return true;
   }
 
-  void initializeFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, MapType& outputMap) {
+  void initializeFromCostmap2D(nav2_costmap_2d::Costmap2DROS & costmap2d, MapType & outputMap)
+  {
     initializeFromCostmap2D(*(costmap2d.getCostmap()), outputMap);
     outputMap.setFrameId(costmap2d.getGlobalFrameID());
   }
 
-  void initializeFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, MapType& outputMap) {
+  void initializeFromCostmap2D(const nav2_costmap_2d::Costmap2D & costmap2d, MapType & outputMap)
+  {
     const double resolution = costmap2d.getResolution();
-    Length length(costmap2d.getSizeInCellsX() * resolution, costmap2d.getSizeInCellsY() * resolution);
+    Length
+      length(costmap2d.getSizeInCellsX() * resolution, costmap2d.getSizeInCellsY() * resolution);
     Position position(costmap2d.getOriginX(), costmap2d.getOriginY());
     // Different conventions.
     position += Position(0.5 * length);
@@ -215,13 +259,18 @@ class Costmap2DConverter {
 
   /*!
    * Load the costmap2d into the specified layer.
-   * @warning This does not lock the costmap2d object, you should take care to do so from outside this function.
+   * @warning This does not lock the costmap2d object,
+   * you should take care to do so from outside this function.
    * @param costmap2d from this costmap type object.
    * @param layer the name of the layer to insert.
    * @param outputMap to this costmap type object.
    * @return true if successful, false otherwise.
    */
-  bool addLayerFromCostmap2D(const costmap_2d::Costmap2D& costmap2d, const std::string& layer, MapType& outputMap) {
+  bool addLayerFromCostmap2D(
+    const nav2_costmap_2d::Costmap2D & costmap2d,
+    const std::string & layer,
+    MapType & outputMap)
+  {
     // Check compliance.
     Size size(costmap2d.getSizeInCellsX(), costmap2d.getSizeInCellsY());
     if ((outputMap.getSize() != size).any()) {
@@ -248,13 +297,18 @@ class Costmap2DConverter {
 
   /*!
    * Load the costmap2d into the specified layer.
-   * @warning This does not lock the costmap2d object, you should take care to do so from outside this function.
+   * @warning This does not lock the costmap2d object,
+   * you should take care to do so from outside this function.
    * @param costmap2d from this costmap type object.
    * @param layer the name of the layer to insert.
    * @param outputMap to this costmap type object.
    * @return true if successful, false otherwise.
    */
-  bool addLayerFromCostmap2D(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer, MapType& outputMap) {
+  bool addLayerFromCostmap2D(
+    nav2_costmap_2d::Costmap2DROS & costmap2d,
+    const std::string & layer,
+    MapType & outputMap)
+  {
     return addLayerFromCostmap2D(*(costmap2d.getCostmap()), layer, outputMap);
   }
 
@@ -267,33 +321,30 @@ class Costmap2DConverter {
    * that here.
    *
    * @warning this does not lock the costmap2d object, you should take care to do so from
-   * outside this function in scope with any addLayerFromCostmap2DAtRobotPose calls you wish to make.
+   * outside this function in scope with any
+   * addLayerFromCostmap2DAtRobotPose calls you wish to make.
    *
    * @param costmap2d : the underlying Costmap2DROS object
    * @param geometry: size of the subwindow to snapshot around the robot pose
    * @param outputMap : initialise the output map with the required parameters
    */
-  bool initializeFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const Length& length, MapType& outputMap) {
+  bool initializeFromCostmap2DAtRobotPose(
+    nav2_costmap_2d::Costmap2DROS & costmap2d,
+    const Length & length,
+    MapType & outputMap)
+  {
     const double resolution = costmap2d.getCostmap()->getResolution();
 
-    // Get the Robot Pose Transform.
-#if ROS_VERSION_MINIMUM(1, 14, 0)
-    geometry_msgs::PoseStamped tfPose;
+    geometry_msgs::msg::PoseStamped tfPose;
     if (!costmap2d.getRobotPose(tfPose)) {
       errorMessage_ = "Could not get robot pose, is it actually published?";
       return false;
     }
     Position robotPosition(tfPose.pose.position.x, tfPose.pose.position.y);
-#else
-    tf::Stamped<tf::Pose> tfPose;
-    if (!costmap2d.getRobotPose(tfPose)) {
-      errorMessage_ = "Could not get robot pose, is it actually published?";
-      return false;
-    }
-    Position robotPosition(tfPose.getOrigin().x(), tfPose.getOrigin().y());
-#endif
+
     // Determine new costmap origin.
-    Position rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY());
+    Position
+      rosMapOrigin(costmap2d.getCostmap()->getOriginX(), costmap2d.getCostmap()->getOriginY());
     Position newCostMapOrigin;
 
     // Note:
@@ -312,25 +363,27 @@ class Costmap2DConverter {
     // if there is an even number of cells
     //   centre of the new grid map at the closest vertex between cells
     // of the current cell
-    int numberOfCellsX = length.x() / resolution;
-    int numberOfCellsY = length.y() / resolution;
+    int numberOfCellsX = static_cast<int>(length.x() / resolution);
+    int numberOfCellsY = static_cast<int>(length.y() / resolution);
     if (numberOfCellsX % 2) {  // odd
-      newCostMapOrigin(0) = std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x();
+      newCostMapOrigin(0) =
+        std::floor(robotCellPosition.x()) * resolution + resolution / 2.0 + rosMapOrigin.x();
     } else {
       newCostMapOrigin(0) = std::round(robotCellPosition.x()) * resolution + rosMapOrigin.x();
     }
     if (numberOfCellsY % 2) {  // odd
-      newCostMapOrigin(1) = std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y();
+      newCostMapOrigin(1) =
+        std::floor(robotCellPosition.y()) * resolution + resolution / 2.0 + rosMapOrigin.y();
     } else {
       newCostMapOrigin(1) = std::round(robotCellPosition.y()) * resolution + rosMapOrigin.y();
     }
 
-    // TODO check the robot pose is in the window
-    // TODO check the geometry fits within the costmap2d window
+    // TODO(tbd): check the robot pose is in the window
+    // TODO(tbd): check the geometry fits within the costmap2d window
 
     // Initialize the output map.
     outputMap.setFrameId(costmap2d.getGlobalFrameID());
-    outputMap.setTimestamp(ros::Time::now().toNSec());
+    outputMap.setTimestamp(costmap2d.now().nanoseconds());
     outputMap.setGeometry(length, resolution, newCostMapOrigin);
     return true;
   }
@@ -344,7 +397,11 @@ class Costmap2DConverter {
    * @param layer the layer name to add.
    * @param outputMap the initialized and filled in map output map.
    */
-  bool addLayerFromCostmap2DAtRobotPose(costmap_2d::Costmap2DROS& costmap2d, const std::string& layer, MapType& outputMap) {
+  bool addLayerFromCostmap2DAtRobotPose(
+    nav2_costmap_2d::Costmap2DROS & costmap2d,
+    const std::string & layer,
+    MapType & outputMap)
+  {
     /****************************************
     ** Asserts
     ****************************************/
@@ -359,20 +416,21 @@ class Costmap2DConverter {
     // 2) check the geometry fits inside the costmap2d subwindow is done below
 
     // Properties.
-    const double resolution = costmap2d.getCostmap()->getResolution();
     const Length geometry = outputMap.getLength();
     const Position position = outputMap.getPosition();
 
     // Copy data.
-    bool isValidWindow = false;
-    costmap_2d::Costmap2D costmapSubwindow;
-    // TODO
-    isValidWindow = costmapSubwindow.copyCostmapWindow(*(costmap2d.getCostmap()),
-                                                       position.x() - geometry.x() / 2.0,  // subwindow_bottom_left_x
-                                                       position.y() - geometry.y() / 2.0,  // subwindow_bottom_left_y
-                                                       geometry.x(), geometry.y());
+    bool isValidWindow;
+    nav2_costmap_2d::Costmap2D costmapSubwindow;
+    // TODO(tbd)
+    isValidWindow = costmapSubwindow.copyCostmapWindow(
+      *(costmap2d.getCostmap()),
+      position.x() - geometry.x() / 2.0,  // subwindow_bottom_left_x
+      position.y() - geometry.y() / 2.0,  // subwindow_bottom_left_y
+      geometry.x(), geometry.y());
     if (!isValidWindow) {
-      // handle differently - e.g. copy the internal part only and lethal elsewhere, but other parts would have to handle being outside too
+      // handle differently - e.g. copy the internal part only and lethal elsewhere,
+      // but other parts would have to handle being outside too
       errorMessage_ = "Subwindow landed outside the costmap, aborting.";
       return false;
     }
@@ -388,10 +446,12 @@ class Costmap2DConverter {
    *
    * @return std::string
    */
-  std::string errorMessage() const { return errorMessage_; }
+  std::string errorMessage() const {return errorMessage_;}
 
- private:
+private:
   std::string errorMessage_;
 };
 
 }  // namespace grid_map
+
+#endif  // GRID_MAP_COSTMAP_2D__COSTMAP_2D_CONVERTER_HPP_
diff --git a/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp b/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp
index 089e97694..23420a053 100644
--- a/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp
+++ b/grid_map_costmap_2d/include/grid_map_costmap_2d/grid_map_costmap_2d.hpp
@@ -1,12 +1,15 @@
 /*
- * grid_map.hpp
+ * grid_map_costmap_2d.hpp
  *
  *  Created on: Jul 14, 2014
  *      Author: Péter Fankhauser
  *	 Institute: ETH Zurich, ANYbotics
  */
 
-#pragma once
+#ifndef GRID_MAP_COSTMAP_2D__GRID_MAP_COSTMAP_2D_HPP_
+#define GRID_MAP_COSTMAP_2D__GRID_MAP_COSTMAP_2D_HPP_
 
 #include <grid_map_core/grid_map_core.hpp>
-#include <grid_map_costmap_2d/Costmap2DConverter.hpp>
+#include <grid_map_costmap_2d/costmap_2d_converter.hpp>
+
+#endif  // GRID_MAP_COSTMAP_2D__GRID_MAP_COSTMAP_2D_HPP_
diff --git a/grid_map_costmap_2d/package.xml b/grid_map_costmap_2d/package.xml
index 1cd177e3c..bf8d0a93e 100644
--- a/grid_map_costmap_2d/package.xml
+++ b/grid_map_costmap_2d/package.xml
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
   <name>grid_map_costmap_2d</name>
   <version>1.6.2</version>
   <description>Interface for grid maps to the costmap_2d format.</description>
@@ -9,8 +10,21 @@
   <url type="website">http://github.com/anybotics/grid_map</url>
   <url type="bugtracker">http://github.com/anybotics/grid_map/issues</url>
   <author email="pfankhauser@anybotics.com">Péter Fankhauser</author>
-  <buildtool_depend>catkin</buildtool_depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>grid_map_cmake_helpers</build_depend>
+
   <depend>grid_map_core</depend>
-  <depend>costmap_2d</depend>
-  <depend>tf</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/grid_map_costmap_2d/rostest/CMakeLists.txt b/grid_map_costmap_2d/rostest/CMakeLists.txt
deleted file mode 100644
index 6ddcc24a5..000000000
--- a/grid_map_costmap_2d/rostest/CMakeLists.txt
+++ /dev/null
@@ -1,25 +0,0 @@
-##############################################################################
-# Unit Tests
-#
-#   Only run when CATKIN_ENABLE_TESTING is true.
-##############################################################################
-
-find_package(rostest REQUIRED)
-
-##############################################################################
-# Macro
-##############################################################################
-
-macro(${PROJECT_NAME}_add_rostest test_name)
-  add_rostest_gtest(test_${test_name} ${test_name}/${test_name}.test ${test_name}/${test_name}.cpp)
-  if (TARGET test_${test_name})
-    add_dependencies(test_${test_name} ${catkin_EXPORTED_TARGETS})
-    target_link_libraries(test_${test_name} ${catkin_LIBRARIES})
-  endif()
-endmacro()
-
-##############################################################################
-# Tests
-##############################################################################
-
-grid_map_costmap_2d_add_rostest(costmap_2d_ros)
diff --git a/grid_map_costmap_2d/rostest/Readme.md b/grid_map_costmap_2d/rostest/Readme.md
deleted file mode 100644
index 9a1f1c28e..000000000
--- a/grid_map_costmap_2d/rostest/Readme.md
+++ /dev/null
@@ -1,10 +0,0 @@
-
-I can never remember how to run them...
-
-* http://wiki.ros.org/rostest/Commandline
-
-```
-rostest --text mytest.test
-```
-
-
diff --git a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.cpp b/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.cpp
deleted file mode 100644
index 298d87a5d..000000000
--- a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.cpp
+++ /dev/null
@@ -1,268 +0,0 @@
-/*****************************************************************************
-** Includes
-*****************************************************************************/
-
-#include <gtest/gtest.h>
-#include <iostream>
-#include <string>
-
-#include <ros/ros.h>
-#if ROS_VERSION_MINIMUM(1,14,0)
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf/transform_datatypes.h>
-#else
-#include <tf/transform_broadcaster.h>
-#endif
-
-#include "costmap_2d_ros.hpp"
-
-/*****************************************************************************
-** Helpers
-*****************************************************************************/
-
-void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster& broadcaster)
-{
-  broadcaster.add("base_link_5x5", tf::Vector3(1.0, 1.0, 0.0), tf::Quaternion(0, 0, 0, 1));
-  broadcaster.add("base_link_4x4", tf::Vector3(1.0, -3.0, 0.0), tf::Quaternion(0, 0, 0, 1));
-  broadcaster.add("base_link_5x5_3x3_offset", tf::Vector3(-3.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1));
-  broadcaster.add("base_link_5x5_3x3_centre", tf::Vector3(-3.5, -3.5, 0.0), tf::Quaternion(0, 0, 0, 1));
-  broadcaster.add("base_link_5x5_2_5x2_5_offset", tf::Vector3(-9.7, 2.4, 0.0), tf::Quaternion(0, 0, 0, 1));
-  broadcaster.startBroadCastingThread();
-}
-
-/*****************************************************************************
-** ROS Costmap Server
-*****************************************************************************/
-
-ROSCostmapServer::ROSCostmapServer(const std::string& name,
-                                   const std::string& baseLinkTransformName,
-                                   const grid_map::Position& origin, const double& width,
-                                   const double& height)
-#if ROS_VERSION_MINIMUM(1,14,0)
-    : tfBuffer_(ros::Duration(1.0)),
-      tfListener_(tfBuffer_)
-#else
-    : tfListener_(ros::Duration(1.0))
-#endif
-{
-  ros::NodeHandle privateNodeHandle("~");
-  // lots of parameters here affect the construction ( e.g. rolling window)
-  // if you don't have any parameters set, then this
-  //  - alot of defaults which get dumped on the ros param server
-  //  - fires up an obstacle layer and an inflation layer
-  //  - creates a publisher for an occupancy grid
-  privateNodeHandle.setParam(name + "/robot_base_frame", baseLinkTransformName);
-  privateNodeHandle.setParam(name + "/origin_x", origin.x());
-  privateNodeHandle.setParam(name + "/origin_y", origin.y());
-  privateNodeHandle.setParam(name + "/width", width);
-  privateNodeHandle.setParam(name + "/height", height);
-  privateNodeHandle.setParam(name + "/plugins", std::vector<std::string>());
-  privateNodeHandle.setParam(name + "/resolution", 0.5);
-  privateNodeHandle.setParam(name + "/robot_radius", 0.03); // clears 1 cell if inside, up to 4 cells on a vertex
-#if ROS_VERSION_MINIMUM(1,14,0)
-  costmap_ = std::make_shared<ROSCostmap>(name, tfBuffer_);
-#else
-  costmap_ = std::make_shared<ROSCostmap>(name, tfListener_);
-#endif
-
-  for ( unsigned int index = 0; index < costmap_->getCostmap()->getSizeInCellsY(); ++index ) {
-    unsigned int dimension = costmap_->getCostmap()->getSizeInCellsX();
-    // @todo assert dimension > 1
-    // set the first row to costmap_2d::FREE_SPACE? but it shows up invisibly in rviz, so don't bother
-    for ( unsigned int fill_index = 0; fill_index < dimension - 2; ++fill_index )
-    {
-      double fraction = static_cast<double>(fill_index + 1) / static_cast<double>(costmap_->getCostmap()->getSizeInCellsX());
-      costmap_->getCostmap()->setCost(fill_index, index, fraction*costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
-    }
-    costmap_->getCostmap()->setCost(dimension - 2, index, costmap_2d::LETHAL_OBSTACLE);
-    costmap_->getCostmap()->setCost(dimension - 1, index, costmap_2d::NO_INFORMATION);
-  }
-}
-
-/*****************************************************************************
-** TransformBroadcaster
-*****************************************************************************/
-
-TransformBroadcaster::~TransformBroadcaster()
-{
-  broadcastingThread_.join();
-}
-
-void TransformBroadcaster::shutdown()
-{
-  shutdownFlag_ = true;
-}
-
-void TransformBroadcaster::add(const std::string& name, tf::Vector3 origin,
-                               const tf::Quaternion& orientation)
-{
-  tf::Transform transform;
-  transform.setOrigin(origin);
-  transform.setRotation(orientation);
-  transforms_.insert(std::pair<std::string, tf::Transform>(name, transform));
-}
-
-void TransformBroadcaster::startBroadCastingThread() {
-  broadcastingThread_ = std::thread(&TransformBroadcaster::broadcast, this);
-}
-
-void TransformBroadcaster::broadcast()
-{
-#if ROS_VERSION_MINIMUM(1,14,0)
-  tf2_ros::TransformBroadcaster tfBroadcaster;
-#else
-  tf::TransformBroadcaster tfBroadcaster;
-#endif
-  while (ros::ok() && !shutdownFlag_) {
-    for (std::pair<std::string, tf::Transform> p : transforms_) {
-      tf::StampedTransform transform(p.second, ros::Time::now(), "map", p.first);
-#if ROS_VERSION_MINIMUM(1,14,0)
-      geometry_msgs::TransformStamped transformMsg;
-      tf::transformStampedTFToMsg(transform, transformMsg);
-      tfBroadcaster.sendTransform(transformMsg);
-#else
-      tfBroadcaster.sendTransform(transform);
-#endif
-    }
-    ros::Duration(0.1).sleep();
-  }
-}
-
-/*****************************************************************************
-** Converter Functions
-*****************************************************************************/
-
-bool fromCostmap2DROS(costmap_2d::Costmap2DROS& ros_costmap, const std::string& layer_name,
-                      grid_map::GridMap& grid_map)
-{
-  grid_map::Costmap2DConverter<grid_map::GridMap> converter;
-  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
-  converter.initializeFromCostmap2D(ros_costmap, grid_map);
-  if (!converter.addLayerFromCostmap2D(ros_costmap, layer_name, grid_map)) {
-    return false;
-  }
-  return true;
-}
-
-bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS& ros_costmap,
-                                 const grid_map::Length& geometry, const std::string& layer_name,
-                                 grid_map::GridMap& grid_map)
-{
-  grid_map::Costmap2DConverter<grid_map::GridMap> converter;
-  boost::lock_guard<costmap_2d::Costmap2D::mutex_t> lock(*(ros_costmap.getCostmap()->getMutex()));
-  if (!converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, grid_map)) {
-    return false;
-  }
-  if (!converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, grid_map)) {
-    return false;
-  }
-  return true;
-}
-
-/*****************************************************************************
-** Tests
-*****************************************************************************/
-
-TEST(Costmap2DROSConversion, full_window)
-{
-  std::cout << std::endl;
-  ROS_INFO("***********************************************************");
-  ROS_INFO("                 Copy Full Window");
-  ROS_INFO("***********************************************************");
-  // preparation
-  std::string layer_name =  "obstacle_costs";
-  ROSCostmapServer ros_costmap_5x5("five_by_five", "base_link_5x5", grid_map::Position(0.0, 0.0), 5.0, 5.0);
-  grid_map::GridMap grid_map_5x5;
-  fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), layer_name, grid_map_5x5);
-  // assert map properties
-  ASSERT_EQ(grid_map_5x5.getFrameId(), ros_costmap_5x5.getROSCostmap()->getGlobalFrameID());
-  ASSERT_EQ(
-      grid_map_5x5.getLength().x(),
-      ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX()
-          * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
-  ASSERT_EQ(
-      grid_map_5x5.getLength().y(),
-      ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY()
-          * ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
-  ASSERT_EQ(grid_map_5x5.getSize()[0], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX());
-  ASSERT_EQ(grid_map_5x5.getSize()[1], ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY());
-  grid_map::Length position = grid_map_5x5.getPosition() - 0.5 * grid_map_5x5.getLength().matrix();
-  ASSERT_EQ(position.x(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginX());
-  ASSERT_EQ(position.y(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginY());
-
-  // assert map data
-  for (unsigned int i = 0; i < 5; ++i) {
-    for (unsigned int j = 0; j < 5; ++j) {
-      std::cout << static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(i, j))
-                << " ";
-    }
-    std::cout << std::endl;
-  }
-  for (unsigned int i = 0; i < 5; ++i) {
-    for (unsigned int j = 0; j < 5; ++j) {
-      std::cout << static_cast<int>(grid_map_5x5.at(layer_name, grid_map::Index(i, j))) << " ";
-    }
-    std::cout << std::endl;
-  }
-  // TODO a function which does the index conversion
-
-  std::cout << "Original cost: " << static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0)) << std::endl;
-  std::cout << "New Cost: " << grid_map_5x5.at(layer_name, grid_map::Index(1,9)) << std::endl;
-  std::vector<float> cost_translation_table;
-  grid_map::Costmap2DCenturyTranslationTable::create(cost_translation_table);
-  unsigned char cost = ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8,0);
-  ASSERT_EQ(grid_map_5x5.at(layer_name, grid_map::Index(1,9)), cost_translation_table[cost]);
-  std::cout << std::endl;
-}
-
-TEST(Costmap2DROSConversion, cost_map_centres)
-{
-  std::cout << std::endl;
-  ROS_INFO("***********************************************************");
-  ROS_INFO("                 Check Subwindow Centres");
-  ROS_INFO("***********************************************************");
-  ROS_INFO("Subwindows are centred as closely as possible to the robot");
-  ROS_INFO("pose, though not exactly. They still need to align with");
-  ROS_INFO("the underlying ros costmap so that they don't introduce a");
-  ROS_INFO("new kind of error. As a result, the centre is shifted from");
-  ROS_INFO("the robot pose to the nearest appropriate point which aligns");
-  ROS_INFO("the new cost map exactly on top of the original ros costmap.");
-  std::cout << std::endl;
-  std::string layer_name =  "obstacle_costs";
-  ROSCostmapServer ros_costmap_5x5_3x3_offset("five_by_five_three_by_three_offset", "base_link_5x5_3x3_offset", grid_map::Position(-6.0, 0.0), 5.0, 5.0);
-  ROSCostmapServer ros_costmap_5x5_3x3_centre("five_by_five_three_by_three_centre", "base_link_5x5_3x3_centre", grid_map::Position(-6.0, -6.0), 5.0, 5.0);
-  ROSCostmapServer ros_costmap_5x5_2_5x2_5_offset("five_by_five_twohalf_by_twohalf_offset", "base_link_5x5_2_5x2_5_offset", grid_map::Position(-12.0, 0.0), 5.0, 5.0);
-  grid_map::GridMap grid_map_5x5_3x3_offset, grid_map_5x5_3x3_centre, grid_map_5x5_2_5x2_5_offset;
-  grid_map::Length geometry_3x3(3.0, 3.0);
-  fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_offset.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_offset);
-  fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_3x3_centre.getROSCostmap()), geometry_3x3, layer_name, grid_map_5x5_3x3_centre);
-  grid_map::Length geometry_2_5x2_5(2.5, 2.5);
-  fromCostmap2DROSAtRobotPose(*(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()), geometry_2_5x2_5, layer_name, grid_map_5x5_2_5x2_5_offset);
-  ROS_INFO_STREAM("  grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_offset.getPosition().transpose());
-  ROS_INFO_STREAM("  grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_centre.getPosition().transpose());
-  ROS_INFO_STREAM("  grid_map_5x5_3x3_offset : " << grid_map_5x5_2_5x2_5_offset.getPosition().transpose());
-  ASSERT_EQ(-3.5, grid_map_5x5_3x3_offset.getPosition().x());
-  ASSERT_EQ(2.5, grid_map_5x5_3x3_offset.getPosition().y());
-  ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().x());
-  ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().y());
-  ASSERT_EQ(-9.75, grid_map_5x5_2_5x2_5_offset.getPosition().x());
-  ASSERT_EQ(2.25, grid_map_5x5_2_5x2_5_offset.getPosition().y());
-  std::cout << std::endl;
-}
-
-/*****************************************************************************
-** Main program
-*****************************************************************************/
-
-int main(int argc, char **argv)
-{
-  ros::init(argc, argv, "test_from_ros_costmaps");
-
-  TransformBroadcaster broadcaster;
-  broadcastCostmap2DROSTestSuiteTransforms(broadcaster);
-
-  testing::InitGoogleTest(&argc, argv);
-  int result = RUN_ALL_TESTS();
-  broadcaster.shutdown();
-  return result;
-}
diff --git a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test b/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test
deleted file mode 100644
index a66b74695..000000000
--- a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.test
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
-  <test test-name="test_costmap_2d_ros" pkg="grid_map_costmap_2d" type="test_costmap_2d_ros"/>
-</launch>
diff --git a/grid_map_costmap_2d/test/CMakeLists.txt b/grid_map_costmap_2d/test/CMakeLists.txt
new file mode 100644
index 000000000..31d13e3bf
--- /dev/null
+++ b/grid_map_costmap_2d/test/CMakeLists.txt
@@ -0,0 +1,16 @@
+ament_add_gtest(${PROJECT_NAME}-test
+  test_grid_map_costmap_2d.cpp
+  test_costmap_2d_converter.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}-test
+  ${dependencies}
+)
+
+ament_add_gtest(costmap-2d-ros-test
+  test_costmap_2d_ros.cpp
+)
+
+ament_target_dependencies(costmap-2d-ros-test
+  ${dependencies}
+)
diff --git a/grid_map_costmap_2d/test/Costmap2DConverterTest.cpp b/grid_map_costmap_2d/test/Costmap2DConverterTest.cpp
deleted file mode 100644
index 7ee1a0788..000000000
--- a/grid_map_costmap_2d/test/Costmap2DConverterTest.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/*
- * Costmap2DConverterTest.cpp
- *
- *  Created on: Nov 23, 2016
- *      Authors: Peter Fankhauser, Gabriel Hottiger
- *      Institute: ETH Zurich, ANYbotics
- */
-
-// Grid map
-#include <grid_map_core/grid_map_core.hpp>
-#include <grid_map_costmap_2d/grid_map_costmap_2d.hpp>
-
-// Gtest
-#include <gtest/gtest.h>
-
-// Eigen
-#include <Eigen/Core>
-
-using namespace grid_map;
-
-template <typename ConversionTable>
-class TestCostmap2DConversion : public testing::Test {
- public:
-  using TestValue = std::tuple<Position, unsigned char, double, bool>;
-
-  //! Constructor
-  TestCostmap2DConversion() : costmap2d_(8, 5, 1.0, 2.0, 3.0) { gridMap_.setGeometry(Length(8.0, 5.0), 1.0, Position(6.0, 5.5)); }
-
-  //! Getter of test data
-  std::vector<TestValue> getTestValues();
-
-  //! Check that maps have same geometry
-  void assertMapGeometry(const GridMap& gridMap, const costmap_2d::Costmap2D& costMap) {
-    // Check map info.
-    // Different conventions: Costmap2d returns the *centerpoint* of the last cell in the map.
-    Length length = gridMap.getLength() - Length::Constant(0.5 * gridMap.getResolution());
-    Length position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix();
-    EXPECT_EQ(costMap.getSizeInMetersX(), length.x());
-    EXPECT_EQ(costMap.getSizeInMetersY(), length.y());
-    EXPECT_EQ(costMap.getSizeInCellsX(), gridMap.getSize()[0]);
-    EXPECT_EQ(costMap.getSizeInCellsY(), gridMap.getSize()[1]);
-    EXPECT_EQ(costMap.getResolution(), gridMap.getResolution());
-    EXPECT_EQ(costMap.getOriginX(), position.x());
-    EXPECT_EQ(costMap.getOriginY(), position.y());
-  }
-
- protected:
-  Costmap2DConverter<GridMap, ConversionTable> costmap2dConverter_;
-  costmap_2d::Costmap2D costmap2d_;
-  GridMap gridMap_;
-};
-
-//! Map type that has only mappings for cost map special values.
-using Costmap2DSpecialTranslationTable = Costmap2DTranslationTable<3, 2, 1, 0>;
-//! Map type that has larger intervals between special values.
-using Costmap2DLargeIntervalsTranslationTable = Costmap2DTranslationTable<5, 500, 400, 100>;
-
-// Test data for special translation
-template <>
-std::vector<TestCostmap2DConversion<Costmap2DSpecialTranslationTable>::TestValue>
-TestCostmap2DConversion<Costmap2DSpecialTranslationTable>::getTestValues() {
-  std::vector<TestValue> testValues;
-  testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, 0.f, true));
-  testValues.emplace_back(TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 1.f, true));
-  testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, 2.f, true));
-  testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, 3.f, true));
-  // Check for grid map to costmap only.
-  testValues.emplace_back(TestValue(Position(5.4, 6.8), costmap_2d::FREE_SPACE, -1.f, false));
-  testValues.emplace_back(TestValue(Position(8.5, 4.5), costmap_2d::LETHAL_OBSTACLE, 4.f, false));
-  return testValues;
-}
-
-// Test data for special translation
-template <>
-std::vector<TestCostmap2DConversion<Costmap2DLargeIntervalsTranslationTable>::TestValue>
-TestCostmap2DConversion<Costmap2DLargeIntervalsTranslationTable>::getTestValues() {
-  std::vector<TestValue> testValues;
-  testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, 100.f, true));
-  testValues.emplace_back(TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 400.f, true));
-  testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, 500.f, true));
-  testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, 5.f, true));
-  testValues.emplace_back(TestValue(Position(3.4, 3.8), 253/11, 100.f + 300.f/11.f, true));
-  // Check for grid map to costmap only.
-  testValues.emplace_back(TestValue(Position(5.4, 6.8), costmap_2d::FREE_SPACE, 83.f, false));
-  testValues.emplace_back(TestValue(Position(8.5, 4.5), costmap_2d::LETHAL_OBSTACLE, 504.f, false));
-  testValues.emplace_back(TestValue(Position(4.7, 5.8), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 444.f, false));
-
-  return testValues;
-}
-
-// Test data for direct translation
-template <>
-std::vector<TestCostmap2DConversion<Costmap2DDirectTranslationTable>::TestValue>
-TestCostmap2DConversion<Costmap2DDirectTranslationTable>::getTestValues() {
-  std::vector<TestValue> testValues;
-  testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, costmap_2d::FREE_SPACE, true));
-  testValues.emplace_back(
-      TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true));
-  testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, costmap_2d::LETHAL_OBSTACLE, true));
-  testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, costmap_2d::NO_INFORMATION, true));
-  testValues.emplace_back(TestValue(Position(5.4, 6.8), 1, 1.0, true));
-  testValues.emplace_back(TestValue(Position(8.5, 4.5), 97, 97.0, true));
-  // Check for grid map to costmap only.
-  testValues.emplace_back(TestValue(Position(4.7, 5.8), costmap_2d::FREE_SPACE, -30.f, false));
-  testValues.emplace_back(TestValue(Position(3.4, 3.8), costmap_2d::LETHAL_OBSTACLE, 270.f, false));
-  return testValues;
-}
-
-// Test data for century translation
-template <>
-std::vector<TestCostmap2DConversion<Costmap2DCenturyTranslationTable>::TestValue>
-TestCostmap2DConversion<Costmap2DCenturyTranslationTable>::getTestValues() {
-  std::vector<TestValue> testValues;
-  testValues.emplace_back(TestValue(Position(3.2, 5.1), costmap_2d::FREE_SPACE, 0.f, true));
-  testValues.emplace_back(TestValue(Position(4.2, 4.1), costmap_2d::INSCRIBED_INFLATED_OBSTACLE, 99.f, true));
-  testValues.emplace_back(TestValue(Position(6.2, 3.1), costmap_2d::LETHAL_OBSTACLE, 100.f, true));
-  testValues.emplace_back(TestValue(Position(5.2, 7.8), costmap_2d::NO_INFORMATION, -1.f, true));
-  testValues.emplace_back(TestValue(Position(5.4, 6.8), 253/11, 99.f/11.f, true));
-  // Check for grid map to costmap only.
-  testValues.emplace_back(TestValue(Position(4.7, 5.8), costmap_2d::FREE_SPACE, -30.f, false));
-  testValues.emplace_back(TestValue(Position(3.4, 3.8), costmap_2d::LETHAL_OBSTACLE, 270.f, false));
-  return testValues;
-}
-
-using TranslationTableTestTypes = ::testing::Types<Costmap2DSpecialTranslationTable, Costmap2DLargeIntervalsTranslationTable,
-                                                   Costmap2DDirectTranslationTable, Costmap2DCenturyTranslationTable>;
-
-TYPED_TEST_CASE(TestCostmap2DConversion, TranslationTableTestTypes);
-
-TYPED_TEST(TestCostmap2DConversion, initializeFromCostmap2d) {
-  // Convert to grid map.
-  GridMap gridMap;
-  this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap);
-  this->assertMapGeometry(gridMap, this->costmap2d_);
-}
-
-TYPED_TEST(TestCostmap2DConversion, initializeFromGridMap) {
-  // Convert to Costmap2D.
-  costmap_2d::Costmap2D costmap2d;
-  this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap2d);
-  this->assertMapGeometry(this->gridMap_, costmap2d);
-}
-
-TYPED_TEST(TestCostmap2DConversion, addLayerFromCostmap2d) {
-  // Create grid map.
-  const std::string layer("layer");
-  GridMap gridMap;
-  this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap);
-
-  // Fill in test data to Costmap2D.
-  for (const auto& testValue : this->getTestValues()) {
-    if (std::get<3>(testValue)) {
-      unsigned int xIndex, yIndex;
-      ASSERT_TRUE(this->costmap2d_.worldToMap(std::get<0>(testValue).x(), std::get<0>(testValue).y(), xIndex, yIndex));
-      this->costmap2d_.getCharMap()[this->costmap2d_.getIndex(xIndex, yIndex)] = std::get<1>(testValue);
-    }
-  }
-
-  // Copy data.
-  this->costmap2dConverter_.addLayerFromCostmap2D(this->costmap2d_, layer, gridMap);
-
-  // Check data.
-  for (const auto& testValue : this->getTestValues()) {
-    if (std::get<3>(testValue)) {
-      EXPECT_EQ(std::get<2>(testValue), gridMap.atPosition(layer, std::get<0>(testValue)));
-    }
-  }
-}
-
-TYPED_TEST(TestCostmap2DConversion, setCostmap2DFromGridMap) {
-  // Create costmap2d.
-  costmap_2d::Costmap2D costmap;
-  this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap);
-
-  // Fill in test data to grid map.
-  const std::string layer("layer");
-  this->gridMap_.add(layer);
-  for (const auto& testValue : this->getTestValues()) {
-    Index index;
-    this->gridMap_.getIndex(std::get<0>(testValue), index);
-    this->gridMap_.get(layer)(index(0), index(1)) = std::get<2>(testValue);
-  }
-
-  // Copy data.
-  this->costmap2dConverter_.setCostmap2DFromGridMap(this->gridMap_, layer, costmap);
-
-  // Check data.
-  for (const auto& testValue : this->getTestValues()) {
-    unsigned int xIndex, yIndex;
-    ASSERT_TRUE(costmap.worldToMap(std::get<0>(testValue).x(), std::get<0>(testValue).y(), xIndex, yIndex));
-    costmap.getCharMap()[costmap.getIndex(xIndex, yIndex)] = std::get<1>(testValue);
-  }
-}
\ No newline at end of file
diff --git a/grid_map_costmap_2d/test/test_costmap_2d_converter.cpp b/grid_map_costmap_2d/test/test_costmap_2d_converter.cpp
new file mode 100644
index 000000000..4adcb4861
--- /dev/null
+++ b/grid_map_costmap_2d/test/test_costmap_2d_converter.cpp
@@ -0,0 +1,355 @@
+/*
+ * test_costmap_2d_converter.cpp
+ *
+ *  Created on: Nov 23, 2016
+ *      Authors: Peter Fankhauser, Gabriel Hottiger
+ *      Institute: ETH Zurich, ANYbotics
+ */
+
+// Gtest
+#include <gtest/gtest.h>
+
+#include <tuple>
+#include <vector>
+#include <string>
+
+// Grid map
+#include "grid_map_core/grid_map_core.hpp"
+#include "grid_map_costmap_2d/grid_map_costmap_2d.hpp"
+
+template<typename ConversionTable>
+class TestCostmap2DConversion
+  : public testing::Test
+{
+public:
+  using TestValue = std::tuple<grid_map::Position, unsigned char, double, bool>;
+
+  //! Constructor
+  TestCostmap2DConversion()
+  : costmap2d_(8, 5, 1.0, 2.0, 3.0)
+  {
+    gridMap_.setGeometry(grid_map::Length(8.0, 5.0), 1.0, grid_map::Position(6.0, 5.5));
+  }
+
+  //! Getter of test data
+  std::vector<TestValue> getTestValues();
+
+  //! Check that maps have same geometry
+  void assertMapGeometry(
+    const grid_map::GridMap & gridMap,
+    const nav2_costmap_2d::Costmap2D & costMap)
+  {
+    // Check map info.
+    // Different conventions: Costmap2d returns the *centerpoint* of the last cell in the map.
+    grid_map::Length length =
+      gridMap.getLength() - grid_map::Length::Constant(0.5 * gridMap.getResolution());
+    grid_map::Length position = gridMap.getPosition() - 0.5 * gridMap.getLength().matrix();
+    EXPECT_EQ(costMap.getSizeInMetersX(), length.x());
+    EXPECT_EQ(costMap.getSizeInMetersY(), length.y());
+    EXPECT_EQ(costMap.getSizeInCellsX(), (unsigned int) gridMap.getSize()[0]);
+    EXPECT_EQ(costMap.getSizeInCellsY(), (unsigned int) gridMap.getSize()[1]);
+    EXPECT_EQ(costMap.getResolution(), gridMap.getResolution());
+    EXPECT_EQ(costMap.getOriginX(), position.x());
+    EXPECT_EQ(costMap.getOriginY(), position.y());
+  }
+
+protected:
+  grid_map::Costmap2DConverter<grid_map::GridMap, ConversionTable> costmap2dConverter_;
+  nav2_costmap_2d::Costmap2D costmap2d_;
+  grid_map::GridMap gridMap_;
+};
+
+//! Map type that has only mappings for cost map special values.
+using Costmap2DSpecialTranslationTable = grid_map::Costmap2DTranslationTable<3, 2, 1, 0>;
+//! Map type that has larger intervals between special values.
+using Costmap2DLargeIntervalsTranslationTable =
+  grid_map::Costmap2DTranslationTable<5, 500, 400, 100>;
+
+// Test data for special translation
+template<>
+std::vector<TestCostmap2DConversion<Costmap2DSpecialTranslationTable>::TestValue>
+TestCostmap2DConversion<Costmap2DSpecialTranslationTable>::getTestValues()
+{
+  std::vector<TestValue> testValues;
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.2, 5.1),
+      nav2_costmap_2d::FREE_SPACE,
+      0.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.2, 4.1),
+      nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+      1.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(6.2, 3.1),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      2.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(5.2, 7.8),
+      nav2_costmap_2d::NO_INFORMATION,
+      3.f,
+      true));
+  // Check for grid map to costmap only.
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(5.4, 6.8),
+      nav2_costmap_2d::FREE_SPACE,
+      -1.f,
+      false));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(8.5, 4.5),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      4.f,
+      false));
+  return testValues;
+}
+
+// Test data for special translation
+template<>
+std::vector<TestCostmap2DConversion<Costmap2DLargeIntervalsTranslationTable>::TestValue>
+TestCostmap2DConversion<Costmap2DLargeIntervalsTranslationTable>::getTestValues()
+{
+  std::vector<TestValue> testValues;
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.2, 5.1),
+      nav2_costmap_2d::FREE_SPACE,
+      100.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.2, 4.1),
+      nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+      400.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(6.2, 3.1),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      500.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(5.2, 7.8),
+      nav2_costmap_2d::NO_INFORMATION,
+      5.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.4, 3.8),
+      253 / 11,
+      100.f + 300.f / 11.f,
+      true));
+  // Check for grid map to costmap only.
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(5.4, 6.8),
+      nav2_costmap_2d::FREE_SPACE,
+      83.f,
+      false));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(8.5, 4.5),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      504.f,
+      false));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.7, 5.8),
+      nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+      444.f,
+      false));
+
+  return testValues;
+}
+
+// Test data for direct translation
+template<>
+std::vector<TestCostmap2DConversion<grid_map::Costmap2DDirectTranslationTable>::TestValue>
+TestCostmap2DConversion<grid_map::Costmap2DDirectTranslationTable>::getTestValues()
+{
+  std::vector<TestValue> testValues;
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.2, 5.1),
+      nav2_costmap_2d::FREE_SPACE,
+      nav2_costmap_2d::FREE_SPACE,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.2, 4.1),
+      nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+      nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(6.2, 3.1),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(5.2, 7.8),
+      nav2_costmap_2d::NO_INFORMATION,
+      nav2_costmap_2d::NO_INFORMATION,
+      true));
+  testValues.emplace_back(TestValue(grid_map::Position(5.4, 6.8), 1, 1.0, true));
+  testValues.emplace_back(TestValue(grid_map::Position(8.5, 4.5), 97, 97.0, true));
+  // Check for grid map to costmap only.
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.7, 5.8),
+      nav2_costmap_2d::FREE_SPACE,
+      -30.f,
+      false));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.4, 3.8),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      270.f,
+      false));
+  return testValues;
+}
+
+// Test data for century translation
+template<>
+std::vector<TestCostmap2DConversion<grid_map::Costmap2DCenturyTranslationTable>::TestValue>
+TestCostmap2DConversion<grid_map::Costmap2DCenturyTranslationTable>::getTestValues()
+{
+  std::vector<TestValue> testValues;
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.2, 5.1),
+      nav2_costmap_2d::FREE_SPACE,
+      0.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.2, 4.1),
+      nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE,
+      99.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(6.2, 3.1),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      100.f,
+      true));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(5.2, 7.8),
+      nav2_costmap_2d::NO_INFORMATION,
+      -1.f,
+      true));
+  testValues.emplace_back(TestValue(grid_map::Position(5.4, 6.8), 253 / 11, 99.f / 11.f, true));
+  // Check for grid map to costmap only.
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(4.7, 5.8),
+      nav2_costmap_2d::FREE_SPACE,
+      -30.f,
+      false));
+  testValues.emplace_back(
+    TestValue(
+      grid_map::Position(3.4, 3.8),
+      nav2_costmap_2d::LETHAL_OBSTACLE,
+      270.f,
+      false));
+  return testValues;
+}
+
+using TranslationTableTestTypes = ::testing::Types<
+  Costmap2DSpecialTranslationTable,
+  Costmap2DLargeIntervalsTranslationTable,
+  grid_map::Costmap2DDirectTranslationTable,
+  grid_map::Costmap2DCenturyTranslationTable>;
+
+TYPED_TEST_CASE(
+  TestCostmap2DConversion, TranslationTableTestTypes, testing::internal::DefaultNameGenerator);
+
+TYPED_TEST(TestCostmap2DConversion, initializeFromCostmap2d)
+{
+  // Convert to grid map.
+  grid_map::GridMap gridMap;
+  this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap);
+  this->assertMapGeometry(gridMap, this->costmap2d_);
+}
+
+TYPED_TEST(TestCostmap2DConversion, initializeFromGridMap)
+{
+  // Convert to Costmap2D.
+  nav2_costmap_2d::Costmap2D costmap2d;
+  this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap2d);
+  this->assertMapGeometry(this->gridMap_, costmap2d);
+}
+
+TYPED_TEST(TestCostmap2DConversion, addLayerFromCostmap2d)
+{
+  // Create grid map.
+  const std::string layer("layer");
+  grid_map::GridMap gridMap;
+  this->costmap2dConverter_.initializeFromCostmap2D(this->costmap2d_, gridMap);
+
+  // Fill in test data to Costmap2D.
+  for (const auto & testValue : this->getTestValues()) {
+    if (std::get<3>(testValue)) {
+      unsigned int xIndex, yIndex;
+      ASSERT_TRUE(
+        this->costmap2d_.worldToMap(
+          std::get<0>(testValue).x(),
+          std::get<0>(testValue).y(),
+          xIndex,
+          yIndex));
+      this->costmap2d_.getCharMap()[this->costmap2d_.getIndex(xIndex, yIndex)] =
+        std::get<1>(testValue);
+    }
+  }
+
+  // Copy data.
+  this->costmap2dConverter_.addLayerFromCostmap2D(this->costmap2d_, layer, gridMap);
+
+  // Check data.
+  for (const auto & testValue : this->getTestValues()) {
+    if (std::get<3>(testValue)) {
+      EXPECT_EQ(std::get<2>(testValue), gridMap.atPosition(layer, std::get<0>(testValue)));
+    }
+  }
+}
+
+TYPED_TEST(TestCostmap2DConversion, setCostmap2DFromGridMap)
+{
+  // Create costmap2d.
+  nav2_costmap_2d::Costmap2D costmap;
+  this->costmap2dConverter_.initializeFromGridMap(this->gridMap_, costmap);
+
+  // Fill in test data to grid map.
+  const std::string layer("layer");
+  this->gridMap_.add(layer);
+  for (const auto & testValue : this->getTestValues()) {
+    grid_map::Index index;
+    this->gridMap_.getIndex(std::get<0>(testValue), index);
+    this->gridMap_.get(layer)(index(0), index(1)) = std::get<2>(testValue);
+  }
+
+  // Copy data.
+  this->costmap2dConverter_.setCostmap2DFromGridMap(this->gridMap_, layer, costmap);
+
+  // Check data.
+  for (const auto & testValue : this->getTestValues()) {
+    unsigned int xIndex, yIndex;
+    ASSERT_TRUE(
+      costmap.worldToMap(
+        std::get<0>(testValue).x(),
+        std::get<0>(testValue).y(),
+        xIndex,
+        yIndex));
+    costmap.getCharMap()[costmap.getIndex(xIndex, yIndex)] = std::get<1>(testValue);
+  }
+}
diff --git a/grid_map_costmap_2d/test/test_costmap_2d_ros.cpp b/grid_map_costmap_2d/test/test_costmap_2d_ros.cpp
new file mode 100644
index 000000000..429213cf5
--- /dev/null
+++ b/grid_map_costmap_2d/test/test_costmap_2d_ros.cpp
@@ -0,0 +1,340 @@
+/*****************************************************************************
+** Includes
+*****************************************************************************/
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2/transform_datatypes.h"
+
+#include "test_costmap_2d_ros.hpp"
+
+/*****************************************************************************
+** Global rclcpp::Node
+*****************************************************************************/
+std::shared_ptr<rclcpp::Node> node_ = nullptr;
+
+/*****************************************************************************
+** Helpers
+*****************************************************************************/
+
+void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster & broadcaster)
+{
+  broadcaster.add(
+    "base_link_5x5",
+    tf2::Vector3(1.0, 1.0, 0.0),
+    tf2::Quaternion(0, 0, 0, 1));
+  broadcaster.add(
+    "base_link_4x4",
+    tf2::Vector3(1.0, -3.0, 0.0),
+    tf2::Quaternion(0, 0, 0, 1));
+  broadcaster.add(
+    "base_link_5x5_3x3_offset",
+    tf2::Vector3(-3.7, 2.4, 0.0),
+    tf2::Quaternion(0, 0, 0, 1));
+  broadcaster.add(
+    "base_link_5x5_3x3_centre",
+    tf2::Vector3(-3.5, -3.5, 0.0),
+    tf2::Quaternion(0, 0, 0, 1));
+  broadcaster.add(
+    "base_link_5x5_2_5x2_5_offset",
+    tf2::Vector3(-9.7, 2.4, 0.0),
+    tf2::Quaternion(0, 0, 0, 1));
+  broadcaster.startBroadCastingThread();
+}
+
+/*****************************************************************************
+** ROS Costmap Server
+*****************************************************************************/
+
+ROSCostmapServer::ROSCostmapServer(
+  const std::string & name,
+  const std::string & baseLinkTransformName,
+  const grid_map::Position & origin,
+  const int & width,
+  const int & height)
+: tfBuffer_(node_->get_clock()),
+  tfListener_(tfBuffer_)
+{
+  // lots of parameters here affect the construction ( e.g. rolling window)
+  // if you don't have any parameters set, then this
+  //  - alot of defaults which get dumped on the ros param server
+  //  - fires up an obstacle layer and an inflation layer
+  //  - creates a publisher for an occupancy grid
+
+  costmap_ = std::make_shared<ROSCostmap>(name);
+  costmap_->set_parameter(rclcpp::Parameter{"plugin_names", std::vector<std::string>()});
+  costmap_->set_parameter(rclcpp::Parameter{"plugin_types", std::vector<std::string>()});
+  costmap_->set_parameter(rclcpp::Parameter{"robot_base_frame", baseLinkTransformName});
+  costmap_->set_parameter(rclcpp::Parameter{"origin_x", origin.x()});
+  costmap_->set_parameter(rclcpp::Parameter{"origin_y", origin.y()});
+  costmap_->set_parameter(rclcpp::Parameter{"width", width});
+  costmap_->set_parameter(rclcpp::Parameter{"height", height});
+  costmap_->set_parameter(rclcpp::Parameter{"resolution", 0.5});
+  // clears 1 cell if inside, up to 4 cells on a vertex
+  costmap_->set_parameter(rclcpp::Parameter{"robot_radius", 0.03});
+
+  costmap_->configure();
+  costmap_->activate();
+
+  for (unsigned int index = 0; index < costmap_->getCostmap()->getSizeInCellsY(); ++index) {
+    unsigned int dimension = costmap_->getCostmap()->getSizeInCellsX();
+    // @todo assert dimension > 1
+    // set the first row to nav2_costmap_2d::FREE_SPACE?
+    // but it shows up invisibly in rviz, so don't bother
+    for (unsigned int fill_index = 0; fill_index < dimension - 2; ++fill_index) {
+      double fraction = static_cast<double>(fill_index + 1) /
+        static_cast<double>(costmap_->getCostmap()->getSizeInCellsX());
+      costmap_->getCostmap()->setCost(
+        fill_index,
+        index,
+        fraction * nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+    }
+    costmap_->getCostmap()->setCost(dimension - 2, index, nav2_costmap_2d::LETHAL_OBSTACLE);
+    costmap_->getCostmap()->setCost(dimension - 1, index, nav2_costmap_2d::NO_INFORMATION);
+  }
+}
+
+/*****************************************************************************
+** TransformBroadcaster
+*****************************************************************************/
+
+TransformBroadcaster::~TransformBroadcaster()
+{
+  broadcastingThread_.join();
+}
+
+void TransformBroadcaster::shutdown()
+{
+  shutdownFlag_ = true;
+}
+
+void TransformBroadcaster::add(
+  const std::string & name,
+  tf2::Vector3 origin,
+  const tf2::Quaternion & orientation)
+{
+  geometry_msgs::msg::Transform transform;
+  transform.translation.x = origin.x();
+  transform.translation.y = origin.y();
+  transform.translation.z = origin.z();
+  transform.rotation.x = orientation.x();
+  transform.rotation.y = orientation.y();
+  transform.rotation.z = orientation.z();
+  transform.rotation.w = orientation.w();
+  transforms_.insert(std::pair<std::string, geometry_msgs::msg::Transform>(name, transform));
+}
+
+void TransformBroadcaster::startBroadCastingThread()
+{
+  broadcastingThread_ = std::thread(&TransformBroadcaster::broadcast, this);
+}
+
+void TransformBroadcaster::broadcast()
+{
+  while (rclcpp::ok() && !shutdownFlag_) {
+    for (std::pair<std::string, geometry_msgs::msg::Transform> p : transforms_) {
+      geometry_msgs::msg::TransformStamped transformMsg;
+      transformMsg.header.stamp = node_->now();
+      transformMsg.header.frame_id = "map";
+      transformMsg.child_frame_id = p.first;
+      transformMsg.transform = p.second;
+      tfBroadcaster_.sendTransform(transformMsg);
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+}
+
+/*****************************************************************************
+** Converter Functions
+*****************************************************************************/
+
+bool fromCostmap2DROS(
+  nav2_costmap_2d::Costmap2DROS & ros_costmap,
+  const std::string & layer_name,
+  grid_map::GridMap & grid_map)
+{
+  grid_map::Costmap2DConverter<grid_map::GridMap> converter;
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(
+    *(ros_costmap.getCostmap()->getMutex()));
+  converter.initializeFromCostmap2D(ros_costmap, grid_map);
+  return converter.addLayerFromCostmap2D(ros_costmap, layer_name, grid_map);
+}
+
+bool fromCostmap2DROSAtRobotPose(
+  nav2_costmap_2d::Costmap2DROS & ros_costmap,
+  const grid_map::Length & geometry,
+  const std::string & layer_name,
+  grid_map::GridMap & grid_map)
+{
+  grid_map::Costmap2DConverter<grid_map::GridMap> converter;
+  std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(
+    *(ros_costmap.getCostmap()->getMutex()));
+  if (!converter.initializeFromCostmap2DAtRobotPose(ros_costmap, geometry, grid_map)) {
+    return false;
+  }
+  return converter.addLayerFromCostmap2DAtRobotPose(ros_costmap, layer_name, grid_map);
+}
+
+/*****************************************************************************
+** Tests
+*****************************************************************************/
+
+TEST(Costmap2DROSConversion, full_window)
+{
+  std::cout << std::endl;
+  RCLCPP_INFO(node_->get_logger(), "***********************************************************");
+  RCLCPP_INFO(node_->get_logger(), "                 Copy Full Window");
+  RCLCPP_INFO(node_->get_logger(), "***********************************************************");
+  // preparation
+  std::string layer_name = "obstacle_costs";
+  ROSCostmapServer
+    ros_costmap_5x5("five_by_five", "base_link_5x5", grid_map::Position(0.0, 0.0), 5.0, 5.0);
+  grid_map::GridMap grid_map_5x5;
+  fromCostmap2DROS(*(ros_costmap_5x5.getROSCostmap()), layer_name, grid_map_5x5);
+  // assert map properties
+  ASSERT_EQ(grid_map_5x5.getFrameId(), ros_costmap_5x5.getROSCostmap()->getGlobalFrameID());
+  ASSERT_EQ(
+    grid_map_5x5.getLength().x(),
+    ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX() *
+    ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
+  ASSERT_EQ(
+    grid_map_5x5.getLength().y(),
+    ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY() *
+    ros_costmap_5x5.getROSCostmap()->getCostmap()->getResolution());
+  ASSERT_EQ(
+    (unsigned int) grid_map_5x5.getSize()[0],
+    ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsX());
+  ASSERT_EQ(
+    (unsigned int) grid_map_5x5.getSize()[1],
+    ros_costmap_5x5.getROSCostmap()->getCostmap()->getSizeInCellsY());
+  grid_map::Length position = grid_map_5x5.getPosition() - 0.5 * grid_map_5x5.getLength().matrix();
+  ASSERT_EQ(position.x(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginX());
+  ASSERT_EQ(position.y(), ros_costmap_5x5.getROSCostmap()->getCostmap()->getOriginY());
+
+  // assert map data
+  for (unsigned int i = 0; i < 5; ++i) {
+    for (unsigned int j = 0; j < 5; ++j) {
+      std::cout <<
+        static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(i, j)) << " ";
+    }
+    std::cout << std::endl;
+  }
+  for (unsigned int i = 0; i < 5; ++i) {
+    for (unsigned int j = 0; j < 5; ++j) {
+      std::cout << static_cast<int>(grid_map_5x5.at(layer_name, grid_map::Index(i, j))) << " ";
+    }
+    std::cout << std::endl;
+  }
+  // TODO(tbd): a function which does the index conversion
+
+  std::cout << "Original cost: " <<
+    static_cast<int>(ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8, 0)) <<
+    std::endl;
+  std::cout << "New Cost: " << grid_map_5x5.at(layer_name, grid_map::Index(1, 9)) << std::endl;
+  std::vector<float> cost_translation_table;
+  grid_map::Costmap2DCenturyTranslationTable::create(cost_translation_table);
+  unsigned char cost = ros_costmap_5x5.getROSCostmap()->getCostmap()->getCost(8, 0);
+  ASSERT_EQ(grid_map_5x5.at(layer_name, grid_map::Index(1, 9)), cost_translation_table[cost]);
+  std::cout << std::endl;
+}
+
+TEST(Costmap2DROSConversion, cost_map_centres)
+{
+  std::cout << std::endl;
+  RCLCPP_INFO(node_->get_logger(), "***********************************************************");
+  RCLCPP_INFO(node_->get_logger(), "                 Check Subwindow Centres");
+  RCLCPP_INFO(node_->get_logger(), "***********************************************************");
+  RCLCPP_INFO(node_->get_logger(), "Subwindows are centred as closely as possible to the robot");
+  RCLCPP_INFO(node_->get_logger(), "pose, though not exactly. They still need to align with");
+  RCLCPP_INFO(node_->get_logger(), "the underlying ros costmap so that they don't introduce a");
+  RCLCPP_INFO(node_->get_logger(), "new kind of error. As a result, the centre is shifted from");
+  RCLCPP_INFO(node_->get_logger(), "the robot pose to the nearest appropriate point which aligns");
+  RCLCPP_INFO(node_->get_logger(), "the new cost map exactly on top of the original ros costmap.");
+  std::cout << std::endl;
+  std::string layer_name = "obstacle_costs";
+  ROSCostmapServer ros_costmap_5x5_3x3_offset(
+    "five_by_five_three_by_three_offset",
+    "base_link_5x5_3x3_offset",
+    grid_map::Position(-6.0, 0.0),
+    5,
+    5);
+  ROSCostmapServer ros_costmap_5x5_3x3_centre(
+    "five_by_five_three_by_three_centre",
+    "base_link_5x5_3x3_centre",
+    grid_map::Position(-6.0, -6.0),
+    5,
+    5);
+  ROSCostmapServer ros_costmap_5x5_2_5x2_5_offset(
+    "five_by_five_twohalf_by_twohalf_offset",
+    "base_link_5x5_2_5x2_5_offset",
+    grid_map::Position(-12.0, 0.0),
+    5,
+    5);
+  grid_map::GridMap grid_map_5x5_3x3_offset, grid_map_5x5_3x3_centre, grid_map_5x5_2_5x2_5_offset;
+  grid_map::Length geometry_3x3(3.0, 3.0);
+  fromCostmap2DROSAtRobotPose(
+    *(ros_costmap_5x5_3x3_offset.getROSCostmap()),
+    geometry_3x3,
+    layer_name,
+    grid_map_5x5_3x3_offset);
+  fromCostmap2DROSAtRobotPose(
+    *(ros_costmap_5x5_3x3_centre.getROSCostmap()),
+    geometry_3x3,
+    layer_name,
+    grid_map_5x5_3x3_centre);
+  grid_map::Length geometry_2_5x2_5(2.5, 2.5);
+  fromCostmap2DROSAtRobotPose(
+    *(ros_costmap_5x5_2_5x2_5_offset.getROSCostmap()),
+    geometry_2_5x2_5,
+    layer_name,
+    grid_map_5x5_2_5x2_5_offset);
+  RCLCPP_INFO_STREAM(
+    node_->get_logger(),
+    "  grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_offset.getPosition().transpose());
+  RCLCPP_INFO_STREAM(
+    node_->get_logger(),
+    "  grid_map_5x5_3x3_offset : " << grid_map_5x5_3x3_centre.getPosition().transpose());
+  RCLCPP_INFO_STREAM(
+    node_->get_logger(),
+    "  grid_map_5x5_3x3_offset : " << grid_map_5x5_2_5x2_5_offset.getPosition().transpose());
+  ASSERT_EQ(-3.5, grid_map_5x5_3x3_offset.getPosition().x());
+  ASSERT_EQ(2.5, grid_map_5x5_3x3_offset.getPosition().y());
+  ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().x());
+  ASSERT_EQ(-3.5, grid_map_5x5_3x3_centre.getPosition().y());
+  ASSERT_EQ(-9.75, grid_map_5x5_2_5x2_5_offset.getPosition().x());
+  ASSERT_EQ(2.25, grid_map_5x5_2_5x2_5_offset.getPosition().y());
+  std::cout << std::endl;
+}
+
+/*****************************************************************************
+** Main program
+*****************************************************************************/
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  node_ = std::make_shared<rclcpp::Node>("test_from_ros_costmaps");
+
+  TransformBroadcaster broadcaster(node_);
+  broadcastCostmap2DROSTestSuiteTransforms(broadcaster);
+
+  testing::InitGoogleTest(&argc, argv);
+
+  int test_result = RUN_ALL_TESTS();
+  RCLCPP_INFO(node_->get_logger(), "gtest return value: %d", test_result);
+
+  broadcaster.shutdown();
+  rclcpp::shutdown();
+
+  node_.reset();
+
+  return test_result;
+}
diff --git a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.hpp b/grid_map_costmap_2d/test/test_costmap_2d_ros.hpp
similarity index 54%
rename from grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.hpp
rename to grid_map_costmap_2d/test/test_costmap_2d_ros.hpp
index 34a01259b..86e58cbb9 100644
--- a/grid_map_costmap_2d/rostest/costmap_2d_ros/costmap_2d_ros.hpp
+++ b/grid_map_costmap_2d/test/test_costmap_2d_ros.hpp
@@ -1,22 +1,28 @@
-#pragma once
+#ifndef TEST_COSTMAP_2D_ROS_HPP_
+#define TEST_COSTMAP_2D_ROS_HPP_
 
 /*****************************************************************************
 ** Includes
 *****************************************************************************/
 
-#include <costmap_2d/costmap_2d_ros.h>
-#include <grid_map_core/grid_map_core.hpp>
-#include <grid_map_costmap_2d/grid_map_costmap_2d.hpp>
-#if ROS_VERSION_MINIMUM(1,14,0)
-#include <tf2_ros/transform_listener.h>
-#else
-#include <tf/transform_listener.h>
-#endif
-
 #include <atomic>
 #include <map>
 #include <string>
 #include <thread>
+#include <memory>
+#include <mutex>
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "tf2_ros/transform_listener.h"
+#include "geometry_msgs/msg/transform.hpp"
+#include "geometry_msgs/msg/vector3.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "grid_map_core/grid_map_core.hpp"
+#include "grid_map_costmap_2d/grid_map_costmap_2d.hpp"
 
 /*****************************************************************************
 ** Transforms
@@ -25,20 +31,28 @@
 /**
  * @brief Broadcast a set of transforms useful for various demos.
  */
-class TransformBroadcaster {
+class TransformBroadcaster
+{
 public:
-  TransformBroadcaster() : shutdownFlag_(false) {}
+  explicit TransformBroadcaster(std::shared_ptr<rclcpp::Node> & node)
+  : shutdownFlag_(false),
+    tfBroadcaster_(*node)
+  {}
   virtual ~TransformBroadcaster();
-  void add(const std::string& name, tf::Vector3 origin, const tf::Quaternion& orientation);
+  void add(
+    const std::string & name,
+    tf2::Vector3 origin,
+    const tf2::Quaternion & orientation);
 
   void startBroadCastingThread();
   void broadcast();
   void shutdown();
 
 private:
-  std::map<std::string, tf::Transform> transforms_;
+  std::map<std::string, geometry_msgs::msg::Transform> transforms_;
   std::thread broadcastingThread_;
   std::atomic<bool> shutdownFlag_;
+  tf2_ros::TransformBroadcaster tfBroadcaster_;
 };
 
 /**
@@ -51,24 +65,34 @@ class TransformBroadcaster {
  * - second last stripe is filled with LETHAL_OBSTACLE cost
  * - last stripe is filled with NO_INFORMATION cost
  */
-class ROSCostmapServer {
+class ROSCostmapServer
+{
 public:
-  typedef costmap_2d::Costmap2DROS ROSCostmap;
+  typedef nav2_costmap_2d::Costmap2DROS ROSCostmap;
   typedef std::shared_ptr<ROSCostmap> ROSCostmapPtr;
 
-  ROSCostmapServer(const std::string& name, const std::string& baseLinkTransformName,
-                   const grid_map::Position& origin, const double& width, const double& height);
+  ROSCostmapServer(
+    const std::string & name,
+    const std::string & baseLinkTransformName,
+    const grid_map::Position & origin,
+    const int & width,
+    const int & height);
+
+  ~ROSCostmapServer()
+  {
+    costmap_->deactivate();
+    costmap_->shutdown();
+  }
 
-  ROSCostmapPtr getROSCostmap() { return costmap_; };
+  ROSCostmapPtr getROSCostmap()
+  {
+    return costmap_;
+  }
 
 private:
   ROSCostmapPtr costmap_;
-#if ROS_VERSION_MINIMUM(1,14,0)
   tf2_ros::Buffer tfBuffer_;
   tf2_ros::TransformListener tfListener_;
-#else
-  tf::TransformListener tfListener_;
-#endif
 };
 
 
@@ -82,5 +106,6 @@ class ROSCostmapServer {
  *
  * @param[in] broadcaster : uninitialised broadcaster object
  */
-void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster& broadcaster);
+void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster & broadcaster);
 
+#endif  // TEST_COSTMAP_2D_ROS_HPP_
diff --git a/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp b/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp
index 862017d3b..bb44eebdd 100644
--- a/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp
+++ b/grid_map_costmap_2d/test/test_grid_map_costmap_2d.cpp
@@ -1,5 +1,5 @@
 /*
- * test_grid_map.cpp
+ * test_grid_map_costmap_2d.cpp
  *
  *  Created on: Feb 10, 2014
  *      Author: Péter Fankhauser
@@ -10,9 +10,9 @@
 #include <gtest/gtest.h>
 
 // Run all the tests that were declared with TEST()
-int main(int argc, char **argv)
+int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  srand((int)time(0));
+  srand(static_cast<unsigned int>(time(nullptr)));
   return RUN_ALL_TESTS();
 }
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
new file mode 100644
index 000000000..6f6448bc0
--- /dev/null
+++ b/tools/ros2_dependencies.repos
@@ -0,0 +1,5 @@
+repositories:
+  ros-planning/navigation2:
+    type: git
+    url: https://github.com/ros-planning/navigation2.git
+    version: master