diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7b2aa44..a638b93 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,6 +1,6 @@
 # --- CMake version và project name ---
 cmake_minimum_required(VERSION 3.10)
-project(costmap_2d)
+project(robot_costmap_2d)
 
 # --- C++ standard và position independent code ---
 set(CMAKE_CXX_STANDARD 17)            # Sử dụng C++17
@@ -10,8 +10,8 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)  # Thư viện có thể build thành sh
 # Dùng để runtime linker tìm thư viện đã build trước khi install
 set(CMAKE_SKIP_BUILD_RPATH FALSE)
 set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
+set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
+set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # --- Dependencies ---
@@ -23,7 +23,7 @@ find_package(GTest REQUIRED)           # Google Test cho unit test
 find_package(PCL REQUIRED COMPONENTS common io)   # Point Cloud Library
 
 # --- Define macro để dùng trong code ---
-add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+add_definitions(-DROBOT_COSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
 
 # --- Include directories ---
 # Thêm các folder chứa header files
@@ -39,9 +39,9 @@ link_directories(${PCL_LIBRARY_DIRS})  # Thêm thư viện PCL vào linker path
 # --- Eigen và PCL definitions ---
 add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
 
-# --- Core library: costmap_2d ---
+# --- Core library: robot_costmap_2d ---
 # Tạo thư viện chính
-add_library(costmap_2d
+add_library(robot_costmap_2d
     src/costmap_2d_robot.cpp
     src/array_parser.cpp
     src/costmap_2d.cpp
@@ -54,7 +54,7 @@ add_library(costmap_2d
 )
 
 # --- Link các thư viện phụ thuộc ---
-target_link_libraries(costmap_2d
+target_link_libraries(robot_costmap_2d
     ${Boost_LIBRARIES}        # Boost
     robot_std_msgs                   
     robot_sensor_msgs
@@ -65,8 +65,8 @@ target_link_libraries(costmap_2d
     robot_visualization_msgs
     voxel_grid
     tf3
-    tf3_geometry_msgs
-    tf3_sensor_msgs
+    robot_tf3_geometry_msgs
+    robot_tf3_sensor_msgs
     data_convert
     robot_xmlrpcpp                  # XMLRPC
     yaml-cpp
@@ -75,7 +75,7 @@ target_link_libraries(costmap_2d
 )
 
 # --- Include directories cho target ---
-target_include_directories(costmap_2d 
+target_include_directories(robot_costmap_2d 
         PUBLIC 
             ${Boost_INCLUDE_DIRS}  # Boost headers
             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # Khi build từ source
@@ -83,23 +83,23 @@ target_include_directories(costmap_2d
         )
 
 # --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-install(TARGETS costmap_2d
-  EXPORT costmap_2d-targets
+install(TARGETS robot_costmap_2d
+  EXPORT robot_costmap_2d-targets
   ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
   LIBRARY DESTINATION lib      # Thư viện động .so
   RUNTIME DESTINATION bin      # File thực thi (nếu có)
   INCLUDES DESTINATION include # Cài đặt include
 )
 
-# --- Xuất export set costmap_2dTargets thành file CMake module ---
-# --- Tạo file lib/cmake/costmap_2d/costmap_2dTargets.cmake ---
+# --- Xuất export set robot_costmap_2dTargets thành file CMake module ---
+# --- Tạo file lib/cmake/robot_costmap_2d/robot_costmap_2dTargets.cmake ---
 # --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(costmap_2d REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE costmap_2d::costmap_2d) ---
-install(EXPORT costmap_2d-targets 
-  FILE costmap_2d-targets.cmake
-  NAMESPACE costmap_2d::
-  DESTINATION lib/cmake/costmap_2d
+# --- Find_package(robot_costmap_2d REQUIRED) ---
+# --- Target_link_libraries(my_app PRIVATE robot_costmap_2d::robot_costmap_2d) ---
+install(EXPORT robot_costmap_2d-targets 
+  FILE robot_costmap_2d-targets.cmake
+  NAMESPACE robot_costmap_2d::
+  DESTINATION lib/cmake/robot_costmap_2d
 )
 
 # --- Cài đặt headers ---
@@ -123,7 +123,7 @@ add_library(plugins
 
 target_link_libraries(plugins 
     PRIVATE
-        costmap_2d 
+        robot_costmap_2d 
         ${Boost_LIBRARIES} 
         yaml-cpp
         robot_time
@@ -144,13 +144,13 @@ install(TARGETS plugins
 
 install(EXPORT plugins-targets 
   FILE plugins-targets.cmake
-  NAMESPACE costmap_2d::
+  NAMESPACE robot_costmap_2d::
   DESTINATION lib/cmake/plugins
 )
 
 
 # --- Option để bật/tắt test ---
-option(BUILD_COSTMAP_TESTS "Build costmap_2d test executables" ON)
+option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)
 
 if(BUILD_COSTMAP_TESTS)
     # --- Test executables ---
@@ -159,8 +159,8 @@ if(BUILD_COSTMAP_TESTS)
     add_executable(test_plugin test/static_layer_test.cpp)
 
     # --- Link thư viện cho test ---
-    target_link_libraries(test_array_parser PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
-    target_link_libraries(test_costmap PRIVATE costmap_2d GTest::GTest GTest::Main pthread)
+    target_link_libraries(test_array_parser PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
+    target_link_libraries(test_costmap PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
     target_link_libraries(test_plugin PRIVATE 
     
         ${Boost_LIBRARIES}
@@ -171,24 +171,24 @@ if(BUILD_COSTMAP_TESTS)
         yaml-cpp
         tf3
         robot_time
-        costmap_2d
+        robot_costmap_2d
         GTest::GTest GTest::Main
     )
     
     # --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries ---
     set_target_properties(test_array_parser PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
         LINK_FLAGS "-Wl,--disable-new-dtags"
     )
     set_target_properties(test_costmap PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
         LINK_FLAGS "-Wl,--disable-new-dtags"
     )
     set_target_properties(test_plugin PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
         LINK_FLAGS "-Wl,--disable-new-dtags"
     )
 endif()
\ No newline at end of file
diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml
index f24eb96..35e2c17 100644
--- a/config/costmap_params.yaml
+++ b/config/costmap_params.yaml
@@ -1,4 +1,4 @@
-costmap_2d:
+robot_costmap_2d:
   global_frame: map
   robot_base_frame: base_link
   rolling_window: false
diff --git a/include/costmap_2d/critical_layer.h b/include/costmap_2d/critical_layer.h
deleted file mode 100644
index 86e354f..0000000
--- a/include/costmap_2d/critical_layer.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
-#define COSTMAP_2D_CRITICAL_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class CriticalLayer : public StaticLayer
-{
-public:
-  CriticalLayer();
-  virtual ~CriticalLayer();
-private:
-  unsigned char interpretValue(unsigned char value) override;
-  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
-};
-}
-
-#endif // COSTMAP_2D_CRITICAL_LAYER_H_
\ No newline at end of file
diff --git a/include/costmap_2d/preferred_layer.h b/include/costmap_2d/preferred_layer.h
deleted file mode 100644
index ac192ea..0000000
--- a/include/costmap_2d/preferred_layer.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#ifndef COSTMAP_2D_PREFERRED_LAYER_H_
-#define COSTMAP_2D_PREFERRED_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class PreferredLayer : public StaticLayer
-{
-public:
-  PreferredLayer();
-  virtual ~PreferredLayer();
-private:
-  unsigned char interpretValue(unsigned char value);
-};
-}
-
-#endif // COSTMAP_2D_PREFERRED_LAYER_
\ No newline at end of file
diff --git a/include/costmap_2d/testing_helper.h b/include/costmap_2d/testing_helper.h
deleted file mode 100644
index c32446f..0000000
--- a/include/costmap_2d/testing_helper.h
+++ /dev/null
@@ -1,99 +0,0 @@
-#ifndef COSTMAP_2D_TESTING_HELPER_H
-#define COSTMAP_2D_TESTING_HELPER_H
-
-#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 <robot_sensor_msgs/point_cloud2_iterator.h>
-
-const double MAX_Z(1.0);
-
-char printableCost(unsigned char cost)
-{
-  switch (cost)
-  {
-  case costmap_2d::NO_INFORMATION: return '?';
-  case costmap_2d::LETHAL_OBSTACLE: return 'L';
-  case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
-  case costmap_2d::FREE_SPACE: return '.';
-  default: return '0' + (unsigned char) (10 * cost / 255);
-  }
-}
-
-void printMap(costmap_2d::Costmap2D& costmap)
-{
-  printf("map:\n");
-  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
-    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
-      printf("%4d", int(costmap.getCost(j, i)));
-    }
-    printf("\n\n");
-  }
-}
-
-unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
-{
-  unsigned int count = 0;
-  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
-    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
-      unsigned char c = costmap.getCost(j, i);
-      if ((equal && c == value) || (!equal && c != value))
-      {
-        count+=1;
-      }
-    }
-  }
-  return count;
-}
-
-void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
-{
-  costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
-  slayer->initialize(&layers, "static", &tf);
-}
-
-costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
-{
-  costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
-  olayer->initialize(&layers, "obstacles", &tf);
-  layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(olayer));
-  return olayer;
-}
-
-void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
-                    double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
-  robot_sensor_msgs::PointCloud2 cloud;
-  robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
-  modifier.setPointCloud2FieldsByString(1, "xyz");
-  modifier.resize(1);
-  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
-  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
-  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
-  *iter_x = x;
-  *iter_y = y;
-  *iter_z = z;
-
-  robot_geometry_msgs::Point p;
-  p.x = ox;
-  p.y = oy;
-  p.z = oz;
-
-  costmap_2d::Observation obs(p, cloud, 100.0, 100.0);  // obstacle range = raytrace range = 100.0
-  olayer->addStaticObservation(obs, true, true);
-}
-
-costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
-{
-  costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
-  ilayer->initialize(&layers, "inflation", &tf);
-  boost::shared_ptr<costmap_2d::Layer> ipointer(ilayer);
-  layers.addPlugin(ipointer);
-  return ilayer;
-}
-
-
-#endif  // COSTMAP_2D_TESTING_HELPER_H
diff --git a/include/costmap_2d/unpreferred_layer.h b/include/costmap_2d/unpreferred_layer.h
deleted file mode 100644
index 96e1c07..0000000
--- a/include/costmap_2d/unpreferred_layer.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef COSTMAP_2D_UNPREFERRED_LAYER_H_
-#define COSTMAP_2D_UNPREFERRED_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class UnPreferredLayer : public StaticLayer
-{
-public:
-  UnPreferredLayer();
-  virtual ~UnPreferredLayer();
-
-private:
-  unsigned char interpretValue(unsigned char value);
-
-};
-
-}
-
-#endif // COSTMAP_2D_UNPREFERRED_LAYER_
\ No newline at end of file
diff --git a/include/costmap_2d/array_parser.h b/include/robot_costmap_2d/array_parser.h
similarity index 91%
rename from include/costmap_2d/array_parser.h
rename to include/robot_costmap_2d/array_parser.h
index 0275bde..cbe32dc 100755
--- a/include/costmap_2d/array_parser.h
+++ b/include/robot_costmap_2d/array_parser.h
@@ -28,13 +28,13 @@
  *
  * author: Dave Hershberger
  */
-#ifndef COSTMAP_2D_ARRAY_PARSER_H
-#define COSTMAP_2D_ARRAY_PARSER_H
+#ifndef ROBOT_COSTMAP_2D_ARRAY_PARSER_H
+#define ROBOT_COSTMAP_2D_ARRAY_PARSER_H
 
 #include <vector>
 #include <string>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /** @brief Parse a vector of vectors of floats from a string.
@@ -46,6 +46,6 @@ namespace costmap_2d
  * anything, like part of a successful parse. */
 std::vector<std::vector<float> > parseVVF(const std::string& input, std::string& error_return);
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_ARRAY_PARSER_H
+#endif  // ROBOT_COSTMAP_2D_ARRAY_PARSER_H
diff --git a/include/costmap_2d/cost_values.h b/include/robot_costmap_2d/cost_values.h
similarity index 93%
rename from include/costmap_2d/cost_values.h
rename to include/robot_costmap_2d/cost_values.h
index fdd4d68..55a56de 100755
--- a/include/costmap_2d/cost_values.h
+++ b/include/robot_costmap_2d/cost_values.h
@@ -34,10 +34,10 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#ifndef COSTMAP_2D_COST_VALUES_H_
-#define COSTMAP_2D_COST_VALUES_H_
+#ifndef ROBOT_COSTMAP_2D_COST_VALUES_H_
+#define ROBOT_COSTMAP_2D_COST_VALUES_H_
 /** Provides a mapping for often used cost values */
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 static const unsigned char NO_INFORMATION = 255;
 static const unsigned char LETHAL_OBSTACLE = 254;
@@ -47,4 +47,4 @@ static const unsigned char FREE_SPACE = 60;
 static const unsigned char PREFERRED_SPACE = 20;
 static const unsigned char UNPREFERRED_SPACE = 100;
 }
-#endif  // COSTMAP_2D_COST_VALUES_H_
+#endif  // ROBOT_COSTMAP_2D_COST_VALUES_H_
diff --git a/include/costmap_2d/costmap_2d.h b/include/robot_costmap_2d/costmap_2d.h
similarity index 98%
rename from include/costmap_2d/costmap_2d.h
rename to include/robot_costmap_2d/costmap_2d.h
index b94cc92..faf741b 100755
--- a/include/costmap_2d/costmap_2d.h
+++ b/include/robot_costmap_2d/costmap_2d.h
@@ -35,15 +35,15 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_2D_H_
-#define COSTMAP_2D_COSTMAP_2D_H_
+#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
+#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H_
 
 #include <vector>
 #include <queue>
 #include <robot_geometry_msgs/Point.h>
 #include <boost/thread.hpp>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 // convenient for storing x/y point pairs
@@ -464,6 +464,6 @@ protected:
     std::vector<MapLocation>& cells_;
   };
 };
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_COSTMAP_2D_H
+#endif  // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_H
diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/robot_costmap_2d/costmap_2d_robot.h
similarity index 95%
rename from include/costmap_2d/costmap_2d_robot.h
rename to include/robot_costmap_2d/costmap_2d_robot.h
index f3b7066..de643b5 100755
--- a/include/costmap_2d/costmap_2d_robot.h
+++ b/include/robot_costmap_2d/costmap_2d_robot.h
@@ -35,12 +35,12 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_
-#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
+#ifndef ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
+#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
 
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/layer.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/footprint.h>
 
 #include <robot_geometry_msgs/Polygon.h>
 #include <robot_geometry_msgs/PolygonStamped.h>
@@ -49,7 +49,7 @@
 #include <tf3/LinearMath/Transform.h>
 #include <robot/rate.h>
 #include <data_convert/data_convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <robot_xmlrpcpp/XmlRpcValue.h>
 
@@ -70,7 +70,7 @@ public:
   }
 };
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
@@ -182,7 +182,7 @@ public:
   /** @brief Returns the current padded footprint as a robot_geometry_msgs::Polygon. */
   robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
   {
-    return costmap_2d::toPolygon(padded_footprint_);
+    return robot_costmap_2d::toPolygon(padded_footprint_);
   }
 
   /** @brief Return the current footprint of the robot as a vector of points.
@@ -275,6 +275,6 @@ private:
   void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
 };
 // class Costmap2DROBOT
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_COSTMAP_2D_ROBOT_H
+#endif  // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H
diff --git a/include/costmap_2d/costmap_layer.h b/include/robot_costmap_2d/costmap_layer.h
similarity index 88%
rename from include/costmap_2d/costmap_layer.h
rename to include/robot_costmap_2d/costmap_layer.h
index f3c19e2..89f3790 100755
--- a/include/costmap_2d/costmap_layer.h
+++ b/include/robot_costmap_2d/costmap_layer.h
@@ -35,13 +35,13 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
-#define COSTMAP_2D_COSTMAP_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
+#define ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
 
-#include <costmap_2d/layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 class CostmapLayer : public Layer, public Costmap2D
@@ -79,7 +79,7 @@ protected:
    * TrueOverwrite means every value from this layer
    * is written into the master grid.
    */
-  void updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /*
    * Updates the master_grid within the specified
@@ -88,7 +88,7 @@ protected:
    * Overwrite means every valid value from this layer
    * is written into the master grid (does not copy NO_INFORMATION)
    */
-  void updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /*
    * Updates the master_grid within the specified
@@ -99,7 +99,7 @@ protected:
    * it is overwritten. If the layer's value is NO_INFORMATION,
    * the master value does not change.
    */
-  void updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /*
    * Updates the master_grid within the specified
@@ -113,7 +113,7 @@ protected:
    * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
    * the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).
    */
-  void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  void updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   /**
    * Updates the bounding box specified in the parameters to include
@@ -147,5 +147,5 @@ private:
   double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
 };
 
-}  // namespace costmap_2d
-#endif  // COSTMAP_2D_COSTMAP_LAYER_H_
+}  // namespace robot_costmap_2d
+#endif  // ROBOT_COSTMAP_2D_COSTMAP_LAYER_H_
diff --git a/include/costmap_2d/costmap_math.h b/include/robot_costmap_2d/costmap_math.h
similarity index 95%
rename from include/costmap_2d/costmap_math.h
rename to include/robot_costmap_2d/costmap_math.h
index a3c1864..7b8c76d 100755
--- a/include/costmap_2d/costmap_math.h
+++ b/include/robot_costmap_2d/costmap_math.h
@@ -35,8 +35,8 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_COSTMAP_MATH_H_
-#define COSTMAP_2D_COSTMAP_MATH_H_
+#ifndef ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
+#define ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
 
 #include <math.h>
 #include <algorithm>
@@ -66,4 +66,4 @@ bool intersects(std::vector<robot_geometry_msgs::Point>& polygon, float testx, f
 
 bool intersects(std::vector<robot_geometry_msgs::Point>& polygon1, std::vector<robot_geometry_msgs::Point>& polygon2);
 
-#endif  // COSTMAP_2D_COSTMAP_MATH_H_
+#endif  // ROBOT_COSTMAP_2D_COSTMAP_MATH_H_
diff --git a/include/robot_costmap_2d/critical_layer.h b/include/robot_costmap_2d/critical_layer.h
new file mode 100644
index 0000000..c31bd62
--- /dev/null
+++ b/include/robot_costmap_2d/critical_layer.h
@@ -0,0 +1,19 @@
+#ifndef ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
+#define ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
+
+#include <robot_costmap_2d/static_layer.h>
+
+namespace robot_costmap_2d
+{
+class CriticalLayer : public StaticLayer
+{
+public:
+  CriticalLayer();
+  virtual ~CriticalLayer();
+private:
+  unsigned char interpretValue(unsigned char value) override;
+  void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;
+};
+}
+
+#endif // ROBOT_COSTMAP_2D_CRITICAL_LAYER_H_
\ No newline at end of file
diff --git a/include/costmap_2d/directional_layer.h b/include/robot_costmap_2d/directional_layer.h
similarity index 85%
rename from include/costmap_2d/directional_layer.h
rename to include/robot_costmap_2d/directional_layer.h
index 6338c61..86fac6c 100644
--- a/include/costmap_2d/directional_layer.h
+++ b/include/robot_costmap_2d/directional_layer.h
@@ -1,10 +1,10 @@
-#ifndef COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
-#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
+#ifndef ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
+#define ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
 
-#include <costmap_2d/static_layer.h>
+#include <robot_costmap_2d/static_layer.h>
 #include <robot_nav_msgs/Path.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
   class DirectionalLayer : public StaticLayer
   {
@@ -32,4 +32,4 @@ namespace costmap_2d
 	};
 }
 
-#endif // COSTMAP_2D_DIRECTIONAL_LAYER_H_
\ No newline at end of file
+#endif // ROBOT_COSTMAP_2D_DIRECTIONAL_LAYER_H_
\ No newline at end of file
diff --git a/include/costmap_2d/footprint.h b/include/robot_costmap_2d/footprint.h
similarity index 97%
rename from include/costmap_2d/footprint.h
rename to include/robot_costmap_2d/footprint.h
index 3bc0475..83e3553 100755
--- a/include/costmap_2d/footprint.h
+++ b/include/robot_costmap_2d/footprint.h
@@ -35,8 +35,8 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_FOOTPRINT_H
-#define COSTMAP_2D_FOOTPRINT_H
+#ifndef ROBOT_COSTMAP_2D_FOOTPRINT_H
+#define ROBOT_COSTMAP_2D_FOOTPRINT_H
 
 #include <robot_geometry_msgs/Polygon.h>
 #include <robot_geometry_msgs/PolygonStamped.h>
@@ -46,7 +46,7 @@
 #include <robot/node_handle.h>
 #include <robot_xmlrpcpp/XmlRpcValue.h> 
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /**
@@ -144,6 +144,6 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
 //  * will see the new value. */
 // void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<robot_geometry_msgs::Point>& footprint);
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_FOOTPRINT_H
+#endif  // ROBOT_COSTMAP_2D_FOOTPRINT_H
diff --git a/include/costmap_2d/inflation_layer.h b/include/robot_costmap_2d/inflation_layer.h
similarity index 94%
rename from include/costmap_2d/inflation_layer.h
rename to include/robot_costmap_2d/inflation_layer.h
index 7b1f798..50eed86 100755
--- a/include/costmap_2d/inflation_layer.h
+++ b/include/robot_costmap_2d/inflation_layer.h
@@ -35,14 +35,14 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_INFLATION_LAYER_H_
-#define COSTMAP_2D_INFLATION_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
+#define ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
 
-#include <costmap_2d/layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
 #include <boost/thread.hpp>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 /**
  * @class CellData
@@ -84,7 +84,7 @@ public:
   virtual void onInitialize();
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y);
-  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
   virtual bool isDiscretized()
   {
     return true;
@@ -192,6 +192,6 @@ private:
   bool need_reinflation_;  ///< Indicates that the entire costmap should be reinflated next time around.
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_INFLATION_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_INFLATION_LAYER_H_
diff --git a/include/costmap_2d/layer.h b/include/robot_costmap_2d/layer.h
similarity index 95%
rename from include/costmap_2d/layer.h
rename to include/robot_costmap_2d/layer.h
index 95e9cc5..bcff269 100755
--- a/include/costmap_2d/layer.h
+++ b/include/robot_costmap_2d/layer.h
@@ -34,16 +34,16 @@
  *
  * Author: David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_LAYER_H_
-#define COSTMAP_2D_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_LAYER_H_
+#define ROBOT_COSTMAP_2D_LAYER_H_
 
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/utils.h>
+#include <robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/utils.h>
 #include <string>
 #include <tf3/buffer_core.h>
 #include <robot/node_handle.h>
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 class LayeredCostmap;
 
@@ -164,6 +164,6 @@ private:
 
 using PluginLayerPtr = boost::shared_ptr<Layer>;
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_LAYER_H_
diff --git a/include/costmap_2d/layered_costmap.h b/include/robot_costmap_2d/layered_costmap.h
similarity index 92%
rename from include/costmap_2d/layered_costmap.h
rename to include/robot_costmap_2d/layered_costmap.h
index 8155a2a..f7438c8 100755
--- a/include/costmap_2d/layered_costmap.h
+++ b/include/robot_costmap_2d/layered_costmap.h
@@ -35,16 +35,16 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_
-#define COSTMAP_2D_LAYERED_COSTMAP_H_
+#ifndef ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
+#define ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
 
-#include <costmap_2d/cost_values.h>
-#include <costmap_2d/layer.h>
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/cost_values.h>
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/costmap_2d.h>
 #include <vector>
 #include <string>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 class Layer;
 
@@ -101,7 +101,7 @@ public:
 
   bool isTrackingUnknown()
   {
-    return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION;
+    return costmap_.getDefaultValue() == robot_costmap_2d::NO_INFORMATION;
   }
 
   std::vector<boost::shared_ptr<Layer> >* getPlugins()
@@ -172,6 +172,6 @@ private:
   std::vector<robot_geometry_msgs::Point> footprint_;
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_LAYERED_COSTMAP_H_
+#endif  // ROBOT_COSTMAP_2D_LAYERED_COSTMAP_H_
diff --git a/include/costmap_2d/observation.h b/include/robot_costmap_2d/observation.h
similarity index 95%
rename from include/costmap_2d/observation.h
rename to include/robot_costmap_2d/observation.h
index b70e6d4..49853e0 100755
--- a/include/costmap_2d/observation.h
+++ b/include/robot_costmap_2d/observation.h
@@ -29,13 +29,13 @@
  * Authors: Conor McGann
  */
 
-#ifndef COSTMAP_2D_OBSERVATION_H_
-#define COSTMAP_2D_OBSERVATION_H_
+#ifndef ROBOT_COSTMAP_2D_OBSERVATION_H_
+#define ROBOT_COSTMAP_2D_OBSERVATION_H_
 
 #include <robot_geometry_msgs/Point.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /**
@@ -98,5 +98,5 @@ public:
   double obstacle_range_, raytrace_range_;
 };
 
-}  // namespace costmap_2d
-#endif  // COSTMAP_2D_OBSERVATION_H_
+}  // namespace robot_costmap_2d
+#endif  // ROBOT_COSTMAP_2D_OBSERVATION_H_
diff --git a/include/costmap_2d/observation_buffer.h b/include/robot_costmap_2d/observation_buffer.h
similarity index 95%
rename from include/costmap_2d/observation_buffer.h
rename to include/robot_costmap_2d/observation_buffer.h
index 4cb9e51..95c13d2 100755
--- a/include/costmap_2d/observation_buffer.h
+++ b/include/robot_costmap_2d/observation_buffer.h
@@ -34,14 +34,14 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
-#define COSTMAP_2D_OBSERVATION_BUFFER_H_
+#ifndef ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
+#define ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
 
 #include <vector>
 #include <list>
 #include <string>
 #include <robot/time.h>
-#include <costmap_2d/observation.h>
+#include <robot_costmap_2d/observation.h>
 #include <tf3/buffer_core.h>
 
 #include <robot_sensor_msgs/PointCloud2.h>
@@ -49,7 +49,7 @@
 // Thread support
 #include <boost/thread.hpp>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 /**
  * @class ObservationBuffer
@@ -150,5 +150,5 @@ private:
   double obstacle_range_, raytrace_range_;
   double tf_tolerance_;
 };
-}  // namespace costmap_2d
-#endif  // COSTMAP_2D_OBSERVATION_BUFFER_H_
+}  // namespace robot_costmap_2d
+#endif  // ROBOT_COSTMAP_2D_OBSERVATION_BUFFER_H_
diff --git a/include/costmap_2d/obstacle_layer.h b/include/robot_costmap_2d/obstacle_layer.h
similarity index 77%
rename from include/costmap_2d/obstacle_layer.h
rename to include/robot_costmap_2d/obstacle_layer.h
index d2ed709..c323bf9 100755
--- a/include/costmap_2d/obstacle_layer.h
+++ b/include/robot_costmap_2d/obstacle_layer.h
@@ -35,13 +35,13 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_
-#define COSTMAP_2D_OBSTACLE_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
+#define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
 
-#include <costmap_2d/costmap_layer.h>
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/observation_buffer.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/costmap_layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/footprint.h>
 
 #include <robot_nav_msgs/OccupancyGrid.h>
 
@@ -51,7 +51,7 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 class ObstacleLayer : public CostmapLayer
@@ -66,14 +66,14 @@ public:
   virtual void onInitialize();
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y);
-  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void activate();
   virtual void deactivate();
   virtual void reset();
 
   // for testing purposes
-  void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
+  void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
 protected:
@@ -103,7 +103,7 @@ protected:
    * @param buffer A pointer to the observation buffer to update
    */
   void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
-                          const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
+                          const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
 
   /**
    * @brief  A callback to handle buffering PointCloud2 messages
@@ -111,21 +111,21 @@ protected:
    * @param buffer A pointer to the observation buffer to update
    */
   void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
-                           const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
+                           const boost::shared_ptr<robot_costmap_2d::ObservationBuffer>& buffer);
 
   /**
    * @brief  Get the observations used to mark space
    * @param marking_observations A reference to a vector that will be populated with the observations
    * @return True if all the observation buffers are current, false otherwise
    */
-  bool getMarkingObservations(std::vector<costmap_2d::Observation>& marking_observations) const;
+  bool getMarkingObservations(std::vector<robot_costmap_2d::Observation>& marking_observations) const;
 
   /**
    * @brief  Get the observations used to clear space
    * @param clearing_observations A reference to a vector that will be populated with the observations
    * @return True if all the observation buffers are current, false otherwise
    */
-  bool getClearingObservations(std::vector<costmap_2d::Observation>& clearing_observations) const;
+  bool getClearingObservations(std::vector<robot_costmap_2d::Observation>& clearing_observations) const;
 
   /**
    * @brief  Clear freespace based on one observation
@@ -135,7 +135,7 @@ protected:
    * @param max_x
    * @param max_y
    */
-  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
+  virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
 
   void updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y,
@@ -151,12 +151,12 @@ protected:
 
   laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
 
-  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<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > observation_buffers_;  ///< @brief Used to store observations from various sensors
+  std::vector<boost::shared_ptr<robot_costmap_2d::ObservationBuffer> > marking_buffers_;  ///< @brief Used to store observation buffers used for marking obstacles
+  std::vector<boost::shared_ptr<robot_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_;
+  std::vector<robot_costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
 
   bool rolling_window_;
   bool stop_receiving_data_ = false;
@@ -167,6 +167,6 @@ private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_OBSTACLE_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
diff --git a/include/robot_costmap_2d/preferred_layer.h b/include/robot_costmap_2d/preferred_layer.h
new file mode 100644
index 0000000..e1252af
--- /dev/null
+++ b/include/robot_costmap_2d/preferred_layer.h
@@ -0,0 +1,18 @@
+#ifndef ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
+#define ROBOT_COSTMAP_2D_PREFERRED_LAYER_H_
+
+#include <robot_costmap_2d/static_layer.h>
+
+namespace robot_costmap_2d
+{
+class PreferredLayer : public StaticLayer
+{
+public:
+  PreferredLayer();
+  virtual ~PreferredLayer();
+private:
+  unsigned char interpretValue(unsigned char value);
+};
+}
+
+#endif // ROBOT_COSTMAP_2D_PREFERRED_LAYER_
\ No newline at end of file
diff --git a/include/costmap_2d/static_layer.h b/include/robot_costmap_2d/static_layer.h
similarity index 90%
rename from include/costmap_2d/static_layer.h
rename to include/robot_costmap_2d/static_layer.h
index 80024f9..3df4a59 100755
--- a/include/costmap_2d/static_layer.h
+++ b/include/robot_costmap_2d/static_layer.h
@@ -35,17 +35,17 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_STATIC_LAYER_H_
-#define COSTMAP_2D_STATIC_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_STATIC_LAYER_H_
+#define ROBOT_COSTMAP_2D_STATIC_LAYER_H_
 
-#include <costmap_2d/costmap_layer.h>
-#include <costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/costmap_layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
 
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_map_msgs/OccupancyGridUpdate.h>
 #include <string>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 class StaticLayer : public CostmapLayer
@@ -60,7 +60,7 @@ public:
 
   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                             double* max_x, double* max_y);
-  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+  virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
 protected:
@@ -91,6 +91,6 @@ private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_STATIC_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_STATIC_LAYER_H_
diff --git a/include/robot_costmap_2d/testing_helper.h b/include/robot_costmap_2d/testing_helper.h
new file mode 100644
index 0000000..b864449
--- /dev/null
+++ b/include/robot_costmap_2d/testing_helper.h
@@ -0,0 +1,99 @@
+#ifndef ROBOT_COSTMAP_2D_TESTING_HELPER_H
+#define ROBOT_COSTMAP_2D_TESTING_HELPER_H
+
+#include<robot_costmap_2d/cost_values.h>
+#include<robot_costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/static_layer.h>
+#include <robot_costmap_2d/obstacle_layer.h>
+#include <robot_costmap_2d/inflation_layer.h>
+
+#include <robot_sensor_msgs/point_cloud2_iterator.h>
+
+const double MAX_Z(1.0);
+
+char printableCost(unsigned char cost)
+{
+  switch (cost)
+  {
+  case robot_costmap_2d::NO_INFORMATION: return '?';
+  case robot_costmap_2d::LETHAL_OBSTACLE: return 'L';
+  case robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
+  case robot_costmap_2d::FREE_SPACE: return '.';
+  default: return '0' + (unsigned char) (10 * cost / 255);
+  }
+}
+
+void printMap(robot_costmap_2d::Costmap2D& costmap)
+{
+  printf("map:\n");
+  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
+    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
+      printf("%4d", int(costmap.getCost(j, i)));
+    }
+    printf("\n\n");
+  }
+}
+
+unsigned int countValues(robot_costmap_2d::Costmap2D& costmap, unsigned char value, bool equal = true)
+{
+  unsigned int count = 0;
+  for (int i = 0; i < costmap.getSizeInCellsY(); i++){
+    for (int j = 0; j < costmap.getSizeInCellsX(); j++){
+      unsigned char c = costmap.getCost(j, i);
+      if ((equal && c == value) || (!equal && c != value))
+      {
+        count+=1;
+      }
+    }
+  }
+  return count;
+}
+
+void addStaticLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
+{
+  robot_costmap_2d::StaticLayer* slayer = new robot_costmap_2d::StaticLayer();
+  layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(slayer));
+  slayer->initialize(&layers, "static", &tf);
+}
+
+robot_costmap_2d::ObstacleLayer* addObstacleLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
+{
+  robot_costmap_2d::ObstacleLayer* olayer = new robot_costmap_2d::ObstacleLayer();
+  olayer->initialize(&layers, "obstacles", &tf);
+  layers.addPlugin(boost::shared_ptr<robot_costmap_2d::Layer>(olayer));
+  return olayer;
+}
+
+void addObservation(robot_costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
+                    double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
+  robot_sensor_msgs::PointCloud2 cloud;
+  robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
+  modifier.setPointCloud2FieldsByString(1, "xyz");
+  modifier.resize(1);
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
+  robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
+  *iter_x = x;
+  *iter_y = y;
+  *iter_z = z;
+
+  robot_geometry_msgs::Point p;
+  p.x = ox;
+  p.y = oy;
+  p.z = oz;
+
+  robot_costmap_2d::Observation obs(p, cloud, 100.0, 100.0);  // obstacle range = raytrace range = 100.0
+  olayer->addStaticObservation(obs, true, true);
+}
+
+robot_costmap_2d::InflationLayer* addInflationLayer(robot_costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
+{
+  robot_costmap_2d::InflationLayer* ilayer = new robot_costmap_2d::InflationLayer();
+  ilayer->initialize(&layers, "inflation", &tf);
+  boost::shared_ptr<robot_costmap_2d::Layer> ipointer(ilayer);
+  layers.addPlugin(ipointer);
+  return ilayer;
+}
+
+
+#endif  // ROBOT_COSTMAP_2D_TESTING_HELPER_H
diff --git a/include/robot_costmap_2d/unpreferred_layer.h b/include/robot_costmap_2d/unpreferred_layer.h
new file mode 100644
index 0000000..1da5a86
--- /dev/null
+++ b/include/robot_costmap_2d/unpreferred_layer.h
@@ -0,0 +1,21 @@
+#ifndef ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
+#define ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_H_
+
+#include <robot_costmap_2d/static_layer.h>
+
+namespace robot_costmap_2d
+{
+class UnPreferredLayer : public StaticLayer
+{
+public:
+  UnPreferredLayer();
+  virtual ~UnPreferredLayer();
+
+private:
+  unsigned char interpretValue(unsigned char value);
+
+};
+
+}
+
+#endif // ROBOT_COSTMAP_2D_UNPREFERRED_LAYER_
\ No newline at end of file
diff --git a/include/costmap_2d/utils.h b/include/robot_costmap_2d/utils.h
similarity index 94%
rename from include/costmap_2d/utils.h
rename to include/robot_costmap_2d/utils.h
index cb098b8..899bdd7 100644
--- a/include/costmap_2d/utils.h
+++ b/include/robot_costmap_2d/utils.h
@@ -1,11 +1,11 @@
-#ifndef COSTMAP_2D_UTILS_H
-#define COSTMAP_2D_UTILS_H
+#ifndef ROBOT_COSTMAP_2D_UTILS_H
+#define ROBOT_COSTMAP_2D_UTILS_H
 #include <yaml-cpp/yaml.h>
 #include <filesystem>
 #include <string>
 #include <iostream>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
     template<typename T>
     T loadParam(const YAML::Node& node, const std::string& key, const T& default_value)
@@ -64,4 +64,4 @@ namespace costmap_2d
 
 }
 
-#endif  // COSTMAP_2D_UTILS_H
\ No newline at end of file
+#endif  // ROBOT_COSTMAP_2D_UTILS_H
\ No newline at end of file
diff --git a/include/costmap_2d/voxel_grid.h b/include/robot_costmap_2d/voxel_grid.h
similarity index 80%
rename from include/costmap_2d/voxel_grid.h
rename to include/robot_costmap_2d/voxel_grid.h
index 4e516cb..9bf5b93 100644
--- a/include/costmap_2d/voxel_grid.h
+++ b/include/robot_costmap_2d/voxel_grid.h
@@ -6,15 +6,15 @@
 // uint32 size_y
 // uint32 size_z
 
-#ifndef VOXEL_GRID_COSTMAP_2D_H
-#define VOXEL_GRID_COSTMAP_2D_H
+#ifndef VOXEL_GRID_ROBOT_COSTMAP_2D_H
+#define VOXEL_GRID_ROBOT_COSTMAP_2D_H
 
 #include <vector>
 #include <robot_std_msgs/Header.h>
 #include <robot_geometry_msgs/Point32.h>
 #include <robot_geometry_msgs/Vector3.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
     struct VoxelGrid
     {
@@ -29,4 +29,4 @@ namespace costmap_2d
     
 }
 
-#endif // VOXEL_GRID_COSTMAP_2D_H
+#endif // VOXEL_GRID_ROBOT_COSTMAP_2D_H
diff --git a/include/costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h
similarity index 89%
rename from include/costmap_2d/voxel_layer.h
rename to include/robot_costmap_2d/voxel_layer.h
index 960780d..a38dff7 100755
--- a/include/costmap_2d/voxel_layer.h
+++ b/include/robot_costmap_2d/voxel_layer.h
@@ -35,23 +35,23 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#ifndef COSTMAP_2D_VOXEL_LAYER_H_
-#define COSTMAP_2D_VOXEL_LAYER_H_
+#ifndef ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
+#define ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
 
-#include <costmap_2d/layer.h>
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/observation_buffer.h>
-#include <costmap_2d/voxel_grid.h>
+#include <robot_costmap_2d/layer.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/voxel_grid.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_sensor_msgs/LaserScan.h>
 #include <laser_geometry/laser_geometry.hpp>
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>
-#include <costmap_2d/obstacle_layer.h>
+#include <robot_costmap_2d/obstacle_layer.h>
 #include <voxel_grid/voxel_grid.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 class VoxelLayer : public ObstacleLayer
@@ -85,7 +85,7 @@ protected:
 private:
   bool getParams(const std::string& config_file_name, robot::NodeHandle &nh);
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
-  virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
+  virtual void raytraceFreespace(const robot_costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);
 
 
@@ -137,6 +137,6 @@ private:
   }
 };
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
-#endif  // COSTMAP_2D_VOXEL_LAYER_H_
+#endif  // ROBOT_COSTMAP_2D_VOXEL_LAYER_H_
diff --git a/package.xml b/package.xml
index 963c6aa..be108a7 100644
--- a/package.xml
+++ b/package.xml
@@ -1,9 +1,9 @@
 <package>
-  <name>costmap_2d</name>
+  <name>robot_costmap_2d</name>
   <version>0.7.10</version>
   <description>
-    costmap_2d is the second generation of the transform library, which lets
-    the user keep track of multiple coordinate frames over time. costmap_2d
+    robot_costmap_2d is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. robot_costmap_2d
     maintains the relationship between coordinate frames in a tree
     structure buffered in time, and lets the user transform points,
     vectors, etc between any two coordinate frames at any desired
@@ -15,7 +15,7 @@
   <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
   <license>BSD</license>
 
-  <url type="website">http://www.ros.org/wiki/costmap_2d</url>
+  <url type="website">http://www.ros.org/wiki/robot_costmap_2d</url>
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
diff --git a/plugins/critical_layer.cpp b/plugins/critical_layer.cpp
index 887f05f..8bf4806 100644
--- a/plugins/critical_layer.cpp
+++ b/plugins/critical_layer.cpp
@@ -1,11 +1,11 @@
-#include <costmap_2d/critical_layer.h>
+#include <robot_costmap_2d/critical_layer.h>
 #include <boost/dll/alias.hpp>
 
-using costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::CRITICAL_SPACE;
+using robot_costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::CRITICAL_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 CriticalLayer::CriticalLayer(){}
@@ -21,7 +21,7 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
     return NO_INFORMATION;
 }
 
-void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   // printf("TEST PLUGIN CRITICAL\n");
   if (!map_received_)
diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp
index 856ee67..5a29690 100644
--- a/plugins/directional_layer.cpp
+++ b/plugins/directional_layer.cpp
@@ -1,18 +1,18 @@
-#include <costmap_2d/directional_layer.h>
+#include <robot_costmap_2d/directional_layer.h>
 #include <data_convert/data_convert.h>
 #include <tf3/convert.h>
 #include <tf3/utils.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <boost/dll/alias.hpp>
 
 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
 
-using costmap_2d::CRITICAL_SPACE;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::CRITICAL_SPACE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::NO_INFORMATION;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
   DirectionalLayer::DirectionalLayer()
   {}
@@ -79,7 +79,7 @@ namespace costmap_2d
           int index = getIndex(ix / 2, iy / 2);
           directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
           directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
-          costmap_[index] = costmap_2d::NO_INFORMATION;
+          costmap_[index] = robot_costmap_2d::NO_INFORMATION;
         }
       }
       map_frame_ = new_map.header.frame_id;
@@ -144,7 +144,7 @@ namespace costmap_2d
             || p == path.poses.back()
            )
         {
-          if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
+          if (value == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == robot_costmap_2d::LETHAL_OBSTACLE)
           {
             std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
             std::pair<unsigned int, double> y_val(my, p.pose.position.y);
@@ -187,7 +187,7 @@ namespace costmap_2d
     for (int i = 0; i < size_costmap; i++)
     {
       int8_t value;
-      if (costmap[i] == costmap_2d::NO_INFORMATION)
+      if (costmap[i] == robot_costmap_2d::NO_INFORMATION)
       {
         value = -1;
       }
@@ -211,24 +211,24 @@ namespace costmap_2d
   unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
   {
     double yaw_lane;
-    unsigned char cost = costmap_2d::NO_INFORMATION;
+    unsigned char cost = robot_costmap_2d::NO_INFORMATION;
 
     int index = getIndex(x, y);
     for (auto &lane : directional_areas_[index])
     {
       if (lane > 359)
       {
-        cost = std::min(cost, costmap_2d::NO_INFORMATION);
+        cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
       }
       else
       {
         double yaw_lane = (double)lane / 180 * M_PI;
         if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
-          cost = std::min(cost, costmap_2d::FREE_SPACE);
+          cost = std::min(cost, robot_costmap_2d::FREE_SPACE);
         else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
-          cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
+          cost = std::min(cost, robot_costmap_2d::LETHAL_OBSTACLE);
         else
-          cost = std::min(cost, costmap_2d::NO_INFORMATION);
+          cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
       }
     }
     return cost;
@@ -278,10 +278,10 @@ namespace costmap_2d
         for (int j = y[i].first; j <= y_max; j++)
         {
           int y_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -290,10 +290,10 @@ namespace costmap_2d
         for (int k = y[i].first; k >= y_min; k--)
         {
           int y_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -304,10 +304,10 @@ namespace costmap_2d
         for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
         {
           int x_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -316,10 +316,10 @@ namespace costmap_2d
         for (int k = x[i].first; k >= 0; k--)
         {
           int x_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -332,10 +332,10 @@ namespace costmap_2d
         for (int j = x[i].first; j <= x_max; j++)
         {
           int x_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -344,10 +344,10 @@ namespace costmap_2d
         for (int k = x[i].first; k >= x_min; k--)
         {
           int x_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -358,10 +358,10 @@ namespace costmap_2d
         for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
         {
           int y_ = j;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -370,10 +370,10 @@ namespace costmap_2d
         for (int k = y[i].first; k >= 0; k--)
         {
           int y_ = k;
-          if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
+          if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
             continue;
           unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
-          if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
+          if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
             costmap_[getIndex(x_, y_)] = value;
           else
             break;
@@ -391,7 +391,7 @@ namespace costmap_2d
       for (unsigned int ix = 0; ix < size_x; ix++)
       {
         int index = getIndex(ix, iy);
-        costmap_[index] = costmap_2d::NO_INFORMATION;
+        costmap_[index] = robot_costmap_2d::NO_INFORMATION;
       }
     }
   }
diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp
index 44a636b..dc2f58d 100755
--- a/plugins/inflation_layer.cpp
+++ b/plugins/inflation_layer.cpp
@@ -36,18 +36,18 @@
  *         David V. Lu!!
  *********************************************************************/
 #include <algorithm>
-#include <costmap_2d/inflation_layer.h>
-#include <costmap_2d/costmap_math.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/footprint.h>
 #include <boost/thread.hpp>
 #include <boost/dll/alias.hpp>
 
 
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
-using costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+using robot_costmap_2d::NO_INFORMATION;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 InflationLayer::InflationLayer()
@@ -91,7 +91,7 @@ void InflationLayer::onInitialize()
 bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
     try {
-        std::string folder = COSTMAP_2D_DIR;
+        std::string folder = ROBOT_COSTMAP_2D_DIR;
         std::string path_source = getSourceFile(folder,config_file_name);
 
         YAML::Node config = YAML::LoadFile(path_source);
@@ -132,7 +132,7 @@ bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeH
 void InflationLayer::matchSize()
 {
   boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
-  costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
+  robot_costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
   resolution_ = costmap->getResolution();
   cell_inflation_radius_ = cellDistance(inflation_radius_);
   computeCaches();
@@ -191,7 +191,7 @@ void InflationLayer::onFootprintChanged()
             layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
 }
 
-void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
   if (cell_inflation_radius_ == 0)
@@ -416,4 +416,4 @@ static boost::shared_ptr<Layer> create_inflation_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_inflation_plugin, InflationLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp
index f085a7b..1a394b9 100755
--- a/plugins/obstacle_layer.cpp
+++ b/plugins/obstacle_layer.cpp
@@ -35,8 +35,8 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/obstacle_layer.h>
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/obstacle_layer.h>
+#include <robot_costmap_2d/costmap_math.h>
 
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
 
@@ -44,14 +44,14 @@
 #include <boost/dll/alias.hpp>
 
 
-using costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::FREE_SPACE;
 
-using costmap_2d::ObservationBuffer;
-using costmap_2d::Observation;
+using robot_costmap_2d::ObservationBuffer;
+using robot_costmap_2d::Observation;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void ObstacleLayer::onInitialize()
@@ -75,7 +75,7 @@ ObstacleLayer::~ObstacleLayer()
 bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
   try {
-      std::string folder = COSTMAP_2D_DIR;
+      std::string folder = ROBOT_COSTMAP_2D_DIR;
       std::string path_source = getSourceFile(folder,config_file_name);
 
       
@@ -405,11 +405,11 @@ void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot
     }
 }
 
-void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void ObstacleLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (footprint_clearing_enabled_)
   {
-    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
+    setConvexPolygonCost(transformed_footprint_, robot_costmap_2d::FREE_SPACE);
   }
   
   switch (combination_method_)
@@ -426,7 +426,7 @@ void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
 
 }
 
-void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
+void ObstacleLayer::addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing)
 {
   if (marking)
     static_marking_observations_.push_back(obs);
@@ -597,4 +597,4 @@ static boost::shared_ptr<Layer> create_obstacle_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_obstacle_plugin, ObstacleLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/plugins/preferred_layer.cpp b/plugins/preferred_layer.cpp
index 0b55945..094f83f 100644
--- a/plugins/preferred_layer.cpp
+++ b/plugins/preferred_layer.cpp
@@ -1,12 +1,12 @@
-#include <costmap_2d/preferred_layer.h>
+#include <robot_costmap_2d/preferred_layer.h>
 #include <boost/dll/alias.hpp>
 
-using costmap_2d::NO_INFORMATION;
-using costmap_2d::FREE_SPACE;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::PREFERRED_SPACE;
+using robot_costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::FREE_SPACE;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::PREFERRED_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 PreferredLayer::PreferredLayer(){}
diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp
index e52f79f..b522936 100755
--- a/plugins/static_layer.cpp
+++ b/plugins/static_layer.cpp
@@ -36,20 +36,20 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/static_layer.h>
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/static_layer.h>
+#include <robot_costmap_2d/costmap_math.h>
 
 #include <tf3/convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <boost/dll/alias.hpp>
 
 
-using costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::FREE_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 StaticLayer::StaticLayer()
@@ -75,7 +75,7 @@ void StaticLayer::onInitialize()
 bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
     try {
-        std::string folder = COSTMAP_2D_DIR;
+        std::string folder = ROBOT_COSTMAP_2D_DIR;
         std::string path_source = getSourceFile(folder,config_file_name);
         
         YAML::Node config = YAML::LoadFile(path_source);
@@ -324,7 +324,7 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
   has_updated_data_ = false;
 }
 
-void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!map_received_)
     return;
@@ -389,4 +389,4 @@ static boost::shared_ptr<Layer> create_static_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_static_plugin, StaticLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/plugins/unpreferred_layer.cpp b/plugins/unpreferred_layer.cpp
index fa4457f..f6307d7 100644
--- a/plugins/unpreferred_layer.cpp
+++ b/plugins/unpreferred_layer.cpp
@@ -1,11 +1,11 @@
-#include <costmap_2d/unpreferred_layer.h>
+#include <robot_costmap_2d/unpreferred_layer.h>
 #include <boost/dll/alias.hpp>
 
-using costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::PREFERRED_SPACE;
+using robot_costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::PREFERRED_SPACE;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 UnPreferredLayer::UnPreferredLayer(){}
diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp
index 38e6fae..e8a746d 100755
--- a/plugins/voxel_layer.cpp
+++ b/plugins/voxel_layer.cpp
@@ -35,20 +35,20 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/voxel_layer.h>
+#include <robot_costmap_2d/voxel_layer.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
 #include <boost/dll/alias.hpp>
 
 #define VOXEL_BITS 16
 
-using costmap_2d::NO_INFORMATION;
-using costmap_2d::LETHAL_OBSTACLE;
-using costmap_2d::FREE_SPACE;
+using robot_costmap_2d::NO_INFORMATION;
+using robot_costmap_2d::LETHAL_OBSTACLE;
+using robot_costmap_2d::FREE_SPACE;
 
-using costmap_2d::ObservationBuffer;
-using costmap_2d::Observation;
+using robot_costmap_2d::ObservationBuffer;
+using robot_costmap_2d::Observation;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void VoxelLayer::onInitialize()
@@ -68,7 +68,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
 {
   try 
   {
-    std::string folder = COSTMAP_2D_DIR;
+    std::string folder = ROBOT_COSTMAP_2D_DIR;
     std::string path_source = getSourceFile(folder,config_file_name);
     
     YAML::Node config = YAML::LoadFile(path_source);
@@ -228,7 +228,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
 
   // if (publish_voxel_)
   // {
-  //   costmap_2d::VoxelGrid grid_msg;
+  //   robot_costmap_2d::VoxelGrid grid_msg;
   //   unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
   //   grid_msg.size_x = voxel_grid_.sizeX();
   //   grid_msg.size_y = voxel_grid_.sizeY();
@@ -490,4 +490,4 @@ static boost::shared_ptr<Layer> create_voxel_plugin() {
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
 BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/src/array_parser.cpp b/src/array_parser.cpp
index 144a860..810e4e4 100755
--- a/src/array_parser.cpp
+++ b/src/array_parser.cpp
@@ -34,7 +34,7 @@
 #include <sstream>
 #include <vector>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 /** @brief Parse a vector of vector of floats from a string.
@@ -112,4 +112,4 @@ std::vector<std::vector<float> > parseVVF(const std::string& input, std::string&
   return result;
 }
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp
index 34b0ebb..26599ef 100755
--- a/src/costmap_2d.cpp
+++ b/src/costmap_2d.cpp
@@ -35,12 +35,12 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
 #include <cstdio>
 
 using namespace std;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
                      double origin_x, double origin_y, unsigned char default_value) :
@@ -485,4 +485,4 @@ bool Costmap2D::saveMap(std::string file_name)
   return true;
 }
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp
index dbf13ff..308132a 100644
--- a/src/costmap_2d_robot.cpp
+++ b/src/costmap_2d_robot.cpp
@@ -41,9 +41,9 @@
 #include <algorithm>
 #include <vector>
 
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/utils.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/utils.h>
 
 #include <tf3/convert.h>
 #include <tf3/utils.h>
@@ -53,7 +53,7 @@
 
 using namespace std;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
     layered_costmap_(NULL),
@@ -83,11 +83,11 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
 {
   try 
   {
-    std::string folder = COSTMAP_2D_DIR;
+    std::string folder = ROBOT_COSTMAP_2D_DIR;
     std::string path_source = getSourceFile(folder,config_file_name);
 
     YAML::Node config = YAML::LoadFile(path_source);
-    YAML::Node layer = config["costmap_2d"];
+    YAML::Node layer = config["robot_costmap_2d"];
 
     std::string global_frame =
     loadParam(layer, "global_frame", std::string("map"));
@@ -567,4 +567,4 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector<robot_geometry_msgs::Point
                      padded_footprint_, oriented_footprint);
 }
 
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/src/costmap_layer.cpp b/src/costmap_layer.cpp
index f2e4669..1b389e1 100755
--- a/src/costmap_layer.cpp
+++ b/src/costmap_layer.cpp
@@ -1,6 +1,6 @@
-#include<costmap_2d/costmap_layer.h>
+#include<robot_costmap_2d/costmap_layer.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
@@ -60,7 +60,7 @@ void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, d
     has_extra_bounds_ = false;
 }
 
-void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CostmapLayer::updateWithMax(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!enabled_)
     return;
@@ -100,7 +100,7 @@ void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i,
   }
 }
 
-void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
+void CostmapLayer::updateWithTrueOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
                                            int max_i, int max_j)
 {
   if (!enabled_)
@@ -119,7 +119,7 @@ void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, i
   }
 }
 
-void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CostmapLayer::updateWithOverwrite(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!enabled_)
     return;
@@ -138,7 +138,7 @@ void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int m
   }
 }
 
-void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+void CostmapLayer::updateWithAddition(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
   if (!enabled_)
     return;
@@ -161,8 +161,8 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
       else
       {
         int sum = old_cost + costmap_[it];
-        if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-            master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
+        if (sum >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+            master_array[it] = robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
         else
             master_array[it] = sum;
       }
@@ -170,4 +170,4 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
     }
   }
 }
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
diff --git a/src/costmap_math.cpp b/src/costmap_math.cpp
index 2bfe1b4..b66deb8 100755
--- a/src/costmap_math.cpp
+++ b/src/costmap_math.cpp
@@ -27,7 +27,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <costmap_2d/costmap_math.h>
+#include <robot_costmap_2d/costmap_math.h>
 
 double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
 {
diff --git a/src/footprint.cpp b/src/footprint.cpp
index 47dc31f..bbc2983 100755
--- a/src/footprint.cpp
+++ b/src/footprint.cpp
@@ -27,15 +27,15 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <costmap_2d/costmap_math.h>
+#include <robot_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 <robot_costmap_2d/footprint.h>
+#include <robot_costmap_2d/array_parser.h>
 #include <robot_geometry_msgs/Point32.h>
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 void calculateMinAndMaxDistances(const std::vector<robot_geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
@@ -301,4 +301,4 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
   return footprint;
 }
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
diff --git a/src/layer.cpp b/src/layer.cpp
index 9267f84..79ec417 100755
--- a/src/layer.cpp
+++ b/src/layer.cpp
@@ -27,9 +27,9 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "costmap_2d/layer.h"
+#include "robot_costmap_2d/layer.h"
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
 Layer::Layer()
@@ -53,4 +53,4 @@ const std::vector<robot_geometry_msgs::Point>& Layer::getFootprint() const
   return layered_costmap_->getFootprint();
 }
 
-}  // end namespace costmap_2d
+}  // end namespace robot_costmap_2d
diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp
index 7bef187..a66dbaa 100755
--- a/src/layered_costmap.cpp
+++ b/src/layered_costmap.cpp
@@ -35,8 +35,8 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/footprint.h>
+#include <robot_costmap_2d/layered_costmap.h>
+#include <robot_costmap_2d/footprint.h>
 #include <cstdio>
 #include <string>
 #include <algorithm>
@@ -44,7 +44,7 @@
 
 using std::vector;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 
   LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
@@ -178,7 +178,7 @@ namespace costmap_2d
   void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
   {
     footprint_ = footprint_spec;
-    costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
+    robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
 
     for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
@@ -187,4 +187,4 @@ namespace costmap_2d
     }
   }
 
-} // namespace costmap_2d
+} // namespace robot_costmap_2d
diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp
index 02c4191..a6fbcad 100755
--- a/src/observation_buffer.cpp
+++ b/src/observation_buffer.cpp
@@ -34,16 +34,16 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#include <costmap_2d/observation_buffer.h>
+#include <robot_costmap_2d/observation_buffer.h>
 
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
-#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_sensor_msgs/tf3_sensor_msgs.h>
 #include <robot_sensor_msgs/point_cloud2_iterator.h>
 
 using namespace std;
 using namespace tf3;
 
-namespace costmap_2d
+namespace robot_costmap_2d
 {
 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                      double min_obstacle_height, double max_obstacle_height, double obstacle_range,
@@ -276,5 +276,5 @@ void ObservationBuffer::resetLastUpdated()
 {
   last_updated_ = robot::Time::now();
 }
-}  // namespace costmap_2d
+}  // namespace robot_costmap_2d
 
diff --git a/test/array_parser_test.cpp b/test/array_parser_test.cpp
index 7ac2a39..0abd8ce 100644
--- a/test/array_parser_test.cpp
+++ b/test/array_parser_test.cpp
@@ -29,9 +29,9 @@
 
 #include <gtest/gtest.h>
 
-#include "costmap_2d/array_parser.h"
+#include "robot_costmap_2d/array_parser.h"
 
-using namespace costmap_2d;
+using namespace robot_costmap_2d;
 
 TEST(array_parser, basic_operation)
 {
diff --git a/test/coordinates_test.cpp b/test/coordinates_test.cpp
index 065ef2e..c1e2cc9 100644
--- a/test/coordinates_test.cpp
+++ b/test/coordinates_test.cpp
@@ -35,9 +35,9 @@
 // Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp
 
 #include <gtest/gtest.h>
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d.h>
 
-using namespace costmap_2d;
+using namespace robot_costmap_2d;
 
 TEST(CostmapCoordinates, easy_coordinates_test)
 {
diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp
index 6ed8283..20d91a2 100644
--- a/test/static_layer_test.cpp
+++ b/test/static_layer_test.cpp
@@ -1,28 +1,28 @@
-#include <costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/cost_values.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/cost_values.h>
 #include <gtest/gtest.h>
 #include <tf3/buffer_core.h>
 #include <robot_geometry_msgs/PoseWithCovariance.h>
 
-namespace costmap_2d {
+namespace robot_costmap_2d {
 
 class CostmapTester : public testing::Test {
   public:
     CostmapTester(tf3::BufferCore& tf);
     void checkConsistentCosts();
-    void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
-    void compareCells(costmap_2d::Costmap2D& costmap, 
+    void compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
+    void compareCells(robot_costmap_2d::Costmap2D& costmap, 
         unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
     virtual void TestBody(){}
 
   private:
-    costmap_2d::Costmap2DROBOT costmap_ros_;
+    robot_costmap_2d::Costmap2DROBOT costmap_ros_;
 };
 
 CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){}
 
 void CostmapTester::checkConsistentCosts(){
-  costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
+  robot_costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
 
   //get a copy of the costmap contained by our ros wrapper
   costmap->saveMap("costmap_test.pgm");
@@ -35,7 +35,7 @@ void CostmapTester::checkConsistentCosts(){
   }
 }
 
-void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
+void CostmapTester::compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
   //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
   for(int offset_x = -1; offset_x <= 1; ++offset_x){
     for(int offset_y = -1; offset_y <= 1; ++offset_y){
@@ -51,18 +51,18 @@ void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsig
 }
 
 //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
-void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
+void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
   double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));
 
   unsigned char cell_cost = costmap.getCost(x, y);
   unsigned char neighbor_cost = costmap.getCost(nx, ny);
 
-  if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
+  if(cell_cost == robot_costmap_2d::LETHAL_OBSTACLE){
     //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
-    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
+    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == robot_costmap_2d::FREE_SPACE));
   }
-  else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
+  else if(cell_cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
     //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
     double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
@@ -72,19 +72,19 @@ void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x,
       printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
       costmap.saveMap("failing_costmap.pgm");
     }
-    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE));
+    EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));
   }
 }
 };
 
-costmap_2d::CostmapTester* map_tester = NULL;
+robot_costmap_2d::CostmapTester* map_tester = NULL;
 
 
 int main(int argc, char** argv){
 
   tf3::BufferCore tf(tf3::Duration(10));
 
-  map_tester = new costmap_2d::CostmapTester(tf);
+  map_tester = new robot_costmap_2d::CostmapTester(tf);
 
 
   return(0);