HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
b41d614d1b
pnkx_nav_core/doc/architecture_discussion.md
HiepLM 45d965671e created
2025-12-05 11:12:17 +07:00

20 KiB
Raw Blame History

Thảo luận về Kiến trúc Navigation cho AMR

Mục tiêu dự án

Nghiên cứu và phát triển hệ thống navigation (di chuyển bám quỹ đạo) cho robot AMR dạng hai bánh vi sai, không sử dụng ROS, viết thuần C++ với CMake trên Linux.

Yêu cầu chức năng

Từ readme.md:

  • Có khả năng thay đổi mô hình kinematics (dạng hai bánh vi sai, dạng steering bicycle, ...)
  • Có chức năng di chuyển từ điểm khởi đầu đến điểm đích
  • Có chức năng xoay tròn
  • Có chức năng đi thẳng
  • Có chức năng thay đổi vận tốc

Sơ đồ kiến trúc hệ thống

Kiến trúc được thiết kế theo mô hình layered, tách biệt các concerns và cho phép mở rộng dễ dàng:

flowchart TB
  %% ========== API LAYER ==========
  subgraph API["🌐 API Layer"]
    direction TB
    CAPI["<b>📦 C API</b><br/>━━━━━━━━━━━━━━━━<br/>🔌 nav_c_api<br/>💻 P/Invoke cho .NET/C#<br/>📝 Wrapper Functions<br/>🔗 Interop Layer"]
    style API fill:#E3F2FD,stroke:#1976D2,stroke-width:4px,color:#000
    style CAPI fill:#BBDEFB,stroke:#1976D2,stroke-width:3px,font-size:14px
  end

  %% ========== USER CONTROLLER LAYER ==========
  subgraph UserController["🎮 User Controller Layer"]
    direction TB
    UserCtrl["<b>🔌 User Controller Plugin</b><br/>━━━━━━━━━━━━━━━━<br/>📚 Dynamic Loader<br/>⚙️ boost::dll<br/>🎯 Custom Behavior<br/>🔄 Runtime Loading"]
    style UserController fill:#F3E5F5,stroke:#7B1FA2,stroke-width:4px,color:#000
    style UserCtrl fill:#E1BEE7,stroke:#7B1FA2,stroke-width:3px,font-size:14px
  end

  %% ========== INTERFACE LAYER ==========
  subgraph Interface["🔌 Interface Layer"]
    direction TB
    BaseNav["<b>📋 BaseNavigation</b><br/>━━━━━━━━━━━━━━━━<br/>🏗️ move_base_core::BaseNavigation<br/>🎯 Abstract Interface<br/>━━━━━━━━━━━━━━━━<br/>📍 moveTo, dockTo<br/>🔄 rotateTo, moveStraightTo<br/>⏸️ pause, resume, cancel<br/>📊 getRobotPose"]
    style Interface fill:#FFF3E0,stroke:#E65100,stroke-width:4px,color:#000
    style BaseNav fill:#FFE0B2,stroke:#E65100,stroke-width:3px,font-size:14px
  end

  %% ========== MOVE BASE CORE ==========
  subgraph MoveBaseCore["⚙️ Move Base Core"]
    direction TB
    MoveBase["<b>🚀 MoveBase</b><br/>━━━━━━━━━━━━━━━━<br/>📦 move_base::MoveBase<br/>🔄 State Machine<br/>━━━━━━━━━━━━━━━━<br/>📊 PLANNING<br/>🎮 CONTROLLING<br/>🧹 CLEARING<br/>━━━━━━━━━━━━━━━━<br/>🎛️ Control Loop<br/>🔄 executeCycle"]
    style MoveBaseCore fill:#E8F5E9,stroke:#2E7D32,stroke-width:4px,color:#000
    style MoveBase fill:#C8E6C9,stroke:#2E7D32,stroke-width:3px,font-size:14px
  end

  %% ========== PLANNING LAYER ==========
  subgraph Planning["🗺️ Planning Layer - Plugin System"]
    direction LR
    GP["<b>🌍 Global Planner</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::BaseGlobalPlanner<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🎯 A*, D*, Hybrid A*<br/>📐 Long-range Planning<br/>🗺️ Global Path"]
    LP["<b>📍 Local Planner</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::BaseLocalPlanner<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🎯 DWA, TEB, MKT<br/>🚗 Velocity Commands<br/>🛡️ Obstacle Avoidance"]
    RB["<b>🔄 Recovery Behaviors</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_core::RecoveryBehavior<br/>🔌 Dynamic Plugin<br/>━━━━━━━━━━━━━━━━<br/>🧹 Clear Costmap<br/>🔄 Rotate Recovery<br/>🚨 Emergency Stop"]
    style Planning fill:#E1F5FE,stroke:#0277BD,stroke-width:4px,color:#000
    style GP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
    style LP fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
    style RB fill:#B3E5FC,stroke:#0277BD,stroke-width:3px,font-size:13px
  end

  %% ========== 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"]
    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 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
  end

  %% ========== ALGORITHMS LAYER ==========
  subgraph Algorithms["🧮 Algorithms Layer"]
    direction LR
    MKTAlgo["<b>🚗 MKT Algorithm</b><br/>━━━━━━━━━━━━━━━━<br/>⚙️ Diff Drive Kinematics<br/>🚲 Bicycle Kinematics<br/>📐 Trajectory Generation"]
    ScoreAlgo["<b>📊 Score Algorithm</b><br/>━━━━━━━━━━━━━━━━<br/>📈 Trajectory Scoring<br/>✅ Goal Checking<br/>🎯 Path Evaluation"]
    KalmanAlgo["<b>🔍 Kalman Filter</b><br/>━━━━━━━━━━━━━━━━<br/>📊 State Estimation<br/>🔮 Sensor Fusion<br/>📉 Noise Filtering"]
    style Algorithms fill:#FCE4EC,stroke:#C2185B,stroke-width:4px,color:#000
    style MKTAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
    style ScoreAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
    style KalmanAlgo fill:#F8BBD0,stroke:#C2185B,stroke-width:3px,font-size:13px
  end

  %% ========== DATA SOURCES ==========
  subgraph DataSources["📡 Data Sources"]
    direction TB
    Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 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"]
    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 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"]
    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
    style TF fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Odom fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Laser fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
    style Map fill:#FFF59D,stroke:#F57F17,stroke-width:3px,font-size:12px
  end

  %% ========== 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"]
    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
    style BaseCtrl fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
  end

  %% ========== CONNECTIONS ==========
  %% API to User Controller
  CAPI -->|"🔗 P/Invoke"| UserCtrl

  %% User Controller to Interface
  UserCtrl -->|"🎯 Uses"| BaseNav

  %% Interface to MoveBase
  BaseNav -->|"⚙️ Implements"| MoveBase

  %% MoveBase to Planning
  MoveBase -->|"🎛️ Manages"| GP
  MoveBase -->|"🎛️ Manages"| LP
  MoveBase -->|"🎛️ Manages"| RB

  %% MoveBase to Costmap
  MoveBase -->|"🎛️ Manages"| GC
  MoveBase -->|"🎛️ Manages"| LC

  %% Data Sources to Components
  Goal -->|"📥 Input"| UserCtrl
  Goal -->|"📥 Input"| BaseNav
  Loc -->|"📍 Pose"| MoveBase
  TF -->|"🔄 Transforms"| MoveBase
  Laser -->|"📡 Scan Data"| LC
  Map -->|"🗺️ Static Map"| GC
  Odom -->|"⚡ Velocity"| LP
  Odom -->|"📍 Pose"| MoveBase

  %% Planning Flow
  GC -->|"🗺️ Costmap"| GP
  GP -->|"🛤️ Global Path"| LP
  LC -->|"🗺️ Costmap"| LP
  LC -->|"🗺️ Costmap"| RB
  GC -->|"🗺️ Costmap"| RB

  %% Algorithm Integration
  MKTAlgo -->|"🚗 Kinematics"| LP
  ScoreAlgo -->|"📊 Scoring"| LP
  KalmanAlgo -.->|"🔍 Filtering"| Loc

  %% Control Flow
  LP -->|"⚡ Velocity Cmd"| CmdVel
  CmdVel -->|"▶️ Execute"| BaseCtrl
  BaseCtrl -->|"📊 Feedback"| Odom

  %% Styling Classes
  classDef implemented fill:#C8E6C9,stroke:#2E7D32,stroke-width:4px,color:#000
  classDef partial fill:#FFF9C4,stroke:#F57F17,stroke-width:3px,color:#000
  classDef todo fill:#FFCDD2,stroke:#C62828,stroke-width:3px,color:#000

  class BaseNav,MoveBase,GP,LP,RB,GC,LC,CAPI,MKTAlgo,ScoreAlgo,KalmanAlgo,UserCtrl implemented
  class BaseCtrl,Loc,Odom,Map,Laser,TF partial

Đề xuất giải pháp kiến trúc

1. Xác định phạm vi & yêu cầu vận hành

Cần làm rõ:

  • Kịch bản sử dụng chính: go-to-goal trong môi trường indoor/outdoor? Tốc độ di chuyển? Độ chính xác yêu cầu?
  • Cảm biến có sẵn: encoder, IMU, lidar, camera? Dữ liệu nào đã có?
  • Yêu cầu real-time: chu kỳ điều khiển (control loop frequency), độ trễ tối đa cho phép?

2. Kiến trúc tổng thể

Các layer chính:

  1. API Layer ✅

    • C API (nav_c_api) cho P/Invoke với .NET/C#
    • Wrapper functions cho tất cả BaseNavigation methods

  2. User Controller Layer ✅

    • User Controller Plugin: Dynamically allocated từ loader controller plugin
    • Sử dụng boost::dll để load plugins động
    • Cho phép user định nghĩa controller riêng để điều khiển navigation behavior
    • Kết nối với BaseNavigation interface

  3. Interface Layer ✅

    • move_base_core::BaseNavigation: Abstract interface cho navigation
    • Định nghĩa các operations: moveTo, dockTo, rotateTo, moveStraightTo, pause, resume, cancel
    • Là lớp trung gian định nghĩa các phương thức và thuộc tính liên quan đến navigation

  4. Move Base Core ✅

    • move_base::MoveBase: Core implementation của BaseNavigation
    • Quản lý state machine (PLANNING, CONTROLLING, CLEARING)
    • Điều phối global/local planner và recovery behaviors
    • Quản lý costmaps (global và local)
    • Thực thi control loop (executeCycle)

  5. Planning Layer ✅

    • nav_core::BaseGlobalPlanner: Interface cho global planners (A*, D*, etc.)
    • nav_core::BaseLocalPlanner: Interface cho local planners (DWA, TEB, MKT, etc.)
    • nav_core::RecoveryBehavior: Interface cho recovery behaviors
    • Plugin system sử dụng boost::dll để dynamic loading

  6. Costmap Layer ✅

    • costmap_2d::Costmap2DROBOT: Global và local costmap
    • Costmap layers: static map, obstacles, inflation
    • Frame management: map (global), odom (local)

  7. Algorithms Layer ✅

    • mkt_algorithm: Diff drive và bicycle kinematics algorithms
    • score_algorithm: Trajectory scoring và goal checking
    • kalman: Filtering algorithms

  8. Data Sources ⚠️ (Interface cần định nghĩa)

    • Localization source (Pnkx Loc)
    • Odometry source
    • Sensor transforms (tf3)
    • Map server
    • Laser sensors

  9. Control Layer ⚠️ (Cần implementation)

    • Base Controller interface (Diff/Steer drive)
    • Velocity command execution

Định dạng dữ liệu:

  • geometry_msgs::Pose2D / geometry_msgs::PoseStamped (vị trí + hướng)
  • geometry_msgs::Twist (vận tốc linear/angular)
  • std::vector<geometry_msgs::PoseStamped> (đường đi)
  • costmap_2d::Costmap2D (bản đồ chi phí)

3. Thiết kế từng module (interface level)

Các interface đã có: ✅

  • move_base_core::BaseNavigation ✅

    • initialize(TFListenerPtr) - Khởi tạo với TF listener
    • moveTo(goal, xy_tol, yaw_tol) - Di chuyển đến goal
    • dockTo(marker, goal, ...) - Docking đến marker
    • rotateTo(goal, yaw_tol) - Xoay tại chỗ
    • moveStraightTo(goal, xy_tol) - Đi thẳng
    • pause(), resume(), cancel() - Điều khiển trạng thái
    • getRobotPose(...) - Lấy vị trí robot

  • nav_core::BaseGlobalPlanner ✅

    • makePlan(start, goal, plan) - Tạo global path
    • initialize(name, costmap_robot) - Khởi tạo với costmap

  • nav_core::BaseLocalPlanner ✅

    • computeVelocityCommands(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
    • setTwistLinear/Angular(...) - Set velocity limits

  • nav_core::RecoveryBehavior ✅

    • runBehavior() - Thực thi recovery behavior

  • costmap_2d::Costmap2DROBOT ✅

    • Wrapper cho costmap với robot footprint
    • Thread-safe access với mutex

Các interface cần bổ sung: ⚠️

  • ILocalizationSource ⚠️

    • getCurrentPose() - Lấy vị trí hiện tại từ localization
    • isAvailable() - Kiểm tra localization có sẵn không

  • IOdometrySource ⚠️

    • getCurrentPose() - Lấy vị trí từ odometry
    • getCurrentVelocity() - Lấy vận tốc hiện tại
    • isAvailable() - Kiểm tra odometry có sẵn không

  • IBaseController ⚠️

    • executeVelocity(cmd_vel) - Thực thi velocity command
    • stop() - Dừng robot ngay lập tức
    • getCurrentVelocity() - Lấy vận tốc thực tế

  • IMapProvider ⚠️

    • getMap() - Lấy static map
    • isMapAvailable() - Kiểm tra map có sẵn không

Plugin mechanism: ✅

  • Sử dụng boost::dll để dynamic loading plugins
  • Factory pattern với boost::function và boost::dll::import
  • Config file YAML để specify plugin names
  • Plugin interfaces: BaseGlobalPlanner, BaseLocalPlanner, RecoveryBehavior

4. Cơ chế giao tiếp & đồng bộ

Lựa chọn transport:

  • Shared memory + mutex (cho real-time nhẹ)
  • Message queue (ZeroMQ, nanomsg)
  • Event loop với callback thread-safe

Time synchronization:

  • Sử dụng std::chrono cho timestamp
  • Buffer dữ liệu để xử lý dữ liệu không đồng bộ

Threading strategy:

  • Mỗi module một thread riêng, hoặc
  • Scheduler chung quản lý tất cả

5. Chiến lược an toàn & recovery

Monitoring:

  • Heartbeat mechanism
  • Watchdog timer

Recovery behaviors:

  • Không tìm được đường đi
  • Mất localization
  • Obstacle chặn đường
  • Emergency stop mechanism
  • Giới hạn vận tốc theo trạng thái

6. Config & Logging/Diagnostics

Configuration:

  • File YAML/JSON cho:
    • Kinematics parameters
    • Velocity limits
    • Planner parameters
    • Sensor configurations

Logging & Debugging:

  • Logging framework (spdlog?)
  • Telemetry interface
  • Visualizer tool (SDL/ImGui) để debug map/path

7. Trạng thái triển khai & lộ trình

Đã hoàn thành: ✅

  1. ✅ Interface Layer: BaseNavigation interface
  2. ✅ Implementation Layer: MoveBase core logic
  3. ✅ Planning Layer: Plugin system cho global/local planners và recovery
  4. ✅ Costmap Layer: Global và local costmap với layers
  5. ✅ Algorithms Layer: MKT algorithms, score algorithm, kalman
  6. ✅ API Layer: C API wrapper cho .NET integration
  7. ✅ Supporting Libraries: tf3, robot_time, geometry_msgs, nav_2d_utils

Đang triển khai / Cần bổ sung: ⚠️

  1. ⚠️ Data Sources Interfaces:

    • ILocalizationSource interface
    • IOdometrySource interface
    • IMapProvider interface
    • Integration với Pnkx Loc, odometry sources

  2. ⚠️ Base Controller:

    • IBaseController interface
    • Diff drive controller implementation
    • Steer drive controller implementation
    • Velocity command execution

  3. ⚠️ Control Loop:

    • Control loop trong MoveBase (executeCycle)
    • State machine management hoàn chỉnh
    • Threading và synchronization

  4. ⚠️ User Controller Plugin System:

    • Factory để load user controller plugins
    • Integration với BaseNavigation

Lộ trình tiếp theo:

Phase 1: Data Sources & Base Controller (Ưu tiên cao)

  • Định nghĩa interfaces cho data sources
  • Implement Base Controller interface và diff drive controller
  • Integration với MoveBase

Phase 2: Control Loop & State Management (Ưu tiên cao)

  • Hoàn thiện executeCycle trong MoveBase
  • State machine management
  • Threading strategy

Phase 3: User Controller Plugin System (Ưu tiên trung bình)

  • Factory pattern cho user controllers
  • Plugin loading mechanism
  • Integration testing

Phase 4: Testing & Optimization (Ưu tiên trung bình)

  • Unit tests cho các module
  • Integration tests
  • Performance optimization

Testing strategy:

  • Unit tests cho các module độc lập (gtest?)
  • Integration tests cho full navigation stack
  • Simulation environment (2D simulator) - TODO
  • Hardware-in-the-loop testing - TODO

Cấu trúc thư mục

pnkx_nav_core/
├── src/
│   ├── Navigations/
│   │   ├── Cores/
│   │   │   ├── move_base_core/     # BaseNavigation interface
│   │   │   ├── nav_core/            # Planner interfaces
│   │   │   ├── nav_core_adapter/    # Adapter utilities
│   │   │   └── nav_core2/           # Additional nav utilities
│   │   ├── Libraries/
│   │   │   ├── costmap_2d/          # Costmap system
│   │   │   ├── tf3/                 # Transform system
│   │   │   ├── robot_time/          # Time management
│   │   │   ├── geometry2/           # Geometry utilities
│   │   │   └── ...                  # Other supporting libraries
│   │   └── Packages/
│   │       └── move_base/           # MoveBase implementation
│   ├── Algorithms/
│   │   ├── Cores/
│   │   │   └── score_algorithm/     # Trajectory scoring
│   │   └── Libraries/
│   │       ├── mkt_algorithm/      # MKT kinematics algorithms
│   │       ├── kalman/              # Kalman filtering
│   │       └── angles/               # Angle utilities
│   └── APIs/
│       └── c_api/                   # C API wrapper
├── build/                            # Build artifacts
└── doc/                              # Documentation

Ghi chú

  • ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
  • ⚠️ 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
  • 🔌 Plugin system sử dụng boost::dll cho dynamic loading