# `nav_c_api_2` — hướng dẫn cho khung C#

Tài liệu cho phía host .NET/C# khi chuyển nền navigation từ `libmove_base.so` sang
`libmove_base2.so`.

Bám sát mã nguồn tại thời điểm viết:
`pnkx_nav_core/src/APIs/c_api`, `Test/move_base2`, `pnkx_nav_core/src/Navigations/Packages/move_base`
(bản cũ, dùng để đối chiếu).

---

## 0. Đọc nhanh — 8 việc phải làm ở phía C#

| # | Việc | Vì sao |
|---|---|---|
| 1 | Gọi `navigation_add_static_map` **trước** `navigation_initialize` | Planner lattice chặn tới khi costmap có kích thước khác 0, và tự `exit(1)` khi chờ quá lâu — giết cả tiến trình |
| 2 | Gọi `navigation_set_robot_footprint` **trước** `navigation_initialize` | Bản cũ gọi trước là rơi mất; bản mới cache lại và áp đúng lúc dựng costmap |
| 3 | Thay `navigation_initialize` bằng `navigation_initialize_checked` | Hàm cũ luôn trả `true`, kể cả khi lõi dựng hỏng |
| 4 | Thêm `navigation_shutdown` vào đường tắt, **trước** `navigation_destroy` | Bản mới có control thread + thread costmap + thread mission đang chạy |
| 5 | Bỏ mọi suy đoán từ giá trị trả về của `navigation_set_twist_linear/_angular` | Giờ luôn trả `true` nếu số hợp lệ; tín hiệu "đang cancel" của bản cũ không còn |
| 6 | Bỏ mọi so sánh chuỗi `feed_back_str`, chuyển sang so enum | Nội dung chuỗi do lõi mới đặt, khác hoàn toàn bản cũ |
| 7 | Không coi `navigation_pause` trả về là robot đã dừng | Lệnh chỉ được ghi nhận, có hiệu lực ở cycle sau (≤ 33 ms @ 30 Hz) |
| 8 | Gọi các hàm `navigation_free_*` mới sau mỗi getter cấp bộ nhớ | Trước đây không có hàm giải phóng nào cho grid/scan/cloud/plan — mỗi lời gọi rò một lần |

Ba việc **nên** làm thêm: dùng `navigation_get_state` thay cho `navigation_get_feedback` ở vòng poll
(không cấp phát chuỗi); dùng `navigation_add_depth_camera_data` để bật clear theo frustum; dùng
`navigation_move_to_order_v2` nếu fleet master có gửi order update.

---

## 1. Bố cục — và các hàm cũ nằm ở đâu

**Một thư viện `.so` duy nhất.** `nav_c_api_2` không phải một thư viện khác, không phải một handle
khác, không phải một tầng bọc. Nó là thêm file nguồn vào **cùng** thư viện đó.

```
libnav_c_api.so
├── nav_c_api.cpp     → 50 hàm cũ   (không sửa, không dịch lại, không bọc)
├── convertor.cpp     → order_free
├── nav_c_api_2.cpp   → 13 hàm mới
└── convertor_2.cpp   → converter cho depth camera
```

64 symbol, cùng một `.so`, không hàm nào trùng tên hàm nào.

**Vậy các hàm không đổi thì C# gọi thế nào? Y hệt hôm nay, không sửa một dòng.** Cùng
`DllImport("nav_c_api")`, cùng tên symbol, cùng chữ ký, cùng struct. C# không dùng file header —
P/Invoke nối theo **tên symbol** trong `.so`, mà tên đó không đổi. Lớp binding hiện có tiếp tục chạy
nguyên trạng.

Cái C# phải thêm chỉ là **13 `DllImport` mới + 4 struct mới** (phụ lục B). Cái C# phải *sửa* là hành
vi, không phải khai báo — đúng 8 việc ở mục 0.

Hai header chỉ dành cho bên gọi C/C++:

| File | Nội dung |
|---|---|
| `include/nav_c_api.h` | Bề mặt cũ. Không đổi chữ ký hàm nào. |
| `include/nav_c_api_2.h` | Phần thêm. `#include` luôn `nav_c_api.h`, nên bên gọi C/C++ chỉ cần include file này. |

Tên thư viện sinh ra là `libnav_c_api.so` (target `nav_c_api`), nên phía C# là
`[DllImport("nav_c_api")]`. Tài liệu cũ có chỗ ghi `libnavigation_c_api.so` — kiểm tra lại chuỗi
khung hiện tại đang dùng.

Mọi hàm dùng `CallingConvention.Cdecl`. `bool` của C++ là 1 byte, phải khai
`[return: MarshalAs(UnmanagedType.I1)]`. `size_t` trên Linux x64 là 8 byte — dùng `UIntPtr`.

---

## 2. Điều kiện vận hành trước khi chạy

| Việc | Giá trị |
|---|---|
| `PNKX_NAV_CORE_CONFIG_DIR` | Trỏ vào `Test/move_base2/config/runtime` — **không** phải `pnkx_nav_core/config` |
| YAML chọn plugin | `MoveBase: library_path: libmove_base2` (đã có sẵn trong `move_base_common_params.yaml`) |
| Đường tìm `.so` | `libmove_base2.so` và toàn bộ plugin planner/recovery/costmap phải nằm trong `PNKX_NAV_CORE_LIBRARY_PATH`, `devel/lib` hoặc `LD_LIBRARY_PATH` |
| Nhịp control loop | `controller_frequency: 30.0` — mọi độ trễ "một cycle" nói trong tài liệu này là ~33 ms |

`navigation_create()` tra symbol `"MoveBase"`. `move_base2` export cả `MoveBase` lẫn `MoveBase2`, nên
đổi runtime chỉ tốn một dòng YAML, không phải sửa C#.

---

## 3. Trình tự khởi tạo

Thứ tự này **bắt buộc**, không phải khuyến nghị. Sai thứ tự ở bước 3 làm chết cả tiến trình.

```
1. tf3_buffer_create()                    → có buffer TF
2. navigation_create()                    → nạp libmove_base2.so, chưa chạy gì
3. bơm TF liên tục vào buffer             → bắt đầu NGAY, chạy suốt vòng đời
4. navigation_set_robot_footprint()       → trước initialize
5. navigation_add_static_map("/map", ...) → BẮT BUỘC trước initialize
6. navigation_initialize_checked()        → nặng, chặn, tự start control thread
7. kiểm tra kết quả trả về                → false thì đọc navigation_get_status_text()
8. bắt đầu vòng bơm sensor định kỳ
9. từ đây mới gửi goal
```

```csharp
// ---- 1..2 -------------------------------------------------------------------------------
tfHandle = tf3_buffer_create();
navHandle = navigation_create();
if (navHandle == IntPtr.Zero)
    throw new InvalidOperationException("navigation_create failed — kiểm tra library_path của MoveBase");

// ---- 3 ----------------------------------------------------------------------------------
// TF phải chảy TRƯỚC initialize: costmap lấy pose robot ngay trong chu kỳ cập nhật đầu tiên.
StartTfPump();

// ---- 4 ----------------------------------------------------------------------------------
// Footprint theo chiều ngược kim đồng hồ, toạ độ trong frame base_link, đơn vị mét.
var footprint = new[]
{
    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 },
};
navigation_set_robot_footprint(navHandle, footprint, (UIntPtr)footprint.Length);

// ---- 5 ----------------------------------------------------------------------------------
// Tên "/map" phải khớp map_topic trong costmap_common_params.yaml.
navigation_add_static_map(navHandle, "/map", occupancyGrid);

// ---- 6..7 -------------------------------------------------------------------------------
// Chặn hàng giây: dựng 2 costmap, nạp planner/controller/recovery, dựng mission layer,
// start control thread. Không gọi từ UI thread.
if (!navigation_initialize_checked(navHandle, tfHandle))
{
    IntPtr reason = navigation_get_status_text(navHandle);
    string text = Marshal.PtrToStringAnsi(reason) ?? "unknown";
    nav_c_api_free_string(reason);
    throw new InvalidOperationException($"navigation core not ready: {text}");
}

// ---- 8..9 -------------------------------------------------------------------------------
StartSensorPumps();
```

**Vì sao static map phải đi trước.** `initialize()` nạp planner ở pha hai.
`SBPLLatticePlanner::initialize` chặn tới khi costmap báo kích thước khác 0, mà kích thước đó chỉ có
khi một static map đã tới `StaticLayer`. Nạp planner trước khi map vào được là khoá chết: planner chờ
map, map chờ planner xong. SBPL `exit(1)` sau 2 giây. `move_base2` xử lý bằng cách cất static map đã
nhận rồi **phát lại** ngay trước khi nạp planner — nhưng nó chỉ phát lại được cái host đã gửi.

**Về `map_save_` / `map_name_save_`.** Hai biến public đó là cách bản cũ bù cho đúng vấn đề trên. Với
`move_base2` host **không cần** đụng tới chúng.

---

## 4. Bơm dữ liệu: cái gì, tên gì, tần số nào

Tên truyền vào các hàm `navigation_add_*` phải khớp **giá trị `topic`** khai trong
`Test/move_base2/config/runtime/costmap_common_params.yaml`, không phải khoá của observation source.
Sai tên thì dữ liệu được nhận rồi bị bỏ ở tầng layer, im lặng.

| Dữ liệu | Hàm | Tên (`topic` trong YAML) | Tần số | Bắt buộc |
|---|---|---|---|---|
| Bản đồ tĩnh | `navigation_add_static_map` | `/map` | Khi bản đồ đổi | **Có** — và trước `initialize` |
| Laser sau | `navigation_add_laser_scan` | `/b_scan` | Theo nguồn (~10–40 Hz) | Có, nếu dùng laser |
| Point cloud depth | `navigation_add_point_cloud2` | `/camera/depth/points_proc` | Theo camera | Tuỳ cấu hình |
| Point cloud depth phải | `navigation_add_point_cloud2` | `/camera_right/depth/points_proc` | Theo camera | Tuỳ cấu hình |
| Depth camera (clear frustum) | `navigation_add_depth_camera_data` | `/camera/depth/data` | Theo camera (~10–15 Hz) | Cần nếu muốn clear ghost |
| Depth camera phải | `navigation_add_depth_camera_data` | `/camera_right/depth/data` | Theo camera | Cần nếu muốn clear ghost |
| Odometry | `navigation_add_odometry` | tên tự do | Theo nguồn odometry | **Có** |
| TF | qua `tf3` buffer, không qua API này | — | Liên tục | **Có** |

Ghi chú quan trọng:

- **Odometry giờ là dữ liệu điều khiển, không phải dữ liệu hiển thị.** `move_base2` lấy vận tốc đo
  được từ đó và đẩy xuống controller mỗi cycle. Bơm thưa hoặc bỏ qua là controller mất feedback vận
  tốc.
- **Nguồn `/camera/depth/data` khai `data_type: DepthCameraData` và `frustum_clearing_enabled: true`.**
  Chỉ `navigation_add_depth_camera_data` đẩy vào được nguồn này. Point cloud không thay thế được —
  không có intrinsics thì không dựng được frustum, không clear được.
- **Không bơm nhanh hơn nguồn thật.** Đường depth tốn CPU nhất trong các đường sensor.
- **Toàn bộ đường sensor không thread-safe.** Xem mục 8.

---

## 5. Trình tự tắt

```csharp
public void Dispose()
{
    // 1. Dừng MỌI thread C# đang gọi navigation_add_* và đợi chúng thoát hẳn.
    sensorCts.Cancel();
    Task.WaitAll(sensorTasks, TimeSpan.FromSeconds(2));

    // 2. Quiesce lõi khi tiến trình còn sống đầy đủ. Trả về trong khoảng 1 giây.
    //    Sau bước này lõi không phát lệnh vận tốc nữa, nhưng getter vẫn gọi được.
    navigation_shutdown(navHandle);

    // 3. Thả instance.
    navigation_destroy(navHandle);
    navHandle = IntPtr.Zero;

    // 4. Buffer TF sau cùng — lõi còn tham chiếu nó cho tới hết bước 3.
    tf3_buffer_destroy(tfHandle);
    tfHandle = IntPtr.Zero;
}
```

Không được đảo bước 1 và 2, và không được để `navigation_destroy` chạy trong finalizer/GC: thời điểm
đó do runtime .NET quyết định, có thể rơi vào lúc nó đã tháo dỡ một phần trong khi control thread vẫn
đang chạy.

---

## 6. Tham chiếu từng hàm

Ký hiệu cột "Đổi so với bản cũ": **=** giữ nguyên · **~** cùng chữ ký, khác hành vi · **+** hàm mới.

### 6.1. Vòng đời

| | Hàm | Đổi |
|---|---|---|
| | `NavigationHandle navigation_create(void)` | = |

Nạp `libmove_base2.so` qua symbol `MoveBase` và tạo một instance. Trả `NULL` khi không tìm được
library (sai `library_path`, sai `PNKX_NAV_CORE_CONFIG_DIR`, hoặc `.so` không nằm trong đường tìm).
Chưa chạy thread nào.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_initialize(NavigationHandle, TFListenerHandle)` | ~ |

Dựng costmap, nạp planner/controller/recovery, dựng mission layer, start control thread. **Chặn**,
tốn hàng giây. **Luôn trả `true`** — contract bên dưới trả `void` nên không có gì để trả về. Dùng
`navigation_initialize_checked` thay thế.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_initialize_checked(NavigationHandle, TFListenerHandle)` | + |

Như trên, nhưng đọc lại cờ sẵn sàng của lõi và trả về đúng kết quả thật. Trả `false` khi bất kỳ pha
nào hỏng: dựng costmap, cấu hình sensor, nạp plugin, cấu hình control loop, start control thread.
Lý do cụ thể lấy bằng `navigation_get_status_text`.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_is_ready(NavigationHandle)` | + |

Lõi đã sẵn sàng nhận goal chưa. Rẻ, poll được. Không cấp phát gì.

| | Hàm | Đổi |
|---|---|---|
| | `void navigation_shutdown(NavigationHandle)` | + |

Dừng control thread, thread cập nhật costmap và thread mission — **không huỷ instance**. Sau lời gọi
này mọi getter vẫn an toàn. Idempotent. Không ném exception ra ngoài biên C.

| | Hàm | Đổi |
|---|---|---|
| | `void navigation_destroy(NavigationHandle)` | ~ |

Thả instance. Bản cũ gọi thẳng là chấp nhận được vì nó không override `shutdown()`. Bản mới **phải**
gọi `navigation_shutdown` trước.

### 6.2. Trạng thái và phản hồi

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_feedback(NavigationHandle, NavFeedback &out)` | ~ |

Trả trạng thái, chuỗi mô tả, pose 2D hiện tại, cờ `goal_checked`, cờ `is_ready`.

- `feed_back_str` được `strdup`, **phải** giải phóng bằng `nav_c_api_free_string`.
- **Nội dung `feed_back_str` khác bản cũ** — là chuỗi lý do của control loop mới. Chỉ dùng để log.
- `navigation_state` là nguồn duy nhất nên rẽ nhánh.
- Giữa hai chặng của một order nhiều node, lõi ở `SUCCEEDED`/`PENDING` nhưng API **cố ý báo `ACTIVE`**.
  Nhờ vậy suy "order xong" từ `SUCCEEDED` vẫn đúng — đừng tìm cách đi vòng qua lớp che này.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_state(NavigationHandle, NavigationState *out)` | + |

Chỉ lấy enum trạng thái, không cấp phát chuỗi. Dùng cho vòng poll. `navigation_get_feedback` gọi ở
10 Hz nghĩa là 10 lần `strdup` + 10 lần `free` mỗi giây, và một lần quên `free` là một lần rò.

| | Hàm | Đổi |
|---|---|---|
| | `char *navigation_get_status_text(NavigationHandle)` | + |

Chuỗi mô tả của lõi, hoặc `NULL`. Giải phóng bằng `nav_c_api_free_string`. Chỉ để log — nội dung
không phải hợp đồng.

| | Hàm | Đổi |
|---|---|---|
| | `char *navigation_state_to_string(NavigationState)` | = |

Tên trạng thái dạng chuỗi. Giải phóng bằng `nav_c_api_free_string`. (Hàm này có export nhưng chưa
được khai báo trong `nav_c_api.h` — khai báo thủ công phía C# nếu cần.)

### 6.3. Footprint

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_set_robot_footprint(NavigationHandle, const Point *pts, size_t n)` | ~ |

Đặt hình robot, toạ độ trong frame `base_link`, đơn vị mét, thứ tự ngược kim đồng hồ.

- **Bản cũ:** áp ngay vào hai costmap; gọi trước `initialize` thì **rơi mất im lặng**.
- **Bản mới:** ghi nhận rồi áp trong `initialize` và ở cycle sau nếu đổi. **Nên gọi trước
  `initialize`** để planner đầu tiên thấy đúng hình robot.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_robot_footprint(NavigationHandle, Point *out, size_t &out_count)` | ~ |

- **Bản cũ:** trả footprint của costmap — có giá trị (từ YAML) kể cả khi host chưa từng set.
- **Bản mới:** trả đúng cái host đã set. **Rỗng nếu host chưa gọi `set_robot_footprint`.**

Cảnh báo về chữ ký: mảng do bên gọi cấp mà **không có tham số dung lượng** — hàm không biết mảng có
đủ chỗ không. Cấp dư (ví dụ 64 điểm) khi gọi.

### 6.4. Lệnh di chuyển

Tất cả trả `true` khi lệnh được nhận, không phải khi robot tới nơi. Theo dõi kết quả bằng
`navigation_get_state`.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to(NavigationHandle, PoseStamped goal)` | ~ |

Đi tới một pose. Quaternion không hợp lệ bị từ chối ngay tại API. Trong `move_base2` goal này đi qua
mission layer như một mission một chặng, để dùng chung đường huỷ và vòng đời với order VDA5050; mission
layer tắt thì rơi xuống đường trực tiếp.

Dung sai `xy`/`yaw` đến từ profile trong YAML, không đặt được theo từng goal (bản cũ có tham số dung
sai trong contract nhưng `c_api` chưa bao giờ truyền).

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to_order(NavigationHandle, Order order, PoseStamped goal)` | ~ |

Gửi order VDA5050 đầy đủ. Trong `move_base2` order được **cắt thành từng chặng** tại các node có
action, lọc theo cờ `released`, và nối tiếp được khi fleet master release thêm horizon. `orderId` và
`orderUpdateId` là định danh order ở tầng mission — điền đúng.

Nếu `Order` lấy từ `convert2COrder()` thì gọi `order_free()` khi xong.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to_nodes_edges(NavigationHandle, const Node*, size_t, const Edge*, size_t, PoseStamped)` | ~ |

Tiện ích: gửi order chỉ từ mảng node/edge. **Không đặt được `orderId`/`orderUpdateId`** — cả hai để
rỗng/0, nên mission layer không phân biệt được order update với order mới. Dùng
`navigation_move_to_order_v2` nếu cần order update.

> Bản trước của hàm này dựng `Order` **không khởi tạo**, khiến bộ chuyển đổi đọc con trỏ rác trên
> stack. Đã sửa. Nếu khung C# đang chạy với `.so` cũ và thấy crash ngẫu nhiên hoặc chuỗi vô nghĩa ở
> đường order, đây là nguyên nhân.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_to_order_v2(NavigationHandle, const char *order_id, uint32_t order_update_id, const Node*, size_t, const Edge*, size_t, PoseStamped)` | + |

Như trên nhưng có định danh thật. Quy ước: cùng `order_id` + `order_update_id` tăng dần = cập nhật
tuyến đang chạy; đổi `order_id` = order mới thay thế hoàn toàn. `order_id` rỗng bị từ chối.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_dock_to(NavigationHandle, const char *marker, PoseStamped goal)` | = |
| | `bool navigation_dock_to_order(NavigationHandle, Order, const char *marker, PoseStamped)` | = |
| | `bool navigation_dock_to_nodes_edges(...)` | ~ (cùng vấn đề `Order`, đã sửa) |
| | `bool navigation_dock_to_order_v2(NavigationHandle, const char *marker, const char *order_id, uint32_t, const Node*, size_t, const Edge*, size_t, PoseStamped)` | + |

Docking dùng profile riêng (`docking_planner_name`) và tên marker. Docking **không** đi qua mission
layer — nó xuống thẳng lõi.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_move_straight_to(NavigationHandle, double distance)` | = |

Đi thẳng theo hướng hiện tại. Tham số là **khoảng cách [m]**, không phải pose: hàm tự đọc pose robot
rồi tính goal. Âm là lùi. Vì nó đọc pose ngay lúc gọi nên TF phải sẵn sàng.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_rotate_to(NavigationHandle, PoseStamped goal)` | = |

Quay tại chỗ tới hướng của `goal` (chỉ dùng phần orientation).

### 6.5. Vòng đời nhiệm vụ

| | Hàm | Đổi |
|---|---|---|
| | `void navigation_pause(NavigationHandle)` | ~ |
| | `void navigation_resume(NavigationHandle)` | ~ |
| | `void navigation_cancel(NavigationHandle)` | ~ |

- **Bản cũ:** đồng bộ, có hiệu lực ngay trong lời gọi; ném `std::runtime_error` nếu lõi chưa dựng
  xong (C API nuốt, C# thấy im lặng).
- **Bản mới:** chỉ ghi nhận yêu cầu; control thread áp ở cycle kế tiếp (≤ 33 ms @ 30 Hz). Không bao
  giờ ném.

Hệ quả: **trả về ≠ robot đã dừng.** Muốn biết đã dừng, chờ `navigation_get_state` báo `PAUSED`, hoặc
theo dõi `navigation_get_twist`. Nếu C# có tầng an toàn giả định dừng tức thì thì phải sửa ở phía C#.

`cancel` từ host huỷ **cả order** (gồm hàng đợi mission), khác với huỷ nội bộ do chính mission layer
phát ra chỉ dừng chặng đang chạy.

### 6.6. Trần vận tốc

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_set_twist_linear(NavigationHandle, double x, double y, double z)` | ~ |
| | `bool navigation_set_twist_angular(NavigationHandle, double x, double y, double z)` | ~ |

**Đây là trần vận tốc, không phải lệnh jog.** Đường này để tầng an toàn hạ tốc độ robot.

- **Dấu của `x` tuyến tính chọn chiều**: `x < 0` đặt trần lùi, còn lại đặt trần tiến. Hai trần được
  giữ riêng.
- Đơn vị: tuyến tính `[m/s]`, góc `[rad/s]`.
- `NaN`/`Inf` bị từ chối, trả `false`.
- Có hiệu lực ở cycle kế tiếp.
- **Bản cũ** đẩy thẳng xuống controller và trả `false` khi đang có cancel (kèm `unlock()` nội bộ).
  **Bản mới** chỉ kiểm tra số: giá trị hợp lệ luôn trả `true`. Tín hiệu "đang cancel" **không còn tồn
  tại** — C# nào dựa vào nó phải chuyển sang đọc trạng thái.

### 6.7. Truy vấn pose và vận tốc

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_robot_pose_stamped(NavigationHandle, PoseStamped &out)` | = |
| | `bool navigation_get_robot_pose_2d(NavigationHandle, Pose2D &out)` | = |

Pose robot trong global frame. Trả `false` khi TF chưa đủ. Bản `PoseStamped` cấp `frame_id` bằng
`strdup` — giải phóng bằng `nav_c_api_free_string`.

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_twist(NavigationHandle, Twist2DStamped &out)` | ~ |

**Lệnh vận tốc đang phát**, không phải vận tốc đo được. Đây là thứ host publish ra `cmd_vel`.

Bản mới có cửa ân hạn: khi không có yêu cầu nào chạy, **stamp đứng yên**; stamp bằng 0 nghĩa là lõi
chưa từng điều khiển. C# nên kiểm tra stamp trước khi publish, tránh đè lên teleop.

`out.header.frame_id` được `strdup` — giải phóng bằng `nav_c_api_free_string`.

### 6.8. Bơm dữ liệu sensor vào

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_add_static_map(NavigationHandle, const char *name, OccupancyGrid)` | ~ |
| | `bool navigation_add_laser_scan(NavigationHandle, const char *name, LaserScan)` | = |
| | `bool navigation_add_point_cloud(NavigationHandle, const char *name, PointCloud)` | = |
| | `bool navigation_add_point_cloud2(NavigationHandle, const char *name, PointCloud2)` | = |
| | `bool navigation_add_odometry(NavigationHandle, const char *name, Odometry)` | ~ |
| | `bool navigation_add_depth_camera_data(NavigationHandle, const char *topic, DepthCameraData)` | + |

Chung cho cả nhóm: dữ liệu được **copy** ngay trong lời gọi, buffer C# giải phóng được ngay sau khi
hàm trả về. `name` phải khớp `topic` trong YAML costmap.

- `navigation_add_static_map`: gọi được **trước** `initialize`, và bắt buộc phải thế (mục 3).
- `navigation_add_odometry`: giờ là nguồn vận tốc đo được của controller, không còn chỉ để hiển thị.
- `navigation_add_depth_camera_data` (mới): đường duy nhất tới `VoxelLayer` để **clear theo frustum**
  — xoá vật cản ma nằm trong tầm nhìn camera. Yêu cầu:
  - `header.frame_id` là **optical frame** của camera, và TF từ frame đó tới base frame phải có trong
    buffer tf3 tại đúng `header.stamp`;
  - `depth.encoding` đúng thật (`"16UC1"` hoặc `"32FC1"`), `step` là số byte mỗi hàng;
  - `camera_info` là intrinsics của **đúng độ phân giải đang gửi**, không phải của stream màu;
  - ảnh depth và camera info **cùng một thời điểm** — ghép lệch hai luồng là clear sai vùng.

### 6.9. Đọc lại và xoá dữ liệu sensor

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_static_map(NavigationHandle, const char *name, OccupancyGrid &out)` | = |
| | `bool navigation_get_laser_scan(NavigationHandle, const char *name, LaserScan &out)` | = |
| | `bool navigation_get_point_cloud(NavigationHandle, const char *name, PointCloud &out)` | = |
| | `bool navigation_get_point_cloud2(NavigationHandle, const char *name, PointCloud2 &out)` | = |
| | `bool navigation_get_all_static_maps(NavigationHandle, NamedOccupancyGrid *out, size_t &n)` | = |
| | `bool navigation_get_all_laser_scans(...)`, `..._point_clouds(...)`, `..._point_cloud2s(...)` | = |
| | `bool navigation_remove_static_map / _laser_scan / _point_cloud / _point_cloud2` | = |
| | `bool navigation_remove_all_static_maps / _laser_scans / _point_clouds / _point_cloud2s` | = |
| | `bool navigation_remove_all_data(NavigationHandle)` | = |

Các getter **cấp bộ nhớ** — dùng `navigation_free_*` (mục 6.11) sau khi xong.

Bản mới cất **bản đã lọc** của laser scan, đúng bản mà costmap nhìn thấy, nên getter và costmap không
còn lệch nguồn.

Cảnh báo về chữ ký `get_all_*`: mảng do bên gọi cấp, **không có tham số dung lượng**. Cấp dư.

### 6.10. Dữ liệu hiển thị

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_get_global_data(NavigationHandle, PlannerDataOutput *out)` | ~ |
| | `bool navigation_get_local_data(NavigationHandle, PlannerDataOutput *out)` | ~ |

Trả plan, lưới costmap (hoặc bản update), cờ `is_costmap_updated`, và footprint. Hàm **tự giải phóng
nội dung cũ** của struct được truyền vào, nên tái sử dụng một struct qua nhiều lần poll là đúng cách
và không rò. Lần điền cuối cùng thì giải phóng bằng `navigation_free_planner_data`.

Ba khác biệt so với bản cũ:

1. **`plan` của `get_local_data` giờ có dữ liệu.** Bản cũ không bao giờ điền trường này cho local.
2. **`costmap` vẫn được điền đầy đủ** qua bộ xuất costmap — không rỗng.
3. **`footprint` đổi ý nghĩa.** Bản cũ: polygon **đã pad và đã transform tới pose robot**, `frame_id`
   là global frame — vẽ thẳng lên bản đồ là đúng chỗ. Bản mới: polygon **thô** host đã set, chưa
   transform, `frame_id` là `base_link`, và **rỗng nếu host chưa set footprint**.

> Điểm 3 làm vỡ hiển thị footprint. Chỗ sửa đúng nằm ở `move_base2` (lấy footprint từ costmap thay vì
> từ bản thô) chứ không phải bắt C# tự transform. Xem `C_API_MOVE_BASE2_PLAN.md` mục 8.4. Cho tới khi
> sửa, C# muốn vẽ đúng thì phải tự xoay/tịnh tiến footprint theo pose lấy từ
> `navigation_get_robot_pose_2d` — chấp nhận lệch thời gian giữa hai lời gọi.

### 6.11. Bộ nhớ

| | Hàm | Đổi |
|---|---|---|
| | `void nav_c_api_free_string(char *)` | = |
| | `void order_free(Order *)` | = |
| | `void navigation_free_occupancy_grid(OccupancyGrid *)` | + |
| | `void navigation_free_laser_scan(LaserScan *)` | + |
| | `void navigation_free_point_cloud(PointCloud *)` | + |
| | `void navigation_free_point_cloud2(PointCloud2 *)` | + |
| | `void navigation_free_planner_data(PlannerDataOutput *)` | + |

Trước đây chỉ có hai hàm đầu, nên grid/scan/cloud/plan **không có hàm giải phóng nào** — mỗi lời gọi
getter rò một lần. Năm hàm mới đóng lỗ đó. Tất cả đều an toàn với `NULL`, xoá luôn con trỏ và bộ đếm,
gọi hai lần không sao.

Nguyên tắc: cấp phát ở trong thư viện thì **phải** giải phóng bằng hàm của thư viện. Đừng gọi `free()`
của libc từ C#.

### 6.12. Tiện ích hình học

| | Hàm | Đổi |
|---|---|---|
| | `bool navigation_offset_goal_2d(const Pose2D&, const char *frame_id, double d, PoseStamped &out)` | = |
| | `bool navigation_offset_goal_stamped(const PoseStamped&, double d, PoseStamped &out)` | = |

Dịch một pose đi `d` mét dọc theo hướng của chính nó (âm là lùi). Thuần tính toán, không đụng lõi.
(Hai hàm này có export nhưng chưa được khai báo trong `nav_c_api.h`.)

---

## 7. Quy tắc bộ nhớ — ai giải phóng cái gì

| Nguồn | Giải phóng bằng |
|---|---|
| `NavFeedback.feed_back_str` | `nav_c_api_free_string` |
| `navigation_get_status_text` | `nav_c_api_free_string` |
| `navigation_state_to_string` | `nav_c_api_free_string` |
| `PoseStamped.header.frame_id`, `Twist2DStamped.header.frame_id` từ getter | `nav_c_api_free_string` |
| `OccupancyGrid` từ `navigation_get_static_map` | `navigation_free_occupancy_grid` |
| `LaserScan` từ `navigation_get_laser_scan` | `navigation_free_laser_scan` |
| `PointCloud` / `PointCloud2` từ getter | `navigation_free_point_cloud` / `_point_cloud2` |
| `PlannerDataOutput` từ `get_global_data` / `get_local_data` | `navigation_free_planner_data` |
| `Order` từ `convert2COrder` | `order_free` |
| Mảng truyền **vào** các hàm `add_*` / `move_to_*` | C# tự giữ; thư viện đã copy xong khi hàm trả về |

---

## 8. Thread safety

| Nhóm | Gọi được từ thread nào |
|---|---|
| `navigation_add_*` (mọi đường sensor) | **Một thread duy nhất.** Đường sensor không thread-safe. Nếu C# có nhiều nguồn chạy song song, dồn qua một hàng đợi rồi bơm từ một thread. |
| `navigation_set_robot_footprint` | Thread bất kỳ (ghi dưới mutex, áp ở cycle sau) |
| `pause` / `resume` / `cancel` / `set_twist_*` | Thread bất kỳ (ghi dưới mutex, áp ở cycle sau) |
| `move_to*` / `dock_to*` / `rotate_to` / `move_straight_to` | Thread bất kỳ |
| `get_feedback` / `get_state` / `get_twist` / `get_robot_pose_*` | Thread bất kỳ |
| `get_global_data` / `get_local_data` | Thread bất kỳ; mỗi consumer nên có **struct riêng**, không dùng chung một `PlannerDataOutput` giữa nhiều timer |
| `navigation_initialize*` / `navigation_shutdown` / `navigation_destroy` | Thread khởi tạo/thread tắt, **không** song song với nhóm sensor |

---

## 9. Chẩn đoán nhanh

| Hiện tượng | Nguyên nhân thường gặp |
|---|---|
| `navigation_create` trả `NULL` | Sai `PNKX_NAV_CORE_CONFIG_DIR`, thiếu khoá `MoveBase: library_path`, hoặc `libmove_base2.so` không nằm trong đường tìm |
| Tiến trình chết ngay trong `initialize` | Chưa bơm static map trước `initialize` — SBPL chờ costmap rồi `exit(1)` |
| `initialize_checked` trả `false` | Đọc `navigation_get_status_text` — chuỗi nói rõ pha nào hỏng |
| Gửi goal xong không có gì xảy ra, không log | Control thread chưa chạy: `initialize` đã hỏng nhưng code cũ không kiểm tra kết quả |
| Robot không tránh vật cản từ camera | Sai tên `topic` khi bơm, hoặc thiếu TF của optical frame, hoặc chưa dùng `navigation_add_depth_camera_data` |
| Vật cản ma không bao giờ biến mất | Chưa bơm `DepthCameraData` — point cloud không clear được theo frustum |
| Footprint vẽ sai chỗ trên bản đồ | Mục 6.10 điểm 3 |
| Bộ nhớ tăng dần | Thiếu `navigation_free_*` ở đường getter, hoặc thiếu `nav_c_api_free_string` cho `feed_back_str` |
| Tắt chương trình bị treo hoặc crash | Thiếu `navigation_shutdown`, hoặc còn thread bơm sensor chạy khi tắt |
| Đổi tham số planner mãi không có tác dụng | Sửa nhầm cây config — `move_base2` đọc `Test/move_base2/config/runtime`, không đọc `pnkx_nav_core/config` |

---

## 10. Chưa xác nhận

- Chưa đọc được mã C# hiện tại, nên chưa biết chỗ nào đang thật sự dựa vào các hành vi cũ ở mục 6.5,
  6.6 và 6.10.
- Trường `footprint` trong dữ liệu hiển thị: đã đối chiếu cả hai bản và khác biệt là chắc chắn, nhưng
  chưa xác nhận phía C# có đang vẽ nó lên bản đồ hay không.
- Các hàm mới trong `nav_c_api_2` mới chỉ được kiểm tra biên dịch, **chưa chạy trên robot hay
  Gazebo**. Đường depth camera cần kiểm tra thực tế bằng bộ đếm chẩn đoán của cổng sensor.
- `navigation_move_straight_to` với khoảng cách âm (lùi) trong `move_base2` chưa được kiểm tra.

---

## Phụ lục A — bảng symbol đầy đủ trong `libnav_c_api.so`

Lấy bằng `nm --defined-only` trên các object file. Cột "Nguồn" cho biết symbol nằm trong file nguồn
nào — chỉ để tra cứu, C# không cần biết: mọi symbol đều nằm chung một `.so`.

### 50 symbol cũ — `nav_c_api.cpp`, C# **không đổi gì**

```
nav_c_api_free_string                navigation_get_point_cloud2
navigation_add_laser_scan            navigation_get_robot_footprint
navigation_add_odometry              navigation_get_robot_pose_2d
navigation_add_point_cloud           navigation_get_robot_pose_stamped
navigation_add_point_cloud2          navigation_get_static_map
navigation_add_static_map            navigation_get_twist
navigation_cancel                    navigation_initialize
navigation_create                    navigation_move_straight_to
navigation_destroy                   navigation_move_to
navigation_dock_to                   navigation_move_to_nodes_edges
navigation_dock_to_nodes_edges       navigation_move_to_order
navigation_dock_to_order             navigation_offset_goal_2d
navigation_get_all_laser_scans       navigation_offset_goal_stamped
navigation_get_all_point_cloud2s     navigation_pause
navigation_get_all_point_clouds      navigation_remove_all_data
navigation_get_all_static_maps       navigation_remove_all_laser_scans
navigation_get_feedback              navigation_remove_all_point_cloud2s
navigation_get_global_data           navigation_remove_all_point_clouds
navigation_get_laser_scan            navigation_remove_all_static_maps
navigation_get_local_data            navigation_remove_laser_scan
navigation_get_point_cloud           navigation_remove_point_cloud
navigation_resume                    navigation_remove_point_cloud2
navigation_rotate_to                 navigation_remove_static_map
navigation_set_robot_footprint       navigation_state_to_string
navigation_set_twist_angular         navigation_set_twist_linear
```

Cộng `order_free` từ `convertor.cpp`.

### 13 symbol mới — `nav_c_api_2.cpp`

```
navigation_initialize_checked        navigation_add_depth_camera_data
navigation_is_ready                  navigation_move_to_order_v2
navigation_shutdown                  navigation_dock_to_order_v2
navigation_get_state                 navigation_free_occupancy_grid
navigation_get_status_text           navigation_free_laser_scan
navigation_free_point_cloud          navigation_free_point_cloud2
navigation_free_planner_data
```

---

## Phụ lục B — phần C# phải thêm

Đây là **toàn bộ** phần khai báo mới. Mọi thứ khác trong lớp binding hiện tại giữ nguyên.

### B.1. Bốn struct mới

Ba struct đầu chưa từng xuất hiện trong bề mặt cũ vì đường depth camera chưa bao giờ được phơi ra.

```csharp
[StructLayout(LayoutKind.Sequential)]
public struct RegionOfInterest
{
    public uint x_offset;
    public uint y_offset;
    public uint height;
    public uint width;
    [MarshalAs(UnmanagedType.I1)] public bool do_rectify;
}

[StructLayout(LayoutKind.Sequential)]
public struct Image
{
    public Header header;
    public uint   height;
    public uint   width;
    public IntPtr encoding;      // char*  — "16UC1" hoặc "32FC1"
    public byte   is_bigendian;
    public uint   step;          // số byte mỗi hàng
    public IntPtr data;          // uint8_t*
    public UIntPtr data_count;
}

[StructLayout(LayoutKind.Sequential)]
public struct CameraInfo
{
    public Header  header;
    public uint    height;
    public uint    width;
    public IntPtr  distortion_model;   // char*
    public IntPtr  D;                  // double*
    public UIntPtr D_count;
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]  public double[] K;
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]  public double[] R;
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 12)] public double[] P;
    public uint binning_x;
    public uint binning_y;
    public RegionOfInterest roi;
}

[StructLayout(LayoutKind.Sequential)]
public struct DepthCameraData
{
    public Header     header;        // stamp + optical frame của mẫu
    public Image      depth;
    public CameraInfo camera_info;   // intrinsics của ĐÚNG ảnh trên
}
```

### B.2. Mười ba `DllImport` mới

```csharp
private const string Lib = "nav_c_api";
private const CallingConvention Cdecl = CallingConvention.Cdecl;

// ---- vòng đời ---------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_initialize_checked(IntPtr handle, IntPtr tf3Buffer);

[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_is_ready(IntPtr handle);

[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_shutdown(IntPtr handle);

// ---- trạng thái -------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_state(IntPtr handle, out NavigationState state);

[DllImport(Lib, CallingConvention = Cdecl)]
public static extern IntPtr navigation_get_status_text(IntPtr handle);   // free: nav_c_api_free_string

// ---- sensor -----------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_add_depth_camera_data(
    IntPtr handle, [MarshalAs(UnmanagedType.LPStr)] string topic, DepthCameraData data);

// ---- order ------------------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to_order_v2(
    IntPtr handle,
    [MarshalAs(UnmanagedType.LPStr)] string orderId, uint orderUpdateId,
    [In] Node[] nodes, UIntPtr nodeCount,
    [In] Edge[] edges, UIntPtr edgeCount,
    PoseStamped goal);

[DllImport(Lib, CallingConvention = Cdecl)] [return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_dock_to_order_v2(
    IntPtr handle, [MarshalAs(UnmanagedType.LPStr)] string marker,
    [MarshalAs(UnmanagedType.LPStr)] string orderId, uint orderUpdateId,
    [In] Node[] nodes, UIntPtr nodeCount,
    [In] Edge[] edges, UIntPtr edgeCount,
    PoseStamped goal);

// ---- giải phóng bộ nhớ ------------------------------------------------------------------
[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_occupancy_grid(ref OccupancyGrid grid);

[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_laser_scan(ref LaserScan scan);

[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_point_cloud(ref PointCloud cloud);

[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_point_cloud2(ref PointCloud2 cloud);

[DllImport(Lib, CallingConvention = Cdecl)]
public static extern void navigation_free_planner_data(ref PlannerDataOutput data);
```

### B.3. Ví dụ bơm một mẫu depth camera

Ảnh depth thường vài MB. Ghim mảng lại thay vì để bộ marshaller sao chép thêm một lần — thư viện đã
copy sang phía C++ ngay trong lời gọi, nên gỡ ghim ngay sau khi trả về là an toàn.

```csharp
public bool PushDepthSample(string topic, string opticalFrame,
                            uint sec, uint nsec,
                            byte[] depthBytes, uint height, uint width, uint step,
                            double[] k, double[] p)
{
    var frameId   = Marshal.StringToHGlobalAnsi(opticalFrame);
    var encoding  = Marshal.StringToHGlobalAnsi("16UC1");
    var distModel = Marshal.StringToHGlobalAnsi("plumb_bob");
    var pinned    = GCHandle.Alloc(depthBytes, GCHandleType.Pinned);
    try
    {
        var header = new Header { seq = 0, sec = sec, nsec = nsec, frame_id = frameId };

        var data = new DepthCameraData
        {
            header = header,
            depth = new Image
            {
                header       = header,
                height       = height,
                width        = width,
                encoding     = encoding,
                is_bigendian = 0,
                step         = step,
                data         = pinned.AddrOfPinnedObject(),
                data_count   = (UIntPtr)depthBytes.Length,
            },
            camera_info = new CameraInfo
            {
                header           = header,
                height           = height,
                width            = width,
                distortion_model = distModel,
                D                = IntPtr.Zero,
                D_count          = UIntPtr.Zero,
                K                = k,                 // 9 phần tử
                R                = new double[9],
                P                = p,                 // 12 phần tử
                binning_x        = 0,
                binning_y        = 0,
                roi              = default,
            },
        };

        return navigation_add_depth_camera_data(navHandle, topic, data);
    }
    finally
    {
        pinned.Free();
        Marshal.FreeHGlobal(distModel);
        Marshal.FreeHGlobal(encoding);
        Marshal.FreeHGlobal(frameId);
    }
}
```

### B.4. Ví dụ đọc dữ liệu hiển thị mà không rò bộ nhớ

```csharp
// MỘT struct cho MỖI consumer, giữ lại giữa các lần poll: hàm tự giải phóng nội dung cũ.
// Đừng dùng chung một struct giữa nhiều timer.
private PlannerDataOutput globalData;

private void OnGlobalTimer()
{
    if (!navigation_get_global_data(navHandle, ref globalData))
        return;

    DrawPlan(globalData.plan);
    DrawCostmap(globalData.costmap, globalData.is_costmap_updated, globalData.costmap_update);
}

// Khi dừng consumer — nếu không, lần điền cuối cùng sẽ rò.
private void StopGlobalTimer()
{
    timer.Stop();
    navigation_free_planner_data(ref globalData);
}
```
