313 lines
11 KiB
Markdown
313 lines
11 KiB
Markdown
# 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.
|