# Dock Planner

Global planner tao duong di cho robot tiep can tram dock (tram sac, tram hang) bang duong cong NURBS.

## Cau truc thu muc

```
dock_planner/
├── include/dock_planner/
│   ├── dock_planner.h          # Lop DockPlanner - plugin interface
│   ├── dock_calc.h             # Lop DockCalc - logic tinh toan duong di
│   └── utils/
│       ├── curve_common.h      # Thu vien NURBS curve
│       ├── pose.h              # Chuyen doi pose/quaternion
│       └── common.h            # Tien ich chung
├── src/
│   ├── dock_planner.cpp        # Khoi tao, load param, goi DockCalc
│   ├── dock_calc.cpp           # Thuat toan chinh: NURBS path generation
│   └── utils/
│       ├── curve_common.cpp
│       ├── line_common.cpp
│       └── pose.cpp
└── config/
    └── dock_global_params.yaml # Tham so cau hinh
```

## Pipeline tong quan

```mermaid
flowchart TB
    subgraph INIT["1. KHOI TAO - DockPlanner::initialize"]
        I1["Lien ket costmap_robot"] --> I2["Load ROS params:<br/>cost_threshold, calib_safety_distance"]
        I2 --> I3["Load footprint_dock<br/>tu param server"]
    end

    subgraph PLAN["2. LAP KE HOACH - DockPlanner::makePlan"]
        P1["Lay footprint_robot<br/>tu costmap"] --> P2["Goi DockCalc::makePlanMoveToDock"]
        P2 --> P3["Chuyen vector Pose<br/>thanh vector PoseStamped"]
    end

    subgraph CALC["3. TINH TOAN - DockCalc::makePlanMoveToDock"]
        C1["Tinh RobotGeometry + safety_distance"]
        C2["Xac dinh huong tiep can<br/>forward / backward"]
        C3["Tinh angle_threshold<br/>heuristic"]
        C4["Tao duong tam NURBS<br/>findPathTMP"]
        C5["Tim control points<br/>segment 1"]
        C6["Sinh NURBS segment 1"]
        C7["Sinh NURBS segment 2"]
        C8["Ghep path: start + seg1 + seg2 + goal"]
        C1 --> C2 --> C3 --> C4 --> C5 --> C6 --> C7 --> C8
    end

    INIT --> PLAN --> CALC
```

## So do giai thuat chi tiet

### 1. Khoi tao (initialize)

```mermaid
flowchart LR
    A["DockPlanner::initialize(name, costmap_robot)"] --> B{Da initialized?}
    B -- Co --> Z["return true"]
    B -- Chua --> C["Lien ket costmap_robot_<br/>costmap_<br/>frame_id_"]
    C --> D["getParam:<br/>check_free_space<br/>cost_threshold<br/>calib_safety_distance"]
    D --> E["Copy params<br/>sang DockCalc"]
    E --> F["footprintFromParams<br/>→ footprint_dock_"]
    F --> G["initialized_ = true"]
    G --> Z
```

### 2. Lap ke hoach (makePlan)

```mermaid
flowchart TD
    A["makePlan(start, goal, plan)"] --> B{initialized_?}
    B -- false --> FAIL["return false"]
    B -- true --> C["footprint_robot =<br/>costmap_robot_->getRobotFootprint()"]
    C --> D["makePlanMoveToDock<br/>(start, goal, footprint_robot,<br/>footprint_dock_, degree=2, step=0.02)"]
    D --> E{Thanh cong?}
    E -- false --> FAIL
    E -- true --> F["Chuyen moi Pose thanh PoseStamped<br/>(gan timestamp, frame_id)"]
    F --> G["return true"]
```

### 3. Thuat toan chinh (makePlanMoveToDock)

```mermaid
flowchart TD
    START["makePlanMoveToDock(start, goal)"] --> GEOM

    subgraph GEOM["BUOC 1: Tinh hinh hoc"]
        G1["computeGeometry(footprint_robot)<br/>→ robot.length, robot.radius"]
        G2["computeGeometry(footprint_dock)<br/>→ dock.length"]
        G3["safety_distance =<br/>(robot.radius + calib) * 2"]
        G1 --> G3
        G2 --> G3
    end

    GEOM --> VALID{robot.length > 0<br/>AND safety > 0?}
    VALID -- false --> ERR["return false"]
    VALID -- true --> POSE_C

    subgraph POSE_C["BUOC 2: Tinh diem trung gian C"]
        PC1["distance_min_C =<br/>(robot.length + dock.length) / 2"]
        PC2["yaw_goal = normalizeAngle(goal.yaw)"]
        PC3["Pose_C = offsetPoint(goal, yaw_goal, distance_min_C)"]
        PC1 --> PC3
        PC2 --> PC3
    end

    POSE_C --> DIR

    subgraph DIR["BUOC 3: Xac dinh huong di"]
        D1["t = compute_t(start, goal, Pose_C)"]
        D2{t < 0 ?}
        D3["Lat yaw_goal 180 do<br/>Tinh lai Pose_C va t<br/>dir = backward"]
        D4["dir = forward"]
        D1 --> D2
        D2 -- Co --> D3
        D2 -- Khong --> D4
    end

    DIR --> T_CHECK{t > 1 ?}
    T_CHECK -- false --> ERR
    T_CHECK -- true --> ANGLE

    subgraph ANGLE["BUOC 4: Tinh angle threshold"]
        A1["Pose_D = compute_D(goal, Pose_C, t)"]
        A2["distance_AD =<br/>pointToSegmentDistance(start, goal, Pose_C)"]
        A3["scale = f(distance_AD)<br/>angle_threshold =<br/>min((alpha+|scale|)*beta*pi, gamma*pi)"]
        A1 --> A3
        A2 --> A3
    end

    ANGLE --> TMP_PATH

    subgraph TMP_PATH["BUOC 5: Tao duong tam"]
        T1["Tinh Pose_F = offsetPoint(goal, yaw, dist_F)"]
        T2["mid = trung diem(C, F)"]
        T3["findPathTMP({C, mid, F})<br/>→ saved_poses_tmp"]
        T1 --> T2 --> T3
    end

    TMP_PATH --> TMP_OK{Path tmp ok?}
    TMP_OK -- false --> ERR
    TMP_OK -- true --> SEG1

    subgraph SEG1["BUOC 6: Sinh segment 1"]
        S1["findNURBSControlPoints<br/>(start, saved_poses_tmp, angle_threshold)<br/>→ control_points_1, dir"]
        S2["Xac dinh chieu reverse<br/>cua segment 1"]
        S3["generateNURBSPath(control_points_1)<br/>→ first_segment"]
        S1 --> S2 --> S3
    end

    SEG1 --> SEG1_OK{first_segment<br/>khong rong?}
    SEG1_OK -- false --> ERR
    SEG1_OK -- true --> SEG2

    subgraph SEG2["BUOC 7: Sinh segment 2"]
        S4["mid_ctrl = trung diem<br/>(first_segment.back, goal)"]
        S5["generateNURBSPath<br/>({last_of_seg1, mid_ctrl, goal})<br/>→ second_segment"]
        S4 --> S5
    end

    SEG2 --> ASSEMBLE["BUOC 8: Ghep duong di<br/>start + seg1 + seg2 + goal"]
    ASSEMBLE --> OK["return true"]
```

### 4. Sinh duong NURBS (generateNURBSPath)

```mermaid
flowchart TD
    A["generateNURBSPath<br/>(control_points, check_free,<br/>edge_end, reverse)"] --> B["setupSpline(control_points, degree)<br/>Tao knot vector + weights<br/>Nap vao Spline_Inf"]
    B --> C["Tinh chieu dai NURBS<br/>calculateNURBSLength"]
    C --> D["step_new = resolution / length<br/>(adaptive step)"]
    D --> E["Duyet t = 0 → 1<br/>(buoc step_new)"]

    E --> F["point = CalculateCurvePoint(t)"]
    F --> G{point hop le?<br/>khong NaN?}
    G -- false --> E
    G -- true --> H{Kiem tra<br/>isFreeSpace?}
    H -- Bi chan --> I["obstacle_flag = true<br/>BREAK"]
    H -- Thong --> J["Luu pose<br/>Cap nhat orientation"]
    J --> E

    I --> K["result.clear()"]
    E -- t > 1.0 --> L["Gan orientation<br/>cho diem cuoi"]
    L --> M["return result"]
    K --> M
```

### 5. Tim control points (findNURBSControlPoints)

```mermaid
flowchart TD
    A["findNURBSControlPoints<br/>(start, saved_poses, angle_threshold)"] --> B["findNearestPointOnPath<br/>→ idx, dir"]
    B --> C{idx != -1 ?}

    C -- idx hop le --> D["control_points = {start, saved[idx]}"]
    D --> E{dir == 1?}
    E -- Co --> F["Tim diem lui ve<br/>sao cho dist >= target<br/>→ end_idx"]
    F --> G["Them saved[end_idx]"]
    E -- Khong, dir == -1 --> H["Them saved.back()"]

    C -- idx == -1 --> I["mid = trung diem<br/>(start, nearest)"]
    I --> J["control_points =<br/>{start, mid, nearest}"]

    G --> K["return true"]
    H --> K
    J --> K
```

### 6. Tim diem gan nhat tren duong (findNearestPointOnPath)

```mermaid
flowchart TD
    A["findNearestPointOnPath<br/>(pose, saved_poses, angle_threshold)"] --> B["Tim nearest_idx<br/>(khoang cach nho nhat)"]
    B --> C["Duyet nguoc tu nearest_idx → 1"]
    C --> D{signedAngle >= threshold?}
    D -- Co --> E["nearest_idx = i<br/>found_flag_1 = true"]
    D -- Khong --> C

    E --> F{Kiem tra khoang cach<br/>distance_2 >= distance_1?}
    F -- Co --> G["dir = 1<br/>return nearest_idx"]
    F -- Khong --> H["Duyet xuoi tu nearest_idx → cuoi"]
    C -- Het vong --> H

    H --> I{signedAngle >= threshold - 0.05?}
    I -- Co --> J["dir = -1<br/>return nearest_idx"]
    I -- Khong, i == cuoi --> K["dir = 0<br/>return -1"]

    J --> L{dist hop ly?}
    L -- false --> K
    L -- true --> J
```

## Mo ta cac diem hinh hoc

```
    F ──────── mid ──────── C ──────── Goal
    |          (duong tam)          |     |
    |                               |     |
    |     Pose_D                   |     |
    |       ╲                      |     |
    |        ╲  distance_AD       |     |
    |         ╲                    |     |
    Start      ╲                   |     |
                                   |     |
          ◄── distance_min_F ────►|     |
                         ◄── dist_C ──►|
```

- **Goal**: Vi tri dock dich
- **Pose_C**: Diem offset tu Goal theo huong yaw_goal, cach `(robot.length + dock.length) / 2`
- **Pose_F**: Diem xa hon tren cung huong, cach Goal = `dist_C + dist_CD + dist_DF`
- **Pose_D**: Hinh chieu vuong goc tu Start len duong Goal→C keo dai
- **distance_AD**: Khoang cach vuong goc tu Start den duong Goal→C
- **Duong tam (C → mid → F)**: Duong NURBS tam de tim diem noi segment 1

## Luong du lieu giua cac lop

```mermaid
flowchart LR
    subgraph Plugin["DockPlanner (Plugin Interface)"]
        init["initialize()"]
        make["makePlan()"]
    end

    subgraph Core["DockCalc (Core Algorithm)"]
        calc["makePlanMoveToDock()"]
        nurbs["generateNURBSPath()"]
        tmp["findPathTMP()"]
        ctrl["findNURBSControlPoints()"]
        near["findNearestPointOnPath()"]
        setup["setupSpline()"]
    end

    subgraph Lib["Utils"]
        curve["CurveCommon<br/>(NURBS engine)"]
        spline["Spline_Inf<br/>(knot/ctrl/weight)"]
    end

    init -->|"config params"| calc
    make -->|"start, goal,<br/>footprints"| calc
    calc --> tmp
    calc --> ctrl
    calc --> nurbs
    ctrl --> near
    tmp --> setup
    nurbs --> setup
    setup --> curve
    setup --> spline
    curve -->|"CalculateCurvePoint"| nurbs
    curve -->|"CalculateCurvePoint"| tmp
```

## Tham so cau hinh

| Tham so | Kieu | Mac dinh | Mo ta |
|---------|------|----------|-------|
| `cost_threshold` | int | 200 | Nguong chi phi vat can (0-255) |
| `calib_safety_distance` | double | 0.0 | Khoang cach an toan bo sung (met) |
| `check_free_space` | bool | false | Bat/tat kiem tra vung tu do |
| `degree` (hardcode) | int | 2 | Bac cua duong cong NURBS |
| `step` (hardcode) | double | 0.02 | Buoc lay mau tham so t |

## Ghi chu

- **NURBS (Non-Uniform Rational B-Spline)**: Duong cong bac 2 voi 3 control points, knot vector dang clamped `[0,0,0,1,1,1]`.
- **Adaptive step**: Buoc lay mau duoc tinh tu `resolution / curve_length` de dam bao mat do diem dong deu theo do phan giai costmap.
- **2-segment path**: Duong di gom 2 doan NURBS noi tiep — segment 1 dua robot tu vi tri hien tai vao duong tiep can, segment 2 dua robot tu diem noi den dock.