diff --git a/nav2_bringup/launch/nav2_bringup_launch.py b/nav2_bringup/launch/nav2_bringup_launch.py
index 884832d134c562864f7709a5f4aa8805980f2e0a..0ef4338565a3611826a99c001bec64241dbf2598 100644
--- a/nav2_bringup/launch/nav2_bringup_launch.py
+++ b/nav2_bringup/launch/nav2_bringup_launch.py
@@ -90,12 +90,6 @@ def generate_launch_description():
         output='screen',
         parameters=[configured_params])
 
-    start_world_model_cmd = launch_ros.actions.Node(
-        package='nav2_world_model',
-        node_executable='world_model',
-        output='screen',
-        parameters=[configured_params])
-
     start_dwb_cmd = launch_ros.actions.Node(
         package='dwb_controller',
         node_executable='dwb_controller',
@@ -147,7 +141,6 @@ def generate_launch_description():
     ld.add_action(start_lifecycle_manager_cmd)
     ld.add_action(start_map_server_cmd)
     ld.add_action(start_localizer_cmd)
-    ld.add_action(start_world_model_cmd)
     ld.add_action(start_dwb_cmd)
     ld.add_action(start_planner_cmd)
     ld.add_action(start_recovery_cmd)
diff --git a/nav2_bringup/launch/nav2_navigation_launch.py b/nav2_bringup/launch/nav2_navigation_launch.py
index 9e4da4b9cf285d82a0f383e42d9b6b003d133e7f..bbd4bcfbcdacdd5d638dfc51d5392c50824b60a4 100644
--- a/nav2_bringup/launch/nav2_navigation_launch.py
+++ b/nav2_bringup/launch/nav2_navigation_launch.py
@@ -68,12 +68,6 @@ def generate_launch_description():
                 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
             description='Full path to the behavior tree xml file to use'),
 
-        launch_ros.actions.Node(
-            package='nav2_world_model',
-            node_executable='world_model',
-            output='screen',
-            parameters=[configured_params]),
-
         launch_ros.actions.Node(
             package='dwb_controller',
             node_executable='dwb_controller',
@@ -108,8 +102,7 @@ def generate_launch_description():
             output='screen',
             parameters=[{'use_sim_time': use_sim_time},
                         {'autostart': autostart},
-                        {'node_names': ['world_model',
-                                        'dwb_controller',
+                        {'node_names': ['dwb_controller',
                                         'navfn_planner',
                                         'bt_navigator']}]),
 
diff --git a/nav2_bringup/launch/nav2_params.yaml b/nav2_bringup/launch/nav2_params.yaml
index d95d4cfa32467de3cc17ea0c77c72aef98314c3a..2a7941f92a65df00c57698066ef396ee518290b2 100644
--- a/nav2_bringup/launch/nav2_params.yaml
+++ b/nav2_bringup/launch/nav2_params.yaml
@@ -148,8 +148,7 @@ lifecycle_manager:
   ros__parameters:
     use_sim_time: True
     autostart: True
-    node_names: ['map_server', 'amcl',
-                 'world_model', 'dwb_controller',
+    node_names: ['map_server', 'amcl', 'dwb_controller',
                  'navfn_planner', 'bt_navigator']
 
 lifecycle_manager_service_client:
@@ -173,7 +172,3 @@ navfn_planner_GetCostmap_client:
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True
-
-world_model:
-  ros__parameters:
-    use_sim_time: True
diff --git a/nav2_bringup/launch/nav2_simulation_launch.py b/nav2_bringup/launch/nav2_simulation_launch.py
index 543e714f3e4201fa26a45c24e46c8668ad477002..471e34124e646284ff66567a8b46835748331005 100644
--- a/nav2_bringup/launch/nav2_simulation_launch.py
+++ b/nav2_bringup/launch/nav2_simulation_launch.py
@@ -145,11 +145,11 @@ def generate_launch_description():
             ['__params:=', configured_params]],
         cwd=[launch_dir], output='screen')
 
-    start_world_model_cmd = launch.actions.ExecuteProcess(
+    start_planner_cmd = launch.actions.ExecuteProcess(
         cmd=[
             os.path.join(
-                get_package_prefix('nav2_world_model'),
-                'lib/nav2_world_model/world_model'),
+                get_package_prefix('nav2_navfn_planner'),
+                'lib/nav2_navfn_planner/navfn_planner'),
             ['__params:=', configured_params]],
         cwd=[launch_dir], output='screen')
 
@@ -161,14 +161,6 @@ def generate_launch_description():
             ['__params:=', configured_params]],
         cwd=[launch_dir], output='screen')
 
-    start_planner_cmd = launch.actions.ExecuteProcess(
-        cmd=[
-            os.path.join(
-                get_package_prefix('nav2_navfn_planner'),
-                'lib/nav2_navfn_planner/navfn_planner'),
-            ['__params:=', configured_params]],
-        cwd=[launch_dir], output='screen')
-
     start_navigator_cmd = launch.actions.ExecuteProcess(
         cmd=[
             os.path.join(
@@ -222,7 +214,6 @@ def generate_launch_description():
     ld.add_action(start_lifecycle_manager_cmd)
     ld.add_action(start_map_server_cmd)
     ld.add_action(start_localizer_cmd)
-    ld.add_action(start_world_model_cmd)
     ld.add_action(start_dwb_cmd)
     ld.add_action(start_planner_cmd)
     ld.add_action(start_navigator_cmd)
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
index 36baa006fe555dea8abcabfa7a1072b5245d068e..f0f8f530d6aabd9cb4cf43c94da80371493d467a 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
@@ -3,6 +3,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2019, Samsung Research America, Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -46,8 +47,12 @@
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "map_msgs/msg/occupancy_grid_update.hpp"
 #include "nav2_msgs/msg/costmap.hpp"
+#include "nav2_msgs/srv/get_costmap.hpp"
+#include "nav2_msgs/msg/costmap.hpp"
 #include "tf2/transform_datatypes.h"
 #include "nav2_util/lifecycle_node.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 namespace nav2_costmap_2d
 {
@@ -119,6 +124,12 @@ private:
   /** @brief Publish the latest full costmap to the new subscriber. */
   // void onNewSubscription(const ros::SingleSubscriberPublisher& pub);
 
+  /** @brief GetCostmap callback service */
+  void costmap_service_callback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response);
+
   nav2_util::LifecycleNode::SharedPtr node_;
   Costmap2D * costmap_;
   std::string global_frame_;
@@ -133,9 +144,13 @@ private:
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
   rclcpp_lifecycle::LifecyclePublisher<map_msgs::msg::OccupancyGridUpdate>::SharedPtr
     costmap_update_pub_;
+
   // Publisher for raw costmap values as msg::Costmap from layered costmap
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
 
+  // Service for getting the costmaps
+  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
+
   nav_msgs::msg::OccupancyGrid grid_;
   nav2_msgs::msg::Costmap costmap_raw_;
   // Translate from 0-255 values in costmap to -1 to 100 values in message.
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
index d5612f2f332474f027649fae544f614ac020f33a..6ce5b043fca3525b79685ceea546f43f39a167a5 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -109,6 +109,7 @@ private:
   unsigned char lethal_threshold_;
   unsigned char unknown_cost_value_;
   bool trinary_costmap_;
+  bool map_received_;
 };
 
 }  // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 04a9512dd690b1e514d321adee9508801a4037dd..8738e50c54ca9bb7d37d8096edf8084ccb048d9a 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -137,6 +137,7 @@ StaticLayer::getParameters()
 
   // Enforce bounds
   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
+  map_received_ = false;
 }
 
 void
@@ -239,6 +240,9 @@ StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
 {
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   processMap(*new_map);
+  if (!map_received_) {
+    map_received_ = true;
+  }
 }
 
 void
@@ -291,6 +295,10 @@ StaticLayer::updateBounds(
   double * max_x,
   double * max_y)
 {
+  if (!map_received_) {
+    return;
+  }
+
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
   if (!layered_costmap_->isRolling() ) {
     if (!(has_updated_data_ || has_extra_bounds_)) {
@@ -318,7 +326,7 @@ StaticLayer::updateCosts(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j)
 {
-  if (!enabled_) {
+  if (!enabled_ || !map_received_) {
     return;
   }
 
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index abc5ccb7b442155345c28bf4beefa27727243864..1eeedef3ce7704cf06244ec477ff3ec7b9d2eeeb 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -3,6 +3,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2019, Samsung Research America, Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -38,6 +39,7 @@
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
 
 #include <string>
+#include <memory>
 
 #include "nav2_costmap_2d/cost_values.hpp"
 
@@ -64,6 +66,12 @@ Costmap2DPublisher::Costmap2DPublisher(
   costmap_update_pub_ = node_->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
     topic_name + "_updates", custom_qos);
 
+  // Create a service that will use the callback function to handle requests.
+  costmap_service_ = node_->create_service<nav2_msgs::srv::GetCostmap>(
+    "get_costmap", std::bind(&Costmap2DPublisher::costmap_service_callback,
+    this, std::placeholders::_1, std::placeholders::_2,
+    std::placeholders::_3));
+
   if (cost_translation_table_ == NULL) {
     cost_translation_table_ = new char[256];
 
@@ -196,4 +204,38 @@ void Costmap2DPublisher::publishCostmap()
   y0_ = costmap_->getSizeInCellsY();
 }
 
+void
+Costmap2DPublisher::costmap_service_callback(
+  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+  const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request>/*request*/,
+  const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response)
+{
+  RCLCPP_DEBUG(node_->get_logger(), "Received costmap service request");
+
+  // TODO(bpwilcox): Grab correct orientation information
+  tf2::Quaternion quaternion;
+  quaternion.setRPY(0.0, 0.0, 0.0);
+
+  auto size_x = costmap_->getSizeInCellsX();
+  auto size_y = costmap_->getSizeInCellsY();
+  auto data_length = size_x * size_y;
+  unsigned char * data = costmap_->getCharMap();
+  auto current_time = node_->now();
+
+  response->map.header.stamp = current_time;
+  response->map.header.frame_id = global_frame_;
+  response->map.metadata.size_x = size_x;
+  response->map.metadata.size_y = size_y;
+  response->map.metadata.resolution = costmap_->getResolution();
+  response->map.metadata.layer = "Master";
+  response->map.metadata.map_load_time = current_time;
+  response->map.metadata.update_time = current_time;
+  response->map.metadata.origin.position.x = costmap_->getOriginX();
+  response->map.metadata.origin.position.y = costmap_->getOriginY();
+  response->map.metadata.origin.position.z = 0.0;
+  response->map.metadata.origin.orientation = tf2::toMsg(quaternion);
+  response->map.data.resize(data_length);
+  response->map.data.assign(data, data + data_length);
+}
+
 }  // end namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
index 1f4896a277389664e8160b94980ab1e02f80caf3..ffb38e4929a5a5ee432a0c3f8baed96551d144b3 100644
--- a/nav2_costmap_2d/src/costmap_2d_ros.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -201,6 +201,7 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
         &Costmap2DROS::mapUpdateLoop, this, map_update_frequency_));
 
   start();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -229,6 +230,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
+  resetLayers();
   delete layered_costmap_;
   layered_costmap_ = nullptr;
 
@@ -373,7 +375,6 @@ Costmap2DROS::mapUpdateLoop(double frequency)
     timer.end();
 
     RCLCPP_DEBUG(get_logger(), "Map update time: %.9f", timer.elapsed_time_in_seconds());
-
     if (publish_cycle_ > rclcpp::Duration(0) && layered_costmap_->isInitialized()) {
       unsigned int x0, y0, xn, yn;
       layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
@@ -383,7 +384,7 @@ Costmap2DROS::mapUpdateLoop(double frequency)
       if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
         (current_time < last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
       {
-        RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
+        RCLCPP_INFO(get_logger(), "Publish costmap at %s", name_.c_str());
         costmap_publisher_->publishCostmap();
         last_publish_ = current_time;
       }
@@ -415,13 +416,11 @@ Costmap2DROS::updateMap()
       double x = pose.pose.position.x;
       double y = pose.pose.position.y;
       double yaw = tf2::getYaw(pose.pose.orientation);
-
       layered_costmap_->updateMap(x, y, yaw);
 
       geometry_msgs::msg::PolygonStamped footprint;
       footprint.header.frame_id = global_frame_;
       footprint.header.stamp = now();
-
       transformFootprint(x, y, yaw, padded_footprint_, footprint);
 
       RCLCPP_DEBUG(get_logger(), "Publishing footprint");
@@ -435,7 +434,6 @@ void
 Costmap2DROS::start()
 {
   RCLCPP_INFO(get_logger(), "start");
-
   std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
 
   // check if we're stopped or just paused
diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
index e6e56029262c28a43dba1b1bb3a134947ddf5d47..099ba42c70e2817782f7d832a4b70080b796081d 100644
--- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp
+++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp
@@ -37,8 +37,8 @@ LifecycleManager::LifecycleManager()
   RCLCPP_INFO(get_logger(), "Creating");
 
   // The default set of node names for the nav2 stack
-  std::vector<std::string> default_node_names{"map_server", "amcl", "world_model", "dwb_controller",
-    "navfn_planner", "bt_navigator"};
+  std::vector<std::string> default_node_names{"map_server", "amcl",
+    "navfn_planner", "dwb_controller", "bt_navigator"};
 
   // The list of names is parameterized, allowing this module to be used with a different set
   // of nodes
diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt
index fda89d39c8a2f5e0696a3475e8d6372d26bf24ae..2061a6f6d437e000297a776eaf8ff19bd2905b0d 100644
--- a/nav2_msgs/CMakeLists.txt
+++ b/nav2_msgs/CMakeLists.txt
@@ -33,4 +33,4 @@ rosidl_generate_interfaces(${PROJECT_NAME}
 
 ament_export_dependencies(rosidl_default_runtime)
 
-ament_package()
\ No newline at end of file
+ament_package()
diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml
index 303712eba6766a80383c96efa005577709d6684c..1b3c725d47d4cc4cf155e1a07b41f400ae2f157f 100644
--- a/nav2_msgs/package.xml
+++ b/nav2_msgs/package.xml
@@ -12,19 +12,12 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <build_depend>nav2_common</build_depend>
 
-  <build_depend>rclcpp</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>builtin_interfaces</build_depend>
-  <build_depend>rosidl_default_generators</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>action_msgs</build_depend>
-
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>builtin_interfaces</exec_depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>action_msgs</exec_depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>rosidl_default_generators</depend>
+  <depend>geometry_msgs</depend>
+  <depend>action_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt
index bd4fe48c8d315cffba79842f9a497a5a2872e7a2..4949da40807104a06421335e48c9afae3a43f393 100644
--- a/nav2_navfn_planner/CMakeLists.txt
+++ b/nav2_navfn_planner/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
 
 nav2_package()
 
@@ -36,6 +37,7 @@ set(dependencies
   geometry_msgs
   builtin_interfaces
   tf2_ros
+  nav2_costmap_2d
 )
 
 add_library(${library_name} SHARED
@@ -74,5 +76,5 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})
-
+ament_export_dependencies(${dependencies})
 ament_package()
diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
index 0705e38f2321985395321373dc761d571901b8a1..fe318cd34be99e09502c2d08d667d5ca00182422 100644
--- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
+++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp
@@ -1,5 +1,6 @@
 // Copyright (c) 2018 Intel Corporation
 // Copyright (c) 2018 Simbe Robotics
+// Copyright (c) 2019 Samsung Research America
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -28,13 +29,13 @@
 #include "nav2_msgs/msg/costmap.hpp"
 #include "nav2_msgs/msg/path.hpp"
 #include "nav2_navfn_planner/navfn.hpp"
-#include "nav2_util/costmap_service_client.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
 namespace nav2_navfn_planner
 {
@@ -110,11 +111,6 @@ protected:
   // Set the corresponding cell cost to be free space
   void clearRobotCell(unsigned int mx, unsigned int my);
 
-  // Request costmap from world model
-  void getCostmap(
-    nav2_msgs::msg::Costmap & costmap,
-    const std::string layer = "master");
-
   // Print costmap to terminal
   void printCostmap(const nav2_msgs::msg::Costmap & costmap);
 
@@ -131,16 +127,15 @@ protected:
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
-  // Service client for getting the costmap
-  nav2_util::CostmapServiceClient costmap_client_{"navfn_planner"};
+  // Global Costmap
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+  std::unique_ptr<std::thread> costmap_thread_;
+  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> costmap_executor_;
 
   // Publishers for the path
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
 
-  // The costmap to use and its size
-  nav2_msgs::msg::Costmap costmap_;
-  uint current_costmap_size_[2];
-
   // The global frame of the costmap
   const std::string global_frame_{"map"};
 
diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml
index 97593b016f4219ca1e2c23a35db2d828ae7f8ce9..917695aa2d40857fb6a89a8a6333451dce947a86 100644
--- a/nav2_navfn_planner/package.xml
+++ b/nav2_navfn_planner/package.xml
@@ -11,28 +11,18 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>rclcpp</build_depend>
-  <build_depend>rclcpp_action</build_depend>
-  <build_depend>rclcpp_lifecycle</build_depend>
-  <build_depend>visualization_msgs</build_depend>
-  <build_depend>nav2_util</build_depend>
-  <build_depend>nav2_msgs</build_depend>
-  <build_depend>nav_msgs</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>builtin_interfaces</build_depend>
-  <build_depend>nav2_common</build_depend>
-  <build_depend>tf2_ros</build_depend>
-
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>rclcpp_action</exec_depend>
-  <exec_depend>rclcpp_lifecycle</exec_depend>
-  <exec_depend>visualization_msgs</exec_depend>
-  <exec_depend>nav2_msgs</exec_depend>
-  <exec_depend>nav2_util</exec_depend>
-  <exec_depend>nav_msgs</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>builtin_interfaces</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>visualization_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>nav2_common</depend>
+  <depend>tf2_ros</depend>
+  <depend>nav2_costmap_2d</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index 44e175c98dc1a2261a5414fb4b185d9dce2cabda..fb683fbe69377214471784ef669961c6c30d638d 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -1,5 +1,6 @@
 // Copyright (c) 2018 Intel Corporation
 // Copyright (c) 2018 Simbe Robotics
+// Copyright (c) 2019 Samsung Research America
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -36,8 +37,10 @@
 #include "nav2_msgs/srv/get_costmap.hpp"
 #include "nav2_navfn_planner/navfn.hpp"
 #include "nav2_util/costmap.hpp"
+#include "nav2_util/node_utils.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "visualization_msgs/msg/marker.hpp"
+#include "nav2_costmap_2d/cost_values.hpp"
 
 using namespace std::chrono_literals;
 
@@ -45,7 +48,7 @@ namespace nav2_navfn_planner
 {
 
 NavfnPlanner::NavfnPlanner()
-: nav2_util::LifecycleNode("navfn_planner", "", true)
+: nav2_util::LifecycleNode("navfn_planner", "", true), costmap_(nullptr)
 {
   RCLCPP_INFO(get_logger(), "Creating");
 
@@ -53,21 +56,33 @@ NavfnPlanner::NavfnPlanner()
   declare_parameter("tolerance", rclcpp::ParameterValue(0.0));
   declare_parameter("use_astar", rclcpp::ParameterValue(false));
 
-  tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
-  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-    rclcpp_node_->get_node_base_interface(),
-    rclcpp_node_->get_node_timers_interface());
-  tf_->setCreateTimerInterface(timer_interface);
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+  // Setup the global costmap
+  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "global_costmap", nav2_util::add_namespaces(std::string{get_namespace()},
+    "global_costmap"));
+  costmap_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
+
+  // Launch a thread to run the costmap node
+  costmap_thread_ = std::make_unique<std::thread>(
+    [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node)
+    {
+      // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll
+      // be able to provide our own executor to spin(), reducing this to a single line
+      costmap_executor_->add_node(node->get_node_base_interface());
+      costmap_executor_->spin();
+      costmap_executor_->remove_node(node->get_node_base_interface());
+    }, costmap_ros_);
 }
 
 NavfnPlanner::~NavfnPlanner()
 {
   RCLCPP_INFO(get_logger(), "Destroying");
+  costmap_executor_->cancel();
+  costmap_thread_->join();
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_configure(const rclcpp_lifecycle::State & /*state*/)
+NavfnPlanner::on_configure(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Configuring");
 
@@ -75,22 +90,24 @@ NavfnPlanner::on_configure(const rclcpp_lifecycle::State & /*state*/)
   get_parameter("tolerance", tolerance_);
   get_parameter("use_astar", use_astar_);
 
-  getCostmap(costmap_);
+  costmap_ros_->on_configure(state);
+  costmap_ = costmap_ros_->getCostmap();
+
   RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
-    costmap_.metadata.size_x, costmap_.metadata.size_y);
+    costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
+
+  tf_ = costmap_ros_->getTfBuffer();
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
   // Create a planner based on the new costmap size
   if (isPlannerOutOfDate()) {
-    current_costmap_size_[0] = costmap_.metadata.size_x;
-    current_costmap_size_[1] = costmap_.metadata.size_y;
-    planner_ = std::make_unique<NavFn>(costmap_.metadata.size_x, costmap_.metadata.size_y);
+    planner_ = std::make_unique<NavFn>(costmap_->getSizeInCellsX(),
+        costmap_->getSizeInCellsY());
   }
 
   // Initialize pubs & subs
   plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
 
-  auto node = shared_from_this();
-
   // Create the action server that we implement with our navigateToPose method
   action_server_ = std::make_unique<ActionServer>(rclcpp_node_, "ComputePathToPose",
       std::bind(&NavfnPlanner::computePathToPose, this));
@@ -99,29 +116,31 @@ NavfnPlanner::on_configure(const rclcpp_lifecycle::State & /*state*/)
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_activate(const rclcpp_lifecycle::State & /*state*/)
+NavfnPlanner::on_activate(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
   plan_publisher_->on_activate();
   action_server_->activate();
+  costmap_ros_->on_activate(state);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+NavfnPlanner::on_deactivate(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
 
   action_server_->deactivate();
   plan_publisher_->on_deactivate();
+  costmap_ros_->on_deactivate(state);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
 nav2_util::CallbackReturn
-NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
 
@@ -130,6 +149,8 @@ NavfnPlanner::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   planner_.reset();
   tf_listener_.reset();
   tf_.reset();
+  costmap_ros_->on_cleanup(state);
+  costmap_ros_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -173,9 +194,8 @@ NavfnPlanner::computePathToPose()
     }
 
     // Get the current costmap
-    getCostmap(costmap_);
     RCLCPP_DEBUG(get_logger(), "Costmap size: %d,%d",
-      costmap_.metadata.size_x, costmap_.metadata.size_y);
+      costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
 
     geometry_msgs::msg::PoseStamped start;
     if (!nav2_util::getCurrentPose(start, *tf_)) {
@@ -184,9 +204,8 @@ NavfnPlanner::computePathToPose()
 
     // Update planner based on the new costmap size
     if (isPlannerOutOfDate()) {
-      current_costmap_size_[0] = costmap_.metadata.size_x;
-      current_costmap_size_[1] = costmap_.metadata.size_y;
-      planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
+      planner_->setNavArr(costmap_->getSizeInCellsX(),
+        costmap_->getSizeInCellsY());
     }
 
     if (action_server_->is_preempt_requested()) {
@@ -243,8 +262,9 @@ NavfnPlanner::computePathToPose()
 bool
 NavfnPlanner::isPlannerOutOfDate()
 {
-  if (!planner_.get() || current_costmap_size_[0] != costmap_.metadata.size_x ||
-    current_costmap_size_[1] != costmap_.metadata.size_y)
+  if (!planner_.get() ||
+    planner_->nx != static_cast<int>(costmap_->getSizeInCellsX()) ||
+    planner_->ny != static_cast<int>(costmap_->getSizeInCellsY()))
   {
     return true;
   }
@@ -282,9 +302,10 @@ NavfnPlanner::makePlan(
   clearRobotCell(mx, my);
 
   // make sure to resize the underlying array that Navfn uses
-  planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
+  planner_->setNavArr(costmap_->getSizeInCellsX(),
+    costmap_->getSizeInCellsY());
 
-  planner_->setCostmap(&costmap_.data[0], true, allow_unknown_);
+  planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
 
   int map_start[2];
   map_start[0] = mx;
@@ -315,7 +336,7 @@ NavfnPlanner::makePlan(
     planner_->calcNavFnDijkstra(true);
   }
 
-  double resolution = costmap_.metadata.resolution;
+  double resolution = costmap_->getResolution();
   geometry_msgs::msg::Pose p, best_pose;
   p = goal;
 
@@ -379,12 +400,10 @@ bool
 NavfnPlanner::computePotential(const geometry_msgs::msg::Point & world_point)
 {
   // make sure to resize the underlying array that Navfn uses
-  planner_->setNavArr(costmap_.metadata.size_x, costmap_.metadata.size_y);
-
-  std::vector<unsigned char> costmapData = std::vector<unsigned char>(
-    costmap_.data.begin(), costmap_.data.end());
+  planner_->setNavArr(costmap_->getSizeInCellsX(),
+    costmap_->getSizeInCellsY());
 
-  planner_->setCostmap(&costmapData[0], true, allow_unknown_);
+  planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_);
 
   unsigned int mx, my;
   if (!worldToMap(world_point.x, world_point.y, mx, my)) {
@@ -437,7 +456,7 @@ NavfnPlanner::getPlanFromPotential(
 
   planner_->setStart(map_goal);
 
-  planner_->calcPath(costmap_.metadata.size_x * 4);
+  planner_->calcPath(costmap_->getSizeInCellsX() * 4);
 
   // extract the plan
   float * x = planner_->getPathX();
@@ -488,7 +507,7 @@ bool
 NavfnPlanner::validPointPotential(
   const geometry_msgs::msg::Point & world_point, double tolerance)
 {
-  double resolution = costmap_.metadata.resolution;
+  const double resolution = costmap_->getResolution();
 
   geometry_msgs::msg::Point p = world_point;
   p.y = world_point.y - tolerance;
@@ -511,23 +530,23 @@ NavfnPlanner::validPointPotential(
 bool
 NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
 {
-  if (wx < costmap_.metadata.origin.position.x || wy < costmap_.metadata.origin.position.y) {
+  if (wx < costmap_->getOriginX() || wy < costmap_->getOriginY()) {
     RCLCPP_ERROR(get_logger(), "wordToMap failed: wx,wy: %f,%f, size_x,size_y: %d,%d", wx, wy,
-      costmap_.metadata.size_x, costmap_.metadata.size_y);
+      costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
     return false;
   }
 
   mx = static_cast<int>(
-    std::round((wx - costmap_.metadata.origin.position.x) / costmap_.metadata.resolution));
+    std::round((wx - costmap_->getOriginX()) / costmap_->getResolution()));
   my = static_cast<int>(
-    std::round((wy - costmap_.metadata.origin.position.y) / costmap_.metadata.resolution));
+    std::round((wy - costmap_->getOriginY()) / costmap_->getResolution()));
 
-  if (mx < costmap_.metadata.size_x && my < costmap_.metadata.size_y) {
+  if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY()) {
     return true;
   }
 
   RCLCPP_ERROR(get_logger(), "wordToMap failed: mx,my: %d,%d, size_x,size_y: %d,%d", mx, my,
-    costmap_.metadata.size_x, costmap_.metadata.size_y);
+    costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY());
 
   return false;
 }
@@ -535,8 +554,8 @@ NavfnPlanner::worldToMap(double wx, double wy, unsigned int & mx, unsigned int &
 void
 NavfnPlanner::mapToWorld(double mx, double my, double & wx, double & wy)
 {
-  wx = costmap_.metadata.origin.position.x + mx * costmap_.metadata.resolution;
-  wy = costmap_.metadata.origin.position.y + my * costmap_.metadata.resolution;
+  wx = costmap_->getOriginX() + mx * costmap_->getResolution();
+  wy = costmap_->getOriginY() + my * costmap_->getResolution();
 }
 
 void
@@ -544,23 +563,7 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my)
 {
   // TODO(orduno): check usage of this function, might instead be a request to
   //               world_model / map server
-  unsigned int index = my * costmap_.metadata.size_x + mx;
-  costmap_.data[index] = nav2_util::Costmap::free_space;
-}
-
-void
-NavfnPlanner::getCostmap(
-  nav2_msgs::msg::Costmap & costmap,
-  const std::string /*layer*/)
-{
-  // TODO(orduno): explicitly provide specifications for costmap using the costmap on the request,
-  //               including master (aggregate) layer
-
-  auto request = std::make_shared<nav2_util::CostmapServiceClient::CostmapServiceRequest>();
-  request->specs.resolution = 1.0;
-
-  auto result = costmap_client_.invoke(request, 5s);
-  costmap = result.get()->map;
+  costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE);
 }
 
 void
diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index f1bc3bfdb5071046595da2990668905df9803270..fe8312655961537a87dfe992b99ddae760c53f97 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(gazebo_ros_pkgs REQUIRED)
 find_package(nav2_amcl REQUIRED)
 find_package(nav2_lifecycle_manager REQUIRED)
 find_package(rclpy REQUIRED)
+find_package(nav2_navfn_planner REQUIRED)
 find_package(navigation2)
 
 nav2_package()
@@ -33,6 +34,7 @@ set(dependencies
   std_msgs
   tf2_geometry_msgs
   rclpy
+  nav2_navfn_planner
 )
 
 if(BUILD_TESTING)
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index 0eb5f3742a50ebcf452c31e65b088dcd27fc5078..99da8fe278b92d573a99cbd8b44624ef8a2851b6 100644
--- a/nav2_system_tests/package.xml
+++ b/nav2_system_tests/package.xml
@@ -15,6 +15,7 @@
   <build_depend>nav2_util</build_depend>
   <build_depend>nav2_msgs</build_depend>
   <build_depend>nav2_lifecycle_manager</build_depend>
+  <build_depend>nav2_navfn_planner</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>visualization_msgs</build_depend>
   <build_depend>nav2_amcl</build_depend>
@@ -35,6 +36,7 @@
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>nav2_navfn_planner</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>visualization_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt
index 7ad2a7a4682196ae2baaa54cdcf22ae239bbbd02..0938c087091a5d3b48cd37b1d96bec40f11b3c4f 100644
--- a/nav2_system_tests/src/planning/CMakeLists.txt
+++ b/nav2_system_tests/src/planning/CMakeLists.txt
@@ -1,18 +1,37 @@
-ament_add_gtest_executable(test_planner_node
-  test_planner_node.cpp
+ament_add_gtest_executable(test_planner_costmaps_node
+  test_planner_costmaps_node.cpp
   planner_tester.cpp
 )
 
-ament_target_dependencies(test_planner_node
+ament_target_dependencies(test_planner_costmaps_node
   ${dependencies}
 )
 
-ament_add_test(test_planner
+ament_add_gtest_executable(test_planner_random_node
+  test_planner_random_node.cpp
+  planner_tester.cpp
+)
+
+ament_target_dependencies(test_planner_random_node
+  ${dependencies}
+)
+
+ament_add_test(test_planner_costmaps
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_costmaps_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_planner_costmaps_node>
+    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
+)
+
+ament_add_test(test_planner_random
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_random_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   ENV
     TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
-    TEST_EXECUTABLE=$<TARGET_FILE:test_planner_node>
+    TEST_EXECUTABLE=$<TARGET_FILE:test_planner_random_node>
     TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm
 )
diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp
index f2b33964a34e01819c8d6ac189360804f54cc75e..ca32a2dd8eeb93752c43982ef8b21c602834465a 100644
--- a/nav2_system_tests/src/planning/planner_tester.cpp
+++ b/nav2_system_tests/src/planning/planner_tester.cpp
@@ -17,6 +17,7 @@
 #include <tuple>
 #include <utility>
 #include <memory>
+#include <iostream>
 #include <chrono>
 #include <sstream>
 #include <iomanip>
@@ -35,10 +36,12 @@ namespace nav2_system_tests
 {
 
 PlannerTester::PlannerTester()
-: Node("PlannerTester"), is_active_(false), map_set_(false), costmap_set_(false),
-  using_fake_costmap_(true), costmap_server_running_(false), trinary_costmap_(true),
+: Node("PlannerTester"), is_active_(false),
+  map_set_(false), costmap_set_(false),
+  using_fake_costmap_(true), trinary_costmap_(true),
   track_unknown_space_(false), lethal_threshold_(100), unknown_cost_value_(-1),
-  testCostmapType_(TestCostmap::open_space), map_publish_rate_(100s)
+  testCostmapType_(TestCostmap::open_space), base_transform_(nullptr),
+  map_publish_rate_(100s)
 {
 }
 
@@ -50,24 +53,6 @@ void PlannerTester::activate()
   }
   is_active_ = true;
 
-  startRobotTransform();
-
-  // The client used to invoke the services of the global planner (ComputePathToPose)
-  planner_client_ = rclcpp_action::create_client<nav2_msgs::action::ComputePathToPose>(
-    this->get_node_base_interface(),
-    this->get_node_graph_interface(),
-    this->get_node_logging_interface(),
-    this->get_node_waitables_interface(),
-    "ComputePathToPose");
-
-  // For visualization, we'll publish the map
-  map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map");
-
-  // We start with a 10x10 grid with no obstacles
-  loadSimpleCostmap(TestCostmap::open_space);
-
-  startCostmapServer();
-
   // Launch a thread to process the messages for this node
   spin_thread_ = std::make_unique<std::thread>(
     [&]()
@@ -76,6 +61,21 @@ void PlannerTester::activate()
       executor_.spin();
       executor_.remove_node(this->get_node_base_interface());
     });
+
+  // We start with a 10x10 grid with no obstacles
+  loadSimpleCostmap(TestCostmap::open_space);
+
+  startRobotTransform();
+
+  // The navfn wrapper
+  auto state = rclcpp_lifecycle::State();
+  planner_tester_ = std::make_unique<NavFnPlannerTester>();
+  planner_tester_->onConfigure(state);
+  publishRobotTransform();
+  planner_tester_->onActivate(state);
+
+  // For visualization, we'll publish the map
+  map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map");
 }
 
 void PlannerTester::deactivate()
@@ -90,11 +90,12 @@ void PlannerTester::deactivate()
   spin_thread_->join();
   spin_thread_.reset();
 
-  planner_client_.reset();
+  auto state = rclcpp_lifecycle::State();
+  planner_tester_->onCleanup(state);
+
   map_timer_.reset();
   map_pub_.reset();
   map_.reset();
-  costmap_server_.reset();
   tf_broadcaster_.reset();
 }
 
@@ -112,8 +113,8 @@ void PlannerTester::startRobotTransform()
 
   // Set an initial pose
   geometry_msgs::msg::Point robot_position;
-  robot_position.x = 0.0;
-  robot_position.y = 0.0;
+  robot_position.x = 1.0;
+  robot_position.y = 1.0;
   updateRobotPosition(robot_position);
 
   // Publish the transform periodically
@@ -144,29 +145,6 @@ void PlannerTester::publishRobotTransform()
   }
 }
 
-void PlannerTester::startCostmapServer()
-{
-  if (!costmap_set_) {
-    RCLCPP_ERROR(this->get_logger(), "Costmap must be set before starting the service");
-    return;
-  }
-
-  auto costmap_service_callback = [this](
-    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
-    const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,
-    std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response) -> void
-    {
-      RCLCPP_DEBUG(this->get_logger(), "Incoming costmap request");
-      response->map = costmap_->get_costmap(request->specs);
-    };
-
-  // Create a service that will use the callback function to handle requests.
-  costmap_server_ = create_service<nav2_msgs::srv::GetCostmap>(
-    "GetCostmap", costmap_service_callback);
-
-  costmap_server_running_ = true;
-}
-
 void PlannerTester::loadDefaultMap()
 {
   // Specs for the default map
@@ -225,6 +203,7 @@ void PlannerTester::loadDefaultMap()
 
 void PlannerTester::loadSimpleCostmap(const TestCostmap & testCostmapType)
 {
+  RCLCPP_INFO(get_logger(), "loadSimpleCostmap called.");
   if (costmap_set_) {
     RCLCPP_DEBUG(this->get_logger(), "Setting a new costmap with fake values");
   }
@@ -262,8 +241,6 @@ bool PlannerTester::defaultPlannerTest(
     return false;
   }
 
-  waitForPlanner();
-
   // TODO(orduno) #443 Add support for planners that take into account robot orientation
   geometry_msgs::msg::Point robot_position;
   ComputePathToPoseCommand goal;
@@ -292,7 +269,6 @@ bool PlannerTester::defaultPlannerTest(
 
   // TODO(orduno): #443 On a default test, provide the reference path to compare with the planner
   //               result.
-
   return plannerTest(robot_position, goal, path);
 }
 
@@ -311,8 +287,6 @@ bool PlannerTester::defaultPlannerRandomTests(
     return false;
   }
 
-  waitForPlanner();
-
   // Initialize random number generator
   std::random_device random_device;
   std::mt19937 generator(random_device());
@@ -383,9 +357,10 @@ bool PlannerTester::plannerTest(
 
   // First make available the current robot position for the planner to take as starting point
   updateRobotPosition(robot_position);
+  sleep(0.05);
 
   // Then request to compute a path
-  TaskStatus status = sendRequest(goal, path);
+  TaskStatus status = createPlan(goal, path);
 
   RCLCPP_DEBUG(this->get_logger(), "Path request status: %d", status);
 
@@ -394,51 +369,25 @@ bool PlannerTester::plannerTest(
   } else if (status == TaskStatus::SUCCEEDED) {
     // TODO(orduno): #443 check why task may report success while planner returns a path of 0 points
     RCLCPP_DEBUG(this->get_logger(), "Got path, checking for possible collisions");
-
     return isCollisionFree(path) && isWithinTolerance(robot_position, goal, path);
   }
 
   return false;
 }
 
-TaskStatus PlannerTester::sendRequest(
+TaskStatus PlannerTester::createPlan(
   const ComputePathToPoseCommand & goal,
   ComputePathToPoseResult & path)
 {
-  nav2_msgs::action::ComputePathToPose::Goal action_goal;
-  action_goal.pose = goal;
-  auto future_goal_handle = planner_client_->async_send_goal(action_goal);
-
-  RCLCPP_DEBUG(this->get_logger(), "Waiting for goal acceptance");
-  auto status_request = future_goal_handle.wait_for(seconds(5));
-  if (status_request != std::future_status::ready) {
-    RCLCPP_ERROR(this->get_logger(), "Failed to send the goal");
-    return TaskStatus::FAILED;
-  }
-
-  auto goal_handle = future_goal_handle.get();
-  if (!goal_handle) {
-    RCLCPP_ERROR(this->get_logger(), "Goal rejected");
-    return TaskStatus::FAILED;
-  }
-
-  auto future_result = planner_client_->async_get_result(goal_handle);
-
-  RCLCPP_DEBUG(this->get_logger(), "Wait for the server to be done with the action");
-  auto status_result = future_result.wait_for(seconds(10));
-  if (status_result != std::future_status::ready) {
-    RCLCPP_ERROR(this->get_logger(), "Failed to get a plan within the allowed time");
-    return TaskStatus::FAILED;
-  }
+  // Update the costmap of the planner to the set data
+  planner_tester_->setCostmap(costmap_.get());
 
-  auto result = future_result.get();
-  if (result.code != rclcpp_action::ResultCode::SUCCEEDED) {
-    return TaskStatus::FAILED;
+  // Call planning algorithm
+  if (planner_tester_->createPath(goal, path)) {
+    return TaskStatus::SUCCEEDED;
   }
 
-  path = result.result->path;
-
-  return TaskStatus::SUCCEEDED;
+  return TaskStatus::FAILED;
 }
 
 bool PlannerTester::isCollisionFree(const ComputePathToPoseResult & path)
@@ -529,13 +478,4 @@ void PlannerTester::printPath(const ComputePathToPoseResult & path) const
   RCLCPP_INFO(get_logger(), ss.str().c_str());
 }
 
-void PlannerTester::waitForPlanner()
-{
-  RCLCPP_DEBUG(this->get_logger(), "Waiting for ComputePathToPose action server");
-
-  if (!planner_client_ || !planner_client_->wait_for_action_server(10s)) {
-    RCLCPP_ERROR(this->get_logger(), "Planner not running");
-    throw std::runtime_error("Planner not running");
-  }
-}
 }  // namespace nav2_system_tests
diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp
index 1d24408bcd0b1284dc032ccbab080f7cb1cb9a28..5dfacacdf7d96d9ab0239f17def5f4e27874c0bc 100644
--- a/nav2_system_tests/src/planning/planner_tester.hpp
+++ b/nav2_system_tests/src/planning/planner_tester.hpp
@@ -19,6 +19,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <algorithm>
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
@@ -31,11 +32,78 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "nav2_navfn_planner/navfn_planner.hpp"
 #include "tf2_ros/transform_broadcaster.h"
 
 namespace nav2_system_tests
 {
 
+class NavFnPlannerTester : public nav2_navfn_planner::NavfnPlanner
+{
+public:
+  NavFnPlannerTester()
+  : NavfnPlanner()
+  {
+  }
+
+  void printCostmap()
+  {
+    // print costmap for debug
+    for (uint i = 0; i != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY(); i++) {
+      if (i % costmap_->getSizeInCellsX() == 0) {
+        std::cout << "" << std::endl;
+      }
+      std::cout << costmap_ros_->getCostmap()->getCharMap()[i] << " ";
+    }
+    std::cout << "" << std::endl;
+  }
+
+  void setCostmap(nav2_util::Costmap * costmap)
+  {
+    nav2_msgs::msg::CostmapMetaData prop;
+    nav2_msgs::msg::Costmap cm = costmap->get_costmap(prop);
+    prop = cm.metadata;
+    costmap_ros_->getCostmap()->resizeMap(prop.size_x, prop.size_y,
+      prop.resolution, prop.origin.position.x, prop.origin.position.x);
+    unsigned char * costmap_ptr = costmap_ros_->getCostmap()->getCharMap();
+    delete[] costmap_ptr;
+    costmap_ptr = new unsigned char[prop.size_x * prop.size_y];
+    std::copy(cm.data.begin(), cm.data.end(), costmap_ptr);
+  }
+
+  bool createPath(
+    const geometry_msgs::msg::PoseStamped & goal,
+    nav2_msgs::msg::Path & path)
+  {
+    geometry_msgs::msg::PoseStamped start;
+    if (!nav2_util::getCurrentPose(start, *tf_, "map", "base_link", 0.1)) {
+      return false;
+    }
+
+    if (isPlannerOutOfDate()) {
+      planner_->setNavArr(costmap_->getSizeInCellsX(),
+        costmap_->getSizeInCellsY());
+    }
+
+    return makePlan(start.pose, goal.pose, tolerance_, path);
+  }
+
+  void onCleanup(const rclcpp_lifecycle::State & state)
+  {
+    on_cleanup(state);
+  }
+
+  void onActivate(const rclcpp_lifecycle::State & state)
+  {
+    on_activate(state);
+  }
+
+  void onConfigure(const rclcpp_lifecycle::State & state)
+  {
+    on_configure(state);
+  }
+};
+
 enum class TaskStatus : int8_t
 {
   SUCCEEDED = 1,
@@ -75,11 +143,17 @@ public:
     const float acceptable_fail_ratio);
 
 private:
+  void setCostmap();
+
+  TaskStatus createPlan(
+    const ComputePathToPoseCommand & goal,
+    ComputePathToPoseResult & path
+  );
+
   bool is_active_;
   bool map_set_;
   bool costmap_set_;
   bool using_fake_costmap_;
-  bool costmap_server_running_;
 
   // Parameters of the costmap
   bool trinary_costmap_;
@@ -94,15 +168,13 @@ private:
   // The costmap representation of the static map
   std::unique_ptr<nav2_util::Costmap> costmap_;
 
+  // The global planner
+  std::unique_ptr<NavFnPlannerTester> planner_tester_;
+
   // A thread for spinning the ROS node and the executor used
   std::unique_ptr<std::thread> spin_thread_;
   rclcpp::executors::SingleThreadedExecutor executor_;
 
-  // The tester must provide the costmap service
-  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_server_;
-  void setCostmap();
-  void startCostmapServer();
-
   // The tester must provide the robot pose through a transform
   std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
@@ -111,10 +183,6 @@ private:
   void startRobotTransform();
   void updateRobotPosition(const geometry_msgs::msg::Point & position);
 
-  // The interface to the global planner
-  std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::ComputePathToPose>> planner_client_;
-  void waitForPlanner();
-
   // Occupancy grid publisher for visualization
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
   rclcpp::TimerBase::SharedPtr map_timer_;
@@ -129,12 +197,6 @@ private:
     const ComputePathToPoseCommand & goal,
     ComputePathToPoseResult & path);
 
-  // Sends the request to the planner and gets the result.
-  TaskStatus sendRequest(
-    const ComputePathToPoseCommand & goal,
-    ComputePathToPoseResult & path
-  );
-
   bool isCollisionFree(const ComputePathToPoseResult & path);
 
   bool isWithinTolerance(
diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_launch.py b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py
new file mode 100755
index 0000000000000000000000000000000000000000..8f02f3f8b421eb73ec5ae2f893507c7a7e5f223a
--- /dev/null
+++ b/nav2_system_tests/src/planning/test_planner_costmaps_launch.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+
+# 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
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+
+    ld = LaunchDescription([])
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_planner_costmaps_node',
+        output='screen'
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())
diff --git a/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d43a1c58c561272d97c93d2e4ea9ec53c9a074b
--- /dev/null
+++ b/nav2_system_tests/src/planning/test_planner_costmaps_node.cpp
@@ -0,0 +1,60 @@
+// 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. Reserved.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "planner_tester.hpp"
+#include "nav2_util/lifecycle_utils.hpp"
+
+using namespace std::chrono_literals;
+
+using nav2_system_tests::PlannerTester;
+using nav2_util::TestCostmap;
+
+using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
+using ComputePathToPoseResult = nav2_msgs::msg::Path;
+
+// rclcpp::init can only be called once per process, so this needs to be a global variable
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+TEST_F(PlannerTester, testSimpleCostmaps)
+{
+  activate();
+
+  std::vector<TestCostmap> costmaps = {
+    TestCostmap::open_space,
+    TestCostmap::bounded,
+    TestCostmap::top_left_obstacle,
+    TestCostmap::bottom_left_obstacle,
+    TestCostmap::maze1,
+    TestCostmap::maze2
+  };
+
+  ComputePathToPoseResult result;
+
+  for (auto costmap : costmaps) {
+    loadSimpleCostmap(costmap);
+    EXPECT_EQ(true, defaultPlannerTest(result));
+  }
+}
diff --git a/nav2_system_tests/src/planning/test_planner_launch.py b/nav2_system_tests/src/planning/test_planner_random_launch.py
similarity index 82%
rename from nav2_system_tests/src/planning/test_planner_launch.py
rename to nav2_system_tests/src/planning/test_planner_random_launch.py
index 4353158be935ecb5785e074245257d06729faf26..87ddb4ff8be1d1e9ff166c106a37a80875876b87 100755
--- a/nav2_system_tests/src/planning/test_planner_launch.py
+++ b/nav2_system_tests/src/planning/test_planner_random_launch.py
@@ -21,23 +21,17 @@ from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import ExecuteProcess
 
-import launch_ros.actions
-
 from launch_testing.legacy import LaunchTestService
 
 
 def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
 
-    run_navfn = launch_ros.actions.Node(
-        package='nav2_navfn_planner',
-        node_executable='navfn_planner',
-        output='screen')
-    ld = LaunchDescription([run_navfn])
+    ld = LaunchDescription([])
 
     test1_action = ExecuteProcess(
         cmd=[testExecutable],
-        name='test_planner_node',
+        name='test_planner_random_node',
         output='screen'
     )
 
diff --git a/nav2_system_tests/src/planning/test_planner_node.cpp b/nav2_system_tests/src/planning/test_planner_random_node.cpp
similarity index 56%
rename from nav2_system_tests/src/planning/test_planner_node.cpp
rename to nav2_system_tests/src/planning/test_planner_random_node.cpp
index 9111ead0d707133deef14d029615f04a620c266a..51c60ac5c7f2b332e275fb679c6476cd37339ebf 100644
--- a/nav2_system_tests/src/planning/test_planner_node.cpp
+++ b/nav2_system_tests/src/planning/test_planner_random_node.cpp
@@ -15,10 +15,10 @@
 #include <gtest/gtest.h>
 #include <memory>
 #include <vector>
+#include <iostream>
 
 #include "rclcpp/rclcpp.hpp"
 #include "planner_tester.hpp"
-#include "nav2_util/lifecycle_utils.hpp"
 
 using namespace std::chrono_literals;
 
@@ -38,45 +38,19 @@ public:
 
 RclCppFixture g_rclcppfixture;
 
-// #TODO(orduno) Multiple startups and shutdowns of navfn kills the navfn process #1100
-//               Re-enable after this is resolved
-// TEST_F(PlannerTester, testSimpleCostmaps)
-// {
-//   activate();
-
-//   nav2_util::startup_lifecycle_nodes("/navfn_planner");
-
-//   std::vector<TestCostmap> costmaps = {
-//     TestCostmap::open_space,
-//     TestCostmap::bounded,
-//     TestCostmap::top_left_obstacle,
-//     TestCostmap::bottom_left_obstacle,
-//     TestCostmap::maze1,
-//     TestCostmap::maze2
-//   };
-
-//   ComputePathToPoseResult result;
-
-//   for (auto costmap : costmaps) {
-//     loadSimpleCostmap(costmap);
-//     EXPECT_EQ(true, defaultPlannerTest(result));
-//   }
-
-//   nav2_util::reset_lifecycle_nodes("/navfn_planner");
-
-//   deactivate();
-// }
-
 TEST_F(PlannerTester, testWithHundredRandomEndPoints)
 {
   activate();
   loadDefaultMap();
 
-  nav2_util::startup_lifecycle_nodes("/navfn_planner");
-
-  EXPECT_EQ(true, defaultPlannerRandomTests(100, 0.1));
-
-  nav2_util::reset_lifecycle_nodes("/navfn_planner");
+  bool success = false;
+  int num_tries = 3;
+  for (int i = 0; i != num_tries; i++) {
+    success = success || defaultPlannerRandomTests(100, 0.1);
+    if (success) {
+      break;
+    }
+  }
 
-  deactivate();
+  EXPECT_EQ(true, success);
 }
diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py
index f186a7994170cc4014c799a2cfac6d1ecbe4db51..551ca84b0f9da076d038c05c1bb9983c66aad39d 100755
--- a/nav2_system_tests/src/system/test_system_launch.py
+++ b/nav2_system_tests/src/system/test_system_launch.py
@@ -68,12 +68,6 @@ def generate_launch_description():
             output='screen',
             parameters=[{'use_sim_time': use_sim_time}, {'yaml_filename': map_yaml_file}]),
 
-        launch_ros.actions.Node(
-            package='nav2_world_model',
-            node_executable='world_model',
-            output='screen',
-            parameters=[params_file]),
-
         launch_ros.actions.Node(
             package='nav2_amcl',
             node_executable='amcl',
@@ -114,7 +108,7 @@ def generate_launch_description():
             node_name='lifecycle_manager',
             output='screen',
             parameters=[{'use_sim_time': use_sim_time},
-                        {'node_names': ['map_server', 'amcl', 'world_model',
+                        {'node_names': ['map_server', 'amcl',
                          'dwb_controller', 'navfn_planner', 'bt_navigator']},
                         {'autostart': True}]),
     ])
diff --git a/nav2_system_tests/src/updown/nav2_params.yaml b/nav2_system_tests/src/updown/nav2_params.yaml
index 23320c8d1e0701128b800f436fb5c739ed61dc28..471917ea8fc6c7088ef1c4c1b690b3778b58d2db 100644
--- a/nav2_system_tests/src/updown/nav2_params.yaml
+++ b/nav2_system_tests/src/updown/nav2_params.yaml
@@ -153,10 +153,6 @@ robot_state_publisher:
   ros__parameters:
     use_sim_time: True
 
-world_model:
-  ros__parameters:
-    use_sim_time: True
-
 global_costmap:
   global_costmap:
     ros__parameters:
diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py
index 52ae47634846cd25ff21670e3cbc1a195e13334d..2fe3cacf8d53208c6ec6b795fdfecaa9e85ab2a7 100755
--- a/nav2_system_tests/src/updown/test_updown_launch.py
+++ b/nav2_system_tests/src/updown/test_updown_launch.py
@@ -79,14 +79,6 @@ def generate_launch_description():
             '--ros-args', ['__params:=', params_file]],
         cwd=[launch_dir], output='screen')
 
-    start_world_model_cmd = launch.actions.ExecuteProcess(
-        cmd=[
-            os.path.join(
-                get_package_prefix('nav2_world_model'),
-                'lib/nav2_world_model/world_model'),
-            '--ros-args', ['__params:=', params_file]],
-        cwd=[launch_dir], output='screen')
-
     start_dwb_cmd = launch.actions.ExecuteProcess(
         cmd=[
             os.path.join(
@@ -143,7 +135,6 @@ def generate_launch_description():
     ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(start_map_server_cmd)
     ld.add_action(start_localizer_cmd)
-    ld.add_action(start_world_model_cmd)
     ld.add_action(start_dwb_cmd)
     ld.add_action(start_planner_cmd)
     ld.add_action(start_navigator_cmd)
diff --git a/nav2_util/include/nav2_util/costmap_service_client.hpp b/nav2_util/include/nav2_util/costmap_service_client.hpp
deleted file mode 100644
index de42ac303d5344b849e39fb826ec77181ccbe2e9..0000000000000000000000000000000000000000
--- a/nav2_util/include/nav2_util/costmap_service_client.hpp
+++ /dev/null
@@ -1,47 +0,0 @@
-// 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.
-
-#ifndef NAV2_UTIL__COSTMAP_SERVICE_CLIENT_HPP_
-#define NAV2_UTIL__COSTMAP_SERVICE_CLIENT_HPP_
-
-#include <string>
-
-#include "nav2_msgs/srv/get_costmap.hpp"
-#include "nav2_util/service_client.hpp"
-
-namespace nav2_util
-{
-
-class CostmapServiceClient : public ServiceClient<nav2_msgs::srv::GetCostmap>
-{
-public:
-  explicit CostmapServiceClient(const std::string & parent_node_name)
-  : ServiceClient<nav2_msgs::srv::GetCostmap>("GetCostmap", parent_node_name)
-  {
-  }
-
-  explicit CostmapServiceClient(rclcpp::Node::SharedPtr & node)
-  : ServiceClient<nav2_msgs::srv::GetCostmap>("GetCostmap", node)
-  {
-  }
-
-  using CostmapServiceRequest =
-    ServiceClient<nav2_msgs::srv::GetCostmap>::RequestType;
-  using CostmapServiceResponse =
-    ServiceClient<nav2_msgs::srv::GetCostmap>::ResponseType;
-};
-
-}  // namespace nav2_util
-
-#endif  // NAV2_UTIL__COSTMAP_SERVICE_CLIENT_HPP_
diff --git a/nav2_world_model/CHANGELOG.rst b/nav2_world_model/CHANGELOG.rst
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/nav2_world_model/CMakeLists.txt b/nav2_world_model/CMakeLists.txt
deleted file mode 100644
index 112304901631006055c77a046a3eeb1fe99a3f12..0000000000000000000000000000000000000000
--- a/nav2_world_model/CMakeLists.txt
+++ /dev/null
@@ -1,63 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(nav2_world_model)
-
-find_package(ament_cmake REQUIRED)
-find_package(nav2_common REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(nav2_util)
-find_package(nav2_msgs)
-find_package(nav2_costmap_2d REQUIRED)
-find_package(tf2_ros REQUIRED)
-
-nav2_package()
-
-include_directories(
-  include
-)
-
-set(executable_name world_model)
-
-add_executable(${executable_name}
-  src/main.cpp
-)
-
-set(library_name ${executable_name}_core)
-
-add_library(${library_name} SHARED
-  src/world_model.cpp
-)
-
-# prevent pluginlib from using boost
-target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
-
-set(dependencies
-  rclcpp
-  nav2_util
-  nav2_msgs
-  nav2_costmap_2d
-  tf2_ros
-)
-
-ament_target_dependencies(${executable_name}
-  ${dependencies}
-)
-
-target_link_libraries(${executable_name} ${library_name})
-
-ament_target_dependencies(${library_name}
-  ${dependencies}
-)
-
-install(TARGETS ${executable_name} ${library_name}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_export_include_directories(include)
-
-ament_package()
diff --git a/nav2_world_model/README.md b/nav2_world_model/README.md
deleted file mode 100644
index 7de9d871deedfb8b730c682fb8074d0d3007f0e6..0000000000000000000000000000000000000000
--- a/nav2_world_model/README.md
+++ /dev/null
@@ -1,12 +0,0 @@
-# Nav2 World Model
-
-`nav2_world_model` is a package containing an exposed environmental representation. Today, it uses the `nav2_costmap_2d` layered costmap as the world model. In the future, this is the entry point for applications to request or view information about the environment around it. The implementations such as costmaps or height maps will provide the buffering of data and representations as they require. They are then wrapped for generalized use for path planning and control.
-
-## ROS1 Comparison
-
-ROS1 Navigation contains `costmap_2d` which is directly used as the evironmental model. This package is able to consume `nav2_costmap_2d` as an implementation of a world model, but can also utilize other world models to suit the needs of a specific application. Rather than querying `costmap_2d` for information, applications will query `nav2_world_model` which will in turn use the current world model and retrieve the information requested.
-
-## Future
-
-* Additional implementations/support for different environmental representations like `grid_maps`, height maps, traversibility maps, etc. 
-* Utilities for converting types for use in other applications like control and planning. 
diff --git a/nav2_world_model/include/nav2_world_model/world_model.hpp b/nav2_world_model/include/nav2_world_model/world_model.hpp
deleted file mode 100644
index dac5ce96be6ff40d390e940dbf9792098e3d275e..0000000000000000000000000000000000000000
--- a/nav2_world_model/include/nav2_world_model/world_model.hpp
+++ /dev/null
@@ -1,68 +0,0 @@
-// 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.
-
-#ifndef NAV2_WORLD_MODEL__WORLD_MODEL_HPP_
-#define NAV2_WORLD_MODEL__WORLD_MODEL_HPP_
-
-#include <memory>
-#include <thread>
-
-#include "nav2_costmap_2d/costmap_2d_ros.hpp"
-#include "nav2_util/lifecycle_node.hpp"
-#include "nav2_msgs/msg/costmap.hpp"
-#include "nav2_msgs/srv/get_costmap.hpp"
-
-namespace nav2_world_model
-{
-
-class WorldModel : public nav2_util::LifecycleNode
-{
-public:
-  WorldModel();
-  ~WorldModel();
-
-protected:
-  // Implement the lifecycle interface
-  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
-  nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
-
-  // The WorldModel provides these services
-  rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
-
-  void costmap_service_callback(
-    const std::shared_ptr<rmw_request_id_t> request_header,
-    const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,
-    const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response);
-
-  // The implementation of the WorldModel uses a Costmap2DROS node
-  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
-
-  // The thread for the Costmap2DROS node
-  std::unique_ptr<std::thread> costmap_thread_;
-
-  // The frame_id and metadata layer values used in the service response message
-  static constexpr const char * frame_id_{"map"};
-  static constexpr const char * metadata_layer_{"Master"};
-
-  // An executor used to spin the costmap node
-  std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> costmap_executor_;
-};
-
-}  // namespace nav2_world_model
-
-#endif  // NAV2_WORLD_MODEL__WORLD_MODEL_HPP_
diff --git a/nav2_world_model/package.xml b/nav2_world_model/package.xml
deleted file mode 100644
index 82b22c029253654ed80bb09e7b7d3c72db0dee15..0000000000000000000000000000000000000000
--- a/nav2_world_model/package.xml
+++ /dev/null
@@ -1,35 +0,0 @@
-<?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>nav2_world_model</name>
-  <version>0.2.4</version>
-  <description>TODO</description>
-  <maintainer email="brian.wilcox@intel.com">Brian Wilcox</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <build_depend>rclcpp</build_depend>
-  <build_depend>nav2_util</build_depend>
-  <build_depend>nav2_msgs</build_depend>
-  <build_depend>nav2_costmap_2d</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_depend>nav2_common</build_depend>
-
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>nav2_util</exec_depend>
-  <exec_depend>nav2_msgs</exec_depend>
-  <exec_depend>nav2_costmap_2d</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-
-  <test_depend>ament_lint_common</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>ament_cmake_pytest</test_depend>
-  <test_depend>launch</test_depend>
-  <test_depend>launch_testing</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/nav2_world_model/src/main.cpp b/nav2_world_model/src/main.cpp
deleted file mode 100644
index 4fef67a9a835e90bf44cbe4488b0bb937f65cf82..0000000000000000000000000000000000000000
--- a/nav2_world_model/src/main.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-// 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.
-
-#include <memory>
-
-#include "nav2_world_model/world_model.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  auto node = std::make_shared<nav2_world_model::WorldModel>();
-  rclcpp::spin(node->get_node_base_interface());
-  rclcpp::shutdown();
-
-  return 0;
-}
diff --git a/nav2_world_model/src/world_model.cpp b/nav2_world_model/src/world_model.cpp
deleted file mode 100644
index 6e4693ea0e2e31afce8356e90db6b6f98c72ee1a..0000000000000000000000000000000000000000
--- a/nav2_world_model/src/world_model.cpp
+++ /dev/null
@@ -1,152 +0,0 @@
-// 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.
-
-#include "nav2_world_model/world_model.hpp"
-#include <memory>
-#include <string>
-#include "nav2_util/node_utils.hpp"
-
-
-using namespace std::placeholders;
-
-namespace nav2_world_model
-{
-
-WorldModel::WorldModel()
-: nav2_util::LifecycleNode("world_model")
-{
-  RCLCPP_INFO(get_logger(), "Creating World Model");
-
-  // The costmap node is used in the implementation of the world model
-  costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
-    "global_costmap", nav2_util::add_namespaces(std::string{get_namespace()}, "global_costmap"));
-
-  // Create an executor that will be used to spin the costmap node
-  costmap_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
-
-  // Launch a thread to run the costmap node
-  costmap_thread_ = std::make_unique<std::thread>(
-    [&](rclcpp_lifecycle::LifecycleNode::SharedPtr node)
-    {
-      // TODO(mjeronimo): Once Brian pushes his change upstream to rlcpp executors, we'll
-      // be able to provide our own executor to spin(), reducing this to a single line
-      costmap_executor_->add_node(node->get_node_base_interface());
-      costmap_executor_->spin();
-      costmap_executor_->remove_node(node->get_node_base_interface());
-    }, costmap_ros_);
-}
-
-WorldModel::~WorldModel()
-{
-  RCLCPP_INFO(get_logger(), "Destroying");
-  costmap_executor_->cancel();
-  costmap_thread_->join();
-}
-
-nav2_util::CallbackReturn
-WorldModel::on_configure(const rclcpp_lifecycle::State & state)
-{
-  RCLCPP_INFO(get_logger(), "Configuring");
-
-  costmap_ros_->on_configure(state);
-
-  // Create a service that will use the callback function to handle requests.
-  costmap_service_ = create_service<nav2_msgs::srv::GetCostmap>("GetCostmap",
-      std::bind(&WorldModel::costmap_service_callback, this, _1, _2, _3));
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-WorldModel::on_activate(const rclcpp_lifecycle::State & state)
-{
-  RCLCPP_INFO(get_logger(), "Activating");
-
-  costmap_ros_->on_activate(state);
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-WorldModel::on_deactivate(const rclcpp_lifecycle::State & state)
-{
-  RCLCPP_INFO(get_logger(), "Deactivating");
-
-  costmap_ros_->on_deactivate(state);
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-WorldModel::on_cleanup(const rclcpp_lifecycle::State & state)
-{
-  RCLCPP_INFO(get_logger(), "Cleaning up");
-
-  costmap_ros_->on_cleanup(state);
-  costmap_service_.reset();
-
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-WorldModel::on_error(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state");
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-nav2_util::CallbackReturn
-WorldModel::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
-{
-  RCLCPP_INFO(get_logger(), "Shutting down");
-  return nav2_util::CallbackReturn::SUCCESS;
-}
-
-void
-WorldModel::costmap_service_callback(
-  const std::shared_ptr<rmw_request_id_t>/*request_header*/,
-  const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request>/*request*/,
-  const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response)
-{
-  RCLCPP_DEBUG(get_logger(), "Received costmap service request");
-
-  // TODO(bpwilcox): Grab correct orientation information
-  tf2::Quaternion quaternion;
-  quaternion.setRPY(0.0, 0.0, 0.0);
-
-  nav2_costmap_2d::Costmap2D * costmap_ = costmap_ros_->getCostmap();
-
-  auto size_x = costmap_->getSizeInCellsX();
-  auto size_y = costmap_->getSizeInCellsY();
-  auto data_length = size_x * size_y;
-  unsigned char * data = costmap_->getCharMap();
-  auto current_time = now();
-
-  response->map.header.stamp = current_time;
-  response->map.header.frame_id = frame_id_;
-  response->map.metadata.size_x = size_x;
-  response->map.metadata.size_y = size_y;
-  response->map.metadata.resolution = costmap_->getResolution();
-  response->map.metadata.layer = metadata_layer_;
-  response->map.metadata.map_load_time = current_time;
-  response->map.metadata.update_time = current_time;
-  response->map.metadata.origin.position.x = costmap_->getOriginX();
-  response->map.metadata.origin.position.y = costmap_->getOriginY();
-  response->map.metadata.origin.position.z = 0.0;
-  response->map.metadata.origin.orientation = tf2::toMsg(quaternion);
-  response->map.data.resize(data_length);
-  response->map.data.assign(data, data + data_length);
-}
-
-}  // namespace nav2_world_model
diff --git a/navigation2/package.xml b/navigation2/package.xml
index a20561b6db22dd0f4230b1efc1097369b2bcd48a..73deb1b94ce85c7969c0c98e92c970b69c2604c1 100644
--- a/navigation2/package.xml
+++ b/navigation2/package.xml
@@ -27,7 +27,6 @@
   <exec_depend>nav2_behavior_tree</exec_depend>
   <exec_depend>nav2_util</exec_depend>
   <exec_depend>nav2_voxel_grid</exec_depend>
-  <exec_depend>nav2_world_model</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/tools/run_test_suite.bash b/tools/run_test_suite.bash
index 434f69a2b5cf71c96f588acacfd1360eccf6a022..236253c74ac66f2de9d6163f4165490cb31ad6ab 100755
--- a/tools/run_test_suite.bash
+++ b/tools/run_test_suite.bash
@@ -23,6 +23,7 @@ colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "te
 colcon test-result --verbose
 
 $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_localization$
-$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner$
+$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_costmaps$
+$SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_random$
 $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator$
 $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator_with_dijkstra$