HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
fd022bd9d7
pnkx_nav_core/doc/architecture_discussion.md
HiepLM 145fb2088e update
2026-01-12 15:49:25 +07:00

32 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/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
    LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_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/>📍 robot_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/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
    Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 robot_sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
    Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 robot_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/>📤 robot_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 ✅

    • robot_nav_core::BaseGlobalPlanner: Interface cho global planners
    • Global Planner Implementations:
      • custom_planner - Custom path planner với curve support và merge path calculation
      • dock_planner - Docking-specific planner với dock calculation utilities
      • two_points_planner - Simple planner cho two-point navigation
    • robot_nav_core::BaseLocalPlanner: Interface cho local planners
    • Local Planner Implementation:
      • pnkx_local_planner - Main local planner với multiple behaviors:
        • Position planning (MKT algorithm)
        • Docking planning
        • Go straight planning
        • Rotate planning
    • robot_nav_core::RecoveryBehavior: Interface cho recovery behaviors
    • Plugin system sử dụng boost::dll và robot::PluginLoaderHelper để dynamic loading

  6. Costmap Layer ✅

    • robot_costmap_2d::Costmap2DROBOT: Global và local costmap wrapper
    • robot_costmap_2d::LayeredCostmap: Layered costmap system
    • Costmap Layers (8 implementations):
      • StaticLayer - Static map từ OccupancyGrid
      • ObstacleLayer - Dynamic obstacles từ sensors
      • InflationLayer - Cost inflation cho robot footprint
      • VoxelLayer - 3D voxel grid cho obstacles
      • CriticalLayer - Critical zones marking
      • DirectionalLayer - Directional cost preferences
      • PreferredLayer - Preferred path zones
      • UnpreferredLayer - Unpreferred zones
    • Frame management: map (global), odom (local)
    • Thread-safe access với mutex
    • Observation buffer cho sensor data

  7. Algorithms Layer ✅

    • MKT Algorithm (mkt_algorithm):
      • Differential drive kinematics:
        • diff_go_straight - Go straight behavior
        • diff_predictive_trajectory - Predictive trajectory following
        • diff_rotate_to_goal - Rotate to goal
      • Bicycle model kinematics:
        • bicycle_go_straight - Go straight với bicycle model
        • bicycle_pure_pursuit - Pure pursuit control
        • bicycle_rotate_to_goal - Rotate behavior
    • MKT Plugins (mkt_plugins):
      • goal_checker - Goal checking algorithms
      • simple_goal_checker - Simple goal checking
      • standard_traj_generator - Standard trajectory generation
      • limited_accel_generator - Acceleration-limited trajectory generation
      • kinematic_parameters - Kinematics parameter management
      • xy_theta_iterator - Trajectory iteration utilities
      • velocity_iterator - Velocity space iteration
      • one_d_velocity_iterator - 1D velocity iteration
    • Score Algorithm (score_algorithm):
      • Trajectory scoring và evaluation
      • Goal checking utilities
      • Trajectory generator interface
    • Kalman Filter (kalman):
      • State estimation
      • Sensor fusion
      • Noise filtering

  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:

  • robot_geometry_msgs::Pose2D / robot_geometry_msgs::PoseStamped (vị trí + hướng)
  • robot_geometry_msgs::Twist (vận tốc linear/angular)
  • std::vector<robot_geometry_msgs::PoseStamped> (đường đi)
  • robot_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

  • robot_nav_core::BaseGlobalPlanner ✅

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

  • robot_nav_core::BaseLocalPlanner ✅

    • computeVelocityCommands(odom, 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 (position, docking, go_straight, rotate)
    • setTwistLinear/Angular(...) - Set velocity limits
    • getTwistLinear/Angular(...) - Get velocity limits
    • Implementation: pnkx_local_planner với multiple behaviors

  • robot_nav_core::RecoveryBehavior ✅

    • runBehavior() - Thực thi recovery behavior
    • initialize(name, tf, global_costmap, local_costmap) - Khởi tạo

  • robot_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_alias
  • robot::PluginLoaderHelper để tìm library path từ plugin name
  • Config file YAML để specify plugin names
  • Plugin interfaces:
    • robot_nav_core::BaseGlobalPlanner - Global planners
    • robot_nav_core::BaseLocalPlanner - Local planners
    • robot_nav_core::RecoveryBehavior - Recovery behaviors
    • robot_costmap_2d::Layer - Costmap layers
  • Plugin registration với BOOST_DLL_ALIAS macro

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:

    • move_base_core::BaseNavigation - Abstract interface hoàn chỉnh
    • move_base_core::common.h - Common types và utilities

  2. ✅ Implementation Layer:

    • move_base::MoveBase - Core implementation với state machine
    • executeCycle() - Control loop implementation
    • State management (PLANNING, CONTROLLING, CLEARING, PAUSED)
    • Threading với planner thread và control loop

  3. ✅ Planning Layer - Plugin System:

    • Global Planners (3 implementations):
      • custom_planner - Custom path planning với curve support
      • dock_planner - Docking-specific planner
      • two_points_planner - Simple two-point planner
    • Local Planners (1 implementation):
      • pnkx_local_planner - Main local planner với:
        • pnkx_docking_local_planner - Docking behavior
        • pnkx_go_straight_local_planner - Go straight behavior
        • pnkx_rotate_local_planner - Rotate behavior
    • Recovery Behaviors: Interface đã định nghĩa
    • Plugin loading với boost::dll và robot::PluginLoaderHelper

  4. ✅ Costmap Layer:

    • robot_costmap_2d::Costmap2DROBOT - Global và local costmap
    • Costmap Layers (8 implementations):
      • static_layer - Static map layer
      • obstacle_layer - Dynamic obstacles
      • inflation_layer - Cost inflation
      • voxel_layer - 3D voxel grid
      • critical_layer - Critical zones
      • directional_layer - Directional costs
      • preferred_layer - Preferred paths
      • unpreferred_layer - Unpreferred zones
    • Layered costmap system với plugin support

  5. ✅ Algorithms Layer:

    • MKT Algorithm:
      • Differential drive kinematics (go_straight, rotate, predictive)
      • Bicycle model kinematics (go_straight, rotate, pure pursuit)
    • MKT Plugins:
      • goal_checker - Goal checking algorithms
      • standard_traj_generator - Standard trajectory generation
      • kinematic_parameters - Kinematics parameter management
      • xy_theta_iterator - Trajectory iteration
      • limited_accel_generator - Acceleration-limited trajectories
    • Score Algorithm: Trajectory scoring và goal checking
    • Kalman Filter: State estimation và filtering

  6. ✅ API Layer:

    • nav_c_api - C API wrapper cho .NET/C# P/Invoke
    • Wrapper functions cho BaseNavigation methods

  7. ✅ Supporting Libraries:

    • tf3 - Transform system (buffer_core, transforms)
    • robot_time - Time management (Time, Duration, Timer, Rate)
    • robot_cpp - Core utilities:
      • robot::init() - Initialization
      • robot::NodeHandle - YAML config loading
      • robot::PluginLoaderHelper - Plugin discovery
    • robot_nav_2d_utils - 2D navigation utilities (conversions, path_ops, polygons, bounds, tf_help)
    • robot_nav_2d_msgs - 2D navigation messages
    • geometry_msgs - Geometry message types (29 types)
    • robot_nav_msgs - Navigation messages (24 types)
    • robot_sensor_msgs - Sensor messages (30 types)
    • laser_geometry - Laser scan geometry utilities
    • voxel_grid - Voxel grid implementation
    • data_convert - Data conversion utilities
    • nav_grid - Navigation grid utilities

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

  1. ⚠️ Data Sources Interfaces:

    • ILocalizationSource interface - Cần định nghĩa
    • IOdometrySource interface - Cần định nghĩa
    • IMapProvider interface - Cần định nghĩa
    • Integration với Pnkx Loc, odometry sources - Hiện tại dùng trực tiếp qua MoveBase methods

  2. ⚠️ Base Controller:

    • IBaseController interface - Cần định nghĩa
    • Diff drive controller implementation - Cần implement
    • Steer drive controller implementation - Cần implement
    • Velocity command execution - Hiện tại chỉ có setTwistLinear/Angular trong local planner

  3. ⚠️ User Controller Plugin System:

    • Factory để load user controller plugins - Cần implement
    • Interface cho User Controller - Cần định nghĩa
    • Integration với BaseNavigation - Cần implement

  4. ⚠️ Testing & Documentation:

    • Unit tests cho các modules - Một số đã có (costmap, bounds, etc.)
    • Integration tests - Cần bổ sung
    • Simulation environment - TODO

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 ✅
│   │   │   │   ├── include/move_base_core/
│   │   │   │   │   ├── navigation.h         # BaseNavigation abstract class
│   │   │   │   │   └── common.h             # Common types (TFListenerPtr, etc.)
│   │   │   ├── robot_nav_core/              # Planner interfaces ✅
│   │   │   │   ├── include/robot_nav_core/
│   │   │   │   │   ├── base_global_planner.h
│   │   │   │   │   ├── base_local_planner.h
│   │   │   │   │   └── recovery_behavior.h
│   │   │   ├── robot_nav_core_adapter/      # Adapter utilities ✅
│   │   │   │   ├── include/robot_nav_core_adapter/
│   │   │   │   │   ├── global_planner_adapter.h
│   │   │   │   │   ├── local_planner_adapter.h
│   │   │   │   │   └── costmap_adapter.h
│   │   │   └── robot_nav_core2/             # Additional nav utilities ✅
│   │   │       ├── include/robot_nav_core2/
│   │   │       │   ├── global_planner.h
│   │   │       │   ├── local_planner.h
│   │   │       │   └── costmap.h
│   │   ├── Libraries/
│   │   │   ├── nav_grid/                    # Navigation grid utilities ✅
│   │   │   └── (costmap, tf3, robot_time ở Libraries/)
│   │   └── Packages/
│   │       └── move_base/                   # MoveBase implementation ✅
│   │           ├── include/move_base/
│   │           │   └── move_base.h
│   │           └── src/
│   │               ├── move_base.cpp
│   │               └── move_base_main.cpp
│   │
│   ├── Algorithms/
│   │   ├── Cores/
│   │   │   └── score_algorithm/             # Trajectory scoring ✅
│   │   │       ├── include/score_algorithm/
│   │   │       │   ├── score_algorithm.h
│   │   │       │   ├── goal_checker.h
│   │   │       │   └── trajectory_generator.h
│   │   ├── Libraries/
│   │   │   ├── mkt_algorithm/               # MKT kinematics algorithms ✅
│   │   │   │   ├── include/mkt_algorithm/
│   │   │   │   │   ├── diff/                # Differential drive
│   │   │   │   │   │   ├── diff_go_straight.h
│   │   │   │   │   │   ├── diff_predictive_trajectory.h
│   │   │   │   │   │   └── diff_rotate_to_goal.h
│   │   │   │   │   └── bicycle/             # Bicycle model
│   │   │   │   │       ├── bicycle.h
│   │   │   │   │       ├── go_straight.h
│   │   │   │   │       └── rotate_to_goal.h
│   │   │   ├── mkt_plugins/                 # MKT plugin components ✅
│   │   │   │   ├── include/mkt_plugins/
│   │   │   │   │   ├── goal_checker.h
│   │   │   │   │   ├── standard_traj_generator.h
│   │   │   │   │   ├── kinematic_parameters.h
│   │   │   │   │   └── xy_theta_iterator.h
│   │   │   ├── mkt_msgs/                    # MKT message types ✅
│   │   │   ├── kalman/                      # Kalman filtering ✅
│   │   │   └── angles/                      # Angle utilities ✅
│   │   └── Packages/
│   │       ├── global_planners/
│   │       │   ├── custom_planner/           # Custom global planner ✅
│   │       │   ├── dock_planner/            # Docking planner ✅
│   │       │   └── two_points_planner/      # Two points planner ✅
│   │       └── local_planners/
│   │           └── pnkx_local_planner/      # PNKX local planner ✅
│   │               ├── include/pnkx_local_planner/
│   │               │   ├── pnkx_local_planner.h
│   │               │   ├── pnkx_docking_local_planner.h
│   │               │   ├── pnkx_go_straight_local_planner.h
│   │               │   └── pnkx_rotate_local_planner.h
│   │
│   ├── Libraries/
│   │   ├── costmap_2d/                      # Costmap system ✅
│   │   │   ├── include/robot_costmap_2d/
│   │   │   │   ├── costmap_2d_robot.h
│   │   │   │   ├── layered_costmap.h
│   │   │   │   └── layer.h
│   │   │   ├── plugins/                     # Costmap layers ✅
│   │   │   │   ├── static_layer.cpp
│   │   │   │   ├── obstacle_layer.cpp
│   │   │   │   ├── inflation_layer.cpp
│   │   │   │   ├── voxel_layer.cpp
│   │   │   │   ├── critical_layer.cpp
│   │   │   │   ├── directional_layer.cpp
│   │   │   │   ├── preferred_layer.cpp
│   │   │   │   └── unpreferred_layer.cpp
│   │   │   └── config/                      # Costmap configs
│   │   ├── tf3/                             # Transform system ✅
│   │   ├── robot_time/                      # Time management ✅
│   │   ├── robot_cpp/                       # Core utilities ✅
│   │   │   ├── include/robot/
│   │   │   │   ├── init.h                   # Initialization
│   │   │   │   ├── node_handle.h            # NodeHandle (YAML config)
│   │   │   │   └── plugin_loader_helper.h   # Plugin loader
│   │   ├── robot_nav_2d_utils/              # 2D nav utilities ✅
│   │   ├── robot_nav_2d_msgs/               # 2D nav messages ✅
│   │   ├── geometry2/                       # Geometry utilities ✅
│   │   │   ├── robot_tf3_geometry_msgs/
│   │   │   └── robot_tf3_sensor_msgs/
│   │   ├── common_msgs/                     # Message types ✅
│   │   │   ├── robot_geometry_msgs/
│   │   │   ├── robot_nav_msgs/
│   │   │   ├── robot_sensor_msgs/
│   │   │   ├── robot_std_msgs/
│   │   │   ├── robot_protocol_msgs/
│   │   │   └── robot_visualization_msgs/
│   │   ├── laser_geometry/                  # Laser geometry ✅
│   │   ├── voxel_grid/                      # Voxel grid ✅
│   │   ├── data_convert/                    # Data conversion ✅
│   │   └── xmlrpcpp/                        # XML-RPC utilities ✅
│   │
│   └── APIs/
│       └── c_api/                           # C API wrapper ✅
│           ├── include/nav_c_api.h
│           └── src/nav_c_api.cpp
│
├── build/                                   # Build artifacts
├── config/                                  # Configuration files
├── examples/                                # Example code
└── doc/                                     # Documentation
    ├── architecture_discussion.md
    ├── implementation_plan.md
    └── folders.md

Ghi chú

  • ✅ Kiến trúc cốt lõi đã được triển khai với plugin system linh hoạt
  • ✅ 3 Global Planners đã triển khai: custom_planner, dock_planner, two_points_planner
  • ✅ 1 Local Planner đã triển khai: pnkx_local_planner với 4 behaviors (position, docking, go_straight, rotate)
  • ✅ 8 Costmap Layers đã triển khai: static, obstacle, inflation, voxel, critical, directional, preferred, unpreferred
  • ✅ MKT Algorithms hỗ trợ cả differential drive và bicycle model
  • ✅ Plugin System hoàn chỉnh với boost::dll và robot::PluginLoaderHelper
  • ⚠️ 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, hỗ trợ cả Catkin và Standalone CMake
  • 🔌 Plugin system sử dụng boost::dll cho dynamic loading
  • 🎯 MoveBase đã có control loop (executeCycle) và state machine hoàn chỉnh
  • 📚 Có đầy đủ message types: geometry_msgs (29), nav_msgs (24), sensor_msgs (30), std_msgs (32)