pnkx_nav_core/implementation_plan.md at 63b3effbb96fe8fa3e44125ef68390115b2bf8ff

HiepLM/pnkx_nav_core

Fork 0

HiepLM 45d965671e created

2025-12-05 11:12:17 +07:00

7.2 KiB

Raw Blame History

Phân tích Kiến trúc Hiện tại

✅ Đã có sẵn:

Interface Layer:
- move_base_core::BaseNavigation - Interface chính
- move_base::MoveBase - Implementation
Core Components:
- nav_core::BaseGlobalPlanner - Interface cho global planner
- nav_core::BaseLocalPlanner - Interface cho local planner
- nav_core::RecoveryBehavior - Interface cho recovery behaviors
- Plugin system với boost::dll để load dynamic plugins
Costmap System:
- costmap_2d::Costmap2DROBOT - Global và local costmap
- Costmap layers (static, obstacle, inflation, etc.)
Supporting Libraries:
- tf3 - Transform system
- robot_time - Time management
- geometry_msgs - Message types
- nav_2d_msgs, nav_2d_utils - 2D navigation utilities

❌ Cần bổ sung/hoàn thiện:

User Controller Plugin System:
- Factory để tạo User Controller từ plugin
- Interface cho User Controller
- Integration với BaseNavigation
Data Sources Integration:
- Pnkx Loc interface
- Odometry Source interface
- Sensor Transform management
- Map Server interface
- Laser Sensor Sources interface
Base Controller:
- Diff drive controller
- Steer drive controller
- Velocity command execution
Hoàn thiện MoveBase:
- Control loop implementation
- State machine management
- Recovery behavior execution

Kế hoạch Implementation

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

1.1 Odometry Source Interface

File: src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h

class IOdometrySource {
  virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
  virtual bool isAvailable() = 0;
};

1.2 Localization Source Interface (Pnkx Loc)

File: src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h

class ILocalizationSource {
  virtual geometry_msgs::PoseStamped getRobotPose() = 0;
  virtual std::string getGlobalFrame() = 0;
  virtual bool isLocalized() = 0;
};

1.3 Map Provider Interface

File: src/Navigations/Cores/move_base_core/include/move_base_core/map_provider.h

class IMapProvider {
  virtual nav_msgs::OccupancyGrid getMap() = 0;
  virtual bool isMapAvailable() = 0;
};

1.4 Sensor Data Interface

File: src/Navigations/Cores/move_base_core/include/move_base_core/sensor_source.h

class ISensorSource {
  virtual sensor_msgs::LaserScan getLatestScan() = 0;
  virtual bool hasNewData() = 0;
};

Phase 2: Base Controller (Ưu tiên cao)

2.1 Base Controller Interface

File: src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h

class IBaseController {
  virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
  virtual bool stop() = 0;
  virtual geometry_msgs::Twist getCurrentVelocity() = 0;
};

2.2 Diff Drive Controller

File: src/Navigations/Packages/base_controllers/include/base_controllers/diff_drive_controller.h

Implement IBaseController
Convert Twist to wheel velocities
Handle kinematics cho differential drive

2.3 Steer Drive Controller

File: src/Navigations/Packages/base_controllers/include/base_controllers/steer_drive_controller.h

Implement IBaseController
Convert Twist to steering angles và wheel velocities
Handle kinematics cho bicycle/steering model

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

3.1 User Controller Interface

File: src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h

class IUserController {
  virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
  virtual void onNavigationStateChanged(move_base_core::State state) = 0;
  virtual void onFeedback(const NavFeedback& feedback) = 0;
};

3.2 User Controller Factory

File: src/Navigations/Cores/move_base_core/src/user_controller_factory.cpp

Load plugin từ config
Create instance với boost::dll
Manage lifecycle

Phase 4: Control Loop & State Machine (Ưu tiên cao)

4.1 Control Loop Implementation

File: src/Navigations/Packages/move_base/src/move_base_control_loop.cpp

Main control loop thread
Planner thread management
Controller execution
Frequency control

4.2 State Machine

File: src/Navigations/Packages/move_base/src/move_base_state_machine.cpp

State transitions
Recovery behavior triggers
Goal management

Phase 5: Integration & Testing (Ưu tiên trung bình)

5.1 Integration Layer

Connect tất cả components
Data flow management
Error handling

5.2 Testing

Unit tests
Integration tests
Simulation tests

Cấu trúc Thư mục Đề xuất

src/Navigations/
├── Cores/
│   ├── move_base_core/          ✅ Đã có
│   │   ├── interfaces/
│   │   │   ├── odometry_source.h      ⚠️ Cần tạo
│   │   │   ├── localization_source.h ⚠️ Cần tạo
│   │   │   ├── map_provider.h         ⚠️ Cần tạo
│   │   │   ├── sensor_source.h        ⚠️ Cần tạo
│   │   │   ├── base_controller.h      ⚠️ Cần tạo
│   │   │   └── user_controller.h      ⚠️ Cần tạo
│   │   └── src/
│   │       └── user_controller_factory.cpp ⚠️ Cần tạo
│   ├── nav_core/                ✅ Đã có
│   └── nav_core2/               ✅ Đã có
│
├── Packages/
│   ├── move_base/               ✅ Đã có (cần hoàn thiện)
│   │   └── src/
│   │       ├── move_base_control_loop.cpp  ⚠️ Cần tạo
│   │       └── move_base_state_machine.cpp ⚠️ Cần tạo
│   │
│   └── base_controllers/        ⚠️ Cần tạo
│       ├── include/
│       │   └── base_controllers/
│       │       ├── diff_drive_controller.h
│       │       └── steer_drive_controller.h
│       └── src/
│           ├── diff_drive_controller.cpp
│           └── steer_drive_controller.cpp
│
└── Libraries/
    └── (các libraries hiện có) ✅

Thứ tự Implementation

Sprint 1: Foundation (1-2 tuần)

✅ Tạo interfaces cho data sources
✅ Tạo base controller interface
✅ Implement diff drive controller cơ bản

Sprint 2: Integration (1-2 tuần)

✅ Integrate data sources vào MoveBase
✅ Implement control loop
✅ Connect base controller

Sprint 3: Plugin System (1 tuần)

✅ User controller plugin system
✅ Factory pattern implementation

Sprint 4: State Machine & Recovery (1 tuần)

✅ State machine implementation
✅ Recovery behavior integration

Sprint 5: Testing & Polish (1 tuần)

✅ Unit tests
✅ Integration tests
✅ Documentation

Notes

Tất cả interfaces nên follow pattern của BaseNavigation
Sử dụng boost::dll cho plugin system (đã có sẵn)
Thread safety: sử dụng boost::mutex và boost::recursive_mutex
Error handling: throw exceptions, return bool cho simple operations

7.2 KiB Raw Blame History

Kế hoạch Implementation Navigation System