Skip to content

Add subscription options to odometry subscriptions#4961

Closed
pele1410 wants to merge 4 commits intoros-navigation:jazzyfrom
Metal-Shark-Sharktech:odometry-qos-overrides
Closed

Add subscription options to odometry subscriptions#4961
pele1410 wants to merge 4 commits intoros-navigation:jazzyfrom
Metal-Shark-Sharktech:odometry-qos-overrides

Conversation

@pele1410
Copy link
Copy Markdown
Contributor

@pele1410 pele1410 commented Mar 5, 2025

Basic Info

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

Description of contribution in a few bullet points

Added rclcpp::SubscriptionOptions to the odometry topic subscribers to allow qos overrides in the configuration. We run our odometry topics with Unreliable QoS to prevent network issues that are common with high-rate, reliable DDS transmissions. This change allows us to override the Reliable QoS of the odom subscriptions to allow matching.

Description of documentation updates required from your changes

I would expect that this should be documented as a new configuration option, but I'm not entirely sure where that would happen.

Description of how this change was tested

We run a gazebo simulation of a surface vessel (similar to the WAM-V). Using the following configuration ros__parameters:

maneuver/controller_server:
    ros__parameters:
        ...
        qos_overrides:
            /localization/global/odometry:
                subscription:
                    reliability: best_effort
                    depth: 1

maneuver/task_manager:
    ros__parameters:
        ...
        qos_overrides:
            /localization/local/odometry:
                subscription:
                    reliability: best_effort
                    depth: 1

Prior to this change, I would see QoS mismatches between the nav2 susbcribers and my odom publishers:

[]$ ros2 topic info -v /localization/local/odometry
Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: ekf_odom
Node namespace: /localization/local
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: PUBLISHER
GID: 01.10.05.60.b9.86.71.a6.70.56.6a.1c.00.00.2d.03
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 5
...
Node name: task_manager
Node namespace: /maneuver
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: SUBSCRIPTION
GID: 01.10.5d.47.e3.a9.04.4e.97.ff.1b.54.00.00.33.04
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

and

[]$ ros2 topic info -v /localization/global/odometry
Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: ekf_map
Node namespace: /localization/global
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: PUBLISHER
GID: 01.10.a7.a9.a4.52.ec.04.d2.54.89.14.00.00.2d.03
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 5
...
Node name: controller_server
Node namespace: /maneuver
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: SUBSCRIPTION
GID: 01.10.de.ff.08.3b.c1.67.5b.7a.2a.9e.00.01.36.04
QoS profile:
  Reliability: RELIABLE
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

After this change, I verified there were no such mismatches.

[]$ ros2 topic info -v /localization/local/odometry
Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: ekf_odom
Node namespace: /localization/local
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: PUBLISHER
GID: 01.10.05.60.b9.86.71.a6.70.56.6a.1c.00.00.2d.03
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 5
...
Node name: task_manager
Node namespace: /maneuver
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: SUBSCRIPTION
GID: 01.10.e7.0e.a9.87.2c.19.ae.f1.2e.bb.00.00.33.04
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Node name: task_manager_bt_rclcpp_node
Node namespace: /maneuver
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: SUBSCRIPTION
GID: 01.10.e7.0e.a9.87.2c.19.ae.f1.2e.bb.00.00.47.04
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (5)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

and

[]$ ros2 topic info -v /localization/global/odometry
Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: ekf_map
Node namespace: /localization/global
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: PUBLISHER
GID: 01.10.a7.a9.a4.52.ec.04.d2.54.89.14.00.00.2d.03
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 5
...
Node name: controller_server
Node namespace: /maneuver
Topic type: nav_msgs/msg/Odometry
Topic type hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0
Endpoint type: SUBSCRIPTION
GID: 01.10.9e.8f.e6.2f.00.f4.2d.9a.e5.3f.00.01.36.04
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): KEEP_LAST (1)
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Future work that may be required in bullet points

N/A

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

@mergify
Copy link
Copy Markdown
Contributor

mergify bot commented Mar 5, 2025

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

@pele1410
Copy link
Copy Markdown
Contributor Author

pele1410 commented Mar 5, 2025

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

I don't have a mechanism to test this using anything but Jazzy as that is what our system runs with. What options do I have?

@SteveMacenski
Copy link
Copy Markdown
Member

SteveMacenski commented Mar 6, 2025 •
edited
Loading

@pele1410 this PR is simple enough and I understand it well, its OK to cherry pick this to main and as long as CI passes, I'm OK with it.

If you submit the main PR, I can just merge the pair together. I would though like you to write up a migration guide entry for this change to let people know they can remap the QoS on the odometry smoother, that's useful information.

SteveMacenski
SteveMacenski reviewed Mar 6, 2025
View reviewed changes
@pele1410 pele1410 mentioned this pull request Mar 6, 2025
Closed
7 tasks
@pele1410
Copy link
Copy Markdown
Contributor Author

pele1410 commented Mar 6, 2025

@pele1410 this PR is simple enough and I understand it well, its OK to cherry pick this to main and as long as CI passes, I'm OK with it.

If you submit the main PR, I can just merge the pair together. I would though like you to write up a migration guide entry for this change to let people know they can remap the QoS on the odometry smoother, that's useful information.

#4968

I believe this is what you were asking for. I'm not super familiar with the Github flow.

@pele1410 pele1410 force-pushed the odometry-qos-overrides branch from a12e333 to f1b373d Compare March 6, 2025 19:27
@SteveMacenski
Copy link
Copy Markdown
Member

SteveMacenski commented Jun 23, 2025

@pele1410 this however we can merge in since my PR isn't going to be backported as it changes a ton of things. Can you re-trigger CI with a rebase?

Christopher Thompson added 4 commits July 7, 2025 11:05
Add subscription options to odometry subscriptions
c304963 
Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>
Fix whitespace issue
41edb82 
Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>
Actually fix whitespace
b401086 
Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>
Style fixes
4dd1d48 
Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>
@pele1410 pele1410 force-pushed the odometry-qos-overrides branch from f1b373d to 4dd1d48 Compare July 7, 2025 11:42
@SteveMacenski
Copy link
Copy Markdown
Member

SteveMacenski commented Jul 7, 2025

@pele1410 the test still fails the usual way

C++ exception with description "cannot set or declare a parameter, or change the callback from within set callback" thrown in the test body.

Were the changes to rclcpp backported to Jazzy?

@pele1410 pele1410 mentioned this pull request Jul 8, 2025
Merged
@pele1410
Copy link
Copy Markdown
Contributor Author

pele1410 commented Jul 8, 2025

Doesn't look like they were. I've asked the rclcpp guys in the PR if it can be backported.

@pele1410
Copy link
Copy Markdown
Contributor Author

pele1410 commented Jul 9, 2025 •
edited
Loading

ros2/rclcpp#2742 (comment)

Doesn't look like it's going to be backported. Wondering what our options are here.

  • On the Jazzy branch, can you tolerate an EXPECT_FAIL on that 1 test case?
  • I could make a 2nd OdomSmoother object that doesn't do the qos_policies

Frankly, the second suggestion would be something I do in my own repository rendering this MR moot.

@SteveMacenski
Copy link
Copy Markdown
Member

SteveMacenski commented Jul 10, 2025

We can't merge this as-is since that dynamic parameter would cause a crash for users. We have 3 real options:

  1. Abandon this PR
  2. Remove the duration as a dynamic parameter in Jazzy so that it cannot be reinitalized
  3. Update the OdomSmoother API to have a setDuration() for changing the odom duration so we don't make a new object but just update the variable of the existing one

I'm pretty happy with any of those options for Jazzy - though prefer 1 or 3.

@pele1410
Copy link
Copy Markdown
Contributor Author

pele1410 commented Jul 10, 2025

While it pains me to do so; I'm going to go for option 1. I've moved the changes into my own codebase already.

@pele1410 pele1410 closed this Jul 10, 2025
@SteveMacenski
Copy link
Copy Markdown
Member

SteveMacenski commented Jul 10, 2025 •
edited
Loading

OK - sorry to hear that but I get it. I look forward to future contributions - sorry that this one took so long and was a pain!

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

blocked

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

2 participants

@pele1410 @SteveMacenski