Port of Regulated Pure Pursuit#69
Open
fmrico wants to merge 3 commits into
Open
Conversation
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
There was a problem hiding this comment.
Pull request overview
Adds a new EasyNav controller plugin implementing Nav2’s Regulated Pure Pursuit controller (including the optional Dynamic Window Pure Pursuit extension), adapted to EasyNav’s ControllerMethodBase / update_rt(NavState&) architecture and perception-based obstacle regulation.
Changes:
- Introduces
RegulatedPurePursuitControllerimplementation + pluginlib registration for EasyNav. - Adds pure helper/heuristic headers for regulation terms and DWPP dynamic-window velocity selection.
- Adds initial unit tests + README documenting algorithm, adaptations, parameters, and example config.
Reviewed changes
Copilot reviewed 10 out of 10 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| controllers/easynav_regulated_pp_controller/CMakeLists.txt | Builds/exports the new controller shared library and registers the plugin. |
| controllers/easynav_regulated_pp_controller/package.xml | Declares package metadata and dependencies for the new controller plugin. |
| controllers/easynav_regulated_pp_controller/easynav_regulated_pp_controller_plugins.xml | Pluginlib description for loading the controller via class name. |
| controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp | Public controller API, parameters, and helper method declarations. |
| controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp | Core control loop, lookahead/carrot logic, regulation, rotation behaviors, and DWPP integration. |
| controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp | Pure math heuristics for curvature/obstacle/approach velocity regulation. |
| controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/dynamic_window_pure_pursuit_functions.hpp | DWPP math utilities for dynamic-window computation and optimal (v, w) selection. |
| controllers/easynav_regulated_pp_controller/README.md | Documentation of algorithm, adaptations from Nav2, parameters, and tuning notes. |
| controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt | Adds a gtest target for the new controller tests. |
| controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp | Unit tests for heuristics, DWPP window logic, and controller geometry helpers. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+88
to
+92
| if (current_vel > Eps) { | ||
| // if the current velocity is positive, acceleration means an increase in speed | ||
| candidate_max_vel = current_vel + max_accel * dt; | ||
| candidate_min_vel = current_vel + max_decel * dt; | ||
| } else if (current_vel < -Eps) { |
Comment on lines
+288
to
+296
| const double min_feasible = last_angular_vel_ - max_angular_accel_ * dt; | ||
| const double max_feasible = last_angular_vel_ + max_angular_accel_ * dt; | ||
| angular_vel = std::clamp(angular_vel, min_feasible, max_feasible); | ||
|
|
||
| // Slow down to avoid overshooting the target angle. | ||
| const double max_vel_to_stop = std::sqrt(2.0 * max_angular_accel_ * std::fabs(angle_to_target)); | ||
| if (std::fabs(angular_vel) > max_vel_to_stop) { | ||
| angular_vel = sign * max_vel_to_stop; | ||
| } |
Comment on lines
+253
to
+257
| int main(int argc, char ** argv) | ||
| { | ||
| testing::InitGoogleTest(&argc, argv); | ||
| return RUN_ALL_TESTS(); | ||
| } |
Comment on lines
+161
to
+165
| - For Adaptive Pure Pursuit behavior, disable all boolean regulation parameters except | ||
| `use_velocity_scaled_lookahead_dist`, and tune `lookahead_time`, `min_lookahead_dist` and | ||
| `max_lookahead_dist`. | ||
| - For plain Pure Pursuit behavior, disable all boolean parameters and tune `lookahead_dist`. | ||
| - `use_obstacle_regulated_linear_velocity_scaling` can over-trigger in tightly cluttered |
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
Member
|
Hi @fmrico, Your Do you mind updating your branch so we can run the CI again? BTW, the controller looks amazing! |
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
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.
Hi,
This PR contains a port of the Nav2 Regulated Pure Pursuit Controller to the EasyNav plugin architecture (
easynav::ControllerMethodBase), including its optionalDynamic Window Pure Pursuit (DWPP) extension.
Summary
easynav_regulated_pp_controller, a new controller plugin undereasynav_plugins/controllers: a port of Nav2's Regulated Pure PursuitController to EasyNav's
ControllerMethodBase/update_rt(NavState&)plugin design.for kinematically-feasible velocity/acceleration limited commands.
curvature-based slow-down, approach-to-goal slow-down, and
rotate-to-rough-heading / rotate-to-goal-heading.
Adaptations vs. the Nav2 version
EasyNav has no costmap, no per-controller collision-arc checking, and no
separate goal-checker/TF-transform plugins, so:
by the nearest point in the fused point-cloud perception
(
use_obstacle_regulated_linear_velocity_scaling).colision_checker.*logic alreadypresent in
ControllerMethodBase, instead of duplicating an arc-based check.goal_tolerance.*fromNavState(or fallbackparameters), matching the convention used by other EasyNav controllers.
pose, since
pathandrobot_posealready share a frame in EasyNav.the robot (
findClosestPoseIndex), since EasyNav delivers the full,un-pruned global path rather than a plan already pruned to a local window
around the robot.