PPCS: PX4 Position Controller Switch

ROS2 workspace điều khiển vị trí drone PX4 ở chế độ offboard, với inner-loop controller có thể đổi động giữa PID / LQR / LQI qua một ROS parameter. Đi kèm waypoint sequencer, RViz2 visualizer, và Ground Control Station chạy trong trình duyệt. Giao tiếp với PX4 qua uXRCE-DDS (không dùng MAVLink/MAVROS).

Target: Ubuntu 22.04 · ROS2 Humble · PX4 SITL (gz_x500 Gazebo)

Screenshots

GCS (Ground Control Station)

Browser UI tại http://localhost:8000 — telemetry realtime, canvas 2D NED trajectory view, controls arm/takeoff/land.

Gazebo SITL

PX4 SITL với model gz_x500 trong Gazebo.

RViz2 — Trajectory Visualization

/trajectory/path hiển thị quỹ đạo thực tế (ENU) trong RViz2.

Benchmark — Tracking Error

Output của benchmark_tracking.py: position error và velocity error theo thời gian cho PID/LQR/LQI.

Evaluation Table

Architecture

                ┌────────────────────────────────── ┐
  Browser ◀─WS─▶│  px4_gcs  (FastAPI + ROS2 node)   │
  (vanilla JS)  │  ProcessManager spawns ↓          │
                └──┬──────────┬──────────┬──────────┘
                   │          │          │
              PX4 SITL  MicroXRCE    controller stack
              (gz_x500)  Agent UDP       │
                   ▲       :8888         │
                   └──uXRCE-DDS───────   ┘
                           │
          ┌────────────────┴─────────────────────── ─┐
          │     DDS topics  /fmu/in/*  /fmu/out/*    │
          └──┬────────────────┬──────────────────────┘
             │                │
   vehicle_local_position  vehicle_status / battery_status
             │                │
             ▼                ▼
   gen_trajectory ──/waypoint/current──▶ position_controller_node
         │                                      │
         ▼                              (PID | LQR | LQI)
   trajectory_visualizer                        │
         │                                      ▼
         ▼                         /fmu/in/trajectory_setpoint
       RViz2                       /fmu/in/offboard_control_mode
                                   /fmu/in/vehicle_command

ProcessManager trong px4_gcs tự spawn PX4 SITL, MicroXRCEAgent, controller và trajectory node — bạn không cần mở nhiều terminal.

Packages

Package	Ngôn ngữ	Vai trò
`px4_position_controller`	C++	Node chính, interface `IController`, 3 implementations (PID/LQR/LQI), GTest cho PID
`px4_gen_trajectory`	C++	Waypoint sequencer, chuyển điểm khi drone vào ngưỡng radius
`px4_gcs`	Python	FastAPI/WebSocket GCS + `ProcessManager` + trajectory generators (square/circle/triangle/helix/…)
`px4_trajectory_visualizer`	C++	Subscribe `VehicleLocalPosition`, publish `nav_msgs/Path` cho RViz2
`px4_test_bridge`	C++	Mirror `vehicle_status` / `battery_status` ra topic ROS thường
`px4_msgs`	—	Vendor — định nghĩa message PX4 (không sửa)

Prerequisites

Thứ	Phiên bản
Ubuntu	22.04 LTS
ROS2	Humble
PX4-Autopilot	clone & build một lần (xem bên dưới)
Micro XRCE-DDS Agent	build từ source (xem bên dưới)
Eigen3	`sudo apt install libeigen3-dev`

Quickstart

1. PX4 SITL (clone & build một lần)

git clone --recursive https://github.com/PX4/PX4-Autopilot.git ~/PX4-Autopilot
cd ~/PX4-Autopilot
make px4_sitl gz_x500        # smoke test, Ctrl+C khi thấy chạy

2. Micro XRCE-DDS Agent

git clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.git ~/Micro-XRCE-DDS-Agent
cd ~/Micro-XRCE-DDS-Agent && mkdir build && cd build
cmake .. && make && sudo make install && sudo ldconfig

3. Build workspace

cd ~/ros2_px4_ws
colcon build --symlink-install
source install/setup.bash

4. Chạy (qua GCS — khuyến nghị)

source install/setup.bash
ros2 launch px4_gcs gcs.launch.py
# Mở trình duyệt tại http://localhost:8000

GCS sẽ tự spawn PX4 SITL, MicroXRCEAgent, controller và trajectory node.

4b. Chạy (không qua GCS)

# Terminal 1 — PX4 SITL
cd ~/PX4-Autopilot && make px4_sitl gz_x500

# Terminal 2 — DDS bridge
MicroXRCEAgent udp4 -p 8888

# Terminal 3 — Controller stack
ros2 launch px4_position_controller sim.launch.py

Controllers

Tất cả implementations conform interface IController:

struct State    { Eigen::Vector3d pos, vel; };           // NED
struct Setpoint { Eigen::Vector3d pos, vel; };           // pos + FF velocity

class IController {
  virtual void init(rclcpp::Node*) = 0;
  virtual Eigen::Vector3d update(const State&, const Setpoint&, double dt) = 0;
  virtual void reset() = 0;
  virtual std::string name() const = 0;
};

Factory PositionControllerNode::makeController(type) chọn implementation từ ROS param controller_type:

Giá trị param	Controller	Ghi chú
`"PID"`	PIDController	Có GTest, tracking ổn định
`"LQR"`	LQRController	Default — state-dependent, relinearize 10 Hz (Foehn & Scaramuzza ICRA 2018)
`"LQI"`	LQIController	LQR + integral, giảm steady-state error

Đổi controller khi đang chạy (không cần rebuild):

ros2 param set /position_controller_node controller_type LQR

Gain matrix lưu tại src/px4_position_controller/config/config_K.yaml.

Topics

position_controller_node

Hướng	Topic	Type
Sub	`/fmu/out/vehicle_local_position`	`px4_msgs/VehicleLocalPosition`
Sub	`/waypoint/current`	`geometry_msgs/PointStamped`
Pub	`/fmu/in/trajectory_setpoint`	`px4_msgs/TrajectorySetpoint`
Pub	`/fmu/in/offboard_control_mode`	`px4_msgs/OffboardControlMode`
Pub	`/fmu/in/vehicle_command`	`px4_msgs/VehicleCommand`
Pub	`/debug_accel`	`geometry_msgs/Vector3Stamped`

gen_trajectory_node

Hướng	Topic	Type
Sub	`/fmu/out/vehicle_local_position`	`px4_msgs/VehicleLocalPosition`
Pub	`/waypoint/current`	`geometry_msgs/PointStamped` (frame: `map`, NED)

trajectory_visualizer_node

Hướng	Topic	Type
Sub	`/fmu/out/vehicle_local_position`	`px4_msgs/VehicleLocalPosition`
Pub	`/trajectory/path`	`nav_msgs/Path` (frame: `map`, ENU)

NED → ENU: x_enu = y_ned, y_enu = x_ned, z_enu = -z_ned

GCS

gcs_node.py chạy đồng thời:

FastAPI trên :8000, phục vụ static/ và WebSocket /ws
ROS2 subscribers cho vehicle_local_position, vehicle_status, battery_status, debug_accel → đẩy telemetry lên browser qua WS
ROS2 publishers gửi vehicle_command, waypoint list, controller params về stack
ProcessManager quản lý lifecycle (spawn / kill / stdout feed) cho PX4 SITL, MicroXRCEAgent, controller stack

Frontend không có build step — vanilla JS + canvas 2D NED view. Mở thẳng từ trình duyệt.

Trajectory shapes: square, circle, triangle, hexagon, octagon, figure8, helix, star, custom.

Benchmarking & Scripts

# Đọc rosbag từ flight_logs/, plot tracking error (pos + vel)
python3 src/px4_position_controller/scripts/benchmark_tracking.py

# Python prototype LQR ngoài ROS (SciPy CARE solver)
python3 src/px4_position_controller/scripts/prototype_lqr.py

Flight logs được record tự động khi chạy sim.launch.py vào flight_logs/.

RViz2

rviz2

Fixed Frame: map
Add → By topic → /trajectory/path → Path

Code Map

src/
├── px4_position_controller/
│   ├── include/px4_position_controller/   # IController, PID/LQR/LQI headers
│   ├── src/                               # position_controller_node + implementations
│   ├── config/config_K.yaml              # gain matrices
│   ├── scripts/                          # prototype_lqr.py, benchmark_tracking.py
│   ├── test/                             # GTest cho PIDController
│   └── lqr_spec.md                       # Implementation spec (Foehn & Scaramuzza)
├── px4_gen_trajectory/src/               # Waypoint sequencer
├── px4_gcs/
│   ├── px4_gcs/gcs_node.py              # FastAPI + ROS2 node
│   ├── px4_gcs/process_manager.py       # Subprocess lifecycle
│   ├── px4_gcs/trajectory_utils.py      # Shape generators
│   └── static/js/                        # app.js, charts.js, trajectory.js, controls.js
├── px4_trajectory_visualizer/src/        # RViz2 path publisher
├── px4_test_bridge/                      # px4_msgs → std ROS topics
└── px4_msgs/                             # Vendor (không sửa)
flight_logs/                              # Rosbag output
benchmark/                                # Benchmark scripts & results

Status

Tính năng	Trạng thái
PID tracking (square trajectory)	Hoạt động ổn định, GTest pass
GCS launch → fly → land end-to-end	Hoạt động
LQR controller	Code xong, đang tune gains, chưa có unit test
LQI controller	Code xong, đang tune
HITL / hardware thực	Chưa thử

Known limits:

Khi chạy (không GCS) trong sim.launch.py — controller đang đọc default gain cho PID từ code.
SITL only, chưa validate trên HITL/real hardware.
LQR/LQI chưa có unit test riêng (chỉ PID có GTest).

License

MIT — xem package.xml của từng package. File LICENSE ở root sẽ được thêm sau.

Name		Name	Last commit message	Last commit date
Latest commit History 19 Commits
benchmark		benchmark
docs/images		docs/images
src		src
.gitignore		.gitignore
README.md		README.md

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Repository files navigation

PPCS: PX4 Position Controller Switch

Screenshots

GCS (Ground Control Station)

Gazebo SITL

RViz2 — Trajectory Visualization

Benchmark — Tracking Error

Evaluation Table

Architecture

Packages

Prerequisites

Quickstart

1. PX4 SITL (clone & build một lần)

2. Micro XRCE-DDS Agent

3. Build workspace

4. Chạy (qua GCS — khuyến nghị)

4b. Chạy (không qua GCS)

Controllers

Topics

position_controller_node

gen_trajectory_node

trajectory_visualizer_node

GCS

Benchmarking & Scripts

RViz2

Code Map

Status

License

About

Uh oh!

Releases

Packages

Uh oh!

Uh oh!

Contributors

Uh oh!

Languages

Folders and files

Latest commit

History

Repository files navigation

PPCS: PX4 Position Controller Switch

Screenshots

GCS (Ground Control Station)

Gazebo SITL

RViz2 — Trajectory Visualization

Benchmark — Tracking Error

Evaluation Table

Architecture

Packages

Prerequisites

Quickstart

1. PX4 SITL (clone & build một lần)

2. Micro XRCE-DDS Agent

3. Build workspace

4. Chạy (qua GCS — khuyến nghị)

4b. Chạy (không qua GCS)

Controllers

Topics

position_controller_node

gen_trajectory_node

trajectory_visualizer_node

GCS

Benchmarking & Scripts

RViz2

Code Map

Status

License

About

Resources

Uh oh!

Stars

Watchers

Forks

Releases

Packages 0

Uh oh!

Uh oh!

Contributors

Uh oh!

Languages

Packages