update

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)
+if (NOT TARGET xmlrpcpp)
-   add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_xmlrpcpp) 
+   add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/xmlrpcpp) 
 endif()
 
 if (NOT TARGET robot_cpp)

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)
+- `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
-- `geometry_msgs::Twist` (vận tốc linear/angular)
+- `robot_geometry_msgs::Twist` (vận tốc linear/angular)
-- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
+- `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
 - `costmap_2d::Costmap2D` (bản đồ chi phí)
 
 ### 3. Thiết kế từng module (interface level)

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 robot_geometry_msgs::PoseStamped getCurrentPose() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 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;
 };

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 <robot_geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
-#include <geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Point.h>
-#include <geometry_msgs/Vector3.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) {
+    robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
-        geometry_msgs::PoseStamped cpp_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);
+    robot_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 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;

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 <robot_geometry_msgs/Pose2D.h>
-#include <geometry_msgs/PoseStamped.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>

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;
 
     /**

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

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_)

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 <robot_geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.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,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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, 
+  bool getGoalPose(const robot_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, 
+                   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_;
+  std::vector<robot_geometry_msgs::Pose2D> reached_intermediate_goals_;
-  geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
+  robot_geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
-  geometry_msgs::Pose2D goal_;
+  robot_geometry_msgs::Pose2D goal_;
   robot_nav_2d_msgs::Path2D global_plan_;
 
   unsigned int start_index_saved_;

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;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
-    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    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_; 

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 <robot_geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.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,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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 

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, 
+  bool isGoalReached(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose, 
-                             const std::vector<geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
+                             const std::vector<robot_geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
 
 protected:
 

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,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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:
 

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 <robot_geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.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,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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 

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 <robot_geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PointStamped.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_;

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,
+bool GoStraight::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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;

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);
+  reached_intermediate_goals_pub_   = nh_.advertise<robot_geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
-  current_goal_pub_                 = nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
+  current_goal_pub_                 = nh_.advertise<robot_geometry_msgs::PoseStamped>("current_goal", 1);
-  closet_robot_goal_pub_            = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
+  closet_robot_goal_pub_            = nh_.advertise<robot_geometry_msgs::PoseStamped>("closet_robot_goal", 1);
-  cmd_raw_pub_                      = nh_.advertise<geometry_msgs::Twist>("cmd_raw", 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,
+bool Bicycle::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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, 
+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, geometry_msgs::Pose2D &sub_goal, 
+                          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>'
+  // 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<geometry_msgs::Pose2D> plan = global_plan.poses;
+  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];
+    robot_geometry_msgs::Pose2D p1 = plan[i];
-    geometry_msgs::Pose2D p2 = plan[i+1];
+    robot_geometry_msgs::Pose2D p2 = plan[i+1];
-    // geometry_msgs::Pose2D p3 = plan[i+2];
+    // 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)

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,
+bool RotateToGoal::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+                       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;

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 robot_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 &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

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 robot_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 &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 robot_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 &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;
+        robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
-        geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].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>
+std::vector<robot_geometry_msgs::Point>
-mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution)
+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 robot_geometry_msgs::Point &start = abs_footprint[i];
-    const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+    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);

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;

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;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
-    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    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_; 

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);
 

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: 

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;

src/Algorithms/Packages/global_planners/custom_planner

@@ -1 +1 @@
-Subproject commit 377ebe3d6f66c6a1c7f2d1a1d76fd1b330a93952
+Subproject commit 6d55c6c4beb86ae77c20cba3f26c6231a16912fa

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, 
+  virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
-                        const geometry_msgs::PoseStamped& goal, 
+                        const robot_geometry_msgs::PoseStamped& goal, 
-                        std::vector<geometry_msgs::PoseStamped>& plan);
+                        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_; 
 

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,
+  bool TwoPointsPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
-    const geometry_msgs::PoseStamped& goal,
+    const robot_geometry_msgs::PoseStamped& goal,
-    std::vector<geometry_msgs::PoseStamped>& plan)
+    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();

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_;
+      robot_geometry_msgs::Vector3 linear_;
-      geometry_msgs::Vector3 angular_;
+      robot_geometry_msgs::Vector3 angular_;
 
     private:
       /**

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

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).

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);
+  robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
-  geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
+  robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
-  std::vector<geometry_msgs::PoseStamped> docking_plan;
+  std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
 
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {

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
   {

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 robot_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& 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)
         {

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit 71adf1390fe7c1bad3823d25a06db43a9c46278b
+Subproject commit 2c3d7d586d1a664573b99f45d8521950d46db30a

src/Libraries/data_convert

@@ -1 +1 @@
-Subproject commit 8b22de38ac16eccce348e505e12cfafdf33e2943
+Subproject commit b1aaa1a9463853ec16574b27182c5753c630a407

src/Libraries/geometry2

@@ -1 +1 @@
-Subproject commit 3c51b228ec4d3f3223225142d3c13e485e01de52
+Subproject commit 2987c1a481390e4d0bf08cf97aee3bc758d23ad1

src/Libraries/laser_geometry

@@ -1 +1 @@
-Subproject commit 1b06dd9122b46f8d8a7c0414d8fa5043656f7933
+Subproject commit a183d4bb7bdd1e6eb44be08911cc1a4e4590c4e4

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;
 
 

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 "=================================")

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(robot_geometry_msgs::Point)` | `robot_geometry_msgs::Point pointToPoint3D(Point2D)`
-| `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(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)`
+| `Pose2DStamped poseStampedToPose2D(robot_geometry_msgs::PoseStamped)` | `robot_geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
-||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
+||`robot_geometry_msgs::PoseStamped pose2DToPoseStamped(robot_geometry_msgs::Pose2D, std::string, robot::Time)`|
-||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
+||`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)`
+| `Polygon2D polygon3Dto2D(robot_geometry_msgs::Polygon)` |`robot_geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
-| `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
+| `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` |

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 <robot_geometry_msgs/Point.h>
-#include <geometry_msgs/Pose.h>
+#include <robot_geometry_msgs/Pose.h>
-#include <geometry_msgs/Twist.h>
+#include <robot_geometry_msgs/Twist.h>
-#include <geometry_msgs/PolygonStamped.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_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_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 ::robot_geometry_msgs::Point &point);
-  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point32 &point);
+  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point32 &point);
-  ::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::Point32 pointToPoint32(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);
+  ::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::robot_geometry_msgs::PoseStamped &pose);
-  ::geometry_msgs::Pose pose2DToPose(const ::geometry_msgs::Pose2D &pose2d);
+  ::robot_geometry_msgs::Pose pose2DToPose(const ::robot_geometry_msgs::Pose2D &pose2d);
-  ::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 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);
 
   // Path Transformations
   ::robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path);
-  nav_msgs::Path posesToPath(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
+  nav_msgs::Path posesToPath(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
-  ::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);
-  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 pathToPath(const ::robot_nav_2d_msgs::Path2D &path2d);
 
   // Polygon Transformations
-  ::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);
-  ::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);
-  ::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);
-  ::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);
 
   // 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);
+  ::robot_geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
-  ::geometry_msgs::Pose2D getOrigin2D(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);
 

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

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

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

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;

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)
       {

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());

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);
+    robot_geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
-    geometry_msgs::PoseStamped out_3d_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;

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;

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

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;
   */

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;

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
  */

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;

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>
 

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;
 

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);