optimal and add file readme
This commit is contained in:
312
README.md
312
README.md
@@ -0,0 +1,312 @@
|
||||
# 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.
|
||||
|
||||
Reference in New Issue
Block a user