Remove unnecessary sqrt calculations by RasmusLar · Pull Request #4942 · ros-navigation/navigation2

RasmusLar · 2025-02-22T16:28:24Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu
Robotic platform tested on	Isaac Sim v4.2.0
Does this PR contain AI generated software?	Yes (GH Copilot)

Description of contribution in a few bullet points

I replaced calculations which used hypot with a comparison with the square values, to avoid sqrt calculations to improve performance
EDIT: Unit tests moved over to Change to goal checker orientation for yaw angle #4988
~~While writing unit tests, fixed discrepancy in goal checker orientation, which was checking for < tolerance instead of <= tolerance, as all the other limit checks are.~~
~~Reduced tolerance time for the progress checker unit tests to 0.1 seconds, to reduce test runtime from ~17 to ~7 seconds.~~

Description of documentation updates required from your changes

I think no documentation update is needed.

Description of how this change was tested

I wrote unit tests and tested in Isaac sim for ~10 minutes

Future work that may be required in bullet points

Other performance improvements that I can see, reduced copying values from const references

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

codecov · 2025-02-22T17:05:43Z

Codecov Report

Attention: Patch coverage is 94.11765% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...av2_controller/plugins/simple_progress_checker.cpp	91.66%	1 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_controller/plugins/pose_progress_checker.cpp	`96.66% <100.00%> (ø)`
nav2_controller/plugins/stopped_goal_checker.cpp	`100.00% <100.00%> (ø)`
...av2_controller/plugins/simple_progress_checker.cpp	`98.00% <91.66%> (-2.00%)`	⬇️

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

RasmusLar · 2025-02-22T22:23:48Z

nav2_controller/plugins/simple_progress_checker.cpp


 bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
 {
-  return pose_distance(pose, baseline_pose_) > radius_;


I just thought of an edge case, if radius_ was negative before, this would always return true.
This is no longer the case, should that be checked?

See new commit as a suggestion to solve this, resolve if suggestion is good.

RasmusLar · 2025-02-22T22:24:44Z

nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp

  rclcpp::Clock::SharedPtr clock_;

-  double radius_;
+  double radius_, radius_sqr_;


Depending on how negative radius_ values should be checked, this should also be removed?

See new commit as a suggestion to solve this, resolve if suggestion is good.

SteveMacenski · 2025-03-10T21:46:57Z

I replaced calculations which used hypot with a comparison with the square values, to avoid sqrt calculations to improve performance

I'm all for performance improvements, but can you highlight what the runtime metrics are for these improvements? Most of these operations are small and infrequent, so it kind of surprises me if this improves the performance in a measurable way. If there's not a measurable change, I'd prefer to not merge in additional complexity

Reduced tolerance time for the progress checker unit tests to 0.1 seconds, to reduce test runtime from ~17 to ~7 seconds.

I'm a huge fan of this though! Our Ci times are high and small improvements like this really add up quickly.

--

Thanks for the contribution! Sorry for the delay, I was on a trip to India and then I fell sick due to said trip 😆

RasmusLar · 2025-03-11T13:57:49Z

I'm all for performance improvements, but can you highlight what the runtime metrics are for these improvements? Most of these operations are small and infrequent, so it kind of surprises me if this improves the performance in a measurable way. If there's not a measurable change, I'd prefer to not merge in additional complexity
I only ran them separately, and locally it was like a 20% improvement on the function itself.

I guess the improvements are minor in the whole context of Nav2, but also I thought the complexity increase was minor. I guess the biggest issue would be readability.
I can try to set some time aside to do some tests to see how big of a difference it does in the context of the whole.

I'm a huge fan of this though! Our Ci times are high and small improvements like this really add up quickly.

I can separate the tests out into a different PR also when I get time

Thanks for the contribution! Sorry for the delay, I was on a trip to India and then I fell sick due to said trip 😆

Ouch, the one downside about traveling, hope you're better! 😄

SteveMacenski · 2025-03-11T16:30:30Z

I can separate the tests out into a different PR also when I get time

Please! That's easy for me to merge

I can try to set some time aside to do some tests to see how big of a difference it does in the context of the whole.

OK!

-To improve performance of the progress and goal checkers, the sqrt calculations were removed. -Fixed discrepancy in goal checker orientation, which was checking for < limit instead of <= limit, as all the other limit checks are. -Reduced sleep time for the progress checker to 0.1 seconds, to reduce test time from ~17 to ~7 seconds. Signed-off-by: Rasmus Larsson <rasmus.larsson@accenture.com>

After changing to using square values for the radius in the simple progress checker, there was a need to check for negative values, aswell as unit tests for that edge case. Signed-off-by: Rasmus Larsson <rasmus.larsson@accenture.com>

RasmusLar · 2025-03-15T07:49:32Z

Please! That's easy for me to merge

Added that in separate PR: #4988 and updated this description

RasmusLar · 2025-03-16T17:43:12Z

And I think you're right, the improvement is negligible. so I will close this :)

RasmusLar force-pushed the fix/main/controllerPluginPerf branch from 4913c4f to 7860666 Compare February 22, 2025 16:32

RasmusLar force-pushed the fix/main/controllerPluginPerf branch 2 times, most recently from de28ad2 to 87b842f Compare February 22, 2025 21:20

RasmusLar commented Feb 22, 2025

View reviewed changes

RasmusLar mentioned this pull request Mar 15, 2025

Change to goal checker orientation for yaw angle #4988

Merged

7 tasks

RasmusLar added 2 commits March 15, 2025 09:47

Handle negative radius in simple progress checker

82ce1ce

After changing to using square values for the radius in the simple progress checker, there was a need to check for negative values, aswell as unit tests for that edge case. Signed-off-by: Rasmus Larsson <rasmus.larsson@accenture.com>

RasmusLar force-pushed the fix/main/controllerPluginPerf branch from 31f026a to 82ce1ce Compare March 15, 2025 07:47

RasmusLar closed this Mar 16, 2025

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Remove unnecessary sqrt calculations#4942

Remove unnecessary sqrt calculations#4942
RasmusLar wants to merge 2 commits intoros-navigation:mainfrom
RasmusLar:fix/main/controllerPluginPerf

RasmusLar commented Feb 22, 2025 •

edited

Loading

Uh oh!

codecov bot commented Feb 22, 2025 •

edited

Loading

Uh oh!

RasmusLar Feb 22, 2025

Uh oh!

RasmusLar Feb 23, 2025

Uh oh!

RasmusLar Feb 22, 2025

Uh oh!

RasmusLar Feb 23, 2025

Uh oh!

SteveMacenski commented Mar 10, 2025 •

edited

Loading

Uh oh!

RasmusLar commented Mar 11, 2025

Uh oh!

SteveMacenski commented Mar 11, 2025

Uh oh!

RasmusLar commented Mar 15, 2025

Uh oh!

RasmusLar commented Mar 16, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

RasmusLar commented Feb 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

codecov bot commented Feb 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

RasmusLar Feb 22, 2025

Choose a reason for hiding this comment

Uh oh!

RasmusLar Feb 23, 2025

Choose a reason for hiding this comment

Uh oh!

RasmusLar Feb 22, 2025

Choose a reason for hiding this comment

Uh oh!

RasmusLar Feb 23, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Mar 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

RasmusLar commented Mar 11, 2025

Uh oh!

SteveMacenski commented Mar 11, 2025

Uh oh!

RasmusLar commented Mar 15, 2025

Uh oh!

RasmusLar commented Mar 16, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

RasmusLar commented Feb 22, 2025 •

edited

Loading

codecov bot commented Feb 22, 2025 •

edited

Loading

SteveMacenski commented Mar 10, 2025 •

edited

Loading