Hiep update

.gitmodules

@@ -7,9 +7,6 @@
 [submodule "src/Libraries/geometry2"]
 	path = src/Libraries/geometry2
 	url = https://git.pnkr.asia/DuongTD/geometry2.git
-[submodule "src/Libraries/xmlrpcpp"]
-	path = src/Libraries/xmlrpcpp
-	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git
 [submodule "src/Libraries/laser_geometry"]
 	path = src/Libraries/laser_geometry
 	url = https://git.pnkr.asia/DuongTD/laser_geometry.git
@@ -22,7 +19,6 @@
 [submodule "src/Libraries/data_convert"]
 	path = src/Libraries/data_convert
 	url = https://git.pnkr.asia/DuongTD/data_convert.git
-
 [submodule "src/Libraries/robot_cpp"]
 	path = src/Libraries/robot_cpp
 	url = http://git.pnkx/HiepLM/robot_cpp.git
@@ -32,3 +28,6 @@
 [submodule "src/Algorithms/Packages/global_planners/dock_planner"]
 	path = src/Algorithms/Packages/global_planners/dock_planner
 	url = https://git.pnkr.asia/DuongTD/dock_planner.git
+[submodule "src/Libraries/xmlrpcpp"]
+	path = src/Libraries/xmlrpcpp
+	url = https://git.pnkr.asia/DuongTD/xmlrpcpp.git

CMakeLists.txt

@@ -98,57 +98,57 @@ if (NOT TARGET move_base_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/move_base_core)
 endif()
 
-# if (NOT TARGET mkt_msgs)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs)
-# endif()
+if (NOT TARGET mkt_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs)
+endif()
 
-# if (NOT TARGET angles)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles)
-# endif()
+if (NOT TARGET angles)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles)
+endif()
 
-# if (NOT TARGET kalman)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman)
-# endif()
+if (NOT TARGET kalman)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman)
+endif()
 
-# if (NOT TARGET score_algorithm)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
-# endif()
+if (NOT TARGET score_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
+endif()
 
-# if (NOT TARGET mkt_plugins)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins)
-# endif()
+if (NOT TARGET mkt_plugins)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins)
+endif()
 
-# if (NOT TARGET mkt_algorithm)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
-# endif()
+if (NOT TARGET mkt_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
+endif()
 
-# if (NOT TARGET two_points_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
-# endif()
+if (NOT TARGET two_points_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
+endif()
 
-# if (NOT TARGET custom_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner)
-# endif()
+if (NOT TARGET custom_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner)
+endif()
 
-# if (NOT TARGET dock_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner)
-# endif()
+if (NOT TARGET dock_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner)
+endif()
 
-# if (NOT TARGET pnkx_local_planner)
-#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
-# endif()
+if (NOT TARGET pnkx_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
+endif()
 
 
 # 2. Main packages (phụ thuộc vào cores)
-message(STATUS "[move_base] Shared library configured")
+# message(STATUS "[move_base] Shared library configured")
 if (NOT TARGET move_base)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
 endif()
 
 # C API for .NET/C# integration
-if (NOT TARGET navigation_c_api)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
-endif()
+# if (NOT TARGET navigation_c_api)
+#     add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api)
+# endif()
 
 message(STATUS "========================================")
 message(STATUS "All packages configured successfully")

config/mprim/ekf.yaml

@@ -65,9 +65,9 @@ odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspeci
 base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
 world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
 
-# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry,
 # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
-# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# robot_sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
 # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
 # default values, and must be specified.
 odom0: odom

doc/architecture_discussion.md

@@ -67,8 +67,8 @@ flowchart TB
   %% ========== COSTMAP LAYER ==========
   subgraph Costmap["🗺️ Costmap Layer"]
     direction LR
-    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
-    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
+    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
+    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
     style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
     style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
     style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
@@ -89,12 +89,12 @@ 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"]
-    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"]
+    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
+    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 robot_sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
+    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 robot_nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
     style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
     style Goal fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
     style Loc fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
@@ -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
@@ -211,7 +211,7 @@ Cần làm rõ:
    - Plugin system sử dụng `boost::dll` để dynamic loading
 
 5. **Costmap Layer** ✅
-   - `costmap_2d::Costmap2DROBOT`: Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap
    - Costmap layers: static map, obstacles, inflation
    - Frame management: map (global), odom (local)
 
@@ -232,10 +232,10 @@ Cần làm rõ:
    - Velocity command execution
 
 **Định dạng dữ liệu:**
-- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
-- `geometry_msgs::Twist` (vận tốc linear/angular)
-- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
-- `costmap_2d::Costmap2D` (bản đồ chi phí)
+- `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
+- `robot_geometry_msgs::Twist` (vận tốc linear/angular)
+- `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
+- `robot_costmap_2d::Costmap2D` (bản đồ chi phí)
 
 ### 3. Thiết kế từng module (interface level)
 
@@ -264,7 +264,7 @@ Cần làm rõ:
 - `nav_core::RecoveryBehavior` ✅
   - `runBehavior()` - Thực thi recovery behavior
 
-- `costmap_2d::Costmap2DROBOT` ✅
+- `robot_costmap_2d::Costmap2DROBOT` ✅
   - Wrapper cho costmap với robot footprint
   - Thread-safe access với mutex
 
@@ -411,7 +411,7 @@ pnkx_nav_core/
 │   │   │   ├── nav_core_adapter/    # Adapter utilities
 │   │   │   └── nav_core2/           # Additional nav utilities
 │   │   ├── Libraries/
-│   │   │   ├── costmap_2d/          # Costmap system
+│   │   │   ├── robot_costmap_2d/          # Costmap system
 │   │   │   ├── tf3/                 # Transform system
 │   │   │   ├── robot_time/          # Time management
 │   │   │   ├── geometry2/           # Geometry utilities

doc/folders.md

@@ -6,16 +6,16 @@ Mô tả cấu trúc:
 │   │   ├── include/
 │   │   └── test/
 │   ├── CMakeLists.txt
-│   ├── nav_msgs/
+│   ├── robot_nav_msgs/
 │   │   ├── include/
 │   │   └── test/
 │   ├── CMakeLists.txt
-│   ├── sensor_msgs/
+│   ├── robot_sensor_msgs/
 │   │   ├── cfg/
 │   │   ├── include/
 │   │   └── test/
 │   ├── CMakeLists.txt
-│   ├── std_msgs/
+│   ├── robot_std_msgs/
 │   │   ├── include/
 │   │   └── CMakeLists.txt
 │   └── CMakeLists.txt (root)

doc/implementation_plan.md

@@ -15,7 +15,7 @@
    - Plugin system với `boost::dll` để load dynamic plugins
 
 3. **Costmap System:**
-   - `costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
    - Costmap layers (static, obstacle, inflation, etc.)
 
 4. **Supporting Libraries:**
@@ -56,8 +56,8 @@
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
 ```cpp
 class IOdometrySource {
-  virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::PoseStamped getCurrentPose() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
   virtual bool isAvailable() = 0;
 };
 ```
@@ -66,7 +66,7 @@ class IOdometrySource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
 ```cpp
 class ILocalizationSource {
-  virtual geometry_msgs::PoseStamped getRobotPose() = 0;
+  virtual robot_geometry_msgs::PoseStamped getRobotPose() = 0;
   virtual std::string getGlobalFrame() = 0;
   virtual bool isLocalized() = 0;
 };
@@ -76,7 +76,7 @@ class ILocalizationSource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h`
 ```cpp
 class IMapProvider {
-  virtual nav_msgs::OccupancyGrid getMap() = 0;
+  virtual robot_nav_msgs::OccupancyGrid getMap() = 0;
   virtual bool isMapAvailable() = 0;
 };
 ```
@@ -85,7 +85,7 @@ class IMapProvider {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h`
 ```cpp
 class ISensorSource {
-  virtual sensor_msgs::LaserScan getLatestScan() = 0;
+  virtual robot_sensor_msgs::LaserScan getLatestScan() = 0;
   virtual bool hasNewData() = 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;
 };

examples/QUICK_START.md

@@ -85,9 +85,9 @@ cp ../../build/src/APIs/c_api/libnav_c_api.so .
 
 # 5. Set library path (quan trọng!)
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
@@ -118,9 +118,9 @@ ls -la ../../build/src/APIs/c_api/libnav_c_api.so
 
 # Set LD_LIBRARY_PATH (bao gồm tất cả dependencies)
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
@@ -143,6 +143,6 @@ ldd libnav_c_api.so
 → Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`.
    Nếu vẫn gặp lỗi, kiểm tra:
    - Library đã được build đầy đủ: `make nav_c_api` trong build directory
-   - Dependencies đã được build: `make move_base costmap_2d`
+   - Dependencies đã được build: `make move_base robot_costmap_2d`
    - LD_LIBRARY_PATH đã được set đúng
 

examples/README.md

@@ -78,9 +78,9 @@ Tạo file `NavigationExample.csproj`:
 ```bash
 # Set LD_LIBRARY_PATH để tìm được tất cả dependencies
 export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
-export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
+export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
 export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH

examples/run_example.sh

@@ -95,9 +95,9 @@ export LD_LIBRARY_PATH="$BUILD_DIR:$LD_LIBRARY_PATH"
 # Library directories for dependencies
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_time:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/xmlrpcpp:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/costmap_2d:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"

src/APIs/c_api/src/nav_c_api.cpp

@@ -2,10 +2,10 @@
 #include <move_base_core/navigation.h>
 #include <move_base_core/common.h>
 #include <move_base/move_base.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Vector3.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Vector3.h>
 #include <tf3/buffer_core.h>
 #include <string>
 #include <vector>
@@ -21,8 +21,8 @@
 
 namespace {
     // Convert C PoseStamped to C++ PoseStamped
-    geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
-        geometry_msgs::PoseStamped cpp_pose;
+    robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
+        robot_geometry_msgs::PoseStamped cpp_pose;
         cpp_pose.header.seq = c_pose->header.seq;
         cpp_pose.header.stamp.sec = c_pose->header.sec;
         cpp_pose.header.stamp.nsec = c_pose->header.nsec;
@@ -40,7 +40,7 @@ namespace {
     }
 
     // Convert C++ PoseStamped to C PoseStamped
-    void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
+    void cpp_to_c_pose_stamped(const robot_geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
         c_pose->header.seq = cpp_pose.header.seq;
         c_pose->header.sec = cpp_pose.header.stamp.sec;
         c_pose->header.nsec = cpp_pose.header.stamp.nsec;
@@ -59,7 +59,7 @@ namespace {
     }
 
     // Convert C++ Pose2D to C Pose2D
-    void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
+    void cpp_to_c_pose_2d(const robot_geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
         c_pose->x = cpp_pose.x;
         c_pose->y = cpp_pose.y;
         c_pose->theta = cpp_pose.theta;
@@ -112,12 +112,12 @@ extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double p
         return false;
     }
 
-    geometry_msgs::Pose2D pose;
+    robot_geometry_msgs::Pose2D pose;
     pose.x = pose_x;
     pose.y = pose_y;
     pose.theta = pose_theta;
 
-    geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
+    robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
     cpp_to_c_pose_stamped(result, out_goal);
     return true;
 }
@@ -128,8 +128,8 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, doubl
         return false;
     }
 
-    geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
-    geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
+    robot_geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
+    robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
     cpp_to_c_pose_stamped(result, out_goal);
     return true;
 }
@@ -260,10 +260,10 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        std::vector<geometry_msgs::Point> footprint;
+        std::vector<robot_geometry_msgs::Point> footprint;
         footprint.reserve(point_count);
         for (size_t i = 0; i < point_count; ++i) {
-            geometry_msgs::Point pt;
+            robot_geometry_msgs::Point pt;
             pt.x = points[i].x;
             pt.y = points[i].y;
             pt.z = points[i].z;
@@ -284,7 +284,7 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* g
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
     } catch (...) {
         return false;
@@ -300,7 +300,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
     } catch (...) {
         return false;
@@ -315,7 +315,7 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
     } catch (...) {
         return false;
@@ -330,7 +330,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
+        robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
         return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
     } catch (...) {
         return false;
@@ -378,7 +378,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::Vector3 linear;
+        robot_geometry_msgs::Vector3 linear;
         linear.x = linear_x;
         linear.y = linear_y;
         linear.z = linear_z;
@@ -396,7 +396,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::Vector3 angular;
+        robot_geometry_msgs::Vector3 angular;
         angular.x = angular_x;
         angular.y = angular_y;
         angular.z = angular_z;
@@ -413,7 +413,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::PoseStamped cpp_pose;
+        robot_geometry_msgs::PoseStamped cpp_pose;
         if (nav->getRobotPose(cpp_pose)) {
             cpp_to_c_pose_stamped(cpp_pose, out_pose);
             return true;
@@ -431,7 +431,7 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        geometry_msgs::Pose2D cpp_pose;
+        robot_geometry_msgs::Pose2D cpp_pose;
         if (nav->getRobotPose(cpp_pose)) {
             cpp_to_c_pose_2d(cpp_pose, out_pose);
             return true;

src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h

@@ -3,8 +3,8 @@
 
 #include <robot/node_handle.h>
 #include <robot/console.h>
-#include <geometry_msgs/Pose2D.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_msgs/Path2D.h>

src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h

@@ -11,8 +11,8 @@
 #include <nav_core2/exceptions.h>
 #include <nav_core2/costmap.h>
 
-#include <costmap_2d/costmap_2d_robot.h>
-#include <geometry_msgs/PoseArray.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
+#include <robot_geometry_msgs/PoseArray.h>
 #include <mkt_msgs/Trajectory2D.h>
 #include <score_algorithm/trajectory_generator.h>
 
@@ -38,7 +38,7 @@ namespace score_algorithm
      * @param tf TFListener pointer
      * @param costmap_robot Costmap pointer
      */
-    virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+    virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                             const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
   
     /**
@@ -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;
 
     /**
@@ -144,7 +144,7 @@ namespace score_algorithm
 
     std::string name_;
     TFListenerPtr tf_;
-    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
     TrajectoryGenerator::Ptr traj_;
 
     int index_samples_;

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/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>score_algorithm</name>
+  <version>0.7.10</version>
+  <description>
+    score_algorithm is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. score_algorithm
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/score_algorithm</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

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/angles/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>angles</name>
+  <version>0.7.10</version>
+  <description>
+    angles is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. angles
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/angles</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Algorithms/Libraries/kalman/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>kalman</name>
+  <version>0.7.10</version>
+  <description>
+    kalman is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. kalman
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/kalman</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -29,9 +29,9 @@ set(PACKAGES_DIR
     kalman
     angles
     nav_grid
-    costmap_2d
-    sensor_msgs
-    std_msgs
+    robot_costmap_2d
+    robot_sensor_msgs
+    robot_std_msgs
 )
 
 # Tìm tất cả file source cho diff library

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h

@@ -4,8 +4,8 @@
 #include <score_algorithm/score_algorithm.h>
 #include <mkt_algorithm/equation_line.h>
 #include <pluginlib/class_loader.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 #include <kalman/kalman.h>
 
 namespace mkt_algorithm
@@ -39,8 +39,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -53,30 +53,30 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 
 protected:
 
-  unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
+  unsigned int getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
                             const double distance, unsigned int start_index, unsigned int last_valid_index) const;
 
-  double journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
+  double journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
 
 
-  bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
-                   const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, 
+  bool getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
+                   const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                    double &target_direction);
                          
-  bool findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
+  bool findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
 
   robot::NodeHandle nh_priv_, nh_;
   std::string frame_id_path_;
   robot::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
 
-  std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
-  geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
-  geometry_msgs::Pose2D goal_;
+  std::vector<robot_geometry_msgs::Pose2D> reached_intermediate_goals_;
+  robot_geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; 
+  robot_geometry_msgs::Pose2D goal_;
   robot_nav_2d_msgs::Path2D global_plan_;
 
   unsigned int start_index_saved_;

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/equation_line.h

@@ -1,7 +1,7 @@
 #ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
 #define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
 #include <ros/ros.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 #include <iostream>
 
 namespace mkt_algorithm
@@ -24,8 +24,8 @@ class LineGenerator
 {
   /* data */
   struct LineEquation {
-    geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
-    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    robot_geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
     float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
   };
 
@@ -46,7 +46,7 @@ public:
    * @param start_point   start point of the line
    * @param end_point     end point of the line
    */
-  virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
+  virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);
 
   /**
    * @brief Calculating error 
@@ -54,14 +54,14 @@ public:
    * @param[in] pose
    * @param[in] rev         
    */
-  virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
+  virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);
 
   /**
    * @brief Calculating tolerance 
    * @param query_pose The pose to check
    * @param goal_pose The pose to check against
    */
-  virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
+  virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);
 
   /* Properties */
   LineEquation      Leq_; 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h

@@ -3,8 +3,8 @@
 
 #include <score_algorithm/score_algorithm.h>
 #include <pluginlib/class_loader.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 
 namespace mkt_algorithm
 {
@@ -37,8 +37,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -51,7 +51,7 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 
 protected:
@@ -60,7 +60,7 @@ protected:
 
   double steering_fix_wheel_distance_x_;
   double steering_fix_wheel_distance_y_;
-  geometry_msgs::Pose2D goal_;
+  robot_geometry_msgs::Pose2D goal_;
   robot::Time time_last_cmd_;
 
 }; // class GoStraight 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h

@@ -28,8 +28,8 @@ public:
    * @param velocity The robot's current velocity
    * @return True if goal is reached
    */
-  bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose, 
-                             const std::vector<geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
+  bool isGoalReached(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose, 
+                             const std::vector<robot_geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
 
 protected:
 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h

@@ -36,8 +36,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -45,7 +45,7 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 protected:
 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h

@@ -3,8 +3,8 @@
 
 #include <score_algorithm/score_algorithm.h>
 #include <pluginlib/class_loader.h>
-#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PoseArray.h>
+#include <robot_geometry_msgs/PoseStamped.h>
 
 namespace mkt_algorithm
 {
@@ -37,8 +37,8 @@ public:
    * @param goal The final goal (costmap frame)
    * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
    */
-  virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+  virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) override;
 
   /**
@@ -51,20 +51,20 @@ public:
    * @param pose  
    * @param velocity 
   */
-  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
+  virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
 
 
 protected:
   robot::NodeHandle nh_;
   robot::NodeHandle nh_kinematics_;
-  geometry_msgs::Pose2D sub_goal_;
+  robot_geometry_msgs::Pose2D sub_goal_;
 
   double steering_fix_wheel_distance_x_;
   double steering_fix_wheel_distance_y_;
   double angle_threshold_;
   double current_direction_;
   double velocity_rotate_min_, velocity_rotate_max_;
-  geometry_msgs::Pose2D goal_;
+  robot_geometry_msgs::Pose2D goal_;
   robot::Time time_last_cmd_;
 
 }; // class RotateToGoal 

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h

@@ -24,7 +24,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Calculating algorithm

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h

@@ -5,8 +5,8 @@
 #include <score_algorithm/score_algorithm.h>
 
 #include <boost/dll/alias.hpp>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PointStamped.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PointStamped.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <nav_core2/exceptions.h>
 #include <nav_core2/costmap.h>
@@ -14,7 +14,7 @@
 #include <angles/angles.h>
 #include <tf3/exceptions.h>
 #include <tf3/utils.h>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
 #include <kalman/kalman.h>
 #include <vector>
 
@@ -44,7 +44,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -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
@@ -157,7 +157,7 @@ namespace mkt_algorithm
        */
       bool transformGlobalPlan(
           TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-          const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
+          const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
           robot_nav_2d_msgs::Path2D &transformed_plan);
 
       /**
@@ -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/include/mkt_algorithm/diff/diff_rotate_to_goal.h

@@ -23,7 +23,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

src/Algorithms/Libraries/mkt_algorithm/package.xml

@@ -1,91 +1,26 @@
-<?xml version="1.0"?>
-<package format="2">
+<package>
   <name>mkt_algorithm</name>
-  <version>0.0.0</version>
-  <description>The mkt_algorithm package</description>
+  <version>0.7.10</version>
+  <description>
+    mkt_algorithm is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. mkt_algorithm
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="robotics@todo.todo">robotics</maintainer>
+  <url type="website">http://www.ros.org/wiki/mkt_algorithm</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
+  <build_depend>libconsole-bridge-dev</build_depend>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+  <run_depend>libconsole-bridge-dev</run_depend>
 
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/mkt_algorithm</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>score_algorithm</build_depend>
-  <build_depend>robot_nav_2d_msgs</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
-  <build_depend>robot_nav_2d_utils</build_depend>
-  <build_depend>kalman</build_depend>
-  <build_depend>ddynamic_reconfigure</build_depend>
-  <build_depend>costmap_2d</build_depend>
-  <build_depend>base_local_planner</build_depend>
-
-  <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>score_algorithm</build_export_depend>
-  <build_export_depend>robot_nav_2d_msgs</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>sensor_msgs</build_export_depend>
-  <build_export_depend>robot_nav_2d_utils</build_export_depend>
-  <build_export_depend>kalman</build_export_depend>
-  <build_export_depend>ddynamic_reconfigure</build_export_depend>
-  <build_export_depend>costmap_2d</build_export_depend>
-  <build_export_depend>base_local_planner</build_export_depend>
-
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>score_algorithm</exec_depend>
-  <exec_depend>robot_nav_2d_msgs</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-  <exec_depend>robot_nav_2d_utils</exec_depend>
-  <exec_depend>kalman</exec_depend>
-  <exec_depend>ddynamic_reconfigure</exec_depend>
-  <exec_depend>costmap_2d</exec_depend>
-  <exec_depend>base_local_planner</exec_depend>
-
-  <depend>pluginlib</depend>
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <score_algorithm plugin="${prefix}/plugins.xml"/>
-  </export>
-</package>
+</package>

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp

@@ -22,8 +22,8 @@ void GoStraight::reset()
   time_last_cmd_ = robot::Time::now();
 }
 
-bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+bool GoStraight::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) 
 {
   goal_ = goal;
@@ -37,12 +37,12 @@ bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_
   return  true;
 }
             
-robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
+robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
 {
   robot_nav_2d_msgs::Twist2DStamped result;
   result.header.stamp = robot::Time::now();
 
-  geometry_msgs::Pose2D steer_to_baselink_pose;
+  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
   
   if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
     return result;

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp

@@ -38,10 +38,10 @@ void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListen
     controller_frequency_param_name_ = 1.0;
   }
 
-  reached_intermediate_goals_pub_   = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
-  current_goal_pub_                 = nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
-  closet_robot_goal_pub_            = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
-  cmd_raw_pub_                      = nh_.advertise<geometry_msgs::Twist>("cmd_raw", 1);
+  reached_intermediate_goals_pub_   = nh_.advertise<robot_geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
+  current_goal_pub_                 = nh_.advertise<robot_geometry_msgs::PoseStamped>("current_goal", 1);
+  closet_robot_goal_pub_            = nh_.advertise<robot_geometry_msgs::PoseStamped>("closet_robot_goal", 1);
+  cmd_raw_pub_                      = nh_.advertise<robot_geometry_msgs::Twist>("cmd_raw", 1);
   x_direction_ = y_direction_= theta_direction_ = 0;
 
 // kalman
@@ -87,8 +87,8 @@ void Bicycle::reset()
   last_actuator_update_ = robot::Time(0);
 }
 
-bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+bool Bicycle::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) 
 {
   nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
@@ -119,12 +119,12 @@ bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msg
   return true;
 }
             
-robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
+robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
 {
   robot_nav_2d_msgs::Twist2DStamped result;
   result.header.stamp = robot::Time::now();
   
-  geometry_msgs::Pose2D steer_to_baselink_pose;
+  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
   
   if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
     return result;
@@ -154,7 +154,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
     const double d_theta = goal_.theta - sub_goal_.theta;
     if(dd < 1e-5)
     {
-      geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
+      robot_geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
       alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta);
       phi_steer = atan2(2 * L * sin(alpha), 1.0);
     }
@@ -198,7 +198,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
 
   if(is_filter_)
   {
-    geometry_msgs::Twist msg;
+    robot_geometry_msgs::Twist msg;
     msg.linear.x = result.velocity.x;
     msg.angular.z = phi_steer;
     cmd_raw_pub_.publish(msg);
@@ -220,7 +220,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
   return result;
 }
 
-unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
+unsigned int Bicycle::getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
                                    const double distance, unsigned int start_index, unsigned int last_valid_index) const
 {
   if (start_index >= last_valid_index)
@@ -253,8 +253,8 @@ unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, cons
   return goal_index;
 }
 
-bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
-                          const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, 
+bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, 
+                          const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                           double &target_direction)
 {
   const nav_core2::Costmap &costmap = *costmap_;
@@ -265,8 +265,8 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
     ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
     return false;
   }
-  // Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
-  std::vector<geometry_msgs::Pose2D> plan = global_plan.poses;
+  // Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<robot_geometry_msgs::Pose2D>'
+  std::vector<robot_geometry_msgs::Pose2D> plan = global_plan.poses;
 
   std::vector<unsigned int> index_s;
   if(!findSubGoalIndex(plan, index_s))
@@ -425,7 +425,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
     // check if goal already reached
     bool goal_already_reached = false;
 
-    geometry_msgs::PoseArray poseArrayMsg;
+    robot_geometry_msgs::PoseArray poseArrayMsg;
     poseArrayMsg.header.stamp = robot::Time::now();
     poseArrayMsg.header.frame_id = frame_id_path_;
 	  // std::stringstream ss;
@@ -433,7 +433,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
     {
       double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
       // ss << distance << " ";
-      geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
+      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
       poseArrayMsg.poses.push_back(pose);
     }
     reached_intermediate_goals_pub_.publish(poseArrayMsg);
@@ -538,14 +538,14 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
   {
     if(goal_index < (unsigned int)(plan.size() -1))
     {
-      const geometry_msgs::Pose2D p1 = plan[start_index];
+      const robot_geometry_msgs::Pose2D p1 = plan[start_index];
       int index;
       for(index = start_index+1; index < goal_index; index++)
       {
-        geometry_msgs::Pose2D pose = plan[index];
+        robot_geometry_msgs::Pose2D pose = plan[index];
         if(robot_nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_)  break;
       }      
-      const geometry_msgs::Pose2D p2 = plan[index];
+      const robot_geometry_msgs::Pose2D p2 = plan[index];
       if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
       {
         const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
@@ -569,32 +569,32 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
   }
 
   // Publish start_index
-  geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
+  robot_geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
   closet_robot_goal_pub_.publish(pose_st);
 
   // Publish goal_index
-  geometry_msgs::PoseStamped pose_g = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
+  robot_geometry_msgs::PoseStamped pose_g = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
   current_goal_pub_.publish(pose_g);
  
   return true;
 }
 
-bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
+bool Bicycle::findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
 {
   if (plan.empty())
   {
     return false;
   }
   double x_direction_saved = 0.0;
-  geometry_msgs::PoseArray poseArrayMsg;
+  robot_geometry_msgs::PoseArray poseArrayMsg;
   poseArrayMsg.header.stamp = robot::Time::now();
   poseArrayMsg.header.frame_id = frame_id_path_;
 
   for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)
   {
-    geometry_msgs::Pose2D p1 = plan[i];
-    geometry_msgs::Pose2D p2 = plan[i+1];
-    // geometry_msgs::Pose2D p3 = plan[i+2];
+    robot_geometry_msgs::Pose2D p1 = plan[i];
+    robot_geometry_msgs::Pose2D p2 = plan[i+1];
+    // robot_geometry_msgs::Pose2D p3 = plan[i+2];
     double angle1;
     if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
       angle1 = atan2(p2.y - p1.y, p2.x - p1.x);
@@ -605,7 +605,7 @@ bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, s
     if(target_direction * x_direction_saved < 0.0)
     {
       index_s.push_back(i);
-      geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]);
+      robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]);
       poseArrayMsg.poses.push_back(pose);
     }
     x_direction_saved = target_direction;
@@ -622,7 +622,7 @@ bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, s
   return true;
 }
 
-double Bicycle::journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
+double Bicycle::journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
 {
   double S = 0; 
   if(last_valid_index - start_index > 1)

src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp

@@ -25,8 +25,8 @@ void RotateToGoal::reset()
   time_last_cmd_ = robot::Time::now();
 }
 
-bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
-                       const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
+bool RotateToGoal::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
+                       const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
                        double& x_direction, double& y_direction, double& theta_direction) 
 {
   goal_ = goal;
@@ -38,12 +38,12 @@ bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2
   return  true;
 }
             
-robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
+robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
 {
   robot_nav_2d_msgs::Twist2DStamped result;
   result.header.stamp = robot::Time::now();
 
-  geometry_msgs::Pose2D steer_to_baselink_pose;
+  robot_geometry_msgs::Pose2D steer_to_baselink_pose;
   
   if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
     return result;

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -3,7 +3,7 @@
 #include <robot/console.h>
 
 void mkt_algorithm::diff::GoStraight::initialize(
-    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
@@ -18,15 +18,15 @@ void mkt_algorithm::diff::GoStraight::initialize(
     nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
-    std::vector<geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+    std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
       double min_length = 1e10;
       double max_length = 0.0;
       for (size_t i = 0; i < footprint.size(); ++i)
       {
-        const geometry_msgs::Point &p1 = footprint[i];
-        const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+        const robot_geometry_msgs::Point &p1 = footprint[i];
+        const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
         double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
         if (len > max_length)
         {
@@ -125,7 +125,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   // Dynamically adjust look ahead distance based on the speed
   double lookahead_dist = this->getLookAheadDistance(twist);
   // Return false if the transformed global plan is empty
-  geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
+  robot_geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
   lookahead_dist += fabs(front.y);
 
   // Get lookahead point and publish for visualization

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -5,7 +5,7 @@
 #define LIMIT_VEL_THETA 0.33
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
-    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
@@ -20,15 +20,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
     footprint_spec_ = costmap_robot_->getRobotFootprint();
-    std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+    std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
       double min_length = 1e10;
       double max_length = 0.0;
       for (size_t i = 0; i < footprint.size(); ++i)
       {
-        const geometry_msgs::Point &p1 = footprint[i];
-        const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+        const robot_geometry_msgs::Point &p1 = footprint[i];
+        const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
         double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
         if (len > max_length)
         {
@@ -218,15 +218,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     return false;
   }
 
-  std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
+  std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
   if (footprint.size() > 1)
   {
     double min_length = 1e10;
     double max_length = 0.0;
     for (size_t i = 0; i < footprint.size(); ++i)
     {
-      const geometry_msgs::Point &p1 = footprint[i];
-      const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
+      const robot_geometry_msgs::Point &p1 = footprint[i];
+      const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
       double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
       if (len > max_length)
       {
@@ -308,15 +308,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
 
   if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq)
   {
-    const geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
+    const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
     int index;
     for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
     {
-      geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
+      robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
       if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_)
         break;
     }
-    const geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
+    const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
     if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
     {
       const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
@@ -344,11 +344,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
         }
       }
 
-      geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
+      robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
                                       ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
-                                      : robot_nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D());
+                                      : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
 
-      geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
+      robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
       // teb_local_planner::PoseSE2 start_pose(front);
       // teb_local_planner::PoseSE2 goal_pose(back);
@@ -419,7 +419,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   nh_priv_.param("allow_rotate", allow_rotate, false);
 
   // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y);
-  geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
+  robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
   const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
@@ -544,10 +544,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     kf_->update(y, dt, A);
 
     // Check if Kalman filter's estimated velocity exceeds v_max
-    if (avoid_obstacles_ && (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE ||
-                             cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE))
+    if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
+                             cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
+                             cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
+                             cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
     {
       drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
     }
@@ -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;
@@ -826,7 +826,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
 
 bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-    const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
+    const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
     robot_nav_2d_msgs::Path2D &transformed_plan)
 {
   // this method is a slightly modified version of base_local_planner/goal_functions.h
@@ -915,8 +915,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
       // Calculate distance to previous pose
       if (i > 0 && max_plan_length > 0)
       {
-        geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
-        geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
+        robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
+        robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
         plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2));
         if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
         {
@@ -1029,12 +1029,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
   // ss << linear_vel << " ";
   // limit the linear velocity by proximity to obstacles
   if (use_cost_regulated_linear_velocity_scaling_ &&
-      pose_cost != static_cast<double>(costmap_2d::NO_INFORMATION) &&
-      pose_cost != static_cast<double>(costmap_2d::FREE_SPACE))
+      pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
+      pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
   {
     const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
     const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
-                                                std::log(pose_cost / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
+                                                std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
                                             inscribed_radius;
 
     if (min_distance_to_obstacle < cost_scaling_dist_)
@@ -1097,29 +1097,29 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
   // ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
 }
 
-std::vector<geometry_msgs::Point>
-mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution)
+std::vector<robot_geometry_msgs::Point>
+mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
 {
 
   // Dịch sang tọa độ tuyệt đối
-  std::vector<geometry_msgs::Point> abs_footprint;
+  std::vector<robot_geometry_msgs::Point> abs_footprint;
   double cos_th = std::cos(pose.theta);
   double sin_th = std::sin(pose.theta);
   for (auto pt : footprint)
   {
     pt.x *= 1.2;
     pt.y *= 1.2;
-    geometry_msgs::Point abs_pt;
+    robot_geometry_msgs::Point abs_pt;
     abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
     abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
     abs_footprint.push_back(abs_pt);
   }
 
-  std::vector<geometry_msgs::Point> points;
+  std::vector<robot_geometry_msgs::Point> points;
   for (size_t i = 0; i < abs_footprint.size(); ++i)
   {
-    const geometry_msgs::Point &start = abs_footprint[i];
-    const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+    const robot_geometry_msgs::Point &start = abs_footprint[i];
+    const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
 
     double dx = end.x - start.x;
     double dy = end.y - start.y;
@@ -1131,7 +1131,7 @@ mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::P
       if (j == steps && i != abs_footprint.size() - 1)
         continue; // tránh lặp
       double t = static_cast<double>(j) / steps;
-      geometry_msgs::Point pt;
+      robot_geometry_msgs::Point pt;
       pt.x = start.x + t * dx;
       pt.y = start.y + t * dy;
       points.push_back(pt);

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp

@@ -5,7 +5,7 @@
 #include <angles/angles.h>
 
 void mkt_algorithm::diff::RotateToGoal::initialize(
-    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
+    robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {

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_msgs/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>mkt_msgs</name>
+  <version>0.7.10</version>
+  <description>
+    mkt_msgs is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. mkt_msgs
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/mkt_msgs</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt

@@ -33,9 +33,9 @@ set(PACKAGES_DIR
     robot_nav_2d_utils
     nav_core2
     geometry_msgs
-    nav_msgs
-    std_msgs
-    sensor_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    robot_sensor_msgs
     angles
 )
 

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h

@@ -1,7 +1,7 @@
 #ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
 #define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
 #include <robot/node_handle.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_geometry_msgs/Pose2D.h>
 #include <iostream>
 
 namespace mkt_plugins
@@ -24,8 +24,8 @@ class LineGenerator
 {
   /* data */
   struct LineEquation {
-    geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
-    geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
+    robot_geometry_msgs::Pose2D Gi;   /* 𝑮𝒊(𝒙𝒊,𝒚𝒊)  */
+    robot_geometry_msgs::Pose2D Gj;   /* 𝑮𝒋(𝒙𝒋,𝒚𝒋)  */
     float a, b, c ;   /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */  
   };
 
@@ -46,7 +46,7 @@ public:
    * @param start_point   start point of the line
    * @param end_point     end point of the line
    */
-  virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
+  virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);
 
   /**
    * @brief Calculating error 
@@ -54,14 +54,14 @@ public:
    * @param[in] pose
    * @param[in] rev         
    */
-  virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
+  virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);
 
   /**
    * @brief Calculating tolerance 
    * @param query_pose The pose to check
    * @param goal_pose The pose to check against
    */
-  virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
+  virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);
 
   /* Properties */
   LineEquation      Leq_; 

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/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>mkt_plugins</name>
+  <version>0.7.10</version>
+  <description>
+    mkt_plugins is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. mkt_plugins
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/mkt_plugins</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

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/two_points_planner/CMakeLists.txt

@@ -27,13 +27,13 @@ file(GLOB HEADERS "include/two_points_planner/*.h")
 
 # Dependencies packages (internal libraries)
 set(PACKAGES_DIR
-    costmap_2d
+    robot_costmap_2d
     nav_core
     geometry_msgs
-    nav_msgs
-    std_msgs
+    robot_nav_msgs
+    robot_std_msgs
     tf3
-    tf3_geometry_msgs
+    robot_tf3_geometry_msgs
 )
 
 # Tạo thư viện shared (.so)

src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h

@@ -4,9 +4,9 @@
 
 #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_costmap_2d/costmap_2d_robot.h>
 #include <robot/node_handle.h>
 
 namespace two_points_planner
@@ -31,7 +31,7 @@ public:
    * @param  name The name of this planner
    * @param  costmap_robot A pointer to the ROS wrapper of the costmap to use
    */
-  TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
+  TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
 
   /**
    * @brief  Initialization function for the TwoPointsPlanner object
@@ -39,7 +39,7 @@ public:
    * @param  costmap_robot A pointer to the ROS wrapper of the costmap to use
    */
   virtual bool initialize(std::string name, 
-                          costmap_2d::Costmap2DROBOT* costmap_robot);
+                          robot_costmap_2d::Costmap2DROBOT* costmap_robot);
   
   /**
    * @brief Given a goal pose in the world, compute a plan
@@ -48,9 +48,9 @@ public:
    * @param plan The plan... filled by the planner
    * @return True if a valid plan was found, false otherwise
    */
-  virtual bool makePlan(const geometry_msgs::PoseStamped& start, 
-                        const geometry_msgs::PoseStamped& goal, 
-                        std::vector<geometry_msgs::PoseStamped>& plan);
+  virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start, 
+                        const robot_geometry_msgs::PoseStamped& goal, 
+                        std::vector<robot_geometry_msgs::PoseStamped>& plan);
 
   /**
    * @brief Create a new TwoPointsPlanner instance
@@ -71,8 +71,8 @@ protected:
   bool allow_unknown_;
 
   std::string name_;
-  costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
-  std::vector<geometry_msgs::Point> footprint_;
+  robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
+  std::vector<robot_geometry_msgs::Point> footprint_;
   unsigned int current_env_width_;
   unsigned int current_env_height_; 
 

src/Algorithms/Packages/global_planners/two_points_planner/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>two_points_planner</name>
+  <version>0.7.10</version>
+  <description>
+    two_points_planner is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. two_points_planner
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/two_points_planner</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -1,11 +1,11 @@
 #include <robot/console.h>
 #include <two_points_planner/two_points_planner.h>
-#include <costmap_2d/inflation_layer.h>
+#include <robot_costmap_2d/inflation_layer.h>
 #include <boost/dll/alias.hpp>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
 #include <tf3/LinearMath/Quaternion.h>
 #include <tf3/utils.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <robot/time.h>
 #include <robot/console.h>
 #include <cmath>
@@ -15,13 +15,13 @@ namespace two_points_planner
 {
   TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}
 
-  TwoPointsPlanner::TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
+  TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
     : initialized_(false), costmap_robot_(NULL)
   {
     initialize(name, costmap_robot);
   }
 
-  bool TwoPointsPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
+  bool TwoPointsPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
   {
     if (!initialized_)
     {
@@ -66,11 +66,11 @@ namespace two_points_planner
 
     // Bug 
     // // check if the costmap has an inflation layer
-    // for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
+    // for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
     //   layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
     //   ++layer)
     // {
-    //   boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
+    //   boost::shared_ptr<robot_costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<robot_costmap_2d::InflationLayer>(*layer);
     //   if (!inflation_layer)
     //     continue;
 
@@ -83,14 +83,14 @@ namespace two_points_planner
   // This rescales the costmap according to a rosparam which sets the obstacle cost
   unsigned char TwoPointsPlanner::costMapCostToSBPLCost(unsigned char newcost)
   {
-    if (newcost == costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == costmap_2d::NO_INFORMATION))
+    if (newcost == robot_costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == robot_costmap_2d::NO_INFORMATION))
       return lethal_obstacle_;
-    else if (newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    else if (newcost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
       return inscribed_inflated_obstacle_;
     else if (newcost == 0)
       return 0;
-    else if (newcost == costmap_2d::NO_INFORMATION)
-      return  costmap_2d::FREE_SPACE;
+    else if (newcost == robot_costmap_2d::NO_INFORMATION)
+      return  robot_costmap_2d::FREE_SPACE;
     else
     {
       unsigned char cost = newcost;
@@ -100,9 +100,9 @@ namespace two_points_planner
     }
   }
 
-  bool TwoPointsPlanner::makePlan(const geometry_msgs::PoseStamped& start,
-    const geometry_msgs::PoseStamped& goal,
-    std::vector<geometry_msgs::PoseStamped>& plan)
+  bool TwoPointsPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
+    const robot_geometry_msgs::PoseStamped& goal,
+    std::vector<robot_geometry_msgs::PoseStamped>& plan)
   {
     if (!initialized_)
     {
@@ -151,8 +151,8 @@ namespace two_points_planner
     }
     unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
     unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
-    if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-      || end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
+    if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+      || end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
     {
       std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
       return false;
@@ -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/CMakeLists.txt

@@ -29,18 +29,18 @@ include_directories(
 # Dependencies packages (internal libraries)
 set(PACKAGES_DIR
     geometry_msgs
-    nav_msgs
-    std_msgs
-    sensor_msgs
-    visualization_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    robot_sensor_msgs
+    robot_visualization_msgs
     robot_nav_2d_utils
     nav_core2
     mkt_msgs
     score_algorithm
-    costmap_2d
+    robot_costmap_2d
     tf3
-    tf3_geometry_msgs
-    tf3_sensor_msgs
+    robot_tf3_geometry_msgs
+    robot_tf3_sensor_msgs
     robot_cpp
     angles
 )

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h

@@ -28,7 +28,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
@@ -110,7 +110,7 @@ namespace pnkx_local_planner
        * @param tf TFListener pointer
        * @param costmap_robot Costmap pointer
        */
-      void initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+      void initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                       score_algorithm::TrajectoryGenerator::Ptr traj_generator);
 
       bool getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal);
@@ -118,7 +118,7 @@ namespace pnkx_local_planner
       bool geLocalGoal(robot_nav_2d_msgs::Pose2DStamped &local_goal);
 
       bool getLocalPath(const robot_nav_2d_msgs::Pose2DStamped &local_pose,
-                        const robot_nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path);
+                        const robot_nav_2d_msgs::Pose2DStamped &local_goal, robot_nav_msgs::Path &local_path);
 
       bool initialized_;
       bool is_detected_;
@@ -140,8 +140,8 @@ namespace pnkx_local_planner
       nav_core::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
-      geometry_msgs::Vector3 linear_;
-      geometry_msgs::Vector3 angular_;
+      robot_geometry_msgs::Vector3 linear_;
+      robot_geometry_msgs::Vector3 angular_;
 
     private:
       /**
@@ -156,7 +156,7 @@ namespace pnkx_local_planner
 
       robot::NodeHandle nh_, nh_priv_;
       TFListenerPtr tf_;
-      costmap_2d::Costmap2DROBOT *costmap_robot_;
+      robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
       std::function<nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h

@@ -26,7 +26,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h

@@ -33,7 +33,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 setGoalPose - Sets the global goal pose
@@ -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
@@ -183,8 +183,8 @@ namespace pnkx_local_planner
     robot_nav_2d_msgs::Path2D transformed_plan_;
     robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
 
-    costmap_2d::Costmap2D *costmap_;
-    costmap_2d::Costmap2DROBOT *costmap_robot_;
+    robot_costmap_2d::Costmap2D *costmap_;
+    robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
     nav_grid::NavGridInfo info_;
     boost::recursive_mutex configuration_mutex_;
     bool update_costmap_before_planning_;

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h

@@ -26,7 +26,7 @@ namespace pnkx_local_planner
      * @param costmap_robot Costmap pointer
      */
     void initialize(robot::NodeHandle &parent, const std::string &name,
-                    TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
+                    TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
      * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h

@@ -11,9 +11,9 @@
 #include <tf3/buffer_core.h>
 #include <tf3/convert.h>
 #include <tf3/utils.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
-#include <costmap_2d/costmap_2d_robot.h>
+#include <robot_costmap_2d/costmap_2d_robot.h>
 
 namespace pnkx_local_planner
 {
@@ -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).
@@ -42,7 +42,7 @@ namespace pnkx_local_planner
    */
   bool transformGlobalPlan(
       TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
-      const costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
+      const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
       robot_nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false);
 
 }

src/Algorithms/Packages/local_planners/pnkx_local_planner/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>pnkx_local_planner</name>
+  <version>0.7.10</version>
+  <description>
+    pnkx_local_planner is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. pnkx_local_planner
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/pnkx_local_planner</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp

@@ -6,10 +6,10 @@
 #include <robot_nav_2d_utils/parameters.h>
 #include <robot_nav_2d_utils/footprint.h>
 #include <angles/angles.h>
-#include <nav_msgs/Path.h>
-#include <std_msgs/Bool.h>
-#include <std_msgs/UInt16.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <robot_nav_msgs/Path.h>
+#include <robot_std_msgs/Bool.h>
+#include <robot_std_msgs/UInt16.h>
+#include <robot_visualization_msgs/MarkerArray.h>
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
 
@@ -34,7 +34,7 @@ pnkx_local_planner::PNKXDockingLocalPlanner::~PNKXDockingLocalPlanner()
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
-                                                      TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+                                                      TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
 {
   if (!initialized_)
   {
@@ -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);
@@ -490,7 +490,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
               {
                 dkpl_.front()->is_detected_ = true;
                 start_docking_ = true;
-                nav_msgs::Path path;
+                robot_nav_msgs::Path path;
                 robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
                 dkpl_.front()->getLocalPath(local_pose, local_goal, path);
                 this->setPlan(robot_nav_2d_utils::pathToPath(path));
@@ -572,7 +572,7 @@ pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
     docking_nav_.reset();
 }
 
-void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
+void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
                                                                              score_algorithm::TrajectoryGenerator::Ptr traj_generator)
 {
   nh_ = nh;
@@ -731,11 +731,11 @@ 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)
+    const robot_nav_2d_msgs::Pose2DStamped &local_pose, const robot_nav_2d_msgs::Pose2DStamped &local_goal, robot_nav_msgs::Path &local_path)
 {
-  geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
-  geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
-  std::vector<geometry_msgs::PoseStamped> docking_plan;
+  robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
+  robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
+  std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
 
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp

@@ -9,7 +9,7 @@
 // #include <g2o/core/base_unary_edge.h>
 // #include <g2o/core/base_multi_edge.h>
 #include <angles/angles.h>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
 #include "pnkx_local_planner/pnkx_go_straight_local_planner.h"
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
@@ -30,7 +30,7 @@ pnkx_local_planner::PNKXGoStraightLocalPlanner::~PNKXGoStraightLocalPlanner()
     goal_checker_.reset();
 }
 
-void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
+void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
 {
   if (!initialized_)
   {

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -9,7 +9,7 @@
 
 #include <tf3/convert.h>
 #include <tf3/utils.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <mkt_msgs/Trajectory2D.h>
 #include <boost/dll/import.hpp>
@@ -37,7 +37,7 @@ pnkx_local_planner::PNKXLocalPlanner::~PNKXLocalPlanner()
 }
 
 void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
-                                                      TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
+                                                      TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
 {
   if (!initialized_)
   {
@@ -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/pnkx_rotate_local_planner.cpp

@@ -9,7 +9,7 @@
 #include <mkt_msgs/Trajectory2D.h>
 
 #include <angles/angles.h>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/Path.h>
 #include <boost/dll/import.hpp>
 #include <boost/dll/alias.hpp>
 
@@ -29,7 +29,7 @@ pnkx_local_planner::PNKXRotateLocalPlanner::~PNKXRotateLocalPlanner()
     goal_checker_.reset();
 }
 
-void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
+void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
 {
   if (!initialized_)
   {

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp

@@ -3,16 +3,16 @@
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot/console.h>
 #include <data_convert/data_convert.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, robot_nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance)
 {
   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;
@@ -89,7 +89,7 @@ double pnkx_local_planner::getSquareDistance(const geometry_msgs::Pose2D& pose_a
 
 bool pnkx_local_planner::transformGlobalPlan(
   TFListenerPtr tf, const robot_nav_2d_msgs::Path2D& global_plan, const robot_nav_2d_msgs::Pose2DStamped& pose,
-  const costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
+  const robot_costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
   robot_nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame)
 {
   if (global_plan.poses.size() == 0)
@@ -148,15 +148,15 @@ bool pnkx_local_planner::transformGlobalPlan(
 
     double sq_prune_dist = 0.1;
 
-    std::vector<geometry_msgs::Point> footprint = costmap->getRobotFootprint();
+    std::vector<robot_geometry_msgs::Point> footprint = costmap->getRobotFootprint();
     size_t n = footprint.size();
     if (n > 1)
     {
       double max_length = 0.0;
       for (size_t i = 0; i < n; ++i)
       {
-        const geometry_msgs::Point& p1 = footprint[i];
-        const geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
+        const robot_geometry_msgs::Point& p1 = footprint[i];
+        const robot_geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
         double len = std::hypot(p2.x - p1.x, p2.y - p1.y);
         if (len > max_length)
         {

src/Libraries/robot_nav_2d_msgs/CMakeLists.txt

@@ -24,7 +24,7 @@ target_include_directories(robot_nav_2d_msgs
 # Các dependencies này cũng là header-only từ common_msgs
 target_link_libraries(robot_nav_2d_msgs
     INTERFACE
-        std_msgs
+        robot_std_msgs
         geometry_msgs
 )
 
@@ -41,7 +41,7 @@ install(TARGETS robot_nav_2d_msgs
         RUNTIME DESTINATION bin)
 
 # Export targets
-# Bây giờ có thể export dependencies vì std_msgs và geometry_msgs đã được export
+# Bây giờ có thể export dependencies vì robot_std_msgs và geometry_msgs đã được export
 install(EXPORT robot_nav_2d_msgs-targets
         FILE robot_nav_2d_msgs-targets.cmake
         NAMESPACE robot_nav_2d_msgs::
@@ -56,6 +56,6 @@ message(STATUS "Headers found:")
 foreach(hdr ${HEADERS})
     message(STATUS "  - ${hdr}")
 endforeach()
-message(STATUS "Dependencies: std_msgs, geometry_msgs")
+message(STATUS "Dependencies: robot_std_msgs, geometry_msgs")
 message(STATUS "=================================")
 

src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h

@@ -10,7 +10,7 @@
 #include <vector>
 #include <memory>
 
-#include <std_msgs/Header.h>
+#include <robot_std_msgs/Header.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 
 namespace robot_nav_2d_msgs
@@ -30,7 +30,7 @@ struct Path2D_
   (void)_alloc;
     }
 
-   typedef  ::std_msgs::Header  _header_type;
+   typedef  ::robot_std_msgs::Header  _header_type;
   _header_type header;
 
    typedef std::vector< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> >> _poses_type;

src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h

@@ -10,9 +10,9 @@
 #include <vector>
 #include <memory>
 
-#include <std_msgs/Header.h>
+#include <robot_std_msgs/Header.h>
 #include <robot_nav_2d_msgs/ComplexPolygon2D.h>
-#include <std_msgs/ColorRGBA.h>
+#include <robot_std_msgs/ColorRGBA.h>
 
 namespace robot_nav_2d_msgs
 {
@@ -35,13 +35,13 @@ struct Polygon2DCollection_
 
 
 
-   typedef  ::std_msgs::Header  _header_type;
+   typedef  ::robot_std_msgs::Header  _header_type;
   _header_type header;
 
    typedef std::vector< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> >> _polygons_type;
   _polygons_type polygons;
 
-   typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type;
+   typedef std::vector< ::robot_std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_std_msgs::ColorRGBA >> _colors_type;
   _colors_type colors;
 
 

src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h

@@ -10,7 +10,7 @@
 #include <vector>
 #include <memory>
 
-#include <std_msgs/Header.h>
+#include <robot_std_msgs/Header.h>
 #include <robot_nav_2d_msgs/Polygon2D.h>
 
 namespace robot_nav_2d_msgs
@@ -32,7 +32,7 @@ struct Polygon2DStamped_
 
 
 
-   typedef  ::std_msgs::Header  _header_type;
+   typedef  ::robot_std_msgs::Header  _header_type;
   _header_type header;
 
    typedef  ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator>  _polygon_type;

src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h

@@ -10,8 +10,8 @@
 #include <vector>
 #include <memory>
 
-#include <std_msgs/Header.h>
-#include <geometry_msgs/Pose2D.h>
+#include <robot_std_msgs/Header.h>
+#include <robot_geometry_msgs/Pose2D.h>
 
 namespace robot_nav_2d_msgs
 {
@@ -32,10 +32,10 @@ struct Pose2DStamped_
 
 
 
-   typedef  ::std_msgs::Header  _header_type;
+   typedef  ::robot_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_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h

@@ -10,7 +10,7 @@
 #include <vector>
 #include <memory>
 
-#include <std_msgs/Header.h>
+#include <robot_std_msgs/Header.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 
 namespace robot_nav_2d_msgs
@@ -32,7 +32,7 @@ struct Twist2DStamped_
 
 
 
-   typedef  ::std_msgs::Header  _header_type;
+   typedef  ::robot_std_msgs::Header  _header_type;
   _header_type header;
 
    typedef  ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator>  _velocity_type;

src/Libraries/robot_nav_2d_msgs/package.xml

@@ -0,0 +1,26 @@
+<package>
+  <name>robot_nav_2d_msgs</name>
+  <version>0.7.10</version>
+  <description>
+    robot_nav_2d_msgs is the second generation of the transform library, which lets
+    the user keep track of multiple coordinate frames over time. robot_nav_2d_msgs
+    maintains the relationship between coordinate frames in a tree
+    structure buffered in time, and lets the user transform points,
+    vectors, etc between any two coordinate frames at any desired
+    point in time.
+  </description>
+  <author>Tully Foote</author>
+  <author>Eitan Marder-Eppstein</author>
+  <author>Wim Meeussen</author>
+  <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
+  <license>BSD</license>
+
+  <url type="website">http://www.ros.org/wiki/robot_nav_2d_msgs</url>
+    
+  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+  <build_depend>libconsole-bridge-dev</build_depend>
+
+  <run_depend>libconsole-bridge-dev</run_depend>
+
+</package>

src/Libraries/robot_nav_2d_utils/CMakeLists.txt

@@ -12,8 +12,8 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 # Find dependencies
 find_package(console_bridge REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread)
-find_package(xmlrpcpp QUIET)
-if(NOT xmlrpcpp_FOUND)
+find_package(robot_xmlrpcpp QUIET)
+if(NOT robot_xmlrpcpp_FOUND)
     # Try alternative package name
     find_package(XmlRpcCpp QUIET)
 endif()
@@ -28,8 +28,8 @@ target_include_directories(conversions
 target_link_libraries(conversions
     PUBLIC
         robot_nav_2d_msgs
-        geometry_msgs
-        nav_msgs
+        robot_geometry_msgs
+        robot_nav_msgs
         nav_grid
         nav_core2
         robot_cpp
@@ -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
 )
 
@@ -63,21 +63,21 @@ target_include_directories(polygons
         $<INSTALL_INTERFACE:include>
 )
 
-if(xmlrpcpp_FOUND)
-    target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS})
+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
-        PRIVATE ${xmlrpcpp_LIBRARIES})
+        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 ${xmlrpcpp_INCLUDE_DIRS})
+    target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
     target_link_libraries(polygons
-        PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
-        PRIVATE ${xmlrpcpp_LIBRARIES})
+        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
+        PRIVATE ${robot_xmlrpcpp_LIBRARIES})
 endif()
 
 set_target_properties(polygons PROPERTIES
@@ -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, robot_nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
 message(STATUS "=================================")

src/Libraries/robot_nav_2d_utils/README.md

@@ -2,7 +2,7 @@
 A handful of useful utility functions for nav_core2 packages.
  * Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds.
  * [Conversions](doc/Conversions.md) - Tools for converting between `robot_nav_2d_msgs` and other types.
- * OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `robot_nav_2d_msgs::Twist`
+ * OdomSubscriber - subscribes to the standard `robot_nav_msgs::Odometry` message and provides access to the velocity component as a `robot_nav_2d_msgs::Twist`
  * Parameters - a couple ROS parameter patterns
  * PathOps - functions for working with `robot_nav_2d_msgs::Path2D` objects (beyond strict conversion)
  * [Plugin Mux](doc/PluginMux.md) - tool for switching between multiple `pluginlib` plugins

src/Libraries/robot_nav_2d_utils/doc/Conversions.md

@@ -5,35 +5,35 @@
 ## Twist Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
+| `Twist2D twist3Dto2D(robot_geometry_msgs::Twist)` | `robot_geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
 
 ## Point Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)`
-| `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)`
+| `Point2D pointToPoint2D(robot_geometry_msgs::Point)` | `robot_geometry_msgs::Point pointToPoint3D(Point2D)`
+| `Point2D pointToPoint2D(robot_geometry_msgs::Point32)` | `robot_geometry_msgs::Point32 pointToPoint32(Point2D)`
 
 ## Pose Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
-||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
-||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
+| `Pose2DStamped poseStampedToPose2D(robot_geometry_msgs::PoseStamped)` | `robot_geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
+||`robot_geometry_msgs::PoseStamped pose2DToPoseStamped(robot_geometry_msgs::Pose2D, std::string, robot::Time)`|
+||`robot_geometry_msgs::Pose pose2DToPose(robot_geometry_msgs::Pose2D)`|
 | `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
 
 ## Path Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)`
-| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, robot::Time)`
+| `Path2D pathToPath(robot_nav_msgs::Path)` |`robot_nav_msgs::Path pathToPath(Path2D)`
+| `Path2D posesToPath2D(std::vector<robot_geometry_msgs::PoseStamped>)` | `robot_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, `robot_nav_msgs::Path posesToPath(std::vector<robot_geometry_msgs::PoseStamped>)`
 
 ## Polygon Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-| `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
-| `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
+| `Polygon2D polygon3Dto2D(robot_geometry_msgs::Polygon)` |`robot_geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
+| `Polygon2DStamped polygon3Dto2D(robot_geometry_msgs::PolygonStamped)` | `robot_geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
 
 ## Info Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
@@ -42,11 +42,11 @@ Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
 
 | to `nav_grid` info | from `nav_grid` info |
 | -- | -- |
-|`nav_grid::NavGridInfo infoToInfo(nav_msgs::MapMetaData, std::string)` | `nav_msgs::MapMetaData infoToInfo(nav_grid::NavGridInfo)`
+|`nav_grid::NavGridInfo infoToInfo(robot_nav_msgs::MapMetaData, std::string)` | `robot_nav_msgs::MapMetaData infoToInfo(nav_grid::NavGridInfo)`
 
 | 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/doc/PolygonsAndFootprints.md

@@ -1,5 +1,5 @@
 # robot_nav_2d_utils Polygons and Footprints
-This library represents a replacement for [costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
+This library represents a replacement for [robot_costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_costmap_2d/include/robot_costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
 
 ## Polygons and the Parameter Server
 There have historically been three primary ways to specify a polygon/footprint on the parameter server. The first is to simply specify a radius which is converted to a hexadecagon, i.e. polygon with sixteen sides. This can be read with
@@ -21,7 +21,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlr
 
 If the `XmlRpcValue` is a string, it will call the `polygonFromString` method.
 
-The above are the traditional methods that were supported by the original `costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
+The above are the traditional methods that were supported by the original `robot_costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
 
 ```
 robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h

@@ -35,10 +35,10 @@
 #ifndef ROBOT_NAV_2D_UTILS_CONVERSIONS_H
 #define ROBOT_NAV_2D_UTILS_CONVERSIONS_H
 
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/PolygonStamped.h>
+#include <robot_geometry_msgs/Point.h>
+#include <robot_geometry_msgs/Pose.h>
+#include <robot_geometry_msgs/Twist.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Point2D.h>
@@ -47,8 +47,8 @@
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_msgs/NavGridInfo.h>
 #include <robot_nav_2d_msgs/UIntBounds.h>
-#include <nav_msgs/MapMetaData.h>
-#include <nav_msgs/Path.h>
+#include <robot_nav_msgs/MapMetaData.h>
+#include <robot_nav_msgs/Path.h>
 #include <nav_grid/nav_grid.h>
 #include <nav_core2/bounds.h>
 // #include <tf/tf.h>
@@ -58,44 +58,44 @@
 namespace robot_nav_2d_utils
 {
   // Twist Transformations
-  ::geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
-  ::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::geometry_msgs::Twist &cmd_vel);
+  ::robot_geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
+  ::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::robot_geometry_msgs::Twist &cmd_vel);
 
   // Point Transformations
-  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point &point);
-  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point32 &point);
-  ::geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
-  ::geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
+  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point &point);
+  ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point32 &point);
+  ::robot_geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
+  ::robot_geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
 
   // Pose Transformations
   // ::robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose);
-  ::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::geometry_msgs::PoseStamped &pose);
-  ::geometry_msgs::Pose pose2DToPose(const ::geometry_msgs::Pose2D &pose2d);
-  ::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
-  ::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::geometry_msgs::Pose2D &pose2d,
+  ::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::robot_geometry_msgs::PoseStamped &pose);
+  ::robot_geometry_msgs::Pose pose2DToPose(const ::robot_geometry_msgs::Pose2D &pose2d);
+  ::robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
+  ::robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_geometry_msgs::Pose2D &pose2d,
                                                    const ::std::string &frame, const robot::Time &stamp);
 
   // Path Transformations
-  ::robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path);
-  nav_msgs::Path posesToPath(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
-  ::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
-  nav_msgs::Path poses2DToPath(const ::std::vector<::geometry_msgs::Pose2D> &poses,
+  ::robot_nav_2d_msgs::Path2D pathToPath(const robot_nav_msgs::Path &path);
+  robot_nav_msgs::Path posesToPath(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
+  ::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
+  robot_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);
+  robot_nav_msgs::Path pathToPath(const ::robot_nav_2d_msgs::Path2D &path2d);
 
   // Polygon Transformations
-  ::geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
-  ::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::geometry_msgs::Polygon &polygon_3d);
-  ::geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
-  ::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::geometry_msgs::PolygonStamped &polygon_3d);
+  ::robot_geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
+  ::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::robot_geometry_msgs::Polygon &polygon_3d);
+  ::robot_geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
+  ::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::robot_geometry_msgs::PolygonStamped &polygon_3d);
 
   // Info Transformations
   ::robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info);
   nav_grid::NavGridInfo fromMsg(const ::robot_nav_2d_msgs::NavGridInfo &msg);
-  ::geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
-  ::geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info);
-  nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const ::std::string &frame);
-  nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
+  ::robot_geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
+  ::robot_geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info);
+  nav_grid::NavGridInfo infoToInfo(const robot_nav_msgs::MapMetaData &metadata, const ::std::string &frame);
+  robot_nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
 
   // Bounds Transformations
   robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds);

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h

@@ -44,7 +44,7 @@ namespace robot_nav_2d_utils
   /**
    * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius
    *
-   * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
+   * Analagous to robot_costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
    * is present.
    */
   robot_nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle &nh, bool write = true);

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h

@@ -36,7 +36,7 @@
 #define ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H
 
 #include <robot_nav_2d_utils/conversions.h>
-#include <nav_msgs/Odometry.h>
+#include <robot_nav_msgs/Odometry.h>
 #include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <boost/thread/mutex.hpp>
 #include <string>
@@ -63,14 +63,14 @@ namespace robot_nav_2d_utils
     {
       std::string odom_topic;
       // nh.param("odom_topic", odom_topic, default_topic);
-      // odom_sub_ = nh.subscribe<nav_msgs::Odometry>(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1));
+      // odom_sub_ = nh.subscribe<robot_nav_msgs::Odometry>(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1));
     }
 
     inline robot_nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; }
     inline robot_nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; }
 
   protected:
-    void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
+    void odomCallback(const robot_nav_msgs::Odometry::ConstPtr &msg)
     {
       std::cout << "odom received!" << std::endl;
       boost::mutex::scoped_lock lock(odom_mutex_);

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/plugin_mux.h

@@ -37,7 +37,7 @@
 
 #include <ros/ros.h>
 #include <pluginlib/class_loader.h>
-#include <std_msgs/String.h>
+#include <robot_std_msgs/String.h>
 #include <robot_nav_2d_msgs/SwitchPlugin.h>
 #include <map>
 #include <string>
@@ -101,7 +101,7 @@ namespace robot_nav_2d_utils
       current_plugin_ = name;
 
       // Update ROS info
-      std_msgs::String str_msg;
+      robot_std_msgs::String str_msg;
       str_msg.data = current_plugin_;
       current_plugin_pub_.publish(str_msg);
       private_nh_.setParam(ros_name_, current_plugin_);
@@ -225,7 +225,7 @@ namespace robot_nav_2d_utils
       : plugin_loader_(plugin_package, plugin_class), private_nh_("~"), ros_name_(ros_name), switch_callback_(nullptr)
   {
     // Create the latched publisher
-    current_plugin_pub_ = private_nh_.advertise<std_msgs::String>(ros_name_, 1, true);
+    current_plugin_pub_ = private_nh_.advertise<robot_std_msgs::String>(ros_name_, 1, true);
 
     // Load Plugins
     std::string plugin_class_name;

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h

@@ -37,8 +37,8 @@
 
 #include <robot_nav_2d_msgs/Polygon2D.h>
 #include <robot_nav_2d_msgs/ComplexPolygon2D.h>
-#include <geometry_msgs/Pose2D.h>
-#include <xmlrpcpp/XmlRpcValue.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_xmlrpcpp/XmlRpcValue.h>
 #include <vector>
 #include <string>
 
@@ -99,7 +99,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string
  *                       3 or more elements, and the sub-arrays should all have exactly 2 elements
  *                       (x and y coordinates).
  */
-robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc);
+robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc);
 
 /**
  * @brief Create XMLRPC Value for writing the polygon to the parameter server
@@ -107,7 +107,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlr
  * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
  * @return XmlRpcValue
  */
-XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
+robot::XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true);
 
 // /**
 //  * @brief Save a polygon to a parameter
@@ -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;
@@ -138,7 +138,7 @@ namespace robot_nav_2d_utils
   //   return pose;
   // }
 
-  robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path)
+  robot_nav_2d_msgs::Path2D pathToPath(const robot_nav_msgs::Path &path)
   {
     robot_nav_2d_msgs::Path2D path2d;
     path2d.header = path.header;
@@ -152,9 +152,9 @@ namespace robot_nav_2d_utils
     return path2d;
   }
 
-  nav_msgs::Path posesToPath(const std::vector<geometry_msgs::PoseStamped> &poses)
+  robot_nav_msgs::Path posesToPath(const std::vector<robot_geometry_msgs::PoseStamped> &poses)
   {
-    nav_msgs::Path path;
+    robot_nav_msgs::Path path;
     if (poses.empty())
       return path;
 
@@ -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,10 +186,10 @@ namespace robot_nav_2d_utils
     return path;
   }
 
-  // nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
+  // robot_nav_msgs::Path poses2DToPath(const std::vector<robot_geometry_msgs::Pose2D>& poses,
   //                              const std::string& frame, const robot::Time& stamp)
   // {
-  //   nav_msgs::Path path;
+  //   robot_nav_msgs::Path path;
   //   path.poses.resize(poses.size());
   //   path.header.frame_id = frame;
   //   path.header.stamp = stamp;
@@ -200,9 +200,9 @@ namespace robot_nav_2d_utils
   //   return path;
   // }
 
-  nav_msgs::Path pathToPath(const robot_nav_2d_msgs::Path2D &path2d)
+  robot_nav_msgs::Path pathToPath(const robot_nav_2d_msgs::Path2D &path2d)
   {
-    nav_msgs::Path path;
+    robot_nav_msgs::Path path;
     path.header = path2d.header;
     path.poses.resize(path2d.poses.size());
     for (unsigned int i = 0; i < path.poses.size(); i++)
@@ -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;
@@ -275,7 +275,7 @@ namespace robot_nav_2d_utils
     return info;
   }
 
-  nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const std::string &frame)
+  nav_grid::NavGridInfo infoToInfo(const robot_nav_msgs::MapMetaData &metadata, const std::string &frame)
   {
     nav_grid::NavGridInfo info;
     info.frame_id = frame;
@@ -291,26 +291,26 @@ 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;
   }
 
-  nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info)
+  robot_nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info)
   {
-    nav_msgs::MapMetaData metadata;
+    robot_nav_msgs::MapMetaData metadata;
     metadata.resolution = info.resolution;
     metadata.width = info.width;
     metadata.height = info.height;

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

@@ -159,13 +159,13 @@ Polygon2D polygonFromString(const std::string& polygon_string)
 /**
  * @brief Helper function. Convert value to double
  */
-double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value)
+double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value)
 {
-  if (value.getType() == XmlRpc::XmlRpcValue::TypeInt)
+  if (value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt)
   {
     return static_cast<double>(static_cast<int>(value));
   }
-  else if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble)
+  else if (value.getType() == robot::XmlRpc::XmlRpcValue::TypeDouble)
   {
     return static_cast<double>(value);
   }
@@ -178,9 +178,9 @@ double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value)
 /**
  * @brief Helper function. Convert value to double array
  */
-std::vector<double> getNumberVectorFromXMLRPC(XmlRpc::XmlRpcValue& value)
+std::vector<double> getNumberVectorFromXMLRPC(robot::XmlRpc::XmlRpcValue& value)
 {
-  if (value.getType() != XmlRpc::XmlRpcValue::TypeArray)
+  if (value.getType() != robot::XmlRpc::XmlRpcValue::TypeArray)
   {
     throw PolygonParseException("Subarray must have type list.");
   }
@@ -192,15 +192,15 @@ std::vector<double> getNumberVectorFromXMLRPC(XmlRpc::XmlRpcValue& value)
   return array;
 }
 
-Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
+Polygon2D polygonFromXMLRPC(robot::XmlRpc::XmlRpcValue& polygon_xmlrpc)
 {
-  if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
+  if (polygon_xmlrpc.getType() == robot::XmlRpc::XmlRpcValue::TypeString &&
       polygon_xmlrpc != "" && polygon_xmlrpc != "[]")
   {
     return polygonFromString(std::string(polygon_xmlrpc));
   }
 
-  if (polygon_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeStruct)
+  if (polygon_xmlrpc.getType() == robot::XmlRpc::XmlRpcValue::TypeStruct)
   {
     if (!polygon_xmlrpc.hasMember("x") || !polygon_xmlrpc.hasMember("y"))
     {
@@ -212,7 +212,7 @@ Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
   }
 
   // Make sure we have an array of at least 3 elements.
-  if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray)
+  if (polygon_xmlrpc.getType() != robot::XmlRpc::XmlRpcValue::TypeArray)
   {
     std::stringstream err_ss;
     err_ss << "Polygon must be specified as a list of lists. Found object of type " << polygon_xmlrpc.getType()
@@ -229,8 +229,8 @@ Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
   for (int i = 0; i < polygon_xmlrpc.size(); ++i)
   {
     // Make sure each element of the list is an array of size 2. (x and y coordinates)
-    XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i];
-    if (point_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
+    robot::XmlRpc::XmlRpcValue& point_xml = polygon_xmlrpc[i];
+    if (point_xml.getType() != robot::XmlRpc::XmlRpcValue::TypeArray)
     {
       std::stringstream err_ss;
       err_ss << "Each point must be specified as a list. Found object of type " << point_xml.getType() << " instead.";
@@ -274,9 +274,9 @@ Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc)
 /**
  * @brief Helper method to convert a vector of doubles
  */
-XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
+robot::XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
 {
-  XmlRpc::XmlRpcValue xml;
+  robot::XmlRpc::XmlRpcValue xml;
   xml.setSize(array.size());
   for (unsigned int i = 0; i < array.size(); ++i)
   {
@@ -285,9 +285,9 @@ XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector<double>& array)
   return xml;
 }
 
-XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
+robot::XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays)
 {
-  XmlRpc::XmlRpcValue xml;
+  robot::XmlRpc::XmlRpcValue xml;
   if (array_of_arrays)
   {
     xml.setSize(polygon.points.size());
@@ -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

@@ -36,13 +36,13 @@
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_utils/conversions.h>
-#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
+#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 #include <string>
 
 namespace robot_nav_2d_utils
 {
   bool transformPose(const TFListenerPtr tf, const ::std::string frame,
-                     const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose,
+                     const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
                      const bool extrapolation_fallback)
   {
     // if (in_pose.header.frame_id == frame)
@@ -60,7 +60,7 @@ namespace robot_nav_2d_utils
     // {
     //   if (!extrapolation_fallback)
     //     throw;
-    //   geometry_msgs::PoseStamped latest_in_pose;
+    //   robot_geometry_msgs::PoseStamped latest_in_pose;
     //   latest_in_pose.header.frame_id = in_pose.header.frame_id;
     //   latest_in_pose.pose = in_pose.pose;
     //   tf->transform(latest_in_pose, out_pose, frame);
@@ -78,8 +78,8 @@ namespace robot_nav_2d_utils
                      const ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_nav_2d_msgs::Pose2DStamped &out_pose,
                      const bool extrapolation_fallback)
   {
-    geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
-    geometry_msgs::PoseStamped out_3d_pose;
+    robot_geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
+    robot_geometry_msgs::PoseStamped out_3d_pose;
 
     bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, extrapolation_fallback);
     if (ret)
@@ -89,7 +89,7 @@ namespace robot_nav_2d_utils
     return ret;
   }
 
-  ::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
+  ::robot_geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
                                              const ::std::string &frame_id)
   {
     robot_nav_2d_msgs::Pose2DStamped local_pose;

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/mainpage.dox

@@ -29,7 +29,7 @@ Some packages that implement this interface:
 - tf2_eigen
 - tf2_geometry_msgs
 - tf2_kdl
-- tf2_sensor_msgs
+- tf2_robot_sensor_msgs
 
 More documentation for the conversion interface is available on the <A HREF="http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions">ROS Wiki</A>.
 

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

src/Libraries/tf3/test/static_cache_test.cpp

@@ -31,7 +31,7 @@
 #include <tf3/time_cache.h>
 #include <stdexcept>
 
-#include <geometry_msgs/TransformStamped.h>
+#include <robot_geometry_msgs/TransformStamped.h>
 
 #include <cmath>