Reactive obstacle avoidance for a PX4 multirotor using the classic Vector Field Histogram (Borenstein & Koren, 1991). The drone builds a polar histogram of obstacle density around itself every control tick, picks the most open direction that still heads toward the goal, and flies it — no global path, no map, purely reactive.
Learning project. WIP testing in a complex sim world.sdf
The demo has been accelerated
Rviz version
You can check out HOLO-DWA (same environment, DWA algorithm, a detailed installation of env).
obstacles (fixed cylinders | live LiDAR pillars)
│
▼
1. polar histogram split 360° into sectors (5°→72 bins); each obstacle adds
density max(0, a − b·d_surface) over the bins it
subtends (enlarged by the robot radius). Nearer/wider =
denser and over more bins.
2. smoothing circular moving average → kills single-bin jitter
3. valleys runs of bins below `threshold` = candidate openings
4. steering the opening nearest the goal bearing; goal-open → straight
at goal, else wide opening steers s_max/2 in from the near
edge, narrow opening steers to its centre
5. velocity setpoint heading = steer bearing, speed ∝ how open ahead, z holds
cruise altitude
It is 2D: the histogram is a horizontal slice at cruise altitude. An obstacle counts only if it reaches the slice, so the low (1.2 m) cylinders are correctly seen straight through at 2 m — honest 2D, not faked 3D fly-over. (3D is a documented future extension, see WORKLOG.)
VFH/
├── run.sh one-shot tmux launcher (PX4+gz+agent+bridge+nodes)
├── WORKLOG.md decisions, findings, tuning
├── logs/ flight CSVs
└── VFH_OA/ ament_python package (vfh_oa)
├── vfh_oa/
│ ├── vfh_core.py the algorithm — pure numpy, no ROS
│ ├── world_spec.py single source of truth for the worlds
│ ├── obstacle_sensor_node.py fixed cylinders | live LiDAR pillars
│ └── vfh_planner_node.py reactive planner + PX4 offboard flight
├── config/vfh_params.yaml
├── launch/vfh_nodes.launch.py
├── worlds/ generated SDF (gen_world.py)
├── rviz/vfh.rviz
├── test/test_vfh_core.py 23 unit tests
└── tools/
├── viz_vfh.py matplotlib: scene + polar histogram
├── sim_offline.py no-ROS reactive-loop sim (tuning)
├── gen_world.py world_spec → SDF
└── check_log.py flight CSV summary
vfh_core.py has no ROS dependency — it is unit-tested and driven offline
before ever touching Gazebo, because the histogram and valley search are the
parts of VFH easiest to get wrong.
# 1. build
cd ~/ws && colcon build --packages-select vfh_oa --symlink-install
source install/setup.bash
# 2. verify the algorithm with no ROS at all
cd src/VFH/VFH_OA
python3 -m pytest test/ # 23 tests
python3 tools/viz_vfh.py --world vfh_forest # scene + polar histogram PNGs
python3 tools/sim_offline.py --sweep # every world × sensing mode
python3 tools/gen_world.py # (re)generate the SDF worlds
# 3. fly it (headless or with a GUI)
cd ~/ws/src/VFH
./run.sh # vfh_test, fixed obstacles, default goal
WORLD=vfh_forest ./run.sh # the wall course
OBST_SOURCE=lidar ./run.sh # drive VFH off the real 2D LiDAR scan
./run.sh 8 -3 # custom goal (Gazebo ENU x y)
./run.sh kill # tear down
# 4. summarize the flight
python3 VFH_OA/tools/check_log.py --world vfh_testrun.sh env overrides: WORLD (vfh_test|vfh_forest), OBST_SOURCE
(fixed|lidar), V_MAX, HEADLESS=1, RVIZ, PX4_DIR.
| topic | dir | type | note |
|---|---|---|---|
/fmu/out/vehicle_odometry |
in | px4_msgs/VehicleOdometry | position, velocity, attitude (NED) |
/lidar |
in | sensor_msgs/LaserScan | 2D LiDAR (lidar mode), bridged from gz |
/vfh/obstacles |
internal | std_msgs/Float32MultiArray | rows (n,e,radius,top) NED |
/fmu/in/trajectory_setpoint |
out | px4_msgs/TrajectorySetpoint | velocity setpoint, NaN passthrough |
/vfh/planner_markers |
out | visualization_msgs/MarkerArray | polar histogram spokes + steer/goal arrows |
/vfh/flown_path, /vfh/obstacle_markers |
out | Path / MarkerArray | RViz |
Internal frame is PX4 NED; goals are given in Gazebo world ENU and
converted on the way in. Bearing convention θ = atan2(east, north)
(0 = North, +π/2 = East).
| param | default | meaning |
|---|---|---|
sector_angle |
5.0° | histogram resolution (72 bins) |
density_a, density_b |
7.0, 1.0 | density = a − b·d_surface; blocks out to 5 m |
threshold |
2.0 | bins below this are free (candidate valleys) |
smooth_window |
5 | moving-average window |
robot_radius |
0.5 m | histogram enlargement (prop + safety) |
wide_valley |
9 bins | wide/narrow opening cutoff (s_max) |
v_max |
1.0 m/s | cruise speed |
goal_threshold |
0.5 m | goal acceptance |
Tuned offline in sim_offline.py, not in Gazebo — see WORKLOG.
vfh_forest puts a wall of five cylinders straight across the direct line to
the goal. A potential-field planner (APF_OA) stalls at the wall's centre where
attraction and repulsion cancel. VFH sees the wall blocking ahead and a free
opening past its finite end, and steers there — it rounds the wall instead of
wedging. This is the goal.md Step 5(a) comparison, reproducible offline with
python3 tools/sim_offline.py --world vfh_forest.
Headless PX4 SITL + Gazebo flights (./run.sh, all four combinations):
| world | mode | result | length | min clearance | stuck |
|---|---|---|---|---|---|
| vfh_test | fixed | REACHED | 12.9 m | 0.97 m | 0 |
| vfh_test | lidar | REACHED | 15.0 m | 1.36 m | 0 |
| vfh_forest | fixed | REACHED | 17.4 m | 1.11 m | 0 |
| vfh_forest | lidar | REACHED | 20.4 m | 1.40 m | 0 |
Every course reached with zero stuck episodes; the forest wall is rounded in both fixed and lidar mode. Real-flight path lengths match the offline sim and clearances are a touch better (PX4 smooths the velocity setpoints).
Offline sim (tools/sim_offline.py --sweep, perfect velocity tracking):
world mode result time len clear
vfh_test fixed REACHED 13.4s 12.7m 0.79m
vfh_test lidar REACHED 13.4s 12.7m 0.79m
vfh_forest fixed REACHED 17.4s 17.2m 0.86m
vfh_forest lidar REACHED 17.4s 17.2m 0.86m