diff --git a/CMakeLists.txt b/CMakeLists.txt
index 78ecc11..4515bfa 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -38,8 +38,8 @@ if (NOT TARGET robot_time)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time)
 endif()
 
-if (NOT TARGET robot_xmlrpcpp)
-   add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_xmlrpcpp) 
+if (NOT TARGET xmlrpcpp)
+   add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/xmlrpcpp) 
 endif()
 
 if (NOT TARGET robot_cpp)
diff --git a/doc/architecture_discussion.md b/doc/architecture_discussion.md
index fefb5c2..a5ceb30 100644
--- a/doc/architecture_discussion.md
+++ b/doc/architecture_discussion.md
@@ -89,10 +89,10 @@ flowchart TB
   %% ========== DATA SOURCES ==========
   subgraph DataSources["📡 Data Sources"]
     direction TB
-    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
+    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
     Loc["<b>🌍 Localization</b><br/>━━━━━━━━━━━━━━━━<br/>📍 Pnkx Loc<br/>🗺️ Global Pose<br/>🔄 Pose Updates"]
     TF["<b>🔄 Transform System</b><br/>━━━━━━━━━━━━━━━━<br/>📐 tf3::BufferCore<br/>🌐 Coordinate Frames<br/>⏱️ Time Synchronization"]
-    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
+    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
     Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
     Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
     style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
@@ -107,7 +107,7 @@ flowchart TB
   %% ========== CONTROL LOOP ==========
   subgraph ControlLoop["🔄 Control Loop"]
     direction LR
-    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
+    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 robot_geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
     BaseCtrl["<b>🎮 Base Controller</b><br/>━━━━━━━━━━━━━━━━<br/>🚗 diff_driver_controller<br/>🚲 steer_drive_controller<br/>━━━━━━━━━━━━━━━━<br/>⚙️ Kinematics<br/>🔧 Hardware Interface"]
     style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000
     style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
@@ -232,9 +232,9 @@ Cần làm rõ:
    - Velocity command execution
 
 **Định dạng dữ liệu:**
-- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
-- `geometry_msgs::Twist` (vận tốc linear/angular)
-- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
+- `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í)
 
 ### 3. Thiết kế từng module (interface level)
diff --git a/doc/implementation_plan.md b/doc/implementation_plan.md
index 0c6391b..bcde82e 100644
--- a/doc/implementation_plan.md
+++ b/doc/implementation_plan.md
@@ -56,8 +56,8 @@
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
 ```cpp
 class IOdometrySource {
-  virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::PoseStamped getCurrentPose() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
   virtual bool isAvailable() = 0;
 };
 ```
@@ -66,7 +66,7 @@ class IOdometrySource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
 ```cpp
 class ILocalizationSource {
-  virtual geometry_msgs::PoseStamped getRobotPose() = 0;
+  virtual robot_geometry_msgs::PoseStamped getRobotPose() = 0;
   virtual std::string getGlobalFrame() = 0;
   virtual bool isLocalized() = 0;
 };
@@ -96,9 +96,9 @@ class ISensorSource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h`
 ```cpp
 class IBaseController {
-  virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
+  virtual bool setVelocity(const robot_geometry_msgs::Twist& cmd_vel) = 0;
   virtual bool stop() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
 };
 ```
 
@@ -120,7 +120,7 @@ class IBaseController {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h`
 ```cpp
 class IUserController {
-  virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
+  virtual void onGoalReceived(const robot_geometry_msgs::PoseStamped& goal) = 0;
   virtual void onNavigationStateChanged(move_base_core::State state) = 0;
   virtual void onFeedback(const NavFeedback& feedback) = 0;
 };
diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp
index 8ff27c4..b1f0545 100644
--- a/src/APIs/c_api/src/nav_c_api.cpp
+++ b/src/APIs/c_api/src/nav_c_api.cpp
@@ -2,10 +2,10 @@
 #include <move_base_core/navigation.h>
 #include <move_base_core/common.h>
 #include <move_base/move_base.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Vector3.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Vector3.h>
 #include <tf3/buffer_core.h>
 #include <string>
 #include <vector>
@@ -21,8 +21,8 @@
 
 namespace {
     // Convert C PoseStamped to C++ PoseStamped
-    geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
-        geometry_msgs::PoseStamped cpp_pose;
+    robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
+        robot_geometry_msgs::PoseStamped cpp_pose;
         cpp_pose.header.seq = c_pose->header.seq;
         cpp_pose.header.stamp.sec = c_pose->header.sec;
         cpp_pose.header.stamp.nsec = c_pose->header.nsec;
@@ -40,7 +40,7 @@ namespace {
     }
 
     // Convert C++ PoseStamped to C PoseStamped
-    void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
+    void cpp_to_c_pose_stamped(const robot_geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
         c_pose->header.seq = cpp_pose.header.seq;
         c_pose->header.sec = cpp_pose.header.stamp.sec;
         c_pose->header.nsec = cpp_pose.header.stamp.nsec;
@@ -59,7 +59,7 @@ namespace {
     }
 
     // Convert C++ Pose2D to C Pose2D
-    void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
+    void cpp_to_c_pose_2d(const robot_geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
         c_pose->x = cpp_pose.x;
         c_pose->y = cpp_pose.y;
         c_pose->theta = cpp_pose.theta;
@@ -112,12 +112,12 @@ extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double p
         return false;
     }
 
-    geometry_msgs::Pose2D pose;
+    robot_geometry_msgs::Pose2D pose;
     pose.x = pose_x;
     pose.y = pose_y;
     pose.theta = pose_theta;
 
-    geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
+    robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
     cpp_to_c_pose_stamped(result, out_goal);
     return true;
 }
@@ -128,8 +128,8 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, doubl
         return false;
     }
 
-    geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
-    geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
+    robot_geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
+    robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
     cpp_to_c_pose_stamped(result, out_goal);
     return true;
 }
@@ -260,10 +260,10 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        std::vector<geometry_msgs::Point> footprint;
+        std::vector<robot_geometry_msgs::Point> footprint;
         footprint.reserve(point_count);
         for (size_t i = 0; i < point_count; ++i) {
-            geometry_msgs::Point pt;
+            robot_geometry_msgs::Point pt;
             pt.x = points[i].x;
             pt.y = points[i].y;
             pt.z = points[i].z;
@@ -284,7 +284,7 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* g
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
     } catch (...) {
         return false;
@@ -300,7 +300,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
     } catch (...) {
         return false;
@@ -315,7 +315,7 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
     } catch (...) {
         return false;
@@ -330,7 +330,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
     } catch (...) {
         return false;
@@ -378,7 +378,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::Vector3 linear;
+        robot_geometry_msgs::Vector3 linear;
         linear.x = linear_x;
         linear.y = linear_y;
         linear.z = linear_z;
@@ -396,7 +396,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::Vector3 angular;
+        robot_geometry_msgs::Vector3 angular;
         angular.x = angular_x;
         angular.y = angular_y;
         angular.z = angular_z;
@@ -413,7 +413,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_pose;
+        robot_geometry_msgs::PoseStamped cpp_pose;
         if (nav->getRobotPose(cpp_pose)) {
             cpp_to_c_pose_stamped(cpp_pose, out_pose);
             return true;
@@ -431,7 +431,7 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::Pose2D cpp_pose;
+        robot_geometry_msgs::Pose2D cpp_pose;
         if (nav->getRobotPose(cpp_pose)) {
             cpp_to_c_pose_2d(cpp_pose, out_pose);
             return true;
diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h
index 6432a34..ebd0015 100755
--- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h
+++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h
@@ -3,8 +3,8 @@
 
 #include <robot/node_handle.h>
 #include <robot/console.h>
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_msgs/Path2D.h>
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 b834a7e..3bcd08c 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
@@ -12,7 +12,7 @@
 #include <nav_core2/costmap.h>
 
 #include <costmap_2d/costmap_2d_robot.h>
-#include <geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseArray.h>
 #include <mkt_msgs/Trajectory2D.h>
 #include <score_algorithm/trajectory_generator.h>
 
@@ -99,7 +99,7 @@ namespace score_algorithm
      * @param last_valid_index
      * @return goal index
      */
-    virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan,
+    virtual unsigned int getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan,
                                       const double distance, unsigned int start_index, unsigned int last_valid_index) const;
 
     /**
diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h
index 5a6ca3d..8ccbb3d 100755
--- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h
+++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h
@@ -8,7 +8,7 @@
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <memory>
 #include <vector>
-#include <geometry_msgs/Vector3.h>
+#include <robot_geometry_msgs/Vector3.h>
 
 namespace score_algorithm
 {
@@ -63,28 +63,28 @@ namespace score_algorithm
      * @param linear the velocity command
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistLinear(geometry_msgs::Vector3 linear) = 0;
+    virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) = 0;
 
     /**
      * @brief get velocity for x, y asix of the robot
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) = 0;
+    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) = 0;
 
     /**
      * @brief Set velocity theta for z asix of the robot
      * @param angular the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) = 0;
+    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) = 0;
 
     /**
      * @brief Get velocity theta for z asix of the robot
      * @param direct The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) = 0;
+    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) = 0;
 
     /**
      * @brief Test to see whether there are more twists to test
diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
index 44e278d..a19ea5a 100644
--- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
+++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
@@ -3,7 +3,7 @@
 #include <robot_nav_2d_utils/conversions.h>
 #include <angles/angles.h>
 
-unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan,
+unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan,
                                                            const double distance, unsigned int start_index, unsigned int last_valid_index) const
 {
   if (start_index >= last_valid_index)
@@ -46,7 +46,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
     return false;
   }
   double x_direction_saved = 0.0;
-  geometry_msgs::PoseArray poseArrayMsg;
+  robot_geometry_msgs::PoseArray poseArrayMsg;
   poseArrayMsg.header.stamp = robot::Time::now();
   poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
 
@@ -80,7 +80,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
         {
           seq_s.push_back(plan[i].header.seq);
           mutes.push_back(plan[i]);
-          geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
+          robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
           poseArrayMsg.poses.push_back(pose);
         }
       }
@@ -103,7 +103,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
         {
           seq_s.push_back(plan[i + 1].header.seq);
           mutes.push_back(plan[i + 1]);
-          geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
+          robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
           poseArrayMsg.poses.push_back(pose);
         }
         p2 = p3;
@@ -115,7 +115,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
   {
     seq_s.push_back(plan.back().header.seq);
     mutes.push_back(plan.back());
-    geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
+    robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
     poseArrayMsg.poses.push_back(pose);
   }
   else
@@ -124,7 +124,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
     {
       seq_s.push_back(plan.back().header.seq);
       mutes.push_back(plan.back());
-      geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
+      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
       poseArrayMsg.poses.push_back(pose);
     }
   }
@@ -369,7 +369,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     // check if goal already reached
     bool goal_already_reached = false;
 
-    // geometry_msgs::PoseArray poseArrayMsg;
+    // robot_geometry_msgs::PoseArray poseArrayMsg;
     // poseArrayMsg.header.stamp = robot::Time::now();
     // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
     // int c = 0;
@@ -377,7 +377,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
     {
       double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose));
 
-      // geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
+      // robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
       // poseArrayMsg.poses.push_back(pose);
 
       if (distance < xy_local_goal_tolerance_)
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h
index 8860308..f3fa588 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h
@@ -4,8 +4,8 @@
 #include <score_algorithm/score_algorithm.h>
 #include <mkt_algorithm/equation_line.h>
 #include <pluginlib/class_loader.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <kalman/kalman.h>
 
 namespace mkt_algorithm
@@ -39,8 +39,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -53,30 +53,30 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 
 protected:
 
-  unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
+  unsigned int getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
                             const double distance, unsigned int start_index, unsigned int last_valid_index) const;
 
-  double journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
+  double journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
 
 
-  bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
-                   const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, 
+  bool getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
+                   const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                    double &target_direction);
                          
-  bool findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
+  bool findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
 
   robot::NodeHandle nh_priv_, nh_;
   std::string frame_id_path_;
   robot::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
 
-  std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
-  geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
-  geometry_msgs::Pose2D goal_;
+  std::vector<robot_geometry_msgs::Pose2D> reached_intermediate_goals_;
+  robot_geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
+  robot_geometry_msgs::Pose2D goal_;
   robot_nav_2d_msgs::Path2D global_plan_;
 
   unsigned int start_index_saved_;
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h
index 0ca788e..e201e10 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h
@@ -1,7 +1,7 @@
 #ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
 #define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
 #include <ros/ros.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 #include <iostream>
 
 namespace mkt_algorithm
@@ -24,8 +24,8 @@ class LineGenerator
 {
   /* data */
   struct LineEquation {
-    geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
-    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    robot_geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
     float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
   };
 
@@ -46,7 +46,7 @@ public:
    * @param start_point   start point of the line
    * @param end_point     end point of the line
    */
-  virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
+  virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);
 
   /**
    * @brief Calculating error 
@@ -54,14 +54,14 @@ public:
    * @param[in] pose
    * @param[in] rev         
    */
-  virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
+  virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);
 
   /**
    * @brief Calculating tolerance 
    * @param query_pose The pose to check
    * @param goal_pose The pose to check against
    */
-  virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
+  virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);
 
   /* Properties */
   LineEquation      Leq_; 
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h
index 779e3c6..ffd165f 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h
@@ -3,8 +3,8 @@
 
 #include <score_algorithm/score_algorithm.h>
 #include <pluginlib/class_loader.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 
 namespace mkt_algorithm
 {
@@ -37,8 +37,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -51,7 +51,7 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 
 protected:
@@ -60,7 +60,7 @@ protected:
 
   double steering_fix_wheel_distance_x_;
   double steering_fix_wheel_distance_y_;
-  geometry_msgs::Pose2D goal_;
+  robot_geometry_msgs::Pose2D goal_;
   robot::Time time_last_cmd_;
 
 }; // class GoStraight 
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h
index 2d09c42..6c24369 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h
@@ -28,8 +28,8 @@ public:
    * @param velocity The robot's current velocity
    * @return True if goal is reached
    */
-  bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose, 
-                             const std::vector<geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
+  bool isGoalReached(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose, 
+                             const std::vector<robot_geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
 
 protected:
 
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h
index 5e20b6b..631c996 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h
@@ -36,8 +36,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -45,7 +45,7 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 protected:
 
diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h
index fb6b86a..599f915 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h
+++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h
@@ -3,8 +3,8 @@
 
 #include <score_algorithm/score_algorithm.h>
 #include <pluginlib/class_loader.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 
 namespace mkt_algorithm
 {
@@ -37,8 +37,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -51,20 +51,20 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 
 protected:
   robot::NodeHandle nh_;
   robot::NodeHandle nh_kinematics_;
-  geometry_msgs::Pose2D sub_goal_;
+  robot_geometry_msgs::Pose2D sub_goal_;
 
   double steering_fix_wheel_distance_x_;
   double steering_fix_wheel_distance_y_;
   double angle_threshold_;
   double current_direction_;
   double velocity_rotate_min_, velocity_rotate_max_;
-  geometry_msgs::Pose2D goal_;
+  robot_geometry_msgs::Pose2D goal_;
   robot::Time time_last_cmd_;
 
 }; // class RotateToGoal 
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 36e7730..c5ca300 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
@@ -5,8 +5,8 @@
 #include <score_algorithm/score_algorithm.h>
 
 #include <boost/dll/alias.hpp>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PointStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PointStamped.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <nav_core2/exceptions.h>
 #include <nav_core2/costmap.h>
@@ -127,7 +127,7 @@ namespace mkt_algorithm
        * @param carrot_pose
        * @return carrot message
        */
-      geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);
+      robot_geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);
 
       /**
        * @brief Prune global plan
@@ -210,7 +210,7 @@ namespace mkt_algorithm
           const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
           const double &pose_cost, double &linear_vel, double &sign_x);
 
-      std::vector<geometry_msgs::Point> interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution);
+      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
 
       /**
        * @brief Cost at pose
@@ -291,7 +291,7 @@ namespace mkt_algorithm
 
       // Control frequency
       double control_duration_;
-      std::vector<geometry_msgs::Point> footprint_spec_;
+      std::vector<robot_geometry_msgs::Point> footprint_spec_;
 
       robot::Time last_actuator_update_;
       boost::shared_ptr<KalmanFilter> kf_;
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp
index f2de62e..26f2316 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp
@@ -22,8 +22,8 @@ void GoStraight::reset()
   time_last_cmd_ = robot::Time::now();
 }
 
-bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+bool GoStraight::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) 
 {
   goal_ = goal;
@@ -37,12 +37,12 @@ bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_
   return  true;
 }
             
-robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
+robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
 {
   robot_nav_2d_msgs::Twist2DStamped result;
   result.header.stamp = robot::Time::now();
 
-  geometry_msgs::Pose2D steer_to_baselink_pose;
+  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
   
   if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
     return result;
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp
index 4db5271..2c85547 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp
@@ -38,10 +38,10 @@ void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListen
     controller_frequency_param_name_ = 1.0;
   }
 
-  reached_intermediate_goals_pub_   = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
-  current_goal_pub_                 = nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
-  closet_robot_goal_pub_            = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
-  cmd_raw_pub_                      = nh_.advertise<geometry_msgs::Twist>("cmd_raw", 1);
+  reached_intermediate_goals_pub_   = nh_.advertise<robot_geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
+  current_goal_pub_                 = nh_.advertise<robot_geometry_msgs::PoseStamped>("current_goal", 1);
+  closet_robot_goal_pub_            = nh_.advertise<robot_geometry_msgs::PoseStamped>("closet_robot_goal", 1);
+  cmd_raw_pub_                      = nh_.advertise<robot_geometry_msgs::Twist>("cmd_raw", 1);
   x_direction_ = y_direction_= theta_direction_ = 0;
 
 // kalman
@@ -87,8 +87,8 @@ void Bicycle::reset()
   last_actuator_update_ = robot::Time(0);
 }
 
-bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+bool Bicycle::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) 
 {
   nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
@@ -119,12 +119,12 @@ bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msg
   return true;
 }
             
-robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
+robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
 {
   robot_nav_2d_msgs::Twist2DStamped result;
   result.header.stamp = robot::Time::now();
   
-  geometry_msgs::Pose2D steer_to_baselink_pose;
+  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
   
   if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
     return result;
@@ -154,7 +154,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
     const double d_theta = goal_.theta - sub_goal_.theta;
     if(dd < 1e-5)
     {
-      geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
+      robot_geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
       alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta);
       phi_steer = atan2(2 * L * sin(alpha), 1.0);
     }
@@ -198,7 +198,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
 
   if(is_filter_)
   {
-    geometry_msgs::Twist msg;
+    robot_geometry_msgs::Twist msg;
     msg.linear.x = result.velocity.x;
     msg.angular.z = phi_steer;
     cmd_raw_pub_.publish(msg);
@@ -220,7 +220,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
   return result;
 }
 
-unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
+unsigned int Bicycle::getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
                                    const double distance, unsigned int start_index, unsigned int last_valid_index) const
 {
   if (start_index >= last_valid_index)
@@ -253,8 +253,8 @@ unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, cons
   return goal_index;
 }
 
-bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
-                          const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, 
+bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
+                          const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                           double &target_direction)
 {
   const nav_core2::Costmap &costmap = *costmap_;
@@ -265,8 +265,8 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
     ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
     return false;
   }
-  // Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
-  std::vector<geometry_msgs::Pose2D> plan = global_plan.poses;
+  // Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<robot_geometry_msgs::Pose2D>'
+  std::vector<robot_geometry_msgs::Pose2D> plan = global_plan.poses;
 
   std::vector<unsigned int> index_s;
   if(!findSubGoalIndex(plan, index_s))
@@ -425,7 +425,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
     // check if goal already reached
     bool goal_already_reached = false;
 
-    geometry_msgs::PoseArray poseArrayMsg;
+    robot_geometry_msgs::PoseArray poseArrayMsg;
     poseArrayMsg.header.stamp = robot::Time::now();
     poseArrayMsg.header.frame_id = frame_id_path_;
 	  // std::stringstream ss;
@@ -433,7 +433,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
     {
       double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
       // ss << distance << " ";
-      geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
+      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
       poseArrayMsg.poses.push_back(pose);
     }
     reached_intermediate_goals_pub_.publish(poseArrayMsg);
@@ -538,14 +538,14 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
   {
     if(goal_index < (unsigned int)(plan.size() -1))
     {
-      const geometry_msgs::Pose2D p1 = plan[start_index];
+      const robot_geometry_msgs::Pose2D p1 = plan[start_index];
       int index;
       for(index = start_index+1; index < goal_index; index++)
       {
-        geometry_msgs::Pose2D pose = plan[index];
+        robot_geometry_msgs::Pose2D pose = plan[index];
         if(robot_nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_)  break;
       }      
-      const geometry_msgs::Pose2D p2 = plan[index];
+      const robot_geometry_msgs::Pose2D p2 = plan[index];
       if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
       {
         const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
@@ -569,32 +569,32 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
   }
 
   // Publish start_index
-  geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
+  robot_geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
   closet_robot_goal_pub_.publish(pose_st);
 
   // Publish goal_index
-  geometry_msgs::PoseStamped pose_g = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
+  robot_geometry_msgs::PoseStamped pose_g = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
   current_goal_pub_.publish(pose_g);
  
   return true;
 }
 
-bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
+bool Bicycle::findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
 {
   if (plan.empty())
   {
     return false;
   }
   double x_direction_saved = 0.0;
-  geometry_msgs::PoseArray poseArrayMsg;
+  robot_geometry_msgs::PoseArray poseArrayMsg;
   poseArrayMsg.header.stamp = robot::Time::now();
   poseArrayMsg.header.frame_id = frame_id_path_;
 
   for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)
   {
-    geometry_msgs::Pose2D p1 = plan[i];
-    geometry_msgs::Pose2D p2 = plan[i+1];
-    // geometry_msgs::Pose2D p3 = plan[i+2];
+    robot_geometry_msgs::Pose2D p1 = plan[i];
+    robot_geometry_msgs::Pose2D p2 = plan[i+1];
+    // robot_geometry_msgs::Pose2D p3 = plan[i+2];
     double angle1;
     if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
       angle1 = atan2(p2.y - p1.y, p2.x - p1.x);
@@ -605,7 +605,7 @@ bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, s
     if(target_direction * x_direction_saved < 0.0)
     {
       index_s.push_back(i);
-      geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]);
+      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]);
       poseArrayMsg.poses.push_back(pose);
     }
     x_direction_saved = target_direction;
@@ -622,7 +622,7 @@ bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, s
   return true;
 }
 
-double Bicycle::journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
+double Bicycle::journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
 {
   double S = 0; 
   if(last_valid_index - start_index > 1)
diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp
index c817b83..0555d4f 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp
@@ -25,8 +25,8 @@ void RotateToGoal::reset()
   time_last_cmd_ = robot::Time::now();
 }
 
-bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+bool RotateToGoal::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) 
 {
   goal_ = goal;
@@ -38,12 +38,12 @@ bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2
   return  true;
 }
             
-robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
+robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
 {
   robot_nav_2d_msgs::Twist2DStamped result;
   result.header.stamp = robot::Time::now();
 
-  geometry_msgs::Pose2D steer_to_baselink_pose;
+  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
   
   if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
     return result;
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 01e2f4a..7bb8c8a 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
@@ -18,15 +18,15 @@ void mkt_algorithm::diff::GoStraight::initialize(
     nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
-    std::vector<geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+    std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
       double min_length = 1e10;
       double max_length = 0.0;
       for (size_t i = 0; i < footprint.size(); ++i)
       {
-        const geometry_msgs::Point &p1 = footprint[i];
-        const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+        const robot_geometry_msgs::Point &p1 = footprint[i];
+        const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
         double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
         if (len > max_length)
         {
@@ -125,7 +125,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   // Dynamically adjust look ahead distance based on the speed
   double lookahead_dist = this->getLookAheadDistance(twist);
   // Return false if the transformed global plan is empty
-  geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
+  robot_geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
   lookahead_dist += fabs(front.y);
 
   // Get lookahead point and publish for visualization
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 88a4d12..e08673c 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
@@ -20,15 +20,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
     footprint_spec_ = costmap_robot_->getRobotFootprint();
-    std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+    std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
       double min_length = 1e10;
       double max_length = 0.0;
       for (size_t i = 0; i < footprint.size(); ++i)
       {
-        const geometry_msgs::Point &p1 = footprint[i];
-        const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+        const robot_geometry_msgs::Point &p1 = footprint[i];
+        const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
         double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
         if (len > max_length)
         {
@@ -218,15 +218,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     return false;
   }
 
-  std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+  std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
   if (footprint.size() > 1)
   {
     double min_length = 1e10;
     double max_length = 0.0;
     for (size_t i = 0; i < footprint.size(); ++i)
     {
-      const geometry_msgs::Point &p1 = footprint[i];
-      const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+      const robot_geometry_msgs::Point &p1 = footprint[i];
+      const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
       double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
       if (len > max_length)
       {
@@ -308,15 +308,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
 
   if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq)
   {
-    const geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
+    const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
     int index;
     for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
     {
-      geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
+      robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
       if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_)
         break;
     }
-    const geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
+    const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
     if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
     {
       const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
@@ -344,11 +344,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
         }
       }
 
-      geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
+      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
                                       ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
-                                      : robot_nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D());
+                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
 
-      geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
+      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
       // teb_local_planner::PoseSE2 start_pose(front);
       // teb_local_planner::PoseSE2 goal_pose(back);
@@ -419,7 +419,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   nh_priv_.param("allow_rotate", allow_rotate, false);
 
   // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y);
-  geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
+  robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
   const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
@@ -768,9 +768,9 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
   return goal_pose_it;
 }
 
-geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose)
+robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose)
 {
-  geometry_msgs::PointStamped carrot_msg;
+  robot_geometry_msgs::PointStamped carrot_msg;
   carrot_msg.header = carrot_pose.header;
   carrot_msg.point.x = carrot_pose.pose.x;
   carrot_msg.point.y = carrot_pose.pose.y;
@@ -915,8 +915,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
       // Calculate distance to previous pose
       if (i > 0 && max_plan_length > 0)
       {
-        geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
-        geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
+        robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
+        robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
         plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2));
         if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
         {
@@ -1097,29 +1097,29 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
   // ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
 }
 
-std::vector<geometry_msgs::Point>
-mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution)
+std::vector<robot_geometry_msgs::Point>
+mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
 {
 
   // Dịch sang tọa độ tuyệt đối
-  std::vector<geometry_msgs::Point> abs_footprint;
+  std::vector<robot_geometry_msgs::Point> abs_footprint;
   double cos_th = std::cos(pose.theta);
   double sin_th = std::sin(pose.theta);
   for (auto pt : footprint)
   {
     pt.x *= 1.2;
     pt.y *= 1.2;
-    geometry_msgs::Point abs_pt;
+    robot_geometry_msgs::Point abs_pt;
     abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
     abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
     abs_footprint.push_back(abs_pt);
   }
 
-  std::vector<geometry_msgs::Point> points;
+  std::vector<robot_geometry_msgs::Point> points;
   for (size_t i = 0; i < abs_footprint.size(); ++i)
   {
-    const geometry_msgs::Point &start = abs_footprint[i];
-    const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+    const robot_geometry_msgs::Point &start = abs_footprint[i];
+    const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
 
     double dx = end.x - start.x;
     double dy = end.y - start.y;
@@ -1131,7 +1131,7 @@ mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::P
       if (j == steps && i != abs_footprint.size() - 1)
         continue; // tránh lặp
       double t = static_cast<double>(j) / steps;
-      geometry_msgs::Point pt;
+      robot_geometry_msgs::Point pt;
       pt.x = start.x + t * dx;
       pt.y = start.y + t * dy;
       points.push_back(pt);
diff --git a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h
index ee3d74c..6d5268f 100644
--- a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h
+++ b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h
@@ -9,7 +9,7 @@
 #include <vector>
 #include <memory>
 #include <robot_nav_2d_msgs/Twist2D.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 
 namespace mkt_msgs
 {
@@ -31,7 +31,7 @@ namespace mkt_msgs
     typedef ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
     _velocity_type velocity;
 
-    typedef std::vector<geometry_msgs::Pose2D, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<geometry_msgs::Pose2D>> _poses_type;
+    typedef std::vector<robot_geometry_msgs::Pose2D, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<robot_geometry_msgs::Pose2D>> _poses_type;
     _poses_type poses;
 
     typedef std::vector<robot::Duration, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<robot::Duration>> _time_offsets_type;
diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h
index cf891b3..8c4a306 100644
--- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h
+++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h
@@ -1,7 +1,7 @@
 #ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
 #define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
 #include <robot/node_handle.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 #include <iostream>
 
 namespace mkt_plugins
@@ -24,8 +24,8 @@ class LineGenerator
 {
   /* data */
   struct LineEquation {
-    geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
-    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    robot_geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
     float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
   };
 
@@ -46,7 +46,7 @@ public:
    * @param start_point   start point of the line
    * @param end_point     end point of the line
    */
-  virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
+  virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);
 
   /**
    * @brief Calculating error 
@@ -54,14 +54,14 @@ public:
    * @param[in] pose
    * @param[in] rev         
    */
-  virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
+  virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);
 
   /**
    * @brief Calculating tolerance 
    * @param query_pose The pose to check
    * @param goal_pose The pose to check against
    */
-  virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
+  virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);
 
   /* Properties */
   LineEquation      Leq_; 
diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h
index 99e88c8..ee26ac5 100644
--- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h
+++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h
@@ -38,28 +38,28 @@ namespace mkt_plugins
      * @param linear the velocity command
      * @return True if set is done, false otherwise
      */
-    bool setTwistLinear(geometry_msgs::Vector3 linear) override;
+    bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;
 
     /**
      * @brief get velocity for x, y asix of the robot
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
+    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;
 
     /**
      * @brief Set velocity theta for z asix of the robot
      * @param angular the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
+    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;
 
     /**
      * @brief Get velocity theta for z asix of the robot
      * @param direct The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
+    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;
 
     /**
      * @brief Start a new iteration with current velocity
@@ -138,7 +138,7 @@ namespace mkt_plugins
      * @param dt amount of time in seconds
      * @return New pose after dt seconds
      */
-    virtual geometry_msgs::Pose2D computeNewPosition(const geometry_msgs::Pose2D start_pose,
+    virtual robot_geometry_msgs::Pose2D computeNewPosition(const robot_geometry_msgs::Pose2D start_pose,
                                                      const robot_nav_2d_msgs::Twist2D &vel,
                                                      const double dt);
 
diff --git a/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp b/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp
index 522c45c..2b142b8 100644
--- a/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp
+++ b/src/Algorithms/Libraries/mkt_plugins/src/equation_line.cpp
@@ -5,7 +5,7 @@
 namespace mkt_plugins
 {
 
-bool LineGenerator::calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point)
+bool LineGenerator::calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point)
 {
   /**
    *  Phương trình đường thẳng có dạng: 
@@ -19,7 +19,7 @@ bool LineGenerator::calculatorEquationLine(geometry_msgs::Pose2D start_point, ge
   return Leq_.a != 0 || Leq_.b !=0;
 }
 
-void LineGenerator::calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev)
+void LineGenerator::calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev)
 {
   float pha = rev? M_PI : 0;
   float Yaw_rad   = pose.theta - pha;
@@ -87,7 +87,7 @@ void LineGenerator::calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, boo
  
 }
 
-double LineGenerator::calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose)
+double LineGenerator::calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose)
 {
   /**
    *  Phương trình đường thẳng có dạng: 
diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
index 7c7918b..60d7570 100644
--- a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
+++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
@@ -59,7 +59,7 @@ namespace mkt_plugins
     kinematics_->setDirect(direct);
   }
 
-  bool StandardTrajectoryGenerator::setTwistLinear(geometry_msgs::Vector3 linear)
+  bool StandardTrajectoryGenerator::setTwistLinear(robot_geometry_msgs::Vector3 linear)
   {
     try
     {
@@ -93,9 +93,9 @@ namespace mkt_plugins
     return true;
   }
 
-  geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct)
+  robot_geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct)
   {
-    geometry_msgs::Vector3 linear;
+    robot_geometry_msgs::Vector3 linear;
     try
     {
       if (direct)
@@ -116,7 +116,7 @@ namespace mkt_plugins
     return linear;
   }
 
-  bool StandardTrajectoryGenerator::setTwistAngular(geometry_msgs::Vector3 angular)
+  bool StandardTrajectoryGenerator::setTwistAngular(robot_geometry_msgs::Vector3 angular)
   {
     try
     {
@@ -130,9 +130,9 @@ namespace mkt_plugins
     return true;
   }
 
-  geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct)
+  robot_geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct)
   {
-    geometry_msgs::Vector3 angular;
+    robot_geometry_msgs::Vector3 angular;
     try
     {
       if (direct)
@@ -205,7 +205,7 @@ namespace mkt_plugins
     traj.velocity = cmd_vel;
 
     //  simulate the trajectory
-    geometry_msgs::Pose2D pose = start_pose.pose;
+    robot_geometry_msgs::Pose2D pose = start_pose.pose;
     robot_nav_2d_msgs::Twist2D vel = start_vel;
     double running_time = 0.0;
     std::vector<double> steps = getTimeSteps(cmd_vel);
@@ -245,10 +245,10 @@ namespace mkt_plugins
     return new_vel;
   }
 
-  geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose,
+  robot_geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const robot_geometry_msgs::Pose2D start_pose,
                                                                         const robot_nav_2d_msgs::Twist2D &vel, const double dt)
   {
-    geometry_msgs::Pose2D new_pose;
+    robot_geometry_msgs::Pose2D new_pose;
     new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt;
     new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
     new_pose.theta = start_pose.theta + vel.theta * dt;
diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner
index 377ebe3..6d55c6c 160000
--- a/src/Algorithms/Packages/global_planners/custom_planner
+++ b/src/Algorithms/Packages/global_planners/custom_planner
@@ -1 +1 @@
-Subproject commit 377ebe3d6f66c6a1c7f2d1a1d76fd1b330a93952
+Subproject commit 6d55c6c4beb86ae77c20cba3f26c6231a16912fa
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 8fc77d0..2498c0e 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
@@ -4,7 +4,7 @@
 
 #include <iostream>
 #include <vector>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <nav_core/base_global_planner.h>
 #include <costmap_2d/costmap_2d_robot.h>
 #include <robot/node_handle.h>
@@ -48,9 +48,9 @@ public:
    * @param plan The plan... filled by the planner
    * @return True if a valid plan was found, false otherwise
    */
-  virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
-                        const geometry_msgs::PoseStamped& goal, 
-                        std::vector<geometry_msgs::PoseStamped>& plan);
+  virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
+                        const robot_geometry_msgs::PoseStamped& goal, 
+                        std::vector<robot_geometry_msgs::PoseStamped>& plan);
 
   /**
    * @brief Create a new TwoPointsPlanner instance
@@ -72,7 +72,7 @@ protected:
 
   std::string name_;
   costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
-  std::vector<geometry_msgs::Point> footprint_;
+  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 8709d62..a1a9ae3 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
@@ -100,9 +100,9 @@ namespace two_points_planner
     }
   }
 
-  bool TwoPointsPlanner::makePlan(const geometry_msgs::PoseStamped& start,
-    const geometry_msgs::PoseStamped& goal,
-    std::vector<geometry_msgs::PoseStamped>& plan)
+  bool TwoPointsPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
+    const robot_geometry_msgs::PoseStamped& goal,
+    std::vector<robot_geometry_msgs::PoseStamped>& plan)
   {
     if (!initialized_)
     {
@@ -188,7 +188,7 @@ namespace two_points_planner
     {
       double fraction = static_cast<double>(i) / num_points;
 
-      geometry_msgs::PoseStamped pose;
+      robot_geometry_msgs::PoseStamped pose;
       pose.header.stamp = plan_time;
       pose.header.frame_id = costmap_robot_->getGlobalFrameID();
       pose.pose.position.x = start.pose.position.x + fraction * dx;
@@ -199,7 +199,7 @@ namespace two_points_planner
       tf3::Quaternion interpolated_quat;
       interpolated_quat.setRPY(0, 0, theta);
 
-      // Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion
+      // Chuyển đổi trực tiếp từ tf3::Quaternion sang robot_geometry_msgs::Quaternion
       pose.pose.orientation.x = interpolated_quat.x();
       pose.pose.orientation.y = interpolated_quat.y();
       pose.pose.orientation.z = interpolated_quat.z();
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 54564a3..8210570 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
@@ -140,8 +140,8 @@ namespace pnkx_local_planner
       nav_core::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
-      geometry_msgs::Vector3 linear_;
-      geometry_msgs::Vector3 angular_;
+      robot_geometry_msgs::Vector3 linear_;
+      robot_geometry_msgs::Vector3 angular_;
 
     private:
       /**
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 4e6a3cd..290c278 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
@@ -67,28 +67,28 @@ namespace pnkx_local_planner
      * @param linear the velocity command
      * @return True if set is done, false otherwise
      */
-    bool setTwistLinear(geometry_msgs::Vector3 linear) override;
+    bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;
 
     /**
      * @brief get velocity for x, y asix of the robot
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
+    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;
 
     /**
      * @brief Set velocity theta for z asix of the robot
      * @param angular the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
+    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;
 
     /**
      * @brief Get velocity theta for z asix of the robot
      * @param direct The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
+    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;
 
     /**
      * @brief Check if the kinematic parameters are currently locked
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 269fd13..9d9fbc3 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
@@ -23,7 +23,7 @@ namespace pnkx_local_planner
   bool transformGlobalPose(TFListenerPtr tf, const std::string &global_frame,
                            const robot_nav_2d_msgs::Pose2DStamped &stamped_pose, robot_nav_2d_msgs::Pose2DStamped &newer_pose);
 
-  double getSquareDistance(const geometry_msgs::Pose2D &pose_a, const geometry_msgs::Pose2D &pose_b);
+  double getSquareDistance(const robot_geometry_msgs::Pose2D &pose_a, const robot_geometry_msgs::Pose2D &pose_b);
 
   /**
    * @brief  Transforms the global plan of the robot from the planner frame to the local frame (modified).
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 19354ab..7fe882f 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
@@ -298,7 +298,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
     {
       this->lock();
-      geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
+      robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
       traj_generator_->setTwistLinear(linear);
       linear.x *= (-1);
       traj_generator_->setTwistLinear(linear);
@@ -733,9 +733,9 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(ro
 bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
     const robot_nav_2d_msgs::Pose2DStamped &local_pose, const robot_nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path)
 {
-  geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
-  geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
-  std::vector<geometry_msgs::PoseStamped> docking_plan;
+  robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
+  robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
+  std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
 
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {
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 2b81784..41a5bf5 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
@@ -299,7 +299,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
   return cmd_vel;
 }
 
-bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(geometry_msgs::Vector3 linear)
+bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(robot_geometry_msgs::Vector3 linear)
 {
   try
   {
@@ -330,7 +330,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(geometry_msgs::Vector3
   }
 }
 
-geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct)
+robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct)
 {
   try
   {
@@ -345,7 +345,7 @@ geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool
   }
 }
 
-bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(geometry_msgs::Vector3 angular)
+bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(robot_geometry_msgs::Vector3 angular)
 {
   try
   {
@@ -361,7 +361,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(geometry_msgs::Vector
   }
 }
 
-geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct)
+robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct)
 {
   try
   {
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 067237f..8feae1e 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
@@ -10,9 +10,9 @@ bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, st
   if (!tf)
     return false;
 
-  geometry_msgs::PoseStamped global_pose_stamped;
+  robot_geometry_msgs::PoseStamped global_pose_stamped;
   tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
-  geometry_msgs::PoseStamped robot_pose;
+  robot_geometry_msgs::PoseStamped robot_pose;
   tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
   robot_pose.header.frame_id = base_frame_id;
   robot_pose.header.stamp = robot::Time();
@@ -80,7 +80,7 @@ bool pnkx_local_planner::transformGlobalPose(TFListenerPtr tf, const std::string
   return result;
 }
 
-double pnkx_local_planner::getSquareDistance(const geometry_msgs::Pose2D& pose_a, const geometry_msgs::Pose2D& pose_b)
+double pnkx_local_planner::getSquareDistance(const robot_geometry_msgs::Pose2D& pose_a, const robot_geometry_msgs::Pose2D& pose_b)
 {
   double x_diff = pose_a.x - pose_b.x;
   double y_diff = pose_a.y - pose_b.y;
@@ -148,15 +148,15 @@ bool pnkx_local_planner::transformGlobalPlan(
 
     double sq_prune_dist = 0.1;
 
-    std::vector<geometry_msgs::Point> footprint = costmap->getRobotFootprint();
+    std::vector<robot_geometry_msgs::Point> footprint = costmap->getRobotFootprint();
     size_t n = footprint.size();
     if (n > 1)
     {
       double max_length = 0.0;
       for (size_t i = 0; i < n; ++i)
       {
-        const geometry_msgs::Point& p1 = footprint[i];
-        const geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
+        const robot_geometry_msgs::Point& p1 = footprint[i];
+        const robot_geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
         double len = std::hypot(p2.x - p1.x, p2.y - p1.y);
         if (len > max_length)
         {
diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d
index 71adf13..2c3d7d5 160000
--- a/src/Libraries/costmap_2d
+++ b/src/Libraries/costmap_2d
@@ -1 +1 @@
-Subproject commit 71adf1390fe7c1bad3823d25a06db43a9c46278b
+Subproject commit 2c3d7d586d1a664573b99f45d8521950d46db30a
diff --git a/src/Libraries/data_convert b/src/Libraries/data_convert
index 8b22de3..b1aaa1a 160000
--- a/src/Libraries/data_convert
+++ b/src/Libraries/data_convert
@@ -1 +1 @@
-Subproject commit 8b22de38ac16eccce348e505e12cfafdf33e2943
+Subproject commit b1aaa1a9463853ec16574b27182c5753c630a407
diff --git a/src/Libraries/geometry2 b/src/Libraries/geometry2
index 3c51b22..2987c1a 160000
--- a/src/Libraries/geometry2
+++ b/src/Libraries/geometry2
@@ -1 +1 @@
-Subproject commit 3c51b228ec4d3f3223225142d3c13e485e01de52
+Subproject commit 2987c1a481390e4d0bf08cf97aee3bc758d23ad1
diff --git a/src/Libraries/laser_geometry b/src/Libraries/laser_geometry
index 1b06dd9..a183d4b 160000
--- a/src/Libraries/laser_geometry
+++ b/src/Libraries/laser_geometry
@@ -1 +1 @@
-Subproject commit 1b06dd9122b46f8d8a7c0414d8fa5043656f7933
+Subproject commit a183d4bb7bdd1e6eb44be08911cc1a4e4590c4e4
diff --git a/src/Libraries/nav_2d_msgs/CMakeLists.txt b/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt
similarity index 100%
rename from src/Libraries/nav_2d_msgs/CMakeLists.txt
rename to src/Libraries/robot_nav_2d_msgs/CMakeLists.txt
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h
similarity index 95%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h
index c2c2471..9fd69cd 100644
--- a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h
+++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h
@@ -11,7 +11,7 @@
 #include <memory>
 
 #include <std_msgs/Header.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 
 namespace robot_nav_2d_msgs
 {
@@ -35,7 +35,7 @@ struct Pose2DStamped_
    typedef  ::std_msgs::Header  _header_type;
   _header_type header;
 
-   typedef  ::geometry_msgs::Pose2D  _pose_type;
+   typedef  ::robot_geometry_msgs::Pose2D  _pose_type;
   _pose_type pose;
 
 
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h
diff --git a/src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h
similarity index 100%
rename from src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h
rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h
diff --git a/src/Libraries/nav_2d_msgs/package.xml b/src/Libraries/robot_nav_2d_msgs/package.xml
similarity index 100%
rename from src/Libraries/nav_2d_msgs/package.xml
rename to src/Libraries/robot_nav_2d_msgs/package.xml
diff --git a/src/Libraries/nav_2d_utils/CMakeLists.txt b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt
similarity index 91%
rename from src/Libraries/nav_2d_utils/CMakeLists.txt
rename to src/Libraries/robot_nav_2d_utils/CMakeLists.txt
index cebd27c..a2bff7a 100755
--- a/src/Libraries/nav_2d_utils/CMakeLists.txt
+++ b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt
@@ -28,7 +28,7 @@ target_include_directories(conversions
 target_link_libraries(conversions
     PUBLIC
         robot_nav_2d_msgs
-        geometry_msgs
+        robot_geometry_msgs
         nav_msgs
         nav_grid
         nav_core2
@@ -52,7 +52,7 @@ target_include_directories(path_ops
 target_link_libraries(path_ops
     PUBLIC
         robot_nav_2d_msgs
-        geometry_msgs
+        robot_geometry_msgs
         robot_cpp
 )
 
@@ -66,17 +66,17 @@ target_include_directories(polygons
 if(robot_xmlrpcpp_FOUND)
     target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
     target_link_libraries(polygons
-        PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
+        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
         PRIVATE ${robot_xmlrpcpp_LIBRARIES})
 elseif(XmlRpcCpp_FOUND)
     target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
     target_link_libraries(polygons 
-        PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp 
+        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp 
         PRIVATE ${XmlRpcCpp_LIBRARIES})
 else()
     target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
     target_link_libraries(polygons
-        PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
+        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
         PRIVATE ${robot_xmlrpcpp_LIBRARIES})
 endif()
 
@@ -110,7 +110,7 @@ target_include_directories(tf_help
 target_link_libraries(tf_help
     PUBLIC
         robot_nav_2d_msgs
-        geometry_msgs
+        robot_geometry_msgs
         nav_core2
         tf3
         robot_cpp
@@ -166,5 +166,5 @@ message(STATUS "Project: ${PROJECT_NAME}")
 message(STATUS "Version: ${PROJECT_VERSION}")
 message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
 message(STATUS "Libraries: conversions, path_ops, polygons, bounds, tf_help")
-message(STATUS "Dependencies: robot_nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
+message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
 message(STATUS "=================================")
diff --git a/src/Libraries/nav_2d_utils/README.md b/src/Libraries/robot_nav_2d_utils/README.md
similarity index 100%
rename from src/Libraries/nav_2d_utils/README.md
rename to src/Libraries/robot_nav_2d_utils/README.md
diff --git a/src/Libraries/nav_2d_utils/doc/Conversions.md b/src/Libraries/robot_nav_2d_utils/doc/Conversions.md
similarity index 52%
rename from src/Libraries/nav_2d_utils/doc/Conversions.md
rename to src/Libraries/robot_nav_2d_utils/doc/Conversions.md
index 5588628..c8e08d2 100755
--- a/src/Libraries/nav_2d_utils/doc/Conversions.md
+++ b/src/Libraries/robot_nav_2d_utils/doc/Conversions.md
@@ -5,35 +5,35 @@
 ## Twist Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
+| `Twist2D twist3Dto2D(robot_geometry_msgs::Twist)` | `robot_geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
 
 ## Point Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)`
-| `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)`
+| `Point2D pointToPoint2D(robot_geometry_msgs::Point)` | `robot_geometry_msgs::Point pointToPoint3D(Point2D)`
+| `Point2D pointToPoint2D(robot_geometry_msgs::Point32)` | `robot_geometry_msgs::Point32 pointToPoint32(Point2D)`
 
 ## Pose Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
-||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
-||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
+| `Pose2DStamped poseStampedToPose2D(robot_geometry_msgs::PoseStamped)` | `robot_geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
+||`robot_geometry_msgs::PoseStamped pose2DToPoseStamped(robot_geometry_msgs::Pose2D, std::string, robot::Time)`|
+||`robot_geometry_msgs::Pose pose2DToPose(robot_geometry_msgs::Pose2D)`|
 | `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
 
 ## Path Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
 | `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)`
-| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, robot::Time)`
+| `Path2D posesToPath2D(std::vector<robot_geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<robot_geometry_msgs::Pose2D>, std::string, robot::Time)`
 
-Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
+Also, `nav_msgs::Path posesToPath(std::vector<robot_geometry_msgs::PoseStamped>)`
 
 ## Polygon Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
-| `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
+| `Polygon2D polygon3Dto2D(robot_geometry_msgs::Polygon)` |`robot_geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
+| `Polygon2DStamped polygon3Dto2D(robot_geometry_msgs::PolygonStamped)` | `robot_geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
 
 ## Info Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
@@ -46,7 +46,7 @@ Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
 
 | to two dimensional pose | to three dimensional pose |
 | -- | -- |
-| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`|
+| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `robot_geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`|
 
 ## Bounds Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
diff --git a/src/Libraries/nav_2d_utils/doc/PluginMux.md b/src/Libraries/robot_nav_2d_utils/doc/PluginMux.md
similarity index 100%
rename from src/Libraries/nav_2d_utils/doc/PluginMux.md
rename to src/Libraries/robot_nav_2d_utils/doc/PluginMux.md
diff --git a/src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md b/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md
similarity index 100%
rename from src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md
rename to src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md
diff --git a/src/Libraries/nav_2d_utils/include/mapbox/LICENSE b/src/Libraries/robot_nav_2d_utils/include/mapbox/LICENSE
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/mapbox/LICENSE
rename to src/Libraries/robot_nav_2d_utils/include/mapbox/LICENSE
diff --git a/src/Libraries/nav_2d_utils/include/mapbox/NOTES b/src/Libraries/robot_nav_2d_utils/include/mapbox/NOTES
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/mapbox/NOTES
rename to src/Libraries/robot_nav_2d_utils/include/mapbox/NOTES
diff --git a/src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp b/src/Libraries/robot_nav_2d_utils/include/mapbox/earcut.hpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp
rename to src/Libraries/robot_nav_2d_utils/include/mapbox/earcut.hpp
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/bounds.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/bounds.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/conversions.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
similarity index 66%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/conversions.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
index 17d1d1b..e3932fb 100755
--- a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/conversions.h
+++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
@@ -35,10 +35,10 @@
 #ifndef ROBOT_NAV_2D_UTILS_CONVERSIONS_H
 #define ROBOT_NAV_2D_UTILS_CONVERSIONS_H
 
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/PolygonStamped.h>
+#include <robot_geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Pose.h>
+#include <robot_geometry_msgs/Twist.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Point2D.h>
@@ -58,42 +58,42 @@
 namespace robot_nav_2d_utils
 {
   // Twist Transformations
-  ::geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
-  ::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::geometry_msgs::Twist &cmd_vel);
+  ::robot_geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
+  ::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::robot_geometry_msgs::Twist &cmd_vel);
 
   // Point Transformations
-  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point &point);
-  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point32 &point);
-  ::geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
-  ::geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
+  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point &point);
+  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point32 &point);
+  ::robot_geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
+  ::robot_geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
 
   // Pose Transformations
   // ::robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose);
-  ::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::geometry_msgs::PoseStamped &pose);
-  ::geometry_msgs::Pose pose2DToPose(const ::geometry_msgs::Pose2D &pose2d);
-  ::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
-  ::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::geometry_msgs::Pose2D &pose2d,
+  ::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::robot_geometry_msgs::PoseStamped &pose);
+  ::robot_geometry_msgs::Pose pose2DToPose(const ::robot_geometry_msgs::Pose2D &pose2d);
+  ::robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
+  ::robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_geometry_msgs::Pose2D &pose2d,
                                                    const ::std::string &frame, const robot::Time &stamp);
 
   // Path Transformations
   ::robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path);
-  nav_msgs::Path posesToPath(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
-  ::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
-  nav_msgs::Path poses2DToPath(const ::std::vector<::geometry_msgs::Pose2D> &poses,
+  nav_msgs::Path posesToPath(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
+  ::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
+  nav_msgs::Path poses2DToPath(const ::std::vector<::robot_geometry_msgs::Pose2D> &poses,
                                const ::std::string &frame, const robot::Time &stamp);
   nav_msgs::Path pathToPath(const ::robot_nav_2d_msgs::Path2D &path2d);
 
   // Polygon Transformations
-  ::geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
-  ::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::geometry_msgs::Polygon &polygon_3d);
-  ::geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
-  ::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::geometry_msgs::PolygonStamped &polygon_3d);
+  ::robot_geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
+  ::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::robot_geometry_msgs::Polygon &polygon_3d);
+  ::robot_geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
+  ::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::robot_geometry_msgs::PolygonStamped &polygon_3d);
 
   // Info Transformations
   ::robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info);
   nav_grid::NavGridInfo fromMsg(const ::robot_nav_2d_msgs::NavGridInfo &msg);
-  ::geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
-  ::geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info);
+  ::robot_geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
+  ::robot_geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info);
   nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const ::std::string &frame);
   nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
 
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/footprint.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/footprint.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/parameters.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/parameters.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/path_ops.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
similarity index 95%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
index a385587..85e5111 100755
--- a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
+++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
@@ -42,7 +42,7 @@ namespace robot_nav_2d_utils
   /**
    * @brief Calculate the linear distance between two poses
    */
-  double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1);
+  double poseDistance(const robot_geometry_msgs::Pose2D &pose0, const robot_geometry_msgs::Pose2D &pose1);
 
   /**
    * @brief Calculate the length of the plan, starting from the given index
@@ -52,7 +52,7 @@ namespace robot_nav_2d_utils
   /**
    * @brief Calculate the length of the plan from the pose on the plan closest to the given pose
    */
-  double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose);
+  double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const robot_geometry_msgs::Pose2D &query_pose);
 
   /**
    * @brief Increase plan resolution to match that of the costmap by adding points linearly between points
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h
similarity index 100%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/polygons.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
similarity index 98%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/polygons.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
index b44eb8d..597178f 100755
--- a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/polygons.h
+++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
@@ -37,7 +37,7 @@
 
 #include <robot_nav_2d_msgs/Polygon2D.h>
 #include <robot_nav_2d_msgs/ComplexPolygon2D.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 #include <robot_xmlrpcpp/XmlRpcValue.h>
 #include <vector>
 #include <string>
@@ -148,7 +148,7 @@ bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msg
  * @return A new moved polygon
  */
 robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon,
-                                         const geometry_msgs::Pose2D& pose);
+                                         const robot_geometry_msgs::Pose2D& pose);
 
 /**
  * @brief Check if a given point is inside of a polygon
diff --git a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/tf_help.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
similarity index 93%
rename from src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
index 8f01839..369dd7e 100755
--- a/src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
+++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
@@ -36,7 +36,7 @@
 #define ROBOT_NAV_2D_UTILS_TF_HELP_H
 
 #include <nav_core2/common.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <string>
 
@@ -54,7 +54,7 @@ namespace robot_nav_2d_utils
    * @return True if successful transform
    */
   bool transformPose(const TFListenerPtr tf, const ::std::string frame,
-                     const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose,
+                     const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
                      const bool extrapolation_fallback = true);
 
   /**
@@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
    * @param frame_id Frame to transform the pose into
    * @return The resulting transformed pose
    */
-  geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
+  robot_geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
                                              const ::std::string &frame_id);
 
 } // namespace robot_nav_2d_utils
diff --git a/src/Libraries/nav_2d_utils/package.xml b/src/Libraries/robot_nav_2d_utils/package.xml
similarity index 100%
rename from src/Libraries/nav_2d_utils/package.xml
rename to src/Libraries/robot_nav_2d_utils/package.xml
diff --git a/src/Libraries/nav_2d_utils/setup.py b/src/Libraries/robot_nav_2d_utils/setup.py
similarity index 100%
rename from src/Libraries/nav_2d_utils/setup.py
rename to src/Libraries/robot_nav_2d_utils/setup.py
diff --git a/src/Libraries/nav_2d_utils/src/bounds.cpp b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/src/bounds.cpp
rename to src/Libraries/robot_nav_2d_utils/src/bounds.cpp
diff --git a/src/Libraries/nav_2d_utils/src/conversions.cpp b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp
similarity index 80%
rename from src/Libraries/nav_2d_utils/src/conversions.cpp
rename to src/Libraries/robot_nav_2d_utils/src/conversions.cpp
index be735cc..52c7d86 100755
--- a/src/Libraries/nav_2d_utils/src/conversions.cpp
+++ b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp
@@ -38,16 +38,16 @@
 namespace robot_nav_2d_utils
 {
 
-  geometry_msgs::Twist twist2Dto3D(const robot_nav_2d_msgs::Twist2D &cmd_vel_2d)
+  robot_geometry_msgs::Twist twist2Dto3D(const robot_nav_2d_msgs::Twist2D &cmd_vel_2d)
   {
-    geometry_msgs::Twist cmd_vel;
+    robot_geometry_msgs::Twist cmd_vel;
     cmd_vel.linear.x = cmd_vel_2d.x;
     cmd_vel.linear.y = cmd_vel_2d.y;
     cmd_vel.angular.z = cmd_vel_2d.theta;
     return cmd_vel;
   }
 
-  robot_nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist &cmd_vel)
+  robot_nav_2d_msgs::Twist2D twist3Dto2D(const robot_geometry_msgs::Twist &cmd_vel)
   {
     robot_nav_2d_msgs::Twist2D cmd_vel_2d;
     cmd_vel_2d.x = cmd_vel.linear.x;
@@ -56,7 +56,7 @@ namespace robot_nav_2d_utils
     return cmd_vel_2d;
   }
 
-  robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point &point)
+  robot_nav_2d_msgs::Point2D pointToPoint2D(const robot_geometry_msgs::Point &point)
   {
     robot_nav_2d_msgs::Point2D output;
     output.x = point.x;
@@ -64,7 +64,7 @@ namespace robot_nav_2d_utils
     return output;
   }
 
-  robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32 &point)
+  robot_nav_2d_msgs::Point2D pointToPoint2D(const robot_geometry_msgs::Point32 &point)
   {
     robot_nav_2d_msgs::Point2D output;
     output.x = point.x;
@@ -72,17 +72,17 @@ namespace robot_nav_2d_utils
     return output;
   }
 
-  geometry_msgs::Point pointToPoint3D(const robot_nav_2d_msgs::Point2D &point)
+  robot_geometry_msgs::Point pointToPoint3D(const robot_nav_2d_msgs::Point2D &point)
   {
-    geometry_msgs::Point output;
+    robot_geometry_msgs::Point output;
     output.x = point.x;
     output.y = point.y;
     return output;
   }
 
-  geometry_msgs::Point32 pointToPoint32(const robot_nav_2d_msgs::Point2D &point)
+  robot_geometry_msgs::Point32 pointToPoint32(const robot_nav_2d_msgs::Point2D &point)
   {
-    geometry_msgs::Point32 output;
+    robot_geometry_msgs::Point32 output;
     output.x = point.x;
     output.y = point.y;
     return output;
@@ -99,7 +99,7 @@ namespace robot_nav_2d_utils
   //   return pose2d;
   // }
 
-  robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose)
+  robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const robot_geometry_msgs::PoseStamped &pose)
   {
     robot_nav_2d_msgs::Pose2DStamped pose2d;
     pose2d.header = pose.header;
@@ -109,27 +109,27 @@ namespace robot_nav_2d_utils
     return pose2d;
   }
 
-  geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D &pose2d)
+  robot_geometry_msgs::Pose pose2DToPose(const robot_geometry_msgs::Pose2D &pose2d)
   {
-    geometry_msgs::Pose pose;
+    robot_geometry_msgs::Pose pose;
     pose.position.x = pose2d.x;
     pose.position.y = pose2d.y;
     // pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
     return pose;
   }
 
-  geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_nav_2d_msgs::Pose2DStamped &pose2d)
+  robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_nav_2d_msgs::Pose2DStamped &pose2d)
   {
-    geometry_msgs::PoseStamped pose;
+    robot_geometry_msgs::PoseStamped pose;
     pose.header = pose2d.header;
     pose.pose = pose2DToPose(pose2d.pose);
     return pose;
   }
 
-  // geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
+  // robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_geometry_msgs::Pose2D& pose2d,
   //                                                const std::string& frame, const robot::Time& stamp)
   // {
-  //   geometry_msgs::PoseStamped pose;
+  //   robot_geometry_msgs::PoseStamped pose;
   //   pose.header.frame_id = frame;
   //   pose.header.stamp = stamp;
   //   pose.pose.position.x = pose2d.x;
@@ -152,7 +152,7 @@ namespace robot_nav_2d_utils
     return path2d;
   }
 
-  nav_msgs::Path posesToPath(const std::vector<geometry_msgs::PoseStamped> &poses)
+  nav_msgs::Path posesToPath(const std::vector<robot_geometry_msgs::PoseStamped> &poses)
   {
     nav_msgs::Path path;
     if (poses.empty())
@@ -168,7 +168,7 @@ namespace robot_nav_2d_utils
     return path;
   }
 
-  robot_nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped> &poses)
+  robot_nav_2d_msgs::Path2D posesToPath2D(const std::vector<robot_geometry_msgs::PoseStamped> &poses)
   {
     robot_nav_2d_msgs::Path2D path;
     if (poses.empty())
@@ -186,7 +186,7 @@ namespace robot_nav_2d_utils
     return path;
   }
 
-  // nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
+  // nav_msgs::Path poses2DToPath(const std::vector<robot_geometry_msgs::Pose2D>& poses,
   //                              const std::string& frame, const robot::Time& stamp)
   // {
   //   nav_msgs::Path path;
@@ -213,9 +213,9 @@ namespace robot_nav_2d_utils
     return path;
   }
 
-  geometry_msgs::Polygon polygon2Dto3D(const robot_nav_2d_msgs::Polygon2D &polygon_2d)
+  robot_geometry_msgs::Polygon polygon2Dto3D(const robot_nav_2d_msgs::Polygon2D &polygon_2d)
   {
-    geometry_msgs::Polygon polygon;
+    robot_geometry_msgs::Polygon polygon;
     polygon.points.reserve(polygon_2d.points.size());
     for (const auto &pt : polygon_2d.points)
     {
@@ -224,7 +224,7 @@ namespace robot_nav_2d_utils
     return polygon;
   }
 
-  robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon &polygon_3d)
+  robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const robot_geometry_msgs::Polygon &polygon_3d)
   {
     robot_nav_2d_msgs::Polygon2D polygon;
     polygon.points.reserve(polygon_3d.points.size());
@@ -235,15 +235,15 @@ namespace robot_nav_2d_utils
     return polygon;
   }
 
-  geometry_msgs::PolygonStamped polygon2Dto3D(const robot_nav_2d_msgs::Polygon2DStamped &polygon_2d)
+  robot_geometry_msgs::PolygonStamped polygon2Dto3D(const robot_nav_2d_msgs::Polygon2DStamped &polygon_2d)
   {
-    geometry_msgs::PolygonStamped polygon;
+    robot_geometry_msgs::PolygonStamped polygon;
     polygon.header = polygon_2d.header;
     polygon.polygon = polygon2Dto3D(polygon_2d.polygon);
     return polygon;
   }
 
-  robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped &polygon_3d)
+  robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const robot_geometry_msgs::PolygonStamped &polygon_3d)
   {
     robot_nav_2d_msgs::Polygon2DStamped polygon;
     polygon.header = polygon_3d.header;
@@ -291,18 +291,18 @@ namespace robot_nav_2d_utils
     return info;
   }
 
-  geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info)
+  robot_geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info)
   {
-    geometry_msgs::Pose origin;
+    robot_geometry_msgs::Pose origin;
     origin.position.x = info.origin_x;
     origin.position.y = info.origin_y;
     origin.orientation.w = 1.0;
     return origin;
   }
 
-  geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info)
+  robot_geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info)
   {
-    geometry_msgs::Pose2D origin;
+    robot_geometry_msgs::Pose2D origin;
     origin.x = info.origin_x;
     origin.y = info.origin_y;
     return origin;
diff --git a/src/Libraries/nav_2d_utils/src/footprint.cpp b/src/Libraries/robot_nav_2d_utils/src/footprint.cpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/src/footprint.cpp
rename to src/Libraries/robot_nav_2d_utils/src/footprint.cpp
diff --git a/src/Libraries/nav_2d_utils/src/path_ops.cpp b/src/Libraries/robot_nav_2d_utils/src/path_ops.cpp
similarity index 96%
rename from src/Libraries/nav_2d_utils/src/path_ops.cpp
rename to src/Libraries/robot_nav_2d_utils/src/path_ops.cpp
index accd82c..6bb7a25 100755
--- a/src/Libraries/nav_2d_utils/src/path_ops.cpp
+++ b/src/Libraries/robot_nav_2d_utils/src/path_ops.cpp
@@ -40,7 +40,7 @@
 namespace robot_nav_2d_utils
 {
 
-  double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
+  double poseDistance(const robot_geometry_msgs::Pose2D &pose0, const robot_geometry_msgs::Pose2D &pose1)
   {
     return hypot(pose0.x - pose1.x, pose0.y - pose1.y);
   }
@@ -55,7 +55,7 @@ namespace robot_nav_2d_utils
     return length;
   }
 
-  double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose)
+  double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const robot_geometry_msgs::Pose2D &query_pose)
   {
     if (plan.poses.size() == 0)
       return 0.0;
@@ -87,10 +87,10 @@ namespace robot_nav_2d_utils
     global_plan_out.poses.push_back(last_stp);
 
     double sq_resolution = resolution * resolution;
-    geometry_msgs::Pose2D last = last_stp.pose;
+    robot_geometry_msgs::Pose2D last = last_stp.pose;
     for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i)
     {
-      geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose;
+      robot_geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose;
       double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y);
       if (sq_dist > sq_resolution)
       {
diff --git a/src/Libraries/nav_2d_utils/src/polygons.cpp b/src/Libraries/robot_nav_2d_utils/src/polygons.cpp
similarity index 99%
rename from src/Libraries/nav_2d_utils/src/polygons.cpp
rename to src/Libraries/robot_nav_2d_utils/src/polygons.cpp
index 3ec1174..eb4d98b 100755
--- a/src/Libraries/nav_2d_utils/src/polygons.cpp
+++ b/src/Libraries/robot_nav_2d_utils/src/polygons.cpp
@@ -365,7 +365,7 @@ bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msg
 }
 
 robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon,
-                                         const geometry_msgs::Pose2D& pose)
+                                         const robot_geometry_msgs::Pose2D& pose)
 {
   robot_nav_2d_msgs::Polygon2D new_polygon;
   new_polygon.points.resize(polygon.points.size());
diff --git a/src/Libraries/nav_2d_utils/src/tf_help.cpp b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
similarity index 89%
rename from src/Libraries/nav_2d_utils/src/tf_help.cpp
rename to src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
index 00254cb..b86170b 100755
--- a/src/Libraries/nav_2d_utils/src/tf_help.cpp
+++ b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
@@ -42,7 +42,7 @@
 namespace robot_nav_2d_utils
 {
   bool transformPose(const TFListenerPtr tf, const ::std::string frame,
-                     const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose,
+                     const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
                      const bool extrapolation_fallback)
   {
     // if (in_pose.header.frame_id == frame)
@@ -60,7 +60,7 @@ namespace robot_nav_2d_utils
     // {
     //   if (!extrapolation_fallback)
     //     throw;
-    //   geometry_msgs::PoseStamped latest_in_pose;
+    //   robot_geometry_msgs::PoseStamped latest_in_pose;
     //   latest_in_pose.header.frame_id = in_pose.header.frame_id;
     //   latest_in_pose.pose = in_pose.pose;
     //   tf->transform(latest_in_pose, out_pose, frame);
@@ -78,8 +78,8 @@ namespace robot_nav_2d_utils
                      const ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_nav_2d_msgs::Pose2DStamped &out_pose,
                      const bool extrapolation_fallback)
   {
-    geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
-    geometry_msgs::PoseStamped out_3d_pose;
+    robot_geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
+    robot_geometry_msgs::PoseStamped out_3d_pose;
 
     bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, extrapolation_fallback);
     if (ret)
@@ -89,7 +89,7 @@ namespace robot_nav_2d_utils
     return ret;
   }
 
-  ::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
+  ::robot_geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
                                              const ::std::string &frame_id)
   {
     robot_nav_2d_msgs::Pose2DStamped local_pose;
diff --git a/src/Libraries/nav_2d_utils/test/bounds_test.cpp b/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/test/bounds_test.cpp
rename to src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp
diff --git a/src/Libraries/nav_2d_utils/test/compress_test.cpp b/src/Libraries/robot_nav_2d_utils/test/compress_test.cpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/test/compress_test.cpp
rename to src/Libraries/robot_nav_2d_utils/test/compress_test.cpp
diff --git a/src/Libraries/nav_2d_utils/test/param_tests.cpp b/src/Libraries/robot_nav_2d_utils/test/param_tests.cpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/test/param_tests.cpp
rename to src/Libraries/robot_nav_2d_utils/test/param_tests.cpp
diff --git a/src/Libraries/nav_2d_utils/test/param_tests.launch b/src/Libraries/robot_nav_2d_utils/test/param_tests.launch
similarity index 100%
rename from src/Libraries/nav_2d_utils/test/param_tests.launch
rename to src/Libraries/robot_nav_2d_utils/test/param_tests.launch
diff --git a/src/Libraries/nav_2d_utils/test/polygon_tests.cpp b/src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp
similarity index 99%
rename from src/Libraries/nav_2d_utils/test/polygon_tests.cpp
rename to src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp
index b3bcf5a..bdd6b93 100755
--- a/src/Libraries/nav_2d_utils/test/polygon_tests.cpp
+++ b/src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp
@@ -148,7 +148,7 @@ TEST(Polygon2D, broken_arrays)
 TEST(Polygon2D, test_move)
 {
   Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
-  geometry_msgs::Pose2D pose;
+  robot_geometry_msgs::Pose2D pose;
   Polygon2D square2 = robot_nav_2d_utils::movePolygonToPose(square, pose);
   EXPECT_TRUE(robot_nav_2d_utils::equals(square, square2));
   pose.x = 15;
diff --git a/src/Libraries/nav_2d_utils/test/resolution_test.cpp b/src/Libraries/robot_nav_2d_utils/test/resolution_test.cpp
similarity index 100%
rename from src/Libraries/nav_2d_utils/test/resolution_test.cpp
rename to src/Libraries/robot_nav_2d_utils/test/resolution_test.cpp
diff --git a/src/Libraries/tf3/CHANGELOG.rst b/src/Libraries/tf3/CHANGELOG.rst
index fa0c4ba..139c4eb 100644
--- a/src/Libraries/tf3/CHANGELOG.rst
+++ b/src/Libraries/tf3/CHANGELOG.rst
@@ -489,7 +489,7 @@ Changelog for package tf2
 * more compatability
 * bringing in helper functions for buffer_core from tf.h/cpp
 * rethrowing to new exceptions
-* converting Storage to geometry_msgs::TransformStamped
+* converting Storage to robot_geometry_msgs::TransformStamped
 * removing deprecated useage
 * cleaning up includes
 * moving all implementations into cpp file
diff --git a/src/Libraries/tf3/include/tf3/buffer_core.h b/src/Libraries/tf3/include/tf3/buffer_core.h
index ca8777e..75f0fc6 100644
--- a/src/Libraries/tf3/include/tf3/buffer_core.h
+++ b/src/Libraries/tf3/include/tf3/buffer_core.h
@@ -163,7 +163,7 @@ public:
    * New in geometry 1.1
    */
   /*
-  geometry_msgs::Twist
+  robot_geometry_msgs::Twist
     lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
 		const tf::Point & reference_point, const std::string& reference_point_frame, 
 		const robot::Time& time, const robot::Duration& averaging_interval) const;
@@ -181,7 +181,7 @@ public:
    * New in geometry 1.1
    */
   /*
-  geometry_msgs::Twist
+  robot_geometry_msgs::Twist
     lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, 
 		const robot::Time& time, const robot::Duration& averaging_interval) const;
   */
diff --git a/src/Libraries/tf3/include/tf3/compat.h b/src/Libraries/tf3/include/tf3/compat.h
index bf6ab63..d4cabb2 100644
--- a/src/Libraries/tf3/include/tf3/compat.h
+++ b/src/Libraries/tf3/include/tf3/compat.h
@@ -1,4 +1,4 @@
-// Compatibility types to avoid using robot:: or geometry_msgs:: namespaces inside tf3
+// Compatibility types to avoid using robot:: or robot_geometry_msgs:: namespaces inside tf3
 #ifndef TF3_COMPAT_H
 #define TF3_COMPAT_H
 
@@ -7,7 +7,7 @@
 
 namespace tf3 {
 
-// Minimal header/message equivalents owned by tf3 (no robot:: or geometry_msgs::)
+// Minimal header/message equivalents owned by tf3 (no robot:: or robot_geometry_msgs::)
 struct HeaderMsg
 {
   uint32_t seq;
diff --git a/src/Libraries/tf3/include/tf3/impl/utils.h b/src/Libraries/tf3/include/tf3/impl/utils.h
index 8411f92..65f959f 100644
--- a/src/Libraries/tf3/include/tf3/impl/utils.h
+++ b/src/Libraries/tf3/include/tf3/impl/utils.h
@@ -53,7 +53,7 @@ template<typename T>
   }
 
 /** Generic version of toQuaternion. It tries to convert the argument
- * to a geometry_msgs::Quaternion
+ * to a robot_geometry_msgs::Quaternion
  * \param t some object
  * \return a copy of the same quaternion as a tf3::Quaternion
  */
diff --git a/src/Libraries/tf3/src/buffer_core.cpp b/src/Libraries/tf3/src/buffer_core.cpp
index fe38f4c..ff7ae1b 100644
--- a/src/Libraries/tf3/src/buffer_core.cpp
+++ b/src/Libraries/tf3/src/buffer_core.cpp
@@ -672,14 +672,14 @@ tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_f
 
 
 /*
-geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
+robot_geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
                                           const std::string& observation_frame, 
                                           const robot::Time& time, 
                                           const robot::Duration& averaging_interval) const
 {
   try
   {
-  geometry_msgs::Twist t;
+  robot_geometry_msgs::Twist t;
   old_tf_.lookupTwist(tracking_frame, observation_frame, 
                       time, averaging_interval, t);
   return t;
@@ -702,7 +702,7 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
   }
 }
 
-geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
+robot_geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame, 
                                           const std::string& observation_frame, 
                                           const std::string& reference_frame,
                                           const tf3::Point & reference_point, 
@@ -711,7 +711,7 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
                                           const robot::Duration& averaging_interval) const
 {
   try{
-  geometry_msgs::Twist t;
+  robot_geometry_msgs::Twist t;
   old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame,
                       time, averaging_interval, t);
   return t;
diff --git a/src/Libraries/tf3/test/cache_unittest.cpp b/src/Libraries/tf3/test/cache_unittest.cpp
index 98dcd46..c3891b2 100644
--- a/src/Libraries/tf3/test/cache_unittest.cpp
+++ b/src/Libraries/tf3/test/cache_unittest.cpp
@@ -32,7 +32,7 @@
 #include "tf3/LinearMath/Quaternion.h"
 #include <stdexcept>
 
-#include <geometry_msgs/TransformStamped.h>
+#include <robot_geometry_msgs/TransformStamped.h>
 
 #include <cmath>
 
diff --git a/src/Libraries/tf3/test/simple_tf3_core.cpp b/src/Libraries/tf3/test/simple_tf3_core.cpp
index fbe511e..08085a5 100644
--- a/src/Libraries/tf3/test/simple_tf3_core.cpp
+++ b/src/Libraries/tf3/test/simple_tf3_core.cpp
@@ -36,7 +36,7 @@
 TEST(tf3, setTransformFail)
 {
   tf3::BufferCore tfc;
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   EXPECT_FALSE(tfc.setTransform(st, "authority1"));
 
 }
@@ -44,7 +44,7 @@ TEST(tf3, setTransformFail)
 TEST(tf3, setTransformValid)
 {
   tf3::BufferCore tfc;
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
   st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
@@ -56,7 +56,7 @@ TEST(tf3, setTransformValid)
 TEST(tf3, setTransformInvalidQuaternion)
 {
   tf3::BufferCore tfc;
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
   st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
@@ -86,7 +86,7 @@ TEST(tf3_canTransform, Nothing_Exists)
 TEST(tf3_lookupTransform, LookupException_One_Exists)
 {
   tf3::BufferCore tfc;
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
   st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
@@ -99,7 +99,7 @@ TEST(tf3_lookupTransform, LookupException_One_Exists)
 TEST(tf3_canTransform, One_Exists)
 {
   tf3::BufferCore tfc;
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.frame_id = "foo";
   st.header.stamp = robot::Time(1.0);
   st.child_frame_id = "child";
@@ -113,7 +113,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
   tf3::BufferCore mBC;
   tf3::TestBufferCore tBC;
 
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.stamp = robot::Time(0);
   st.transform.rotation.w = 1;
 
@@ -196,7 +196,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
   tf3::BufferCore mBC;
   tf3::TestBufferCore tBC;
 
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.stamp = robot::Time(0);
   st.transform.rotation.w = 1;
 
@@ -243,7 +243,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
   tf3::BufferCore mBC;
   tf3::TestBufferCore tBC;
 
-  geometry_msgs::TransformStamped st;
+  robot_geometry_msgs::TransformStamped st;
   st.header.stamp = robot::Time(0);
   st.transform.rotation.w = 1;
 
diff --git a/src/Libraries/tf3/test/speed_test.cpp b/src/Libraries/tf3/test/speed_test.cpp
index 597d9d5..6adc118 100644
--- a/src/Libraries/tf3/test/speed_test.cpp
+++ b/src/Libraries/tf3/test/speed_test.cpp
@@ -50,7 +50,7 @@ int main(int argc, char** argv)
   console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
 
   tf3::BufferCore bc;
-  geometry_msgs::TransformStamped t;
+  robot_geometry_msgs::TransformStamped t;
   t.header.stamp = robot::Time(1);
   t.header.frame_id = "root";
   t.child_frame_id = "0";
@@ -106,7 +106,7 @@ int main(int argc, char** argv)
   std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
   std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
   CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str());
-  geometry_msgs::TransformStamped out_t;
+  robot_geometry_msgs::TransformStamped out_t;
 
   const uint32_t count = 1000000;
   CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);
diff --git a/src/Libraries/tf3/test/static_cache_test.cpp b/src/Libraries/tf3/test/static_cache_test.cpp
index eeb27d7..a2498eb 100644
--- a/src/Libraries/tf3/test/static_cache_test.cpp
+++ b/src/Libraries/tf3/test/static_cache_test.cpp
@@ -31,7 +31,7 @@
 #include <tf3/time_cache.h>
 #include <stdexcept>
 
-#include <geometry_msgs/TransformStamped.h>
+#include <robot_geometry_msgs/TransformStamped.h>
 
 #include <cmath>
 
diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
index 008a62f..2b6a97e 100755
--- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
+++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
@@ -4,10 +4,10 @@
 #include <memory>
 
 // geometry msgs
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/Vector3.h>
+#include <robot_geometry_msgs/Twist.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Vector3.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
 
 // robot protocol msgs
@@ -49,7 +49,7 @@ namespace move_base_core
 	{
 		State navigation_state;							// Current navigation state
 		std::string feed_back_str;					// Description or debug message
-		geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
+		robot_geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
 		bool goal_checked;									// Whether the current goal is verified
 		bool is_ready;											// Robot is ready for commands
 	};
@@ -111,9 +111,9 @@ namespace move_base_core
 	 *                         Positive moves forward, negative moves backward.
 	 * @return                 A new PoseStamped offset from the input pose, in the given frame.
 	 */
-	inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
+	inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
 	{
-		geometry_msgs::PoseStamped goal;
+		robot_geometry_msgs::PoseStamped goal;
 		// goal.header.frame_id = frame_id;
 		// goal.header.stamp = robot::Time::now();
 		// goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
@@ -136,9 +136,9 @@ namespace move_base_core
 	 * @param offset_distance  Distance to offset along the current heading direction (in meters).
 	 * @return                 A new PoseStamped offset from the original pose.
 	 */
-	inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::PoseStamped &pose, double offset_distance)
+	inline robot_geometry_msgs::PoseStamped offset_goal(const robot_geometry_msgs::PoseStamped &pose, double offset_distance)
 	{
-		geometry_msgs::Pose2D pose2d;
+		robot_geometry_msgs::Pose2D pose2d;
 		pose2d.x = pose.pose.position.x;
 		pose2d.y = pose.pose.position.y;
 		// pose2d.theta = tf2::getYaw(pose.pose.orientation);
@@ -184,13 +184,13 @@ namespace move_base_core
 			|      P4(-0.3, -0.2)       P1(0.3, -0.2)
 			+-------------------------------> X
 
-			std::vector<geometry_msgs::Point> footprint;
+			std::vector<robot_geometry_msgs::Point> footprint;
 			1. footprint.push_back(make_point(0.3, -0.2));
 			2. footprint.push_back(make_point(0.3, 0.2));
 			3. footprint.push_back(make_point(-0.3, 0.2));
 			4. footprint.push_back(make_point(-0.3, -0.2));
 		 */
-		virtual void setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt) = 0;
+		virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) = 0;
 
 		/**
 		 * @brief Send a goal for the robot to navigate to.
@@ -199,7 +199,7 @@ namespace move_base_core
 		 * @param yaw_goal_tolerance Acceptable angular error (radians).
 		 * @return True if goal was accepted and sent successfully.
 		 */
-		virtual bool moveTo(const geometry_msgs::PoseStamped &goal,
+		virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
 												double xy_goal_tolerance = 0.0,
 												double yaw_goal_tolerance = 0.0) = 0;
 
@@ -212,7 +212,7 @@ namespace move_base_core
 		 * @return True if goal was accepted and sent successfully.
 		 */
 		virtual bool moveTo(const robot_protocol_msgs::Order &msg,
-												const geometry_msgs::PoseStamped &goal,
+												const robot_geometry_msgs::PoseStamped &goal,
 												double xy_goal_tolerance = 0.0,
 												double yaw_goal_tolerance = 0.0) = 0;
 
@@ -225,7 +225,7 @@ namespace move_base_core
 		 * @return True if docking command succeeded.
 		 */
 		virtual bool dockTo(const std::string &maker,
-												const geometry_msgs::PoseStamped &goal,
+												const robot_geometry_msgs::PoseStamped &goal,
 												double xy_goal_tolerance = 0.0,
 												double yaw_goal_tolerance = 0.0) = 0;
 
@@ -238,7 +238,7 @@ namespace move_base_core
 		 * @return True if docking command succeeded.
 		 */
 		virtual bool dockTo(const robot_protocol_msgs::Order &msg,
-												const geometry_msgs::PoseStamped &goal,
+												const robot_geometry_msgs::PoseStamped &goal,
 												double xy_goal_tolerance = 0.0,
 												double yaw_goal_tolerance = 0.0) = 0;
 
@@ -248,7 +248,7 @@ namespace move_base_core
 		 * @param xy_goal_tolerance Acceptable positional error (meters).
 		 * @return True if command issued successfully.
 		 */
-		virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal,
+		virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
 																double xy_goal_tolerance = 0.0) = 0;
 
 		/**
@@ -257,7 +257,7 @@ namespace move_base_core
 		 * @param yaw_goal_tolerance Acceptable angular error (radians).
 		 * @return True if rotation command was sent successfully.
 		 */
-		virtual bool rotateTo(const geometry_msgs::PoseStamped &goal,
+		virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
 													double yaw_goal_tolerance = 0.0) = 0;
 
 		/**
@@ -280,28 +280,28 @@ namespace move_base_core
 		 * @param linear Linear velocity vector (x, y, z).
 		 * @return True if the command was accepted.
 		 */
-		virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) = 0;
+		virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) = 0;
 
 		/**
 		 * @brief Send limited angular velocity command.
 		 * @param angular Angular velocity vector (x, y, z).
 		 * @return True if the command was accepted.
 		 */
-		virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) = 0;
+		virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) = 0;
 
 		/**
 		 * @brief Get the robot’s pose as a PoseStamped.
 		 * @param pose Output parameter with the robot’s current pose.
 		 * @return True if pose was successfully retrieved.
 		 */
-		virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) = 0;
+		virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) = 0;
 
 		/**
 		 * @brief Get the robot’s pose as a 2D pose.
 		 * @param pose Output parameter with the robot’s current 2D pose.
 		 * @return True if pose was successfully retrieved.
 		 */
-		virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
+		virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) = 0;
 
 		/**
 		 * @brief Get the robot’s twist.
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 c5e0db7..0bdfe84 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
@@ -37,7 +37,7 @@
 #ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H
 #define NAV_CORE_BASE_GLOBAL_PLANNER_H
 
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <costmap_2d/costmap_2d_robot.h>
 #include <robot_protocol_msgs/Order.h>
 #include <memory>
@@ -58,8 +58,8 @@ namespace nav_core {
        * @param plan The plan... filled by the planner
        * @return True if a valid plan was found, false otherwise
        */
-      virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
-          const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;
+      virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
+          const robot_geometry_msgs::PoseStamped& goal, std::vector<robot_geometry_msgs::PoseStamped>& plan) = 0;
 
       /**
        * @brief Given a goal pose in the world, compute a plan
@@ -69,8 +69,8 @@ namespace nav_core {
        * @param cost The plans calculated cost
        * @return True if a valid plan was found, false otherwise
        */
-      virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
-                            const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,
+      virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
+                            const robot_geometry_msgs::PoseStamped& goal, std::vector<robot_geometry_msgs::PoseStamped>& plan,
                             double& cost)
       {
         cost = 0;
@@ -85,9 +85,9 @@ namespace nav_core {
        * @return True if a valid plan was found, false otherwise
        */
        virtual bool makePlan(const robot_protocol_msgs::Order& msg,
-        const geometry_msgs::PoseStamped& start, 
-        const geometry_msgs::PoseStamped& goal, 
-        std::vector<geometry_msgs::PoseStamped>& plan)
+        const robot_geometry_msgs::PoseStamped& start, 
+        const robot_geometry_msgs::PoseStamped& goal, 
+        std::vector<robot_geometry_msgs::PoseStamped>& plan)
         {
           return false;
         }
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 ae1319b..de73009 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
@@ -37,8 +37,8 @@
 #ifndef NAV_CORE_BASE_LOCAL_PLANNER_H
 #define NAV_CORE_BASE_LOCAL_PLANNER_H
 
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Twist.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Twist.h>
 #include <costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
 #include <memory>
@@ -58,7 +58,7 @@ namespace nav_core
      * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
      * @return True if a valid velocity command was found, false otherwise
      */
-    virtual bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) = 0;
+    virtual bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) = 0;
 
     /**
      * @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
@@ -72,34 +72,34 @@ namespace nav_core
      * @param linear the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return false; }
+    virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) { return false; }
 
     /**
      * @brief Get velocity for x, y asix of the robot
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
+    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
 
     /**
      * @brief Set velocity theta for z asix of the robot
      * @param angular the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; }
+    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) { return false; }
 
     /**
      * @brief Get velocity theta for z asix of the robot
      * @param direct The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
+    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
 
     /**
      * @brief Get velocity for x, y asix of the robot
      * @return The current velocity
      */
-    virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
+    virtual robot_geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
 
     /**
      * @brief Check if the kinematic parameters are currently locked
@@ -130,7 +130,7 @@ namespace nav_core
      * @param plan The plan to pass to the local planner
      * @return True if the plan was updated successfully, false otherwise
      */
-    virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
+    virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) = 0;
 
     /**
      * @brief  Constructs the local planner
diff --git a/src/Navigations/Cores/nav_core2/README.md b/src/Navigations/Cores/nav_core2/README.md
index b5fe550..eada1a6 100755
--- a/src/Navigations/Cores/nav_core2/README.md
+++ b/src/Navigations/Cores/nav_core2/README.md
@@ -30,7 +30,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|
-|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<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.|
+|`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
 Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
@@ -39,8 +39,8 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 |---|--|---|
 |`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|
 |(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<geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
-|`bool computeVelocityCommands(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.|
+|`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.|
 |`bool isGoalReached()` | `bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
 
 ## Exceptions
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 a58289c..910af1a 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
@@ -40,7 +40,7 @@
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
-#include <geometry_msgs/Twist.h>
+#include <robot_geometry_msgs/Twist.h>
 #include <string>
 #include <costmap_2d/costmap_2d_robot.h>
 #include <memory>
@@ -110,34 +110,34 @@ namespace nav_core2
      * @param linear the velocity command
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return true; }
+    virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) { return true; }
 
     /**
      * @brief get velocity for x, y asix of the robot
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
+    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
 
     /**
      * @brief Set velocity theta for z asix of the robot
      * @param angular the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; }
+    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) { return false; }
 
     /**
      * @brief Get velocity theta for z asix of the robot
      * @param direct The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
+    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
 
     /**
      * @brief Get velocity for x, y asix of the robot
      * @return The current velocity
      */
-    virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
+    virtual robot_geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
 
     /**
      * @brief Check if the kinematic parameters are currently locked
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 da40894..9fdabf4 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
@@ -84,7 +84,7 @@ namespace nav_core_adapter
      * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
      * @return True if a valid velocity command was found, false otherwise
      */
-    bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) override;
+    bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) override;
 
     /**
      * @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
@@ -98,34 +98,34 @@ namespace nav_core_adapter
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistLinear(geometry_msgs::Vector3 linear) override;
+    virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;
 
     /**
      * @brief get velocity for x, y asix of the robot
      * @param linear The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
+    virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;
 
     /**
      * @brief Set velocity theta for z asix of the robot
      * @param angular the command velocity
      * @return True if set is done, false otherwise
      */
-    virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
+    virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;
 
     /**
      * @brief Get velocity theta for z asix of the robot
      * @param direct The velocity command
      * @return True if set is done, false otherwise
      */
-    virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
+    virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;
 
     /**
      * @brief Get actual velocity for x, y asix of the robot
      * @return The current velocity
      */
-    virtual geometry_msgs::Twist getActualTwist() override;
+    virtual robot_geometry_msgs::Twist getActualTwist() override;
 
     /**
      * @brief Check if the kinematic parameters are currently locked
@@ -156,7 +156,7 @@ namespace nav_core_adapter
      * @param plan The plan to pass to the local planner
      * @return True if the plan was updated successfully, false otherwise
      */
-    bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) override;
+    bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) override;
 
     /**
      * @brief Create a new LocalPlannerAdapter
diff --git a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
index 44f56aa..d1ee5ce 100755
--- a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
+++ b/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
@@ -81,9 +81,9 @@ namespace nav_core_adapter
   robot_nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
                                                       const robot_nav_2d_msgs::Pose2DStamped& goal)
   {
-    geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
+    robot_geometry_msgs::PoseStamped start3d = robot_nav_2d_utils::pose2DToPoseStamped(start),
                               goal3d = robot_nav_2d_utils::pose2DToPoseStamped(goal);
-    std::vector<geometry_msgs::PoseStamped> plan;
+    std::vector<robot_geometry_msgs::PoseStamped> plan;
     bool ret = planner_->makePlan(start3d, goal3d, plan);
     if (!ret)
     {
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 72d957f..e177764 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
@@ -165,7 +165,7 @@ namespace nav_core_adapter
   /**
    * @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands
    */
-  bool LocalPlannerAdapter::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
+  bool LocalPlannerAdapter::computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     if (!has_active_goal_)
@@ -196,7 +196,7 @@ namespace nav_core_adapter
     return true;
   }
 
-  bool LocalPlannerAdapter::setTwistLinear(geometry_msgs::Vector3 linear)
+  bool LocalPlannerAdapter::setTwistLinear(robot_geometry_msgs::Vector3 linear)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     try
@@ -212,7 +212,7 @@ namespace nav_core_adapter
     }
   }
 
-  geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct)
+  robot_geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     try
@@ -228,7 +228,7 @@ namespace nav_core_adapter
     }
   }
 
-  bool LocalPlannerAdapter::setTwistAngular(geometry_msgs::Vector3 angular)
+  bool LocalPlannerAdapter::setTwistAngular(robot_geometry_msgs::Vector3 angular)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     try
@@ -244,7 +244,7 @@ namespace nav_core_adapter
     }
   }
 
-  geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct)
+  robot_geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     try
@@ -260,7 +260,7 @@ namespace nav_core_adapter
     }
   }
 
-  geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
+  robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
   {
     if (odom_sub_)
       return robot_nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
@@ -342,7 +342,7 @@ namespace nav_core_adapter
   /**
    * @brief Convert from 2d to 3d and call child setPlan
    */
-  bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
+  bool LocalPlannerAdapter::setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &orig_global_plan)
   {
     boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
     robot_nav_2d_msgs::Path2D path = robot_nav_2d_utils::posesToPath2D(orig_global_plan);
@@ -384,7 +384,7 @@ namespace nav_core_adapter
 
   bool LocalPlannerAdapter::getRobotPose(robot_nav_2d_msgs::Pose2DStamped &pose2d)
   {
-    geometry_msgs::PoseStamped current_pose;
+    robot_geometry_msgs::PoseStamped current_pose;
     if (!costmap_robot_->getRobotPose(current_pose))
     {
       std::cout << "Could not get robot pose" << std::endl;
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 0ffb17f..6c2ced4 100755
--- a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
+++ b/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp
@@ -43,7 +43,7 @@ TEST(LocalPlannerAdapter, unload_local_planner)
   // This empty transform is added to satisfy the constructor of
   // Costmap2DROBOT, which waits for the transform from map to base_link
   // to become available.
-  geometry_msgs::TransformStamped base_rel_map;
+  robot_geometry_msgs::TransformStamped base_rel_map;
   base_rel_map.child_frame_id = "base_link";
   base_rel_map.header.frame_id = "map";
   base_rel_map.transform.rotation.w = 1.0;
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 67db766..19e7b67 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
@@ -82,7 +82,7 @@ namespace move_base
      * @param fprt A vector of points representing the robot's footprint polygon.
      *             The points should be ordered counter-clockwise.
      */
-    virtual void setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt) override;
+    virtual void setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt) override;
 
     /**
      * @brief Send a goal for the robot to navigate to.
@@ -91,7 +91,7 @@ namespace move_base
      * @param yaw_goal_tolerance Acceptable angular error (radians).
      * @return True if goal was accepted and sent successfully.
      */
-    virtual bool moveTo(const geometry_msgs::PoseStamped &goal,
+    virtual bool moveTo(const robot_geometry_msgs::PoseStamped &goal,
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;
 
@@ -104,7 +104,7 @@ namespace move_base
      * @return True if goal was accepted and sent successfully.
      */
     virtual bool moveTo(const robot_protocol_msgs::Order &msg,
-                        const geometry_msgs::PoseStamped &goal,
+                        const robot_geometry_msgs::PoseStamped &goal,
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;
 
@@ -117,7 +117,7 @@ namespace move_base
      * @return True if docking command succeeded.
      */
     virtual bool dockTo(const std::string &maker,
-                        const geometry_msgs::PoseStamped &goal,
+                        const robot_geometry_msgs::PoseStamped &goal,
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;
 
@@ -130,7 +130,7 @@ namespace move_base
      * @return True if docking command succeeded.
      */
     virtual bool dockTo(const robot_protocol_msgs::Order &msg,
-                        const geometry_msgs::PoseStamped &goal,
+                        const robot_geometry_msgs::PoseStamped &goal,
                         double xy_goal_tolerance = 0.0,
                         double yaw_goal_tolerance = 0.0) override;
 
@@ -141,7 +141,7 @@ namespace move_base
      * @param xy_goal_tolerance Acceptable positional error (meters).
      * @return True if command issued successfully.
      */
-    virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal,
+    virtual bool moveStraightTo(const robot_geometry_msgs::PoseStamped &goal,
                                 double xy_goal_tolerance = 0.0) override;
 
     /**
@@ -150,7 +150,7 @@ namespace move_base
      * @param yaw_goal_tolerance Acceptable angular error (radians).
      * @return True if rotation command was sent successfully.
      */
-    virtual bool rotateTo(const geometry_msgs::PoseStamped &goal,
+    virtual bool rotateTo(const robot_geometry_msgs::PoseStamped &goal,
                           double yaw_goal_tolerance = 0.0) override;
 
     /**
@@ -173,28 +173,28 @@ namespace move_base
      * @param linear Linear velocity vector (x, y, z).
      * @return True if the command was accepted.
      */
-    virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) override;
+    virtual bool setTwistLinear(const robot_geometry_msgs::Vector3 &linear) override;
 
     /**
      * @brief Send limited angular velocity command.
      * @param angular Angular velocity vector (x, y, z).
      * @return True if the command was accepted.
      */
-    virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) override;
+    virtual bool setTwistAngular(const robot_geometry_msgs::Vector3 &angular) override;
 
     /**
      * @brief Get the robot’s pose as a PoseStamped.
      * @param pose Output parameter with the robot’s current pose.
      * @return True if pose was successfully retrieved.
      */
-    virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) override;
+    virtual bool getRobotPose(robot_geometry_msgs::PoseStamped &pose) override;
 
     /**
      * @brief Get the robot’s pose as a 2D pose.
      * @param pose Output parameter with the robot’s current 2D pose.
      * @return True if pose was successfully retrieved.
      */
-    virtual bool getRobotPose(geometry_msgs::Pose2D &pose) override;
+    virtual bool getRobotPose(robot_geometry_msgs::Pose2D &pose) override;
 
 
     /**
@@ -229,7 +229,7 @@ namespace move_base
      * @param goal A reference to the goal to pursue
      * @return True if processing of the goal is done, false otherwise
      */
-     bool executeCycle(geometry_msgs::PoseStamped &goal);
+     bool executeCycle(robot_geometry_msgs::PoseStamped &goal);
 
     /**
      * @brief Set velocity for yaw goal tolerance of the robot
@@ -255,7 +255,7 @@ namespace move_base
      * @param  plan Will be filled in with the plan made by the planner
      * @return  True if planning succeeds, false otherwise
      */
-    bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);
+    bool makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan);
 
     /**
      * @brief  Load the recovery behaviors for the navigation stack from the parameter server
@@ -285,19 +285,19 @@ namespace move_base
      */
     void resetState();
 
-    // void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);
+    // void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);
 
     void planThread();
 
     // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
 
-    bool isQuaternionValid(const geometry_msgs::Quaternion &q);
+    bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
 
-    bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
+    bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
 
-    double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2);
+    double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
 
-    geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg);
+    robot_geometry_msgs::PoseStamped goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg);
 
     // /**
     //  * @brief This is used to wake the planner at periodic intervals.
@@ -324,7 +324,7 @@ namespace move_base
     unsigned int recovery_index_;
     bool recovery_behavior_enabled_;
 
-    geometry_msgs::PoseStamped global_pose_;
+    robot_geometry_msgs::PoseStamped global_pose_;
     double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
     double planner_patience_, controller_patience_;
     int32_t max_planning_retries_;
@@ -341,18 +341,18 @@ namespace move_base
     RecoveryTrigger recovery_trigger_;
 
     robot::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
-    geometry_msgs::PoseStamped oscillation_pose_;
+    robot_geometry_msgs::PoseStamped oscillation_pose_;
 
     // set up plan triple buffer
-    std::vector<geometry_msgs::PoseStamped> *planner_plan_;
-    std::vector<geometry_msgs::PoseStamped> *latest_plan_;
-    std::vector<geometry_msgs::PoseStamped> *controller_plan_;
+    std::vector<robot_geometry_msgs::PoseStamped> *planner_plan_;
+    std::vector<robot_geometry_msgs::PoseStamped> *latest_plan_;
+    std::vector<robot_geometry_msgs::PoseStamped> *controller_plan_;
 
     // set up the planner's thread
     bool runPlanner_;
     boost::recursive_mutex planner_mutex_;
     boost::condition_variable_any planner_cond_;
-    geometry_msgs::PoseStamped planner_goal_;
+    robot_geometry_msgs::PoseStamped planner_goal_;
     boost::thread *planner_thread_;
 
     move_base::MoveBaseConfig last_config_;
@@ -363,8 +363,8 @@ namespace move_base
     bool cancel_ctr_;
     bool pause_ctr_, paused_;
     double min_approach_linear_velocity_{0.1};
-    geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
-    geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
+    robot_geometry_msgs::Vector3 old_linear_fwd_, old_linear_bwd_;
+    robot_geometry_msgs::Vector3 old_angular_fwd_, old_angular_rev_;
     double original_xy_goal_tolerance_, original_yaw_goal_tolerance_;
 
     // defined planner name
diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index 958f590..2ea3a21 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -152,9 +152,9 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
     min_approach_linear_velocity_ *= 1.2;
     // set up plan triple buffer
-    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
-    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
-    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
+    planner_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
+    latest_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
+    controller_plan_ = new std::vector<robot_geometry_msgs::PoseStamped>();
 
     // set up the planner's thread
     planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
@@ -357,7 +357,7 @@ void move_base::MoveBase::swapPlanner(std::string base_global_planner)
 {
 }
 
-void move_base::MoveBase::setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt)
+void move_base::MoveBase::setRobotFootprint(const std::vector<robot_geometry_msgs::Point> &fprt)
 {
   if (planner_costmap_robot_)
     planner_costmap_robot_->setUnpaddedRobotFootprint(fprt);
@@ -366,7 +366,7 @@ void move_base::MoveBase::setRobotFootprint(const std::vector<geometry_msgs::Poi
     controller_costmap_robot_->setUnpaddedRobotFootprint(fprt);
 }
 
-bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
+bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   if (setup_)
   {
@@ -427,14 +427,14 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double
 }
 
 bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
-                                 const geometry_msgs::PoseStamped &goal,
+                                 const robot_geometry_msgs::PoseStamped &goal,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   return false;
 }
 
 
-bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::PoseStamped &goal,
+bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   std::string maker_sources;
@@ -521,13 +521,13 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::
 }
 
 bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
-                                 const geometry_msgs::PoseStamped &goal,
+                                 const robot_geometry_msgs::PoseStamped &goal,
                                  double xy_goal_tolerance, double yaw_goal_tolerance)
 {
   return false;
 }
 
-bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
+bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
 {
   if (setup_)
   {
@@ -575,7 +575,7 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal,
   return true;
 }
 
-bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance)
+bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal, double yaw_goal_tolerance)
 {
   if (setup_)
   {
@@ -619,7 +619,7 @@ bool move_base::MoveBase::rotateTo(const geometry_msgs::PoseStamped &goal, doubl
   }
   if (cancel_ctr_)
     cancel_ctr_ = false;
-  geometry_msgs::PoseStamped pose;
+  robot_geometry_msgs::PoseStamped pose;
   if (!this->getRobotPose(pose))
   {
     if (nav_feedback_)
@@ -693,7 +693,7 @@ void move_base::MoveBase::cancel()
   cancel_ctr_ = true;
 }
 
-bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &pose)
+bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose)
 {
   if (!initialized_)
     return false;
@@ -701,11 +701,11 @@ bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &pose)
   return true;
 }
 
-bool move_base::MoveBase::getRobotPose(geometry_msgs::Pose2D &pose)
+bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::Pose2D &pose)
 {
   if (!initialized_)
     return false;
-  geometry_msgs::PoseStamped global_pose;
+  robot_geometry_msgs::PoseStamped global_pose;
   this->getRobotPose(global_pose, planner_costmap_robot_);
   pose.x = global_pose.pose.position.x;
   pose.y = global_pose.pose.position.y;
@@ -720,7 +720,7 @@ bool move_base::MoveBase::getRobotPose(geometry_msgs::Pose2D &pose)
   return true;
 }
 
-bool move_base::MoveBase::setTwistLinear(const geometry_msgs::Vector3 &linear)
+bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &linear)
 {
   try
   {
@@ -747,7 +747,7 @@ bool move_base::MoveBase::setTwistLinear(const geometry_msgs::Vector3 &linear)
   }
 }
 
-bool move_base::MoveBase::setTwistAngular(const geometry_msgs::Vector3 &angular)
+bool move_base::MoveBase::setTwistAngular(const robot_geometry_msgs::Vector3 &angular)
 {
   if (tc_ && !cancel_ctr_)
   {
@@ -799,7 +799,7 @@ void move_base::MoveBase::setXyGoalTolerance(double xy_goal_tolerance)
 
 // }
 
-bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan)
+bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal, std::vector<robot_geometry_msgs::PoseStamped> &plan)
 {
   // make sure to set the plan to be empty initially
   plan.clear();
@@ -814,14 +814,14 @@ bool move_base::MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::
   boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
 
   // get the starting pose of the robot
-  geometry_msgs::PoseStamped global_pose;
+  robot_geometry_msgs::PoseStamped global_pose;
   if (!getRobotPose(global_pose, planner_costmap_robot_))
   {
     std::cerr << "Unable to get starting pose of robot, unable to create global plan" << std::endl;
     return false;
   }
 
-  const geometry_msgs::PoseStamped &start = global_pose;
+  const robot_geometry_msgs::PoseStamped &start = global_pose;
 
   // if the planner fails or returns a zero length plan, planning failed
   if (!planner_->makePlan(start, goal, plan) || plan.empty())
@@ -1002,15 +1002,15 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
 
 void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
 {
-  geometry_msgs::PoseStamped global_pose;
+  robot_geometry_msgs::PoseStamped global_pose;
 
   // clear the planner's costmap
   getRobotPose(global_pose, planner_costmap_robot_);
 
-  std::vector<geometry_msgs::Point> clear_poly;
+  std::vector<robot_geometry_msgs::Point> clear_poly;
   double x = global_pose.pose.position.x;
   double y = global_pose.pose.position.y;
-  geometry_msgs::Point pt;
+  robot_geometry_msgs::Point pt;
 
   pt.x = x - size_x / 2;
   pt.y = y - size_y / 2;
@@ -1084,7 +1084,7 @@ void move_base::MoveBase::resetState()
   }
 }
 
-// void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);
+// void goalCB(const robot_geometry_msgs::PoseStamped::ConstPtr &goal);
 
 void move_base::MoveBase::planThread()
 {
@@ -1106,7 +1106,7 @@ void move_base::MoveBase::planThread()
   //   robot::Time start_time = robot::Time::now();
 
   //   // time to plan! get a copy of the goal and unlock the mutex
-  //   geometry_msgs::PoseStamped temp_goal = planner_goal_;
+  //   robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
   //   lock.unlock();
   //   std::cout << "Planning..." << std::endl;
   //   // run planner
@@ -1118,7 +1118,7 @@ void move_base::MoveBase::planThread()
   //   {
   //     std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl;
   //     // pointer swap the plans under mutex (the controller will pull from latest_plan_)
-  //     std::vector<geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
+  //     std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
 
   //     lock.lock();
   //     planner_plan_ = latest_plan_;
@@ -1185,7 +1185,7 @@ void move_base::MoveBase::planThread()
 
 // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
 
-bool move_base::MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q)
+bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternion &q)
 {
   // first we need to check if the quaternion has nan's or infs
   if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
@@ -1219,11 +1219,11 @@ bool move_base::MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q)
   return true;
 }
 
-bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
+bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
 {
   
   tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
-  geometry_msgs::PoseStamped robot_pose;
+  robot_geometry_msgs::PoseStamped robot_pose;
   tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
   robot_pose.header.frame_id = robot_base_frame_;
   robot_pose.header.stamp = robot::Time();     // latest available
@@ -1263,15 +1263,15 @@ bool move_base::MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose,
   return true;
 }
 
-double move_base::MoveBase::distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
+double move_base::MoveBase::distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2)
 {
   return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
 }
 
-geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg)
+robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg)
 {
   std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
-  geometry_msgs::PoseStamped goal_pose, global_pose;
+  robot_geometry_msgs::PoseStamped goal_pose, global_pose;
   goal_pose = goal_pose_msg;
 
   goal_pose.header.stamp = robot::Time();     // latest available