[RotationShimController] Rotate to goal heading #4332

gennartan · 2024-05-11T06:35:06Z

When arriving in the goal xy tolerance, the rotation shim controller takes back the control to command the robot to rotate in the goal heading orientation.

The initial goal of the rotationShimController was to rotate the robot at the beginning of a navigation towards the paths orientation because some controllers are not good at performing in place rotations. For the same reason, the rotationShimController should be able to rotate the robot towards the goal heading.

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4289
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simulation on custom robot
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Add new feature to rotationShimController: It can rotate towards the goal heading when arriving in goal XY tolerance

Description of documentation updates required from your changes

Added new parameter ´rotate_to_goal_heading´, so need to add that to default configs and documentation page

Future work that may be required in bullet points

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

SteveMacenski · 2024-05-13T20:58:51Z

@gennartan please see #4290 (comment) if you hadn't already

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

SteveMacenski · 2024-05-13T21:07:55Z

Please also make sure to sign off your commits. See the DCO failed job for details

When arriving in the goal xy tolerance, the rotation shim controller takes back the control and perform the necessary rotation to be in the correct goal orientation. This can be useful when the final orientaion of the robot matters. It can also simplify the configuration of the primary controller since it would not have to be configured to be able to reach the target orientation.

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

SteveMacenski · 2024-05-28T18:20:52Z

Essentially done though, this looks good to me. Did you test this out?

gennartan · 2024-05-28T19:09:38Z

Essentially done though, this looks good to me. Did you test this out?

Yes I tested it in simulation. It works fine.

@gennartan please see #4290 (comment) if you hadn't already

I missed this comment the other day. Now it is fixed (and tested) 😄

SteveMacenski · 2024-05-28T19:13:41Z

@gennartan see my new review comments, some are outstanding still. Then I think the last thing left if just adding the new parameter to the configuration guide for this plugin + add to the migration guide that this is a new available feature!

SteveMacenski · 2024-05-28T20:42:50Z

I think just docs left now!

When arriving in the goal xy tolerance, the rotation shim controller takes back the control to command the robot to rotate in the goal heading orientation. The initial goal of the rotationShimController was to rotate the robot at the beginning of a navigation towards the paths orientation because some controllers are not good at performing in place rotations. For the same reason, the rotationShimController should be able to rotate the robot towards the goal heading. Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>

SteveMacenski · 2024-05-29T18:55:34Z

gennartan · 2024-05-29T19:26:54Z

here is the PR: ros-navigation/docs.nav2.org#561

SteveMacenski · 2024-05-29T19:40:27Z

Thanks @gennartan !

When arriving in the goal xy tolerance, the rotation shim controller takes back the control to command the robot to rotate in the goal heading orientation. The initial goal of the rotationShimController was to rotate the robot at the beginning of a navigation towards the paths orientation because some controllers are not good at performing in place rotations. For the same reason, the rotationShimController should be able to rotate the robot towards the goal heading. Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>

* Add configure and cleanup transitions to lifecycle manager and client (#4371) Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * [RotationShimController] Rotate to goal heading (#4332) When arriving in the goal xy tolerance, the rotation shim controller takes back the control to command the robot to rotate in the goal heading orientation. The initial goal of the rotationShimController was to rotate the robot at the beginning of a navigation towards the paths orientation because some controllers are not good at performing in place rotations. For the same reason, the rotationShimController should be able to rotate the robot towards the goal heading. Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com> * bump to 1.2.9 for release --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com> Co-authored-by: Joni Pöllänen <jonipol@users.noreply.github.com> Co-authored-by: Saitama <gennartan@users.noreply.github.com>

SteveMacenski · 2024-05-31T21:16:15Z

@gennartan your PR has a failing test now! https://app.circleci.com/pipelines/github/ros-navigation/navigation2/11752/workflows/7593ca53-120e-481f-9fba-326f26f6e32e/jobs/36042/tests

Can you take a look and fix the issue?

tonynajjar · 2024-05-31T21:26:32Z

nav2_rotation_shim_controller/test/test_shim_controller.cpp

+  // of the goal. The rotation shim controller should rotate toward the goal heading
+  // then it will throw an exception because the costmap is bogus
+  controller->setPlan(path);
+  EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);


This test consistently fails for me on rolling. @gennartan would it be possible for you to check it out? It failed on CI too . No idea how it passed for the PR

Ah sorry, just saw Steve commented already

I fixed the error related to the EXPECT_THROW and implemented real tests for this feature.

But I am still getting error while testing and I cannot figure out what is causing it. Could you help me @tonynajjar or @SteveMacenski ?

build/nav2_rotation_shim_controller/test_results/nav2_rotation_shim_controller/flake8.xunit.xml: 3 tests, 0 errors, 3 failures, 0 skipped - nav2_rotation_shim_controller.flake8 E501 (./install/_local_setup_util_sh.py:14:100) <<< failure message line too long (109 > 99 characters): FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' >>> - nav2_rotation_shim_controller.flake8 E501 (./install/_local_setup_util_sh.py:15:100) <<< failure message line too long (124 > 99 characters): FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' >>> - nav2_rotation_shim_controller.flake8 E501 (./install/_local_setup_util_sh.py:16:100) <<< failure message line too long (125 > 99 characters): FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' >>>

It seems that I am only getting these test error locally, I created a PR and this error doesn't show up.

See the PR: #4391

However the release tests still fails, but not for the rotationShimController

gennartan force-pushed the test_goal_rotation branch from bf25e80 to 20917ee Compare May 11, 2024 06:42

SteveMacenski requested changes May 13, 2024

View reviewed changes

gennartan force-pushed the test_goal_rotation branch 2 times, most recently from 3aabd4a to 4095c29 Compare May 25, 2024 21:27

gennartan requested a review from SteveMacenski May 26, 2024 17:45

SteveMacenski requested changes May 28, 2024

View reviewed changes

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp Show resolved Hide resolved

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp Outdated Show resolved Hide resolved

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp Show resolved Hide resolved

gennartan force-pushed the test_goal_rotation branch from 4095c29 to 549e591 Compare May 28, 2024 18:57

gennartan force-pushed the test_goal_rotation branch 2 times, most recently from 5c13988 to 54c1b34 Compare May 28, 2024 19:54

gennartan force-pushed the test_goal_rotation branch from 54c1b34 to 16e1bd7 Compare May 29, 2024 18:53

SteveMacenski approved these changes May 29, 2024

View reviewed changes

SteveMacenski merged commit 30e2cde into ros-navigation:main May 29, 2024
9 of 10 checks passed

gennartan deleted the test_goal_rotation branch May 29, 2024 19:52

gennartan restored the test_goal_rotation branch May 29, 2024 19:53

tonynajjar reviewed May 31, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[RotationShimController] Rotate to goal heading #4332

[RotationShimController] Rotate to goal heading #4332

gennartan commented May 11, 2024

SteveMacenski commented May 13, 2024 •

edited

SteveMacenski commented May 13, 2024

SteveMacenski commented May 28, 2024

gennartan commented May 28, 2024 •

edited

SteveMacenski commented May 28, 2024

SteveMacenski commented May 28, 2024

SteveMacenski commented May 29, 2024

gennartan commented May 29, 2024

SteveMacenski commented May 29, 2024

SteveMacenski commented May 31, 2024

tonynajjar May 31, 2024 •

edited

tonynajjar May 31, 2024

gennartan Jun 1, 2024 •

edited

gennartan Jun 1, 2024

[RotationShimController] Rotate to goal heading #4332

[RotationShimController] Rotate to goal heading #4332

Conversation

gennartan commented May 11, 2024

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

SteveMacenski commented May 13, 2024 • edited

SteveMacenski commented May 13, 2024

SteveMacenski commented May 28, 2024

gennartan commented May 28, 2024 • edited

SteveMacenski commented May 28, 2024

SteveMacenski commented May 28, 2024

SteveMacenski commented May 29, 2024

gennartan commented May 29, 2024

SteveMacenski commented May 29, 2024

SteveMacenski commented May 31, 2024

tonynajjar May 31, 2024 • edited

Choose a reason for hiding this comment

tonynajjar May 31, 2024

Choose a reason for hiding this comment

gennartan Jun 1, 2024 • edited

Choose a reason for hiding this comment

gennartan Jun 1, 2024

Choose a reason for hiding this comment

SteveMacenski commented May 13, 2024 •

edited

gennartan commented May 28, 2024 •

edited

tonynajjar May 31, 2024 •

edited

gennartan Jun 1, 2024 •

edited