diff --git a/src/libs/costmap_2d/CMakeLists.txt b/src/libs/costmap_2d/CMakeLists.txt
index dce19ae123..899f75953d 100644
--- a/src/libs/costmap_2d/CMakeLists.txt
+++ b/src/libs/costmap_2d/CMakeLists.txt
@@ -10,7 +10,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   #add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
@@ -27,34 +26,32 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(xmlrpcpp REQUIRED)
 find_package(laser_geometry REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(message_filters REQUIRED)
 
-#	TODO(bpwilcox): port remaining packages
+# TODO(bpwilcox): port remaining packages
 #find_package(tf2_sensor_msgs REQUIRED)
-#find_package(dynamic_reconfigure REQUIRED)
-#find_package(message_filters REQUIRED)
+#find_package(dynamic_reconfigure REQUIRED) 
 #find_package(voxel_grid REQUIRED)
 
 remove_definitions(-DDISABLE_LIBUSB-1.0)
 find_package(Eigen3 REQUIRED)
-find_package(Boost REQUIRED COMPONENTS system thread)
+#find_package(Boost REQUIRED COMPONENTS system thread)
 
 include_directories(
   include
   ${ament_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
 )
 
 add_definitions(${EIGEN3_DEFINITIONS})
 
-#	TODO(bpwilcox): Separate msgs generation into separate package
+# TODO(bpwilcox): Separate msgs generation into separate package
 #rosidl_generate_interfaces(${PROJECT_NAME}
 #  "msg/VoxelGrid.msg"
 #
 #	DEPENDENCIES std_msgs geometry_msgs map_msgs
 #)
 
-
 add_library(nav2_costmap_2d
   src/array_parser.cpp
   src/costmap_2d.cpp
@@ -64,10 +61,9 @@ add_library(nav2_costmap_2d
   src/costmap_2d_publisher.cpp
   src/costmap_math.cpp
   src/footprint.cpp
-  src/costmap_layer.cpp
-  # TODO(bpwilcox): need ROS PCL dependencies to port
-  #src/observation_buffer.cpp
-
+  src/costmap_layer.cpp	
+  #	TODO(bpwilcox): need tf2_sensor_msgs ported
+  # src/observation_buffer.cpp  
 )
 
 set(dependencies
@@ -80,21 +76,18 @@ set(dependencies
   xmlrpcpp
   pluginlib
   tf2_geometry_msgs
+  message_filters
 )
 
 ament_target_dependencies(nav2_costmap_2d
   ${dependencies}
 )
 
-target_link_libraries(nav2_costmap_2d
-  ${Boost_LIBRARIES}
-)
-
 add_library(layers
   plugins/inflation_layer.cpp
 
   # TODO(bpwilcox): port additional plugin layers
-  #plugins/static_layer.cpp
+  plugins/static_layer.cpp
   #plugins/obstacle_layer.cpp
   #plugins/voxel_layer.cpp
   #src/observation_buffer.cpp
@@ -102,19 +95,18 @@ add_library(layers
 ament_target_dependencies(layers
   ${dependencies}
  )
-target_link_libraries(layers
+ target_link_libraries(layers
   nav2_costmap_2d
-  ${Boost_LIBRARIES}
 )
 
-#	TODO(bpwilcox): port costmap_2d_markers
+# TODO(bpwilcox): port costmap_2d_markers
 #add_executable(costmap_2d_markers src/costmap_2d_markers.cpp)
 #ament_target_dependencies(costmap_2d_markers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS})
 #target_link_libraries(costmap_2d_markers
   #costmap_2d
 #)
 
-#	TODO(bpwilcox): port costmap_2d_cloud
+# TODO(bpwilcox): port costmap_2d_cloud
 #add_executable(costmap_2d_cloud src/costmap_2d_cloud.cpp)
 #ament_target_dependencies(costmap_2d_cloud ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS})
 #target_link_libraries(costmap_2d_cloud
diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_2d.h b/src/libs/costmap_2d/include/costmap_2d/costmap_2d.h
index ab852d9bc9..26fe0b3b36 100644
--- a/src/libs/costmap_2d/include/costmap_2d/costmap_2d.h
+++ b/src/libs/costmap_2d/include/costmap_2d/costmap_2d.h
@@ -40,8 +40,11 @@
 
 #include <vector>
 #include <queue>
+#include <mutex>
+#include <limits.h>
+#include <stdio.h>
+#include <string.h>
 #include <geometry_msgs/msg/point.hpp>
-#include <boost/thread.hpp>
 
 namespace costmap_2d
 {
@@ -296,7 +299,7 @@ class Costmap2D
   unsigned int cellDistance(double world_dist);
 
   // Provide a typedef to ease future code maintenance
-  typedef boost::recursive_mutex mutex_t;
+  typedef std::recursive_mutex mutex_t;
   mutex_t * getMutex()
   {
     return access_;
diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h
index 152e1b3c14..74152cb636 100644
--- a/src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h
+++ b/src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h
@@ -262,7 +262,7 @@ class Costmap2DROS
   void mapUpdateLoop(double frequency);
   bool map_update_thread_shutdown_;
   bool stop_updates_, initialized_, stopped_, robot_stopped_;
-  boost::thread * map_update_thread_;  ///< @brief A thread for updating the map
+  std::thread * map_update_thread_;  ///< @brief A thread for updating the map
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Time last_publish_;
   rclcpp::Duration publish_cycle_;
@@ -274,7 +274,7 @@ class Costmap2DROS
   //dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
   //costmap_2d::Costmap2DConfig old_config_;
 
-  boost::recursive_mutex configuration_mutex_;
+  std::recursive_mutex configuration_mutex_;
 
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_pub_;
   rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
diff --git a/src/libs/costmap_2d/include/costmap_2d/inflation_layer.h b/src/libs/costmap_2d/include/costmap_2d/inflation_layer.h
index a58a4a9739..71809c4603 100644
--- a/src/libs/costmap_2d/include/costmap_2d/inflation_layer.h
+++ b/src/libs/costmap_2d/include/costmap_2d/inflation_layer.h
@@ -44,7 +44,6 @@
 // TODO(bpwilcox): Resolve dynamic reconfigure dependencies
 //#include <costmap_2d/InflationPluginConfig.h>
 //#include <dynamic_reconfigure/server.h>
-#include <boost/thread.hpp>
 
 namespace costmap_2d
 {
@@ -129,7 +128,7 @@ class InflationLayer : public Layer
 
 protected:
   virtual void onFootprintChanged();
-  boost::recursive_mutex * inflation_access_;
+  std::recursive_mutex * inflation_access_;
 
 private:
   /**
diff --git a/src/libs/costmap_2d/include/costmap_2d/observation.h b/src/libs/costmap_2d/include/costmap_2d/observation.h
index 10df9517b2..8963c067b6 100644
--- a/src/libs/costmap_2d/include/costmap_2d/observation.h
+++ b/src/libs/costmap_2d/include/costmap_2d/observation.h
@@ -32,8 +32,8 @@
 #ifndef COSTMAP_2D_OBSERVATION_H_
 #define COSTMAP_2D_OBSERVATION_H_
 
-#include <geometry_msgs/Point.h>
-#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/msg/point.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 namespace costmap_2d
 {
@@ -66,7 +66,7 @@ class Observation
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    * @param raytrace_range The range out to which an observation should be able to clear via raytracing
    */
-  Observation(geometry_msgs::Point & origin, const sensor_msgs::PointCloud2 & cloud,
+  Observation(geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud,
       double obstacle_range, double raytrace_range)
     : origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
     obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
@@ -88,14 +88,14 @@ class Observation
    * @param cloud The point cloud of the observation
    * @param obstacle_range The range out to which an observation should be able to insert obstacles
    */
-  Observation(const sensor_msgs::PointCloud2 & cloud, double obstacle_range)
+  Observation(const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_range)
     : cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range),
     raytrace_range_(0.0)
   {
   }
 
-  geometry_msgs::Point origin_;
-  sensor_msgs::PointCloud2 * cloud_;
+  geometry_msgs::msg::Point origin_;
+  sensor_msgs::msg::PointCloud2 * cloud_;
   double obstacle_range_, raytrace_range_;
 };
 
diff --git a/src/libs/costmap_2d/include/costmap_2d/observation_buffer.h b/src/libs/costmap_2d/include/costmap_2d/observation_buffer.h
index d9219f1425..b7beebe8da 100644
--- a/src/libs/costmap_2d/include/costmap_2d/observation_buffer.h
+++ b/src/libs/costmap_2d/include/costmap_2d/observation_buffer.h
@@ -40,14 +40,12 @@
 #include <vector>
 #include <list>
 #include <string>
-#include <ros/time.h>
+#include "rclcpp/time.hpp"
 #include <costmap_2d/observation.h>
 #include <tf2_ros/buffer.h>
 
-#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
-// Thread support
-#include <boost/thread.hpp>
 
 namespace costmap_2d
 {
@@ -98,7 +96,7 @@ class ObservationBuffer
    * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
    * @param  cloud The cloud to be buffered
    */
-  void bufferCloud(const sensor_msgs::PointCloud2 & cloud);
+  void bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud);
 
   /**
    * @brief  Pushes copies of all current observations onto the end of the vector passed in
@@ -140,15 +138,16 @@ class ObservationBuffer
   void purgeStaleObservations();
 
   tf2_ros::Buffer & tf2_buffer_;
-  const ros::Duration observation_keep_time_;
-  const ros::Duration expected_update_rate_;
-  ros::Time last_updated_;
+  const rclcpp::Duration observation_keep_time_;
+  const rclcpp::Duration expected_update_rate_;
+  rclcpp::Clock clock_;
+  rclcpp::Time last_updated_;
   std::string global_frame_;
   std::string sensor_frame_;
   std::list<Observation> observation_list_;
   std::string topic_name_;
   double min_obstacle_height_, max_obstacle_height_;
-  boost::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
+  std::recursive_mutex lock_;  ///< @brief A lock for accessing data in callbacks safely
   double obstacle_range_, raytrace_range_;
   double tf_tolerance_;
 };
diff --git a/src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h b/src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h
index 64635334d0..9b64ba1cb9 100644
--- a/src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h
+++ b/src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h
@@ -86,7 +86,7 @@ class ObstacleLayer : public CostmapLayer
    * @param buffer A pointer to the observation buffer to update
    */
   void laserScanCallback(const sensor_msgs::LaserScanConstPtr & message,
-      const boost::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
+      const std::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
 
   /**
    * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
@@ -94,7 +94,7 @@ class ObstacleLayer : public CostmapLayer
    * @param buffer A pointer to the observation buffer to update
    */
   void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr & message,
-      const boost::shared_ptr<ObservationBuffer> & buffer);
+      const std::shared_ptr<ObservationBuffer> & buffer);
 
   /**
    * @brief  A callback to handle buffering PointCloud messages
@@ -102,7 +102,7 @@ class ObstacleLayer : public CostmapLayer
    * @param buffer A pointer to the observation buffer to update
    */
   void pointCloudCallback(const sensor_msgs::PointCloudConstPtr & message,
-      const boost::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
+      const std::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
 
   /**
    * @brief  A callback to handle buffering PointCloud2 messages
@@ -110,7 +110,7 @@ class ObstacleLayer : public CostmapLayer
    * @param buffer A pointer to the observation buffer to update
    */
   void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr & message,
-      const boost::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
+      const std::shared_ptr<costmap_2d::ObservationBuffer> & buffer);
 
   // for testing purposes
   void addStaticObservation(costmap_2d::Observation & obs, bool marking, bool clearing);
@@ -163,11 +163,11 @@ class ObstacleLayer : public CostmapLayer
 
   laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
 
-  std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
-  std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
-  std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
+  std::vector<std::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
+  std::vector<std::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
+  std::vector<std::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
+  std::vector<std::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
+  std::vector<std::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;  ///< @brief Used to store observation buffers used for clearing obstacles
 
   // Used only for testing purposes
   std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
diff --git a/src/libs/costmap_2d/include/costmap_2d/static_layer.h b/src/libs/costmap_2d/include/costmap_2d/static_layer.h
index 41b7365e21..f81f79960c 100644
--- a/src/libs/costmap_2d/include/costmap_2d/static_layer.h
+++ b/src/libs/costmap_2d/include/costmap_2d/static_layer.h
@@ -38,13 +38,13 @@
 #ifndef COSTMAP_2D_STATIC_LAYER_H_
 #define COSTMAP_2D_STATIC_LAYER_H_
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 #include <costmap_2d/costmap_layer.h>
 #include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/GenericPluginConfig.h>
-#include <dynamic_reconfigure/server.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <map_msgs/OccupancyGridUpdate.h>
+//#include <costmap_2d/GenericPluginConfig.h>
+//#include <dynamic_reconfigure/server.h>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <map_msgs/msg/occupancy_grid_update.hpp>
 #include <message_filters/subscriber.h>
 
 namespace costmap_2d
@@ -76,9 +76,11 @@ class StaticLayer : public CostmapLayer
    * map along with its size will determine what parts of the costmap's
    * static map are overwritten.
    */
-  void incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map);
-  void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr & update);
-  void reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level);
+  void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map);
+  void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update);
+  //TODO(bpwilcox): Replace dynamic_reconfigure functionality
+
+  //void reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level);
 
   unsigned char interpretValue(unsigned char value);
 
@@ -92,11 +94,12 @@ class StaticLayer : public CostmapLayer
   bool use_maximum_;
   bool first_map_only_;      ///< @brief Store the first static map and reuse it on reinitializing
   bool trinary_costmap_;
-  ros::Subscriber map_sub_, map_update_sub_;
-
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
+  rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;
   unsigned char lethal_threshold_, unknown_cost_value_;
 
-  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> * dsrv_;
+  //TODO(bpwilcox): Replace dynamic_reconfigure functionality
+  //dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> * dsrv_;
 };
 
 }  // namespace costmap_2d
diff --git a/src/libs/costmap_2d/include/costmap_2d/testing_helper.h b/src/libs/costmap_2d/include/costmap_2d/testing_helper.h
index 196603342d..957808a821 100644
--- a/src/libs/costmap_2d/include/costmap_2d/testing_helper.h
+++ b/src/libs/costmap_2d/include/costmap_2d/testing_helper.h
@@ -1,13 +1,14 @@
 #ifndef COSTMAP_2D_TESTING_HELPER_H
 #define COSTMAP_2D_TESTING_HELPER_H
 
+#include "rclcpp/rclcpp.hpp"
 #include <costmap_2d/cost_values.h>
 #include <costmap_2d/costmap_2d.h>
 #include <costmap_2d/static_layer.h>
 #include <costmap_2d/obstacle_layer.h>
 #include <costmap_2d/inflation_layer.h>
 
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 const double MAX_Z(1.0);
 
@@ -60,7 +61,7 @@ unsigned int countValues(costmap_2d::Costmap2D & costmap, unsigned char value, b
 void addStaticLayer(costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf)
 {
   costmap_2d::StaticLayer * slayer = new costmap_2d::StaticLayer();
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
+  layers.addPlugin(std::shared_ptr<costmap_2d::Layer>(slayer));
   slayer->initialize(&layers, "static", &tf);
 }
 
@@ -69,7 +70,7 @@ costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap & layers
 {
   costmap_2d::ObstacleLayer * olayer = new costmap_2d::ObstacleLayer();
   olayer->initialize(&layers, "obstacles", &tf);
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
+  layers.addPlugin(std::shared_ptr<costmap_2d::Layer>(olayer));
   return olayer;
 }
 
@@ -101,7 +102,7 @@ costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap & laye
 {
   costmap_2d::InflationLayer * ilayer = new costmap_2d::InflationLayer();
   ilayer->initialize(&layers, "inflation", &tf);
-  boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
+  std::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
   layers.addPlugin(ipointer);
   return ilayer;
 }
diff --git a/src/libs/costmap_2d/package.xml b/src/libs/costmap_2d/package.xml
index 8f4cbb9173..5f09f45cfd 100644
--- a/src/libs/costmap_2d/package.xml
+++ b/src/libs/costmap_2d/package.xml
@@ -42,6 +42,7 @@
   <depend>xmlrpcpp</depend>
   <depend>pluginlib</depend>
   <depend>libpcl-all-dev</depend>
+  <depend>message_filters</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/src/libs/costmap_2d/plugins/inflation_layer.cpp b/src/libs/costmap_2d/plugins/inflation_layer.cpp
index c8131c8e2c..ac58e749b1 100644
--- a/src/libs/costmap_2d/plugins/inflation_layer.cpp
+++ b/src/libs/costmap_2d/plugins/inflation_layer.cpp
@@ -39,7 +39,6 @@
 #include <costmap_2d/inflation_layer.h>
 #include <costmap_2d/costmap_math.h>
 #include <costmap_2d/footprint.h>
-#include <boost/thread.hpp>
 #include <pluginlib/class_list_macros.hpp>
 
 PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)
@@ -66,14 +65,14 @@ InflationLayer::InflationLayer()
   last_max_x_(std::numeric_limits<float>::max()),
   last_max_y_(std::numeric_limits<float>::max())
 {
-  inflation_access_ = new boost::recursive_mutex();
+  inflation_access_ = new std::recursive_mutex();
+  enabled_ = true;
 }
 
 void InflationLayer::onInitialize()
 {
   {
-    boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
-    //ros::NodeHandle nh("~/" + name_), g_nh;
+    std::unique_lock<std::recursive_mutex> lock(*inflation_access_);
 
     auto private_nh = rclcpp::Node::make_shared(name_);
     rclcpp::Node::SharedPtr g_nh;
@@ -88,7 +87,7 @@ void InflationLayer::onInitialize()
 
     // TODO(bpwilcox): Resolve dynamic reconfigure dependencies
     /*
-      dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(
+      dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = std::bind(
         &InflationLayer::reconfigureCB, this, _1, _2);
 
        if (dsrv_ != NULL){
@@ -103,6 +102,9 @@ void InflationLayer::onInitialize()
   }
 
   matchSize();
+  //TODO(bpwilcox): Values hard-coded from config file default, replace with dynamic approach
+  setInflationParameters(0.55, 10);
+
 }
 // TODO(bpwilcox): Resolve dynamic reconfigure dependencies
 /*
@@ -119,7 +121,7 @@ void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, ui
 
 void InflationLayer::matchSize()
 {
-  boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
+  std::unique_lock<std::recursive_mutex> lock(*inflation_access_);
   costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap();
   resolution_ = costmap->getResolution();
   cell_inflation_radius_ = cellDistance(inflation_radius_);
@@ -182,7 +184,7 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i,
     int max_i,
     int max_j)
 {
-  boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
+  std::unique_lock<std::recursive_mutex> lock(*inflation_access_);
   if (!enabled_ || (cell_inflation_radius_ == 0)) {
     return;
   }
@@ -376,7 +378,7 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost
   if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius) {
     // Lock here so that reconfiguring the inflation radius doesn't cause segfaults
     // when accessing the cached arrays
-    boost::unique_lock<boost::recursive_mutex> lock(*inflation_access_);
+    std::unique_lock<std::recursive_mutex> lock(*inflation_access_);
 
     inflation_radius_ = inflation_radius;
     cell_inflation_radius_ = cellDistance(inflation_radius_);
diff --git a/src/libs/costmap_2d/plugins/obstacle_layer.cpp b/src/libs/costmap_2d/plugins/obstacle_layer.cpp
index 7ba38b1646..bba20ef34e 100644
--- a/src/libs/costmap_2d/plugins/obstacle_layer.cpp
+++ b/src/libs/costmap_2d/plugins/obstacle_layer.cpp
@@ -128,7 +128,7 @@ void ObstacleLayer::onInitialize()
 
     // create an observation buffer
     observation_buffers_.push_back(
-        boost::shared_ptr<ObservationBuffer
+        std::shared_ptr<ObservationBuffer
         >(new ObservationBuffer(topic, observation_keep_time, expected_update_rate,
             min_obstacle_height,
             max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
@@ -152,17 +152,17 @@ void ObstacleLayer::onInitialize()
 
     // create a callback for the topic
     if (data_type == "LaserScan") {
-      boost::shared_ptr<message_filters::Subscriber<sensor_msgs::LaserScan>
+      std::shared_ptr<message_filters::Subscriber<sensor_msgs::LaserScan>
       > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
 
-      boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
+      std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
           new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
 
       if (inf_is_valid) {
-        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1,
+        filter->registerCallback(std::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1,
               observation_buffers_.back()));
       } else {
-        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanCallback, this, _1,
+        filter->registerCallback(std::bind(&ObstacleLayer::laserScanCallback, this, _1,
               observation_buffers_.back()));
       }
 
@@ -171,7 +171,7 @@ void ObstacleLayer::onInitialize()
 
       observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
     } else if (data_type == "PointCloud") {
-      boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud>
+      std::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud>
       > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
 
       if (inf_is_valid) {
@@ -179,16 +179,16 @@ void ObstacleLayer::onInitialize()
             "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
       }
 
-      boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud>
+      std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud>
       > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50,
             g_nh));
       filter->registerCallback(
-          boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));
+          std::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));
 
       observation_subscribers_.push_back(sub);
       observation_notifiers_.push_back(filter);
     } else {
-      boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>
+      std::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>
       > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
 
       if (inf_is_valid) {
@@ -196,11 +196,11 @@ void ObstacleLayer::onInitialize()
             "obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
       }
 
-      boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
+      std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
       > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50,
             g_nh));
       filter->registerCallback(
-          boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));
+          std::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));
 
       observation_subscribers_.push_back(sub);
       observation_notifiers_.push_back(filter);
@@ -221,7 +221,7 @@ void ObstacleLayer::onInitialize()
 void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle & nh)
 {
   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
-  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = boost::bind(
+  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = std::bind(
       &ObstacleLayer::reconfigureCB, this, _1, _2);
   dsrv_->setCallback(cb);
 }
@@ -241,7 +241,7 @@ void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig & config, uin
 }
 
 void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr & message,
-    const boost::shared_ptr<ObservationBuffer> & buffer)
+    const std::shared_ptr<ObservationBuffer> & buffer)
 {
   // project the laser into a point cloud
   sensor_msgs::PointCloud2 cloud;
@@ -264,7 +264,7 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr & mes
 }
 
 void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr & raw_message,
-    const boost::shared_ptr<ObservationBuffer> & buffer)
+    const std::shared_ptr<ObservationBuffer> & buffer)
 {
   // Filter positive infinities ("Inf"s) to max_range.
   float epsilon = 0.0001;  // a tenth of a millimeter
@@ -296,7 +296,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstP
 }
 
 void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr & message,
-    const boost::shared_ptr<ObservationBuffer> & buffer)
+    const std::shared_ptr<ObservationBuffer> & buffer)
 {
   sensor_msgs::PointCloud2 cloud2;
 
@@ -312,7 +312,7 @@ void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr & m
 }
 
 void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr & message,
-    const boost::shared_ptr<ObservationBuffer> & buffer)
+    const std::shared_ptr<ObservationBuffer> & buffer)
 {
   // buffer the point cloud
   buffer->lock();
diff --git a/src/libs/costmap_2d/plugins/static_layer.cpp b/src/libs/costmap_2d/plugins/static_layer.cpp
index 6d22d31d52..ccf2e5e021 100644
--- a/src/libs/costmap_2d/plugins/static_layer.cpp
+++ b/src/libs/costmap_2d/plugins/static_layer.cpp
@@ -38,10 +38,11 @@
  *********************************************************************/
 #include <costmap_2d/static_layer.h>
 #include <costmap_2d/costmap_math.h>
-#include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_list_macros.hpp>
 #include <tf2/convert.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+
 PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)
 
 using costmap_2d::NO_INFORMATION;
@@ -51,76 +52,89 @@ using costmap_2d::FREE_SPACE;
 namespace costmap_2d
 {
 
-StaticLayer::StaticLayer() : dsrv_(NULL) {}
+StaticLayer::StaticLayer() {enabled_ = true;}
 
 StaticLayer::~StaticLayer()
 {
-  if (dsrv_) {
+  /* if (dsrv_) {
     delete dsrv_;
-  }
+  } */
 }
 
 void StaticLayer::onInitialize()
 {
-  ros::NodeHandle nh("~/" + name_), g_nh;
+  auto nh = rclcpp::Node::make_shared(name_);
+  rclcpp::Node::SharedPtr g_nh;
+  g_nh = rclcpp::Node::make_shared("costmap_2d_static");
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(nh);
+
   current_ = true;
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
 
   std::string map_topic;
-  nh.param("map_topic", map_topic, std::string("map"));
-  nh.param("first_map_only", first_map_only_, false);
-  nh.param("subscribe_to_updates", subscribe_to_updates_, false);
 
-  nh.param("track_unknown_space", track_unknown_space_, true);
-  nh.param("use_maximum", use_maximum_, false);
+  map_topic = parameters_client->get_parameter<std::string>("map_topic", std::string("occ_grid"));
+  first_map_only_ = parameters_client->get_parameter<bool>("first_map_only", false);
+  subscribe_to_updates_ = parameters_client->get_parameter<bool>("subscribe_to_updates", false);
+
+  track_unknown_space_ = parameters_client->get_parameter<bool>("track_unknown_space", true);
+  use_maximum_ = parameters_client->get_parameter<bool>("use_maximum", false);
 
   int temp_lethal_threshold, temp_unknown_cost_value;
-  nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
-  nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
-  nh.param("trinary_costmap", trinary_costmap_, true);
+  temp_lethal_threshold = parameters_client->get_parameter<int>("lethal_cost_threshold", 100);
+  temp_unknown_cost_value = parameters_client->get_parameter<int>("unknown_cost_value", -1);
+  trinary_costmap_ = parameters_client->get_parameter<bool>("trinary_costmap", true);
 
   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
   unknown_cost_value_ = temp_unknown_cost_value;
 
   // Only resubscribe if topic has changed
-  if (map_sub_.getTopic() != ros::names::resolve(map_topic)) {
-    // we'll subscribe to the latched topic that the map server uses
-    ROS_INFO("Requesting the map...");
-    map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
-    map_received_ = false;
-    has_updated_data_ = false;
-
-    ros::Rate r(10);
-    while (!map_received_ && g_nh.ok()) {
-      ros::spinOnce();
-      r.sleep();
-    }
+  //TODO(bpwilcox): resolve ROS2 equivalent to check whether topic has changed. Problem calling get_topic before topic created
+  //if (map_sub_.getTopic() != ros::names::resolve(map_topic)) {
+
+  // we'll subscribe to the latched topic that the map server uses
+  RCLCPP_INFO(rclcpp::get_logger("costmap_2d"), "Requesting the map...");
+  rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
+  custom_qos_profile.depth = 1;
+  custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  map_sub_ = g_nh->create_subscription<nav_msgs::msg::OccupancyGrid>(map_topic,
+      std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1), custom_qos_profile);
+  map_received_ = false;
+  has_updated_data_ = false;
 
-    ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(),
-        getSizeInCellsY(), getResolution());
+  rclcpp::Rate r(10);
+  while (!map_received_ && rclcpp::ok()) {
+    rclcpp::spin_some(g_nh);
+    r.sleep();
+  }
 
-    if (subscribe_to_updates_) {
-      ROS_INFO("Subscribing to updates");
-      map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate,
-          this);
+  RCLCPP_INFO(rclcpp::get_logger(
+        "costmap_2d"), "Received a %d X %d map at %f m/pix", getSizeInCellsX(),
+      getSizeInCellsY(), getResolution());
+
+  if (subscribe_to_updates_) {
+    RCLCPP_INFO(rclcpp::get_logger("costmap_2d"), "Subscribing to updates");
+    map_update_sub_ = g_nh->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
+        map_topic + "_updates",
+        std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1), custom_qos_profile);
 
-    }
-  } else {
-    has_updated_data_ = true;
   }
+/*   } else {
+    has_updated_data_ = true;
+  } */
 
-  if (dsrv_) {
+  /* if (dsrv_) {
     delete dsrv_;
-  }
+  } */
 
-  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
-  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
-      &StaticLayer::reconfigureCB, this, _1, _2);
-  dsrv_->setCallback(cb);
+  //dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
+  //dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = std::bind(
+  //    &StaticLayer::reconfigureCB, this, _1, _2);
+  //dsrv_->setCallback(cb);
 }
 
-void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level)
+/* void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level)
 {
   if (config.enabled != enabled_) {
     enabled_ = config.enabled;
@@ -129,7 +143,7 @@ void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32
     width_ = size_x_;
     height_ = size_y_;
   }
-}
+} */
 
 void StaticLayer::matchSize()
 {
@@ -159,11 +173,13 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
   return scale * LETHAL_OBSTACLE;
 }
 
-void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map)
+void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
 {
   unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
 
-  ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
+  RCLCPP_DEBUG(rclcpp::get_logger(
+        "costmap_2d"), "Received a %d X %d map at %f m/pix", size_x, size_y,
+      new_map->info.resolution);
 
   // resize costmap if size, resolution or origin do not match
   Costmap2D * master = layered_costmap_->getCostmap();
@@ -175,7 +191,9 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map)
         !layered_costmap_->isSizeLocked()))
   {
     // Update the size of the layered costmap (and all layers, including this one)
-    ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
+    RCLCPP_INFO(rclcpp::get_logger(
+          "costmap_2d"), "Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
+        new_map->info.resolution);
     layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution,
         new_map->info.origin.position.x,
         new_map->info.origin.position.y,
@@ -186,7 +204,8 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map)
       origin_y_ != new_map->info.origin.position.y)
   {
     // only update the size of the costmap stored locally in this layer
-    ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
+    RCLCPP_INFO(rclcpp::get_logger(
+          "costmap_2d"), "Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
         new_map->info.resolution);
     resizeMap(size_x, size_y, new_map->info.resolution,
         new_map->info.origin.position.x, new_map->info.origin.position.y);
@@ -202,6 +221,7 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map)
       ++index;
     }
   }
+
   map_frame_ = new_map->header.frame_id;
 
   // we have a new map, update full size of map
@@ -213,12 +233,14 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr & new_map)
 
   // shutdown the map subscrber if firt_map_only_ flag is on
   if (first_map_only_) {
-    ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
-    map_sub_.shutdown();
+    RCLCPP_INFO(rclcpp::get_logger(
+          "costmap_2d"), "Shutting down the map subscriber. first_map_only flag is on");
+    // TODO(bpwilcox): Resolve shutdown of ros2 subscription
+    //map_sub_.shutdown();
   }
 }
 
-void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr & update)
+void StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
 {
   unsigned int di = 0;
   for (unsigned int y = 0; y < update->height; y++) {
@@ -242,9 +264,10 @@ void StaticLayer::activate()
 
 void StaticLayer::deactivate()
 {
-  map_sub_.shutdown();
+  // TODO(bpwilcox): Resolve shutdown of ros2 subscription
+  //map_sub_.shutdown();
   if (subscribe_to_updates_) {
-    map_update_sub_.shutdown();
+    //map_update_sub_.shutdown();
   }
 }
 
@@ -307,16 +330,17 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, in
     unsigned int mx, my;
     double wx, wy;
     // Might even be in a different frame
-    geometry_msgs::TransformStamped transform;
+    geometry_msgs::msg::TransformStamped transform;
     try {
-      transform = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
+      transform = tf_->lookupTransform(map_frame_, global_frame_, tf2_ros::fromMsg(rclcpp::Time()));
     } catch (tf2::TransformException ex) {
-      ROS_ERROR("%s", ex.what());
+      RCLCPP_ERROR(rclcpp::get_logger("costmap_2d"), "%s", ex.what());
       return;
     }
     // Copy map data given proper transformations
     tf2::Transform tf2_transform;
-    tf2::convert(transform.transform, tf2_transform);
+    tf2::fromMsg(transform.transform, tf2_transform);
+
     for (unsigned int i = min_i; i < max_i; ++i) {
       for (unsigned int j = min_j; j < max_j; ++j) {
         // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
diff --git a/src/libs/costmap_2d/plugins/voxel_layer.cpp b/src/libs/costmap_2d/plugins/voxel_layer.cpp
index 32f90ca021..5070a1b8ca 100644
--- a/src/libs/costmap_2d/plugins/voxel_layer.cpp
+++ b/src/libs/costmap_2d/plugins/voxel_layer.cpp
@@ -68,7 +68,7 @@ void VoxelLayer::onInitialize()
 void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle & nh)
 {
   voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
-  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
+  dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = std::bind(
       &VoxelLayer::reconfigureCB, this, _1, _2);
   voxel_dsrv_->setCallback(cb);
 }
diff --git a/src/libs/costmap_2d/src/costmap_2d.cpp b/src/libs/costmap_2d/src/costmap_2d.cpp
index 4be80a0cba..fa5156bbed 100644
--- a/src/libs/costmap_2d/src/costmap_2d.cpp
+++ b/src/libs/costmap_2d/src/costmap_2d.cpp
@@ -57,14 +57,14 @@ Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, doubl
 void Costmap2D::deleteMaps()
 {
   // clean up data
-  boost::unique_lock<mutex_t> lock(*access_);
+  std::unique_lock<mutex_t> lock(*access_);
   delete[] costmap_;
   costmap_ = NULL;
 }
 
 void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
 {
-  boost::unique_lock<mutex_t> lock(*access_);
+  std::unique_lock<mutex_t> lock(*access_);
   delete[] costmap_;
   costmap_ = new unsigned char[size_x * size_y];
 }
@@ -86,13 +86,13 @@ void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, double resol
 
 void Costmap2D::resetMaps()
 {
-  boost::unique_lock<mutex_t> lock(*access_);
+  std::unique_lock<mutex_t> lock(*access_);
   memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
 }
 
 void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
 {
-  boost::unique_lock<mutex_t> lock(*(access_));
+  std::unique_lock<mutex_t> lock(*(access_));
   unsigned int len = xn - x0;
   for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_) {
     memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
diff --git a/src/libs/costmap_2d/src/costmap_2d_cloud.cpp b/src/libs/costmap_2d/src/costmap_2d_cloud.cpp
index 7cd300bf03..09c9dfe836 100644
--- a/src/libs/costmap_2d/src/costmap_2d_cloud.cpp
+++ b/src/libs/costmap_2d/src/costmap_2d_cloud.cpp
@@ -191,7 +191,7 @@ int main(int argc, char ** argv)
   ros::Publisher pub_marked = n.advertise<sensor_msgs::PointCloud>("voxel_marked_cloud", 2);
   ros::Publisher pub_unknown = n.advertise<sensor_msgs::PointCloud>("voxel_unknown_cloud", 2);
   ros::Subscriber sub = n.subscribe<costmap_2d::VoxelGrid
-    >("voxel_grid", 1, boost::bind(voxelCallback, pub_marked, pub_unknown, _1));
+    >("voxel_grid", 1, std::bind(voxelCallback, pub_marked, pub_unknown, _1));
 
   ros::spin();
 }
diff --git a/src/libs/costmap_2d/src/costmap_2d_markers.cpp b/src/libs/costmap_2d/src/costmap_2d_markers.cpp
index dfb60042dc..078be357a3 100644
--- a/src/libs/costmap_2d/src/costmap_2d_markers.cpp
+++ b/src/libs/costmap_2d/src/costmap_2d_markers.cpp
@@ -142,7 +142,7 @@ int main(int argc, char ** argv)
 
   ros::Publisher pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
   ros::Subscriber sub =
-    n.subscribe<costmap_2d::VoxelGrid>("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
+    n.subscribe<costmap_2d::VoxelGrid>("voxel_grid", 1, std::bind(voxelCallback, pub, _1));
   g_marker_ns = n.resolveName("voxel_grid");
 
   ros::spin();
diff --git a/src/libs/costmap_2d/src/costmap_2d_publisher.cpp b/src/libs/costmap_2d/src/costmap_2d_publisher.cpp
index 5eaa6f2ee5..df426fb3fe 100644
--- a/src/libs/costmap_2d/src/costmap_2d_publisher.cpp
+++ b/src/libs/costmap_2d/src/costmap_2d_publisher.cpp
@@ -35,7 +35,6 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <boost/bind.hpp>
 #include <costmap_2d/costmap_2d_publisher.h>
 #include <costmap_2d/cost_values.h>
 
@@ -96,7 +95,7 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher&
 // prepare grid_ message for publication.
 void Costmap2DPublisher::prepareGrid()
 {
-  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
   double resolution = costmap_->getResolution();
 
   grid_.header.frame_id = global_frame_;
@@ -145,7 +144,7 @@ void Costmap2DPublisher::publishCostmap()
     prepareGrid();
     costmap_pub_->publish(grid_);
   } else if (x0_ < xn_) {
-    boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+    std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
     // Publish Just an Update
     map_msgs::msg::OccupancyGridUpdate update;
     update.header.stamp = rclcpp::Time();
diff --git a/src/libs/costmap_2d/src/costmap_2d_ros.cpp b/src/libs/costmap_2d/src/costmap_2d_ros.cpp
index eb71ccf3b6..7530c44551 100644
--- a/src/libs/costmap_2d/src/costmap_2d_ros.cpp
+++ b/src/libs/costmap_2d/src/costmap_2d_ros.cpp
@@ -39,6 +39,7 @@
 #include <costmap_2d/costmap_2d_ros.h>
 #include <cstdio>
 #include <string>
+#include <sys/time.h>
 #include <algorithm>
 #include <vector>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -183,7 +184,7 @@ Costmap2DROS::Costmap2DROS(const std::string & name, tf2_ros::Buffer & tf)
 
   // TODO(bpwilcox): resolve dynamic reconfigure dependencies
   //dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
-  //dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
+  //dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = std::bind(&Costmap2DROS::reconfigureCB, this, _1,
   //                                                                            _2);
   //dsrv_->setCallback(cb);
 }
@@ -231,9 +232,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh)
       super_map.setStruct(&map);
       plugins.push_back(super_map);
 
-      //ros::NodeHandle map_layer(nh, "static_layer");
-      //auto map_layer = rclcpp::Node::make_shared("static_layer",nh->get_name);
-      auto map_layer = rclcpp::Node::make_shared("static_layer");
+      auto map_layer = rclcpp::Node::make_shared("static_layer", std::string(nh->get_name()));
 
       move_parameter(nh, map_layer, "map_topic");
       move_parameter(nh, map_layer, "unknown_cost_value");
@@ -241,7 +240,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh)
       move_parameter(nh, map_layer, "track_unknown_space", false);
     }
   }
-  auto obstacles = rclcpp::Node::make_shared("obstacle_layer");
+  auto obstacles = rclcpp::Node::make_shared("obstacle_layer", std::string(nh->get_name()));
   //ros::NodeHandle obstacles(nh, "obstacle_layer");
 
   if (parameters_client->has_parameter("map_type")) {
@@ -279,7 +278,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh)
   }
   move_parameter(nh, obstacles, "observation_sources");
 
-  auto inflation = rclcpp::Node::make_shared("obstacle_layer");
+  auto inflation = rclcpp::Node::make_shared("obstacle_layer", std::string(nh->get_name()));
   //ros::NodeHandle inflation(nh, "inflation_layer");
 
   move_parameter(nh, inflation, "cost_scaling_factor");
@@ -340,7 +339,7 @@ void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t l
 
   old_config_ = config;
 
-  map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
+  map_update_thread_ = new std::thread(std::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
 } */
 
 // TODO(bpwilcox): resolve dynamic reconfigure dependencies
@@ -392,7 +391,7 @@ void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::ms
 void Costmap2DROS::movementCB()
 {
   // don't allow configuration to happen while this check occurs
-  // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
+  // std::recursive_mutex::scoped_lock mcl(configuration_mutex_);
 
   geometry_msgs::msg::PoseStamped new_pose;
   if (!getRobotPose(new_pose)) {
diff --git a/src/libs/costmap_2d/src/footprint.cpp b/src/libs/costmap_2d/src/footprint.cpp
index 118221f339..f7d7b88bb9 100644
--- a/src/libs/costmap_2d/src/footprint.cpp
+++ b/src/libs/costmap_2d/src/footprint.cpp
@@ -29,9 +29,6 @@
 #include <string>
 
 #include <costmap_2d/costmap_math.h>
-#include <boost/tokenizer.hpp>
-#include <boost/foreach.hpp>
-#include <boost/algorithm/string.hpp>
 #include <costmap_2d/footprint.h>
 #include <costmap_2d/array_parser.h>
 #include <geometry_msgs/msg/point32.hpp>
diff --git a/src/libs/costmap_2d/src/layered_costmap.cpp b/src/libs/costmap_2d/src/layered_costmap.cpp
index a8160d7fe0..a6c9e89e3f 100644
--- a/src/libs/costmap_2d/src/layered_costmap.cpp
+++ b/src/libs/costmap_2d/src/layered_costmap.cpp
@@ -84,7 +84,7 @@ void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double
     double origin_y,
     bool size_locked)
 {
-  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
   size_locked_ = size_locked;
   costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
   for (vector<std::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
@@ -98,7 +98,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
 {
   // Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
   // implement thread unsafe updateBounds() functions.
-  boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
+  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
 
   // if we're using a rolling buffer costmap... we need to update the origin using the robot's position
   if (rolling_window_) {
diff --git a/src/libs/costmap_2d/src/observation_buffer.cpp b/src/libs/costmap_2d/src/observation_buffer.cpp
index eff0de57b2..df16aa5117 100644
--- a/src/libs/costmap_2d/src/observation_buffer.cpp
+++ b/src/libs/costmap_2d/src/observation_buffer.cpp
@@ -38,7 +38,7 @@
 
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
-#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 using namespace std;
 using namespace tf2;
@@ -53,7 +53,7 @@ ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_
     double tf_tolerance)
   : tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time),
   expected_update_rate_(expected_update_rate),
-  last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame),
+  last_updated_(clock_.now()), global_frame_(global_frame), sensor_frame_(sensor_frame),
   topic_name_(topic_name),
   min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
   obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
@@ -66,14 +66,17 @@ ObservationBuffer::~ObservationBuffer()
 
 bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
 {
-  ros::Time transform_time = ros::Time::now();
+  //ros::Time transform_time = ros::Time::now();
+  rclcpp::Time transform_time = clock_.now();
   std::string tf_error;
 
-  geometry_msgs::TransformStamped transformStamped;
+  geometry_msgs::msg::TransformStamped transformStamped;
   if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time,
-        ros::Duration(tf_tolerance_), &tf_error))
+        tf2::durationFromSec(tf_tolerance_), &tf_error))
   {
-    ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
+    RCLCPP_ERROR(rclcpp::get_logger(
+          "costmap_2d"), "Transform between %s and %s with tolerance %.2f failed: %s.",
+        new_global_frame.c_str(),
         global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
     return false;
   }
@@ -83,7 +86,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
     try {
       Observation & obs = *obs_it;
 
-      geometry_msgs::PointStamped origin;
+      geometry_msgs::msg::PointStamped origin;
       origin.header.frame_id = global_frame_;
       origin.header.stamp = transform_time;
       origin.point = obs.origin_;
@@ -95,7 +98,8 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
       // we also need to transform the cloud of the observation to the new global frame
       tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
     } catch (TransformException & ex) {
-      ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s",
+      RCLCPP_ERROR(rclcpp::get_logger(
+            "costmap_2d"), "TF Error attempting to transform an observation from %s to %s: %s",
           global_frame_.c_str(),
           new_global_frame.c_str(), ex.what());
       return false;
@@ -107,9 +111,9 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
   return true;
 }
 
-void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2 & cloud)
+void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
 {
-  geometry_msgs::PointStamped global_origin;
+  geometry_msgs::msg::PointStamped global_origin;
 
   // create a new observation on the list to be populated
   observation_list_.push_front(Observation());
@@ -119,7 +123,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2 & cloud)
 
   try {
     // given these observations come from sensors... we'll need to store the origin pt of the sensor
-    geometry_msgs::PointStamped local_origin;
+    geometry_msgs::msg::PointStamped local_origin;
     local_origin.header.stamp = cloud.header.stamp;
     local_origin.header.frame_id = origin_frame;
     local_origin.point.x = 0;
@@ -132,14 +136,14 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2 & cloud)
     observation_list_.front().raytrace_range_ = raytrace_range_;
     observation_list_.front().obstacle_range_ = obstacle_range_;
 
-    sensor_msgs::PointCloud2 global_frame_cloud;
+    sensor_msgs::msg::PointCloud2 global_frame_cloud;
 
     // transform the point cloud
     tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
     global_frame_cloud.header.stamp = cloud.header.stamp;
 
     // now we need to remove observations from the cloud that are below or above our height thresholds
-    sensor_msgs::PointCloud2 & observation_cloud = *(observation_list_.front().cloud_);
+    sensor_msgs::msg::PointCloud2 & observation_cloud = *(observation_list_.front().cloud_);
     observation_cloud.height = global_frame_cloud.height;
     observation_cloud.width = global_frame_cloud.width;
     observation_cloud.fields = global_frame_cloud.fields;
@@ -177,14 +181,16 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2 & cloud)
   } catch (TransformException & ex) {
     // if an exception occurs, we need to remove the empty observation from the list
     observation_list_.pop_front();
-    ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
+    RCLCPP_ERROR(rclcpp::get_logger(
+          "costmap_2d"),
+        "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
         sensor_frame_.c_str(),
         cloud.header.frame_id.c_str(), ex.what());
     return;
   }
 
   // if the update was successful, we want to update the last updated time
-  last_updated_ = ros::Time::now();
+  last_updated_ = clock_.now();
 
   // we'll also remove any stale observations from the list
   purgeStaleObservations();
@@ -208,7 +214,7 @@ void ObservationBuffer::purgeStaleObservations()
   if (!observation_list_.empty()) {
     list<Observation>::iterator obs_it = observation_list_.begin();
     // if we're keeping observations for no time... then we'll only keep one observation
-    if (observation_keep_time_ == ros::Duration(0.0)) {
+    if (observation_keep_time_ == rclcpp::Duration(0.0)) {
       observation_list_.erase(++obs_it, observation_list_.end());
       return;
     }
@@ -227,22 +233,23 @@ void ObservationBuffer::purgeStaleObservations()
 
 bool ObservationBuffer::isCurrent() const
 {
-  if (expected_update_rate_ == ros::Duration(0.0)) {
+  if (expected_update_rate_ == rclcpp::Duration(0.0)) {
     return true;
   }
 
-  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
+  bool current = (clock_.now() - last_updated_).toSec() <= expected_update_rate_.toSec();
   if (!current) {
-    ROS_WARN(
+    RCLCPP_WARN(rclcpp::get_logger(
+          "costmap_2d"),
         "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
         topic_name_.c_str(),
-        (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
+        (clock_.now() - last_updated_).toSec(), expected_update_rate_.toSec());
   }
   return current;
 }
 
 void ObservationBuffer::resetLastUpdated()
 {
-  last_updated_ = ros::Time::now();
+  last_updated_ = clock_.now();
 }
 }  // namespace costmap_2d