Repository navigation
VFH Step 5: Dynamic Speed Control - #85
Open
Khushi0512 wants to merge 22 commits into
Open
Khushi0512 wants to merge 22 commits into
Khushi0512 wants to merge 22 commits into
Conversation
Implements the second incremental step of the Vector Field Histogram (VFH) roadmap agreed with the maintainer in issue ShisatoYano#52: identify contiguous runs of low-density polar histogram sectors ("valleys") that are free enough to be candidate steering directions, and visualize them. This branch is stacked on the not-yet-merged Step 1 branch (feature/vfh-step1-polar-histogram), since valley detection operates directly on Step 1's PolarHistogram output. Its diff against upstream main will show only this step's own changes once Step 1 is merged. - Valley / CandidateValleyDetector (candidate_valley_detector.py): pure-logic valley detection. Marks sectors at/below a configurable smoothed-density threshold as navigable, then groups consecutive navigable sectors into Valley instances (start/end index, width, center angle), correctly handling valleys that wrap across the 0-degree boundary. - PolarHistogramMapper (polar_histogram_mapper.py, extended): now also runs candidate valley detection each frame and draws each detected valley as a green arc just outside the existing density ring. Density-ring and valley drawing were split into private helper methods to keep draw() readable as the mapper grows across VFH steps. - vfh_candidate_valley_detection.py: new simulation entry point, reusing the same obstacle scenario as Step 1's demo so valleys can be compared directly against that step's raw density histogram. - Unit tests for the circular valley-grouping logic (including the wrap-around and full-circle edge cases), the mapper's valley drawing, and the new simulation entry point. - Regenerated pyrightconfig.json / devcontainer.json extraPaths for the new simulation directory (the new component file lives inside Step 1's existing polar_histogram directory, so no new path was needed there). One existing Step 1 test (test_draw_with_no_detections_adds_nothing) was renamed to test_draw_with_no_detections_adds_only_the_full_valley and its assertion updated: with candidate valley detection added, an empty LiDAR scan now correctly produces one full-circle valley instead of zero drawn elements. Direction selection (choosing which valley/angle to actually steer toward) is intentionally left for Step 3 per the agreed roadmap. How it was verified: all 28 tests across the polar-histogram test suite pass via pytest - 13 new tests for this step (valley detection logic, mapper integration, and the new simulation), 1 existing Step 1 mapper test updated to reflect the new full-circle-valley behavior, and the remaining 14 Step 1 tests passing unmodified. Both the Step 1 and Step 2 simulations also run cleanly end to end.
Implements the third incremental step of the Vector Field Histogram (VFH) roadmap agreed with the maintainer in issue ShisatoYano#52: choose one steering direction from the candidate valleys detected in Step 2, by minimizing a weighted cost function over the target(goal) direction, the vehicle's current heading, and the previously selected direction, following Borenstein and Koren (1991). This branch is stacked on the not-yet-merged Step 2 branch (feature/vfh-step2-candidate-valley-detection), since direction selection operates directly on Step 2's Valley output. Its diff against upstream main will show only this step's own changes once Steps 1-2 are merged. - DirectionSelector (direction_selector.py): pure-logic direction selection. Scores each valley's center angle (converted from the vehicle-relative frame CandidateValleyDetector works in, to the global frame) against the target direction, current heading, and the previously selected direction, each with a configurable weight, and picks the lowest-cost candidate. The previous-direction term is what keeps the choice stable frame to frame instead of flipping between two similarly-scored valleys as the vehicle turns. - PolarHistogramMapper (extended): now also computes a target direction each frame (toward a configurable goal point, or "keep going straight" by default) and runs direction selection. Draws the selected direction as a bold blue arrow and the target direction as a thin dashed line, on top of the existing density ring and valley arcs. - vfh_direction_selection.py: new simulation entry point, reusing the same obstacle scenario as Steps 1-2, with a goal point placed beyond one of the moving obstacles so the selected direction visibly bends away from the straight-line target when that obstacle blocks the way. - Unit tests for the cost function (including the vehicle-relative-to- global angle conversion, oscillation avoidance via the previous- direction term, and the same boundary-wrap case tested in Step 2), the mapper's target/selection integration, and the new simulation. One existing Step 2 mapper test (test_draw_with_no_detections_adds_only_ the_full_valley) was renamed to test_draw_with_no_detections_adds_full_ valley_and_target_line and its assertion updated from 1 to 3 drawn elements: direction selection now always draws a target reference line, and - since an obstacle-free scan still produces one full-circle valley - also a selected-direction arrow. Vehicle motion integration (actually steering the vehicle using this selected direction) is intentionally left for Step 4 per the agreed roadmap; this step only selects and visualizes the direction. How it was verified: all 42 tests across the polar-histogram test suite pass via pytest - 14 new tests for this step (cost function logic, mapper integration, and the new simulation), 1 existing Step 2 mapper test updated to reflect the new drawing behavior, and the remaining 27 Step 1/2 tests passing unmodified. All three simulations (Steps 1, 2, and 3) run cleanly end to end.
Stacked on feature/vfh-step3-direction-selection, continuing the maintainer's roadmap for this issue one branch per step. New files: - src/components/control/vfh/vfh_controller.py: VfhController, a new controller (alongside the existing control/lqr, control/mpc, control/pure_pursuit, etc. subfolders) that reads the Step 3 mapper's currently selected global-frame direction each cycle via mapper.get_direction_selector().get_selected_angle_rad(), and converts it into acceleration / yaw rate inputs by simple proportional control. Wired into FourWheelsVehicle's existing controller slot, so no shared vehicle/mapper class needed to change. Falls back to the vehicle's current heading when no direction is selected yet (e.g. every sector blocked). Cruise speed is held constant for now; adjusting speed by obstacle proximity is left for Step 5. A front-tire steering angle is also derived from the commanded yaw rate (bicycle model relation), for visualization only. - src/simulations/mapping/vfh_vehicle_motion_integration/vfh_vehicle_motion_integration.py: same three-obstacle scenario and goal point as Steps 1-3, now with a VfhController actually driving the vehicle instead of only visualizing the selected direction. - test/test_vfh_controller.py: 12 tests covering constructor validation, the heading-fallback case, proportional speed control (accelerate/decelerate), proportional yaw rate control and saturation (including a direction reversal across two update() calls on the same instance), the steer-angle bicycle-model derivation, and the visualization no-op. - test/test_vfh_vehicle_motion_integration.py: 1 test mirroring the established simulation smoke-test pattern. Changed files: - pyrightconfig.json / .devcontainer/devcontainer.json: +1 line each for the new component subfolder (src/components/control/vfh) and +1 line each for the new simulation folder, alphabetically placed. This step doesn't modify any file from Steps 1-3, so verification here covers this step's own tests plus the full fast unit/component suite; Steps 1-3's own slower animated simulation tests weren't re-run since nothing they depend on changed. Step 5 (dynamic speed control) is deferred to the next branch. How it was verified: all 66 tests in the fast unit/component suite pass via pytest - 12 new tests for this step's controller, and the remaining 54 tests across angle_lib, state, vehicle_specification, polar_histogram, candidate_valley_detector, direction_selector and polar_histogram_mapper passing unmodified. This step's new simulation test also passes (44.71s), with the vehicle now visibly driving from its start position toward the goal, steering around all three obstacles.
…p2-candidate-valley-detection # Conflicts: # src/components/mapping/polar_histogram/polar_histogram_mapper.py
…ure/vfh-step3-direction-selection # Conflicts: # src/components/mapping/polar_histogram/polar_histogram_mapper.py
…-step4-vehicle-motion-integration
Per maintainer feedback on PR ShisatoYano#80: doc/EXAMPLES.md is now generated from each simulation script's docstring, which must be exactly the filename followed by one line each of Title/Description/Author (HOWTOCONTRIBUTE.md Step 2, item 6). Updates this step's docstring to match.
…p2-candidate-valley-detection
Same fix as Step 1, applied to this step's own simulation script.
…ure/vfh-step3-direction-selection
Same fix as Step 1, applied to this step's own simulation script.
…-step4-vehicle-motion-integration
Same fix as Step 1, applied to this step's own simulation script.
Upstream main already added a Title/Description to this file (with slightly different wording) as part of rolling out the doc/EXAMPLES.md gallery format repo-wide. Matching that wording exactly here so this file no longer conflicts with main on Steps 2-4's PRs, which carry this file forward through the merge chain.
…p2-candidate-valley-detection
…ure/vfh-step3-direction-selection
…-step4-vehicle-motion-integration
VfhController's target speed is no longer a fixed cruise_speed_mps: it now scales down toward min_speed_mps as the smoothed obstacle density in the current target direction rises toward danger_density, read each cycle via a new PolarHistogram.max_density_in_angle_range() query. Adds the vfh_dynamic_speed_control simulation and its test, and registers the new simulation folder in sparse-checkout, pyrightconfig.json and devcontainer.json, following the pattern used by Steps 1-4.
Khushi0512
force-pushed
the
feature/vfh-step5-dynamic-speed-control
branch
from
October 4, 2026 09:26
5740110 to
af2c36d
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Overview
Adds Step 5 of the VFH roadmap (#52): dynamic speed control. VfhController's target speed is no longer a fixed cruise_speed_mps; it now scales down toward a configurable min_speed_mps as the smoothed obstacle density ahead (in the current steering direction) rises toward a danger_density threshold, read via a new PolarHistogram.max_density_in_angle_range() query.
Adds the vfh_dynamic_speed_control simulation (same obstacle scenario as Steps 1-4, so trajectories are directly comparable), with the required Title/Description/Author docstring, and registers the new folder in sparse-checkout, pyrightconfig.json and devcontainer.json.