diff --git a/.gitmodules b/.gitmodules
index a81ef79..a452f91 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,9 +1,9 @@
 [submodule "src/Libraries/common_msgs"]
 	path = src/Libraries/common_msgs
 	url = https://git.pnkr.asia/DuongTD/common_msgs.git
-[submodule "src/Libraries/costmap_2d"]
-	path = src/Libraries/costmap_2d
-	url = https://git.pnkr.asia/DuongTD/costmap_2d.git
+[submodule "src/Libraries/robot_costmap_2d"]
+	path = src/Libraries/robot_costmap_2d
+	url = https://git.pnkr.asia/DuongTD/robot_costmap_2d.git
 [submodule "src/Libraries/geometry2"]
 	path = src/Libraries/geometry2
 	url = https://git.pnkr.asia/DuongTD/geometry2.git
diff --git a/doc/architecture_discussion.md b/doc/architecture_discussion.md
index ffe3067..377fd8b 100644
--- a/doc/architecture_discussion.md
+++ b/doc/architecture_discussion.md
@@ -67,8 +67,8 @@ flowchart TB
   %% ========== COSTMAP LAYER ==========
   subgraph Costmap["🗺️ Costmap Layer"]
     direction LR
-    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
-    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
+    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
+    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
     style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
     style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
     style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
@@ -211,7 +211,7 @@ Cần làm rõ:
    - Plugin system sử dụng `boost::dll` để dynamic loading
 
 5. **Costmap Layer** ✅
-   - `costmap_2d::Costmap2DROBOT`: Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap
    - Costmap layers: static map, obstacles, inflation
    - Frame management: map (global), odom (local)
 
@@ -235,7 +235,7 @@ Cần làm rõ:
 - `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
 - `robot_geometry_msgs::Twist` (vận tốc linear/angular)
 - `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
-- `costmap_2d::Costmap2D` (bản đồ chi phí)
+- `robot_costmap_2d::Costmap2D` (bản đồ chi phí)
 
 ### 3. Thiết kế từng module (interface level)
 
@@ -264,7 +264,7 @@ Cần làm rõ:
 - `nav_core::RecoveryBehavior` ✅
   - `runBehavior()` - Thực thi recovery behavior
 
-- `costmap_2d::Costmap2DROBOT` ✅
+- `robot_costmap_2d::Costmap2DROBOT` ✅
   - Wrapper cho costmap với robot footprint
   - Thread-safe access với mutex
 
@@ -411,7 +411,7 @@ pnkx_nav_core/
 │   │   │   ├── nav_core_adapter/    # Adapter utilities
 │   │   │   └── nav_core2/           # Additional nav utilities
 │   │   ├── Libraries/
-│   │   │   ├── costmap_2d/          # Costmap system
+│   │   │   ├── robot_costmap_2d/          # Costmap system
 │   │   │   ├── tf3/                 # Transform system
 │   │   │   ├── robot_time/          # Time management
 │   │   │   ├── geometry2/           # Geometry utilities
diff --git a/doc/implementation_plan.md b/doc/implementation_plan.md
index 4fa5fd0..846e79e 100644
--- a/doc/implementation_plan.md
+++ b/doc/implementation_plan.md
@@ -15,7 +15,7 @@
    - Plugin system với `boost::dll` để load dynamic plugins
 
 3. **Costmap System:**
-   - `costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
    - Costmap layers (static, obstacle, inflation, etc.)
 
 4. **Supporting Libraries:**
diff --git a/examples/QUICK_START.md b/examples/QUICK_START.md
index b7f6eba..5e8b6cb 100644
--- a/examples/QUICK_START.md
+++ b/examples/QUICK_START.md
@@ -85,9 +85,9 @@ cp ../../build/src/APIs/c_api/libnav_c_api.so .
 
 # 5. Set library path (quan trọng!)
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
@@ -118,9 +118,9 @@ ls -la ../../build/src/APIs/c_api/libnav_c_api.so
 
 # Set LD_LIBRARY_PATH (bao gồm tất cả dependencies)
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
@@ -143,6 +143,6 @@ ldd libnav_c_api.so
 → Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`.
    Nếu vẫn gặp lỗi, kiểm tra:
    - Library đã được build đầy đủ: `make nav_c_api` trong build directory
-   - Dependencies đã được build: `make move_base costmap_2d`
+   - Dependencies đã được build: `make move_base robot_costmap_2d`
    - LD_LIBRARY_PATH đã được set đúng
 
diff --git a/examples/README.md b/examples/README.md
index d30fdf7..9335296 100644
--- a/examples/README.md
+++ b/examples/README.md
@@ -78,9 +78,9 @@ Tạo file `NavigationExample.csproj`:
 ```bash
 # Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
diff --git a/examples/run_example.sh b/examples/run_example.sh
index 705171a..b651fab 100755
--- a/examples/run_example.sh
+++ b/examples/run_example.sh
@@ -95,9 +95,9 @@ export LD_LIBRARY_PATH="$BUILD_DIR:$LD_LIBRARY_PATH"
 # Library directories for dependencies
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_time:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/xmlrpcpp:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/costmap_2d:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"
diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
index 3bcd08c..2837857 100755
--- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
+++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
@@ -11,7 +11,7 @@
 #include <nav_core2/exceptions.h>
 #include <nav_core2/costmap.h>
 
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot_geometry_msgs/PoseArray.h>
 #include <mkt_msgs/Trajectory2D.h>
 #include <score_algorithm/trajectory_generator.h>
@@ -38,7 +38,7 @@ namespace score_algorithm
      * @param tf TFListener pointer
      * @param costmap_robot Costmap pointer
      */
-    virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+    virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                             const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
   
     /**
@@ -144,7 +144,7 @@ namespace score_algorithm
 
     std::string name_;
     TFListenerPtr tf_;
-    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
     TrajectoryGenerator::Ptr traj_;
 
     int index_samples_;
diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
index 04d7b04..bdb358f 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
+++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
@@ -29,7 +29,7 @@ set(PACKAGES_DIR
     kalman
     angles
     nav_grid
-    costmap_2d
+    robot_costmap_2d
     robot_sensor_msgs
     robot_std_msgs
 )
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h
index 1e43cee..423a082 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h
@@ -24,7 +24,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Calculating algorithm
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
index cd916b6..2a05e3e 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
@@ -44,7 +44,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -157,7 +157,7 @@ namespace mkt_algorithm
        */
       bool transformGlobalPlan(
           TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-          const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
+          const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
           robot_nav_2d_msgs::Path2D &transformed_plan);
 
       /**
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h
index 6a97482..06ab8b0 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h
@@ -23,7 +23,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
index 7bb8c8a..d5388be 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
@@ -3,7 +3,7 @@
 #include <robot/console.h>
 
 void mkt_algorithm::diff::GoStraight::initialize(
-    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
index e08673c..273e5ec 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
@@ -5,7 +5,7 @@
 #define LIMIT_VEL_THETA 0.33
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
-    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
@@ -544,10 +544,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     kf_->update(y, dt, A);
 
     // Check if Kalman filter's estimated velocity exceeds v_max
-    if (avoid_obstacles_ && (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE ||
-                             cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE))
+    if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
+                             cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
+                             cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
+                             cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
     {
       drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
     }
@@ -826,7 +826,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
 
 bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-    const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
+    const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
     robot_nav_2d_msgs::Path2D &transformed_plan)
 {
   // this method is a slightly modified version of base_local_planner/goal_functions.h
@@ -1029,12 +1029,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
   // ss << linear_vel << " ";
   // limit the linear velocity by proximity to obstacles
   if (use_cost_regulated_linear_velocity_scaling_ &&
-      pose_cost != static_cast<double>(costmap_2d::NO_INFORMATION) &&
-      pose_cost != static_cast<double>(costmap_2d::FREE_SPACE))
+      pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
+      pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
   {
     const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
     const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
-                                                std::log(pose_cost / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
+                                                std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
                                             inscribed_radius;
 
     if (min_distance_to_obstacle < cost_scaling_dist_)
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp
index c42683f..61eeb69 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp
@@ -5,7 +5,7 @@
 #include <angles/angles.h>
 
 void mkt_algorithm::diff::RotateToGoal::initialize(
-    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner
index d651201..60e9c56 160000
--- a/src/Algorithms/Packages/global_planners/custom_planner
+++ b/src/Algorithms/Packages/global_planners/custom_planner
@@ -1 +1 @@
-Subproject commit d6512018efc5ef63d64a6aeb97ecaf89d83cbd80
+Subproject commit 60e9c5673f8fd646d628f843ef73e71d1d9b2a17
diff --git a/src/Algorithms/Packages/global_planners/dock_planner b/src/Algorithms/Packages/global_planners/dock_planner
index 9c84e64..b5a1b7b 160000
--- a/src/Algorithms/Packages/global_planners/dock_planner
+++ b/src/Algorithms/Packages/global_planners/dock_planner
@@ -1 +1 @@
-Subproject commit 9c84e64253fe959931cd456cf2eb164af9ee1c92
+Subproject commit b5a1b7b6d8367b5812b32f2ca51bbed6e2a4c99a
diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt
index 6aa8282..81d0db2 100755
--- a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt
+++ b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt
@@ -27,13 +27,13 @@ file(GLOB HEADERS "include/two_points_planner/*.h")
 
 # Dependencies packages (internal libraries)
 set(PACKAGES_DIR
-    costmap_2d
+    robot_costmap_2d
     nav_core
     geometry_msgs
     robot_nav_msgs
     robot_std_msgs
     tf3
-    tf3_geometry_msgs
+    robot_tf3_geometry_msgs
 )
 
 # Tạo thư viện shared (.so)
diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h b/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h
index 2498c0e..da06364 100755
--- a/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h
+++ b/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h
@@ -6,7 +6,7 @@
 #include <vector>
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <nav_core/base_global_planner.h>
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot/node_handle.h>
 
 namespace two_points_planner
@@ -31,7 +31,7 @@ public:
    * @param  name The name of this planner
    * @param  costmap_robot A pointer to the ROS wrapper of the costmap to use
    */
-  TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
+  TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
 
   /**
    * @brief  Initialization function for the TwoPointsPlanner object
@@ -39,7 +39,7 @@ public:
    * @param  costmap_robot A pointer to the ROS wrapper of the costmap to use
    */
   virtual bool initialize(std::string name, 
-                          costmap_2d::Costmap2DROBOT* costmap_robot);
+                          robot_costmap_2d::Costmap2DROBOT* costmap_robot);
   
   /**
    * @brief Given a goal pose in the world, compute a plan
@@ -71,7 +71,7 @@ protected:
   bool allow_unknown_;
 
   std::string name_;
-  costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
+  robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
   std::vector<robot_geometry_msgs::Point> footprint_;
   unsigned int current_env_width_;
   unsigned int current_env_height_; 
diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
index 38bcd94..22069eb 100755
--- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
+++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
@@ -1,11 +1,11 @@
 #include <robot/console.h>
 #include <two_points_planner/two_points_planner.h>
-#include <costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/inflation_layer.h>
 #include <boost/dll/alias.hpp>
 #include <robot_nav_msgs/Path.h>
 #include <tf3/LinearMath/Quaternion.h>
 #include <tf3/utils.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <robot/time.h>
 #include <robot/console.h>
 #include <cmath>
@@ -15,13 +15,13 @@ namespace two_points_planner
 {
   TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}
 
-  TwoPointsPlanner::TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
+  TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
     : initialized_(false), costmap_robot_(NULL)
   {
     initialize(name, costmap_robot);
   }
 
-  bool TwoPointsPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
+  bool TwoPointsPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
   {
     if (!initialized_)
     {
@@ -66,11 +66,11 @@ namespace two_points_planner
 
     // Bug 
     // // check if the costmap has an inflation layer
-    // for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
+    // for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
     //   layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
     //   ++layer)
     // {
-    //   boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
+    //   boost::shared_ptr<robot_costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<robot_costmap_2d::InflationLayer>(*layer);
     //   if (!inflation_layer)
     //     continue;
 
@@ -83,14 +83,14 @@ namespace two_points_planner
   // This rescales the costmap according to a rosparam which sets the obstacle cost
   unsigned char TwoPointsPlanner::costMapCostToSBPLCost(unsigned char newcost)
   {
-    if (newcost == costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == costmap_2d::NO_INFORMATION))
+    if (newcost == robot_costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == robot_costmap_2d::NO_INFORMATION))
       return lethal_obstacle_;
-    else if (newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    else if (newcost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
       return inscribed_inflated_obstacle_;
     else if (newcost == 0)
       return 0;
-    else if (newcost == costmap_2d::NO_INFORMATION)
-      return  costmap_2d::FREE_SPACE;
+    else if (newcost == robot_costmap_2d::NO_INFORMATION)
+      return  robot_costmap_2d::FREE_SPACE;
     else
     {
       unsigned char cost = newcost;
@@ -151,8 +151,8 @@ namespace two_points_planner
     }
     unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
-    if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-      || end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
+    if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+      || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
     {
       std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
       return false;
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt
index 7f8d0f5..dfc3424 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt
@@ -37,10 +37,10 @@ set(PACKAGES_DIR
     nav_core2
     mkt_msgs
     score_algorithm
-    costmap_2d
+    robot_costmap_2d
     tf3
-    tf3_geometry_msgs
-    tf3_sensor_msgs
+    robot_tf3_geometry_msgs
+    robot_tf3_sensor_msgs
     robot_cpp
     angles
 )
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
index 036650c..6a9ed18 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
@@ -28,7 +28,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
@@ -110,7 +110,7 @@ namespace pnkx_local_planner
        * @param tf TFListener pointer
        * @param costmap_robot Costmap pointer
        */
-      void initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+      void initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                       score_algorithm::TrajectoryGenerator::Ptr traj_generator);
 
       bool getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal);
@@ -156,7 +156,7 @@ namespace pnkx_local_planner
 
       robot::NodeHandle nh_, nh_priv_;
       TFListenerPtr tf_;
-      costmap_2d::Costmap2DROBOT *costmap_robot_;
+      robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
       std::function<nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h
index 4188c1b..72953a4 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h
@@ -26,7 +26,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
index 290c278..aef29e6 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
@@ -33,7 +33,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 setGoalPose - Sets the global goal pose
@@ -183,8 +183,8 @@ namespace pnkx_local_planner
     robot_nav_2d_msgs::Path2D transformed_plan_;
     robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
 
-    costmap_2d::Costmap2D *costmap_;
-    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    robot_costmap_2d::Costmap2D *costmap_;
+    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
     nav_grid::NavGridInfo info_;
     boost::recursive_mutex configuration_mutex_;
     bool update_costmap_before_planning_;
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h
index b15388d..1601e0c 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h
@@ -26,7 +26,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h
index 9d9fbc3..13e6407 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h
@@ -11,9 +11,9 @@
 #include <tf3/buffer_core.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 <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 
 namespace pnkx_local_planner
 {
@@ -42,7 +42,7 @@ namespace pnkx_local_planner
    */
   bool transformGlobalPlan(
       TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-      const costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
+      const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
       robot_nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false);
 
 }
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
index 8fc3d63..49e0fea 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
@@ -34,7 +34,7 @@ pnkx_local_planner::PNKXDockingLocalPlanner::~PNKXDockingLocalPlanner()
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
-                                                      TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+                                                      TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
 {
   if (!initialized_)
   {
@@ -572,7 +572,7 @@ pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
     docking_nav_.reset();
 }
 
-void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                                                                              score_algorithm::TrajectoryGenerator::Ptr traj_generator)
 {
   nh_ = nh;
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
index 835bd83..bfb57bb 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
@@ -30,7 +30,7 @@ pnkx_local_planner::PNKXGoStraightLocalPlanner::~PNKXGoStraightLocalPlanner()
     goal_checker_.reset();
 }
 
-void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
+void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
 {
   if (!initialized_)
   {
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
index 41a5bf5..52d0e91 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
@@ -9,7 +9,7 @@
 
 #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 <mkt_msgs/Trajectory2D.h>
 #include <boost/dll/import.hpp>
@@ -37,7 +37,7 @@ pnkx_local_planner::PNKXLocalPlanner::~PNKXLocalPlanner()
 }
 
 void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
-                                                      TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+                                                      TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
 {
   if (!initialized_)
   {
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
index cd4e86f..ddd8803 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
@@ -29,7 +29,7 @@ pnkx_local_planner::PNKXRotateLocalPlanner::~PNKXRotateLocalPlanner()
     goal_checker_.reset();
 }
 
-void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
+void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
 {
   if (!initialized_)
   {
diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp
index 8feae1e..3ff743f 100644
--- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp
+++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp
@@ -3,7 +3,7 @@
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot/console.h>
 #include <data_convert/data_convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, robot_nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance)
 {
@@ -89,7 +89,7 @@ double pnkx_local_planner::getSquareDistance(const robot_geometry_msgs::Pose2D&
 
 bool pnkx_local_planner::transformGlobalPlan(
   TFListenerPtr tf, const robot_nav_2d_msgs::Path2D& global_plan, const robot_nav_2d_msgs::Pose2DStamped& pose,
-  const costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
+  const robot_costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
   robot_nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame)
 {
   if (global_plan.poses.size() == 0)
diff --git a/src/Libraries/common_msgs b/src/Libraries/common_msgs
index 41d47c9..0e48660 160000
--- a/src/Libraries/common_msgs
+++ b/src/Libraries/common_msgs
@@ -1 +1 @@
-Subproject commit 41d47c9c9e7116d837c16930151eb58165039a5b
+Subproject commit 0e486607b7868c37d3c40fed0207435a2e185171
diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d
index 4246453..72b2f3c 160000
--- a/src/Libraries/costmap_2d
+++ b/src/Libraries/costmap_2d
@@ -1 +1 @@
-Subproject commit 4246453ae6598b75be664acef3e9fc276129a131
+Subproject commit 72b2f3c6393d68ea9d3905fb569986520fa4538d
diff --git a/src/Libraries/data_convert b/src/Libraries/data_convert
index b1aaa1a..594f0fe 160000
--- a/src/Libraries/data_convert
+++ b/src/Libraries/data_convert
@@ -1 +1 @@
-Subproject commit b1aaa1a9463853ec16574b27182c5753c630a407
+Subproject commit 594f0fe49e40273a7e2e74e44f0d63fa4fe2cfea
diff --git a/src/Libraries/geometry2 b/src/Libraries/geometry2
index e4db1da..5558086 160000
--- a/src/Libraries/geometry2
+++ b/src/Libraries/geometry2
@@ -1 +1 @@
-Subproject commit e4db1da907b8d77ac8b838d4a03e4e57a8c0eb6f
+Subproject commit 5558086d10e402e7afc605c6c68074ab4f530b9f
diff --git a/src/Libraries/robot_cpp b/src/Libraries/robot_cpp
index cb9dfe5..fc6eb0c 160000
--- a/src/Libraries/robot_cpp
+++ b/src/Libraries/robot_cpp
@@ -1 +1 @@
-Subproject commit cb9dfe5c9be8d99a8b14dd0718a2da986aca5ef4
+Subproject commit fc6eb0c43fb2da4007529187e7328ed93f1fce66
diff --git a/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md b/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md
index 5ecd66b..e44a8e4 100755
--- a/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md
+++ b/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md
@@ -1,5 +1,5 @@
 # robot_nav_2d_utils Polygons and Footprints
-This library represents a replacement for [costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
+This library represents a replacement for [robot_costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_costmap_2d/include/robot_costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
 
 ## Polygons and the Parameter Server
 There have historically been three primary ways to specify a polygon/footprint on the parameter server. The first is to simply specify a radius which is converted to a hexadecagon, i.e. polygon with sixteen sides. This can be read with
@@ -21,7 +21,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlr
 
 If the `XmlRpcValue` is a string, it will call the `polygonFromString` method.
 
-The above are the traditional methods that were supported by the original `costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
+The above are the traditional methods that were supported by the original `robot_costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
 
 ```
 robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h
index 3949bcb..cbed06c 100755
--- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h
+++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h
@@ -44,7 +44,7 @@ namespace robot_nav_2d_utils
   /**
    * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius
    *
-   * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
+   * Analagous to robot_costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
    * is present.
    */
   robot_nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle &nh, bool write = true);
diff --git a/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
index b86170b..136601a 100755
--- a/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
+++ b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
@@ -36,7 +36,7 @@
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_utils/conversions.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <string>
 
 namespace robot_nav_2d_utils
diff --git a/src/Navigations/Cores/nav_core/CMakeLists.txt b/src/Navigations/Cores/nav_core/CMakeLists.txt
index 84208c3..89baf5f 100644
--- a/src/Navigations/Cores/nav_core/CMakeLists.txt
+++ b/src/Navigations/Cores/nav_core/CMakeLists.txt
@@ -19,7 +19,7 @@ include_directories(
 
 # Tạo INTERFACE library (header-only)
 add_library(nav_core INTERFACE)
-target_link_libraries(nav_core INTERFACE costmap_2d tf3 robot_protocol_msgs)
+target_link_libraries(nav_core INTERFACE robot_costmap_2d tf3 robot_protocol_msgs)
 
 # Set include directories
 target_include_directories(nav_core
diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h b/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h
index 0bdfe84..536f2bd 100755
--- a/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h
+++ b/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h
@@ -38,7 +38,7 @@
 #define NAV_CORE_BASE_GLOBAL_PLANNER_H
 
 #include <robot_geometry_msgs/PoseStamped.h>
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot_protocol_msgs/Order.h>
 #include <memory>
 
@@ -97,7 +97,7 @@ namespace nav_core {
        * @param  name The name of this planner
        * @param  costmap_robot A pointer to the wrapper of the costmap to use for planning
        */
-      virtual bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
+      virtual bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
 
       /**
        * @brief  Virtual destructor for the interface
diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h b/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h
index de73009..8753212 100755
--- a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h
+++ b/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h
@@ -39,7 +39,7 @@
 
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_geometry_msgs/Twist.h>
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
 #include <memory>
 
@@ -138,7 +138,7 @@ namespace nav_core
      * @param tf A pointer to a transform listener
      * @param costmap_robot The cost map to use for assigning costs to local plans
      */
-    virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
+    virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
 
     /**
      * @brief  Virtual destructor for the interface
diff --git a/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h b/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h
index c23f111..2616cd6 100755
--- a/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h
+++ b/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h
@@ -37,7 +37,7 @@
 #ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
 #define NAV_CORE_RECOVERY_BEHAVIOR_H
 
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
 #include <memory>
 
@@ -57,7 +57,7 @@ namespace nav_core {
        * @param global_costmap A pointer to the global_costmap used by the navigation stack 
        * @param local_costmap A pointer to the local_costmap used by the navigation stack 
        */
-      virtual void initialize(std::string name, tf3::BufferCore* tf, costmap_2d::Costmap2DROBOT* global_costmap, costmap_2d::Costmap2DROBOT* local_costmap) = 0;
+      virtual void initialize(std::string name, tf3::BufferCore* tf, robot_costmap_2d::Costmap2DROBOT* global_costmap, robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0;
 
       /**
        * @brief   Runs the RecoveryBehavior
diff --git a/src/Navigations/Cores/nav_core2/CMakeLists.txt b/src/Navigations/Cores/nav_core2/CMakeLists.txt
index 2d055de..b0d75f4 100755
--- a/src/Navigations/Cores/nav_core2/CMakeLists.txt
+++ b/src/Navigations/Cores/nav_core2/CMakeLists.txt
@@ -24,7 +24,7 @@ file(GLOB HEADERS "include/nav_core2/*.h")
 add_library(nav_core2 INTERFACE)
 
 target_link_libraries(nav_core2 INTERFACE 
-        costmap_2d 
+        robot_costmap_2d 
         tf3 
         nav_grid 
         robot_nav_2d_msgs
diff --git a/src/Navigations/Cores/nav_core2/README.md b/src/Navigations/Cores/nav_core2/README.md
index eada1a6..e45e938 100755
--- a/src/Navigations/Cores/nav_core2/README.md
+++ b/src/Navigations/Cores/nav_core2/README.md
@@ -3,12 +3,12 @@ A replacement interface for [nav_core](https://github.com/ros-planning/navigatio
 
 There were a few key reasons for creating new interfaces rather than extending the old ones.
  * Use `robot_nav_2d_msgs` to eliminate unused data fields
- * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
+ * Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `robot_costmap_2d::Costmap2DROBOT`.
  * Provide more data in the interfaces for easier testing.
  * Use Exceptions rather than booleans to provide more information about the types of errors encountered.
 
 ## `Costmap`
-`costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
+`robot_costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
  * Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROBOT`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
  * Initialization also started an update thread, which is also not always needed in testing.
  * Using `Costmap2DROBOT` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
@@ -29,7 +29,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
 
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
-|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
+|`void initialize(std::string, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
 |`bool makePlan(const robot_geometry_msgs::PoseStamped&, const robot_geometry_msgs::PoseStamped&, std::vector<robot_geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
 
 ## Local Planner
@@ -37,7 +37,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 
 | `nav_core` | `nav_core2` | comments |
 |---|--|---|
-|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
+|`void initialize(std::string, tf::TransformListener*, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
 |(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
 |`bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
 |`bool computeVelocityCommands(robot_geometry_msgs::Twist&)`|`robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
index 910af1a..5a80370 100755
--- a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
+++ b/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
@@ -42,7 +42,7 @@
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <robot_geometry_msgs/Twist.h>
 #include <string>
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <memory>
 
 namespace nav_core2
@@ -77,7 +77,7 @@ namespace nav_core2
     //                         TFListenerPtr tf, Costmap::Ptr costmap) = 0;
 
     virtual void initialize(robot::NodeHandle &parent, const std::string &name,
-                            TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
+                            TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap) = 0;
 
     /**
      * @brief Sets the global goal for this local planner.
diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
index 1328529..04f9b4a 100755
--- a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
+++ b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
@@ -59,7 +59,7 @@ target_include_directories(global_planner_adapter PRIVATE
 # which has a different ABI (Costmap2DROBOT) and causes missing symbols.
 set(_NAV_CORE_ADAPTER_RPATH
     "${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
-    "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
+    "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
     "${CMAKE_BINARY_DIR}/src/Libraries/tf3"
     "${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
     "${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
diff --git a/src/Navigations/Cores/nav_core_adapter/README.md b/src/Navigations/Cores/nav_core_adapter/README.md
index a0f90dd..069d753 100755
--- a/src/Navigations/Cores/nav_core_adapter/README.md
+++ b/src/Navigations/Cores/nav_core_adapter/README.md
@@ -3,7 +3,7 @@ This package contains adapters for using `nav_core` plugins as `nav_core2` plugi
  * Converting between 2d and 3d datatypes.
  * Converting between returning false and throwing exceptions on failure.
 
-We also provide an adapter for using a `costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
+We also provide an adapter for using a `robot_costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
 
 ## Adapter Classes
  * Global Planner Adapters
diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
index 29fdd44..993f80d 100755
--- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
+++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
@@ -37,12 +37,12 @@
 
 #include <nav_core2/common.h>
 #include <nav_core2/costmap.h>
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 #include <string>
 
 namespace nav_core_adapter
 {
-nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot);
+nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot);
 
 class CostmapAdapter : public nav_core2::Costmap
 {
@@ -57,7 +57,7 @@ public:
    * @param costmap_robot A Costmap2DROBOT object
    * @param needs_destruction Whether to free the costmap_robot object when this class is destroyed
    */
-  void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
+  void initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
 
   // Standard Costmap Interface
   void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
@@ -72,11 +72,11 @@ public:
   void updateInfo(const nav_grid::NavGridInfo& new_info) override;
 
   // Get Costmap Pointer for Backwards Compatibility
-  costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
+  robot_costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
 
 protected:
-  costmap_2d::Costmap2DROBOT* costmap_robot_;
-  costmap_2d::Costmap2D* costmap_;
+  robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
+  robot_costmap_2d::Costmap2D* costmap_;
   bool needs_destruction_;
 };
 
diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
index 4ab4d97..0670d69 100755
--- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
+++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
@@ -67,7 +67,7 @@ protected:
   boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
   nav_core::BaseGlobalPlanner::Ptr planner_;
 
-  costmap_2d::Costmap2DROBOT* costmap_robot_;
+  robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
   nav_core2::Costmap::Ptr costmap_;
 };
 
diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
index 9fdabf4..93168c8 100755
--- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
+++ b/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
@@ -77,7 +77,7 @@ namespace nav_core_adapter
     virtual ~LocalPlannerAdapter();
 
     // Standard ROS Local Planner Interface
-    void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+    void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
@@ -193,7 +193,7 @@ namespace nav_core_adapter
     TFListenerPtr tf_;
 
     std::shared_ptr<CostmapAdapter> costmap_adapter_;
-    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
 
     boost::recursive_mutex configuration_mutex_;
 
diff --git a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp
index be10d5f..338e21f 100755
--- a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp
+++ b/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp
@@ -43,10 +43,10 @@
 namespace nav_core_adapter
 {
 
-nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot)
+nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot)
 {
   nav_grid::NavGridInfo info;
-  costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
+  robot_costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
   info.width = costmap->getSizeInCellsX();
   info.height = costmap->getSizeInCellsY();
   info.resolution = costmap->getResolution();
@@ -64,7 +64,7 @@ CostmapAdapter::~CostmapAdapter()
   }
 }
 
-void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
+void CostmapAdapter::initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
 {
   costmap_robot_ = costmap_robot;
   needs_destruction_ = needs_destruction;
diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
index e177764..fdef733 100755
--- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
+++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
@@ -73,7 +73,7 @@ namespace nav_core_adapter
   /**
    * @brief Load the nav_core2 local planner and initialize it
    */
-  void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+  void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
   {
     tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
     costmap_robot_ = costmap_robot;
diff --git a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
index 6c2ced4..e63972e 100755
--- a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
+++ b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
@@ -51,7 +51,7 @@ TEST(LocalPlannerAdapter, unload_local_planner)
 
   nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter();
 
-  costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
+  robot_costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
   lpa->initialize("lpa", &tf, &costmap_robot);
 
   delete lpa;
diff --git a/src/Navigations/Libraries/nav_grid/README.md b/src/Navigations/Libraries/nav_grid/README.md
index 26967ba..74c8054 100755
--- a/src/Navigations/Libraries/nav_grid/README.md
+++ b/src/Navigations/Libraries/nav_grid/README.md
@@ -17,7 +17,7 @@ Together, these components make a [`nav_grid::NavGridInfo`](include/nav_grid/nav
 The default values are `width=0, height=0, resolution=1.0, frame_id="map", origin_x=0.0, origin_y=0.0`.
 
 ## Coordinate Conversion
-One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
+One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`robot_costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_costmap_2d/include/robot_costmap_2d/robot_costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
  * `gridToWorld` is the same as `mapToWorld`, as both return the world coordinates of the center of the specified cell.
  * `worldToGrid` works like `worldToMapNoBounds`, but it results in either `int` or `double` coordinates depending on the output parameter types. As the result are not bounded by the grid, the results are signed.
  * `worldToGridBounded` is a combination of `worldToMap` and `worldToMapEnforceBounds`. It returns a bool for whether the input coordinates are within the grid AND the output coordinates are forced to be within the grid. The output coordinates are therefore `unsigned int`.
diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h
index 4eb06fe..8757ba7 100755
--- a/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h
+++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h
@@ -91,7 +91,7 @@ inline void worldToGrid(const NavGridInfo& info, double wx, double wy, int& mx,
 /**
  * @brief  Convert from world coordinates to grid coordinates
  *
- * Combined functionality from costmap_2d::worldToMap and costmap_2d::worldToMapEnforceBounds.
+ * Combined functionality from robot_costmap_2d::worldToMap and robot_costmap_2d::worldToMapEnforceBounds.
  * The output parameters are set to grid indexes within the grid, even if the function returns false,
  * meaning the coordinates are outside the grid.
  *
diff --git a/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h b/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h
index 7dfe7f8..768a422 100755
--- a/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h
+++ b/src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h
@@ -43,7 +43,7 @@ namespace nav_grid
 {
 /**
  * @class NavGrid
- * This class is a spiritual successor to the costmap_2d::Costmap2D class, with the key differences being that
+ * This class is a spiritual successor to the robot_costmap_2d::Costmap2D class, with the key differences being that
  * the datatype and data storage methods are not specified, and the frame_id is specified.
  *
  * The templatized nature of the class allows you to store whatever you like at each grid location, including
diff --git a/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md b/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md
index 0242e8c..5bd1167 100644
--- a/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md
+++ b/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md
@@ -86,11 +86,11 @@ Ngược lại, sẽ build ở **Standalone CMake mode**.
 ### Internal Packages (cần build trước):
 - move_base_core
 - nav_core
-- costmap_2d
+- robot_costmap_2d
 - xmlrpcpp
 - node_handle
-- tf3_sensor_msgs
-- tf3_geometry_msgs
+- robot_tf3_sensor_msgs
+- robot_tf3_geometry_msgs
 
 ### System Libraries:
 - Boost
diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt
index 4793bfc..169fca1 100644
--- a/src/Navigations/Packages/move_base/CMakeLists.txt
+++ b/src/Navigations/Packages/move_base/CMakeLists.txt
@@ -49,8 +49,8 @@ include_directories(
 # ========================================================
 set(CMAKE_SKIP_BUILD_RPATH FALSE)
 set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
+set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
+set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # ========================================================
@@ -68,11 +68,11 @@ set(PACKAGES_DIR
   robot_std_msgs 
   move_base_core 
   nav_core 
-  costmap_2d 
+  robot_costmap_2d 
   plugins  # Link với plugins library để có StaticLayer typeinfo
   yaml-cpp
-  tf3_sensor_msgs
-  tf3_geometry_msgs
+  robot_tf3_sensor_msgs
+  robot_tf3_geometry_msgs
   data_convert
   dl
   pthread
diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h
index e42ed6f..8dc31d2 100644
--- a/src/Navigations/Packages/move_base/include/move_base/move_base.h
+++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h
@@ -24,8 +24,8 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/thread/recursive_mutex.hpp>
 
-#include <costmap_2d/costmap_2d_robot.h>
-#include <costmap_2d/costmap_2d.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d.h>
 #include <robot_nav_msgs/GetPlan.h>
 #include <robot_protocol_msgs/Order.h>
 
@@ -293,7 +293,7 @@ namespace move_base
 
     bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
 
-    bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
+    bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);
 
     double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
 
@@ -317,8 +317,8 @@ namespace move_base
     nav_core::BaseGlobalPlanner::Ptr planner_;
     std::vector<nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
     
-    costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
-    costmap_2d::Costmap2DROBOT *planner_costmap_robot_;    
+    robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
+    robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;    
     std::string robot_base_frame_, global_frame_;
     std::vector<std::string> recovery_behavior_names_;
     unsigned int recovery_index_;
diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index 1ff3863..6ea2044 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -9,15 +9,15 @@
 #include <stdexcept>
 #include <cstdlib>
 #include <data_convert/data_convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <boost/algorithm/string.hpp>
 #include <boost/thread.hpp>
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
 #include <robot/plugin_loader_helper.h>
 
-#include <costmap_2d/static_layer.h>
-#include <costmap_2d/voxel_layer.h>
+#include <robot_costmap_2d/static_layer.h>
+#include <robot_costmap_2d/voxel_layer.h>
 #include <robot_nav_msgs/OccupancyGrid.h>
 #include <robot_sensor_msgs/LaserScan.h>
 
@@ -177,14 +177,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
     try
     {
-      planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
+      planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
       planner_costmap_robot_->pause();
-      costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
-      for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
+      robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
+      for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
           layer != layered_costmap_->getPlugins()->end();
           ++layer)
       {
-        boost::shared_ptr<costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<costmap_2d::StaticLayer>(*layer);
+        boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
         if (!static_layer)
           continue;
         robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
@@ -203,7 +203,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
         occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
         for (int i = 0; i < occupancy_grid->data.size(); i++)
         {
-          occupancy_grid->data[i] = costmap_2d::NO_INFORMATION;
+          occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
         }
         if (occupancy_grid)
           static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
@@ -241,14 +241,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
 
     try
     {
-      controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
+      controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
       controller_costmap_robot_->pause();
-      costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
-      for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
+      robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
+      for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
           layer != layered_costmap_->getPlugins()->end();
           ++layer)
       {
-        boost::shared_ptr<costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<costmap_2d::VoxelLayer>(*layer);
+        boost::shared_ptr<robot_costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<robot_costmap_2d::VoxelLayer>(*layer);
         if (!obstacle_layer)
           continue;
         robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan();
@@ -401,7 +401,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
   {
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
 
   try
   {
@@ -496,7 +496,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
   {
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   try
   {
     tc_->swapPlanner(docking_planner_name_);
@@ -559,7 +559,7 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
 
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   tc_->swapPlanner(go_straight_planner_name_);
 
   if (nav_feedback_)
@@ -608,7 +608,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
   {
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   tc_->swapPlanner(rotate_planner_name_);
 
   if (nav_feedback_)
@@ -640,7 +640,7 @@ void move_base::MoveBase::pause()
   {
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   if (!tc_)
   {
     throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -668,7 +668,7 @@ void move_base::MoveBase::resume()
   {
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   if (!tc_)
   {
     throw std::runtime_error("Null 'tc_' pointer encountered");
@@ -689,7 +689,7 @@ void move_base::MoveBase::cancel()
   {
     throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
   }
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
   cancel_ctr_ = true;
 }
 
@@ -811,7 +811,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
     return false;
   }
 
-  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
+  boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
 
   // get the starting pose of the robot
   robot_geometry_msgs::PoseStamped global_pose;
@@ -1028,7 +1028,7 @@ void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
   pt.y = y + size_y / 2;
   clear_poly.push_back(pt);
 
-  planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
+  planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
 
   // clear the controller's costmap
   getRobotPose(global_pose, controller_costmap_robot_);
@@ -1053,7 +1053,7 @@ void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
   pt.y = y + size_y / 2;
   clear_poly.push_back(pt);
 
-  controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
+  controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
 }
 
 void move_base::MoveBase::publishZeroVelocity()
@@ -1219,7 +1219,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
   return true;
 }
 
-bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
+bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap)
 {
   
   tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);