Multi-Agent Formation Control Under Disturbances - Quickstart

This repo contains a 2D single-drone dynamics testbed (extensible to multi-drone) with selectable controllers (PID, P/PI/PD, GT2 Fuzzy, and hybrid PID+Fuzzy), YAML-driven configuration, CSV logging, and Python plotting.

🎓 Status Update (2025‑11‑06)

Current State — Step Response Demo

Positives
- Launch files now use tuned gains: Kp=5.0, Ki=2.5/0.0, Kd=5.0 per group.
- Altitude lanes respected: Gazebo plugin holds each drone at its spawn Z or optional <target_altitude>.
- Visual aids in world: checkpoint markers and wind‑direction arrow.
- Enhanced logging scripts (RELIABLE QoS) and better DDS fallback in launch scripts.
- Wind control utility: scripts/wind_tool.py to set live wind forces.
Known Issues (active)
- /agent_X/odom has 0 publishers intermittently → drones don’t move, logger shows Samples: 0.
- Root cause: gzserver sometimes fails to load the Gazebo model plugin (or exits early). When the plugin loads, it publishes odom; when it doesn’t, odom remains subscriber‑only.

Quick Recovery (Headless)

# 1) Clean stale processes + Fast‑DDS SHM
pkill -9 gzserver gzclient rviz2 ros2 2>/dev/null || true
rm -f /dev/shm/fastrtps_* 2>/dev/null || true

# 2) Rebuild + source
source /opt/ros/humble/setup.bash
colcon build --packages-select agent_control_pkg --symlink-install
source install/setup.bash

# 3) Run headless (scripts set DDS + Gazebo PATHs)
./scripts/run_step_response_demo.sh --headless --no-rviz

# 4) Verify odom publisher present (must be 1)
ros2 topic info -v /agent_0/odom

If Publisher count: 0, the plugin isn’t active. Check run log for clues: tail -n 200 thesis_data/*/*_step_response/simulation.log

Temporarily export explicit paths and launch again to force plugin discovery:

export GAZEBO_PLUGIN_PATH="$(pwd)/install/agent_control_pkg/lib:$GAZEBO_PLUGIN_PATH"
export GAZEBO_MODEL_PATH="$(pwd)/agent_control_pkg/models:$GAZEBO_MODEL_PATH"
export LD_LIBRARY_PATH="$(pwd)/install/agent_control_pkg/lib:$LD_LIBRARY_PATH"
ros2 launch agent_control_pkg formation_step_response_demo.launch.py gazebo_gui:=false rviz:=false

Optional isolation (if gzserver still exits):

Launch without RViz (--no-rviz, already defaulted above) to avoid unrelated Qt/GLIBC issues.
Temporarily comment out the <plugin name="wind_plugin" ...> block in agent_control_pkg/worlds/formation_comparison.world to rule out wind plugin issues.
Run gzserver --verbose on the world to capture Gazebo’s plugin load errors.

🎓 FOR THESIS INSTRUCTOR: Quick Demo Guide

System Status: ⚠ In Progress

Formation pipeline, logging, and visuals are wired; tuning applied.
Gazebo odom publisher intermittenly missing due to plugin load — use the recovery steps above.

Quick Commands for Demo

⭐ ONE COMMAND - COMPLETE DEMO (EASIEST!)

cd /home/mmf/Documents/GitHub/multi-agent-formation-control-under-disturbances
./scripts/run_full_demo.sh              # Everything: GUI + CSV (60s)
./scripts/run_full_demo.sh 90           # 90 seconds

What it does: Full automated demo from start to finish:

✅ Setup ROS2 + workspace
✅ Launch Gazebo + RViz (visible GUI)
✅ Record CSV data (synchronized from t=0)
✅ Stop and display results

1. Visual Demo (Alternative Methods)

./scripts/quick_test.sh                       # 15s system validation (optional)
./scripts/run_formation_demo.sh               # 9-drone comparison demo + CSV logging (default 60s)
./scripts/run_formation_demo.sh --no-record   # Same demo, no CSV (visual only)
./scripts/run_formation_demo.sh --scenario=phased --wind-scale=1.5  # 4-phase wind: increasing strength + settle gaps
./scripts/demo_with_recording.sh 60           # Legacy full GUI + CSV recording

What you'll see (run_formation_demo.sh):

9 drones in 3 groups, each group maintaining triangle formation
Group 0 (Magenta): PID+Fuzzy controllers - Best wind rejection
Group 1 (Cyan): PD controllers - Fastest settling time
Group 2 (Yellow): PID controllers - Balanced performance
All groups move from x=-10 to x=+5 under random wind forces

2. Record Thesis Data (CSV Output Only - Headless)

./scripts/record_thesis_data.sh      # 60s recording (default)
./scripts/record_thesis_data.sh 120  # 120s recording

Output: thesis_data/YYYY-MM-DD/HH-MM-SS_formation/

agent_0_pidfuzzy.csv - PID+Fuzzy controller metrics
agent_3_pd.csv - PD controller metrics
agent_6_pid.csv - PID controller metrics

CSV Columns: timestamp, elapsed_time, error_x/y/z, RMSE, IAE, ITAE, settling_time, is_settled

3. Expected Results (Thesis Hypothesis)

Under strong wind disturbance (≈8.0N mean):

PID+Fuzzy (k_fuzzy=0.7): Lowest RMSE & IAE (best disturbance rejection)
PID (Ki=1.946): Good steady-state, moderate disturbance rejection
PD (no integral): Fast settling but higher steady-state error under wind

Controller Parameters (Optimized)

Controller	Kp	Ki	Kd	k_fuzzy	Notes
PID+Fuzzy	3.501	1.946	3.608	0.7	Wind-aware fuzzy + PID hybrid
PID	3.501	1.946	3.608	-	Standard 3-term PID
PD	3.5	0.0	3.61	-	No integral, fast response

Design specs: Ts5 ≈ 2.7s, Overshoot < 0.2%, Settling threshold = 5cm

Troubleshooting

If demo doesn't start:

# Kill old processes
pkill -9 gzserver; pkill -9 gzclient

# Rebuild workspace
source /opt/ros/humble/setup.bash
colcon build --packages-select my_custom_interfaces_pkg formation_coordinator_pkg agent_control_pkg
source install/setup.bash

# Run quick test to validate
./scripts/quick_test.sh

🚀 NEW: Long Scenario Demo (5-Minute Zigzag Trajectory)

Enhanced Thesis Demo - Advanced Features

NEW in this version:

✅ Waypoint-based zigzag trajectory (5 phases, 300 seconds)
✅ 3D motion with altitude changes (1m → 3m → 1.5m → 2.5m → 2m)
✅ Checkpoint system (automatic saves every 60s)
✅ Enhanced metrics logging (IAE, ITAE, ISE, step info)
✅ Phase-by-phase analysis tools
✅ Real-time dashboard with progress indicators

Quick Start - Long Scenario

cd /home/mmf/Documents/GitHub/multi-agent-formation-control-under-disturbances

# Full 5-minute demo (all features)
./scripts/run_long_thesis_demo.sh

# Quick 1-minute test
./scripts/run_long_thesis_demo.sh --test-mode

# Headless mode (maximum FPS)
./scripts/run_long_thesis_demo.sh --headless

What's Different?

Trajectory:

Phase 1 (0-60s): Start → WP1 | Forward 10m, lateral 6m, climb 1m | Light wind (2N)
Phase 2 (60-120s): WP1 → WP2 | Forward 10m, zigzag 4m, climb 1m | Medium wind (4N)
Phase 3 (120-180s): WP2 → WP3 | Forward 5m, zigzag 5m, descend 1.5m | Heavy gusts (6N)
Phase 4 (180-240s): WP3 → WP4 | Backward 10m, return 3m, climb 1m | Variable wind
Phase 5 (240-300s): WP4 → Final | Forward 5m, stabilize | Stress test

Output Structure:

thesis_data/YYYY-MM-DD/HH-MM-SS_long_scenario/
├── raw_data/                    # Full 300s CSV files
│   ├── agent_0_pidfuzzy_full.csv
│   ├── agent_3_pd_full.csv
│   └── agent_6_pid_full.csv
├── checkpoints/                 # Phase-by-phase snapshots
│   ├── phase1_60s/
│   ├── phase2_120s/
│   ├── phase3_180s/
│   ├── phase4_240s/
│   └── phase5_300s/
└── final_results/               # Summary + analysis
    ├── summary.txt
    └── *.csv (complete datasets)

Analysis Tools

After running the demo:

# 1. Checkpoint analysis (phase-by-phase metrics)
python3 analysis/analyze_checkpoint_data.py thesis_data/YYYY-MM-DD/HH-MM-SS_long_scenario

# 2. Generate plots (trajectory, error, comparison)
python3 analysis/plot_long_scenario.py thesis_data/YYYY-MM-DD/HH-MM-SS_long_scenario

# 3. Thesis report (LaTeX tables + markdown summary)
python3 analysis/generate_thesis_report.py thesis_data/YYYY-MM-DD/HH-MM-SS_long_scenario

# 4. Controller comparison (all phases)
python3 analysis/compare_phases.py thesis_data/YYYY-MM-DD/HH-MM-SS_long_scenario

Technical Details

Waypoint System:

Linear interpolation between waypoints
Smooth transitions (no discontinuities)
3D trajectory support (X, Y, Z)
Configurable via YAML (waypoints.times, waypoints.x, waypoints.y, waypoints.z)

Enhanced Metrics:

IAE (Integral Absolute Error) - accumulated tracking error
ITAE (Integral Time-weighted Absolute Error) - penalizes late errors
ISE (Integral Square Error) - penalizes large errors
Step Info - overshoot, settling time, rise time

Configuration Files:

other_packages/formation_coordinator_pkg/config/
├── long_scenario_group0_fuzzy.yaml   # PID+Fuzzy waypoints
├── long_scenario_group1_pd.yaml      # PD waypoints
└── long_scenario_group2_pid.yaml     # PID waypoints

Expected Results

With the long scenario, you'll be able to:

Compare controllers across different trajectory segments
Analyze phase transitions (how each controller adapts)
Measure disturbance rejection under varying wind conditions
Generate publication-ready figures for thesis

Hypothesis:

PID+Fuzzy should outperform in heavy wind phases (Phase 3, 5)
PD should have fastest response in clean trajectory segments
PID should provide best overall balance across all phases

📊 Thesis Results & Figures Guide (What to Show)

This section summarizes the final controller comparison setup and suggests which plots / screenshots to include in your report.

1. Core Scenario (Formation Comparison Demo)

Command (recommended for thesis figures):

cd ~/Documents/GitHub/multi-agent-formation-control-under-disturbances
source /opt/ros/humble/setup.bash
source install/setup.bash
./scripts/run_formation_demo.sh        # 60s, GUI + CSV

Controllers (fixed gains for fair comparison):
- Group 0 (Magenta, agents 0–2): PID+Fuzzy (pid_fuzzy, k_fuzzy=0.7, fuzzy.wind_scalar=1.5)
- Group 1 (Cyan, agents 3–5): PD
- Group 2 (Yellow, agents 6–8): PID
Disturbance:
- Gazebo wind plugin: mean ≈ 8.0 N, variance ≈ 3.0 N
- Python wind tool (default --wind-mode=tool): constant force on /wind/force and matching /wind/velocity

CSV outputs (for this demo):

thesis_data/YYYY-MM-DD/HH-MM-SS_full_demo/agent_0_pidfuzzy.csv
thesis_data/YYYY-MM-DD/HH-MM-SS_full_demo/agent_3_pd.csv
thesis_data/YYYY-MM-DD/HH-MM-SS_full_demo/agent_6_pid.csv

2. Recommended Figures (by chapter)

A) Method / Setup Chapter

Screenshot 1 – Simulation world:
- Gazebo view showing 9 drones in 3 altitude lanes (Magenta, Cyan, Yellow).
- Optionally overlay wind direction arrow / world markers.
Screenshot 2 – Dashboard overview:
- http://localhost:8000/ui with:
  - 9 active agents
  - Wind panel visible (/wind/velocity, /wind/force)
  - System status + formation map.

B) Controller Comparison (Formation Demo) From the three CSVs above (60s full_demo run):

Plot 1 – X-position tracking (one agent per group):
- x(t) vs x_target(t) for:
  - agent_0 (PID+Fuzzy)
  - agent_3 (PD)
  - agent_6 (PID)
- Show how PD drifts under heavy wind, while PID and PID+Fuzzy track better.
Plot 2 – Y-position / lateral error:
- error_y(t) for agent_0, agent_3, agent_6.
- In the final tuned scenario you should see:
  - PID+Fuzzy: smallest lateral error area (best disturbance rejection),
  - PID: intermediate,
  - PD: clearly larger sustained lateral error.
Plot 3 – RMSE and IAE bar chart (60s window):
- For each controller: PID+Fuzzy, PID, PD
- Bars for:
  - RMSE_total (or rmse_x, rmse_y)
  - IAE_x, IAE_y
- Data comes from the last row of each CSV (rmse_*, iae_* columns).
- Highlight especially that IAE_y (lateral) is lowest for PID+Fuzzy and highest for PD.
Plot 4 – Settling behaviour (is_settled vs time):
- From is_settled column:
  - Plot is_settled(t) as 0/1 for each controller.
- Show that under the relaxed but realistic threshold (10 cm, 1 s window) all controllers eventually settle, but their transient error magnitudes differ.

C) Long Scenario / Extra Results (optional but strong)

Use ./scripts/run_long_thesis_demo.sh once.
Use:
- analysis/plot_long_scenario.py
- analysis/compare_phases.py to generate:
- Trajectory plot (X–Y path) for PID+Fuzzy vs PID vs PD.
- Phase-by-phase IAE / ITAE for each controller (Phase 3 and 5 highlight heavy wind).
Recommended figures:
- Long-scenario trajectory overlay (one figure).
- Bar chart of IAE/ITAE per phase for each controller.

3. How to Refer to Results in the Text

When writing the thesis:

Emphasize that gains are identical for PID and the PID-part of PID+Fuzzy:
- Kp=3.501, Ki=1.946, Kd=3.608
Explain that the only difference is:
- Fuzzy GT2 layer with k_fuzzy=0.7 and stronger wind scalar (fuzzy.wind_scalar=1.5),
- Heavy wind environment (≈8N mean) which exposes disturbance rejection capabilities.
For each figure, explicitly mention:
- Which run folder it uses (e.g. thesis_data/2025-11-21/13-51-46_full_demo/),
- Which agent/controller it corresponds to (PID+Fuzzy = agent_0, PD = agent_3, PID = agent_6),
- Which metric you are comparing (RMSE_x, IAE_y, settling time, etc.).

Using this structure you can quickly pick:

2–3 screenshots (world + dashboard),
3–4 key plots (position tracking, lateral error, RMSE/IAE bars, optional long-scenario), and cover the complete story of “PID+Fuzzy vs PID vs PD under strong wind” in the final report.

Highlights

2D dynamics with ambient wind velocity, linear->quadratic drag blend, asymmetric actuator lag (up/down), semi-implicit Euler
Optional constant wind acceleration bias (wind.ax, wind.ay) for integral-action testing
Controller selection from YAML: pid | p | pi | pd | fuzzy | pid_fuzzy
Hybrid PID+Fuzzy (GT2) with mix gains and separate contribution logging
Model-based feed-forward (drag + wind bias cancellation)
Target prefilter (2x first-order, critical-damped approximation) to reduce overshoot
CSV outputs under outputs/simulations/dynamics2d/YYYYMMDD/run_###__/
Python tooling for metrics, plotting, batch summaries and final reports

What’s New (2025-10)

Output management
- output_settings.run_label → run folder named as run_###__<label>
- output_settings.auto_plot → PNG is saved automatically after each run
- output_settings.max_runs_per_day → keep last N runs per day (older are pruned)
Wind options
- YAML wind.vx, wind.vy (ambient velocity)
- NEW: wind.ax, wind.ay (constant acceleration bias)
CSV additions
- CSV header includes ax_wind, ay_wind and hybrid contributions ax_pid, ay_pid, ax_fuzzy, ay_fuzzy, plus ax_ff, ay_ff
Analysis utilities (under analysis/)
- plot_dynamics_2d.py --t_end <T>: plot a single CSV with optional time window
- plot_compare_positions.py: compare multiple runs (position focus, with zoom 0–20 s)
- compute_pid_from_specs.py: actuator-aware closed-form PID from (Mp, Ts5)
- compute_pid_from_velocity.py: velocity/distance-based PID design (time-optimal inspired)
- auto_tune_pid.py: small grid search for PID specs (Mp, Ts5, beta, tau_mode)
- auto_tune_p_pi.py: grid search for P / PI (for baseline evidence)
- summarize_runs.py: rank all runs (per day) by constraints (OS, Ts5) and metrics
- collect_final_report.py: copy best runs (CSV+PNG) to outputs/final_report/<day>
- cleanup_runs.py: move clutter runs to outputs/simulations/dynamics2d/_trash/<day>

Repo Map

Physics core (header-only): agent_control_pkg/include/agent_control_pkg/core/physics_types.hpp, agent_control_pkg/include/agent_control_pkg/core/drone_physics_core.hpp
Controllers
- PID core: agent_control_pkg/include/agent_control_pkg/pid_controller.hpp, agent_control_pkg/src/pid_controller.cpp
- Interface: agent_control_pkg/include/agent_control_pkg/controllers/controller_base.hpp
- PID adapter: agent_control_pkg/include/agent_control_pkg/controllers/pid_adapter.hpp
- GT2 fuzzy: agent_control_pkg/include/agent_control_pkg/gt2_fuzzy_logic_system.hpp, agent_control_pkg/src/gt2_fuzzy_logic_system.cpp
- Fuzzy adapter: agent_control_pkg/include/agent_control_pkg/controllers/fuzzy_gt2_adapter.hpp, agent_control_pkg/src/controllers/fuzzy_gt2_adapter.cpp
- Hybrid PID+Fuzzy: agent_control_pkg/include/agent_control_pkg/controllers/combined_pid_fuzzy_adapter.hpp, agent_control_pkg/src/controllers/combined_pid_fuzzy_adapter.cpp
ROS2 Nodes: agent_control_pkg/src/ros/agent_controller_node.cpp, agent_control_pkg/plugins/simple_drone_plugin.cpp
Configs: agent_control_pkg/config/ros2/*.yaml, agent_control_pkg/config/fuzzy_params.yaml
Docs: docs/dynamics/quickstart_dynamics2d.md (deprecated), docs/dynamics/dynamics2d_system_and_control.md (deprecated)

Build (ROS2 Development)

# Build all packages
colcon build --packages-select my_custom_interfaces_pkg formation_coordinator_pkg agent_control_pkg

# Or build single package
colcon build --packages-select agent_control_pkg

ROS2 Build & Launch (Linux, ROS 2 Humble)

Build from Source

# Source ROS2
source /opt/ros/humble/setup.bash

# Navigate to workspace
cd /home/mmf/Documents/GitHub/multi-agent-formation-control-under-disturbances

# Build all required packages
colcon build --packages-select my_custom_interfaces_pkg formation_coordinator_pkg agent_control_pkg

# Source workspace
source install/setup.bash

Launch Options

Recommended: Full Gazebo Demo (ROS2 + Gazebo + RViz2):

./scripts/run_demo.sh                    # Full GUI demo with PID controller
./scripts/run_demo.sh --controller pd    # PD controller
./scripts/run_demo.sh --headless         # Headless mode (no Gazebo GUI)

Alternative: ROS2-Only Tests (No Gazebo):

# Single agent test (controller + coordinator, no physics simulation)
ros2 launch agent_control_pkg single_agent_test.launch.py

# Multi-agent formation (3 agents under /agent_i namespaces)
ros2 launch agent_control_pkg multi_agent_formation.launch.py

Configuration Files:

Controller params: agent_control_pkg/config/ros2/agent_controller_*.yaml
Formation params: other_packages/formation_coordinator_pkg/config/formation_config.yaml

Gazebo Simulation (ROS2 + 3D Visualization)

🎓 Thesis Multi-Controller Comparison Demo (NEW!)

Compare All 5 Controllers Simultaneously:

source /opt/ros/humble/setup.bash
cd /home/mmf/Documents/GitHub/multi-agent-formation-control-under-disturbances
source install/setup.bash
./scripts/run_thesis_demo.sh

Features:

✅ 5 drones running simultaneously, each with different controller
✅ Color-coded drones: Red (P), Green (PI), Blue (PD), Yellow (PID), Magenta (PID+Fuzzy)
✅ Triangle formation with first 3 drones
✅ High FPS optimization for smooth performance
✅ Real-time metrics comparison
✅ Individual trajectory visualization for each controller

Performance Modes:

./scripts/run_thesis_demo.sh                # Full demo (GUI + RViz)
./scripts/run_thesis_demo.sh --headless     # No Gazebo GUI (higher FPS)
./scripts/run_thesis_demo.sh --no-rviz      # No RViz2 (save resources)
./scripts/run_thesis_demo.sh --performance  # Headless + No RViz (maximum FPS)
./scripts/run_thesis_demo.sh --triangle     # Only 3 drones (triangle formation)

🚀 Quick Start - Single Drone Demo (30 Seconds)

One-Command Launch:

source /opt/ros/humble/setup.bash
cd /home/mmf/Documents/GitHub/multi-agent-formation-control-under-disturbances
source install/setup.bash
./scripts/run_demo.sh

Or Step-by-Step:

# 1. Setup environment
source /opt/ros/humble/setup.bash
cd /home/mmf/Documents/GitHub/multi-agent-formation-control-under-disturbances
source install/setup.bash

# 2. Clean old processes (automatic in run_demo.sh)
pkill -9 gzserver gzclient 2>/dev/null || true

# 3. Launch simulation
ros2 launch agent_control_pkg demo_presentation.launch.py

🎛️ Launch Options

Different Modes:

# Headless mode (faster, no Gazebo GUI)
./scripts/run_demo.sh --headless

# No RViz2 (terminal only)
./scripts/run_demo.sh --no-rviz

Different Controllers:

./scripts/run_demo.sh --controller pid        # PID (default, Kp=0.538, Ki=0.145, Kd=1.368)
./scripts/run_demo.sh --controller p          # P-only controller
./scripts/run_demo.sh --controller pi         # PI controller
./scripts/run_demo.sh --controller pd         # PD controller
./scripts/run_demo.sh --controller pid_fuzzy  # Hybrid PID+Fuzzy (GT2)

# Or directly with ros2 launch:
ros2 launch agent_control_pkg demo_presentation.launch.py controller_config:=pid
ros2 launch agent_control_pkg demo_presentation.launch.py controller_config:=p_only
ros2 launch agent_control_pkg demo_presentation.launch.py controller_config:=pd
ros2 launch agent_control_pkg demo_presentation.launch.py controller_config:=pi
ros2 launch agent_control_pkg demo_presentation.launch.py controller_config:=pid_fuzzy

Different Worlds:

# Minimal world (fast startup, default)
ros2 launch agent_control_pkg demo_presentation.launch.py

# Professional world (visual aids, grid, axes, scale markers)
ros2 launch agent_control_pkg demo_presentation.launch.py world_file:=demo_presentation.world

📊 What's Included

✅ RViz2 visualization with real-time trajectory path tracking
✅ Real-time metrics publisher (error, RMSE, IAE, ITAE, settling time)
✅ Multiple tuned controllers (P, PI, PD, PID, PID+Fuzzy)
- Default PID gains: Kp=0.538, Ki=0.145, Kd=1.368 (~10% overshoot, 4s settling)
✅ Enhanced quadrotor model with improved physics and visual indicators
✅ SimpleDronePlugin with force-based dynamics, damping, altitude PD control
✅ Two world options:
- minimal_test.world → Fast startup, simple visuals (default)
- demo_presentation.world → Professional presentation mode with grid/axes
✅ Path visualizer node for trajectory recording
✅ Metrics publisher node for performance monitoring

📈 Monitor Real-Time Metrics

Terminal 2 (Optional):

# Performance metrics (error, RMSE, IAE, ITAE, settling time)
ros2 topic echo /agent_0/metrics

# Trajectory path
ros2 topic echo /agent_0/path

# Controller diagnostics (PID/Fuzzy contributions)
ros2 topic echo /agent_0/diagnostics

# Drone position
ros2 topic echo /agent_0/odom --field pose.pose.position

# Target position
ros2 topic echo /agent_0/target_pose --field pose.position

System Architecture (3 Layers)

The simulation uses a hierarchical control architecture:

Formation Coordinator Node (formation_coordinator_pkg)
- Generates reference trajectories
- Publishes /agent_0/target_pose (geometry_msgs/PoseStamped)
- Configurable via formation_coordinator_pkg/config/formation_config.yaml
Agent Controller Node (agent_control_pkg)
- Reads odometry: /agent_0/odom (nav_msgs/Odometry)
- Reads target: /agent_0/target_pose (geometry_msgs/PoseStamped)
- Computes control using PID/Fuzzy/Hybrid controllers
- Publishes acceleration command: /agent_0/cmd_accel (geometry_msgs/Accel)
- Publishes diagnostics: /agent_0/diagnostics (custom diagnostics message)
Gazebo Physics Simulation + SimpleDronePlugin
- Applies force-based dynamics (F=ma)
- Reads /agent_0/cmd_accel and applies forces to drone model
- Publishes state feedback: /agent_0/odom (nav_msgs/Odometry)
- Includes damping, altitude control, and realistic physics
Visualization & Metrics (Optional)
- Path Visualizer Node: Records trajectory → /agent_0/path (nav_msgs/Path)
- Metrics Publisher Node: Computes real-time performance → /agent_0/metrics
- RViz2: 3D visualization with custom display configuration

Configuration Files

Controller Configurations (agent_control_pkg/config/ros2/):

agent_controller_pid.yaml → PID controller (Kp=0.538, Ki=0.145, Kd=1.368)
agent_controller_p_only.yaml → Proportional-only controller
agent_controller_pi.yaml → PI controller (no derivative)
agent_controller_pd.yaml → PD controller (no integral)
agent_controller_pid_fuzzy.yaml → Hybrid PID+Fuzzy (GT2) controller
agent_controller_default.yaml → Default configuration (alias for PID)

World Files (agent_control_pkg/worlds/):

minimal_test.world → Fast startup, basic visuals, single drone at origin
demo_presentation.world → Professional mode with grid, axes, scale markers

Key Components:

Physics Plugin: simple_drone_plugin.cpp
- Force-based dynamics (F=ma)
- Linear and attitude damping
- Altitude PD control (Kp≈8, Kd≈3) for level flight
Launch File: demo_presentation.launch.py
- Configurable controller selection via controller_config argument
- Gazebo server/client management
- ROS2 node orchestration with timed startup
- RViz2 integration with custom config

Monitoring & Debugging

# Check topics
ros2 topic list | grep agent_0

# Monitor real-time data
ros2 topic echo /agent_0/target_pose  # Formation reference
ros2 topic echo /agent_0/odom         # Drone state from Gazebo
ros2 topic echo /agent_0/cmd_accel    # Controller output
ros2 topic echo /agent_0/metrics      # Performance metrics (RMSE, IAE, ITAE)
ros2 topic echo /agent_0/path         # Trajectory path visualization

# Verify PID parameters loaded correctly
ros2 param get /agent_0/agent_controller pid.kp  # Should return 0.538
ros2 param list | grep agent_0        # List all parameters

# Check controller diagnostics
ros2 topic echo /agent_0/diagnostics  # PID/Fuzzy contributions

# Node information
ros2 node list                        # List all active nodes
ros2 node info /agent_0/agent_controller  # Controller node details

CSV Data Analysis

The logging script captures synchronized data for post-processing:

# Columns: stamp, pos_xyz, vel_xyz, cmd_accel_xyz, target_xyz
python3 scripts/log_simulation_csv.py --output outputs/logs/test_run.csv

# Plot with existing analysis tools
python analysis/plot_dynamics_2d.py --csv outputs/logs/test_run.csv

Troubleshooting

Common Issues:

Oscillation/Instability:
- Check if PID gains loaded correctly: ros2 param get /agent_0/agent_controller pid.kp
- Try different controller config: ./scripts/run_demo.sh --controller pd
- Verify target is reachable: ros2 topic echo /agent_0/target_pose
Gazebo frozen/crashed:
- Kill processes: pkill -9 gzserver gzclient
- Remove lock files: rm -rf /tmp/.gazebo*
- Restart launch: ./scripts/run_demo.sh
No movement/drone not responding:
- Verify topics are publishing: ros2 topic hz /agent_0/cmd_accel
- Check if plugin loaded: Look for "SimpleDronePlugin: Loaded" in terminal
- Verify odom is publishing: ros2 topic hz /agent_0/odom
Config changes not applied:
- Rebuild package: colcon build --packages-select agent_control_pkg
- Re-source workspace: source install/setup.bash
- Clear CMake cache if needed: rm -rf build/agent_control_pkg
RViz2 not showing trajectory:
- Check path topic: ros2 topic hz /agent_0/path
- Verify path_visualizer_node is running: ros2 node list | grep path_visualizer
- Reload RViz config: File → Open Config → agent_control_pkg/rviz/formation_demo.rviz

Formation Comparison Demo (9 drones, 3 controller groups)

Compare PID+Fuzzy, PD, and PID groups moving as triangle formations along x under wind.

World: agent_control_pkg/worlds/formation_comparison.world
Launch: agent_control_pkg/launch/formation_comparison_demo.launch.py
Coordinators and configs:
- Group 0 (PID+Fuzzy): other_packages/formation_coordinator_pkg/config/formation_group0_fuzzy.yaml (lane y≈−4)
- Group 1 (PD): other_packages/formation_coordinator_pkg/config/formation_group1_pd.yaml (lane y≈0)
- Group 2 (PID): other_packages/formation_coordinator_pkg/config/formation_group2_pid.yaml (lane y≈+4)

Dynamic center motion

Each group’s formation center moves from x≈−10 → +5 at 0.8 m/s (see motion.* parameters in YAMLs).
Controllers per group:
- PID+Fuzzy (mix.k_fuzzy=0.7, wind input enabled)
- PD (derivative filter enabled)
- PID (stronger derivative filter alpha)

Run

source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-up-to formation_coordinator_pkg agent_control_pkg
source install/setup.bash
ros2 launch agent_control_pkg formation_comparison_demo.launch.py

Headless

ros2 launch agent_control_pkg formation_comparison_demo.launch.py gazebo_gui:=false rviz:=false

Verify targets

ros2 param get /formation_0/formation_coordinator_group0 motion.enable  # true
ros2 topic echo /agent_0/target_pose -n 1            # PoseStamped with increasing x
ros2 topic info /agent_0/target_pose -v              # Publisher from formation_0, Best Effort

Notes

QoS: Coordinators publish Best Effort; controllers subscribe with SensorData QoS. RViz may warn about reliability mismatch; it’s benign.
Moving targets: “settled=NO” in metrics is expected; use RMSE/IAE trends to compare tracking.
PID+Fuzzy advantage appears under stronger wind bias/gusts; tune mix.k_fuzzy (0.5→1.0) and fuzzy output scaling (±6..10 m/s²) for more impact.

Run

# No-wind
build/Debug/dynamics_2d_tester.exe agent_control_pkg/config/dynamics_2d_test.yaml

# Wind
build/Debug/dynamics_2d_tester.exe agent_control_pkg/config/dynamics_2d_wind.yaml

CSV path: outputs/simulations/dynamics2d/YYYYMMDD/run_###/run_###.csv

Plot

python analysis/plot_dynamics_2d.py --csv outputs/simulations/dynamics2d/YYYYMMDD/run_###/run_###.csv

The plot PNG is saved next to the CSV.

Position Comparison Plots

# 3 runs side-by-side (PD, PID, PID+FLS) up to t=20 s
python analysis/plot_compare_positions.py \
  --csv \
    outputs/simulations/dynamics2d/20251014/run_004__pd_vel_2p5/run_004__pd_vel_2p5.csv \
    outputs/simulations/dynamics2d/20251014/run_005__pid_vel_2p5/run_005__pid_vel_2p5.csv \
    outputs/simulations/dynamics2d/20251014/run_008__pidf_kf10_vel_2p5/run_008__pidf_kf10_vel_2p5.csv \
  --labels PD PID PIDF_k1 \
  --t_end 20 \
  --out outputs/simulations/dynamics2d/20251014/compare_positions_20s.png

Summarize and Collect

# Rank runs for a day
python analysis/summarize_runs.py --day 20251014 --os-max 20 --ts-max 8 --top 10

# Collect best runs to a tidy folder
python analysis/collect_final_report.py --day 20251014 --labels pd_vel,pid_vel,pidf_ --top 1

Example Scenarios (Thesis‑ready)

No-wind (reference): PD often excels (plant is 1/s^2, no integral action needed)
Persistent bias (constant acceleration ax/ay): PID better than PD on IAE; PID+FLS better than PID
Time-varying wind (sinusoidal, step, gust): Tests disturbance rejection under dynamic conditions
Robustness test (wind + bias): PID+FLS > PID > PD on IAE (demonstrates hybrid advantage)

Use provided YAMLs under agent_control_pkg/config/experiments/:

*_wbias.yaml → adds constant bias (wind.ax/ay) for integral action testing
*_wind.yaml → adds ambient wind velocity (steady-state, absorbed by drag model)
*_wind_step.yaml → step wind disturbance (t=5s → 8 m/s)
*_wind_tv.yaml → sinusoidal wind (5±3 m/s @ 0.5 Hz)
*_wind_gust.yaml → stochastic gust (turbulence model)
Pick labels and autoplot via output_settings in YAML

Physics and Configuration

Core files
- Physics (header-only): agent_control_pkg/include/agent_control_pkg/core/physics_types.hpp, agent_control_pkg/include/agent_control_pkg/core/drone_physics_core.hpp
- Controllers: PID, GT2 FLS adapters under include/agent_control_pkg/controllers/ and src/controllers/
- Tuning: analysis/compute_pid_from_specs.py, analysis/compute_pid_from_velocity.py, analysis/auto_tune_pid.py
Model: position double-integrator with first-order actuator lag (separate up/down), drag blend (linear->quadratic by speed threshold)
Feed-forward: drag + wind bias cancellation (feedforward section in YAML)
Target prefilter: two cascaded first-order filters (target_prefilter: {enabled, tau})
Output management: output_settings: {run_label, auto_plot, max_runs_per_day}

See also: docs/dynamics/advanced_control_notes.md for prefilter/FF details and closed-form tuning formulas.

Tuning Guide (Overshoot and Settling)

Closed-form actuator-aware mapping from specs (Mp, Ts5) to PID (use compute_pid_from_specs.py)
Velocity-based design from distance and desired cruise speed (use compute_pid_from_velocity.py)
Practical tips
- Increase damping (zeta) first to reduce OS, then adjust wn to hit Ts5
- Setpoint weighting (2-DOF PID) and prefilter reduce OS without hurting disturbance rejection
- Use FF to reduce controller workload in wind/bias cases
- For hybrid, scan k_fuzzy in 0.2..1.2, ensure output MF peaks are near +/- 6..10 m/s^2

ROS2 Roadmap (next phase)

Architecture
- Node agent_controller_node: subscribes to position setpoints, publishes attitude/thrust commands
- Node formation_coordinator_node: generates waypoints/centers (already present as stubs)
- Messages: positions (geometry_msgs/PoseStamped), accelerations or attitude setpoints
Mapping accel->attitude/thrust
- theta_cmd ~= ax_des/g, phi_cmd ~= -ay_des/g (yaw fixed), T_cmd = m*(g+az_des)
- enforce angle/rate limits and slew-rate on commands
Integration path
- Extract controlloop core (PID, FLS, prefilter, FF) into a reusable library
- Wrap as ROS2 node with YAML params mirroring this repo
- Optional SITL: PX4 + Gazebo/Ignition for 3D validation

Roadmap / What’s Next

Add 2-DOF PID setpoint weighting to PIDAdapter and YAML key pid.setpoint_weight_b
k_fuzzy fine scan and MF scaling audit under wind+bias
Wind time-window disturbance scenarios and multi-drone formation runs
Final figure pack and LaTeX-ready tables from summarize_runs.py

Controller Selection (YAML)

controller_settings:
  type: pid_fuzzy   # pid | p | pi | pd | fuzzy | pid_fuzzy
  pid:
    kp: 0.538
    ki: 0.145
    kd: 1.368
    enable_derivative_filter: true
  fls:
    enable: true
    params_file: fuzzy_params.yaml
  mix:
    k_pid: 1.0
    k_fuzzy: 1.0

Notes

Fuzzy params define IT2 triangular FOUs for variables error, dError, optional wind, and an output variable (e.g., output or correction). Rules are [error_set, dError_set, wind_set, output_set].
Hybrid sums u_pid and u_fuzzy, then clamps to +-a_max.

Roadmap / What’s Next

Fuzzy MF/output scaling and k_fuzzy fine scan (0.2–1.2) under wind+bias
Windowed wind disturbances (time windows) and multi‑drone formation tests
Pack final figures (compare PNGs + summary CSVs) for the thesis results section
Optional: script to auto‑export LaTeX‑ready tables from summarize_runs.py

Dynamics (per axis, summary)

Actuator (1st order, asym up/down): a_cmd_f[k] = a_cmd_f[k-1] + alpha*(a_cmd[k]-a_cmd_f[k-1]), alpha = clamp(dt/tau, 0,1)
Relative airspeed: v_rel = v - v_wind, |v_rel| = hypot(v_rel,x, v_rel,y)
Drag blend: a_drag = (1-w)a_lin + wa_quad; a_lin = -(c_lin/m)v_rel, a_quad = -(c_quad/m)|v_rel|*v_rel; w = clamp((|v_rel|-v_thr)/max(v_thr,1e-6),0,1)
Semi-implicit Euler: v[k] = v[k-1] + a*dt, x[k] = x[k-1] + v[k]*dt

Troubleshooting Fuzzy / Hybrid

If pid_fuzzy looks identical to pid:

Scale output sets to [m/s^2] (e.g., peaks +-6..10). Too small -> u_fuzzy ~ 0.
Ensure input sets cover the step range (e up to ~5 m; de/dt up to ~0.5..1.0 m/s).
Increase k_fuzzy (e.g., 0.3..1.0).
Make sure rule consequents match output set names exactly.

Outputs & Hygiene

outputs/ is git-ignored. CSV/plots live per-run under dated folders.
Configs under agent_control_pkg/config. Docs under docs/.

Time-varying Wind Model

Add wind_model section to YAML config for dynamic wind disturbances:

wind_model:
  type: sinusoid  # Options: sinusoid | step | gust
  base_vx: 5.0    # Base wind velocity (m/s)
  base_vy: 0.0
  amp_vx: 3.0     # Amplitude for sinusoid/gust (m/s)
  amp_vy: 0.0
  frequency_hz: 0.5  # For sinusoid mode
  step_time: 5.0     # For step mode (seconds)
  step_vx: 8.0       # Step target velocity
  gust_std: 2.0      # For gust mode (standard deviation)
  gust_alpha: 0.95   # For gust mode (LPF smoothing)

Wind velocity is updated each simulation step and logged in CSV columns vx_wind, vy_wind.

Thesis Deliverables (Results Section)

All comparison plots are available in outputs/simulations/dynamics2d/20251015/:

Key Figures for Thesis

Figure 1: No-wind baseline - compare_positions_20s_nowind.png
- Shows PD optimal performance (no persistent disturbance)
- Reference for evaluating integral action necessity
Figure 2: Persistent bias - compare_positions_20s_nowind_bias.png
- Demonstrates PID superiority over PD (31-37% IAE improvement)
- Shows PID+Fuzzy further improves tracking (3-7% over PID)
Figure 3: Step wind disturbance - compare_positions_20s_wind_step.png
- 8 m/s step at t=5s tests transient response
- Validates controller robustness to sudden disturbances
Figure 4: Stochastic gust - compare_positions_20s_wind_gust.png
- Turbulence model tests steady-state disturbance rejection
- Shows hybrid controller advantage in noisy environments
Figure 5: Time-varying sinusoidal wind - compare_positions_20s_wind_tv.png
- 5±3 m/s @ 0.5 Hz tests periodic disturbances
- Demonstrates tracking performance under oscillatory conditions
Figure 6: Wind + bias robustness - compare_positions_20s_wind_bias.png
- Combined persistent and dynamic disturbances
- Validates PID+FLS > PID > PD ranking (robustness case)

Metrics Summary

Use analysis/summarize_runs.py to generate LaTeX-ready tables:

python analysis/summarize_runs.py --day 20251015 --os-max 20 --ts-max 8 --top 20

Key metrics: Overshoot (%), Settling time (s), IAE, ITAE, RMSE

Name		Name	Last commit message	Last commit date
Latest commit History 288 Commits
.cache/clangd/index		.cache/clangd/index
__pycache__		__pycache__
agent_control_pkg		agent_control_pkg
docs		docs
external		external
monitoring_dashboard		monitoring_dashboard
other_packages		other_packages
results/scientific_validation		results/scientific_validation
scripts		scripts
thesis_data		thesis_data
tuning_results		tuning_results
.clangd		.clangd
.flake8		.flake8
.gitignore		.gitignore
CLAUDE.md		CLAUDE.md
Formation_Control_of_Agents_Under_Dynamic_Meteorological_Conditions.pdf		Formation_Control_of_Agents_Under_Dynamic_Meteorological_Conditions.pdf
README.md		README.md
VALIDATION_RESULTS.md		VALIDATION_RESULTS.md
compile_commands.json		compile_commands.json
fastdds_profiles.xml		fastdds_profiles.xml
multi-agent-formation-control.code-workspace		multi-agent-formation-control.code-workspace
pyproject.toml		pyproject.toml
revised_experiment_simulation_setup.tex		revised_experiment_simulation_setup.tex

Folders and files

Latest commit

History

Repository files navigation

Multi-Agent Formation Control Under Disturbances - Quickstart

🎓 Status Update (2025‑11‑06)

Current State — Step Response Demo

Quick Recovery (Headless)

🎓 FOR THESIS INSTRUCTOR: Quick Demo Guide

System Status: ⚠ In Progress

Quick Commands for Demo

⭐ ONE COMMAND - COMPLETE DEMO (EASIEST!)

1. Visual Demo (Alternative Methods)

2. Record Thesis Data (CSV Output Only - Headless)

3. Expected Results (Thesis Hypothesis)

Controller Parameters (Optimized)

Troubleshooting

🚀 NEW: Long Scenario Demo (5-Minute Zigzag Trajectory)

Enhanced Thesis Demo - Advanced Features

Quick Start - Long Scenario

What's Different?

Analysis Tools

Technical Details

Expected Results

📊 Thesis Results & Figures Guide (What to Show)

1. Core Scenario (Formation Comparison Demo)

2. Recommended Figures (by chapter)

3. How to Refer to Results in the Text

Highlights

What’s New (2025-10)

Repo Map

Build (ROS2 Development)

ROS2 Build & Launch (Linux, ROS 2 Humble)

Build from Source

Launch Options

Gazebo Simulation (ROS2 + 3D Visualization)

🎓 Thesis Multi-Controller Comparison Demo (NEW!)

🚀 Quick Start - Single Drone Demo (30 Seconds)

🎛️ Launch Options

📊 What's Included

📈 Monitor Real-Time Metrics

System Architecture (3 Layers)

Configuration Files

Monitoring & Debugging

CSV Data Analysis

Troubleshooting

Formation Comparison Demo (9 drones, 3 controller groups)

Run

Plot

Position Comparison Plots

Summarize and Collect

Example Scenarios (Thesis‑ready)

Physics and Configuration

Tuning Guide (Overshoot and Settling)

ROS2 Roadmap (next phase)

Roadmap / What’s Next

Controller Selection (YAML)

Roadmap / What’s Next

Dynamics (per axis, summary)

Troubleshooting Fuzzy / Hybrid

Outputs & Hygiene

See Also

Time-varying Wind Model

Thesis Deliverables (Results Section)

Key Figures for Thesis

Metrics Summary

About

Resources

Stars

Watchers

Forks

Releases

Packages

Used by

Contributors

Languages