update ros

CMakeLists.txt

@@ -78,20 +78,20 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
-if (NOT TARGET nav_core)
+if (NOT TARGET robot_nav_core)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()
 
 if (NOT TARGET nav_grid)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) 
 endif()
 
-if (NOT TARGET nav_core2)
+if (NOT TARGET robot_nav_core2)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core2)
 endif()
 
-if (NOT TARGET nav_core_adapter)
+if (NOT TARGET robot_nav_core_adapter)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core_adapter)
 endif()
 
 if (NOT TARGET move_base_core)

src/APIs/c_api/src/nav_c_api.cpp

@@ -449,12 +449,12 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou
 
     try {
         robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
-        robot::move_base_core::NavFeedback cpp_feedback;
+        if (nav->getFeedback() != nullptr) {
-        if (nav->getFeedback(cpp_feedback)) {
+            cpp_to_c_nav_feedback(*nav->getFeedback(), out_feedback);
-            cpp_to_c_nav_feedback(cpp_feedback, out_feedback);
             return true;
-        }
+        } else {
             return false;
+        }
     } catch (...) {
         return false;
     }

src/Algorithms/Cores/score_algorithm/CMakeLists.txt

@@ -1,5 +1,4 @@
 cmake_minimum_required(VERSION 3.10)
-
 # Tên dự án
 project(score_algorithm VERSION 1.0.0 LANGUAGES CXX)
 
@@ -11,50 +10,128 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-set(PACKAGES_DIR robot_nav_2d_msgs robot_nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles)
+# ========================================================
+# Find Packages
+# ========================================================
 
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building score_algorithm with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building score_algorithm with Standalone CMake")
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  ## Find catkin macros and libraries
+  find_package(catkin REQUIRED COMPONENTS
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
+    robot_nav_core2
+    robot_nav_core
+    robot_cpp
+    mkt_msgs
+    angles
+  )
+endif()
+
+## System dependencies
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  ## The catkin_package macro generates cmake config files for your package
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES score_algorithm
+    CATKIN_DEPENDS robot_nav_2d_msgs robot_nav_2d_utils robot_nav_core robot_nav_core2 robot_cpp mkt_msgs
+  )
+endif()
+
 # Thư mục include
 include_directories(
         ${PROJECT_SOURCE_DIR}/include
 )
-# Tạo thư viện shared (.so)
 
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+# ========================================================
+# Build
+# ========================================================
+
+# Tạo thư viện shared (.so)
 add_library(score_algorithm src/score_algorithm.cpp)
+
+# Link libraries
+if(BUILDING_WITH_CATKIN)
   target_link_libraries(score_algorithm 
     PUBLIC
-    ${PACKAGES_DIR}
+      ${catkin_LIBRARIES}
-    robot_cpp
     PRIVATE
       Boost::filesystem
       Boost::system
-
   )
+else()
+  # Standalone mode: link dependencies directly
+  target_link_libraries(score_algorithm 
+    PUBLIC
+      robot_nav_2d_msgs
+      robot_nav_2d_utils
+      robot_nav_core2
+      robot_nav_core2
+      robot_cpp
+      mkt_msgs
+      angles
+    PRIVATE
+      Boost::filesystem
+      Boost::system
+  )
+endif()
+
 set_target_properties(score_algorithm PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
 )
 
+## Add cmake target dependencies
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(score_algorithm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
 target_include_directories(score_algorithm PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 
-# Cài đặt header files
+# ========================================================
-install(DIRECTORY include/score_algorithm 
+# Installation
-        DESTINATION include
+# ========================================================
-        FILES_MATCHING PATTERN "*.h")
 
-# Cài đặt library
+# Export target trong mọi trường hợp để các target khác có thể export và phụ thuộc vào nó
 install(TARGETS score_algorithm  
         EXPORT score_algorithm-targets
         LIBRARY DESTINATION lib
         ARCHIVE DESTINATION lib
         RUNTIME DESTINATION bin)
 
-# Export targets
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (chỉ cho standalone mode)
+  install(DIRECTORY include/score_algorithm 
+          DESTINATION include
+          FILES_MATCHING PATTERN "*.h")
+
+  # Export targets (chỉ cho standalone mode)
   install(EXPORT score_algorithm-targets
           FILE score_algorithm-targets.cmake
           DESTINATION lib/cmake/score_algorithm)
+else()
+  # Khi build với Catkin, vẫn cần export để các target khác có thể export
+  install(EXPORT score_algorithm-targets
+          FILE score_algorithm-targets.cmake
+          DESTINATION lib/cmake/score_algorithm)
+endif()
 
 # Tùy chọn build
 option(BUILD_SHARED_LIBS "Build shared libraries" ON)

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

@@ -7,9 +7,9 @@
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
-#include <nav_core2/costmap.h>
+#include <robot_nav_core2/costmap.h>
 
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot_geometry_msgs/PoseArray.h>

src/Algorithms/Cores/score_algorithm/package.xml

@@ -19,8 +19,19 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>robot_nav_2d_msgs</build_depend>
-
+  <build_depend>robot_nav_2d_utils</build_depend>
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <build_depend>robot_nav_core</build_depend>
+  <build_depend>robot_nav_core2</build_depend>
+  <build_depend>robot_cpp</build_depend>
+  <build_depend>mkt_msgs</build_depend>
+  <build_depend>angles</build_depend>
 
+  <run_depend>robot_nav_2d_msgs</run_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
+  <run_depend>robot_nav_core</run_depend>
+  <run_depend>robot_nav_core2</run_depend>
+  <run_depend>robot_cpp</run_depend>
+  <run_depend>mkt_msgs</run_depend>
+  <run_depend>angles</run_depend>
 </package>

src/Algorithms/Libraries/angles/CMakeLists.txt

@@ -1,10 +1,37 @@
 cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building angles with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building angles with Standalone CMake")
+endif()
+
 project(angles VERSION 1.0.0 LANGUAGES CXX)
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED)
+  catkin_package(
+    INCLUDE_DIRS include
+    # Header-only library; keep LIBRARIES for visibility when exporting
+    LIBRARIES angles
+  )
+endif()
+
 # Create interface library (header-only)
 add_library(angles INTERFACE)
 
@@ -14,18 +41,27 @@ target_include_directories(angles INTERFACE
   $<INSTALL_INTERFACE:include>
 )
 
-# Install headers
+if(BUILDING_WITH_CATKIN)
-install(DIRECTORY include/angles
+  add_dependencies(angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-  DESTINATION include
+endif()
-  FILES_MATCHING PATTERN "*.h"
+
-)
+# ========================================================
+# Installation
+# ========================================================
 
-# Install target
 install(TARGETS angles
   EXPORT angles-targets
   INCLUDES DESTINATION include
 )
 
+if(NOT BUILDING_WITH_CATKIN)
+  # Install headers (standalone)
+  install(DIRECTORY include/angles
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+  )
+endif()
+
 # Export targets
 install(EXPORT angles-targets
   FILE angles-targets.cmake

src/Algorithms/Libraries/angles/package.xml

@@ -19,8 +19,4 @@
     
   <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/CMakeLists.txt

@@ -1,4 +1,18 @@
 cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building kalman with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building kalman with Standalone CMake")
+endif()
+
 project(kalman VERSION 1.0.0 LANGUAGES CXX)
 
 # Chuẩn C++
@@ -10,15 +24,30 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# ========================================================
 # Find dependencies
+# ========================================================
 find_package(Eigen3 REQUIRED)
 
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED)
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES kalman
+    DEPENDS Eigen3
+  )
+endif()
+
 # Thư mục include
 include_directories(
     ${PROJECT_SOURCE_DIR}/include
     ${EIGEN3_INCLUDE_DIRS}
 )
 
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
 # Tìm tất cả file source
 file(GLOB SOURCES "src/kalman.cpp")
 file(GLOB HEADERS "include/kalman/kalman.h")
@@ -30,6 +59,7 @@ add_library(kalman SHARED ${SOURCES} ${HEADERS})
 target_link_libraries(kalman
     PUBLIC
         Eigen3::Eigen
+        $<$<BOOL:${BUILDING_WITH_CATKIN}>:${catkin_LIBRARIES}>
 )
 
 set_target_properties(kalman PROPERTIES
@@ -44,6 +74,10 @@ target_include_directories(kalman
         ${EIGEN3_INCLUDE_DIRS}
 )
 
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(kalman ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
 # Tạo executable cho test
 add_executable(kalman_node src/kalman-test.cpp)
 target_link_libraries(kalman_node
@@ -56,11 +90,9 @@ set_target_properties(kalman_node PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
 )
 
-# Cài đặt header files
+# ========================================================
-install(DIRECTORY include/kalman
+# Installation
-    DESTINATION include
+# ========================================================
-    FILES_MATCHING PATTERN "*.h"
-)
 
 # Cài đặt library
 install(TARGETS kalman
@@ -75,6 +107,14 @@ install(TARGETS kalman_node
     RUNTIME DESTINATION bin
 )
 
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/kalman
+      DESTINATION include
+      FILES_MATCHING PATTERN "*.h"
+  )
+endif()
+
 # Export targets
 install(EXPORT kalman-targets
     FILE kalman-targets.cmake

src/Algorithms/Libraries/kalman/package.xml

@@ -19,8 +19,4 @@
     
   <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

@@ -1,4 +1,18 @@
 cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building mkt_algorithm with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building mkt_algorithm with Standalone CMake")
+endif()
+
 project(mkt_algorithm VERSION 1.0.0 LANGUAGES CXX)
 
 # Chuẩn C++
@@ -10,16 +24,38 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# ========================================================
 # Find dependencies
+# ========================================================
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(Eigen3 REQUIRED)
 
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    geometry_msgs
+    score_algorithm
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
+    kalman
+    angles
+    nav_grid
+    robot_costmap_2d
+    robot_sensor_msgs
+    robot_std_msgs
+    robot_cpp
+  )
+endif()
+
 # Thư mục include
 include_directories(
     ${PROJECT_SOURCE_DIR}/include
     ${EIGEN3_INCLUDE_DIRS}
 )
 
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
 # Dependencies packages
 set(PACKAGES_DIR
     geometry_msgs
@@ -49,6 +85,14 @@ file(GLOB DIFF_HEADERS
 add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
 
 # Link libraries
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(mkt_algorithm_diff
+      PUBLIC
+          ${catkin_LIBRARIES}
+          Boost::system
+          Eigen3::Eigen
+  )
+else()
   target_link_libraries(mkt_algorithm_diff
       PUBLIC
           ${PACKAGES_DIR}
@@ -56,6 +100,11 @@ target_link_libraries(mkt_algorithm_diff
           Boost::system
           Eigen3::Eigen
   )
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(mkt_algorithm_diff ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
 
 set_target_properties(mkt_algorithm_diff PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -69,11 +118,9 @@ target_include_directories(mkt_algorithm_diff
         ${EIGEN3_INCLUDE_DIRS}
 )
 
-# Cài đặt header files
+# ========================================================
-install(DIRECTORY include/mkt_algorithm
+# Installation
-    DESTINATION include
+# ========================================================
-  FILES_MATCHING PATTERN "*.h"
-)
 
 # Cài đặt library
 install(TARGETS mkt_algorithm_diff
@@ -83,6 +130,14 @@ install(TARGETS mkt_algorithm_diff
     RUNTIME DESTINATION bin
 )
 
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/mkt_algorithm
+      DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+  )
+endif()
+
 # Cài đặt plugins.xml (nếu cần cho pluginlib)
 if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
     install(FILES plugins.xml

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

@@ -27,7 +27,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

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

@@ -25,7 +25,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

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

@@ -24,7 +24,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

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

@@ -25,7 +25,7 @@ public:
    * @brief Initialize parameters as needed
    * @param nh NodeHandle to read parameters from
    */
-  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+  virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
 
   /**
    * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

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

@@ -8,8 +8,8 @@
 #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 <robot_nav_core2/exceptions.h>
-#include <nav_core2/costmap.h>
+#include <robot_nav_core2/costmap.h>
 #include <nav_grid/coordinate_conversion.h>
 #include <angles/angles.h>
 #include <tf3/exceptions.h>

src/Algorithms/Libraries/mkt_algorithm/package.xml

@@ -18,9 +18,27 @@
   <url type="website">http://www.ros.org/wiki/mkt_algorithm</url>
     
   <buildtool_depend version_gte="0.5.68">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>robot_nav_2d_utils</build_depend>
+  <build_depend>kalman</build_depend>
+  <build_depend>angles</build_depend>
+  <build_depend>nav_grid</build_depend>
+  <build_depend>robot_costmap_2d</build_depend>
+  <build_depend>robot_sensor_msgs</build_depend>
+  <build_depend>robot_std_msgs</build_depend>
+  <build_depend>robot_cpp</build_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <run_depend>geometry_msgs</run_depend>
-
+  <run_depend>score_algorithm</run_depend>
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <run_depend>robot_nav_2d_msgs</run_depend>
-
+  <run_depend>robot_nav_2d_utils</run_depend>
+  <run_depend>kalman</run_depend>
+  <run_depend>angles</run_depend>
+  <run_depend>nav_grid</run_depend>
+  <run_depend>robot_costmap_2d</run_depend>
+  <run_depend>robot_sensor_msgs</run_depend>
+  <run_depend>robot_std_msgs</run_depend>
+  <run_depend>robot_cpp</run_depend>
 </package>

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

@@ -6,7 +6,7 @@
 namespace  mkt_algorithm
 {
 
-void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
 {
   nh_       = robot::NodeHandle(nh, name);
   name_     = name;

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

@@ -11,7 +11,7 @@
 namespace  mkt_algorithm
 {
 
-void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
 {
   nh_priv_  = robot::NodeHandle(nh, name);
   nh_       = robot::NodeHandle("~");
@@ -257,7 +257,7 @@ bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const r
                           const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal, 
                           double &target_direction)
 {
-  const nav_core2::Costmap &costmap = *costmap_;
+  const robot_nav_core2::Costmap &costmap = *costmap_;
   const nav_grid::NavGridInfo &info = costmap.getInfo();
 
   if (global_plan.poses.empty())
@@ -355,7 +355,7 @@ bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const r
     // double g_y = plan[i].y;
     // unsigned int map_x, map_y;
     // if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
-    //     && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
+    //     && costmap(map_x, map_y) != robot_nav_core2::Costmap::NO_INFORMATION)
     // { 
       // Still on the costmap. Continue.
       double distance = fabs(robot_nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
@@ -394,7 +394,7 @@ bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const r
   //   double g_x = plan[i].x;
   //   double g_y = plan[i].y;
   //   unsigned int map_x, map_y;
-  //   if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
+  //   if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != robot_nav_core2::Costmap::NO_INFORMATION)
   //   {
   //     // Still on the costmap. Continue.
   //     last_valid_index = i;

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

@@ -6,7 +6,7 @@
 namespace  mkt_algorithm
 {
 
-void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
+void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
 {
   nh_       = robot::NodeHandle(nh, name);
   name_     = name;

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

@@ -709,7 +709,7 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
 {
   if (global_plan.poses.empty())
   {
-    throw nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
+    throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
   }
 
   if ((unsigned)global_plan.poses.size() < 2)
@@ -790,7 +790,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
     robot_nav_2d_msgs::Pose2DStamped robot;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
     {
-      throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
@@ -855,7 +855,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped robot_pose;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
     {
-      throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     // we'll discard points on the plan that are outside the local costmap

src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt

@@ -1,5 +1,4 @@
 cmake_minimum_required(VERSION 3.10)
-
 # Tên dự án
 project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX)
 
@@ -7,6 +6,25 @@ project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
+# ========================================================
+# Find Packages
+# ========================================================
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building mkt_msgs with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building mkt_msgs with Standalone CMake")
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  ## Find catkin macros and libraries
+  find_package(catkin REQUIRED COMPONENTS
+    robot_nav_2d_msgs
+    geometry_msgs
+  )
+endif()
+
 # Cho phép các project khác include được header của mkt_msgs
 set(mkt_msgs_INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}/include
@@ -17,9 +35,34 @@ include_directories(
         ${PROJECT_SOURCE_DIR}/include
 )
 
-# Tạo INTERFACE library (header-only)
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+  ## The catkin_package macro generates cmake config files for your package
+  catkin_package(
+    INCLUDE_DIRS include
+    # Header-only library so LIBRARIES is optional; keep for visibility
+    LIBRARIES mkt_msgs
+    CATKIN_DEPENDS robot_nav_2d_msgs geometry_msgs
+  )
+endif()
+
+# ========================================================
+# Build (header-only INTERFACE)
+# ========================================================
 add_library(mkt_msgs INTERFACE)
+
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(mkt_msgs INTERFACE ${catkin_LIBRARIES})
+  add_dependencies(mkt_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+else()
   target_link_libraries(mkt_msgs INTERFACE robot_nav_2d_msgs geometry_msgs)
+endif()
 
 # Set include directories
 target_include_directories(mkt_msgs
@@ -28,10 +71,9 @@ target_include_directories(mkt_msgs
         $<INSTALL_INTERFACE:include>
 )
 
-# Cài đặt header files
+# ========================================================
-install(DIRECTORY include/mkt_msgs 
+# Installation
-        DESTINATION include
+# ========================================================
-        FILES_MATCHING PATTERN "*.h")
 
 install(TARGETS mkt_msgs 
         EXPORT mkt_msgs-targets
@@ -39,7 +81,12 @@ install(TARGETS mkt_msgs
         ARCHIVE DESTINATION lib
         RUNTIME DESTINATION bin)
 
-# Export targets
+# Cài đặt header files (cả catkin và standalone)
+install(DIRECTORY include/mkt_msgs 
+        DESTINATION include
+        FILES_MATCHING PATTERN "*.h")
+
+# Export targets (cho cả hai chế độ)
 install(EXPORT mkt_msgs-targets
          FILE mkt_msgs-targets.cmake
         DESTINATION lib/cmake/mkt_msgs)

src/Algorithms/Libraries/mkt_msgs/package.xml

@@ -19,8 +19,9 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
   
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>robot_nav_2d_msgs</build_depend>
-
+  <build_depend>geometry_msgs</build_depend>
-  <run_depend>libconsole-bridge-dev</run_depend>
 
+  <run_depend>robot_nav_2d_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
 </package>

src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt

@@ -1,4 +1,18 @@
 cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building mkt_plugins with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building mkt_plugins with Standalone CMake")
+endif()
+
 project(mkt_plugins VERSION 1.0.0 LANGUAGES CXX)
 
 # Chuẩn C++
@@ -11,9 +25,27 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# ========================================================
+# Find dependencies
+# ========================================================
+
 # Find system dependencies
 find_package(Boost REQUIRED COMPONENTS system)
 
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    score_algorithm
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
+    robot_nav_core2
+    geometry_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    robot_sensor_msgs
+    angles
+  )
+endif()
+
 # Flags biên dịch
 # Warning flags - disabled to suppress warnings during build
 # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
@@ -21,17 +53,33 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES mkt_plugins_goal_checker mkt_plugins_standard_traj_generator
+    CATKIN_DEPENDS score_algorithm robot_nav_2d_msgs robot_nav_2d_utils robot_nav_core2 geometry_msgs robot_nav_msgs robot_std_msgs robot_sensor_msgs angles
+  )
+endif()
+
 # Thư mục include
 include_directories(
     ${PROJECT_SOURCE_DIR}/include
 )
 
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
 # Dependencies packages (internal libraries)
 set(PACKAGES_DIR
     score_algorithm
     robot_nav_2d_msgs
     robot_nav_2d_utils
-    nav_core2
+    robot_nav_core2
     geometry_msgs
     robot_nav_msgs
     robot_std_msgs
@@ -50,10 +98,18 @@ add_library(${PROJECT_NAME}_goal_checker SHARED
     src/equation_line.cpp
 )
 
+# Link libraries
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME}_goal_checker
+      PUBLIC ${catkin_LIBRARIES}
+      PRIVATE Boost::boost
+  )
+else()
   target_link_libraries(${PROJECT_NAME}_goal_checker
       PUBLIC ${PACKAGES_DIR}
       PRIVATE Boost::boost
   )
+endif()
 
 set_target_properties(${PROJECT_NAME}_goal_checker PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -73,10 +129,17 @@ add_library(${PROJECT_NAME}_standard_traj_generator SHARED
     src/limited_accel_generator.cpp
 )
 
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME}_standard_traj_generator
+      PUBLIC ${catkin_LIBRARIES}
+      PRIVATE Boost::boost
+  )
+else()
   target_link_libraries(${PROJECT_NAME}_standard_traj_generator
       PUBLIC ${PACKAGES_DIR}
       PRIVATE Boost::boost
   )
+endif()
 
 set_target_properties(${PROJECT_NAME}_standard_traj_generator PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -88,17 +151,15 @@ target_include_directories(${PROJECT_NAME}_standard_traj_generator
         $<INSTALL_INTERFACE:include>
 )
 
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME}_goal_checker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME}_standard_traj_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
 #############
 ## Install ##
 #############
 
-# Cài đặt header files
-install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION include/${PROJECT_NAME}
-    FILES_MATCHING PATTERN "*.h"
-    PATTERN ".svn" EXCLUDE
-)
-
 # Cài đặt libraries
 install(TARGETS 
     ${PROJECT_NAME}_standard_traj_generator 
@@ -109,6 +170,15 @@ install(TARGETS
     RUNTIME DESTINATION bin
 )
 
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/${PROJECT_NAME}/
+      DESTINATION include/${PROJECT_NAME}
+      FILES_MATCHING PATTERN "*.h"
+      PATTERN ".svn" EXCLUDE
+  )
+endif()
+
 # Export targets
 install(EXPORT ${PROJECT_NAME}-targets
     FILE ${PROJECT_NAME}-targets.cmake

src/Algorithms/Libraries/mkt_plugins/package.xml

@@ -19,8 +19,31 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>score_algorithm</build_depend>
+  <run_depend>score_algorithm</run_depend>
 
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <build_depend>robot_nav_2d_msgs</build_depend>
+  <run_depend>robot_nav_2d_msgs</run_depend>
+
+  <build_depend>robot_nav_2d_utils</build_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
+
+  <build_depend>robot_nav_core2</build_depend>
+  <run_depend>robot_nav_core2</run_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>robot_sensor_msgs</build_depend>
+  <run_depend>robot_sensor_msgs</run_depend>
+
+  <build_depend>angles</build_depend>
+  <run_depend>angles</run_depend>
 
 </package>

src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp

@@ -2,7 +2,7 @@
 #include <mkt_plugins/limited_accel_generator.h>
 #include <robot_nav_2d_utils/parameters.h>
 #include <boost/dll/alias.hpp>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 #include <vector>
 
 namespace mkt_plugins

src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp

@@ -2,7 +2,7 @@
 #include <mkt_plugins/xy_theta_iterator.h>
 #include <robot_nav_2d_utils/parameters.h>
 #include <boost/dll/alias.hpp>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 #include <algorithm>
 #include <cmath>
 #include <memory>

src/Algorithms/Packages/global_planners/custom_planner

@@ -1 +1 @@
-Subproject commit 60e9c5673f8fd646d628f843ef73e71d1d9b2a17
+Subproject commit 0b01c22019b66c905d6d67611d333742e1e37a55

src/Algorithms/Packages/global_planners/dock_planner

@@ -1 +1 @@
-Subproject commit b5a1b7b6d8367b5812b32f2ca51bbed6e2a4c99a
+Subproject commit 22aa83fe5a66f5c090bcb8c1fafb41cfe777ebc1

src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt

@@ -1,5 +1,18 @@
 cmake_minimum_required(VERSION 3.10)
 
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building two_points_planner with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building two_points_planner with Standalone CMake")
+endif()
+
 # Tên dự án
 project(two_points_planner VERSION 1.0.0 LANGUAGES CXX)
 
@@ -13,14 +26,47 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# ========================================================
+# Find dependencies
+# ========================================================
+
 # Find system dependencies
 find_package(Boost REQUIRED COMPONENTS filesystem system)
 
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    robot_costmap_2d
+    robot_nav_core
+    geometry_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    tf3
+    robot_tf3_geometry_msgs
+    robot_cpp
+  )
+endif()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES two_points_planner
+    CATKIN_DEPENDS robot_costmap_2d robot_nav_core geometry_msgs robot_nav_msgs robot_std_msgs tf3 robot_tf3_geometry_msgs robot_cpp
+  )
+endif()
+
 # Thư mục include
 include_directories(
     ${PROJECT_SOURCE_DIR}/include
 )
 
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
 # Tìm tất cả file source
 file(GLOB SOURCES "src/two_points_planner.cpp")
 file(GLOB HEADERS "include/two_points_planner/*.h")
@@ -28,7 +74,7 @@ file(GLOB HEADERS "include/two_points_planner/*.h")
 # Dependencies packages (internal libraries)
 set(PACKAGES_DIR
     robot_costmap_2d
-    nav_core
+    robot_nav_core
     geometry_msgs
     robot_nav_msgs
     robot_std_msgs
@@ -40,11 +86,23 @@ set(PACKAGES_DIR
 add_library(two_points_planner SHARED ${SOURCES} ${HEADERS})
 
 # Link libraries
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(two_points_planner
+      PUBLIC ${catkin_LIBRARIES}
+      PRIVATE Boost::filesystem
+      PRIVATE Boost::system
+  )
+else()
   target_link_libraries(two_points_planner
       PUBLIC ${PACKAGES_DIR}
       PUBLIC robot_cpp
       PRIVATE Boost::boost
   )
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(two_points_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
 
 set_target_properties(two_points_planner PROPERTIES
     LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -57,12 +115,9 @@ target_include_directories(two_points_planner
         $<INSTALL_INTERFACE:include>
 )
 
-# Cài đặt header files
+# ========================================================
-install(DIRECTORY include/two_points_planner
+# Installation
-    DESTINATION include
+# ========================================================
-    FILES_MATCHING PATTERN "*.h"
-    PATTERN ".svn" EXCLUDE
-)
 
 # Cài đặt library
 install(TARGETS two_points_planner
@@ -71,6 +126,16 @@ install(TARGETS two_points_planner
     ARCHIVE DESTINATION lib
     RUNTIME DESTINATION bin
 )
+
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/two_points_planner
+      DESTINATION include
+      FILES_MATCHING PATTERN "*.h"
+      PATTERN ".svn" EXCLUDE
+  )
+endif()
+
 # Export targets
 install(EXPORT two_points_planner-targets
     FILE two_points_planner-targets.cmake

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

@@ -5,7 +5,7 @@
 #include <iostream>
 #include <vector>
 #include <robot_geometry_msgs/PoseStamped.h>
-#include <nav_core/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot/node_handle.h>
 
@@ -16,7 +16,7 @@ namespace two_points_planner
  * @class TwoPointsPlanner
  * @brief Plugin-based flexible global_planner
  */
-class TwoPointsPlanner : public nav_core::BaseGlobalPlanner
+class TwoPointsPlanner : public robot_nav_core::BaseGlobalPlanner
 {
 public:
   /**
@@ -56,7 +56,7 @@ public:
    * @brief Create a new TwoPointsPlanner instance
    * @return A pointer to the new TwoPointsPlanner instance
    */
-  static nav_core::BaseGlobalPlanner::Ptr create();
+  static robot_nav_core::BaseGlobalPlanner::Ptr create();
 
 protected:
 

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

@@ -19,8 +19,28 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
 
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <build_depend>robot_nav_core</build_depend>
+  <run_depend>robot_nav_core</run_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>tf3</build_depend>
+  <run_depend>tf3</run_depend>
+
+  <build_depend>robot_tf3_geometry_msgs</build_depend>
+  <run_depend>robot_tf3_geometry_msgs</run_depend>
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
 
 </package>

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

@@ -211,7 +211,7 @@ namespace two_points_planner
     return true;
   }
 
-  nav_core::BaseGlobalPlanner::Ptr TwoPointsPlanner::create()
+  robot_nav_core::BaseGlobalPlanner::Ptr TwoPointsPlanner::create()
   {
     return std::make_shared<TwoPointsPlanner>();
   }

src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt

@@ -1,4 +1,18 @@
 cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building pnkx_local_planner with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building pnkx_local_planner with Standalone CMake")
+endif()
+
 project(pnkx_local_planner VERSION 1.0.0 LANGUAGES CXX)
 
 # Chuẩn C++
@@ -11,9 +25,33 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
+# ========================================================
+# Find dependencies
+# ========================================================
+
 # Find system dependencies
 find_package(Boost REQUIRED COMPONENTS system)
 
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    geometry_msgs
+    robot_nav_msgs
+    robot_std_msgs
+    robot_sensor_msgs
+    robot_visualization_msgs
+    robot_nav_2d_utils
+    robot_nav_core2
+    mkt_msgs
+    score_algorithm
+    robot_costmap_2d
+    tf3
+    robot_tf3_geometry_msgs
+    robot_tf3_sensor_msgs
+    robot_cpp
+    angles
+  )
+endif()
+
 # Flags biên dịch
 # Warning flags - disabled to suppress warnings during build
 # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
@@ -21,11 +59,27 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
 set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES pnkx_local_planner pnkx_local_planner_utils
+    CATKIN_DEPENDS geometry_msgs robot_nav_msgs robot_std_msgs robot_sensor_msgs robot_visualization_msgs robot_nav_2d_utils robot_nav_core2 mkt_msgs score_algorithm robot_costmap_2d tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs robot_cpp angles
+  )
+endif()
+
 # Thư mục include
 include_directories(
     ${PROJECT_SOURCE_DIR}/include
 )
 
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
 # Dependencies packages (internal libraries)
 set(PACKAGES_DIR
     geometry_msgs
@@ -34,7 +88,7 @@ set(PACKAGES_DIR
     robot_sensor_msgs
     robot_visualization_msgs
     robot_nav_2d_utils
-    nav_core2
+    robot_nav_core2
     mkt_msgs
     score_algorithm
     robot_costmap_2d
@@ -51,10 +105,17 @@ add_library(${PROJECT_NAME}_utils SHARED
 )
 
 # Link libraries cho utils
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME}_utils
+      PUBLIC ${catkin_LIBRARIES}
+      PRIVATE Boost::boost
+  )
+else()
   target_link_libraries(${PROJECT_NAME}_utils
       PUBLIC ${PACKAGES_DIR}
       PRIVATE Boost::boost
   )
+endif()
 
 set_target_properties(${PROJECT_NAME}_utils PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -76,11 +137,19 @@ add_library(${PROJECT_NAME} SHARED
 )
 
 # Link libraries cho thư viện chính
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME}
+      PUBLIC ${PROJECT_NAME}_utils
+      PUBLIC ${catkin_LIBRARIES}
+      PRIVATE Boost::boost
+  )
+else()
   target_link_libraries(${PROJECT_NAME}
       PUBLIC ${PROJECT_NAME}_utils
       PUBLIC ${PACKAGES_DIR}
       PRIVATE Boost::boost
   )
+endif()
 
 set_target_properties(${PROJECT_NAME} PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@@ -96,6 +165,11 @@ target_include_directories(${PROJECT_NAME}
 # Compile options
 target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-terminate")
 
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME}_utils ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
 # Tùy chọn build
 option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
@@ -104,13 +178,6 @@ option(BUILD_TESTS "Build test programs" OFF)
 ## Install ##
 #############
 
-# Cài đặt header files
-install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION include/${PROJECT_NAME}
-    FILES_MATCHING PATTERN "*.h"
-    PATTERN ".svn" EXCLUDE
-)
-
 # Cài đặt libraries
 install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
     EXPORT ${PROJECT_NAME}-targets
@@ -119,6 +186,15 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
     RUNTIME DESTINATION bin
 )
 
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/${PROJECT_NAME}/
+      DESTINATION include/${PROJECT_NAME}
+      FILES_MATCHING PATTERN "*.h"
+      PATTERN ".svn" EXCLUDE
+  )
+endif()
+
 # Export targets
 install(EXPORT ${PROJECT_NAME}-targets
     FILE ${PROJECT_NAME}-targets.cmake

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

@@ -1,7 +1,7 @@
 #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
 
-#include <nav_core/base_global_planner.h>
+#include <robot_nav_core2/base_global_planner.h>
 #include <pnkx_local_planner/pnkx_local_planner.h>
 
 namespace pnkx_local_planner
@@ -22,7 +22,7 @@ namespace pnkx_local_planner
     virtual ~PNKXDockingLocalPlanner();
 
     /**
-     * @brief nav_core2 initialization
+     * @brief robot_nav_core2 initialization
      * @param name Namespace for this planner
      * @param tf TFListener pointer
      * @param costmap_robot Costmap pointer
@@ -31,7 +31,7 @@ namespace pnkx_local_planner
                     TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
-     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *
      * It is presumed that the global plan is already set.
      *
@@ -46,7 +46,7 @@ namespace pnkx_local_planner
                                                                 const robot_nav_2d_msgs::Twist2D &velocity) override;
 
     /**
-     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     * @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
      *
      * The pose that it checks against is the last pose in the current global plan.
      * The calculation is delegated to the goal_checker plugin.
@@ -61,11 +61,11 @@ namespace pnkx_local_planner
      * @brief Create a new instance of the PNKXDockingLocalPlanner
      * @return A shared pointer to the new instance
      */
-     static nav_core2::LocalPlanner::Ptr create();
+     static robot_nav_core2::LocalPlanner::Ptr create();
 
   protected:
     /**
-     * @brief nav_core2 initialization other
+     * @brief robot_nav_core2 initialization other
      */
     void initializeOthers() override;
     
@@ -75,7 +75,7 @@ namespace pnkx_local_planner
     void getMaker();
 
     /**
-     * @brief nav_core2 reset other
+     * @brief robot_nav_core2 reset other
      */
     void reset() override;
 
@@ -137,7 +137,7 @@ namespace pnkx_local_planner
       std::string docking_planner_name_;
       std::string docking_nav_name_;
 
-      nav_core::BaseGlobalPlanner::Ptr docking_planner_;
+      robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_;
       score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
 
       robot_geometry_msgs::Vector3 linear_;
@@ -158,7 +158,7 @@ namespace pnkx_local_planner
       TFListenerPtr tf_;
       robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
       score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
-      std::function<nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
+      std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
       std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
 
       robot::WallTimer detected_timeout_wt_, delayed_wt_;

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

@@ -20,7 +20,7 @@ namespace pnkx_local_planner
     virtual ~PNKXGoStraightLocalPlanner();
 
     /**
-     * @brief nav_core2 initialization
+     * @brief robot_nav_core2 initialization
      * @param name Namespace for this planner
      * @param tf TFListener pointer
      * @param costmap_robot Costmap pointer
@@ -29,7 +29,7 @@ namespace pnkx_local_planner
                     TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
-     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *
      * It is presumed that the global plan is already set.
      *
@@ -44,7 +44,7 @@ namespace pnkx_local_planner
                                                         const robot_nav_2d_msgs::Twist2D &velocity) override;
 
     /**
-     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     * @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
      *
      * The pose that it checks against is the last pose in the current global plan.
      * The calculation is delegated to the goal_checker plugin.
@@ -59,11 +59,11 @@ namespace pnkx_local_planner
      * @brief Create a new instance of the PNKXGoStraightLocalPlanner
      * @return A shared pointer to the new instance
      */
-    static nav_core2::LocalPlanner::Ptr create();
+    static robot_nav_core2::LocalPlanner::Ptr create();
 
   protected:
     /**
-     * @brief nav_core2 reset other
+     * @brief robot_nav_core2 reset other
      */
     virtual void reset() override;;
 

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

@@ -3,7 +3,7 @@
 
 #include <robot/node_handle.h>
 #include <robot/console.h>
-#include <nav_core2/local_planner.h>
+#include <robot_nav_core2/local_planner.h>
 #include <score_algorithm/trajectory_generator.h>
 #include <score_algorithm/score_algorithm.h>
 #include <score_algorithm/goal_checker.h>
@@ -16,7 +16,7 @@ namespace pnkx_local_planner
    * @class PNKXLocalPlanner
    * @brief Plugin-based flexible local_planner
    */
-  class PNKXLocalPlanner : public nav_core2::LocalPlanner
+  class PNKXLocalPlanner : public robot_nav_core2::LocalPlanner
   {
   public:
     /**
@@ -27,7 +27,7 @@ namespace pnkx_local_planner
     virtual ~PNKXLocalPlanner();
 
     /**
-     * @brief nav_core2 initialization
+     * @brief robot_nav_core2 initialization
      * @param name Namespace for this planner
      * @param tf TFListener pointer
      * @param costmap_robot Costmap pointer
@@ -36,19 +36,19 @@ namespace pnkx_local_planner
                     TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
-     * @brief nav_core2 setGoalPose - Sets the global goal pose
+     * @brief robot_nav_core2 setGoalPose - Sets the global goal pose
      * @param goal_pose The Goal Pose
      */
     void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) override;
 
     /**
-     * @brief nav_core2 setPlan - Sets the global plan
+     * @brief robot_nav_core2 setPlan - Sets the global plan
      * @param path The global plan
      */
     void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
 
     /**
-     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *
      * It is presumed that the global plan is already set.
      *
@@ -99,7 +99,7 @@ namespace pnkx_local_planner
     virtual bool islock() override;
 
     /**
-     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     * @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
      *
      * The pose that it checks against is the last pose in the current global plan.
      * The calculation is delegated to the goal_checker plugin.
@@ -114,7 +114,7 @@ namespace pnkx_local_planner
      * @brief Create a new PNKXLocalPlanner
      * @return A shared pointer to the new PNKXLocalPlanner
      */
-    static nav_core2::LocalPlanner::Ptr create();
+    static robot_nav_core2::LocalPlanner::Ptr create();
 
   protected:
     /**
@@ -131,12 +131,12 @@ namespace pnkx_local_planner
      */
     virtual void unlock();
     /**
-     * @brief nav_core2 initialization other
+     * @brief robot_nav_core2 initialization other
      */
     virtual void initializeOthers();
 
     /**
-     * @brief nav_core2 reset other
+     * @brief robot_nav_core2 reset other
      */
     virtual void reset();
 

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

@@ -20,7 +20,7 @@ namespace pnkx_local_planner
     virtual ~PNKXRotateLocalPlanner();
 
     /**
-     * @brief nav_core2 initialization
+     * @brief robot_nav_core2 initialization
      * @param name Namespace for this planner
      * @param tf TFListener pointer
      * @param costmap_robot Costmap pointer
@@ -29,7 +29,7 @@ namespace pnkx_local_planner
                     TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
 
     /**
-     * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
+     * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *
      * It is presumed that the global plan is already set.
      *
@@ -44,7 +44,7 @@ namespace pnkx_local_planner
                                                         const robot_nav_2d_msgs::Twist2D &velocity) override;
 
     /**
-     * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
+     * @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
      *
      * The pose that it checks against is the last pose in the current global plan.
      * The calculation is delegated to the goal_checker plugin.
@@ -59,11 +59,11 @@ namespace pnkx_local_planner
      * @brief Create a new instance of the PNKXRotateLocalPlanner
      * @return A shared pointer to the new instance
      */
-    static nav_core2::LocalPlanner::Ptr create();
+    static robot_nav_core2::LocalPlanner::Ptr create();
 
   protected:
     /**
-     * @brief nav_core2 reset other
+     * @brief robot_nav_core2 reset other
      */
     virtual void reset() override;
 

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

@@ -5,8 +5,8 @@
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_utils/conversions.h>
 
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 
 #include <tf3/buffer_core.h>
 #include <tf3/convert.h>

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

@@ -19,8 +19,49 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <run_depend>geometry_msgs</run_depend>
 
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>robot_sensor_msgs</build_depend>
+  <run_depend>robot_sensor_msgs</run_depend>
+
+  <build_depend>robot_visualization_msgs</build_depend>
+  <run_depend>robot_visualization_msgs</run_depend>
+
+  <build_depend>robot_nav_2d_utils</build_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
+
+  <build_depend>robot_nav_core2</build_depend>
+  <run_depend>robot_nav_core2</run_depend>
+
+  <build_depend>mkt_msgs</build_depend>
+  <run_depend>mkt_msgs</run_depend>
+
+  <build_depend>score_algorithm</build_depend>
+  <run_depend>score_algorithm</run_depend>
+
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
+
+  <build_depend>tf3</build_depend>
+  <run_depend>tf3</run_depend>
+
+  <build_depend>robot_tf3_geometry_msgs</build_depend>
+  <run_depend>robot_tf3_geometry_msgs</run_depend>
+
+  <build_depend>robot_tf3_sensor_msgs</build_depend>
+  <run_depend>robot_tf3_sensor_msgs</run_depend>
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
+
+  <build_depend>angles</build_depend>
+  <run_depend>angles</run_depend>
 
 </package>

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

@@ -1,5 +1,5 @@
 #include <pnkx_local_planner/pnkx_docking_local_planner.h>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 #include <robot_nav_2d_utils/conversions.h>
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot_nav_2d_utils/path_ops.h>
@@ -51,7 +51,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
 
     nh_ = robot::NodeHandle("~");
     parent_ = parent;
@@ -65,7 +65,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
         path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
       traj_generator_ = traj_gen_loader_();
@@ -89,7 +89,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = algorithm_loader_();
@@ -110,7 +110,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
       rotate_algorithm_ = algorithm_loader_();
@@ -132,7 +132,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
         path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
       goal_checker_ = goal_checker_loader_();
@@ -262,7 +262,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
 
   // Update time stamp of goal pose
@@ -279,9 +279,9 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
       robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
   }
-  catch(const nav_core2::LocalPlannerException& e)
+  catch(const robot_nav_core2::LocalPlannerException& e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 
   double x_direction, y_direction, theta_direction;
@@ -290,7 +290,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
     if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
-      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
     }
     // else
     //   ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
@@ -329,7 +329,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
         }
         if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
         {
-          throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+          throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
           robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
         }
       }
@@ -339,7 +339,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
   {
     if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
     {
-      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
     }
     // else
@@ -366,10 +366,10 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::c
     cmd_vel = this->ScoreAlgorithm(pose, velocity);
     return cmd_vel;
   }
-  catch (const nav_core2::PlannerException &e)
+  catch (const robot_nav_core2::PlannerException &e)
   {
     robot::log_warning_at(__FILE__, __LINE__, "%s", e.what());
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 }
 
@@ -499,7 +499,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             }
             catch (const std::exception &e)
             {
-              throw nav_core2::LocalPlannerException(e.what());
+              throw robot_nav_core2::LocalPlannerException(e.what());
             }
           }
           else if (!dkpl_.front()->docking_planner_ && dkpl_.front()->docking_nav_)
@@ -523,7 +523,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
             }
             catch (const std::exception &e)
             {
-              throw nav_core2::LocalPlannerException(e.what());
+              throw robot_nav_core2::LocalPlannerException(e.what());
             }
           }
         }
@@ -585,8 +585,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
-      docking_planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
+      docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
         path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
       docking_planner_ = docking_planner_loader_();
       if (!docking_planner_)
@@ -608,7 +608,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
   {
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
       docking_nav_ = docking_nav_loader_();
@@ -634,7 +634,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
     std::stringstream re;
     re << "Can not get 'maker_goal_frame' in " << name_;
     robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
-    throw nav_core2::LocalPlannerException(re.str());
+    throw robot_nav_core2::LocalPlannerException(re.str());
   }
 
   nh_priv_.param("vel_x", linear_.x, 0.5);
@@ -691,7 +691,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(r
       std::stringstream re;
       re << "No detected " << maker_goal_frame_;
       robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
-      throw nav_core2::LocalPlannerException(re.str());
+      throw robot_nav_core2::LocalPlannerException(re.str());
     }
   }
   return false;
@@ -724,7 +724,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(ro
       std::stringstream re;
       re << "No detected " << maker_goal_frame_;
       robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
-      throw nav_core2::LocalPlannerException(re.str());
+      throw robot_nav_core2::LocalPlannerException(re.str());
     }
   }
   return false;
@@ -739,7 +739,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
 
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {
-    throw nav_core2::LocalPlannerException("Making plan from goal maker is failed");
+    throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");
     return false;
   }
   else
@@ -763,7 +763,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb(const
   robot::log_warning_at(__FILE__, __LINE__, "%s delay end %f", name_.c_str(), robot::Time::now().toSec());
 }
 
-nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create()
+robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create()
 {
   return std::make_shared<PNKXDockingLocalPlanner>();
 }

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

@@ -1,4 +1,4 @@
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 #include <robot_nav_2d_utils/conversions.h>
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot_nav_2d_utils/path_ops.h>
@@ -47,7 +47,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
     
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
@@ -61,7 +61,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
         path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
       traj_generator_ = traj_gen_loader_();
@@ -84,7 +84,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
       nav_algorithm_ = nav_algorithm_loader_();
@@ -106,7 +106,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
         path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
       goal_checker_ = goal_checker_loader_();
@@ -152,9 +152,9 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
     cmd_vel = this->ScoreAlgorithm(pose, velocity);
     return cmd_vel;
   }
-  catch (const nav_core2::PlannerException &e)
+  catch (const robot_nav_core2::PlannerException &e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 }
 
@@ -171,7 +171,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
@@ -181,17 +181,17 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
   try
   {
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
-      throw nav_core2::LocalPlannerException("Transform global plan is failed");
+      throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
   }
-  catch(const nav_core2::LocalPlannerException& e)
+  catch(const robot_nav_core2::LocalPlannerException& e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 
   double x_direction, y_direction, theta_direction;
   if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
   {
-    throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+    throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
   }
 
   int xytheta_direct[3];
@@ -229,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n
   return ret_nav_;
 }
 
-nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXGoStraightLocalPlanner::create()
+robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXGoStraightLocalPlanner::create()
 {
   return std::make_shared<pnkx_local_planner::PNKXGoStraightLocalPlanner>();
 }

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

@@ -1,5 +1,5 @@
 #include <pnkx_local_planner/pnkx_local_planner.h>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 
 #include <robot_nav_2d_utils/conversions.h>
 #include <robot_nav_2d_utils/tf_help.h>
@@ -54,7 +54,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
 
     parent_ = parent;
     planner_nh_ = robot::NodeHandle(parent_, name);
@@ -221,7 +221,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
 
   // Update time stamp of goal pose
@@ -242,7 +242,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
     if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
-      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
     }
   }
   else
@@ -250,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
     if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
-      throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+      throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
     }
   }
 
@@ -274,9 +274,9 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeV
     cmd_vel = this->ScoreAlgorithm(pose, velocity);
     return cmd_vel;
   }
-  catch (const nav_core2::PlannerException &e)
+  catch (const robot_nav_core2::PlannerException &e)
   {
-    throw nav_core2::LocalPlannerException("computing velocity commands has been broken");
+    throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken");
     return cmd_vel;
   }
 }
@@ -321,11 +321,11 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(robot_geometry_msgs::V
     if (traj_generator_)
       return traj_generator_->setTwistLinear(linear);
     else
-      throw nav_core2::LocalPlannerException("Failed to set linear");
+      throw robot_nav_core2::LocalPlannerException("Failed to set linear");
   }
   catch (const std::exception &e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
     return false;
   }
 }
@@ -337,11 +337,11 @@ robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinea
     if (traj_generator_)
       return traj_generator_->getTwistLinear(direct);
     else
-      throw nav_core2::LocalPlannerException("Failed to get linear");
+      throw robot_nav_core2::LocalPlannerException("Failed to get linear");
   }
   catch (const std::exception &e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 }
 
@@ -352,11 +352,11 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(robot_geometry_msgs::
     if (traj_generator_)
       return traj_generator_->setTwistAngular(angular);
     else
-      throw nav_core2::LocalPlannerException("Failed to set angular");
+      throw robot_nav_core2::LocalPlannerException("Failed to set angular");
   }
   catch (const std::exception &e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
     return false;
   }
 }
@@ -368,11 +368,11 @@ robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngul
     if (traj_generator_)
       return traj_generator_->getTwistAngular(direct);
     else
-      throw nav_core2::LocalPlannerException("Failed to get angular");
+      throw robot_nav_core2::LocalPlannerException("Failed to get angular");
   }
   catch (const std::exception &e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 }
 
@@ -434,7 +434,7 @@ robot_nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transform
   return local_pose;
 }
 
-nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXLocalPlanner::create()
+robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXLocalPlanner::create()
 {
   return std::make_shared<PNKXLocalPlanner>();
 }

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

@@ -1,7 +1,7 @@
 #include "pnkx_local_planner/pnkx_rotate_local_planner.h"
 #include "pnkx_local_planner/transforms.h"
 
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 #include <robot_nav_2d_utils/conversions.h>
 #include <robot_nav_2d_utils/tf_help.h>
 #include <robot_nav_2d_utils/path_ops.h>
@@ -55,7 +55,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
         path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
       traj_generator_ = traj_gen_loader_();
@@ -77,7 +77,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
       rotate_algorithm_ = rotate_algorithm_loader_();
@@ -99,7 +99,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
     planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker"));
     try
     {
-      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
+      std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
       goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
         path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
       goal_checker_ = goal_checker_loader_();
@@ -145,7 +145,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
     info_.origin_y = costmap_->getOriginY();
 
     if (!costmap_robot_->isCurrent())
-      throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
+      throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
 
   // Update time stamp of goal pose
@@ -156,17 +156,17 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
   try
   {
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
-      throw nav_core2::LocalPlannerException("Transform global plan is failed");
+      throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
   }
-  catch(const nav_core2::LocalPlannerException& e)
+  catch(const robot_nav_core2::LocalPlannerException& e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 
   double x_direction, y_direction, theta_direction;
   if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
   {
-    throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
+    throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
   }
 
   int xytheta_direct[3];
@@ -188,9 +188,9 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::co
     cmd_vel = this->ScoreAlgorithm(pose, velocity);
     return cmd_vel;
   }
-  catch (const nav_core2::PlannerException &e)
+  catch (const robot_nav_core2::PlannerException &e)
   {
-    throw nav_core2::LocalPlannerException(e.what());
+    throw robot_nav_core2::LocalPlannerException(e.what());
   }
 }
 
@@ -225,7 +225,7 @@ bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const robot_nav_2
   return ret_nav_;
 }
 
-nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create()
+robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create()
 {
   return std::make_shared<pnkx_local_planner::PNKXRotateLocalPlanner>();
 }

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

@@ -46,19 +46,19 @@ bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, st
   {
     robot::log_warning_throttle(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
     return false;
-    // throw nav_core2::LocalPlannerException(ex.what());
+    // throw robot_nav_core2::LocalPlannerException(ex.what());
   }
   catch (tf3::ConnectivityException& ex)
   {
     robot::log_warning_throttle(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
     return false;
-    // throw nav_core2::LocalPlannerException(ex.what());
+    // throw robot_nav_core2::LocalPlannerException(ex.what());
   }
   catch (tf3::ExtrapolationException& ex)
   {
     robot::log_warning_throttle(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
     return false;
-    // throw nav_core2::LocalPlannerException(ex.what());
+    // throw robot_nav_core2::LocalPlannerException(ex.what());
   }
   // check global_pose timeout
   if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance)
@@ -95,15 +95,15 @@ bool pnkx_local_planner::transformGlobalPlan(
   if (global_plan.poses.size() == 0)
   {
     robot::log_warning("Received plan with zero length");
-    throw nav_core2::PlannerException("Received plan with zero length");
+    throw robot_nav_core2::PlannerException("Received plan with zero length");
   }
 
   transformed_plan.poses.clear();
   if (tf == nullptr)
-    throw nav_core2::PlannerException("TFListenerPtr is null");
+    throw robot_nav_core2::PlannerException("TFListenerPtr is null");
 
   if (global_plan.poses.empty())
-    throw nav_core2::PlannerException("Received plan with zero length");
+    throw robot_nav_core2::PlannerException("Received plan with zero length");
 
   try
   {
@@ -111,7 +111,7 @@ bool pnkx_local_planner::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped robot_pose;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
     {
-      throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     transformed_plan.header.frame_id = costmap->getGlobalFrameID();
@@ -139,7 +139,7 @@ bool pnkx_local_planner::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped costmap_pose;
     if (!robot_nav_2d_utils::transformPose(tf, transformed_plan.header.frame_id, pose, costmap_pose))
     {
-      throw nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame");
     }
 
     robot::log_assert(global_plan.poses.size() >= transformed_plan.poses.size(), "Global plan size is less than transformed plan size");
@@ -182,7 +182,7 @@ bool pnkx_local_planner::transformGlobalPlan(
 
     if (transformed_plan.poses.size() == 0)
     {
-      throw nav_core2::PlannerTFException("Resulting plan has 0 poses in it.");
+      throw robot_nav_core2::PlannerTFException("Resulting plan has 0 poses in it.");
     }
   }
   catch (tf3::LookupException& ex)

src/Libraries/common_msgs

@@ -1 +1 @@
-Subproject commit 5c276afb3469a4618e3d2f3523a27bff2f9fb3c8
+Subproject commit 1df5ed676fe374c63665d434f4d8b2c9923d41a1

src/Libraries/costmap_2d

@@ -1 +1 @@
-Subproject commit b6733ae04cc2ec79409faf05bbae5f70a3c7fcd2
+Subproject commit 80bde38f4d5dbb66bab5a8bd5c2c3faaa870aa9f

src/Libraries/data_convert

@@ -1 +1 @@
-Subproject commit 594f0fe49e40273a7e2e74e44f0d63fa4fe2cfea
+Subproject commit d7eed928fbfb26d822fd4c4cb7ab84e3cb46866a

src/Libraries/geometry2

@@ -1 +1 @@
-Subproject commit 1aaeb4c59df4b1888630e26cea5c5edcdd13a46b
+Subproject commit e0db8d0e20142df254de7232ada253c9ba4d0a08

src/Libraries/laser_geometry

@@ -1 +1 @@
-Subproject commit 831e4e00d79cbc218cd575aecdb3ce3041a37cb0
+Subproject commit 7cb758a986f481ee91afb8be7b407b6329dd61b0

src/Libraries/robot_cpp/PLUGIN_LOADER_README.md

@@ -41,7 +41,7 @@ robot::PluginLoaderHelper loader;
 std::string lib_path = loader.findLibraryPath("CustomPlanner");
 if (!lib_path.empty()) {
     // Sử dụng với boost::dll::import_alias
-    auto planner_loader = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
+    auto planner_loader = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
         lib_path, "CustomPlanner", boost::dll::load_mode::append_decorations);
     planner_ = planner_loader();
 }
@@ -53,14 +53,14 @@ Thay vì hardcode đường dẫn:
 
 ```cpp
 // CŨ (hardcode):
-std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/libcustom_planner.so";
+std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/libcustom_planner.so";
 
 // MỚI (từ config):
 robot::PluginLoaderHelper loader;
 std::string path_file_so = loader.findLibraryPath(global_planner);
 if (path_file_so.empty()) {
     // Fallback nếu không tìm thấy
-    path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/libcustom_planner.so";
+    path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/libcustom_planner.so";
 }
 ```
 
@@ -97,7 +97,7 @@ plugin_libraries:
     library_path: "/path/to/libpnkx_local_planner.so"
 
 search_paths:
-  - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build"
+  - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build"
   - "/path/to/other/libraries"
 ```
 
@@ -126,7 +126,7 @@ Helper sẽ tìm library theo thứ tự:
   1. Đường dẫn trực tiếp (nếu absolute)
   2. `$PNKX_NAV_CORE_CONFIG_DIR/boost_dll_plugins.yaml`
   3. `$PNKX_NAV_CORE_DIR/config/boost_dll_plugins.yaml`
-  4. `/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/config/boost_dll_plugins.yaml`
+  4. `/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/config/boost_dll_plugins.yaml`
   5. `../config/boost_dll_plugins.yaml` (relative paths)
 
 - Symbol name có thể có namespace (ví dụ: `custom_planner::CustomPlanner`), helper sẽ tự động thử tìm cả tên đầy đủ và tên ngắn (`CustomPlanner`).

src/Libraries/robot_cpp/src/node_handle.cpp

@@ -877,7 +877,7 @@ namespace robot
 
     // Try hardcoded fallback paths
     std::vector<std::string> possible_paths = {
-        "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/config",
+        "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/config",
         "../config",
         "../../config",
         "./config"};

src/Libraries/robot_cpp/src/plugin_loader_helper.cpp

@@ -36,7 +36,7 @@ PluginLoaderHelper::PluginLoaderHelper(robot::NodeHandle nh, const std::string&
     // Thêm các subdirectories thường dùng
     search_paths_.push_back(build_dir + "/src/Algorithms/Packages/global_planners");
     search_paths_.push_back(build_dir + "/src/Algorithms/Packages/local_planners");
-    search_paths_.push_back(build_dir + "/src/Navigations/Cores/nav_core_adapter");
+    search_paths_.push_back(build_dir + "/src/Navigations/Cores/robot_nav_core2_adapter");
     search_paths_.push_back(build_dir + "/src/Libraries/robot_costmap_2d");
   }
 }
@@ -319,7 +319,7 @@ std::string PluginLoaderHelper::getBuildDirectory()
   }
 
   // Method 6: Hardcoded fallback
-  std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build";
+  std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build";
   if (std::filesystem::exists(fallback)) {
     return fallback;
   }

src/Libraries/robot_nav_2d_msgs/CMakeLists.txt

@@ -61,8 +61,8 @@ target_link_libraries(robot_nav_2d_msgs
 # Add include directories from dependencies explicitly for Catkin build
 if(BUILDING_WITH_CATKIN)
   # Use relative paths from current source directory
-  # From robot_nav_2d_msgs (pnkx_nav_core/src/Libraries/robot_nav_2d_msgs) 
+  # From robot_nav_2d_msgs (pnkx_robot_nav_core2/src/Libraries/robot_nav_2d_msgs) 
-  # to robot_std_msgs (pnkx_nav_core/src/Libraries/common_msgs/robot_std_msgs) is ../common_msgs/robot_std_msgs
+  # to robot_std_msgs (pnkx_robot_nav_core2/src/Libraries/common_msgs/robot_std_msgs) is ../common_msgs/robot_std_msgs
   get_filename_component(ROBOT_STD_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/robot_std_msgs/include" ABSOLUTE)
   get_filename_component(GEOMETRY_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/robot_geometry_msgs/include" ABSOLUTE)
   target_include_directories(robot_nav_2d_msgs INTERFACE

src/Libraries/robot_nav_2d_msgs/package.xml

@@ -19,8 +19,4 @@
     
   <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

@@ -27,11 +27,20 @@ else()
 endif()
 
 if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_xmlrpcpp)
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_nav_2d_msgs
+    robot_geometry_msgs
+    robot_nav_msgs
+    nav_grid
+    robot_nav_core2
+    tf3
+    robot_tf3_geometry_msgs
+    robot_cpp 
+  )
 
   catkin_package(
     INCLUDE_DIRS include
-    LIBRARIES conversions path_ops polygons bounds tf_help robot_nav_2d_utils
+    LIBRARIES ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help ${PROJECT_NAME}
     # CATKIN_DEPENDS không cần vì dependencies không phải Catkin packages
     DEPENDS console_bridge Boost
   )
@@ -42,19 +51,19 @@ find_package(console_bridge REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread)
 
 # Libraries
-add_library(conversions src/conversions.cpp)
+add_library(${PROJECT_NAME}_conversions src/conversions.cpp)
-target_include_directories(conversions
+target_include_directories(${PROJECT_NAME}_conversions
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(conversions
+target_link_libraries(${PROJECT_NAME}_conversions
     PUBLIC
         robot_nav_2d_msgs
         robot_geometry_msgs
         robot_nav_msgs
         nav_grid
-        nav_core2
+        robot_nav_core2
         robot_cpp
     PRIVATE
         console_bridge::console_bridge
@@ -62,121 +71,123 @@ target_link_libraries(conversions
         Boost::thread
 )
 
-set_target_properties(conversions PROPERTIES
+set_target_properties(${PROJECT_NAME}_conversions PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
   BUILD_RPATH "${CMAKE_BINARY_DIR}"
   INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
-add_library(path_ops src/path_ops.cpp)
+add_library(${PROJECT_NAME}_path_ops src/path_ops.cpp)
-target_include_directories(path_ops
+target_include_directories(${PROJECT_NAME}_path_ops
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(path_ops
+target_link_libraries(${PROJECT_NAME}_path_ops
     PUBLIC
         robot_nav_2d_msgs
         robot_geometry_msgs
         robot_cpp
 )
 
-set_target_properties(path_ops PROPERTIES
+set_target_properties(${PROJECT_NAME}_path_ops PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
   BUILD_RPATH "${CMAKE_BINARY_DIR}"
   INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
-add_library(polygons src/polygons.cpp src/footprint.cpp)
+add_library(${PROJECT_NAME}_polygons src/polygons.cpp src/footprint.cpp)
-target_include_directories(polygons
+target_include_directories(${PROJECT_NAME}_polygons
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
 
 if(robot_xmlrpcpp_FOUND)
-    target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
+    target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
-    target_link_libraries(polygons
+    target_link_libraries(${PROJECT_NAME}_polygons
         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_include_directories(${PROJECT_NAME}_polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
-    target_link_libraries(polygons 
+    target_link_libraries(${PROJECT_NAME}_polygons 
         PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp 
         PRIVATE ${XmlRpcCpp_LIBRARIES})
 else()
-    target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
+    target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
-    target_link_libraries(polygons
+    target_link_libraries(${PROJECT_NAME}_polygons
         PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
         PRIVATE ${robot_xmlrpcpp_LIBRARIES})
 endif()
 
-set_target_properties(polygons PROPERTIES
+set_target_properties(${PROJECT_NAME}_polygons PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
   BUILD_RPATH "${CMAKE_BINARY_DIR}"
   INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
-add_library(bounds src/bounds.cpp)
+add_library(${PROJECT_NAME}_bounds src/bounds.cpp)
-target_include_directories(bounds
+target_include_directories(${PROJECT_NAME}_bounds
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(bounds
+target_link_libraries(${PROJECT_NAME}_bounds
     PUBLIC
         nav_grid
-        nav_core2
+        robot_nav_core2
         robot_cpp
 )
 
-set_target_properties(bounds PROPERTIES
+set_target_properties(${PROJECT_NAME}_bounds PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
   BUILD_RPATH "${CMAKE_BINARY_DIR}"
   INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
-add_library(tf_help src/tf_help.cpp)
+add_library(${PROJECT_NAME}_tf_help src/tf_help.cpp)
-target_include_directories(tf_help
+target_include_directories(${PROJECT_NAME}_tf_help
     PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(tf_help
+target_link_libraries(${PROJECT_NAME}_tf_help
   PUBLIC
     robot_nav_2d_msgs
     robot_geometry_msgs
-        nav_core2
+    robot_nav_msgs
+    robot_nav_core2
     tf3
+    robot_tf3_geometry_msgs
     robot_cpp
 )
 
-set_target_properties(tf_help PROPERTIES
+set_target_properties(${PROJECT_NAME}_tf_help PROPERTIES
   LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
   BUILD_RPATH "${CMAKE_BINARY_DIR}"
   INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
 # Create an INTERFACE library that represents all robot_nav_2d_utils libraries
-add_library(robot_nav_2d_utils INTERFACE)
+add_library(${PROJECT_NAME} INTERFACE)
-target_include_directories(robot_nav_2d_utils
+target_include_directories(${PROJECT_NAME}
     INTERFACE
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(robot_nav_2d_utils
+target_link_libraries(${PROJECT_NAME}
     INTERFACE
-        conversions
+        ${PROJECT_NAME}_conversions
-        path_ops
+        ${PROJECT_NAME}_path_ops
-        polygons
+        ${PROJECT_NAME}_polygons
-        bounds
+        ${PROJECT_NAME}_bounds
-        tf_help
+        ${PROJECT_NAME}_tf_help
         robot_cpp
 )
 
 # Install header files
 if(NOT BUILDING_WITH_CATKIN)
-  install(DIRECTORY include/robot_nav_2d_utils 
+  install(DIRECTORY include/${PROJECT_NAME} 
           DESTINATION include
           FILES_MATCHING PATTERN "*.h")
 
@@ -187,23 +198,23 @@ if(NOT BUILDING_WITH_CATKIN)
 endif()
 
 # Install targets
-install(TARGETS robot_nav_2d_utils conversions path_ops polygons bounds tf_help
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help
-        EXPORT robot_nav_2d_utils-targets
+        EXPORT ${PROJECT_NAME}-targets
         ARCHIVE DESTINATION lib
         LIBRARY DESTINATION lib
         RUNTIME DESTINATION bin)
 
 # Export targets
-install(EXPORT robot_nav_2d_utils-targets
+install(EXPORT ${PROJECT_NAME}-targets
-        FILE robot_nav_2d_utils-targets.cmake
+        FILE ${PROJECT_NAME}-targets.cmake
-        NAMESPACE robot_nav_2d_utils::
+        NAMESPACE ${PROJECT_NAME}::
-        DESTINATION lib/cmake/robot_nav_2d_utils)
+        DESTINATION lib/cmake/${PROJECT_NAME})
 
 # Print configuration info
 message(STATUS "=================================")
 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 "Libraries: ${PROJECT_NAME}_conversions, ${PROJECT_NAME}_path_ops, ${PROJECT_NAME}_polygons, ${PROJECT_NAME}_bounds, ${PROJECT_NAME}_tf_help")
-message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, robot_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, robot_nav_core2, tf3, console_bridge, Boost")
 message(STATUS "=================================")

src/Libraries/robot_nav_2d_utils/README.md

@@ -1,6 +1,6 @@
 # robot_nav_2d_utils
-A handful of useful utility functions for nav_core2 packages.
+A handful of useful utility functions for robot_nav_core2 packages.
- * Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds.
+ * Bounds - Utilities for `robot_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 `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

src/Libraries/robot_nav_2d_utils/doc/Conversions.md

@@ -51,4 +51,4 @@ Also, `robot_nav_msgs::Path posesToPath(std::vector<robot_geometry_msgs::PoseSta
 ## Bounds Transformations
 | to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
 | -- | -- |
-|`robot_nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(robot_nav_2d_msgs::UIntBounds)`|
+|`robot_nav_2d_msgs::UIntBounds toMsg(robot_nav_core2::UIntBounds)`|`robot_nav_core2::UIntBounds fromMsg(robot_nav_2d_msgs::UIntBounds)`|

src/Libraries/robot_nav_2d_utils/doc/PluginMux.md

@@ -2,7 +2,7 @@
 # Plugin Mux
 `PluginMux` is an organizer for switching between multiple different plugins of the same type. It may be easiest to see how it operates through an example. Let's say we have multiple global planners we would like to use at different times, with only one being active at a given time.
 
-This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `nav_core` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config.
+This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `robot_nav_core2` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config.
 
 ```
 global_planner_namespaces:
@@ -23,7 +23,7 @@ To advertise which plugin is active, we publish the namespace on a latched topic
 
 This configuration is all created by creating a plugin mux with the following parameters (parameter names shown for convenience):
 ```
-PluginMux(plugin_package = "nav_core",
+PluginMux(plugin_package = "robot_nav_core2",
           plugin_class = "BaseGlobalPlanner",
           parameter_name = "global_planner_namespaces",
           default_value = "global_planner/GlobalPlanner",

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h

@@ -36,7 +36,7 @@
 #define ROBOT_NAV_2D_UTILS_BOUNDS_H
 
 #include <nav_grid/nav_grid_info.h>
-#include <nav_core2/bounds.h>
+#include <robot_nav_core2/bounds.h>
 #include <vector>
 
 /**
@@ -50,14 +50,14 @@ namespace robot_nav_2d_utils
    * @param info Info for the NavGrid
    * @return bounds covering the entire NavGrid
    */
-  nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info);
+  robot_nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info);
 
   /**
    * @brief return an integral bounds that covers the entire NavGrid
    * @param info Info for the NavGrid
    * @return bounds covering the entire NavGrid
    */
-  nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info);
+  robot_nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info);
 
   /**
    * @brief Translate real-valued bounds to uint coordinates based on nav_grid info
@@ -65,7 +65,7 @@ namespace robot_nav_2d_utils
    * @param bounds floating point bounds object
    * @returns corresponding UIntBounds for parameter
    */
-  nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds);
+  robot_nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::Bounds &bounds);
 
   /**
    * @brief Translate uint bounds to real-valued coordinates based on nav_grid info
@@ -73,7 +73,7 @@ namespace robot_nav_2d_utils
    * @param bounds UIntBounds object
    * @returns corresponding floating point bounds for parameter
    */
-  nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds);
+  robot_nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::UIntBounds &bounds);
 
   /**
    * @brief divide the given bounds up into sub-bounds of roughly equal size
@@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
    * @return vector of a maximum of n_cols*n_rows nonempty bounds
    * @throws std::invalid_argument when n_cols or n_rows is zero
    */
-  std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds &original_bounds,
+  std::vector<robot_nav_core2::UIntBounds> divideBounds(const robot_nav_core2::UIntBounds &original_bounds,
                                                   unsigned int n_cols, unsigned int n_rows);
 
 } // namespace robot_nav_2d_utils

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

@@ -50,7 +50,7 @@
 #include <robot_nav_msgs/MapMetaData.h>
 #include <robot_nav_msgs/Path.h>
 #include <nav_grid/nav_grid.h>
-#include <nav_core2/bounds.h>
+#include <robot_nav_core2/bounds.h>
 // #include <tf/tf.h>
 #include <vector>
 #include <string>
@@ -98,8 +98,8 @@ namespace robot_nav_2d_utils
   robot_nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
 
   // Bounds Transformations
-  robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds);
+  robot_nav_2d_msgs::UIntBounds toMsg(const robot_nav_core2::UIntBounds &bounds);
-  nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
+  robot_nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
 
 } // namespace robot_nav_2d_utils
 

src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h

@@ -35,7 +35,7 @@
 #ifndef ROBOT_NAV_2D_UTILS_TF_HELP_H
 #define ROBOT_NAV_2D_UTILS_TF_HELP_H
 
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <string>

src/Libraries/robot_nav_2d_utils/package.xml

@@ -19,7 +19,17 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>robot_xmlrpcpp</build_depend>
+  <build_depend>robot_nav_2d_msgs</build_depend>
-  <run_depend>robot_xmlrpcpp</run_depend>
+  <build_depend>robot_geometry_msgs</build_depend>
+  <build_depend>robot_nav_msgs</build_depend>
+  <build_depend>nav_grid</build_depend>
+  <build_depend>robot_nav_core2</build_depend>
+  <build_depend>robot_cpp</build_depend>
 
+  <run_depend>robot_nav_2d_msgs</run_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+  <run_depend>nav_grid</run_depend>
+  <run_depend>robot_nav_core2</run_depend>
+  <run_depend>robot_cpp</run_depend>
 </package>

src/Libraries/robot_nav_2d_utils/src/bounds.cpp

@@ -40,35 +40,35 @@
 
 namespace robot_nav_2d_utils
 {
-  nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)
+  robot_nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)
   {
-    return nav_core2::Bounds(info.origin_x, info.origin_y,
+    return robot_nav_core2::Bounds(info.origin_x, info.origin_y,
                              info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height);
   }
 
-  nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info)
+  robot_nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info)
   {
     // bounds are inclusive, so we subtract one
-    return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1);
+    return robot_nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1);
   }
 
-  nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds)
+  robot_nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::Bounds &bounds)
   {
     unsigned int g_min_x, g_min_y, g_max_x, g_max_y;
     worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y);
     worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y);
-    return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y);
+    return robot_nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y);
   }
 
-  nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds)
+  robot_nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::UIntBounds &bounds)
   {
     double min_x, min_y, max_x, max_y;
     gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y);
     gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y);
-    return nav_core2::Bounds(min_x, min_y, max_x, max_y);
+    return robot_nav_core2::Bounds(min_x, min_y, max_x, max_y);
   }
 
-  std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds &original_bounds,
+  std::vector<robot_nav_core2::UIntBounds> divideBounds(const robot_nav_core2::UIntBounds &original_bounds,
                                                   unsigned int n_cols, unsigned int n_rows)
   {
     if (n_cols * n_rows == 0)
@@ -81,7 +81,7 @@ namespace robot_nav_2d_utils
     unsigned int small_width = static_cast<unsigned int>(ceil(static_cast<double>(full_width) / n_cols)),
                  small_height = static_cast<unsigned int>(ceil(static_cast<double>(full_height) / n_rows));
 
-    std::vector<nav_core2::UIntBounds> divided;
+    std::vector<robot_nav_core2::UIntBounds> divided;
 
     for (unsigned int row = 0; row < n_rows; row++)
     {
@@ -92,7 +92,7 @@ namespace robot_nav_2d_utils
       {
         unsigned int min_x = original_bounds.getMinX() + small_width * col;
         unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX());
-        nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y);
+        robot_nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y);
         if (!sub.isEmpty())
           divided.push_back(sub);
       }

src/Libraries/robot_nav_2d_utils/src/conversions.cpp

@@ -126,17 +126,17 @@ namespace robot_nav_2d_utils
     return pose;
   }
 
-  // robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_geometry_msgs::Pose2D& pose2d,
+  robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_geometry_msgs::Pose2D& pose2d,
-  //                                                const std::string& frame, const robot::Time& stamp)
+                                                 const std::string& frame, const robot::Time& stamp)
-  // {
+  {
-  //   robot_geometry_msgs::PoseStamped pose;
+    robot_geometry_msgs::PoseStamped pose;
-  //   pose.header.frame_id = frame;
+    pose.header.frame_id = frame;
-  //   pose.header.stamp = stamp;
+    pose.header.stamp = stamp;
-  //   pose.pose.position.x = pose2d.x;
+    pose.pose.position.x = pose2d.x;
-  //   pose.pose.position.y = pose2d.y;
+    pose.pose.position.y = pose2d.y;
-  //   // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
+    // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
-  //   return pose;
+    return pose;
-  // }
+  }
 
   robot_nav_2d_msgs::Path2D pathToPath(const robot_nav_msgs::Path &path)
   {
@@ -186,19 +186,19 @@ namespace robot_nav_2d_utils
     return path;
   }
 
-  // robot_nav_msgs::Path poses2DToPath(const std::vector<robot_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)
+                               const std::string& frame, const robot::Time& stamp)
-  // {
+  {
-  //   robot_nav_msgs::Path path;
+    robot_nav_msgs::Path path;
-  //   path.poses.resize(poses.size());
+    path.poses.resize(poses.size());
-  //   path.header.frame_id = frame;
+    path.header.frame_id = frame;
-  //   path.header.stamp = stamp;
+    path.header.stamp = stamp;
-  //   for (unsigned int i = 0; i < poses.size(); i++)
+    for (unsigned int i = 0; i < poses.size(); i++)
-  //   {
+    {
-  //     path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
+      path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
-  //   }
+    }
-  //   return path;
+    return path;
-  // }
+  }
 
   robot_nav_msgs::Path pathToPath(const robot_nav_2d_msgs::Path2D &path2d)
   {
@@ -318,7 +318,7 @@ namespace robot_nav_2d_utils
     return metadata;
   }
 
-  robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds)
+  robot_nav_2d_msgs::UIntBounds toMsg(const robot_nav_core2::UIntBounds &bounds)
   {
     robot_nav_2d_msgs::UIntBounds msg;
     msg.min_x = bounds.getMinX();
@@ -328,9 +328,9 @@ namespace robot_nav_2d_utils
     return msg;
   }
 
-  nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
+  robot_nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
   {
-    return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
+    return robot_nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
   }
 
 } // namespace robot_nav_2d_utils

src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp

@@ -37,7 +37,7 @@
 #include <vector>
 
 using robot_nav_2d_utils::divideBounds;
-using nav_core2::UIntBounds;
+using robot_nav_core2::UIntBounds;
 
 /**
  * @brief Count the values in a grid.

src/Libraries/tf3/package.xml

@@ -19,8 +19,5 @@
     
   <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/voxel_grid

@@ -1 +1 @@
-Subproject commit b19cec9f4d4823d7cbbd741d60c239753532e4db
+Subproject commit 4303737ff92db94dc1bcde2d8a3bef527e76355a

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -312,10 +312,9 @@ namespace move_base_core
 
 		/**
 		 * @brief Get the navigation feedback.
-		 * @param feedback Output parameter with the navigation feedback.
+		 * @return Pointer to the navigation feedback.
-		 * @return True if feedback was successfully retrieved.
 		 */
-		virtual bool getFeedback(NavFeedback &feedback) = 0;
+		virtual NavFeedback *getFeedback() = 0;
 
 	protected:
 		// Shared feedback data for navigation status tracking

src/Navigations/Cores/nav_core/CMakeLists.txt

@@ -1,56 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-
-# Tên dự án
-project(nav_core VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# Cho phép các project khác include được header của nav_core
-set(nav_core_INCLUDE_DIRS
-    ${CMAKE_CURRENT_SOURCE_DIR}/include
-    PARENT_SCOPE
-)
-
-include_directories(
-        ${PROJECT_SOURCE_DIR}/include
-)
-
-# Tạo INTERFACE library (header-only)
-add_library(nav_core INTERFACE)
-target_link_libraries(nav_core INTERFACE robot_costmap_2d tf3 robot_protocol_msgs)
-
-# Set include directories
-target_include_directories(nav_core
-    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-# Cài đặt header files
-install(DIRECTORY include/nav_core 
-        DESTINATION include
-        FILES_MATCHING PATTERN "*.h")
-
-install(TARGETS nav_core 
-        EXPORT nav_core-targets
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib
-        RUNTIME DESTINATION bin)
-
-# Export targets
-install(EXPORT nav_core-targets
-         FILE nav_core-targets.cmake
-        DESTINATION lib/cmake/nav_core)
-
-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
-    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")

src/Navigations/Cores/nav_core2/CMakeLists.txt

@@ -1,65 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-
-# Tên dự án
-project(nav_core2 VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# Cho phép các project khác include được header của nav_core2
-set(nav_core2_INCLUDE_DIRS
-    ${CMAKE_CURRENT_SOURCE_DIR}/include
-    PARENT_SCOPE
-)
-
-include_directories(
-        ${PROJECT_SOURCE_DIR}/include
-)
-
-# Tìm tất cả header files
-file(GLOB HEADERS "include/nav_core2/*.h")
-
-# Tạo INTERFACE library (header-only)
-add_library(nav_core2 INTERFACE)
-
-target_link_libraries(nav_core2 INTERFACE 
-        robot_costmap_2d 
-        tf3 
-        nav_grid 
-        robot_nav_2d_msgs
-        robot_cpp
-)
-
-# Set include directories
-target_include_directories(nav_core2
-    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-# Cài đặt header files
-install(DIRECTORY include/nav_core2 
-        DESTINATION include
-        FILES_MATCHING PATTERN "*.h")
-
-# Cài đặt target
-install(TARGETS nav_core2
-        EXPORT nav_core2-targets)
-
-# Export targets
-install(EXPORT nav_core2-targets
-        FILE nav_core2-targets.cmake
-        NAMESPACE nav_core2::
-        DESTINATION lib/cmake/nav_core2)
-
-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
-    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")

src/Navigations/Cores/nav_core2/package.xml

@@ -1,26 +0,0 @@
-<package>
-  <name>nav_core2</name>
-  <version>0.7.10</version>
-  <description>
-    nav_core2 is the second generation of the transform library, which lets
-    the user keep track of multiple coordinate frames over time. nav_core2
-    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/nav_core2</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/Navigations/Cores/nav_core_adapter/CMakeLists.txt

@@ -1,115 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-
-# Tên dự án
-project(nav_core_adapter VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-if(NOT CMAKE_BUILD_TYPE)
-    set(CMAKE_BUILD_TYPE Release)
-endif()
-
-set(PACKAGES_DIR geometry_msgs robot_std_msgs nav_core nav_core2 robot_nav_2d_utils pthread)
-
-find_package(Boost REQUIRED COMPONENTS filesystem system)
-
-# Thư mục include
-include_directories(
-        ${PROJECT_SOURCE_DIR}/include
-)
-# Tạo thư viện shared (.so)
-add_library(costmap_adapter src/costmap_adapter.cpp)
-target_link_libraries(costmap_adapter PRIVATE ${PACKAGES_DIR})
-target_include_directories(costmap_adapter PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-add_library(local_planner_adapter src/local_planner_adapter.cpp)
-target_link_libraries(local_planner_adapter 
-  PRIVATE
-    Boost::filesystem
-    Boost::system
-    dl               
-    costmap_adapter 
-    robot_cpp
-    ${PACKAGES_DIR}
-)
-target_include_directories(local_planner_adapter PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-add_library(global_planner_adapter src/global_planner_adapter.cpp)
-target_link_libraries(global_planner_adapter 
-  PRIVATE
-    Boost::filesystem
-    Boost::system
-    dl               
-    costmap_adapter 
-    ${PACKAGES_DIR}
-)
-
-target_include_directories(global_planner_adapter PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
-# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
-# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
-set(_NAV_CORE_ADAPTER_RPATH
-    "${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
-    "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
-    "${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-    "${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
-    "${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
-)
-
-set_target_properties(costmap_adapter PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-set_target_properties(local_planner_adapter PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-set_target_properties(global_planner_adapter PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-
-# Cài đặt header files
-install(DIRECTORY include/nav_core_adapter 
-        DESTINATION include
-        FILES_MATCHING PATTERN "*.h")
-
-# Cài đặt library
-install(TARGETS global_planner_adapter local_planner_adapter costmap_adapter 
-        EXPORT nav_core_adapter-targets
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib
-        RUNTIME DESTINATION bin)
-
-# Export targets
-install(EXPORT nav_core_adapter-targets
-        FILE nav_core_adapter-targets.cmake
-        DESTINATION lib/cmake/nav_core_adapter)
-
-# Tùy chọn build
-option(BUILD_SHARED_LIBS "Build shared libraries" ON)
-option(BUILD_TESTS "Build test programs" OFF)
-
-# Flags biên dịch
-# Warning flags - disabled to suppress warnings during build
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
-set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
-
-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME}")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
-    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")

src/Navigations/Cores/nav_core_adapter/README.md

@@ -1,48 +0,0 @@
-# nav_core_adapter
-This package contains adapters for using `nav_core` plugins as `nav_core2` plugins and vice versa (more or less). In general, the adaptation process involves
- * Converting between 2d and 3d datatypes.
- * Converting between returning false and throwing exceptions on failure.
-
-We also provide an adapter for using a `robot_costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
-
-## Adapter Classes
- * Global Planner Adapters
-   * `GlobalPlannerAdapter` is used for employing a `nav_core2` global planner interface (such as `dlux_global_planner`) as a `nav_core` plugin, like in `move_base`.
-   * `GlobalPlannerAdapter2` is used for employing a `nav_core` global planner interface (such as `navfn`)
-as a `nav_core2` plugin, like in `locomotor`.
- * Local Planner Adapter
-   * `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity.
-   * There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided.
- * `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROBOT` object. It is not a perfect adaptation, because
-   * `Costmap2DROBOT` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
-   * `setInfo` is not implemented.
-
-## Parameter Setup
-Let's look at a practical example of how to use `dwb_local_planner` in `move_base`.
-
-If you were using `DWA` you would probably have parameters set up like this:
-```
-base_local_planner: dwa_local_planner/DWALocalPlanner
-DWALocalPlanner:
-  acc_lim_x: 0.42
-  ...
-```
-i.e. you specify
- * The name of the planner
- * A bunch of additional parameters within the planner's namespace
-
-To use the adapter, you have to provide additional information.
-```
-base_local_planner: nav_core_adapter::LocalPlannerAdapter
-LocalPlannerAdapter:
-  planner_name: dwb_local_planner::DWBLocalPlanner
-DWBLocalPlanner:
-  acc_lim_x: 0.42
-  ...
-```
-i.e.
- * The name of the planner now points at the adapter
- * The name of the actual planner loaded into the adapter's namespace
- * The planner's parameters still in the planner's namespace.
-
-The process for the global planners is similar.

src/Navigations/Cores/nav_core_adapter/test/unload_test.launch

@@ -1,3 +0,0 @@
-<launch>
-  <test time-limit="10" test-name="unload_test" pkg="nav_core_adapter" type="unload_test" />
-</launch>

src/Navigations/Cores/robot_nav_core/CHANGELOG.rst

@@ -1,5 +1,5 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package nav_core
+Changelog for package robot_nav_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 1.17.3 (2023-01-10)

src/Navigations/Cores/robot_nav_core/CMakeLists.txt

@@ -0,0 +1,120 @@
+cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_nav_core with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_nav_core with Standalone CMake")
+endif()
+
+# Tên dự án
+project(robot_nav_core VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# ========================================================
+# Find dependencies
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    robot_costmap_2d
+    tf3
+    robot_protocol_msgs
+  )
+endif()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  catkin_package(
+    INCLUDE_DIRS include
+    # Header-only library; keep LIBRARIES for visibility when exporting
+    LIBRARIES robot_nav_core
+    CATKIN_DEPENDS robot_costmap_2d tf3 robot_protocol_msgs
+  )
+endif()
+
+# Cho phép các project khác include được header của robot_nav_core
+set(${PROJECT_NAME}_INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+    PARENT_SCOPE
+)
+
+include_directories(
+        ${PROJECT_SOURCE_DIR}/include
+)
+
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+# Link libraries
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME} INTERFACE 
+      ${catkin_LIBRARIES}
+  )
+else()
+  target_link_libraries(${PROJECT_NAME} INTERFACE 
+      robot_costmap_2d 
+      tf3 
+      robot_protocol_msgs
+  )
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
+# Set include directories
+target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# ========================================================
+# Installation
+# ========================================================
+
+install(TARGETS ${PROJECT_NAME} 
+        EXPORT ${PROJECT_NAME}-targets
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/${PROJECT_NAME} 
+          DESTINATION include
+          FILES_MATCHING PATTERN "*.h")
+endif()
+
+# Export targets
+install(EXPORT ${PROJECT_NAME}-targets
+         FILE ${PROJECT_NAME}-targets.cmake
+        DESTINATION lib/cmake/${PROJECT_NAME})
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Headers found:")
+foreach(hdr ${HEADERS})
+    message(STATUS "  - ${hdr}")
+endforeach()
+message(STATUS "=================================")

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_global_planner.h

@@ -34,15 +34,15 @@
 *
 * Author: Eitan Marder-Eppstein
 *********************************************************************/
-#ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H
+#ifndef ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H
-#define NAV_CORE_BASE_GLOBAL_PLANNER_H
+#define ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H
 
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <robot_protocol_msgs/Order.h>
 #include <memory>
 
-namespace nav_core {
+namespace robot_nav_core {
   /**
    * @class BaseGlobalPlanner
    * @brief Provides an interface for global planners used in navigation. All global planners written as plugins for the navigation stack must adhere to this interface.
@@ -107,6 +107,6 @@ namespace nav_core {
     protected:
       BaseGlobalPlanner(){}
   };
-}  // namespace nav_core
+}  // namespace robot_nav_core
 
-#endif  // NAV_CORE_BASE_GLOBAL_PLANNER_H
+#endif  // ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -34,8 +34,8 @@
  *
  * Author: Eitan Marder-Eppstein
  *********************************************************************/
-#ifndef NAV_CORE_BASE_LOCAL_PLANNER_H
+#ifndef ROBOT_NAV_CORE_BASE_LOCAL_PLANNER_H
-#define NAV_CORE_BASE_LOCAL_PLANNER_H
+#define ROBOT_NAV_CORE_BASE_LOCAL_PLANNER_H
 
 #include <robot_geometry_msgs/PoseStamped.h>
 #include <robot_geometry_msgs/Twist.h>
@@ -43,7 +43,7 @@
 #include <tf3/buffer_core.h>
 #include <memory>
 
-namespace nav_core
+namespace robot_nav_core
 {
   /**
    * @class BaseLocalPlanner
@@ -61,7 +61,7 @@ namespace nav_core
     virtual bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) = 0;
 
     /**
-     * @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
+     * @brief Swap object will be injected interjace robot_nav_core::BaseLocalPlanner
      * @param  planner_name  The object name 
      * @return True if instance object is successed, False otherwise
      */
@@ -148,6 +148,6 @@ namespace nav_core
   protected:
     BaseLocalPlanner() {}
   };
-} // namespace nav_core
+} // namespace robot_nav_core
 
-#endif // NAV_CORE_BASE_LOCAL_PLANNER_H
+#endif // ROBOT_NAV_CORE_BASE_LOCAL_PLANNER_H

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/parameter_magic.h

@@ -35,10 +35,10 @@
 * Author: David Lu
 *********************************************************************/
 
-#ifndef NAV_CORE_PARAMETER_MAGIC_H
+#ifndef ROBOT_NAV_CORE_PARAMETER_MAGIC_H
-#define NAV_CORE_PARAMETER_MAGIC_H
+#define ROBOT_NAV_CORE_PARAMETER_MAGIC_H
 
-namespace nav_core
+namespace robot_nav_core
 {
 
 /**
@@ -81,6 +81,6 @@ void warnRenamedParameter(const std::string current_name, const std::string old_
   }
 }
 
-}  // namespace nav_core
+}  // namespace robot_nav_core
 
-#endif  // NAV_CORE_PARAMETER_MAGIC_H
+#endif  // ROBOT_NAV_CORE_PARAMETER_MAGIC_H

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/recovery_behavior.h

@@ -34,14 +34,14 @@
 *
 * Author: Eitan Marder-Eppstein
 *********************************************************************/
-#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
+#ifndef ROBOT_NAV_CORE_RECOVERY_BEHAVIOR_H
-#define NAV_CORE_RECOVERY_BEHAVIOR_H
+#define ROBOT_NAV_CORE_RECOVERY_BEHAVIOR_H
 
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <tf3/buffer_core.h>
 #include <memory>
 
-namespace nav_core {
+namespace robot_nav_core {
   /**
    * @class RecoveryBehavior
    * @brief Provides an interface for recovery behaviors used in navigation. All recovery behaviors written as plugins for the navigation stack must adhere to this interface.
@@ -72,6 +72,6 @@ namespace nav_core {
     protected:
       RecoveryBehavior(){}
   };
-}  // namespace nav_core
+}  // namespace robot_nav_core
 
-#endif  // NAV_CORE_RECOVERY_BEHAVIOR_H
+#endif  // ROBOT_NAV_CORE_RECOVERY_BEHAVIOR_H

src/Navigations/Cores/robot_nav_core/package.xml

@@ -1,9 +1,9 @@
 <package>
-  <name>nav_core</name>
+  <name>robot_nav_core</name>
   <version>0.7.10</version>
   <description>
-    nav_core is the second generation of the transform library, which lets
+    robot_nav_core is the second generation of the transform library, which lets
-    the user keep track of multiple coordinate frames over time. nav_core
+    the user keep track of multiple coordinate frames over time. robot_nav_core
     maintains the relationship between coordinate frames in a tree
     structure buffered in time, and lets the user transform points,
     vectors, etc between any two coordinate frames at any desired
@@ -15,12 +15,17 @@
   <maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
   <license>BSD</license>
 
-  <url type="website">http://www.ros.org/wiki/nav_core</url>
+  <url type="website">http://www.ros.org/wiki/robot_nav_core</url>
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>robot_costmap_2d</build_depend>
+  <run_depend>robot_costmap_2d</run_depend>
 
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <build_depend>tf3</build_depend>
+  <run_depend>tf3</run_depend>
+
+  <build_depend>robot_protocol_msgs</build_depend>
+  <run_depend>robot_protocol_msgs</run_depend>
 
 </package>

src/Navigations/Cores/robot_nav_core2/CMakeLists.txt

@@ -0,0 +1,138 @@
+cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_nav_core2 with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_nav_core2 with Standalone CMake")
+endif()
+
+# Tên dự án
+project(robot_nav_core2 VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# ========================================================
+# Find dependencies
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    robot_costmap_2d
+    tf3
+    nav_grid
+    robot_nav_2d_msgs
+    robot_cpp
+    robot_sensor_msgs
+    robot_std_msgs
+    geometry_msgs
+    robot_nav_msgs
+    robot_map_msgs
+    robot_laser_geometry
+    robot_visualization_msgs
+    robot_voxel_grid
+    robot_tf3_geometry_msgs
+    robot_tf3_sensor_msgs
+    data_convert
+    robot_xmlrpcpp
+  )
+endif()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  catkin_package(
+    INCLUDE_DIRS include
+    # Header-only library; keep LIBRARIES for visibility when exporting
+    LIBRARIES robot_nav_core2
+    CATKIN_DEPENDS robot_costmap_2d tf3 nav_grid robot_nav_2d_msgs robot_cpp
+  )
+endif()
+
+# Cho phép các project khác include được header của robot_nav_core2
+set(robot_nav_core2_INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+    PARENT_SCOPE
+)
+
+include_directories(
+        ${PROJECT_SOURCE_DIR}/include
+)
+
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
+# Tìm tất cả header files
+file(GLOB HEADERS "include/robot_nav_core2/*.h")
+
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+# Link libraries
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME} INTERFACE 
+      ${catkin_LIBRARIES}
+  )
+else()
+  target_link_libraries(${PROJECT_NAME} INTERFACE 
+      robot_costmap_2d 
+      tf3 
+      nav_grid 
+      robot_nav_2d_msgs
+      robot_cpp
+  )
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
+# Set include directories
+target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+)
+
+# ========================================================
+# Installation
+# ========================================================
+
+# Cài đặt target
+install(TARGETS ${PROJECT_NAME}
+        EXPORT ${PROJECT_NAME}-targets)
+
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/${PROJECT_NAME} 
+          DESTINATION include
+          FILES_MATCHING PATTERN "*.h")
+endif()
+
+# Export targets
+install(EXPORT ${PROJECT_NAME}-targets
+        FILE ${PROJECT_NAME}-targets.cmake
+        NAMESPACE ${PROJECT_NAME}::
+        DESTINATION lib/cmake/${PROJECT_NAME})
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Headers found:")
+foreach(hdr ${HEADERS})
+    message(STATUS "  - ${hdr}")
+endforeach()
+message(STATUS "=================================")

src/Navigations/Cores/robot_nav_core2/README.md

@@ -1,5 +1,5 @@
-# nav_core2
+# robot_nav_core2
-A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces.
+A replacement interface for [robot_nav_core2](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_nav_core2/include/robot_nav_core2) that defines basic two dimensional planner interfaces.
 
 There were a few key reasons for creating new interfaces rather than extending the old ones.
  * Use `robot_nav_2d_msgs` to eliminate unused data fields
@@ -13,29 +13,29 @@ There were a few key reasons for creating new interfaces rather than extending t
  * Initialization also started an update thread, which is also not always needed in testing.
  * Using `Costmap2DROBOT` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
 
-The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid<unsigned char>` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as
+The [`robot_nav_core2::Costmap`](include/robot_nav_core2/costmap.h) interface extends the `nav_grid::NavGrid<unsigned char>` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as
  * a mutex
  * a way to potentially track changes to the costmap
  * a public `update` method that can be called in whatever thread you please
 
 The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach.
 
-One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROBOT`.
+One basic implementation is provided in [`BasicCostmap`](include/robot_nav_core2/basic_costmap.h). You can also use the `robot_nav_core2_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROBOT`.
 
 Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms.
 
 ## Global Planner
-Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_global_planner.h) to the new [nav_core2/GlobalPlanner](include/nav_core2/global_planner.h).
+Let us compare the old [robot_nav_core2::BaseGlobalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_nav_core2/include/robot_nav_core2/base_global_planner.h) to the new [robot_nav_core2/GlobalPlanner](include/robot_nav_core2/global_planner.h).
 
-| `nav_core` | `nav_core2` | comments |
+| `robot_nav_core2` | `robot_nav_core2` | comments |
 |---|--|---|
 |`void initialize(std::string, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
 |`bool makePlan(const robot_geometry_msgs::PoseStamped&, const robot_geometry_msgs::PoseStamped&, std::vector<robot_geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
 
 ## Local Planner
-Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
+Now let's compare the old [robot_nav_core2::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_nav_core2/include/robot_nav_core2/base_local_planner.h) to the new [robot_nav_core2/LocalPlanner](include/robot_nav_core2/local_planner.h).
 
-| `nav_core` | `nav_core2` | comments |
+| `robot_nav_core2` | `robot_nav_core2` | comments |
 |---|--|---|
 |`void initialize(std::string, tf::TransformListener*, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
 |(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
@@ -44,9 +44,9 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
 |`bool isGoalReached()` | `bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
 
 ## Exceptions
-A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.
+A hierarchical collection of [exceptions](include/robot_nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.
 ![exception hierarchy tree](doc/exceptions.png)
 Each exception has a corresponding integer "result code" that can be used in ROS interfaces where passing the C++ object is infeasible. Note that due to the hierarchy, the result_code will be for the child-most exception. For example, if you throw a `StartBoundsException` which has a corresponding result code of `6`, it could also be seen as a `InvalidStartPoseException`, `GlobalPlannerException`, `PlannerException` or `NavCore2Exception`, all of which would also have the result code of `6`.
 
 ## Bounds
-For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive).
+For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/robot_nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive).

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/basic_costmap.h

@@ -32,16 +32,16 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NAV_CORE2_BASIC_COSTMAP_H
+#ifndef ROBOT_NAV_CORE2_BASIC_COSTMAP_H
-#define NAV_CORE2_BASIC_COSTMAP_H
+#define ROBOT_NAV_CORE2_BASIC_COSTMAP_H
 
-#include <nav_core2/costmap.h>
+#include <robot_nav_core2/costmap.h>
 #include <string>
 #include <vector>
 
-namespace nav_core2
+namespace robot_nav_core2
 {
-class BasicCostmap : public nav_core2::Costmap
+class BasicCostmap : public robot_nav_core2::Costmap
 {
 public:
   // Standard Costmap Interface
@@ -63,6 +63,6 @@ protected:
   mutex_t my_mutex_;
   std::vector<unsigned char> data_;
 };
-}  // namespace nav_core2
+}  // namespace robot_nav_core2
 
-#endif  // NAV_CORE2_BASIC_COSTMAP_H
+#endif  // ROBOT_NAV_CORE2_BASIC_COSTMAP_H

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/bounds.h

@@ -32,14 +32,14 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NAV_CORE2_BOUNDS_H
+#ifndef ROBOT_NAV_CORE2_BOUNDS_H
-#define NAV_CORE2_BOUNDS_H
+#define ROBOT_NAV_CORE2_BOUNDS_H
 
 #include <algorithm>
 #include <limits>
 #include <string>
 
-namespace nav_core2
+namespace robot_nav_core2
 {
 /**
  * @brief Templatized method for checking if a value falls inside a one-dimensional range
@@ -204,6 +204,6 @@ public:
   unsigned int getHeight() const { return getDimension(min_y_, max_y_); }
 };
 
-}  // namespace nav_core2
+}  // namespace robot_nav_core2
 
-#endif  // NAV_CORE2_BOUNDS_H
+#endif  // ROBOT_NAV_CORE2_BOUNDS_H

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/common.h

@@ -31,12 +31,12 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef NAV_CORE2_COMMON_H
+#ifndef ROBOT_NAV_CORE2_COMMON_H
-#define NAV_CORE2_COMMON_H
+#define ROBOT_NAV_CORE2_COMMON_H
 
 #include <tf3/buffer_core.h>
 #include <memory>
 
 using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
 
-#endif  // NAV_CORE2_COMMON_H
+#endif  // ROBOT_NAV_CORE2_COMMON_H

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/costmap.h

@@ -32,18 +32,18 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NAV_CORE2_COSTMAP_H
+#ifndef ROBOT_NAV_CORE2_COSTMAP_H
-#define NAV_CORE2_COSTMAP_H
+#define ROBOT_NAV_CORE2_COSTMAP_H
 
 #include <robot/node_handle.h>
 #include <nav_grid/nav_grid.h>
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
-#include <nav_core2/bounds.h>
+#include <robot_nav_core2/bounds.h>
 #include <boost/thread.hpp>
 #include <memory>
 #include <string>
 
-namespace nav_core2
+namespace robot_nav_core2
 {
 
 class Costmap : public nav_grid::NavGrid<unsigned char>
@@ -152,6 +152,6 @@ public:
     return UIntBounds();
   }
 };
-}  // namespace nav_core2
+}  // namespace robot_nav_core2
 
-#endif  // NAV_CORE2_COSTMAP_H
+#endif  // ROBOT_NAV_CORE2_COSTMAP_H

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/exceptions.h

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef NAV_CORE2_EXCEPTIONS_H
+#ifndef ROBOT_NAV_CORE2_EXCEPTIONS_H
-#define NAV_CORE2_EXCEPTIONS_H
+#define ROBOT_NAV_CORE2_EXCEPTIONS_H
 
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <stdexcept>
@@ -41,7 +41,7 @@
 #include <memory>
 
 /**************************************************
- * The nav_core2 Planning Exception Hierarchy!!
+ * The robot_nav_core2 Planning Exception Hierarchy!!
  * (with arbitrary integer result codes)
  **************************************************
  *   NavCore2Exception
@@ -66,7 +66,7 @@
  * -1 Unknown
  **************************************************/
 
-namespace nav_core2
+namespace robot_nav_core2
 {
 
 inline std::string poseToString(const robot_nav_2d_msgs::Pose2DStamped& pose)
@@ -316,6 +316,6 @@ public:
     : LocalPlannerException(description, result_code) {}
 };
 
-}  // namespace nav_core2
+}  // namespace robot_nav_core2
 
-#endif  // NAV_CORE2_EXCEPTIONS_H
+#endif  // ROBOT_NAV_CORE2_EXCEPTIONS_H

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h

@@ -31,17 +31,17 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef NAV_CORE2_GLOBAL_PLANNER_H
+#ifndef ROBOT_NAV_CORE2_GLOBAL_PLANNER_H
-#define NAV_CORE2_GLOBAL_PLANNER_H
+#define ROBOT_NAV_CORE2_GLOBAL_PLANNER_H
 
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
-#include <nav_core2/costmap.h>
+#include <robot_nav_core2/costmap.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <string>
 #include <memory>
 
-namespace nav_core2
+namespace robot_nav_core2
 {
 
 /**
@@ -80,6 +80,6 @@ public:
   virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
                                        const robot_nav_2d_msgs::Pose2DStamped& goal) = 0;
 };
-}  // namespace nav_core2
+}  // namespace robot_nav_core2
 
-#endif  // NAV_CORE2_GLOBAL_PLANNER_H
+#endif  // ROBOT_NAV_CORE2_GLOBAL_PLANNER_H

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -31,11 +31,11 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef NAV_CORE2_LOCAL_PLANNER_H
+#ifndef ROBOT_NAV_CORE2_LOCAL_PLANNER_H
-#define NAV_CORE2_LOCAL_PLANNER_H
+#define ROBOT_NAV_CORE2_LOCAL_PLANNER_H
 
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
-#include <nav_core2/costmap.h>
+#include <robot_nav_core2/costmap.h>
 #include <robot_nav_2d_msgs/Path2D.h>
 #include <robot_nav_2d_msgs/Pose2DStamped.h>
 #include <robot_nav_2d_msgs/Twist2D.h>
@@ -45,7 +45,7 @@
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <memory>
 
-namespace nav_core2
+namespace robot_nav_core2
 {
 
   /**
@@ -168,6 +168,6 @@ namespace nav_core2
      */
     virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0;
   };
-} // namespace nav_core2
+} // namespace robot_nav_core2
 
-#endif // NAV_CORE2_LOCAL_PLANNER_H
+#endif // ROBOT_NAV_CORE2_LOCAL_PLANNER_H

src/Navigations/Cores/robot_nav_core2/package.xml

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

src/Navigations/Cores/robot_nav_core2/src/basic_costmap.cpp

@@ -32,9 +32,9 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <nav_core2/basic_costmap.h>
+#include <robot_nav_core2/basic_costmap.h>
 
-namespace nav_core2
+namespace robot_nav_core2
 {
 
 void BasicCostmap::reset()
@@ -57,4 +57,4 @@ void BasicCostmap::setValue(const unsigned int x, const unsigned int y, const un
   data_[getIndex(x, y)] = value;
 }
 
-}  // namespace nav_core2
+}  // namespace robot_nav_core2

src/Navigations/Cores/robot_nav_core2/test/bounds_test.cpp

@@ -32,10 +32,10 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 #include <gtest/gtest.h>
-#include <nav_core2/bounds.h>
+#include <robot_nav_core2/bounds.h>
 
-using nav_core2::Bounds;
+using robot_nav_core2::Bounds;
-using nav_core2::UIntBounds;
+using robot_nav_core2::UIntBounds;
 
 TEST(Bounds, test_bounds_simple)
 {

src/Navigations/Cores/robot_nav_core2/test/exception_test.cpp

@@ -32,7 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 #include <gtest/gtest.h>
-#include <nav_core2/exceptions.h>
+#include <robot_nav_core2/exceptions.h>
 #include <string>
 
 TEST(Exceptions, direct_code_access)
@@ -41,18 +41,18 @@ TEST(Exceptions, direct_code_access)
   // (This version does not create any copies of the exception)
   try
   {
-    throw nav_core2::GlobalPlannerTimeoutException("test case");
+    throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
   }
-  catch (nav_core2::GlobalPlannerTimeoutException& x)
+  catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
   {
     EXPECT_EQ(x.getResultCode(), 12);
   }
 
   try
   {
-    throw nav_core2::GlobalPlannerTimeoutException("test case");
+    throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
   }
-  catch (nav_core2::PlannerException& x)
+  catch (robot_nav_core2::PlannerException& x)
   {
     EXPECT_EQ(x.getResultCode(), 12);
   }
@@ -62,12 +62,12 @@ TEST(Exceptions, indirect_code_access)
 {
   // Make sure the caught exceptions have the same types as the thrown exception
   // (This version copies the exception to a new object with the parent type)
-  nav_core2::PlannerException e("");
+  robot_nav_core2::PlannerException e("");
   try
   {
-    throw nav_core2::GlobalPlannerTimeoutException("test case");
+    throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
   }
-  catch (nav_core2::GlobalPlannerTimeoutException& x)
+  catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
   {
     e = x;
   }
@@ -79,28 +79,28 @@ TEST(Exceptions, rethrow)
   // This version tests the ability to catch specific exceptions when rethrown
   // A copy of the exception is made and rethrown, with the goal being able to catch the specific type
   // and not the parent type.
-  nav_core2::NavCore2ExceptionPtr e;
+  robot_nav_core2::NavCore2ExceptionPtr e;
   try
   {
-    throw nav_core2::GlobalPlannerTimeoutException("test case");
+    throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
   }
-  catch (nav_core2::GlobalPlannerTimeoutException& x)
+  catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
   {
     e = std::current_exception();
   }
 
-  EXPECT_EQ(nav_core2::getResultCode(e), 12);
+  EXPECT_EQ(robot_nav_core2::getResultCode(e), 12);
 
   try
   {
     std::rethrow_exception(e);
   }
-  catch (nav_core2::GlobalPlannerTimeoutException& x)
+  catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
   {
     EXPECT_EQ(x.getResultCode(), 12);
     SUCCEED();
   }
-  catch (nav_core2::PlannerException& x)
+  catch (robot_nav_core2::PlannerException& x)
   {
     FAIL() << "PlannerException caught instead of specific exception";
   }
@@ -108,10 +108,10 @@ TEST(Exceptions, rethrow)
 
 TEST(Exceptions, weird_exception)
 {
-  nav_core2::NavCore2ExceptionPtr e;
+  robot_nav_core2::NavCore2ExceptionPtr e;
 
   // Check what happens with no exception
-  EXPECT_EQ(nav_core2::getResultCode(e), -1);
+  EXPECT_EQ(robot_nav_core2::getResultCode(e), -1);
 
   // Check what happens with a non NavCore2Exception
   try
@@ -123,7 +123,7 @@ TEST(Exceptions, weird_exception)
     e = std::current_exception();
   }
 
-  EXPECT_EQ(nav_core2::getResultCode(e), -1);
+  EXPECT_EQ(robot_nav_core2::getResultCode(e), -1);
 }
 
 int main(int argc, char **argv)

src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt

@@ -0,0 +1,212 @@
+cmake_minimum_required(VERSION 3.10)
+
+# ========================================================
+# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
+# ========================================================
+
+# Detect if building with Catkin
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_nav_core_adapter with Catkin")
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_nav_core_adapter with Standalone CMake")
+endif()
+
+# Tên dự án
+project(robot_nav_core_adapter VERSION 1.0.0 LANGUAGES CXX)
+
+# Chuẩn C++
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+# ========================================================
+# Find dependencies
+# ========================================================
+
+# Find system dependencies
+find_package(Boost REQUIRED COMPONENTS filesystem system)
+
+if(BUILDING_WITH_CATKIN)
+  find_package(catkin REQUIRED COMPONENTS
+    geometry_msgs
+    robot_std_msgs
+    robot_nav_core
+    robot_nav_core2
+    robot_nav_2d_utils
+    robot_cpp
+  )
+endif()
+
+# Dependencies packages (internal libraries)
+set(PACKAGES_DIR geometry_msgs robot_std_msgs robot_nav_core robot_nav_core2 robot_nav_2d_utils pthread)
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+
+if(BUILDING_WITH_CATKIN)
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES costmap_adapter local_planner_adapter global_planner_adapter
+    CATKIN_DEPENDS geometry_msgs robot_std_msgs robot_nav_core robot_nav_core2 robot_nav_2d_utils robot_cpp
+  )
+endif()
+
+# Thư mục include
+include_directories(
+        ${PROJECT_SOURCE_DIR}/include
+)
+
+if(BUILDING_WITH_CATKIN)
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
+# ========================================================
+# Build
+# ========================================================
+
+# Tạo thư viện shared (.so)
+add_library(costmap_adapter src/costmap_adapter.cpp)
+
+# Link libraries
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(costmap_adapter 
+    PRIVATE ${catkin_LIBRARIES}
+  )
+else()
+  target_link_libraries(costmap_adapter 
+    PRIVATE ${PACKAGES_DIR}
+  )
+endif()
+target_include_directories(costmap_adapter PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+add_library(local_planner_adapter src/local_planner_adapter.cpp)
+
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(local_planner_adapter 
+    PRIVATE
+      Boost::filesystem
+      Boost::system
+      dl               
+      costmap_adapter 
+      ${catkin_LIBRARIES}
+  )
+else()
+  target_link_libraries(local_planner_adapter 
+    PRIVATE
+      Boost::filesystem
+      Boost::system
+      dl               
+      costmap_adapter 
+      robot_cpp
+      ${PACKAGES_DIR}
+  )
+endif()
+target_include_directories(local_planner_adapter PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+add_library(global_planner_adapter src/global_planner_adapter.cpp)
+
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(global_planner_adapter 
+    PRIVATE
+      Boost::filesystem
+      Boost::system
+      dl               
+      costmap_adapter 
+      ${catkin_LIBRARIES}
+  )
+else()
+  target_link_libraries(global_planner_adapter 
+    PRIVATE
+      Boost::filesystem
+      Boost::system
+      dl               
+      costmap_adapter 
+      ${PACKAGES_DIR}
+  )
+endif()
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(costmap_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(local_planner_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(global_planner_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
+target_include_directories(global_planner_adapter PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
+# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
+# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
+set(_ROBOT_NAV_CORE_ADAPTER_RPATH
+    "${CMAKE_BINARY_DIR}/src/Navigations/Cores/robot_nav_core_adapter"
+    "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
+    "${CMAKE_BINARY_DIR}/src/Libraries/tf3"
+    "${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
+    "${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
+)
+
+set_target_properties(costmap_adapter PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+set_target_properties(local_planner_adapter PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+set_target_properties(global_planner_adapter PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+)
+
+# ========================================================
+# Installation
+# ========================================================
+
+# Cài đặt library
+install(TARGETS global_planner_adapter local_planner_adapter costmap_adapter 
+        EXPORT robot_nav_core_adapter-targets
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin)
+
+if(NOT BUILDING_WITH_CATKIN)
+  # Cài đặt header files (standalone)
+  install(DIRECTORY include/robot_nav_core_adapter 
+          DESTINATION include
+          FILES_MATCHING PATTERN "*.h")
+endif()
+
+# Export targets
+install(EXPORT robot_nav_core_adapter-targets
+        FILE robot_nav_core_adapter-targets.cmake
+        DESTINATION lib/cmake/robot_nav_core_adapter)
+
+# Tùy chọn build
+option(BUILD_SHARED_LIBS "Build shared libraries" ON)
+option(BUILD_TESTS "Build test programs" OFF)
+
+# Flags biên dịch
+# Warning flags - disabled to suppress warnings during build
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
+set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+# In thông tin cấu hình
+message(STATUS "=================================")
+message(STATUS "Project: ${PROJECT_NAME}")
+message(STATUS "Version: ${PROJECT_VERSION}")
+message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+message(STATUS "Headers found:")
+foreach(hdr ${HEADERS})
+    message(STATUS "  - ${hdr}")
+endforeach()
+message(STATUS "=================================")

src/Navigations/Cores/robot_nav_core_adapter/README.md

@@ -0,0 +1,48 @@
+# robot_nav_core_adapter
+This package contains adapters for using `robot_nav_core2` plugins as `robot_nav_core2` plugins and vice versa (more or less). In general, the adaptation process involves
+ * Converting between 2d and 3d datatypes.
+ * Converting between returning false and throwing exceptions on failure.
+
+We also provide an adapter for using a `robot_costmap_2d::Costmap2DROBOT` as a plugin for the `robot_nav_core2::Costmap` interface.
+
+## Adapter Classes
+ * Global Planner Adapters
+   * `GlobalPlannerAdapter` is used for employing a `robot_nav_core2` global planner interface (such as `dlux_global_planner`) as a `robot_nav_core2` plugin, like in `move_base`.
+   * `GlobalPlannerAdapter2` is used for employing a `robot_nav_core2` global planner interface (such as `navfn`)
+as a `robot_nav_core2` plugin, like in `locomotor`.
+ * Local Planner Adapter
+   * `LocalPlannerAdapter` is used for employing a `robot_nav_core2` local planner interface (such as `dwb_local_planner`) as a `robot_nav_core2` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity.
+   * There is no `LocalPlannerAdapter2`. The `robot_nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `robot_nav_core2` counterpart. This information would be ignored by a `robot_nav_core2` planner, so no adapter is provided.
+ * `CostmapAdapter` provides most of the functionality from `robot_nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROBOT` object. It is not a perfect adaptation, because
+   * `Costmap2DROBOT` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
+   * `setInfo` is not implemented.
+
+## Parameter Setup
+Let's look at a practical example of how to use `dwb_local_planner` in `move_base`.
+
+If you were using `DWA` you would probably have parameters set up like this:
+```
+base_local_planner: dwa_local_planner/DWALocalPlanner
+DWALocalPlanner:
+  acc_lim_x: 0.42
+  ...
+```
+i.e. you specify
+ * The name of the planner
+ * A bunch of additional parameters within the planner's namespace
+
+To use the adapter, you have to provide additional information.
+```
+base_local_planner: robot_nav_core_adapter::LocalPlannerAdapter
+LocalPlannerAdapter:
+  planner_name: dwb_local_planner::DWBLocalPlanner
+DWBLocalPlanner:
+  acc_lim_x: 0.42
+  ...
+```
+i.e.
+ * The name of the planner now points at the adapter
+ * The name of the actual planner loaded into the adapter's namespace
+ * The planner's parameters still in the planner's namespace.
+
+The process for the global planners is similar.

src/Navigations/Cores/robot_nav_core_adapter/cfg/NavCoreAdapter.cfg

@@ -1,9 +1,9 @@
 #!/usr/bin/env python
 
-PACKAGE = 'nav_core_adapter'
+PACKAGE = 'robot_nav_core_adapter'
 
 from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
 
 gen = ParameterGenerator()
 gen.add("planner_name", str_t, 0, "The name of the plugin for the local planner to use with move_base.", "base_local_planner/TrajectoryPlannerROS")
-exit(gen.generate(PACKAGE, "nav_core_adapter", "NavCoreAdapter"))
+exit(gen.generate(PACKAGE, "robot_nav_core_adapter", "NavCoreAdapter"))

src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/costmap_adapter.h

@@ -32,19 +32,19 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
+#ifndef ROBOT_NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
-#define NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
+#define ROBOT_NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
 
-#include <nav_core2/common.h>
+#include <robot_nav_core2/common.h>
-#include <nav_core2/costmap.h>
+#include <robot_nav_core2/costmap.h>
 #include <robot_costmap_2d/costmap_2d_robot.h>
 #include <string>
 
-namespace nav_core_adapter
+namespace robot_nav_core_adapter
 {
 nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot);
 
-class CostmapAdapter : public nav_core2::Costmap
+class CostmapAdapter : public robot_nav_core2::Costmap
 {
 public:
   /**
@@ -61,7 +61,7 @@ public:
 
   // Standard Costmap Interface
   void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
-  nav_core2::Costmap::mutex_t* getMutex() override;
+  robot_nav_core2::Costmap::mutex_t* getMutex() override;
 
   // NavGrid Interface
   void reset() override;
@@ -80,6 +80,6 @@ protected:
   bool needs_destruction_;
 };
 
-}  // namespace nav_core_adapter
+}  // namespace robot_nav_core_adapter
 
-#endif  // NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
+#endif  // ROBOT_NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H

src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/global_planner_adapter.h

@@ -32,45 +32,44 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
+#ifndef ROBOT_NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
-#define NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
+#define ROBOT_NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
 
-#include <nav_core/base_global_planner.h>
+#include <robot_nav_core/base_global_planner.h>
-#include <nav_core2/global_planner.h>
+#include <robot_nav_core2/global_planner.h>
-// #include <pluginlib/class_loader.h>
 #include <boost/dll/import.hpp>
 #include <string>
 #include <vector>
 
-namespace nav_core_adapter
+namespace robot_nav_core_adapter
 {
 
 /**
  * @class GlobalPlannerAdapter
- * @brief used for employing a `nav_core` global planner (such as `navfn`) as a `nav_core2` plugin, like in `locomotor`.
+ * @brief used for employing a `robot_nav_core2` global planner (such as `navfn`) as a `robot_nav_core2` plugin, like in `locomotor`.
  */
-class GlobalPlannerAdapter: public nav_core2::GlobalPlanner
+class GlobalPlannerAdapter: public robot_nav_core2::GlobalPlanner
 {
 public:
   GlobalPlannerAdapter();
 
   // Nav Core 2 Global Planner Interface
   void initialize(const robot::NodeHandle& parent, const std::string& name,
-                  TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
+                  TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
   robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
                                const robot_nav_2d_msgs::Pose2DStamped& goal) override;
 
-  static nav_core2::GlobalPlanner::Ptr create();
+  static robot_nav_core2::GlobalPlanner::Ptr create();
 
 protected:
   // Plugin handling
-  boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
+  boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
-  nav_core::BaseGlobalPlanner::Ptr planner_;
+  robot_nav_core::BaseGlobalPlanner::Ptr planner_;
 
   robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
-  nav_core2::Costmap::Ptr costmap_;
+  robot_nav_core2::Costmap::Ptr costmap_;
 };
 
-}  // namespace nav_core_adapter
+}  // namespace robot_nav_core_adapter
 
-#endif  // NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
+#endif  // ROBOT_NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H