Skip to content

Route server corner smoothing (backport #5226) #5259

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom
Merged
merged 1 commit into from
Jun 12, 2025
Merged

Route server corner smoothing (backport #5226) #5259

merged 1 commit into from
Jun 12, 2025

Conversation

@mergify
Copy link
Contributor

@mergify mergify bot commented Jun 12, 2025

Basic Info

Info Please fill out this column
Ticket(s) this addresses N/A
Primary OS tested on Ubuntu 20.04
Robotic platform tested on Gazebo simulation of ag vehicle
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

  • Added a corner arc class that stitches two successive edges with a circle
  • Modified densify in path_converter.cpp to enable corner smoothing

Description of documentation updates required from your changes

  • No documentation yet

Description of how this change was tested

  • Changes were tested via launch testing to give smoothed path like in the image shown:
    image

Future work that may be required in bullet points

  • Unit testing needs to be written for corner_arc class
  • Documentation for new parameters required

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
This is an automatic backport of pull request #5226 done by [Mergify](https://mergify.com).

@alexanderjyuen @SteveMacenski @mergify
Route server corner smoothing (#5226)
ef0d472 
* added edge length method

Signed-off-by: Alexander Yuen <[email protected]>

* Added corner arc class

Signed-off-by: Alexander Yuen <[email protected]>

* replaced double vectors with Coordinates, added methods to return start and end coordinates

Signed-off-by: Alexander Yuen <[email protected]>

* using Coordinates, fixed direction of tangents

Signed-off-by: Alexander Yuen <[email protected]>

* added corner arc in header, added logger in protected variable

Signed-off-by: Alexander Yuen <[email protected]>

* first pass of corner smoothing algorithm

Signed-off-by: Alexander Yuen <[email protected]>

* reassigning next edge to have a different start, if a corner occurs before it

Signed-off-by: Alexander Yuen <[email protected]>

* using unique pointer instead of raw pointers for new edges and nodes

Signed-off-by: Alexander Yuen <[email protected]>

* added smoothing parameter

Signed-off-by: Alexander Yuen <[email protected]>

* made angle of interpolation a parameter

Signed-off-by: Alexander Yuen <[email protected]>

* const for return methods, added flag for smoothing corners

Signed-off-by: Alexander Yuen <[email protected]>

* moved getEdgeLength() into the Directional Edge struct

Signed-off-by: Alexander Yuen <[email protected]>

* using float instead of double

Signed-off-by: Alexander Yuen <[email protected]>

* smoothing radius is float, couple methods moved to protected

Signed-off-by: Alexander Yuen <[email protected]>

* removed signed_angle_ as a member variable

Signed-off-by: Alexander Yuen <[email protected]>

* removed unnecessary member variables

Signed-off-by: Alexander Yuen <[email protected]>

* removed angle of interpolation and inferring it from path density and radius instead

Signed-off-by: Alexander Yuen <[email protected]>

* consolidated corner arc into one header function

Signed-off-by: Alexander Yuen <[email protected]>

* readded newline

Signed-off-by: Alexander Yuen <[email protected]>

* changed corner arc to corner smoothing

Signed-off-by: Alexander Yuen <[email protected]>

* replaced the use of edges with coordinates to generate smoothing arc, removed storage of nodes and edges

Signed-off-by: Alexander Yuen <[email protected]>

* linting

Signed-off-by: Alexander Yuen <[email protected]>

* fixing cpplint

Signed-off-by: Alexander Yuen <[email protected]>

* linting for headers

Signed-off-by: Alexander Yuen <[email protected]>

* cpplinting

Signed-off-by: Alexander Yuen <[email protected]>

* Update nav2_route/src/path_converter.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_route/src/path_converter.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_route/src/path_converter.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_route/src/path_converter.cpp

Signed-off-by: Steve Macenski <[email protected]>

* Update nav2_route/include/nav2_route/corner_smoothing.hpp

Signed-off-by: Steve Macenski <[email protected]>

* fixed divide by zeros and accessing empty route.edges

Signed-off-by: Alexander Yuen <[email protected]>

* uncrustify linting

Signed-off-by: Alexander Yuen <[email protected]>

* cpp linting

Signed-off-by: Alexander Yuen <[email protected]>

* path converter linting

Signed-off-by: Alexander Yuen <[email protected]>

* changed all doubles to floats

Signed-off-by: Alexander Yuen <[email protected]>

* added check for edges that are colinear to avoid divide by 0, fixed final edge interpolation

Signed-off-by: Alexander Yuen <[email protected]>

* linting

Signed-off-by: Alexander Yuen <[email protected]>

* Update nav2_route/include/nav2_route/corner_smoothing.hpp

Signed-off-by: Steve Macenski <[email protected]>

* added doxygen for corner arc class

Signed-off-by: Alexander Yuen <[email protected]>

* added warning message if corner can't be smoothed

Signed-off-by: Alexander Yuen <[email protected]>

* added smooth_corners to the nav2 params file

Signed-off-by: Alexander Yuen <[email protected]>

* added smoothing flag and radius parameter to README.md'

Signed-off-by: Alexander Yuen <[email protected]>

* typo in README

Signed-off-by: Alexander Yuen <[email protected]>

* added testing for corner smoothing

Signed-off-by: Alexander Yuen <[email protected]>

* Update nav2_route/include/nav2_route/corner_smoothing.hpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Alexander Yuen <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
(cherry picked from commit 792bcac)
@SteveMacenski SteveMacenski merged commit fc775a6 into kilted Jun 12, 2025
7 of 10 checks passed
@SteveMacenski SteveMacenski deleted the mergify/bp/kilted/pr-5226 branch June 12, 2025 02:54
@mergify
Copy link
Contributor Author

mergify bot commented Jun 12, 2025

@mergify[bot], all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @kilted, but it must be in main
to have these changes reflected into new distributions.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@SteveMacenski SteveMacenski SteveMacenski approved these changes

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

3 participants

@SteveMacenski @alexanderjyuen