fix move base

- Remove automatic initialize() call from MoveBase constructor
- Comment out frame_id and plugins in local costmap config
- Remove unused two_points_global_params.yaml config file

.gitignore

@@ -419,3 +419,6 @@ FodyWeavers.xsd
 *.vsix
 
 build
+install
+devel
+

.gitmodules

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

.vscode/settings.json

@@ -1,71 +0,0 @@
-{
-    "files.associations": {
-        "stdexcept": "cpp",
-        "cctype": "cpp",
-        "clocale": "cpp",
-        "cmath": "cpp",
-        "csignal": "cpp",
-        "cstdarg": "cpp",
-        "cstddef": "cpp",
-        "cstdio": "cpp",
-        "cstdlib": "cpp",
-        "cstring": "cpp",
-        "ctime": "cpp",
-        "cwchar": "cpp",
-        "cwctype": "cpp",
-        "array": "cpp",
-        "atomic": "cpp",
-        "strstream": "cpp",
-        "bit": "cpp",
-        "bitset": "cpp",
-        "chrono": "cpp",
-        "compare": "cpp",
-        "complex": "cpp",
-        "concepts": "cpp",
-        "condition_variable": "cpp",
-        "cstdint": "cpp",
-        "deque": "cpp",
-        "list": "cpp",
-        "map": "cpp",
-        "set": "cpp",
-        "unordered_map": "cpp",
-        "vector": "cpp",
-        "exception": "cpp",
-        "algorithm": "cpp",
-        "functional": "cpp",
-        "iterator": "cpp",
-        "memory": "cpp",
-        "memory_resource": "cpp",
-        "numeric": "cpp",
-        "optional": "cpp",
-        "random": "cpp",
-        "ratio": "cpp",
-        "string": "cpp",
-        "string_view": "cpp",
-        "system_error": "cpp",
-        "tuple": "cpp",
-        "type_traits": "cpp",
-        "utility": "cpp",
-        "fstream": "cpp",
-        "initializer_list": "cpp",
-        "iomanip": "cpp",
-        "iosfwd": "cpp",
-        "iostream": "cpp",
-        "istream": "cpp",
-        "limits": "cpp",
-        "mutex": "cpp",
-        "new": "cpp",
-        "ostream": "cpp",
-        "ranges": "cpp",
-        "sstream": "cpp",
-        "stop_token": "cpp",
-        "streambuf": "cpp",
-        "thread": "cpp",
-        "cfenv": "cpp",
-        "cinttypes": "cpp",
-        "typeindex": "cpp",
-        "typeinfo": "cpp",
-        "variant": "cpp",
-        "codecvt": "cpp"
-    }
-}

CMAKE_BUILD_README.md

@@ -0,0 +1,402 @@
+# Hướng dẫn Build bằng CMake
+
+Hướng dẫn chi tiết để build project `pnkx_nav_core` sử dụng CMake standalone (không cần Catkin workspace).
+
+## Mục lục
+
+1. [Yêu cầu hệ thống](#yêu-cầu-hệ-thống)
+2. [Cài đặt Dependencies](#cài-đặt-dependencies)
+3. [Build Project](#build-project)
+4. [Các tùy chọn Build](#các-tùy-chọn-build)
+5. [Install](#install)
+6. [Sử dụng trong project khác](#sử-dụng-trong-project-khác)
+7. [Troubleshooting](#troubleshooting)
+
+---
+
+## Yêu cầu hệ thống
+
+### Hệ điều hành
+- Linux (Ubuntu 18.04+ hoặc tương đương)
+### Compiler
+- **GCC**: phiên bản 7.0 trở lên (hỗ trợ C++17)
+- **Clang**: phiên bản 5.0 trở lên
+
+### CMake
+- **CMake**: phiên bản 3.10 trở lên
+
+Kiểm tra phiên bản:
+```bash
+cmake --version
+g++ --version
+```
+
+---
+
+## Cài đặt Dependencies
+
+### Linux (Ubuntu/Debian)
+
+```bash
+# Cập nhật package list
+sudo apt-get update
+
+# Cài đặt CMake và build tools
+sudo apt-get install -y cmake build-essential
+
+# Cài đặt các thư viện cần thiết
+sudo apt-get install -y \
+    libboost-all-dev \
+    libeigen3-dev \
+    libyaml-cpp-dev \
+    libpcl-dev \
+    libgoogle-glog-dev
+sudo apt install liborocos-kdl-dev
+
+# Optional: Cài đặt Google Test (nếu muốn build tests)
+sudo apt-get install -y libgtest-dev
+```
+
+**Lưu ý:** Hướng dẫn này dành cho hệ thống Linux. Nếu sử dụng các distro khác (CentOS, Fedora, Arch, etc.), vui lòng cài đặt các packages tương ứng bằng package manager của distro đó.
+
+---
+
+## Build Project
+
+### Bước 1: Clone repository
+
+```bash
+git clone https://git.pnkr.asia/HiepLM/pnkx_nav_core.git
+cd pnkx_nav_core
+git submodule init
+git submodule update
+```
+
+### Bước 2: Tạo thư mục build
+
+```bash
+mkdir build
+cd build
+```
+
+### Bước 3: Cấu hình với CMake
+
+```bash
+# Cấu hình cơ bản (Release mode)
+cmake ..
+
+# Hoặc chỉ định build type rõ ràng
+cmake -DCMAKE_BUILD_TYPE=Release ..
+
+# Hoặc Debug mode (để debug)
+cmake -DCMAKE_BUILD_TYPE=Debug ..
+```
+
+### Bước 4: Build
+
+```bash
+# Build tất cả targets
+make
+
+# Hoặc build với nhiều cores (nhanh hơn)
+make -j$(nproc)
+```
+
+### Bước 5: Kiểm tra build thành công
+
+```bash
+# Kiểm tra các thư viện đã được build
+ls -la lib/
+
+# Kiểm tra các executable (nếu có)
+ls -la bin/
+```
+
+---
+
+## Các tùy chọn Build
+
+### Build Type
+
+```bash
+# Release mode (mặc định) - tối ưu hóa
+cmake -DCMAKE_BUILD_TYPE=Release ..
+
+# Debug mode - với debug symbols
+cmake -DCMAKE_BUILD_TYPE=Debug ..
+
+# RelWithDebInfo - Release với debug info
+cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo ..
+
+# MinSizeRel - Tối ưu kích thước
+cmake -DCMAKE_BUILD_TYPE=MinSizeRel ..
+```
+
+### Build chỉ một target cụ thể
+
+```bash
+# Build chỉ một library
+make robot_cpp_node_handle
+
+# Build chỉ move_base
+make move_base
+
+# Build chỉ C API
+make navigation_c_api
+```
+
+### Các tùy chọn CMake khác
+
+```bash
+# Tắt warnings (đã được cấu hình sẵn trong CMakeLists.txt)
+cmake -DCMAKE_CXX_FLAGS="-w" ..
+
+# Chỉ định compiler
+cmake -DCMAKE_CXX_COMPILER=g++-9 ..
+
+# Chỉ định install prefix
+cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..
+
+# Build tests (nếu có)
+cmake -DBUILD_TESTS=ON ..
+```
+
+---
+
+## Install
+
+Sau khi build thành công, bạn có thể install các thư viện và headers vào hệ thống:
+
+```bash
+# Install vào thư mục mặc định (/usr/local)
+sudo make install
+
+# Hoặc install vào thư mục tùy chỉnh
+cmake -DCMAKE_INSTALL_PREFIX=/opt/pnkx_nav_core ..
+make install
+```
+
+Cấu trúc sau khi install:
+```
+/usr/local/
+├── include/
+│   ├── robot/
+│   ├── nav_core2/
+│   ├── mkt_algorithm/
+│   └── ...
+├── lib/
+│   ├── librobot_cpp_node_handle.so
+│   ├── libmove_base.so
+│   └── ...
+└── lib/cmake/
+    ├── robot_cpp/
+    ├── move_base/
+    └── ...
+```
+
+---
+
+## Sử dụng trong project khác
+
+### Sử dụng với CMake
+
+Sau khi install, bạn có thể sử dụng trong CMakeLists.txt của project khác:
+
+```cmake
+cmake_minimum_required(VERSION 3.10)
+project(my_project)
+
+# Tìm các packages
+find_package(robot_cpp REQUIRED)
+find_package(move_base REQUIRED)
+find_package(navigation_c_api REQUIRED)
+
+# Link libraries
+add_executable(my_app main.cpp)
+target_link_libraries(my_app
+    PRIVATE
+        robot_cpp
+        move_base
+        navigation_c_api
+)
+```
+
+### Sử dụng C API (cho .NET/C#)
+
+Xem hướng dẫn chi tiết trong `examples/README.md` và `examples/QUICK_START.md`.
+
+Tóm tắt:
+```bash
+# Copy shared library
+cp build/src/APIs/c_api/libnavigation_c_api.so /path/to/your/project/
+
+# Sử dụng trong C#
+[DllImport("libnavigation_c_api.so")]
+public static extern IntPtr create_navigation();
+```
+
+---
+
+## Troubleshooting
+
+### Lỗi: "Could not find Boost"
+
+```bash
+# Kiểm tra Boost đã được cài đặt
+dpkg -l | grep libboost
+
+# Cài đặt lại nếu thiếu
+sudo apt-get install libboost-all-dev
+```
+
+### Lỗi: "Could not find Eigen3"
+
+```bash
+# Cài đặt Eigen3
+sudo apt-get install libeigen3-dev
+
+# Hoặc chỉ định đường dẫn
+cmake -DEigen3_DIR=/usr/include/eigen3 ..
+```
+
+### Lỗi: "Could not find yaml-cpp"
+
+```bash
+# Cài đặt yaml-cpp
+sudo apt-get install libyaml-cpp-dev
+```
+
+### Lỗi: "undefined reference"
+
+Đảm bảo tất cả dependencies đã được build theo đúng thứ tự:
+1. Core libraries (tf3, robot_time, xmlrpcpp, robot_cpp)
+2. Geometry và messages (geometry2, common_msgs)
+3. Navigation cores (nav_core, nav_core2)
+4. Algorithms (score_algorithm, mkt_algorithm)
+5. Main packages (move_base)
+
+Build lại từ đầu:
+```bash
+cd build
+rm -rf *
+cmake ..
+make -j$(nproc)
+```
+
+### Lỗi: "Permission denied" khi install
+
+```bash
+# Sử dụng sudo cho system-wide install
+sudo make install
+
+# Hoặc install vào user directory
+cmake -DCMAKE_INSTALL_PREFIX=$HOME/local ..
+make install
+```
+
+### Build chậm
+
+```bash
+# Sử dụng nhiều cores
+make -j$(nproc)
+
+# Chỉ build target cần thiết
+make robot_cpp_node_handle  # Thay vì make all
+```
+
+### Clean build
+
+```bash
+# Xóa toàn bộ build directory
+cd build
+rm -rf *
+
+# Hoặc xóa chỉ các file object
+make clean
+```
+
+---
+
+## Cấu trúc Build Output
+
+Sau khi build, cấu trúc thư mục `build/` sẽ như sau:
+
+```
+build/
+├── CMakeCache.txt
+├── CMakeFiles/
+├── cmake_install.cmake
+├── Makefile
+├── lib/
+│   ├── librobot_cpp_console.so
+│   ├── librobot_cpp_node_handle.so
+│   ├── libmove_base.so
+│   ├── libnavigation_c_api.so
+│   └── ...
+├── src/
+│   ├── Libraries/
+│   │   ├── robot_cpp/
+│   │   └── ...
+│   ├── Navigations/
+│   │   └── ...
+│   └── Algorithms/
+│       └── ...
+└── bin/  # (nếu có executables)
+```
+
+---
+
+## Environment Variables
+
+Một số biến môi trường hữu ích:
+
+```bash
+# Chỉ định config directory cho NodeHandle
+export PNKX_NAV_CORE_CONFIG_DIR=/path/to/config
+
+# Chỉ định workspace directory
+export PNKX_NAV_CORE_DIR=/path/to/pnkx_nav_core
+
+# LD_LIBRARY_PATH (nếu không install)
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/path/to/pnkx_nav_core/build/lib
+```
+
+---
+
+## Quick Reference
+
+### Build nhanh (một lệnh)
+
+```bash
+mkdir -p build && cd build && cmake .. && make -j$(nproc)
+```
+
+### Build và Install
+
+```bash
+mkdir -p build && cd build && cmake .. && make -j$(nproc) && sudo make install
+```
+
+### Rebuild từ đầu
+
+```bash
+rm -rf build && mkdir build && cd build && cmake .. && make -j$(nproc)
+```
+
+---
+
+## Liên kết hữu ích
+
+- [CMake Documentation](https://cmake.org/documentation/)
+- [C++17 Reference](https://en.cppreference.com/w/cpp/17)
+- [Examples](./examples/README.md)
+- [Catkin Build Guide](./CATKIN_BUILD_README.md)
+
+---
+
+## Ghi chú
+
+- Project này sử dụng **C++17** standard
+- Warnings đã được tắt trong build configuration (`-w` flag)
+- Tất cả libraries được build dưới dạng **shared libraries** (.so)
+- Build type mặc định là **Release** với optimization `-O3`

CMakeLists.txt

@@ -15,7 +15,9 @@ if(NOT CMAKE_BUILD_TYPE)
 endif()
 
 # Flags chung
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# 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")
 
@@ -28,10 +30,6 @@ message(STATUS "========================================")
 # Build các packages theo thứ tự phụ thuộc
 # 1. Core libraries (header-only hoặc base libraries)
 
-if (NOT TARGET tf3)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/tf3)
-endif()
-
 if (NOT TARGET robot_time)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time)
 endif()
@@ -64,33 +62,36 @@ if (NOT TARGET voxel_grid)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid) 
 endif()
 
-if (NOT TARGET nav_grid)
+if (NOT TARGET robot_nav_2d_msgs)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid) 
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_msgs) 
-endif()
-
-if (NOT TARGET nav_2d_msgs)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs) 
 endif()
 
 if (NOT TARGET costmap_2d)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/costmap_2d)
 endif()
 
-if (NOT TARGET nav_2d_utils)
+if (NOT TARGET robot_nav_2d_utils)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_utils) 
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
-if (NOT TARGET nav_core)
+if (NOT TARGET laser_filter)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
 endif()
 
-if (NOT TARGET nav_core2)
+if (NOT TARGET robot_nav_core)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
+    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_core_adapter)
+if (NOT TARGET robot_nav_core2)
-    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core2)
+endif()
+
+if (NOT TARGET robot_nav_core_adapter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core_adapter)
 endif()
 
 if (NOT TARGET move_base_core)
@@ -113,17 +114,44 @@ if (NOT TARGET score_algorithm)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
 endif()
 
-#if (NOT TARGET mkt_algorithm)
+if (NOT TARGET mkt_plugins)
-#    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins)
-#endif()
+endif()
+
+if (NOT TARGET mkt_algorithm)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
+endif()
 
 if (NOT TARGET two_points_planner)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)
 endif()
 
+if (NOT TARGET custom_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner)
+endif()
+
+if (NOT TARGET dock_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner)
+endif()
+
+if (NOT TARGET pnkx_local_planner)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner)
+endif()
+
+if (NOT TARGET robot_actionlib_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs)
+endif()
+
+if (NOT TARGET robot_move_base_msgs)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_move_base_msgs)
+endif()
+
+if (NOT TARGET robot_clear_costmap_recovery)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery)
+endif()
 
 # 2. Main packages (phụ thuộc vào cores)
-message(STATUS "[move_base] Shared library configured")
+# message(STATUS "[move_base] Shared library configured")
 if (NOT TARGET move_base)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base)
 endif()

config/config/costmap_params.yaml

@@ -0,0 +1,36 @@
+robot_costmap_2d:
+  global_frame: map
+  robot_base_frame: base_link
+  rolling_window: false
+  track_unknown_space: false
+
+  plugins:
+    - name: static_layer
+      type: StaticLayer
+
+    - name: inflation_layer
+      type: InflationLayer
+
+    - name: obstacle_layer
+      type: ObstacleLayer
+
+    - name: voxel_layer
+      type: VoxelLayer
+
+  library_path: ./libplugins.so
+  footprint:
+    - [0.3, 0.3]
+    - [0.3, -0.3]
+    - [-0.3, -0.3]
+    - [-0.3, 0.3]
+
+  transform_tolerance: 0.0
+  update_frequency: 1.0
+  width: 0.0
+  height: 0.0
+  resolution: 0.0
+  origin_x: 0.0
+  origin_y: 0.0
+
+  footprint_padding: 0.0
+  robot_radius: 0.0

config/config/inflation_layer_params.yaml

@@ -0,0 +1,5 @@
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  cost_scaling_factor: 15.0
+  inflation_radius: 0.55

config/config/obstacle_layer_params.yaml

@@ -0,0 +1,19 @@
+obstacle_layer:
+  enabled: true
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  topic: "map"
+  sensor_frame: laser_frame
+  observation_persistence: 0.0
+  expected_update_rate: 0.0
+  data_type: PointCloud
+  min_obstacle_height: 0.0
+  max_obstacle_height: 2.0
+  inf_is_valid: false
+  clearing: false
+  marking: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  footprint_clearing_enabled: true
+  combination_method: 1
+

config/config/static_layer_params.yaml

@@ -0,0 +1,11 @@
+static_layer:
+  enabled: true
+  map_topic: "map"
+  first_map_only: false
+  subscribe_to_updates: false
+  track_unknown_space: true
+  use_maximum: false
+  lethal_cost_threshold: 100
+  unknown_cost_value: -1
+  trinary_costmap: true
+  base_frame_id: "map"

config/config/voxel_layer_params.yaml

@@ -0,0 +1,11 @@
+voxel_layer:
+  enabled: true
+  footprint_clearing_enabled: true
+  max_obstacle_height: 3.0
+  origin_z: 0.0
+  z_resolution: 0.2
+  z_voxels: 16
+  unknown_threshold: 15.0
+  mark_threshold: 0
+  combination_method: 1
+

config/costmap_common_params.yaml

@@ -1,4 +1,4 @@
-robot_base_frame: base_footprint
+robot_base_frame: base_link
 transform_tolerance: 1.0
 obstacle_range: 3.0
 #mark_threshold: 1
@@ -25,7 +25,7 @@ obstacles:
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   f_scan_clearing:
@@ -33,7 +33,7 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_marking:
@@ -41,7 +41,7 @@ obstacles:
     data_type: LaserScan
     clearing: false
     marking: true
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
   b_scan_clearing:
@@ -49,6 +49,8 @@ obstacles:
     data_type: LaserScan
     clearing: true
     marking: false
-    inf_is_valid: false
+    inf_is_valid: true
     min_obstacle_height: 0.0
     max_obstacle_height: 0.25
+
+

config/costmap_global_params.yaml

@@ -0,0 +1,15 @@
+global_costmap:
+  library_path: libplugins
+  robot_base_frame: base_link
+  global_frame: map
+  update_frequency: 1.0
+  publish_frequency: 1.0
+  raytrace_range: 2.0
+  resolution: 0.05
+  z_resolution: 0.2
+  rolling_window: false
+  z_voxels: 10
+  inflation:
+    cost_scaling_factor:  10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+    inflation_radius:     0.6  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+

config/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -0,0 +1,10 @@
+global_costmap:
+    frame_id: map
+    plugins:
+        - {name: navigation_map, type: "StaticLayer" }
+        # - {name: virtual_walls_map, type: "StaticLayer" }
+        - {name: obstacles,  type: "VoxelLayer" }
+        - {name: inflation,  type: "InflationLayer" }
+    obstacles:
+        enabled: false
+        footprint_clearing_enabled: false

config/costmap_local_params.yaml

@@ -0,0 +1,18 @@
+local_costmap: 
+  library_path: libplugins
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 6.0
+  publish_frequency: 6.0
+  rolling_window: true
+  raytrace_range: 2.0
+  resolution: 0.05
+  z_resolution: 0.15
+  z_voxels: 8
+  inflation:
+    cost_scaling_factor: 10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
+    inflation_radius: 0.3  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
+  width: 8.0
+  height: 8.0
+  origin_x: 0.0
+  origin_y: 0.0

config/costmap_local_params_plugins_no_virtual_walls.yaml

@@ -1,8 +1,8 @@
 local_costmap:
   frame_id: odom
   plugins:
-        - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
+    - {name: obstacles,  type: "VoxelLayer" }
-        - {name: inflation,  type: "costmap_2d::InflationLayer" }
+    - {name: inflation,  type: "InflationLayer" }
   obstacles:
     enabled: true
     footprint_clearing_enabled: true

config/custom_global_params.yaml

@@ -1,5 +1,5 @@
-base_global_planner: CustomPlanner
 CustomPlanner:
+  library_path: libcustom_planner
   environment_type: XYThetaLattice
   planner_type: ARAPlanner
   allocated_time: 10.0
@@ -12,6 +12,5 @@ CustomPlanner:
   directory_to_save_paths: "/init/paths"
   pathway_filename: "pathway.txt"
   current_pose_topic_name: "/amcl_pose"
-  map_frame_id: "map"
+
-  base_frame_id: "base_link"
   

config/dock_global_params.yaml

@@ -0,0 +1,11 @@
+DockPlanner:
+  library_path: libdock_planner
+  MyGlobalPlanner:
+    cost_threshold: 200        # Ngưỡng chi phí vật cản (0-255)
+    safety_distance: 2         # Khoảng cách an toàn (cells)
+    use_dijkstra: false        # Sử dụng Dijkstra thay vì A*
+    
+  # File: config/costmap_params.yaml
+  global_costmap:
+    inflation_radius: 0.3      # Bán kính phình vật cản
+    cost_scaling_factor: 10.0  # Hệ số tỷ lệ chi phí

config/move_base_common_params.yaml

@@ -0,0 +1,50 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
+PNKXLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: CustomPlanner
+
+PNKXDockingLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXGoStraightLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+PNKXRotateLocalPlanner:
+  base_local_planner: LocalPlannerAdapter
+  base_global_planner: TwoPointsPlanner
+
+### replanning
+controller_frequency: 30.0   # run controller at 15.0 Hz
+controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
+planner_frequency: 0.0       # don't continually replan (only when controller failed)
+planner_patience: 2.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
+max_planning_retries: 0     # ... or after 10 attempts (whichever happens first)
+oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
+oscillation_distance: 0.5
+### recovery behaviors
+recovery_behavior_enabled: true
+recovery_behaviors: [       
+                      {name: aggressive_reset, type: ClearCostmapRecovery},
+                      {name: conservative_reset, type: ClearCostmapRecovery},                      
+                    ]
+
+conservative_reset:
+  reset_distance: 3.0        # clear obstacles farther away than 3.0 m
+  invert_area_to_clear: true
+
+aggressive_reset:
+  reset_distance: 3.0
+
+ClearCostmapRecovery:
+  library_path: librobot_clear_costmap_recovery
+
+MoveBase:
+  library_path: libmove_base

config/mprim/costmap_global_params.yaml

@@ -1,13 +0,0 @@
-global_costmap:
-    global_frame: map
-    update_frequency: 1.0
-    publish_frequency: 1.0
-    raytrace_range: 2.0
-    resolution: 0.05
-    z_resolution: 0.2
-    rolling_window: false
-    z_voxels: 10
-    inflation:
-      cost_scaling_factor:  10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
-      inflation_radius:     0.6  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
-

config/mprim/costmap_global_params_plugins_no_virtual_walls.yaml

@@ -1,10 +0,0 @@
-global_costmap:
-    frame_id: map
-    plugins:
-        - {name: navigation_map, type: "costmap_2d::StaticLayer" }
-        - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
-        - {name: obstacles,  type: "costmap_2d::VoxelLayer" }
-        - {name: inflation,  type: "costmap_2d::InflationLayer" }
-    obstacles:
-        enabled: false
-        footprint_clearing_enabled: false

config/mprim/costmap_local_params.yaml

@@ -1,16 +0,0 @@
-local_costmap:  
-    global_frame: odom
-    update_frequency: 6.0
-    publish_frequency: 6.0
-    rolling_window: true
-    raytrace_range: 2.0
-    resolution: 0.05
-    z_resolution: 0.15
-    z_voxels: 8
-    inflation:
-      cost_scaling_factor: 10.0  # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
-      inflation_radius: 0.3  # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
-    width: 8.0
-    height: 8.0
-    origin_x: 0.0
-    origin_y: 0.0

config/mprim/ekf.yaml

@@ -62,12 +62,12 @@ publish_acceleration: false
 #         estimation node from robot_localization! However, that instance should *not* fuse the global data.
 map_frame: map                                   # Defaults to "map" if unspecified
 odom_frame: $(arg tf_prefix)odom                 # Defaults to "odom" if unspecified
-base_link_frame: $(arg tf_prefix)base_footprint  # Defaults to "base_link" if unspecified
+base_link_frame: $(arg tf_prefix)base_link  # Defaults to "base_link" if unspecified
 world_frame: $(arg tf_prefix)odom                # Defaults to the value of odom_frame if unspecified
 
-# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# The filter accepts an arbitrary number of inputs from each input message type (robot_nav_msgs/Odometry,
 # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
-# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# robot_sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
 # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
 # default values, and must be specified.
 odom0: odom

config/mprim/localization.yaml

@@ -25,7 +25,7 @@ Amcl:
   update_min_d: 0.05
   update_min_a: 0.05
   odom_frame_id: odom
-  base_frame_id: base_footprint
+  base_frame_id: base_link
   global_frame_id: map
   resample_interval: 1
   transform_tolerance: 0.2

config/mprim/move_base_common_params.yaml

@@ -1,24 +0,0 @@
-
-### replanning
-controller_frequency: 20.0   # run controller at 15.0 Hz
-controller_patience: 0.0    # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
-planner_frequency: 0.0       # don't continually replan (only when controller failed)
-planner_patience: 5.0        # if the first planning attempt failed, abort planning retries after 5.0 s...
-max_planning_retries: 5     # ... or after 10 attempts (whichever happens first)
-oscillation_timeout: -1    # abort controller and trigger recovery behaviors after 30.0 s
-oscillation_distance: 0.4
-### recovery behaviors
-recovery_behavior_enabled: true
-recovery_behaviors: [       
-                      {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
-                      {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},                      
-                    ]
-
-conservative_reset:
-  reset_distance: 3.0        # clear obstacles farther away than 3.0 m
-  invert_area_to_clear: true
-
-aggressive_reset:
-  reset_distance: 3.0
-
-

config/mprim/pnkx_local_planner_params.yaml

@@ -1,186 +0,0 @@
-
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-
-base_local_planner: nav_core_adapter::LocalPlannerAdapter
-yaw_goal_tolerance: 0.017  
-xy_goal_tolerance: 0.03 
-min_approach_linear_velocity: 0.06   # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
-stateful: true
-publish_topic: true  
-
-LocalPlannerAdapter:
-  PNKXLocalPlanner:
-    # Algorithm
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
-    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
-    # Goal checking
-    goal_checker_name: mkt_plugins::GoalChecker 
-
-  PNKXDockingLocalPlanner: 
-    # Algorithm
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
-    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
-    # Goal checking
-    goal_checker_name: mkt_plugins::GoalChecker 
-
-  PNKXGoStraightLocalPlanner:
-    # Algorithm
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    algorithm_nav_name: mkt_algorithm::diff::GoStraight
-    # Goal checking
-    goal_checker_name: mkt_plugins::GoalChecker 
-
-  PNKXRotateLocalPlanner:
-    # Algorithm
-    algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
-    trajectory_generator_name: mkt_plugins::LimitedAccelGenerator 
-    # Goal checking
-    goal_checker_name: mkt_plugins::SimpleGoalChecker
-    stateful: true
-    
-LimitedAccelGenerator:
-  max_vel_x: 1.2
-  min_vel_x: -1.2
-
-  max_vel_y: 0.0  # diff drive robot
-  min_vel_y: 0.0  # diff drive robot
-
-  max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.1    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
-
-  max_vel_theta: 0.9    # max_rot_vel: 1.0  # choose slightly less than the base's capability
-  min_vel_theta: 0.04  # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
-
-  acc_lim_x: 1.0
-  acc_lim_y: 0.0      # diff drive robot
-  acc_lim_theta: 0.9
-  decel_lim_x: -1.0
-  decel_lim_y: -0.0
-  decel_lim_theta: -1.5
-
-  # Whether to split the path into segments or not
-  split_path: true
-  sim_time: 1.5
-  vx_samples: 15
-  vy_samples: 1       
-  vtheta_samples: 10
-  discretize_by_time: true
-  angular_granularity: 0.05
-  linear_granularity: 0.1
-  # sim_period
-  include_last_point: true
-
-PredictiveTrajectory:
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
-  index_samples: 60
-  follow_step_path: true
-
-  # Lookahead
-  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
-  # only when false:
-  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
-  # only when true:
-  min_lookahead_dist: 1.0                     # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 1.6                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
-
-  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
-  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
-  # only when true:
-  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
-  angular_decel_zone: 0.1             
-
-  # stoped
-  rot_stopped_velocity: 0.1
-  trans_stopped_velocity: 0.1
-  
-  # Regulated linear velocity scaling
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
-  # only when true:
-  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
-  regulated_linear_scaling_min_speed: 0.15      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
-
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
-  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
-  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
-  cost_scaling_dist: 0.2                             # (default: 0.6)
-  cost_scaling_gain: 2.0                             # (default: 1.0)
-
-GoStraight:
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
-  index_samples: 60
-  follow_step_path: true
-
-  # Lookahead
-  use_velocity_scaled_lookahead_dist: true   # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
-  # only when false:
-  lookahead_dist: 0.5                        # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
-  # only when true:
-  min_lookahead_dist: 0.8                    # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 2.5                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.6                    # Minimum squared journey to consider for goal (default: 0.2) 
-
-  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
-  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
-  # only when true:
-  rotate_to_heading_min_angle: 0.35   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
-
-  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
-  
-  use_regulated_linear_velocity_scaling: false
-  use_cost_regulated_linear_velocity_scaling: false
-
-RotateToGoal:
-  avoid_obstacles: false
-  xy_local_goal_tolerance: 0.01
-  angle_threshold: 0.6
-  index_samples: 60
-  follow_step_path: true
-
-  # Lookahead
-  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
-  # only when false:
-  lookahead_dist: 0.325                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
-  # only when true:
-  min_lookahead_dist: 0.8                     # The minimum lookahead distance (m) threshold. (default: 0.3)
-  max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
-  lookahead_time: 1.8                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
-  min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
-  max_journey_squared: 0.6                    # Maximum squared journey to consider for goal (default: 0.2) 
-
-  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
-  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
-  # only when true:
-  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
-  angular_decel_zone: 0.1 
-  
-  # stoped
-  rot_stopped_velocity: 0.03
-  trans_stopped_velocity: 0.06
-  
-  # Regulated linear velocity scaling
-  use_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
-  # only when true:
-  regulated_linear_scaling_min_radius: 0.6      # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
-  regulated_linear_scaling_min_speed: 0.05      # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
-
-  # Inflation cost scaling (Limit velocity by proximity to obstacles)
-  use_cost_regulated_linear_velocity_scaling: false   # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
-  inflation_cost_scaling_factor: 2.0                 # (default: 3.0)   # must be > 0
-  cost_scaling_dist: 0.2                             # (default: 0.6)
-  cost_scaling_gain: 2.0                             # (default: 1.0)

config/mprim/ros_diff_drive_controller.yaml

@@ -25,7 +25,7 @@ wheel_radius_multiplier    : 1.0 # default: 1.0
 cmd_vel_timeout: 1.0
 
 # frame_ids (same as real MiR platform)
-base_frame_id: base_footprint # default: base_link base_footprint
+base_frame_id: base_link # default: base_link base_link
 odom_frame_id: odom           # default: odom 
 
 # Velocity and acceleration limits

config/pnkx_local_planner_params.yaml

@@ -0,0 +1,199 @@
+yaw_goal_tolerance: 0.03  
+xy_goal_tolerance: 0.02 
+min_approach_linear_velocity: 0.05 
+
+LocalPlannerAdapter:
+  library_path: liblocal_planner_adapter
+PNKXLocalPlanner:
+  # Algorithm
+  library_path: libpnkx_local_planner
+  trajectory_generator_name: LimitedAccelGenerator 
+  algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
+  algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
+  # Goal checking
+  goal_checker_name: GoalChecker 
+
+PNKXDockingLocalPlanner: 
+  # Algorithm
+  library_path: libpnkx_local_planner
+  trajectory_generator_name: LimitedAccelGenerator 
+  algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
+  algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
+  # Goal checking
+  goal_checker_name: GoalChecker 
+
+PNKXGoStraightLocalPlanner:
+  # Algorithm
+  library_path: libpnkx_local_planner
+  trajectory_generator_name: LimitedAccelGenerator 
+  algorithm_nav_name: MKTAlgorithmDiffGoStraight
+  # Goal checking
+  goal_checker_name: GoalChecker 
+
+PNKXRotateLocalPlanner:
+  # Algorithm
+  library_path: libpnkx_local_planner
+  algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
+  trajectory_generator_name: LimitedAccelGenerator 
+  # Goal checking
+  goal_checker_name: SimpleGoalChecker
+  stateful: true
+    
+LimitedAccelGenerator:
+  library_path: libmkt_plugins_standard_traj_generator
+  max_vel_x: 1.2
+  min_vel_x: -1.2
+
+  max_vel_y: 0.0  # diff drive robot
+  min_vel_y: 0.0  # diff drive robot
+
+  max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
+  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+
+  max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
+  min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity
+
+  acc_lim_x: 1.5
+  acc_lim_y: 0.0      # diff drive robot
+  acc_lim_theta: 1.5
+  decel_lim_x: -1.5
+  decel_lim_y: -0.0
+  decel_lim_theta: -1.5
+
+  # Whether to split the path into segments or not
+  split_path: true
+  sim_time: 1.5
+  vx_samples: 15
+  vy_samples: 1       
+  vtheta_samples: 10
+  discretize_by_time: true
+  angular_granularity: 0.05
+  linear_granularity: 0.1
+  # sim_period
+  include_last_point: true
+
+MKTAlgorithmDiffPredictiveTrajectory:
+  library_path: libmkt_algorithm_diff
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.47
+  index_samples: 60
+  follow_step_path: true
+
+  # Kalman filter tuning (filters v and w commands)
+  kf_q_v: 0.25
+  kf_q_w: 0.8
+  kf_r_v: 0.05
+  kf_r_w: 0.08
+  kf_p0: 0.5
+  kf_filter_angular: false
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.5                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  angular_decel_zone: 0.1             
+
+  # stoped
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
+  
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 1.2
+  final_heading_ki_angular: 0.002
+  final_heading_kd_angular: 0.12
+
+MKTAlgorithmDiffGoStraight:
+  library_path: libmkt_algorithm_diff
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.8
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 2.0                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 1.0                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 1.0                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  angular_decel_zone: 0.1             
+
+  # stoped
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
+  
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 1.5
+  final_heading_ki_angular: 0.2
+  final_heading_kd_angular: 0.05
+
+MKTAlgorithmDiffRotateToGoal:
+  library_path: libmkt_algorithm_diff
+  xy_local_goal_tolerance: 0.02
+  angle_threshold: 0.47
+  index_samples: 60
+  follow_step_path: true
+
+  # Lookahead
+  use_velocity_scaled_lookahead_dist: true    # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
+  # only when false:
+  lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
+  # only when true:
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
+  lookahead_time: 1.9                        # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
+  min_journey_squared: 0.2                    # Minimum squared journey to consider for goal (default: 0.2) 
+  max_journey_squared: 0.8                    # Maximum squared journey to consider for goal (default: 0.2) 
+  max_lateral_accel: 0.9                      # Max lateral accel for speed reduction on curves (m/s^2)
+
+  # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
+  use_rotate_to_heading: true         # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
+  # only when true:
+  rotate_to_heading_min_angle: 0.03   # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
+  angular_decel_zone: 0.1             
+
+  # stoped
+  rot_stopped_velocity: 0.05
+  trans_stopped_velocity: 0.06
+  
+  use_final_heading_alignment: true
+  final_heading_xy_tolerance: 0.1
+  final_heading_angle_tolerance: 0.05
+  final_heading_min_velocity: 0.05
+  final_heading_kp_angular: 2.0
+  near_goal_heading_kp: 1.5
+  near_goal_heading_ki: 0.0
+  near_goal_heading_kd: 0.0
+  
+GoalChecker:
+  library_path: libmkt_plugins_goal_checker
+
+SimpleGoalChecker:
+  library_path: libmkt_plugins_goal_checker

config/two_points_global_params.yaml

@@ -1,3 +1,3 @@
-base_global_planner: TwoPointsPlanner
 TwoPointsPlanner:
+  library_path: libtwo_points_planner
   lethal_obstacle: 20

config1/two_points_global_params.yaml

@@ -1,3 +0,0 @@
-base_global_planner: TwoPointsPlanner
-TwoPointsPlanner:
-  lethal_obstacle: 20

doc/architecture_discussion.md

@@ -67,8 +67,8 @@ flowchart TB
   %% ========== COSTMAP LAYER ==========
   subgraph Costmap["🗺️ Costmap Layer"]
     direction LR
-    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
+    GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
-    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
+    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
     style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
     style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
     style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
@@ -89,12 +89,12 @@ flowchart TB
   %% ========== DATA SOURCES ==========
   subgraph DataSources["📡 Data Sources"]
     direction TB
-    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
+    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
     Loc["<b>🌍 Localization</b><br/>━━━━━━━━━━━━━━━━<br/>📍 Pnkx Loc<br/>🗺️ Global Pose<br/>🔄 Pose Updates"]
     TF["<b>🔄 Transform System</b><br/>━━━━━━━━━━━━━━━━<br/>📐 tf3::BufferCore<br/>🌐 Coordinate Frames<br/>⏱️ Time Synchronization"]
-    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
+    Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
-    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
+    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 robot_sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
-    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
+    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 robot_nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
     style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
     style Goal fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
     style Loc fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
@@ -107,7 +107,7 @@ flowchart TB
   %% ========== CONTROL LOOP ==========
   subgraph ControlLoop["🔄 Control Loop"]
     direction LR
-    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
+    CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 robot_geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
     BaseCtrl["<b>🎮 Base Controller</b><br/>━━━━━━━━━━━━━━━━<br/>🚗 diff_driver_controller<br/>🚲 steer_drive_controller<br/>━━━━━━━━━━━━━━━━<br/>⚙️ Kinematics<br/>🔧 Hardware Interface"]
     style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000
     style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
@@ -205,20 +205,64 @@ Cần làm rõ:
    - Thực thi control loop (executeCycle)
 
 4. **Planning Layer** ✅
-   - `nav_core::BaseGlobalPlanner`: Interface cho global planners (A*, D*, etc.)
+   - `robot_nav_core::BaseGlobalPlanner`: Interface cho global planners
-   - `nav_core::BaseLocalPlanner`: Interface cho local planners (DWA, TEB, MKT, etc.)
+   - **Global Planner Implementations**:
-   - `nav_core::RecoveryBehavior`: Interface cho recovery behaviors
+     - `custom_planner` - Custom path planner với curve support và merge path calculation
-   - Plugin system sử dụng `boost::dll` để dynamic loading
+     - `dock_planner` - Docking-specific planner với dock calculation utilities
+     - `two_points_planner` - Simple planner cho two-point navigation
+   - `robot_nav_core::BaseLocalPlanner`: Interface cho local planners
+   - **Local Planner Implementation**:
+     - `pnkx_local_planner` - Main local planner với multiple behaviors:
+       - Position planning (MKT algorithm)
+       - Docking planning
+       - Go straight planning
+       - Rotate planning
+   - `robot_nav_core::RecoveryBehavior`: Interface cho recovery behaviors
+   - Plugin system sử dụng `boost::dll` và `robot::PluginLoaderHelper` để dynamic loading
 
 5. **Costmap Layer** ✅
-   - `costmap_2d::Costmap2DROBOT`: Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap wrapper
-   - Costmap layers: static map, obstacles, inflation
+   - `robot_costmap_2d::LayeredCostmap`: Layered costmap system
+   - **Costmap Layers** (8 implementations):
+     - `StaticLayer` - Static map từ OccupancyGrid
+     - `ObstacleLayer` - Dynamic obstacles từ sensors
+     - `InflationLayer` - Cost inflation cho robot footprint
+     - `VoxelLayer` - 3D voxel grid cho obstacles
+     - `CriticalLayer` - Critical zones marking
+     - `DirectionalLayer` - Directional cost preferences
+     - `PreferredLayer` - Preferred path zones
+     - `UnpreferredLayer` - Unpreferred zones
    - Frame management: map (global), odom (local)
+   - Thread-safe access với mutex
+   - Observation buffer cho sensor data
 
 6. **Algorithms Layer** ✅
-   - `mkt_algorithm`: Diff drive và bicycle kinematics algorithms
+   - **MKT Algorithm** (`mkt_algorithm`):
-   - `score_algorithm`: Trajectory scoring và goal checking
+     - Differential drive kinematics:
-   - `kalman`: Filtering algorithms
+       - `diff_go_straight` - Go straight behavior
+       - `diff_predictive_trajectory` - Predictive trajectory following
+       - `diff_rotate_to_goal` - Rotate to goal
+     - Bicycle model kinematics:
+       - `bicycle_go_straight` - Go straight với bicycle model
+       - `bicycle_pure_pursuit` - Pure pursuit control
+       - `bicycle_rotate_to_goal` - Rotate behavior
+   - **MKT Plugins** (`mkt_plugins`):
+     - `goal_checker` - Goal checking algorithms
+     - `simple_goal_checker` - Simple goal checking
+     - `standard_traj_generator` - Standard trajectory generation
+     - `limited_accel_generator` - Acceleration-limited trajectory generation
+     - `kinematic_parameters` - Kinematics parameter management
+     - `xy_theta_iterator` - Trajectory iteration utilities
+     - `velocity_iterator` - Velocity space iteration
+     - `one_d_velocity_iterator` - 1D velocity iteration
+   - **Score Algorithm** (`score_algorithm`):
+     - Trajectory scoring và evaluation
+     - Goal checking utilities
+     - Trajectory generator interface
+   - **Kalman Filter** (`kalman`):
+     - State estimation
+     - Sensor fusion
+     - Noise filtering
 
 7. **Data Sources** ⚠️ (Interface cần định nghĩa)
    - Localization source (Pnkx Loc)
@@ -232,10 +276,10 @@ Cần làm rõ:
    - Velocity command execution
 
 **Định dạng dữ liệu:**
-- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
+- `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
-- `geometry_msgs::Twist` (vận tốc linear/angular)
+- `robot_geometry_msgs::Twist` (vận tốc linear/angular)
-- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
+- `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
-- `costmap_2d::Costmap2D` (bản đồ chi phí)
+- `robot_costmap_2d::Costmap2D` (bản đồ chi phí)
 
 ### 3. Thiết kế từng module (interface level)
 
@@ -250,21 +294,25 @@ Cần làm rõ:
   - `pause()`, `resume()`, `cancel()` - Điều khiển trạng thái
   - `getRobotPose(...)` - Lấy vị trí robot
 
-- `nav_core::BaseGlobalPlanner` ✅
+- `robot_nav_core::BaseGlobalPlanner` ✅
   - `makePlan(start, goal, plan)` - Tạo global path
   - `initialize(name, costmap_robot)` - Khởi tạo với costmap
+  - **Implementations**: `custom_planner`, `dock_planner`, `two_points_planner`
 
-- `nav_core::BaseLocalPlanner` ✅
+- `robot_nav_core::BaseLocalPlanner` ✅
-  - `computeVelocityCommands(cmd_vel)` - Tính toán velocity command
+  - `computeVelocityCommands(odom, cmd_vel)` - Tính toán velocity command
   - `setPlan(plan)` - Set global path để follow
   - `isGoalReached()` - Kiểm tra đã đến goal chưa
-  - `swapPlanner(name)` - Thay đổi planner động
+  - `swapPlanner(name)` - Thay đổi planner động (position, docking, go_straight, rotate)
   - `setTwistLinear/Angular(...)` - Set velocity limits
+  - `getTwistLinear/Angular(...)` - Get velocity limits
+  - **Implementation**: `pnkx_local_planner` với multiple behaviors
 
-- `nav_core::RecoveryBehavior` ✅
+- `robot_nav_core::RecoveryBehavior` ✅
   - `runBehavior()` - Thực thi recovery behavior
+  - `initialize(name, tf, global_costmap, local_costmap)` - Khởi tạo
 
-- `costmap_2d::Costmap2DROBOT` ✅
+- `robot_costmap_2d::Costmap2DROBOT` ✅
   - Wrapper cho costmap với robot footprint
   - Thread-safe access với mutex
 
@@ -290,9 +338,15 @@ Cần làm rõ:
 
 **Plugin mechanism:** ✅
 - Sử dụng `boost::dll` để dynamic loading plugins
-- Factory pattern với `boost::function` và `boost::dll::import`
+- Factory pattern với `boost::function` và `boost::dll::import_alias`
+- `robot::PluginLoaderHelper` để tìm library path từ plugin name
 - Config file YAML để specify plugin names
-- Plugin interfaces: `BaseGlobalPlanner`, `BaseLocalPlanner`, `RecoveryBehavior`
+- Plugin interfaces: 
+  - `robot_nav_core::BaseGlobalPlanner` - Global planners
+  - `robot_nav_core::BaseLocalPlanner` - Local planners  
+  - `robot_nav_core::RecoveryBehavior` - Recovery behaviors
+  - `robot_costmap_2d::Layer` - Costmap layers
+- Plugin registration với `BOOST_DLL_ALIAS` macro
 
 ### 4. Cơ chế giao tiếp & đồng bộ
 
@@ -340,36 +394,99 @@ Cần làm rõ:
 
 **Đã hoàn thành:** ✅
 
-1. ✅ **Interface Layer**: `BaseNavigation` interface
+1. ✅ **Interface Layer**: 
-2. ✅ **Implementation Layer**: `MoveBase` core logic
+   - `move_base_core::BaseNavigation` - Abstract interface hoàn chỉnh
-3. ✅ **Planning Layer**: Plugin system cho global/local planners và recovery
+   - `move_base_core::common.h` - Common types và utilities
-4. ✅ **Costmap Layer**: Global và local costmap với layers
+
-5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman
+2. ✅ **Implementation Layer**: 
-6. ✅ **API Layer**: C API wrapper cho .NET integration
+   - `move_base::MoveBase` - Core implementation với state machine
-7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, nav_2d_utils
+   - `executeCycle()` - Control loop implementation
+   - State management (PLANNING, CONTROLLING, CLEARING, PAUSED)
+   - Threading với planner thread và control loop
+
+3. ✅ **Planning Layer - Plugin System**: 
+   - **Global Planners** (3 implementations):
+     - `custom_planner` - Custom path planning với curve support
+     - `dock_planner` - Docking-specific planner
+     - `two_points_planner` - Simple two-point planner
+   - **Local Planners** (1 implementation):
+     - `pnkx_local_planner` - Main local planner với:
+       - `pnkx_docking_local_planner` - Docking behavior
+       - `pnkx_go_straight_local_planner` - Go straight behavior
+       - `pnkx_rotate_local_planner` - Rotate behavior
+   - **Recovery Behaviors**: Interface đã định nghĩa
+   - Plugin loading với `boost::dll` và `robot::PluginLoaderHelper`
+
+4. ✅ **Costmap Layer**: 
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - **Costmap Layers** (8 implementations):
+     - `static_layer` - Static map layer
+     - `obstacle_layer` - Dynamic obstacles
+     - `inflation_layer` - Cost inflation
+     - `voxel_layer` - 3D voxel grid
+     - `critical_layer` - Critical zones
+     - `directional_layer` - Directional costs
+     - `preferred_layer` - Preferred paths
+     - `unpreferred_layer` - Unpreferred zones
+   - Layered costmap system với plugin support
+
+5. ✅ **Algorithms Layer**: 
+   - **MKT Algorithm**:
+     - Differential drive kinematics (go_straight, rotate, predictive)
+     - Bicycle model kinematics (go_straight, rotate, pure pursuit)
+   - **MKT Plugins**:
+     - `goal_checker` - Goal checking algorithms
+     - `standard_traj_generator` - Standard trajectory generation
+     - `kinematic_parameters` - Kinematics parameter management
+     - `xy_theta_iterator` - Trajectory iteration
+     - `limited_accel_generator` - Acceleration-limited trajectories
+   - **Score Algorithm**: Trajectory scoring và goal checking
+   - **Kalman Filter**: State estimation và filtering
+
+6. ✅ **API Layer**: 
+   - `nav_c_api` - C API wrapper cho .NET/C# P/Invoke
+   - Wrapper functions cho BaseNavigation methods
+
+7. ✅ **Supporting Libraries**: 
+   - `tf3` - Transform system (buffer_core, transforms)
+   - `robot_time` - Time management (Time, Duration, Timer, Rate)
+   - `robot_cpp` - Core utilities:
+     - `robot::init()` - Initialization
+     - `robot::NodeHandle` - YAML config loading
+     - `robot::PluginLoaderHelper` - Plugin discovery
+   - `robot_nav_2d_utils` - 2D navigation utilities (conversions, path_ops, polygons, bounds, tf_help)
+   - `robot_nav_2d_msgs` - 2D navigation messages
+   - `geometry_msgs` - Geometry message types (29 types)
+   - `robot_nav_msgs` - Navigation messages (24 types)
+   - `robot_sensor_msgs` - Sensor messages (30 types)
+   - `laser_geometry` - Laser scan geometry utilities
+   - `voxel_grid` - Voxel grid implementation
+   - `data_convert` - Data conversion utilities
+   - `nav_grid` - Navigation grid utilities
 
 **Đang triển khai / Cần bổ sung:** ⚠️
 
 1. ⚠️ **Data Sources Interfaces**: 
-   - `ILocalizationSource` interface
+   - `ILocalizationSource` interface - Cần định nghĩa
-   - `IOdometrySource` interface
+   - `IOdometrySource` interface - Cần định nghĩa
-   - `IMapProvider` interface
+   - `IMapProvider` interface - Cần định nghĩa
-   - Integration với Pnkx Loc, odometry sources
+   - Integration với Pnkx Loc, odometry sources - Hiện tại dùng trực tiếp qua MoveBase methods
 
 2. ⚠️ **Base Controller**:
-   - `IBaseController` interface
+   - `IBaseController` interface - Cần định nghĩa
-   - Diff drive controller implementation
+   - Diff drive controller implementation - Cần implement
-   - Steer drive controller implementation
+   - Steer drive controller implementation - Cần implement
-   - Velocity command execution
+   - Velocity command execution - Hiện tại chỉ có `setTwistLinear/Angular` trong local planner
 
-3. ⚠️ **Control Loop**:
+3. ⚠️ **User Controller Plugin System**:
-   - Control loop trong MoveBase (executeCycle)
+   - Factory để load user controller plugins - Cần implement
-   - State machine management hoàn chỉnh
+   - Interface cho User Controller - Cần định nghĩa
-   - Threading và synchronization
+   - Integration với BaseNavigation - Cần implement
 
-4. ⚠️ **User Controller Plugin System**:
+4. ⚠️ **Testing & Documentation**:
-   - Factory để load user controller plugins
+   - Unit tests cho các modules - Một số đã có (costmap, bounds, etc.)
-   - Integration với BaseNavigation
+   - Integration tests - Cần bổ sung
+   - Simulation environment - TODO
 
 **Lộ trình tiếp theo:**
 
@@ -406,38 +523,144 @@ pnkx_nav_core/
 ├── src/
 │   ├── Navigations/
 │   │   ├── Cores/
-│   │   │   ├── move_base_core/     # BaseNavigation interface
+│   │   │   ├── move_base_core/              # BaseNavigation interface ✅
-│   │   │   ├── nav_core/            # Planner interfaces
+│   │   │   │   ├── include/move_base_core/
-│   │   │   ├── nav_core_adapter/    # Adapter utilities
+│   │   │   │   │   ├── navigation.h         # BaseNavigation abstract class
-│   │   │   └── nav_core2/           # Additional nav utilities
+│   │   │   │   │   └── common.h             # Common types (TFListenerPtr, etc.)
+│   │   │   ├── robot_nav_core/              # Planner interfaces ✅
+│   │   │   │   ├── include/robot_nav_core/
+│   │   │   │   │   ├── base_global_planner.h
+│   │   │   │   │   ├── base_local_planner.h
+│   │   │   │   │   └── recovery_behavior.h
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities ✅
+│   │   │   │   ├── include/robot_nav_core_adapter/
+│   │   │   │   │   ├── global_planner_adapter.h
+│   │   │   │   │   ├── local_planner_adapter.h
+│   │   │   │   │   └── costmap_adapter.h
+│   │   │   └── robot_nav_core2/             # Additional nav utilities ✅
+│   │   │       ├── include/robot_nav_core2/
+│   │   │       │   ├── global_planner.h
+│   │   │       │   ├── local_planner.h
+│   │   │       │   └── costmap.h
 │   │   ├── Libraries/
-│   │   │   ├── costmap_2d/          # Costmap system
+│   │   │   ├── nav_grid/                    # Navigation grid utilities ✅
-│   │   │   ├── tf3/                 # Transform system
+│   │   │   └── (costmap, tf3, robot_time ở Libraries/)
-│   │   │   ├── robot_time/          # Time management
-│   │   │   ├── geometry2/           # Geometry utilities
-│   │   │   └── ...                  # Other supporting libraries
 │   │   └── Packages/
-│   │       └── move_base/           # MoveBase implementation
+│   │       └── move_base/                   # MoveBase implementation ✅
+│   │           ├── include/move_base/
+│   │           │   └── move_base.h
+│   │           └── src/
+│   │               ├── move_base.cpp
+│   │               └── move_base_main.cpp
+│   │
 │   ├── Algorithms/
 │   │   ├── Cores/
-│   │   │   └── score_algorithm/     # Trajectory scoring
+│   │   │   └── score_algorithm/             # Trajectory scoring ✅
-│   │   └── Libraries/
+│   │   │       ├── include/score_algorithm/
-│   │       ├── mkt_algorithm/      # MKT kinematics algorithms
+│   │   │       │   ├── score_algorithm.h
-│   │       ├── kalman/              # Kalman filtering
+│   │   │       │   ├── goal_checker.h
-│   │       └── angles/               # Angle utilities
+│   │   │       │   └── trajectory_generator.h
+│   │   ├── Libraries/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics algorithms ✅
+│   │   │   │   ├── include/mkt_algorithm/
+│   │   │   │   │   ├── diff/                # Differential drive
+│   │   │   │   │   │   ├── diff_go_straight.h
+│   │   │   │   │   │   ├── diff_predictive_trajectory.h
+│   │   │   │   │   │   └── diff_rotate_to_goal.h
+│   │   │   │   │   └── bicycle/             # Bicycle model
+│   │   │   │   │       ├── bicycle.h
+│   │   │   │   │       ├── go_straight.h
+│   │   │   │   │       └── rotate_to_goal.h
+│   │   │   ├── mkt_plugins/                 # MKT plugin components ✅
+│   │   │   │   ├── include/mkt_plugins/
+│   │   │   │   │   ├── goal_checker.h
+│   │   │   │   │   ├── standard_traj_generator.h
+│   │   │   │   │   ├── kinematic_parameters.h
+│   │   │   │   │   └── xy_theta_iterator.h
+│   │   │   ├── mkt_msgs/                    # MKT message types ✅
+│   │   │   ├── kalman/                      # Kalman filtering ✅
+│   │   │   └── angles/                      # Angle utilities ✅
+│   │   └── Packages/
+│   │       ├── global_planners/
+│   │       │   ├── custom_planner/           # Custom global planner ✅
+│   │       │   ├── dock_planner/            # Docking planner ✅
+│   │       │   └── two_points_planner/      # Two points planner ✅
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner ✅
+│   │               ├── include/pnkx_local_planner/
+│   │               │   ├── pnkx_local_planner.h
+│   │               │   ├── pnkx_docking_local_planner.h
+│   │               │   ├── pnkx_go_straight_local_planner.h
+│   │               │   └── pnkx_rotate_local_planner.h
+│   │
+│   ├── Libraries/
+│   │   ├── costmap_2d/                      # Costmap system ✅
+│   │   │   ├── include/robot_costmap_2d/
+│   │   │   │   ├── costmap_2d_robot.h
+│   │   │   │   ├── layered_costmap.h
+│   │   │   │   └── layer.h
+│   │   │   ├── plugins/                     # Costmap layers ✅
+│   │   │   │   ├── static_layer.cpp
+│   │   │   │   ├── obstacle_layer.cpp
+│   │   │   │   ├── inflation_layer.cpp
+│   │   │   │   ├── voxel_layer.cpp
+│   │   │   │   ├── critical_layer.cpp
+│   │   │   │   ├── directional_layer.cpp
+│   │   │   │   ├── preferred_layer.cpp
+│   │   │   │   └── unpreferred_layer.cpp
+│   │   │   └── config/                      # Costmap configs
+│   │   ├── tf3/                             # Transform system ✅
+│   │   ├── robot_time/                      # Time management ✅
+│   │   ├── robot_cpp/                       # Core utilities ✅
+│   │   │   ├── include/robot/
+│   │   │   │   ├── init.h                   # Initialization
+│   │   │   │   ├── node_handle.h            # NodeHandle (YAML config)
+│   │   │   │   └── plugin_loader_helper.h   # Plugin loader
+│   │   ├── robot_nav_2d_utils/              # 2D nav utilities ✅
+│   │   ├── robot_nav_2d_msgs/               # 2D nav messages ✅
+│   │   ├── geometry2/                       # Geometry utilities ✅
+│   │   │   ├── robot_tf3_geometry_msgs/
+│   │   │   └── robot_tf3_sensor_msgs/
+│   │   ├── common_msgs/                     # Message types ✅
+│   │   │   ├── robot_geometry_msgs/
+│   │   │   ├── robot_nav_msgs/
+│   │   │   ├── robot_sensor_msgs/
+│   │   │   ├── robot_std_msgs/
+│   │   │   ├── robot_protocol_msgs/
+│   │   │   └── robot_visualization_msgs/
+│   │   ├── laser_geometry/                  # Laser geometry ✅
+│   │   ├── voxel_grid/                      # Voxel grid ✅
+│   │   ├── data_convert/                    # Data conversion ✅
+│   │   └── xmlrpcpp/                        # XML-RPC utilities ✅
+│   │
 │   └── APIs/
-│       └── c_api/                   # C API wrapper
+│       └── c_api/                           # C API wrapper ✅
+│           ├── include/nav_c_api.h
+│           └── src/nav_c_api.cpp
+│
 ├── build/                                   # Build artifacts
+├── config/                                  # Configuration files
+├── examples/                                # Example code
 └── doc/                                     # Documentation
+    ├── architecture_discussion.md
+    ├── implementation_plan.md
+    └── folders.md
 ```
 
 ## Ghi chú
 
 - ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
+- ✅ **3 Global Planners** đã triển khai: custom_planner, dock_planner, two_points_planner
+- ✅ **1 Local Planner** đã triển khai: pnkx_local_planner với 4 behaviors (position, docking, go_straight, rotate)
+- ✅ **8 Costmap Layers** đã triển khai: static, obstacle, inflation, voxel, critical, directional, preferred, unpreferred
+- ✅ **MKT Algorithms** hỗ trợ cả differential drive và bicycle model
+- ✅ **Plugin System** hoàn chỉnh với `boost::dll` và `robot::PluginLoaderHelper`
 - ⚠️ Cần bổ sung data sources interfaces và base controller
 - 🔄 Kiến trúc được thiết kế để dễ dàng thay đổi thuật toán và mô hình kinematics thông qua plugin system
-- 📦 Tất cả components được build bằng CMake, không phụ thuộc ROS
+- 📦 Tất cả components được build bằng CMake, hỗ trợ cả Catkin và Standalone CMake
 - 🔌 Plugin system sử dụng `boost::dll` cho dynamic loading
+- 🎯 MoveBase đã có control loop (`executeCycle`) và state machine hoàn chỉnh
+- 📚 Có đầy đủ message types: geometry_msgs (29), nav_msgs (24), sensor_msgs (30), std_msgs (32)
 
 
 

doc/folders.md

@@ -1,53 +1,99 @@
-Mô tả cấu trúc:
+# Cấu trúc thư mục pnkx_nav_core
 
-├── common_msgs/
+Mô tả cấu trúc thư mục thực tế của dự án:
-│   ├── build/
+
-│   ├── geometry_msgs/
+```
-│   │   ├── include/
+pnkx_nav_core/
-│   │   └── test/
+├── src/
-│   ├── CMakeLists.txt
+│   ├── Navigations/
-│   ├── nav_msgs/
+│   │   ├── Cores/
-│   │   ├── include/
+│   │   │   ├── move_base_core/              # BaseNavigation interface
-│   │   └── test/
+│   │   │   ├── robot_nav_core/              # Planner interfaces (BaseGlobalPlanner, BaseLocalPlanner, RecoveryBehavior)
-│   ├── CMakeLists.txt
+│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities (global_planner_adapter, local_planner_adapter, costmap_adapter)
-│   ├── sensor_msgs/
+│   │   │   └── robot_nav_core2/             # Additional nav utilities (global_planner, local_planner, costmap)
-│   │   ├── cfg/
+│   │   ├── Libraries/
-│   │   ├── include/
+│   │   │   └── nav_grid/                    # Navigation grid utilities
-│   │   └── test/
+│   │   └── Packages/
-│   ├── CMakeLists.txt
+│   │       └── move_base/                   # MoveBase implementation
-│   ├── std_msgs/
+│   │
-│   │   ├── include/
+│   ├── Algorithms/
-│   │   └── CMakeLists.txt
+│   │   ├── Cores/
-│   └── CMakeLists.txt (root)
+│   │   │   └── score_algorithm/             # Trajectory scoring và goal checking
-|
+│   │   ├── Libraries/
-├── Navigations/
+│   │   │   ├── mkt_algorithm/               # MKT kinematics (diff drive & bicycle)
-│   ├── Cores/
+│   │   │   ├── mkt_plugins/                 # MKT plugin components
-│   │   └── move_base_core/
+│   │   │   ├── mkt_msgs/                    # MKT message types
-│   │       ├── build/
+│   │   │   ├── kalman/                      # Kalman filtering
-│   │       ├── example/
+│   │   │   └── angles/                      # Angle utilities
-│   │       ├── include/
+│   │   └── Packages/
-│   │       ├── .gitignore
+│   │       ├── global_planners/
-│   │       ├── CMakeLists.txt
+│   │       │   ├── custom_planner/           # Custom global planner
-│   │       └── README.md
+│   │       │   ├── dock_planner/            # Docking planner
+│   │       │   └── two_points_planner/      # Two points planner
+│   │       └── local_planners/
+│   │           └── pnkx_local_planner/      # PNKX local planner
+│   │
 │   ├── Libraries/
-│   │   ├── geometry_msgs/
+│   │   ├── costmap_2d/                      # Costmap system với 8 layers
-│   │   │   ├── build/
+│   │   ├── tf3/                             # Transform system
-│   │   │   ├── include/
+│   │   ├── robot_time/                      # Time management
-│   │   │   └── CMakeLists.txt
+│   │   ├── robot_cpp/                       # Core utilities (init, NodeHandle, PluginLoaderHelper)
-│   │   └── libtf2/
+│   │   ├── robot_nav_2d_utils/             # 2D navigation utilities
-│   │       ├── .vscode/
+│   │   ├── robot_nav_2d_msgs/              # 2D navigation messages
-│   │       ├── include/
+│   │   ├── geometry2/                       # Geometry utilities (tf3_geometry_msgs, tf3_sensor_msgs)
-│   │       ├── samples/
+│   │   ├── common_msgs/                     # Message types
-│   │       ├── src/
+│   │   │   ├── robot_geometry_msgs/         # Geometry messages (29 types)
-│   │       ├── .gitignore
+│   │   │   ├── robot_nav_msgs/             # Navigation messages (24 types)
-│   │       ├── DEBIAN_PACKAGING.md
+│   │   │   ├── robot_sensor_msgs/          # Sensor messages (30 types)
-│   │       ├── libtf2_2.0.0_amd64.deb
+│   │   │   ├── robot_std_msgs/             # Standard messages (32 types)
-│   │       ├── Makefile
+│   │   │   ├── robot_protocol_msgs/        # Protocol messages
-│   │       └── README.md
+│   │   │   ├── robot_visualization_msgs/   # Visualization messages
-│   ├── Packages/
+│   │   │   └── utils/                       # Message utilities
-│   │   └── move_base/
+│   │   ├── laser_geometry/                  # Laser geometry utilities
-│   │       ├── build/
+│   │   ├── voxel_grid/                      # Voxel grid
-│   │       ├── include/
+│   │   ├── data_convert/                    # Data conversion
-│   │       ├── src/
+│   │   └── xmlrpcpp/                        # XML-RPC utilities
-│   │       └── CMakeLists.txt
+│   │
-│   └── CMakeLists.txt (root)
+│   └── APIs/
+│       └── c_api/                           # C API wrapper cho .NET/C#
+│
+├── build/                                   # Build artifacts
+├── config/                                  # Configuration files
+├── examples/                                # Example code
+└── doc/                                     # Documentation
+    ├── architecture_discussion.md
+    ├── implementation_plan.md
+    ├── folders.md
+    └── readme.md
+```
+
+## Chi tiết các thư mục chính
+
+### Navigations/
+- **Cores/**: Interfaces và core utilities
+  - `move_base_core`: BaseNavigation interface
+  - `robot_nav_core`: Planner interfaces
+  - `robot_nav_core_adapter`: Adapter layer
+  - `robot_nav_core2`: Additional utilities
+- **Libraries/**: Navigation libraries
+  - `nav_grid`: Grid utilities
+- **Packages/**: Implementations
+  - `move_base`: MoveBase implementation
+
+### Algorithms/
+- **Cores/**: Core algorithms
+  - `score_algorithm`: Trajectory scoring
+- **Libraries/**: Algorithm libraries
+  - `mkt_algorithm`: Kinematics algorithms
+  - `mkt_plugins`: Plugin components
+  - `kalman`: Filtering
+- **Packages/**: Algorithm implementations
+  - `global_planners/`: 3 planners
+  - `local_planners/`: 1 planner với multiple behaviors
+
+### Libraries/
+- **costmap_2d/**: Costmap system với 8 layers
+- **tf3/**: Transform system
+- **robot_cpp/**: Core utilities (init, NodeHandle, PluginLoaderHelper)
+- **common_msgs/**: Message type definitions
+- Các utilities khác: time, geometry, sensors, etc.

doc/implementation_plan.md

@@ -15,14 +15,14 @@
    - Plugin system với `boost::dll` để load dynamic plugins
 
 3. **Costmap System:**
-   - `costmap_2d::Costmap2DROBOT` - Global và local costmap
+   - `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
    - Costmap layers (static, obstacle, inflation, etc.)
 
 4. **Supporting Libraries:**
    - `tf3` - Transform system
    - `robot_time` - Time management
    - `geometry_msgs` - Message types
-   - `nav_2d_msgs`, `nav_2d_utils` - 2D navigation utilities
+   - `robot_nav_2d_msgs`, `robot_nav_2d_utils` - 2D navigation utilities
 
 ### ❌ Cần bổ sung/hoàn thiện:
 
@@ -56,8 +56,8 @@
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
 ```cpp
 class IOdometrySource {
-  virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
+  virtual robot_geometry_msgs::PoseStamped getCurrentPose() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
   virtual bool isAvailable() = 0;
 };
 ```
@@ -66,7 +66,7 @@ class IOdometrySource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
 ```cpp
 class ILocalizationSource {
-  virtual geometry_msgs::PoseStamped getRobotPose() = 0;
+  virtual robot_geometry_msgs::PoseStamped getRobotPose() = 0;
   virtual std::string getGlobalFrame() = 0;
   virtual bool isLocalized() = 0;
 };
@@ -76,7 +76,7 @@ class ILocalizationSource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h`
 ```cpp
 class IMapProvider {
-  virtual nav_msgs::OccupancyGrid getMap() = 0;
+  virtual robot_nav_msgs::OccupancyGrid getMap() = 0;
   virtual bool isMapAvailable() = 0;
 };
 ```
@@ -85,7 +85,7 @@ class IMapProvider {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h`
 ```cpp
 class ISensorSource {
-  virtual sensor_msgs::LaserScan getLatestScan() = 0;
+  virtual robot_sensor_msgs::LaserScan getLatestScan() = 0;
   virtual bool hasNewData() = 0;
 };
 ```
@@ -96,9 +96,9 @@ class ISensorSource {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h`
 ```cpp
 class IBaseController {
-  virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
+  virtual bool setVelocity(const robot_geometry_msgs::Twist& cmd_vel) = 0;
   virtual bool stop() = 0;
-  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
+  virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
 };
 ```
 
@@ -120,7 +120,7 @@ class IBaseController {
 **File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h`
 ```cpp
 class IUserController {
-  virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
+  virtual void onGoalReceived(const robot_geometry_msgs::PoseStamped& goal) = 0;
   virtual void onNavigationStateChanged(move_base_core::State state) = 0;
   virtual void onFeedback(const NavFeedback& feedback) = 0;
 };

examples/CSharpExample.cs

@@ -1,322 +0,0 @@
-using System;
-using System.Runtime.InteropServices;
-using System.Runtime.CompilerServices;
-
-namespace NavigationExample
-{
-    /// <summary>
-    /// C# P/Invoke wrapper for Navigation C API
-    /// </summary>
-    public class NavigationAPI
-    {
-        private const string DllName = "libnav_c_api.so"; // Linux
-        // For Windows: "nav_c_api.dll"
-        // For macOS: "libnav_c_api.dylib"
-
-        // ============================================================================
-        // Enums
-        // ============================================================================
-        public enum NavigationState
-        {
-            Pending = 0,
-            Active = 1,
-            Preempted = 2,
-            Succeeded = 3,
-            Aborted = 4,
-            Rejected = 5,
-            Preempting = 6,
-            Recalling = 7,
-            Recalled = 8,
-            Lost = 9,
-            Planning = 10,
-            Controlling = 11,
-            Clearing = 12,
-            Paused = 13
-        }
-
-        // ============================================================================
-        // Structures
-        // ============================================================================
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Point
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Quaternion
-        {
-            public double x;
-            public double y;
-            public double z;
-            public double w;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Position
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose
-        {
-            public Position position;
-            public Quaternion orientation;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Header
-        {
-            public uint seq;
-            public long sec;
-            public uint nsec;
-            public IntPtr frame_id; // char*
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PoseStamped
-        {
-            public Header header;
-            public Pose pose;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Vector3
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavFeedback
-        {
-            public NavigationState navigation_state;
-            public IntPtr feed_back_str; // char*
-            public Pose2D current_pose;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool goal_checked;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_ready;
-        }
-
-        // ============================================================================
-        // String Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-        // ============================================================================
-        // State Conversion
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern IntPtr navigation_state_to_string(NavigationState state);
-
-        // ============================================================================
-        // Helper Functions
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_2d(
-            double pose_x, double pose_y, double pose_theta,
-            string frame_id, double offset_distance,
-            ref PoseStamped out_goal);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_stamped(
-            ref PoseStamped in_pose, double offset_distance,
-            ref PoseStamped out_goal);
-
-        // ============================================================================
-        // Navigation Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr navigation_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_destroy(IntPtr handle);
-
-        // ============================================================================
-        // TF Listener Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr tf_listener_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void tf_listener_destroy(IntPtr handle);
-
-        // ============================================================================
-        // Navigation Interface Methods
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_robot_footprint(
-            IntPtr handle, Point[] points, UIntPtr point_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to(
-            IntPtr handle, ref PoseStamped goal,
-            double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_dock_to(
-            IntPtr handle, string marker, ref PoseStamped goal,
-            double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(
-            IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_rotate_to(
-            IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_pause(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_resume(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_cancel(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_linear(
-            IntPtr handle, double linear_x, double linear_y, double linear_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_angular(
-            IntPtr handle, double angular_x, double angular_y, double angular_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_stamped(
-            IntPtr handle, ref PoseStamped out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_2d(
-            IntPtr handle, ref Pose2D out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_feedback(
-            IntPtr handle, ref NavFeedback out_feedback);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_feedback(ref NavFeedback feedback);
-
-        // ============================================================================
-        // Helper Methods for String Conversion
-        // ============================================================================
-        public static string MarshalString(IntPtr ptr)
-        {
-            if (ptr == IntPtr.Zero)
-                return string.Empty;
-            return Marshal.PtrToStringAnsi(ptr);
-        }
-    }
-
-    // ============================================================================
-    // Example Usage
-    // ============================================================================
-    class Program
-    {
-        // Helper method để hiển thị file và line number tự động
-        static void LogError(string message,
-            [CallerFilePath] string filePath = "",
-            [CallerLineNumber] int lineNumber = 0,
-            [CallerMemberName] string memberName = "")
-        {
-            // Lấy tên file từ đường dẫn đầy đủ
-            string fileName = System.IO.Path.GetFileName(filePath);
-            Console.WriteLine($"[{fileName}:{lineNumber}] {memberName}: {message}");
-        }
-
-        static void Main(string[] args)
-        {
-            // Create TF listener
-            IntPtr tfHandle = NavigationAPI.tf_listener_create();
-            if (tfHandle == IntPtr.Zero)
-            {
-                LogError("Failed to create TF listener");
-                return;
-            }
-
-            // Create navigation instance
-            IntPtr navHandle = NavigationAPI.navigation_create();
-            if (navHandle == IntPtr.Zero)
-            {
-                LogError("Failed to create navigation instance");
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            // Initialize navigation
-            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
-            {
-                LogError("Failed to initialize navigation");
-                NavigationAPI.navigation_destroy(navHandle);
-                NavigationAPI.tf_listener_destroy(tfHandle);
-                return;
-            }
-
-            // Set robot footprint
-            NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
-            {
-                new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
-                new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
-            };
-            NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
-
-            // Get robot pose
-            NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
-            if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
-            {
-                Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
-            }
-
-            // Get navigation feedback
-            NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
-            if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
-            {
-                string stateStr = NavigationAPI.MarshalString(
-                    NavigationAPI.navigation_state_to_string(feedback.navigation_state));
-                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
-                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
-                NavigationAPI.navigation_free_feedback(ref feedback);
-            }
-
-            // Cleanup
-            NavigationAPI.navigation_destroy(navHandle);
-            NavigationAPI.tf_listener_destroy(tfHandle);
-        }
-    }
-}
-

examples/NavigationExample/NavigationAPI.cs

@@ -0,0 +1,299 @@
+using System;
+using System.Runtime.InteropServices;
+using NavigationExample;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# P/Invoke wrapper for Navigation C API
+    /// </summary>
+    public class NavigationAPI
+    {
+        private const string DllName = "/usr/local/lib/libnav_c_api.so"; // Linux
+        // For Windows: "nav_c_api.dll"
+        // For macOS: "libnav_c_api.dylib"
+
+        // ============================================================================
+        // Enums
+        // ============================================================================
+        public enum NavigationState
+        {
+            Pending = 0,
+            Active = 1,
+            Preempted = 2,
+            Succeeded = 3,
+            Aborted = 4,
+            Rejected = 5,
+            Preempting = 6,
+            Recalling = 7,
+            Recalled = 8,
+            Lost = 9,
+            Planning = 10,
+            Controlling = 11,
+            Clearing = 12,
+            Paused = 13
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct NavFeedback
+        {
+            public NavigationState navigation_state;
+            public IntPtr feed_back_str;  // char*; free with nav_c_api_free_string
+            public Pose2D current_pose;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool goal_checked;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool is_ready;
+        }
+
+
+        /// <summary>Planner data output (plan, costmap, footprint).</summary>
+        [StructLayout(LayoutKind.Sequential)]
+        public struct PlannerDataOutput
+        {
+            public Path2D plan;
+            public OccupancyGrid costmap;
+            public OccupancyGridUpdate costmap_update;
+            [MarshalAs(UnmanagedType.I1)]
+            public bool is_costmap_updated;
+            public PolygonStamped footprint;
+        }
+
+        [StructLayout(LayoutKind.Sequential)]
+        public struct NavigationHandle
+        {
+            public IntPtr ptr;
+        }
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern Header header_create(string frame_id);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern Header header_set_data(
+            uint seq,
+            uint sec,
+            uint nsec,
+            string frame_id);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern Time time_create();
+
+        /// <summary>Free a string allocated by the API (strdup).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void nav_c_api_free_string(IntPtr str);
+
+        /// <summary>Convert NavigationState to string; caller must free with nav_c_api_free_string.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        public static extern IntPtr navigation_state_to_string(NavigationState state);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_feedback(NavigationHandle handle, ref NavFeedback out_feedback);
+
+
+        /// <summary>Helper: copy unmanaged char* to managed string; does not free the pointer.</summary>
+        public static string MarshalString(IntPtr p)
+        {
+            if (p == IntPtr.Zero) return string.Empty;
+            return Marshal.PtrToStringAnsi(p) ?? string.Empty;
+        }
+
+        /// <summary>Free strings inside NavFeedback (feed_back_str). Call after navigation_get_feedback when done.</summary>
+        public static void navigation_free_feedback(ref NavFeedback feedback)
+        {
+            if (feedback.feed_back_str != IntPtr.Zero)
+            {
+                nav_c_api_free_string(feedback.feed_back_str);
+                feedback.feed_back_str = IntPtr.Zero;
+            }
+        }
+
+        // ============================================================================
+        // Navigation Handle Management
+        // ============================================================================
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern NavigationHandle navigation_create();
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_destroy(NavigationHandle handle);
+
+        /// <summary>Initialize navigation using an existing tf3 buffer (from libtf3). Caller owns the buffer and must call tf3_buffer_destroy after navigation_destroy.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_initialize(NavigationHandle handle, IntPtr tf3_buffer);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_robot_footprint(NavigationHandle handle, Point[] points, UIntPtr point_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(NavigationHandle handle, ref Point[] out_points, ref UIntPtr out_count);
+
+        /// <summary>Send a goal for the robot to navigate to (global frame).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to(NavigationHandle handle, PoseStamped goal);
+
+        /// <summary>Navigate using an Order message (graph nodes/edges). Order must be built or obtained from native side; call order_free when done if it was allocated by native.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_order(NavigationHandle handle, Order order, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_to_nodes_edges(NavigationHandle handle, IntPtr nodes, UIntPtr node_count, IntPtr edges, UIntPtr edge_count, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to(NavigationHandle handle, string marker, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_order(NavigationHandle handle, Order order, string marker, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_dock_to_nodes_edges(NavigationHandle handle, string marker, Node[] nodes, Edge[] edges, PoseStamped goal);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_move_straight_to(NavigationHandle handle, double distance);
+
+        /// <summary>Rotate in place to align with target orientation (radians).</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_rotate_to(NavigationHandle handle, double goal_yaw);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_pause(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_resume(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_cancel(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_twist_linear(NavigationHandle handle, double linear_x, double linear_y, double linear_z);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_set_twist_angular(NavigationHandle handle, double angular_x, double angular_y, double angular_z);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_pose_stamped(NavigationHandle handle, ref PoseStamped out_pose);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_pose_2d(NavigationHandle handle, ref Pose2D out_pose);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(NavigationHandle handle, ref Twist2DStamped out_twist);
+
+        // ============================================================================
+        // Navigation Data Management
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_laser_scan(NavigationHandle handle, string laser_scan_name, LaserScan laser_scan);
+    
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_odometry(NavigationHandle handle, string odometry_name, Odometry odometry);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_static_map(NavigationHandle handle, string map_name, OccupancyGrid occupancy_grid);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_static_map(NavigationHandle handle, string map_name, ref OccupancyGrid occupancy_grid);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_point_cloud(NavigationHandle handle, string point_cloud_name, PointCloud point_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_add_point_cloud2(NavigationHandle handle, string point_cloud2_name, PointCloud2 point_cloud2);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_laser_scan(NavigationHandle handle, string laser_scan_name, ref LaserScan out_scan);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_point_cloud(NavigationHandle handle, string point_cloud_name, ref PointCloud out_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_point_cloud2(NavigationHandle handle, string point_cloud2_name, ref PointCloud2 out_cloud);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_static_map(NavigationHandle handle, string map_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_laser_scan(NavigationHandle handle, string laser_scan_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_point_cloud(NavigationHandle handle, string point_cloud_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_point_cloud2(NavigationHandle handle, string point_cloud2_name);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_static_maps(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_laser_scans(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_point_clouds(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_point_cloud2s(NavigationHandle handle);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_remove_all_data(NavigationHandle handle);
+
+        /// <summary>Get all static maps. out_maps must be pre-allocated; use navigation_get_all_static_maps_count or similar to get count first if needed.</summary>
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_static_maps(NavigationHandle handle, [Out] NamedOccupancyGrid[] out_maps, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_laser_scans(NavigationHandle handle, [Out] NamedLaserScan[] out_scans, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_point_clouds(NavigationHandle handle, [Out] NamedPointCloud[] out_clouds, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_all_point_cloud2s(NavigationHandle handle, [Out] NamedPointCloud2[] out_clouds, ref UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_global_data(NavigationHandle handle, ref PlannerDataOutput out_data);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_local_data(NavigationHandle handle, ref PlannerDataOutput out_data);
+    }
+}

examples/NavigationExample/NavigationExample.csproj

@@ -1,10 +1,12 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <OutputType>Exe</OutputType>
-    <TargetFramework>net6.0</TargetFramework>
+    <TargetFramework>net10.0</TargetFramework>
-    <RuntimeIdentifier>linux-x64</RuntimeIdentifier>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
+  <ItemGroup>
+    <PackageReference Include="SixLabors.ImageSharp" Version="3.1.7" />
+  </ItemGroup>
   <ItemGroup>
     <None Include="libnav_c_api.so">
       <CopyToOutputDirectory>Always</CopyToOutputDirectory>

examples/NavigationExample/Program.cs

@@ -1,244 +1,25 @@
 using System;
+using System.Globalization;
+using System.IO;
 using System.Runtime.InteropServices;
 using System.Runtime.CompilerServices;
+using System.Text.RegularExpressions;
+using SixLabors.ImageSharp;
+using SixLabors.ImageSharp.PixelFormats;
+using NavigationExample;
+
 
 namespace NavigationExample
 {
-    /// <summary>
+    /// <summary>Thông tin map đọc từ file YAML (maze.yaml).</summary>
-    /// C# P/Invoke wrapper for Navigation C API
+    public struct MazeMapConfig
-    /// </summary>
-    public class NavigationAPI
     {
-        private const string DllName = "libnav_c_api.so"; // Linux
+        public string ImagePath;       // đường dẫn đầy đủ tới file ảnh
-        // For Windows: "nav_c_api.dll"
+        public float Resolution;
-        // For macOS: "libnav_c_api.dylib"
+        public double OriginX, OriginY, OriginZ;
-
+        public int Negate;            // 0 hoặc 1
-        // ============================================================================
+        public double OccupiedThresh;
-        // Enums
+        public double FreeThresh;
-        // ============================================================================
-        public enum NavigationState
-        {
-            Pending = 0,
-            Active = 1,
-            Preempted = 2,
-            Succeeded = 3,
-            Aborted = 4,
-            Rejected = 5,
-            Preempting = 6,
-            Recalling = 7,
-            Recalled = 8,
-            Lost = 9,
-            Planning = 10,
-            Controlling = 11,
-            Clearing = 12,
-            Paused = 13
-        }
-
-        // ============================================================================
-        // Structures
-        // ============================================================================
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Point
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose2D
-        {
-            public double x;
-            public double y;
-            public double theta;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Quaternion
-        {
-            public double x;
-            public double y;
-            public double z;
-            public double w;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Position
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Pose
-        {
-            public Position position;
-            public Quaternion orientation;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Header
-        {
-            public uint seq;
-            public long sec;
-            public uint nsec;
-            public IntPtr frame_id; // char*
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct PoseStamped
-        {
-            public Header header;
-            public Pose pose;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct Vector3
-        {
-            public double x;
-            public double y;
-            public double z;
-        }
-
-        [StructLayout(LayoutKind.Sequential)]
-        public struct NavFeedback
-        {
-            public NavigationState navigation_state;
-            public IntPtr feed_back_str; // char*
-            public Pose2D current_pose;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool goal_checked;
-            [MarshalAs(UnmanagedType.I1)]
-            public bool is_ready;
-        }
-
-        // ============================================================================
-        // String Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void nav_c_api_free_string(IntPtr str);
-
-        // ============================================================================
-        // State Conversion
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        public static extern IntPtr navigation_state_to_string(NavigationState state);
-
-        // ============================================================================
-        // Helper Functions
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_2d(
-            double pose_x, double pose_y, double pose_theta,
-            string frame_id, double offset_distance,
-            ref PoseStamped out_goal);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_offset_goal_stamped(
-            ref PoseStamped in_pose, double offset_distance,
-            ref PoseStamped out_goal);
-
-        // ============================================================================
-        // Navigation Handle Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr navigation_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_destroy(IntPtr handle);
-
-        // ============================================================================
-        // TF Listener Management
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern IntPtr tf_listener_create();
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void tf_listener_destroy(IntPtr handle);
-
-        // ============================================================================
-        // Navigation Interface Methods
-        // ============================================================================
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_initialize(IntPtr handle, IntPtr tf_handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_robot_footprint(
-            IntPtr handle, Point[] points, UIntPtr point_count);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_to(
-            IntPtr handle, ref PoseStamped goal,
-            double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_dock_to(
-            IntPtr handle, string marker, ref PoseStamped goal,
-            double xy_goal_tolerance, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_move_straight_to(
-            IntPtr handle, ref PoseStamped goal, double xy_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_rotate_to(
-            IntPtr handle, ref PoseStamped goal, double yaw_goal_tolerance);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_pause(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_resume(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_cancel(IntPtr handle);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_linear(
-            IntPtr handle, double linear_x, double linear_y, double linear_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_set_twist_angular(
-            IntPtr handle, double angular_x, double angular_y, double angular_z);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_stamped(
-            IntPtr handle, ref PoseStamped out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_robot_pose_2d(
-            IntPtr handle, ref Pose2D out_pose);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        [return: MarshalAs(UnmanagedType.I1)]
-        public static extern bool navigation_get_feedback(
-            IntPtr handle, ref NavFeedback out_feedback);
-
-        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
-        public static extern void navigation_free_feedback(ref NavFeedback feedback);
-
-        // ============================================================================
-        // Helper Methods for String Conversion
-        // ============================================================================
-        public static string MarshalString(IntPtr ptr)
-        {
-            if (ptr == IntPtr.Zero)
-                return string.Empty;
-            return Marshal.PtrToStringAnsi(ptr);
-        }
     }
 
     // ============================================================================
@@ -246,6 +27,105 @@ namespace NavigationExample
     // ============================================================================
     class Program
     {
+        /// <summary>
+        /// Đọc file maze.yaml (image, resolution, origin, negate, occupied_thresh, free_thresh).
+        /// ImagePath được resolve tuyệt đối theo thư mục chứa file yaml.
+        /// </summary>
+        static bool TryLoadMazeYaml(string yamlPath, out MazeMapConfig config)
+        {
+            config = default;
+            if (string.IsNullOrEmpty(yamlPath) || !File.Exists(yamlPath))
+                return false;
+            string dir = Path.GetDirectoryName(Path.GetFullPath(yamlPath)) ?? "";
+            try
+            {
+                string[] lines = File.ReadAllLines(yamlPath);
+                foreach (string line in lines)
+                {
+                    string trimmed = line.Trim();
+                    if (trimmed.Length == 0 || trimmed.StartsWith("#")) continue;
+                    int colon = trimmed.IndexOf(':');
+                    if (colon <= 0) continue;
+                    string key = trimmed.Substring(0, colon).Trim();
+                    string value = trimmed.Substring(colon + 1).Trim();
+                    if (key == "image")
+                        config.ImagePath = Path.Combine(dir, value);
+                    else if (key == "resolution" && float.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out float res))
+                        config.Resolution = res;
+                    else if (key == "origin")
+                    {
+                        var m = Regex.Match(value, @"\[\s*([-\d.]+\s*),\s*([-\d.]+\s*),\s*([-\d.]+)\s*\]");
+                        if (m.Success)
+                        {
+                            config.OriginX = double.Parse(m.Groups[1].Value, CultureInfo.InvariantCulture);
+                            config.OriginY = double.Parse(m.Groups[2].Value, CultureInfo.InvariantCulture);
+                            config.OriginZ = double.Parse(m.Groups[3].Value, CultureInfo.InvariantCulture);
+                        }
+                    }
+                    else if (key == "negate" && int.TryParse(value, out int neg))
+                        config.Negate = neg;
+                    else if (key == "occupied_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ot))
+                        config.OccupiedThresh = ot;
+                    else if (key == "free_thresh" && double.TryParse(value, NumberStyles.Float, CultureInfo.InvariantCulture, out double ft))
+                        config.FreeThresh = ft;
+                }
+                return !string.IsNullOrEmpty(config.ImagePath);
+            }
+            catch
+            {
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Đọc file ảnh PNG và chuyển pixel sang byte[] occupancy (0=free, 100=occupied, 255=unknown).
+        /// Dùng negate, occupied_thresh, free_thresh từ maze.yaml nếu cung cấp.
+        /// </summary>
+        static byte[] LoadMazePixelsAsOccupancy(string imagePath, out int width, out int height,
+            int negate = 0, double occupiedThresh = 0.65, double freeThresh = 0.196)
+        {
+            width = 0;
+            height = 0;
+            if (string.IsNullOrEmpty(imagePath) || !File.Exists(imagePath))
+            {
+                LogError($"File not found: {imagePath}");
+                return null;
+            }
+            try
+            {
+                using var image = Image.Load<Rgba32>(imagePath);
+                int w = image.Width;
+                int h = image.Height;
+                width = w;
+                height = h;
+                byte[] data = new byte[w * h];
+                image.ProcessPixelRows(accessor =>
+                {
+                    for (int y = 0; y < accessor.Height; y++)
+                    {
+                        Span<Rgba32> row = accessor.GetRowSpan(y);
+                        for (int x = 0; x < row.Length; x++)
+                        {
+                            ref readonly Rgba32 p = ref row[x];
+                            double gray = (p.R + p.G + p.B) / 255.0 / 3.0;
+                            if (negate != 0) gray = 1.0 - gray;
+                            byte occ;
+                            if (gray <= freeThresh) occ = 0;
+                            else if (gray >= occupiedThresh) occ = 100;
+                            else occ = 255; // unknown
+                            data[y * w + x] = occ;
+                        }
+                    }
+                });
+                return data;
+            }
+            catch (Exception ex)
+            {
+                LogError($"Load image failed: {ex.Message}");
+                return null;
+            }
+        }
+
         // Helper method để hiển thị file và line number tự động
         static void LogError(string message,
             [CallerFilePath] string filePath = "",
@@ -259,64 +139,359 @@ namespace NavigationExample
 
         static void Main(string[] args)
         {
-            // Create TF listener
+            // Create tf3 buffer (replaces TF listener; used for all static transforms and navigation init)
-            IntPtr tfHandle = NavigationAPI.tf_listener_create();
+            IntPtr tf3Buffer = TF3API.tf3_buffer_create(10);
-            if (tfHandle == IntPtr.Zero)
+            if (tf3Buffer == IntPtr.Zero)
             {
-                LogError("Failed to create TF listener");
+                LogError("Failed to create tf3 buffer (libtf3.so may be missing)");
+                return;
+            }
+            Console.WriteLine($"[NavigationExample] TF3 buffer created, handle = 0x{tf3Buffer.ToInt64():X16}");
+            string version = Marshal.PtrToStringAnsi(TF3API.tf3_get_version()) ?? "?";
+            Console.WriteLine($"[TF3] {version}");
+
+            // Inject static transforms: map -> odom -> base_footprint -> base_link
+            var tMapOdom = TF3API.CreateStaticTransform("map", "odom", 0, 0, 0, 0, 0, 0, 1);
+            var tOdomFoot = TF3API.CreateStaticTransform("odom", "base_footprint", 0, 0, 0, 0, 0, 0, 1);
+            var tFootLink = TF3API.CreateStaticTransform("base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1);
+            if (!TF3API.tf3_set_transform(tf3Buffer, ref tMapOdom, "NavigationExample", true) ||
+                !TF3API.tf3_set_transform(tf3Buffer, ref tOdomFoot, "NavigationExample", true) ||
+                !TF3API.tf3_set_transform(tf3Buffer, ref tFootLink, "NavigationExample", true))
+            {
+                LogError("Failed to set static TF");
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Create navigation instance
+            // Create navigation instance and initialize with tf3 buffer
-            IntPtr navHandle = NavigationAPI.navigation_create();
+            NavigationAPI.NavigationHandle navHandle = NavigationAPI.navigation_create();
-            if (navHandle == IntPtr.Zero)
+            if (navHandle.ptr == IntPtr.Zero)
             {
                 LogError("Failed to create navigation instance");
-                NavigationAPI.tf_listener_destroy(tfHandle);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Initialize navigation
+            if (!NavigationAPI.navigation_initialize(navHandle, tf3Buffer))
-            if (!NavigationAPI.navigation_initialize(navHandle, tfHandle))
             {
-                LogError("Failed to initialize navigation");
+                LogError("Failed to initialize navigation with tf3 buffer");
                 NavigationAPI.navigation_destroy(navHandle);
-                NavigationAPI.tf_listener_destroy(tfHandle);
+                TF3API.tf3_buffer_destroy(tf3Buffer);
                 return;
             }
 
-            // Set robot footprint
+            while (true)
-            NavigationAPI.Point[] footprint = new NavigationAPI.Point[]
             {
-                new NavigationAPI.Point { x = 0.3, y = -0.2, z = 0.0 },
-                new NavigationAPI.Point { x = 0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = 0.2, z = 0.0 },
-                new NavigationAPI.Point { x = -0.3, y = -0.2, z = 0.0 }
-            };
-            NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
-
-            // Get robot pose
-            NavigationAPI.Pose2D robotPose = new NavigationAPI.Pose2D();
-            if (NavigationAPI.navigation_get_robot_pose_2d(navHandle, ref robotPose))
-            {
-                Console.WriteLine($"Robot pose: x={robotPose.x}, y={robotPose.y}, theta={robotPose.theta}");
-            }
-
-            // Get navigation feedback
                 NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
                 if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
                 {
-                string stateStr = NavigationAPI.MarshalString(
+                    if (feedback.is_ready)
-                    NavigationAPI.navigation_state_to_string(feedback.navigation_state));
+                    {
-                string feedbackStr = NavigationAPI.MarshalString(feedback.feed_back_str);
+                        Console.WriteLine("Navigation is ready");
-                Console.WriteLine($"State: {stateStr}, Feedback: {feedbackStr}");
+                        break;
-                NavigationAPI.navigation_free_feedback(ref feedback);
+                    }
+                    else
+                    {
+                        Console.WriteLine("Navigation is not ready");
+                    }
+                }
+                System.Threading.Thread.Sleep(100);
+            }
+            Console.WriteLine("[NavigationExample] Navigation initialized successfully");
+
+
+            // Set robot footprint
+            Point[] footprint = new Point[]
+            {
+                new Point { x = 0.3, y = -0.2, z = 0.0 },
+                new Point { x = 0.3, y = 0.2, z = 0.0 },
+                new Point { x = -0.3, y = 0.2, z = 0.0 },
+                new Point { x = -0.3, y = -0.2, z = 0.0 }
+            };
+            NavigationAPI.navigation_set_robot_footprint(navHandle, footprint, new UIntPtr((uint)footprint.Length));
+
+  
+            IntPtr fFrameId = Marshal.StringToHGlobalAnsi("fscan");
+            Header fscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(fFrameId));
+            LaserScan fscanHandle;
+            fscanHandle.header = fscanHeader;
+            fscanHandle.angle_min = -1.57f;
+            fscanHandle.angle_max = 1.57f;
+            fscanHandle.angle_increment = 0.785f;
+            fscanHandle.time_increment = 0.0f;
+            fscanHandle.scan_time = 0.1f;
+            fscanHandle.range_min = 0.05f;
+            fscanHandle.range_max = 10.0f;
+            fscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, fscanHandle.ranges, 5);
+            fscanHandle.ranges_count = new UIntPtr(5);
+            fscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, fscanHandle.intensities, 5);
+            fscanHandle.intensities_count = new UIntPtr(5);
+            NavigationAPI.navigation_add_laser_scan(navHandle, "/fscan",  fscanHandle);
+
+            IntPtr bFrameId = Marshal.StringToHGlobalAnsi("bscan");
+            Header bscanHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(bFrameId));
+            LaserScan bscanHandle;
+            bscanHandle.header = bscanHeader;
+            bscanHandle.angle_min = 1.57f;
+            bscanHandle.angle_max = -1.57f;
+            bscanHandle.angle_increment = -0.785f;
+            bscanHandle.time_increment = 0.0f;
+            bscanHandle.scan_time = 0.1f;
+            bscanHandle.range_min = 0.05f;
+            bscanHandle.range_max = 10.0f;
+            bscanHandle.ranges = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 1.0f, 1.2f, 1.1f, 0.9f, 1.3f }, 0, bscanHandle.ranges, 5);
+            bscanHandle.ranges_count = new UIntPtr(5);
+            bscanHandle.intensities = Marshal.AllocHGlobal(sizeof(float) * 5);
+            Marshal.Copy(new float[] { 100.0f, 120.0f, 110.0f, 90.0f, 130.0f }, 0, bscanHandle.intensities, 5);
+            bscanHandle.intensities_count = new UIntPtr(5);
+            NavigationAPI.navigation_add_laser_scan(navHandle, "/bscan",  bscanHandle);
+
+            IntPtr oFrameId = Marshal.StringToHGlobalAnsi("odom");
+            Header odometryHeader = NavigationAPI.header_create(Marshal.PtrToStringAnsi(oFrameId));
+            Odometry odometryHandle = new Odometry();
+            odometryHandle.header = odometryHeader;
+            IntPtr childFrameId = Marshal.StringToHGlobalAnsi("base_footprint");
+            odometryHandle.child_frame_id = childFrameId;
+            odometryHandle.pose.pose.position.x = 0.0;
+            odometryHandle.pose.pose.position.y = 0.0;
+            odometryHandle.pose.pose.position.z = 0.0;
+            odometryHandle.pose.pose.orientation.x = 0.0;
+            odometryHandle.pose.pose.orientation.y = 0.0;
+            odometryHandle.pose.pose.orientation.z = 0.0;
+            odometryHandle.pose.pose.orientation.w = 1.0;
+            
+            double[] pose_covariance = new double[36];
+            for(int i = 0; i < pose_covariance.Length; i++) {
+                pose_covariance[i] = 0.0;
+            }
+            odometryHandle.pose.covariance = Marshal.AllocHGlobal(sizeof(double) * pose_covariance.Length);
+            Marshal.Copy(pose_covariance, 0, odometryHandle.pose.covariance, pose_covariance.Length);
+            odometryHandle.pose.covariance_count = new UIntPtr((uint)pose_covariance.Length);
+
+            odometryHandle.twist.twist.linear.x = 0.0;
+            odometryHandle.twist.twist.linear.y = 0.0;
+            odometryHandle.twist.twist.linear.z = 0.0;
+            odometryHandle.twist.twist.angular.x = 0.0;
+            odometryHandle.twist.twist.angular.y = 0.0;
+            odometryHandle.twist.twist.angular.z = 0.0;
+            double[] twist_covariance = new double[36];
+            for(int i = 0; i < twist_covariance.Length; i++) {
+                twist_covariance[i] = 0.0;
+            }
+            odometryHandle.twist.covariance = Marshal.AllocHGlobal(sizeof(double) * twist_covariance.Length);
+            Marshal.Copy(twist_covariance, 0, odometryHandle.twist.covariance, twist_covariance.Length);
+            odometryHandle.twist.covariance_count = new UIntPtr((uint)twist_covariance.Length);
+            NavigationAPI.navigation_add_odometry(navHandle, "odometry",  odometryHandle);
+
+            // Add static map: đọc maze.yaml rồi load ảnh và cập nhật mapMetaData
+            string mapYamlPath =  "maze.yaml";
+            int mapWidth, mapHeight;
+            byte[] data;
+            MazeMapConfig mapConfig;
+            if (TryLoadMazeYaml(mapYamlPath, out mapConfig))
+            {
+                data = LoadMazePixelsAsOccupancy(mapConfig.ImagePath, out mapWidth, out mapHeight,
+                    mapConfig.Negate, mapConfig.OccupiedThresh, mapConfig.FreeThresh);
+                if (data == null)
+                {
+                    mapWidth = 3;
+                    mapHeight = 10;
+                    data = new byte[30];
+                    for (int i = 0; i < data.Length; i++) data[i] = 100;
+                    Console.WriteLine("YAML loaded but image failed; using default map 3x10");
+                }
+                else
+                {
+                    Console.WriteLine("Loaded map from {0}: {1}x{2}, resolution={3}, origin=({4},{5},{6})",
+                        mapConfig.ImagePath, mapWidth, mapHeight, mapConfig.Resolution,
+                        mapConfig.OriginX, mapConfig.OriginY, mapConfig.OriginZ);
+                }
+            }
+            else
+            {
+                mapWidth = 3;
+                mapHeight = 10;
+                data = new byte[30];
+                for (int i = 0; i < data.Length; i++) data[i] = 100;
+                mapConfig = default;
+                mapConfig.Resolution = 0.05f;
+                mapConfig.OriginX = mapConfig.OriginY = mapConfig.OriginZ = 0.0;
+                Console.WriteLine("maze.yaml not found, using default map 3x10");
             }
 
-            // Cleanup
+            Time mapLoadTime = NavigationAPI.time_create();
+            MapMetaData mapMetaData = new MapMetaData();
+            mapMetaData.map_load_time = mapLoadTime;
+            mapMetaData.resolution = mapConfig.Resolution;
+            mapMetaData.width = (uint)mapWidth;
+            mapMetaData.height = (uint)mapHeight;
+            mapMetaData.origin = new Pose();
+            mapMetaData.origin.position.x = mapConfig.OriginX;
+            mapMetaData.origin.position.y = mapConfig.OriginY;
+            mapMetaData.origin.position.z = mapConfig.OriginZ;
+            mapMetaData.origin.orientation.x = 0.0;
+            mapMetaData.origin.orientation.y = 0.0;
+            mapMetaData.origin.orientation.z = 0.0;
+            mapMetaData.origin.orientation.w = 1.0;
+            OccupancyGrid occupancyGrid = new OccupancyGrid();
+            IntPtr mapFrameId = Marshal.StringToHGlobalAnsi("map");
+            occupancyGrid.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
+            occupancyGrid.info = mapMetaData;
+            occupancyGrid.data = Marshal.AllocHGlobal(sizeof(byte) * data.Length);
+            Marshal.Copy(data, 0, occupancyGrid.data, data.Length);
+            occupancyGrid.data_count = new UIntPtr((uint)data.Length);
+            Console.WriteLine("data length: {0} {1}", data.Length, occupancyGrid.data_count);
+            Console.WriteLine("C# OccupancyGrid sizeof={0} data_off={1} data_count_off={2}",
+                Marshal.SizeOf<OccupancyGrid>(),
+                Marshal.OffsetOf<OccupancyGrid>("data"),
+                Marshal.OffsetOf<OccupancyGrid>("data_count"));
+    
+            NavigationAPI.navigation_add_static_map(navHandle, "/map",  occupancyGrid);
+
+            Twist2DStamped twist = new Twist2DStamped();
+            if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
+            {
+                Console.WriteLine(
+                    "Twist: {0}, {1}, {2}, {3}", 
+                    NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
+            }  
+
+            // Build order (thao cách bom order): header + nodes + edges giống C++
+            Order order = new Order();
+            order.headerId = 1;
+            order.timestamp = Marshal.StringToHGlobalAnsi("2026-02-28 10:00:00");
+            order.version = Marshal.StringToHGlobalAnsi("1.0.0");
+            order.manufacturer = Marshal.StringToHGlobalAnsi("Manufacturer");
+            order.serialNumber = Marshal.StringToHGlobalAnsi("Serial Number");
+            order.orderId = Marshal.StringToHGlobalAnsi("Order ID");
+            order.orderUpdateId = 1;
+
+            // Nodes: giống for (auto node : order.nodes) { node_msg.nodeId = ...; node_msg.nodePosition.x = ...; order_msg.nodes.push_back(node_msg); }
+            int nodeCount = 1;
+            order.nodes = Marshal.AllocHGlobal(Marshal.SizeOf<Node>() * nodeCount);
+            order.nodes_count = new UIntPtr((uint)nodeCount);
+            Node node1 = new Node();
+            node1.nodeId = Marshal.StringToHGlobalAnsi("node-1");
+            node1.sequenceId = 0;
+            node1.nodeDescription = Marshal.StringToHGlobalAnsi("Goal node");
+            node1.released = 0;
+            node1.nodePosition.x = 1.0;
+            node1.nodePosition.y = 1.0;
+            node1.nodePosition.theta = 0.0;
+            node1.nodePosition.allowedDeviationXY = 0.1f;
+            node1.nodePosition.allowedDeviationTheta = 0.05f;
+            node1.nodePosition.mapId = Marshal.StringToHGlobalAnsi("map");
+            node1.nodePosition.mapDescription = Marshal.StringToHGlobalAnsi("");
+            node1.actions = IntPtr.Zero;
+            node1.actions_count = UIntPtr.Zero;
+            Marshal.StructureToPtr(node1, order.nodes, false);
+
+            // Edges: rỗng trong ví dụ này; nếu cần thì alloc và fill tương tự (edge_msg.edgeId, trajectory.controlPoints, ...)
+            order.edges = IntPtr.Zero;
+            order.edges_count = UIntPtr.Zero;
+            order.zoneSetId = Marshal.StringToHGlobalAnsi("");
+
+            PoseStamped goal = new PoseStamped();
+            goal.header = NavigationAPI.header_create(Marshal.PtrToStringAnsi(mapFrameId));
+            goal.pose.position.x = 0.01;
+            goal.pose.position.y = 0.01;
+            goal.pose.position.z = 0.0;
+            goal.pose.orientation.x = 0.0;
+            goal.pose.orientation.y = 0.0;
+            goal.pose.orientation.z = 0.0;
+            goal.pose.orientation.w = 1.0;
+
+
+            // Console.WriteLine("Docking to docking_point");
+            NavigationAPI.navigation_dock_to(navHandle, "charger", goal);
+
+            // NavigationAPI.navigation_move_to_nodes_edges(navHandle, order.nodes, order.nodes_count, order.edges, order.edges_count, goal);
+
+            // NavigationAPI.navigation_move_to_order(navHandle, order, goal);
+            NavigationAPI.navigation_set_twist_linear(navHandle, 0.1, 0.0, 0.0);
+            NavigationAPI.navigation_set_twist_angular(navHandle, 0.0, 0.0, 0.2);
+
+            // NavigationAPI.navigation_move_straight_to(navHandle, 1.0);
+
+            while (true) 
+            {
+                System.Threading.Thread.Sleep(100);
+            //     NavigationAPI.NavFeedback feedback = new NavigationAPI.NavFeedback();
+            //     if (NavigationAPI.navigation_get_feedback(navHandle, ref feedback))
+            //     {
+            //         if (feedback.navigation_state == NavigationAPI.NavigationState.Succeeded)
+            //         {
+            //             Console.WriteLine("Navigation is Succeeded");
+            //             break;
+            //         }
+            //     }
+
+            //     NavigationAPI.PlannerDataOutput globalData = new NavigationAPI.PlannerDataOutput();
+            //     if (NavigationAPI.navigation_get_global_data(navHandle, ref globalData))
+            //     {
+            //         int n = (int)(uint)globalData.plan.poses_count;
+            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
+            //         for (int i = 0; i < n; i++)
+            //         {
+            //             IntPtr posePtr = IntPtr.Add(globalData.plan.poses, i * poseSize);
+            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
+            //             double p_x = p.pose.x;
+            //             double p_y = p.pose.y;
+            //             double p_theta = p.pose.theta;
+            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
+            //         }
+            //         if(globalData.is_costmap_updated) {
+            //             for(int i = 0; i < (int)(uint)globalData.costmap.data_count; i++) {
+            //                 byte cellValue = Marshal.ReadByte(globalData.costmap.data, i);
+            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
+            //             }
+            //         }
+            //         else {
+            //             Console.WriteLine("Global Costmap is not updated");
+            //         }
+            //     }
+
+            //     NavigationAPI.PlannerDataOutput localData = new NavigationAPI.PlannerDataOutput();
+            //     if(NavigationAPI.navigation_get_local_data(navHandle, ref localData)) 
+            //     {
+            //         int n = (int)(uint)localData.plan.poses_count;
+            //         int poseSize = Marshal.SizeOf<Pose2DStamped>();
+            //         for (int i = 0; i < n; i++)
+            //         {
+            //             IntPtr posePtr = IntPtr.Add(localData.plan.poses, i * poseSize);
+            //             Pose2DStamped p = Marshal.PtrToStructure<Pose2DStamped>(posePtr);
+            //             double p_x = p.pose.x;
+            //             double p_y = p.pose.y;
+            //             double p_theta = p.pose.theta;
+            //             Console.WriteLine("Plan: {0}, {1}, {2}", p_x, p_y, p_theta);
+            //         }
+            //         if(localData.is_costmap_updated) {
+            //             for(int i = 0; i < (int)(uint)localData.costmap.data_count; i++) {
+            //                 byte cellValue = Marshal.ReadByte(localData.costmap.data, i);
+            //                 Console.WriteLine("Costmap: {0} {1}", i, cellValue);
+            //             }
+            //         }
+            //         else {
+            //             Console.WriteLine("Local Costmap is not updated");
+            //         }
+            //     }
+            }
+            // Cleanup (destroy nav first, then tf3 buffer)
+
             NavigationAPI.navigation_destroy(navHandle);
-            NavigationAPI.tf_listener_destroy(tfHandle);
+            TF3API.tf3_buffer_destroy(tf3Buffer);
+            Console.WriteLine("Press any key to exit...");
+            try
+            {
+                Console.ReadKey(intercept: true);
+            }
+            catch (InvalidOperationException)
+            {
+                // Running without a real console (e.g. redirected/automated run).
+            }
         }
     }
 }
-

examples/NavigationExample/TF3API.cs

@@ -0,0 +1,133 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+
+   // ============================================================================
+    // TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore)
+    // ============================================================================
+    public static class TF3API
+    {
+        private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib
+
+        public enum TF3_ErrorCode
+        {
+            TF3_OK = 0,
+            TF3_ERROR_LOOKUP = 1,
+            TF3_ERROR_CONNECTIVITY = 2,
+            TF3_ERROR_EXTRAPOLATION = 3,
+            TF3_ERROR_INVALID_ARGUMENT = 4,
+            TF3_ERROR_TIMEOUT = 5,
+            TF3_ERROR_UNKNOWN = 99
+        }
+
+        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
+        public struct TF3_Transform
+        {
+            public long timestamp_sec;
+            public long timestamp_nsec;
+            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
+            public string frame_id;
+            [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
+            public string child_frame_id;
+            public double translation_x;
+            public double translation_y;
+            public double translation_z;
+            public double rotation_x;
+            public double rotation_y;
+            public double rotation_z;
+            public double rotation_w;
+        }
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern IntPtr tf3_buffer_create(int cache_time_sec);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_buffer_destroy(IntPtr buffer);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_set_transform(
+            IntPtr buffer,
+            ref TF3_Transform transform,
+            string authority,
+            [MarshalAs(UnmanagedType.I1)] bool is_static);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_lookup_transform(
+            IntPtr buffer,
+            string target_frame,
+            string source_frame,
+            long time_sec,
+            long time_nsec,
+            out TF3_Transform transform,
+            out TF3_ErrorCode error_code);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_lookup_transform_full(
+            IntPtr buffer,
+            string target_frame,
+            long target_time_sec,
+            long target_time_nsec,
+            string source_frame,
+            long source_time_sec,
+            long source_time_nsec,
+            string fixed_frame,
+            out TF3_Transform transform,
+            out TF3_ErrorCode error_code);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_can_transform(
+            IntPtr buffer,
+            string target_frame,
+            string source_frame,
+            long time_sec,
+            long time_nsec,
+            System.Text.StringBuilder error_msg,
+            int error_msg_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_clear(IntPtr buffer);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void tf3_get_current_time(out long sec, out long nsec);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len);
+
+        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern IntPtr tf3_get_version();
+
+        /// <summary>Helper: create TF3_Transform for static transform (identity or given pose).</summary>
+        public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame,
+            double tx = 0, double ty = 0, double tz = 0,
+            double qx = 0, double qy = 0, double qz = 0, double qw = 1)
+        {
+            var t = new TF3_Transform();
+            t.timestamp_sec = 0;
+            t.timestamp_nsec = 0;
+            t.frame_id = parentFrame ?? "";
+            t.child_frame_id = childFrame ?? "";
+            t.translation_x = tx;
+            t.translation_y = ty;
+            t.translation_z = tz;
+            t.rotation_x = qx;
+            t.rotation_y = qy;
+            t.rotation_z = qz;
+            t.rotation_w = qw;
+            return t;
+        }
+    }
+}

examples/NavigationExample/maze.yaml

@@ -0,0 +1,6 @@
+image: maze.png
+resolution: 0.05
+origin: [0.0, 0.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

examples/NavigationExample/msgs/CommonMsgs.cs

@@ -0,0 +1,274 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+    // ============================================================================
+    // Structures
+    // ============================================================================
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Point
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose2D
+    {
+        public double x;
+        public double y;
+        public double theta;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist2D
+    {
+        public double x;
+        public double y;
+        public double theta;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Quaternion
+    {
+        public double x;
+        public double y;
+        public double z;
+        public double w;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Position
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose
+    {
+        public Point position;
+        public Quaternion orientation;
+    }
+
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Vector3
+    {
+        public double x;
+        public double y;
+        public double z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist
+    {
+        public Vector3 linear;
+        public Vector3 angular;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Header
+    {
+        public uint seq;
+        public uint sec;
+        public uint nsec;
+        public IntPtr frame_id; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PoseStamped
+    {
+        public Header header;
+        public Pose pose;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Twist2DStamped
+    {
+        public Header header;
+        public Twist2D velocity;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct LaserScan
+    {
+        public Header header;
+        public float angle_min;
+        public float angle_max;
+        public float angle_increment;
+        public float time_increment;
+        public float scan_time;
+        public float range_min;
+        public float range_max;
+        public IntPtr ranges;
+        public UIntPtr ranges_count;
+        public IntPtr intensities;
+        public UIntPtr intensities_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PointCloud
+    {
+        public Header header;
+        public IntPtr points;
+        public UIntPtr points_count;
+        public IntPtr channels;
+        public UIntPtr channels_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PointCloud2
+    {
+        public Header header;
+        public uint height;
+        public uint width;
+        public IntPtr fields;
+        public UIntPtr fields_count;
+        [MarshalAs(UnmanagedType.I1)]
+        public bool is_bigendian;
+        public uint point_step;
+        public uint row_step;
+        public IntPtr data;
+        public UIntPtr data_count;
+        [MarshalAs(UnmanagedType.I1)]
+        public bool is_dense;
+    } 
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PoseWithCovariance
+    {
+        public Pose pose;
+        public IntPtr covariance;
+        public UIntPtr covariance_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct TwistWithCovariance    {
+        public Twist twist;
+        public IntPtr covariance;
+        public UIntPtr covariance_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Odometry
+    {
+        public Header header;
+        public IntPtr child_frame_id;
+        public PoseWithCovariance pose;
+        public TwistWithCovariance twist;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct OccupancyGrid
+    {
+        public Header header;
+        public MapMetaData info;
+        public IntPtr data;
+        public UIntPtr data_count;
+    }
+
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct MapMetaData
+    {
+        public Time map_load_time;
+        public float resolution;
+        public uint width;
+        public uint height;
+        public Pose origin;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Time
+    {
+        public uint sec;
+        public uint nsec;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Point32
+    {
+        public float x;
+        public float y;
+        public float z;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Pose2DStamped
+    {
+        public Header header;
+        public Pose2D pose;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Polygon
+    {
+        public IntPtr points;
+        public UIntPtr points_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct PolygonStamped
+    {
+        public Header header;
+        public Polygon polygon;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct OccupancyGridUpdate
+    {
+        public Header header;
+        public int x;
+        public int y;
+        public uint width;
+        public uint height;
+        public IntPtr data;
+        public UIntPtr data_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Path2D
+    {
+        public Header header;
+        public IntPtr poses;
+        public UIntPtr poses_count;
+    }
+
+    /// <summary>Name (char*) + OccupancyGrid for static map list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedOccupancyGrid
+    {
+        public IntPtr name;
+        public OccupancyGrid grid;
+    }
+
+    /// <summary>Name (char*) + LaserScan for laser scan list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedLaserScan
+    {
+        public IntPtr name;
+        public LaserScan scan;
+    }
+
+    /// <summary>Name (char*) + PointCloud for point cloud list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedPointCloud
+    {
+        public IntPtr name;
+        public PointCloud cloud;
+    }
+
+    /// <summary>Name (char*) + PointCloud2 for point cloud2 list API.</summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NamedPointCloud2
+    {
+        public IntPtr name;
+        public PointCloud2 cloud;
+    }
+
+
+}

examples/NavigationExample/msgs/ProtocolMsgsTypes.cs

@@ -0,0 +1,109 @@
+using System;
+using System.Runtime.InteropServices;
+
+namespace NavigationExample
+{
+    /// <summary>
+    /// C# struct layout cho protocol_msgs (robot_protocol_msgs C API).
+    /// Khớp với pnkx_nav_core/src/APIs/c_api/include/protocol_msgs/*.h
+    /// </summary>
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ControlPoint
+    {
+        public double x;
+        public double y;
+        public double weight;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct ActionParameter
+    {
+        public IntPtr key;   // char*
+        public IntPtr value; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct NodePosition
+    {
+        public double x;
+        public double y;
+        public double theta;
+        public float allowedDeviationXY;
+        public float allowedDeviationTheta;
+        public IntPtr mapId;          // char*
+        public IntPtr mapDescription; // char*
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Trajectory
+    {
+        public uint degree;
+        public IntPtr knotVector;           // double*
+        public UIntPtr knotVector_count;
+        public IntPtr controlPoints;        // ControlPoint*
+        public UIntPtr controlPoints_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Action
+    {
+        public IntPtr actionType;       // char*
+        public IntPtr actionId;         // char*
+        public IntPtr actionDescription;// char*
+        public IntPtr blockingType;     // char*
+        public IntPtr actionParameters; // ActionParameter*
+        public UIntPtr actionParameters_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Node
+    {
+        public IntPtr nodeId;           // char*
+        public int sequenceId;
+        public IntPtr nodeDescription;  // char*
+        public byte released;
+        public NodePosition nodePosition;
+        public IntPtr actions;          // Action*
+        public UIntPtr actions_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Edge
+    {
+        public IntPtr edgeId;           // char*
+        public int sequenceId;
+        public IntPtr edgeDescription; // char*
+        public byte released;
+        public IntPtr startNodeId;     // char*
+        public IntPtr endNodeId;       // char*
+        public double maxSpeed;
+        public double maxHeight;
+        public double minHeight;
+        public double orientation;
+        public IntPtr orientationType; // char*
+        public IntPtr direction;       // char*
+        public byte rotationAllowed;
+        public double maxRotationSpeed;
+        public Trajectory trajectory;
+        public double length;
+        public IntPtr actions;          // Action*
+        public UIntPtr actions_count;
+    }
+
+    [StructLayout(LayoutKind.Sequential)]
+    public struct Order
+    {
+        public int headerId;
+        public IntPtr timestamp;        // char*
+        public IntPtr version;         // char*
+        public IntPtr manufacturer;    // char*
+        public IntPtr serialNumber;    // char*
+        public IntPtr orderId;         // char*
+        public uint orderUpdateId;
+        public IntPtr nodes;           // Node*
+        public UIntPtr nodes_count;
+        public IntPtr edges;           // Edge*
+        public UIntPtr edges_count;
+        public IntPtr zoneSetId;      // char*
+    }
+}

examples/QUICK_START.md

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

examples/README.md

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

examples/run_example.sh

@@ -7,7 +7,7 @@ set -e
 SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
 PROJECT_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)"
 BUILD_DIR="$PROJECT_ROOT/build"
-LIB_DIR="$BUILD_DIR/src/APIs/c_api"
+LIB_DIR="$BUILD_DIR"
 EXAMPLE_DIR="$SCRIPT_DIR"
 
 echo "=========================================="
@@ -27,6 +27,7 @@ echo "Building C API library..."
 cd "$BUILD_DIR"
 cmake ..
 make 
+sudo make install
 echo "Library built successfully!"
 
 
@@ -82,22 +83,27 @@ fi
 
 # Luôn copy source code mới nhất (cập nhật file nếu đã có)
 cd "$EXAMPLE_DIR/NavigationExample"
-echo "Updating Program.cs from CSharpExample.cs..."
-cp ../CSharpExample.cs Program.cs
 
-# Bước 3: Copy library
+# # Bước 3: Copy library
-echo "Copying library..."
+# echo "Copying library..."
-cp "$LIB_DIR/libnav_c_api.so" .
+# cp "$LIB_DIR/libnav_c_api.so" .
 
 # Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
-export LD_LIBRARY_PATH="$LIB_DIR:$LD_LIBRARY_PATH"
+# Main build directory (contains libtf3.so, librobot_cpp.so, etc.)
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
+# Library directories for dependencies
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_time:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/xmlrpcpp:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH"
+export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
 export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"
-export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH"
+# Also add the example directory where the library is copied
+export LD_LIBRARY_PATH="$EXAMPLE_DIR/NavigationExample:$LD_LIBRARY_PATH"
+# Add the output directory where .NET places the library
+export LD_LIBRARY_PATH="$EXAMPLE_DIR/NavigationExample/bin/Debug/net6.0/linux-x64:$LD_LIBRARY_PATH"
 
 # Bước 5: Build và chạy
 echo "Building C# project..."

pnkx_nav_core.sln

@@ -0,0 +1,29 @@
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.5.2.0
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "examples", "examples", "{B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "NavigationExample", "examples\NavigationExample\NavigationExample.csproj", "{995839D6-1E72-F444-6587-97EF24F93814}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{995839D6-1E72-F444-6587-97EF24F93814}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(NestedProjects) = preSolution
+		{995839D6-1E72-F444-6587-97EF24F93814} = {B36A84DF-456D-A817-6EDD-3EC3E7F6E11F}
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {1A56EED1-5E6A-48A6-9AEE-F39361B34305}
+	EndGlobalSection
+EndGlobal

setup_catkin_workspace.sh

@@ -36,7 +36,7 @@ echo "Creating symlinks to packages with package.xml..."
 # Find all packages with package.xml in the source tree
 PACKAGES=(
     "src/Libraries/tf3"
-    "src/Libraries/nav_2d_utils"
+    "src/Libraries/robot_nav_2d_utils"
     "src/Navigations/Libraries/nav_grid"
     "src/Navigations/Cores/nav_core"
     "src/Navigations/Cores/nav_core2"

src/APIs/c_api/CMakeLists.txt

@@ -10,13 +10,20 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
-# Dependencies
+# Find Boost (filesystem needed for plugin path / boost::dll usage)
+find_package(Boost REQUIRED COMPONENTS filesystem system)
+
+# Dependencies (robot_nav_2d_utils_conversions provides poseStampedToPose2D used by move_base_core/navigation.h)
 set(PACKAGES_DIR
     move_base_core
-    move_base
+    robot_nav_2d_utils
     tf3
     robot_time
+    robot_cpp
     geometry_msgs
+    angles
+    data_convert
+    robot_protocol_msgs
 )
 
 # Thư mục include
@@ -25,8 +32,28 @@ include_directories(
 )
 
 # Tìm tất cả file source
-file(GLOB SOURCES "src/*.cpp")
+file(GLOB SOURCES 
-file(GLOB HEADERS "include/*.h")
+    "src/*.cpp" 
+    "src/std_msgs/*.cpp" 
+    "src/sensor_msgs/*.cpp" 
+    "src/geometry_msgs/*.cpp"
+    "src/nav_msgs/*.cpp"
+    "src/nav_2d_msgs/*.cpp"
+    "src/robot_nav_msgs/*.cpp"
+    "src/robot_geometry_msgs/*.cpp"
+    "src/map_msgs/*.cpp"
+)
+file(GLOB HEADERS 
+    "include/*.h" 
+    "include/std_msgs/*.h" 
+    "include/sensor_msgs/*.h" 
+    "include/geometry_msgs/*.h"
+    "include/nav_msgs/*.h"
+    "include/nav_2d_msgs/*.h"
+    "include/robot_nav_msgs/*.h"
+    "include/robot_geometry_msgs/*.h"
+    "include/map_msgs/*.h"
+)
 
 # Tạo thư viện shared (.so)
 add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
@@ -35,6 +62,17 @@ add_library(nav_c_api SHARED ${SOURCES} ${HEADERS})
 target_link_libraries(nav_c_api
     PUBLIC
         ${PACKAGES_DIR}
+    PRIVATE
+        Boost::boost
+        Boost::filesystem
+        Boost::system
+        dl
+)
+
+set_target_properties(nav_c_api PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  BUILD_RPATH "${CMAKE_BINARY_DIR}"
+  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
 )
 
 # Set include directories
@@ -69,7 +107,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 option(BUILD_TESTS "Build test programs" OFF)
 
 # Flags biên dịch
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+# 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")
 

src/APIs/c_api/include/convertor.h

@@ -0,0 +1,206 @@
+#ifndef C_API_CONVERTOR_H
+#define C_API_CONVERTOR_H
+
+// C
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/PointCloud.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "nav_msgs/Odometry.h"
+#include "std_msgs/Header.h"
+#include "nav_msgs/OccupancyGrid.h"
+#include "geometry_msgs/Point.h"
+#include "geometry_msgs/PoseStamped.h"
+#include "geometry_msgs/Pose2D.h"
+#include "nav_2d_msgs/Twist2D.h"
+#include "nav_2d_msgs/Twist2DStamped.h"
+#include "nav_2d_msgs/Path2D.h"
+#include "map_msgs/OccupancyGridUpdate.h"
+#include "geometry_msgs/PolygonStamped.h"
+#include "protocol_msgs/Order.h"
+
+// C++
+#include <robot_sensor_msgs/LaserScan.h>
+#include <robot_sensor_msgs/PointCloud.h>
+#include <robot_sensor_msgs/PointCloud2.h>
+#include <robot_nav_msgs/Odometry.h>
+#include <robot_nav_msgs/OccupancyGrid.h>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/Pose2D.h>
+#include <robot_nav_2d_msgs/Twist2D.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
+#include <robot_nav_2d_msgs/Path2D.h>
+#include <robot_map_msgs/OccupancyGridUpdate.h>
+#include <robot_geometry_msgs/PolygonStamped.h>
+#include <robot_protocol_msgs/Order.h>
+
+#include <move_base_core/navigation.h>
+#include <move_base_core/common.h>
+
+/**
+ * @brief Convert C LaserScan to C++ LaserScan
+ * @param laser_scan C LaserScan
+ * @return C++ LaserScan
+ */
+robot_sensor_msgs::LaserScan convert2CppLaserScan(const LaserScan& laser_scan);
+
+/**
+ * @brief Convert C OccupancyGrid to C++ OccupancyGrid
+ * @param occupancy_grid C OccupancyGrid
+ * @return C++ OccupancyGrid
+ */
+robot_nav_msgs::OccupancyGrid convert2CppOccupancyGrid(const OccupancyGrid& occupancy_grid);
+
+/**
+ * @brief Convert C++ OccupancyGrid to C OccupancyGrid
+ * @param cpp C++ OccupancyGrid
+ * @param out C OccupancyGrid
+ */
+void convert2COccupancyGrid(const robot_nav_msgs::OccupancyGrid& cpp, OccupancyGrid& out);
+
+/**
+ * @brief Convert C++ Path2D to C Path2D
+ */
+void convert2CPath2D(const robot_nav_2d_msgs::Path2D& cpp, Path2D& out);
+
+/**
+ * @brief Convert C++ OccupancyGridUpdate to C OccupancyGridUpdate
+ */
+void convert2COccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate& cpp, OccupancyGridUpdate& out);
+
+/**
+ * @brief Convert C++ PolygonStamped to C PolygonStamped
+ */
+void convert2CPolygonStamped(const robot_geometry_msgs::PolygonStamped& cpp, PolygonStamped& out);
+
+/**
+ * @brief Convert C++ LaserScan to C LaserScan
+ * @param cpp C++ LaserScan
+ * @param out C LaserScan
+ */
+void convert2CLaserScan(const robot_sensor_msgs::LaserScan& cpp, LaserScan& out);
+
+/**
+ * @brief Convert C++ PointCloud to C PointCloud
+ * @param cpp C++ PointCloud
+ * @param out C PointCloud
+ */
+void convert2CPointCloud(const robot_sensor_msgs::PointCloud& cpp, PointCloud& out);
+
+/**
+ * @brief Convert C++ PointCloud2 to C PointCloud2
+ * @param cpp C++ PointCloud2
+ * @param out C PointCloud2
+ */
+void convert2CPointCloud2(const robot_sensor_msgs::PointCloud2& cpp, PointCloud2& out);
+
+/**
+ * @brief Convert C PointCloud to C++ PointCloud
+ * @param c C PointCloud
+ * @return C++ PointCloud
+ */
+robot_sensor_msgs::PointCloud convert2CppPointCloud(const PointCloud& c);
+
+/**
+ * @brief Convert C PointCloud2 to C++ PointCloud2
+ * @param c C PointCloud2
+ * @return C++ PointCloud2
+ */
+robot_sensor_msgs::PointCloud2 convert2CppPointCloud2(const PointCloud2& c);
+
+/**
+ * @brief Convert C Odometry to C++ Odometry
+ * @param odometry C Odometry
+ * @return C++ Odometry
+ */
+robot_nav_msgs::Odometry convert2CppOdometry(const Odometry& odometry);
+
+/**
+ * @brief Convert C PoseStamped to C++ PoseStamped
+ * @param pose_stamped C PoseStamped
+ * @return C++ PoseStamped
+ */
+robot_geometry_msgs::PoseStamped convert2CppPoseStamped(const PoseStamped& pose_stamped);
+
+/**
+ * @brief Convert C++ PoseStamped to C PoseStamped
+ * @param cpp_pose_stamped C++ PoseStamped
+ * @return C PoseStamped
+ */
+PoseStamped convert2CPoseStamped(const robot_geometry_msgs::PoseStamped& cpp_pose_stamped);
+
+
+/**
+ * @brief Convert C Pose2D to C++ Pose2D
+ * @param pose_2d C Pose2D
+ * @return C++ Pose2D
+ */
+robot_geometry_msgs::Pose2D convert2CppPose2D(const Pose2D& pose_2d);
+
+/**
+ * @brief Convert C++ Pose2D to C Pose2D
+ * @param cpp_pose_2d C++ Pose2D
+ * @return C Pose2D
+ */
+ Pose2D convert2CPose2D(const robot_geometry_msgs::Pose2D& cpp_pose_2d);
+
+/**
+ * @brief Convert C Twist2D to C++ Twist2D
+ * @param twist_2d C Twist2D
+ * @return C++ Twist2D
+ */
+robot_nav_2d_msgs::Twist2D convert2CppTwist2D(const Twist2D& twist_2d);
+
+
+/**
+ * @brief Convert C++ Twist2D to C Twist2D
+ * @param cpp_twist_2d C++ Twist2D
+ * @return C Twist2D
+ */
+Twist2D convert2CTwist2D(const robot_nav_2d_msgs::Twist2D& cpp_twist_2d);
+
+/**
+ * @brief Convert C Twist2DStamped to C++ Twist2DStamped
+ * @param twist_2d_stamped C Twist2DStamped
+ * @return C++ Twist2DStamped
+ */
+robot_nav_2d_msgs::Twist2DStamped convert2CppTwist2DStamped(const Twist2DStamped& twist_2d_stamped);
+
+
+/**
+ * @brief Convert C++ Twist2DStamped to C Twist2DStamped
+ * @param cpp_twist_2d_stamped C++ Twist2DStamped
+ * @return C Twist2DStamped
+ */
+Twist2DStamped convert2CTwist2DStamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist_2d_stamped);
+
+/**
+ * @brief Convert C Order to C++ Order
+ * @param order C Order
+ * @return C++ Order
+ */
+robot_protocol_msgs::Order convert2CppOrder(const Order& order);
+
+/**
+ * @brief Convert C++ Order to C Order
+ * @param cpp_order C++ Order
+ * @return C Order (caller must call order_free when done to release memory)
+ */
+Order convert2COrder(const robot_protocol_msgs::Order& cpp_order);
+
+/**
+ * @brief Check if a quaternion is valid
+ * @param q Quaternion
+ * @return True if valid, false otherwise
+ */
+bool isQuaternionValid(const Quaternion& q);
+
+/**
+ * @brief Free dynamic memory held by an Order returned from convert2COrder
+ * @param order Order to free (pointers and counts are zeroed)
+ */
+#ifdef __cplusplus
+extern "C"
+#endif
+void order_free(Order* order);
+
+#endif // C_API_CONVERTOR_H

src/APIs/c_api/include/geometry_msgs/Accel.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCEL_H
+#define C_API_GEOMETRY_MSGS_ACCEL_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Vector3 linear;
+  Vector3 angular;
+} Accel;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCEL_H

src/APIs/c_api/include/geometry_msgs/AccelStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCELSTAMPED_H
+#define C_API_GEOMETRY_MSGS_ACCELSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Accel.h"
+
+typedef struct
+{
+  Header header;
+  Accel accel;
+} AccelStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCELSTAMPED_H

src/APIs/c_api/include/geometry_msgs/AccelWithCovariance.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H
+#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Accel.h"
+
+typedef struct
+{
+  Accel accel;
+  double covariance[36];
+} AccelWithCovariance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCE_H

src/APIs/c_api/include/geometry_msgs/AccelWithCovarianceStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H
+#define C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/AccelWithCovariance.h"
+
+typedef struct
+{
+  Header header;
+  AccelWithCovariance accel;
+} AccelWithCovarianceStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_ACCELWITHCOVARIANCESTAMPED_H

src/APIs/c_api/include/geometry_msgs/Inertia.h

@@ -0,0 +1,28 @@
+#ifndef C_API_GEOMETRY_MSGS_INERTIA_H
+#define C_API_GEOMETRY_MSGS_INERTIA_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  double m;
+  Vector3 com;
+  double ixx;
+  double ixy;
+  double ixz;
+  double iyy;
+  double iyz;
+  double izz;
+} Inertia;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_INERTIA_H

src/APIs/c_api/include/geometry_msgs/InertiaStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_INERTIASTAMPED_H
+#define C_API_GEOMETRY_MSGS_INERTIASTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Inertia.h"
+
+typedef struct
+{
+  Header header;
+  Inertia inertia;
+} InertiaStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_INERTIASTAMPED_H

src/APIs/c_api/include/geometry_msgs/Point.h

@@ -0,0 +1,21 @@
+#ifndef C_API_GEOMETRY_MSGS_POINT_H
+#define C_API_GEOMETRY_MSGS_POINT_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+#include <stdint.h>
+#include <stddef.h>
+
+  typedef struct
+  {
+    double x;
+    double y;
+    double z;
+  } Point;
+
+#ifdef __cplusplus
+}
+#endif
+#endif // C_API_GEOMETRY_MSGS_POINT_H

src/APIs/c_api/include/geometry_msgs/Point32.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_POINT32_H
+#define C_API_GEOMETRY_MSGS_POINT32_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  float x;
+  float y;
+  float z;
+} Point32;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POINT32_H

src/APIs/c_api/include/geometry_msgs/PointStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POINTSTAMPED_H
+#define C_API_GEOMETRY_MSGS_POINTSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Point.h"
+
+typedef struct
+{
+  Header header;
+  Point point;
+} PointStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POINTSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Polygon.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_POLYGON_H
+#define C_API_GEOMETRY_MSGS_POLYGON_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Point32.h"
+
+typedef struct
+{
+  Point32 *points;
+  size_t points_count;
+} Polygon;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POLYGON_H

src/APIs/c_api/include/geometry_msgs/PolygonStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H
+#define C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Polygon.h"
+
+typedef struct
+{
+  Header header;
+  Polygon polygon;
+} PolygonStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POLYGONSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Pose.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSE_H
+#define C_API_GEOMETRY_MSGS_POSE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Point.h"
+#include "geometry_msgs/Quaternion.h"
+
+typedef struct
+{
+  Point position;
+  Quaternion orientation;
+} Pose;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSE_H

src/APIs/c_api/include/geometry_msgs/Pose2D.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_POSE2D_H
+#define C_API_GEOMETRY_MSGS_POSE2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double theta;
+} Pose2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSE2D_H

src/APIs/c_api/include/geometry_msgs/PoseArray.h

@@ -0,0 +1,24 @@
+#ifndef C_API_GEOMETRY_MSGS_POSEARRAY_H
+#define C_API_GEOMETRY_MSGS_POSEARRAY_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose.h"
+
+typedef struct
+{
+  Header header;
+  Pose *poses;
+  size_t poses_count;
+} PoseArray;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSEARRAY_H

src/APIs/c_api/include/geometry_msgs/PoseStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSESTAMPED_H
+#define C_API_GEOMETRY_MSGS_POSESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose.h"
+
+typedef struct
+{
+  Header header;
+  Pose pose;
+} PoseStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSESTAMPED_H

src/APIs/c_api/include/geometry_msgs/PoseWithCovariance.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H
+#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Pose.h"
+
+typedef struct
+{
+  Pose pose;
+  double *covariance;
+  size_t covariance_count;
+} PoseWithCovariance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCE_H

src/APIs/c_api/include/geometry_msgs/PoseWithCovarianceStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H
+#define C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/PoseWithCovariance.h"
+
+typedef struct
+{
+  Header header;
+  PoseWithCovariance pose;
+} PoseWithCovarianceStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED_H

src/APIs/c_api/include/geometry_msgs/Quaternion.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_QUATERNION_H
+#define C_API_GEOMETRY_MSGS_QUATERNION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double z;
+  double w;
+} Quaternion;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_QUATERNION_H

src/APIs/c_api/include/geometry_msgs/QuaternionStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H
+#define C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Quaternion.h"
+
+typedef struct
+{
+  Header header;
+  Quaternion quaternion;
+} QuaternionStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_QUATERNIONSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Transform.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TRANSFORM_H
+#define C_API_GEOMETRY_MSGS_TRANSFORM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+#include "geometry_msgs/Quaternion.h"
+
+typedef struct
+{
+  Vector3 translation;
+  Quaternion rotation;
+} Transform;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TRANSFORM_H

src/APIs/c_api/include/geometry_msgs/TransformStamped.h

@@ -0,0 +1,24 @@
+#ifndef C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H
+#define C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Transform.h"
+
+typedef struct
+{
+  Header header;
+  char *child_frame_id;
+  Transform transform;
+} TransformStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TRANSFORMSTAMPED_H

src/APIs/c_api/include/geometry_msgs/Twist.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_TWIST_H
+#define C_API_GEOMETRY_MSGS_TWIST_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Vector3 linear;
+  Vector3 angular;
+} Twist;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWIST_H

src/APIs/c_api/include/geometry_msgs/TwistStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TWISTSTAMPED_H
+#define C_API_GEOMETRY_MSGS_TWISTSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Twist.h"
+
+typedef struct
+{
+  Header header;
+  Twist twist;
+} TwistStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWISTSTAMPED_H

src/APIs/c_api/include/geometry_msgs/TwistWithCovariance.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H
+#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Twist.h"
+
+typedef struct
+{
+  Twist twist;
+  double *covariance;
+  size_t covariance_count;
+} TwistWithCovariance;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCE_H

src/APIs/c_api/include/geometry_msgs/TwistWithCovarianceStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H
+#define C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/TwistWithCovariance.h"
+
+typedef struct
+{
+  Header header;
+  TwistWithCovariance twist;
+} TwistWithCovarianceStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_TWISTWITHCOVARIANCESTAMPED_H

src/APIs/c_api/include/geometry_msgs/Vector3.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_VECTOR3_H
+#define C_API_GEOMETRY_MSGS_VECTOR3_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double z;
+} Vector3;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_VECTOR3_H

src/APIs/c_api/include/geometry_msgs/Vector3Stamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H
+#define C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Header header;
+  Vector3 vector;
+} Vector3Stamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_VECTOR3STAMPED_H

src/APIs/c_api/include/geometry_msgs/Wrench.h

@@ -0,0 +1,22 @@
+#ifndef C_API_GEOMETRY_MSGS_WRENCH_H
+#define C_API_GEOMETRY_MSGS_WRENCH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "geometry_msgs/Vector3.h"
+
+typedef struct
+{
+  Vector3 force;
+  Vector3 torque;
+} Wrench;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_WRENCH_H

src/APIs/c_api/include/geometry_msgs/WrenchStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H
+#define C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Wrench.h"
+
+typedef struct
+{
+  Header header;
+  Wrench wrench;
+} WrenchStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_GEOMETRY_MSGS_WRENCHSTAMPED_H

src/APIs/c_api/include/map_msgs/OccupancyGridUpdate.h

@@ -0,0 +1,33 @@
+#ifndef C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H
+#define C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+
+/**
+ * C representation of robot_map_msgs::OccupancyGridUpdate.
+ * Note: This header is intentionally named *_c.h to avoid
+ * shadowing the C++ header `robot_map_msgs/OccupancyGridUpdate.h`.
+ */
+typedef struct
+{
+  Header header;
+  int32_t x;
+  int32_t y;
+  uint32_t width;
+  uint32_t height;
+  int8_t *data;
+  size_t data_count;
+} OccupancyGridUpdate;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_ROBOT_MAP_MSGS_OCCUPANCYGRIDUPDATE_C_H
+

src/APIs/c_api/include/nav_2d_msgs/ComplexPolygon2D.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H
+#define C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_2d_msgs/Polygon2D.h"
+
+typedef struct
+{
+  Polygon2D outer;
+  Polygon2D *inner;
+  size_t inner_count;
+} ComplexPolygon2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_COMPLEXPOLYGON2D_H

src/APIs/c_api/include/nav_2d_msgs/NavGridInfo.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDINFO_H
+#define C_API_NAV_2D_MSGS_NAVGRIDINFO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint32_t width;
+  uint32_t height;
+  double resolution;
+  char *frame_id;
+  double origin_x;
+  double origin_y;
+} NavGridInfo;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDINFO_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfChars.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/NavGridInfo.h"
+
+typedef struct
+{
+  Time stamp;
+  NavGridInfo info;
+  uint8_t *data;
+  size_t data_count;
+} NavGridOfChars;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARS_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfCharsUpdate.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/UIntBounds.h"
+
+typedef struct
+{
+  Time stamp;
+  UIntBounds bounds;
+  uint8_t *data;
+  size_t data_count;
+} NavGridOfCharsUpdate;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFCHARSUPDATE_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoubles.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/NavGridInfo.h"
+
+typedef struct
+{
+  Time stamp;
+  NavGridInfo info;
+  double *data;
+  size_t data_count;
+} NavGridOfDoubles;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLES_H

src/APIs/c_api/include/nav_2d_msgs/NavGridOfDoublesUpdate.h

@@ -0,0 +1,25 @@
+#ifndef C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H
+#define C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Time.h"
+#include "nav_2d_msgs/UIntBounds.h"
+
+typedef struct
+{
+  Time stamp;
+  UIntBounds bounds;
+  double *data;
+  size_t data_count;
+} NavGridOfDoublesUpdate;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_NAVGRIDOFDOUBLESUPDATE_H

src/APIs/c_api/include/nav_2d_msgs/Path2D.h

@@ -0,0 +1,24 @@
+#ifndef C_API_NAV_2D_MSGS_PATH2D_H
+#define C_API_NAV_2D_MSGS_PATH2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_2d_msgs/Pose2DStamped.h"
+
+typedef struct
+{
+  Header header;
+  Pose2DStamped *poses;
+  size_t poses_count;
+} Path2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_PATH2D_H

src/APIs/c_api/include/nav_2d_msgs/Point2D.h

@@ -0,0 +1,21 @@
+#ifndef C_API_NAV_2D_MSGS_POINT2D_H
+#define C_API_NAV_2D_MSGS_POINT2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+} Point2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POINT2D_H

src/APIs/c_api/include/nav_2d_msgs/Polygon2D.h

@@ -0,0 +1,22 @@
+#ifndef C_API_NAV_2D_MSGS_POLYGON2D_H
+#define C_API_NAV_2D_MSGS_POLYGON2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_2d_msgs/Point2D.h"
+
+typedef struct
+{
+  Point2D *points;
+  size_t points_count;
+} Polygon2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POLYGON2D_H

src/APIs/c_api/include/nav_2d_msgs/Polygon2DCollection.h

@@ -0,0 +1,27 @@
+#ifndef C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H
+#define C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "std_msgs/ColorRGBA.h"
+#include "nav_2d_msgs/ComplexPolygon2D.h"
+
+typedef struct
+{
+  Header header;
+  ComplexPolygon2D *polygons;
+  size_t polygons_count;
+  ColorRGBA *colors;
+  size_t colors_count;
+} Polygon2DCollection;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POLYGON2DCOLLECTION_H

src/APIs/c_api/include/nav_2d_msgs/Polygon2DStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H
+#define C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "nav_2d_msgs/Polygon2D.h"
+
+typedef struct
+{
+  Header header;
+  Polygon2D polygon;
+} Polygon2DStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POLYGON2DSTAMPED_H

src/APIs/c_api/include/nav_2d_msgs/Pose2D32.h

@@ -0,0 +1,22 @@
+#ifndef C_API_NAV_2D_MSGS_POSE2D32_H
+#define C_API_NAV_2D_MSGS_POSE2D32_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  float x;
+  float y;
+  float theta;
+} Pose2D32;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POSE2D32_H

src/APIs/c_api/include/nav_2d_msgs/Pose2DStamped.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_POSE2DSTAMPED_H
+#define C_API_NAV_2D_MSGS_POSE2DSTAMPED_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "std_msgs/Header.h"
+#include "geometry_msgs/Pose2D.h"
+
+typedef struct
+{
+  Header header;
+  Pose2D pose;
+} Pose2DStamped;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_POSE2DSTAMPED_H

src/APIs/c_api/include/nav_2d_msgs/SwitchPlugin.h

@@ -0,0 +1,23 @@
+#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGIN_H
+#define C_API_NAV_2D_MSGS_SWITCHPLUGIN_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+#include "nav_2d_msgs/SwitchPluginRequest.h"
+#include "nav_2d_msgs/SwitchPluginResponse.h"
+
+typedef struct
+{
+  SwitchPluginRequest request;
+  SwitchPluginResponse response;
+} SwitchPlugin;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_SWITCHPLUGIN_H

src/APIs/c_api/include/nav_2d_msgs/SwitchPluginRequest.h

@@ -0,0 +1,20 @@
+#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H
+#define C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  char *new_plugin;
+} SwitchPluginRequest;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINREQUEST_H

src/APIs/c_api/include/nav_2d_msgs/SwitchPluginResponse.h

@@ -0,0 +1,21 @@
+#ifndef C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H
+#define C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  uint8_t success;
+  char *message;
+} SwitchPluginResponse;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_SWITCHPLUGINRESPONSE_H

src/APIs/c_api/include/nav_2d_msgs/Twist2D.h

@@ -0,0 +1,22 @@
+#ifndef C_API_NAV_2D_MSGS_TWIST2D_H
+#define C_API_NAV_2D_MSGS_TWIST2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  double x;
+  double y;
+  double theta;
+} Twist2D;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_TWIST2D_H

src/APIs/c_api/include/nav_2d_msgs/Twist2D32.h

@@ -0,0 +1,22 @@
+#ifndef C_API_NAV_2D_MSGS_TWIST2D32_H
+#define C_API_NAV_2D_MSGS_TWIST2D32_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+typedef struct
+{
+  float x;
+  float y;
+  float theta;
+} Twist2D32;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // C_API_NAV_2D_MSGS_TWIST2D32_H