Implementation of Dynamic Window Pure Pursuit (DWPP) #5591
Conversation
There was a problem hiding this comment.
@doisyg FYI - would this be of interest? See the video
Also, please add unit testing for the functions and features you added. Check out test/ in the package to see some examples. Your main function should be tested for all edge cases at the bare minimum
|
|
||
| declare_parameter_if_not_declared( | ||
| node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); | ||
| declare_parameter_if_not_declared( |
There was a problem hiding this comment.
All new parameters should be added to the RPP docs' configuration guide. Also probably worth adding a note to the migration guide about this new feature and a link to your paper
|
|
||
| last_command_velocity_ = cmd_vel.twist; | ||
|
|
There was a problem hiding this comment.
| last_command_velocity_ = cmd_vel.twist; | |
| last_command_velocity_ = cmd_vel.twist; |
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
Outdated
Show resolved
Hide resolved
|
|
||
| // ---- 1) Dynamic Window ---- | ||
| double dw_vmax = std::min(current_speed.linear.x + A_MAX * DT, V_MAX); | ||
| const double dw_vmin = std::max(current_speed.linear.x - A_MAX * DT, V_MIN); |
There was a problem hiding this comment.
Should we have different min/maxs for accel / decel? Not all robot are symmetric
| double dw_wmax = std::min(current_speed.angular.z + AW_MAX * DT, W_MAX); | ||
| const double dw_wmin = std::max(current_speed.angular.z - AW_MAX * DT, W_MIN); | ||
|
|
||
| // Reflect regulated v (tighten upper limit) |
There was a problem hiding this comment.
A more descriptive comment here would be nice
| } | ||
| } | ||
|
|
||
| if (best_v > -1e290) { |
| } | ||
|
|
||
| // ---- 3) If no intersection exists: Select the one with the smallest Euclidean distance to the line w = k v among the 4 corners | ||
| struct Corner { double v; double w; }; |
There was a problem hiding this comment.
Some of these seem to be utility functions that should go into utility files rather than in the RPP main controller file
| best_v = corners[0].v; | ||
| best_w = corners[0].w; | ||
|
|
||
| for (int i = 0; i < 4; ++i) { |
There was a problem hiding this comment.
if you just make corners a vector of size 4,then you can use corners.size()
|
|
||
| // Apply curvature to angular velocity after constraining linear velocity | ||
| angular_vel = linear_vel * regulation_curvature; | ||
| if (params_->use_dynamic_window == false){ |
There was a problem hiding this comment.
| if (params_->use_dynamic_window == false){ | |
| if (!params_->use_dynamic_window) { |
| } | ||
| else{ |
There was a problem hiding this comment.
| } | |
| else{ | |
| } else { |
Absolutely ! @Decwest, we were discussing internally your DWPP repo (at Dexory) and I am super happy that you guys are working toward a nav2 integration. |
…n2 into feature/implement_dwpp
Signed-off-by: Decwest <[email protected]>
This reverts commit 53c5cd4.
This reverts commit 547c54e.
Signed-off-by: Decwest <[email protected]>
Codecov Report❌ Patch coverage is
... and 9 files with indirect coverage changes 🚀 New features to boost your workflow:
|
|
Let me know when you want me to take a look again after the review comments are addressed! Only things at a glance: Your very large block of code may be more concise as or I'd suggest reviewing your code a bit for how it can be as concise and self-descriptive as possible |
|
@SteveMacenski |
3 participants
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Added Parameters
"OPEN_LOOP": Uses the last commanded velocity (recommended)"CLOSED_LOOP": Uses odometry velocity (may hinder proper acceleration/deceleration)Description of how this change was tested
Simulation video
DWPP_simulation.mp4
Future work that may be required in bullet points
For Maintainers:
backport-*.