feat: implement lifecycle-aware service wrappers

[Prathmesh2931]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5298
Primary OS tested on	Ubuntu
Robotic platform tested on	Nav2 system tests (simulation)
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• ServiceServer wrapper being activated during lifecycle node transition
• Added service->on_activate() calls in planner_server, route_server, and other lifecycle nodes that create services
• Resolved "Service called while not activate" warnings that caused test failures

Description of documentation updates required from your changes

No documentation updates required.

Description of how this change was tested

• Performed validation using colcon test --packages-select nav2_ros_common nav2_planner nav2_route --event-handlers console_cohesion+
• Tested each test of nav2_system_test with FASTDDS_BUILTIN_TRANSPORTS=UDPv4 colcon test --packages-select nav2_system_tests --ctest-args -R test_planner_is_path_valid --event-handlers console_direct+ which previous cause timeout issue or logic fail

---

Future work that may be required in bullet points

• Adding a helper method in LifecycleNode class to automatically manage service lifecycle registration
• The test_route timeout issue which unrelated to service activation , requires changes in action server

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
• Should this be backported to current distributions? If so, tag with backport-*.

[mini-1235]

Please fill in the PR template properly

[Prathmesh2931]

I noticed that the individual tests pass when I run with FASTDDS_BUILTIN_TRANSPORTS=UDPv4, but test_route keeps timing out during the compute_and_track_route action execution. Earlier I figured out that services need to be explicitly activated in on_activate() for lifecycle nodes, otherwise we hit the “service called while not activated” warnings. Adding that fixed those warnings.
Now I’m stuck between two directions. One option is to add some kind of smart gating so that certain critical things, like TF lookups or action execution, are allowed even if the node is technically inactive, just to avoid deadlocks. The other option is to keep things strict and only expose services and actions after on_activate() runs, so the node only appears in the ROS graph once it’s actually ready.
The timeout made me think the action server is waiting on something that only becomes available after activation, which turns thing into loop. I’m not sure yet if the real problem is that my service wrapper is being too strict and blocking things it shouldn’t, or if some dependent components, like the action server in route_server, aren’t getting properly activated in the first place.

[SteveMacenski]

Do also make sure to remove commented out code; if this is still WIP, please mark as a draft :-)

I imagine there's other services that should also be migrated before a complete to-be-merged PR is submitted

[SteveMacenski]

If later handling managed nodes through registering them, will these be registered against the node to transition up/down?

[SteveMacenski]

^

[SteveMacenski]

Still open

[SteveMacenski]

@Prathmesh2931 Can you answer? I'm curious if when we auto transition if we need to even keep all the activate()/deactivate() functions here or if even as sub-modules of the node will auto transition as well.

[Prathmesh2931]

I agree that auto-transition would simplify things and could potentially remove the need for explicit activate() / deactivate() calls.

However at the moment the plugins are not registered for automatic lifecycle propagation from the parent node. Because of that the lifecycle state is not automatically forwarded to sub-modules.

To stay consistent with the existing Nav2 plugin pattern, explicit activation/deactivation calls are currently still required.

If we later refactor to support proper lifecycle propagation, we could revisit and simplify this.

[SteveMacenski]

However at the moment the plugins are not registered for automatic lifecycle propagation from the parent node. Because of that the lifecycle state is not automatically forwarded to sub-modules.

Isn't add_managed_entity in the lifecycle node factory adding all services, even in sub-modules to the list? They're all operating on the same node object. Why wouldn't this work?

[Prathmesh2931]

You are right add_managed_entity could theoretically handle this, if the services were registered globally with the node factory. Since these services are currently managed as private members within the specific server (like PlannerServer or RouteServer) they are not automatically included in that managed list. I have kept the explicitactivate()/deactivate()calls to stay consistent with how Nav2 currently manages internal plugin components, but I agree that a future refactor to use a centralized managed entity list would be a great simplification.

[SteveMacenski]

Earlier I figured out that services need to be explicitly activated in on_activate() for lifecycle nodes, otherwise we hit the “service called while not activated” warnings. Adding that fixed those warnings.

This is correct, we need to now activate them

One option is to add some kind of smart gating so that certain critical things, like TF lookups or action execution, are allowed even if the node is technically inactive, just to avoid deadlocks. The other option is to keep things strict and only expose services and actions after on_activate() runs, so the node only appears in the ROS graph once it’s actually ready.

I don't 100% understand. We are creating the services as usual on usually on_configure, but they're not activated yet so if someone tries calling one you'd get a failure message and a failure should be reported back to the client.

TF has its own thing going on that we're not messing with. Action srever should also be made lifecycle aware so that they cannot be called until activated.

I think something missing from this PR is the failure back to the caller. If not active, the service request should return failure. though, the mechanism which that happens would basically need to have some "success" field in the service definitions to set the failure (I think). Actions on the other hand have a nice goal rejection mechanism that we can use for this generically (i.e. in the goal acceptance stage, if not active, reject).

The timeout made me think the action server is waiting on something that only becomes available after activation, which turns thing into loop.

For example?

[SteveMacenski]

Let me know when this is ready for another review; I see alot of commented out lines still so it doesn't look yet ready for a review. Also see the remaining open comments

[codecov]

Codecov Report

❌ Patch coverage is 98.43750% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_route/src/operations_manager.cpp	85.71%	2 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_amcl/src/amcl_node.cpp	89.64% <100.00%> (+0.08%)	⬆️
...r_tree/plugins/control/pause_resume_controller.hpp	100.00% <ø> (ø)
...r_tree/plugins/control/pause_resume_controller.cpp	94.66% <100.00%> (+0.38%)	⬆️
...2_collision_monitor/src/collision_monitor_node.cpp	97.03% <100.00%> (+0.02%)	⬆️
...d/include/nav2_costmap_2d/costmap_2d_publisher.hpp	100.00% <100.00%> (ø)
...tmap_2d/plugins/costmap_filters/costmap_filter.cpp	88.05% <100.00%> (-5.79%)	⬇️
nav2_costmap_2d/src/clear_costmap_service.cpp	44.15% <100.00%> (+8.33%)	⬆️
nav2_costmap_2d/src/costmap_2d_ros.cpp	90.02% <100.00%> (-0.18%)	⬇️
nav2_docking/opennav_docking/src/dock_database.cpp	97.53% <100.00%> (-2.47%)	⬇️
nav2_lifecycle_manager/src/lifecycle_manager.cpp	90.24% <100.00%> (+0.24%)	⬆️
... and 23 more

... and 24 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[Prathmesh2931]

@SteveMacenski, can you let me know if I'm on the right track with this gated ServiceServer in nav2_ros_common? I have used SFINAE to handle various service responses and weak_ptr to avoid circular references. I have verified the fix across the pkg—including the nav2_route null pointer issues and TF test stability and all builds and tests are successful. Once you give the green light or any changes to suggest I can add to it :)

[Prathmesh2931]

@mini-1235, it would be great if you could take a look at this once.

[Prathmesh2931]

@SteveMacenski , just want to follow up when you have a moment . I have verified implementation of gated ServiceServerconfirmed that the SFINAE helpers are correctly handling the various service responses as we discussed.
All 16 CI checks are passing, and the fix remains stable against the latest main branch.Please let me know if there is further refinements needed . Thanks!

[SteveMacenski]

My apologies, I'm needed to triage my mental load and I want to get that first PR with the subscribers done before messing with the services. I didn't expect you to get this done so fast so sorry for the delay :(

Please review the other comments that are still open from my first review, there are still quite a few of the files that changed that shouldn't be (such as the yamls for the route graph) and commented out lines that need to be removed

[Prathmesh2931]

And there is one more thing, We need to make template with utility that handles both shared_ptr(for unit tests) and weak_ptr(for planner) safely during construction phase . Instead of Previous complex template. Which will handle :

• std::shared_ptr<LifecycleNode>
• std::weak_ptr<LifecycleNode>
• std::shared_ptr<rclcpp::Node>
• std::weak_ptr<rclcpp::Node>

I am working on it currently

[Prathmesh2931]

Most probably, CI is running out of resources. I checked on my local system, did a full build, and ran all package test cases, which showed 100% passing. So what should I do?
Can you Review it once and give me further suggestion .

[Prathmesh2931]

I believe the build and tests are now successful. Please let me know if I should proceed with these changes.

[SteveMacenski]

Thank you, this looks much better now!

[SteveMacenski]

Close! Please review any remaining open comments, there are several unaddressed

[SteveMacenski]

Smaller clean up now but largely approved!

@mini-1235 please review to sanity check me and make sure they didn't miss any other services they should have converted

[mini-1235]

@mini-1235 please review to sanity check me and make sure they didn't miss any other services they should have converted

I am honestly not very familiar with the objective of the issue at the moment. I will read through it and review this PR by the end of the week

[Prathmesh2931]

@SteveMacenski, thank you for the feedback and approval of the pattern.I'd like to clarify some points:

On Test Validity & Default Responses

The tests pass because they explicitly verify that requests are rejected when the service is inactive. The callback counter (a) remains zero, which confirms the request was not processed.
The RCLCPP_ERROR in the ServiceServer wrapper ensures lifecycle violations are clearly logged, making debugging straightforward even though the client receives a default-constructed response.

Example from test:

[ERROR] [test_node_inactive]: Service 'empty_srv_inactive' called while not activated. Rejecting request.
EXPECT_EQ(a, 0);  // Passes - callback never executed

This confirms service do not execute while inactive.

On add_managed_entity & Private Members

You are absolutely right - the private/public status of service members is irrelevant to the lifecycle propagation.
The nav2::LifecycleNode::create_service() factory automatically registers the service using add_managed_entity(). The base rclcpp_lifecycle::LifecycleNode maintains this managed entity list and propagates transitions to all registered entities.

The Flow is :

1. Service created via node->create_service<T>(...)
2. Factory calls node->add_managed_entity(service)
3. Base class tracks the managed entity .
4. On transitions, base class calls on_activate() / on_deactivate() on all managed entities

below is snippet which , I've added in interface factory:

auto srv = std::make_shared<nav2::ServiceServer<SrvT>>(
    service_name, node, cb, callback_group);

  // Register the service as  managed entity
  auto lifecycle_node = std::dynamic_pointer_cast<nav2::LifecycleNode>(node);
  if (lifecycle_node) {
    lifecycle_node->add_managed_entity(srv);
  } else {
    srv->on_activate();
  }

This remove need for manual lifecycle handling in each server with on_activate() implementation

On Migration Status

I have completed a full audit of the codebase - all 26 production services are now migrated to the lifecycle-aware wrapper:

Updated packages:

• nav2_amcl (3 services)
• nav2_costmap_2d (7 services)
• nav2_map_server (6 services)
• nav2_lifecycle_manager (2 services)
• nav2_planner (1 service)
• nav2_route (3 services)
• nav2_collision_monitor (1 service)
• opennav_docking (1 service)
• nav2_behavior_tree (2 services)

The remaining rclcpp::Service iinstances exist only in test utilities and do not require lifecycle handling.

On Memory Management

All services perform explicit cleanup using .reset() in their respective on_cleanup() methods. This ensures proper memory release and allows safe node reconfiguration without leaks.

Example pattern used across all servers::

CallbackReturn on_cleanup(const State & state) {
  service_name_.reset();
  return CallbackReturn::SUCCESS;
}

On Forward Declaration

The circular dependency is handled via forward declaration:

namespace nav2 { class LifecycleNode; }

This allows interface_factories.hpp to cast to nav2::LifecycleNode (to access add_managed_entity()) without requiring the full class definition, consistent with existing rclcpp header design.

---

Migration is complete. All services now respect lifecycle state, auto-register with managed entity lists, and clean up properly in on_cleanup(). No manual on_activate() calls remain in server classes.

Let me Know if any changes to add or additional feedback :)

[mini-1235]

Looking at the latest commit, I noticed that some services, such as toggle_cm_service_ and get_cost_service_, still need to manually call on_activate(). However, others like global_loc_srv_ and clear_around_pose_service_ do not require that call.

I am having trouble understanding the discrepancy between these two cases. Could you please clarify what the difference is between them, @Prathmesh2931?

[mini-1235]

Also please pull in / rebase main to fix the test errors

[Prathmesh2931]

Looking at the latest commit, I noticed that some services, such as toggle_cm_service_ and get_cost_service_, still need to manually call on_activate(). However, others like global_loc_srv_ and clear_around_pose_service_ do not require that call.

I am having trouble understanding the discrepancy between these two cases. Could you please clarify what the difference is between them, @Prathmesh2931?
@mini-1235 ,

1 . New auto managed :

For standard services like global_loc_srv_ and clear_around_pose_service_, I am now using the nav2::LifecycleNode::create_service() factory.
How it works: This factory automatically registers the service as a managed_entity within the node.
The Result: When the main Node transitions to ACTIVE, the ROS 2 lifecycle manager automatically toggles these services. This is why I am removing the manual on_activate() calls for these are now redundant.

2 . Plugins & Sub-modules:

For services like toggle_cm_service_ (Collision Monitor) and get_cost_service_ (Costmap), these live inside Plugins.
The Difference: While the factory handles the registration so the node knows the service exists, the Plugin itself must still control its internal state.
The Reason :Plugins often have complex internal logic (timer, shared buffer, or subscribers) that must be toggled in a specific order. So we need to keep on_activate() calls strictly inside the Plugin's own activate() to ensure encapsulation.

[SteveMacenski]

We want to manually transition right now, because we need to setup an ordered policy about which interfaces are activated in which order to prevent segfaults/inactive states, especially for services and subscriptions which could use publisher or clients in their callback.

add_managed_entity does not automatically cycle up on activation, I do not believe. Do you have evidence that this is happening? We do not call https://github.com/ros2/rclcpp/blob/aea98d665a2937243891f6d9ca075d497fd04db5/rclcpp_lifecycle/src/lifecycle_node.cpp#L718 since we override it in each of our callbacks. Thereby https://github.com/ros2/rclcpp/blob/aea98d665a2937243891f6d9ca075d497fd04db5/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp#L596-L604 is not called

After we implement the sub/service/client then we can create our own eq. to that function where we transition up managed entities in an ordered way (publishers, clients, subscriptions, servers) so that senders are created before receivers so that any receiver callback that sends is able to be run. Right now the order in rclcpp_lifecycle (which we do not call) is based on the order its registered.

[Prathmesh2931]

The factory now only handles the non-lifecycle case (srv->on_activate() when node isn't a nav2::LifecycleNode), while lifecycle_node.hpp handles registration via add_managed_entity() for lifecycle nodes. This follows the same pattern as subscriptions and ensures services activate when users call the base class on_activate() in their server implementations.

[Prathmesh2931]

@mini-1235 , can you look forward if there are any service remaining to include around nav2 server that , I might have missed .

[SteveMacenski]

Can you respond to my items in #5893 (comment)? I'm concerned that this appears (?) to be working when nothing should be or is actually activated previously. add_managed_entity should not be activating (yet) since we don't call the base class version of on_activate.

[Prathmesh2931]

Can you respond to my items in #5893 (comment)? I'm concerned that this appears (?) to be working when nothing should be or is actually activated previously. add_managed_entity should not be activating (yet) since we don't call the base class version of on_activate.

You are absolutely right . After reviewing rclcpp_lifecycle implementation , I confirmed that current use of add_managed_entity() is of 'no - op ' because Nav2 server is overriding on_activate() without calling base class implementation.

Test appear to pass:

The auto-activation fallback in constructor activates the service while node is already ACTIVE . But these does not solve issue for service created during on_configure() - they are register but never cycle up .

I was thinking to solve these without relying on add_managed_entity():

1. To store service in Nav2 specific managed_service_servers_ vector using weak_ptr .
2. Provide activate/deactivate_managed_service_server() as helper .
3. Developer call explicitly in the activate/deactivate() override.

This maintain order policy while keeping activation explicit and visible .
Alternative can be call to rclcpp_lifecycle::LifecycleNode::on_activate(state) at end of every Nav2 server transition .
Just let me any further correction needed or something which , I might be missing out .

[mini-1235]

Can you also double check that you have tested these changes in simulation? For example, try a few services from AMCL/costmap/route server to confirm they are all still functioning correctly

[mini-1235]

Can you explain the changes here

[mini-1235]

Revert? I don't see how it is related?

[SteveMacenski]

The auto-activation fallback in constructor activates the service while node is already ACTIVE . But these does not solve issue for service created during on_configure() - they are register but never cycle up .

Is that really true? If we're in the on_activate state, we shouldn't be Active, we're in the process of becoming active. You see the check for if we're active passing when constructing the service in the on_activate() callback? That seems wrong...

I was thinking to solve these without relying on add_managed_entity():

I'd like to keep add_managed_entity() because I think long term we're going to exploit that to prevent us from explicitly needing to call active/deactive on all these lifecycle interfaces. I wanted to wait until we had all the pub/sub/service/clients done before doing this though so that it was a 1-time switch and we didn't leave the stack in a weird state of partial auto- and partial manual-activation.

To store service in Nav2 specific managed_service_servers_ vector using weak_ptr .
Provide activate/deactivate_managed_service_server() as helper .
Developer call explicitly in the activate/deactivate() override.

This is very similar to my high level intent. I was actually going to leave the managed_entities_ object alone, then we iterate over it when we activate/deactive to sort them by sub/pub/action/client/service -- then activate/deactive them in that order. We could store it ourselves that is stored already by type, but I see benefits from being as close to rclcpp as possible (and having some of this work go back into rclcpp possibly). Since active/deactivate are 'rare' and non-steady-state-runtime states, I'm OK with doing a bit of sorting and casting (should still be very fast).

Then in on_activate and deactivate, we call our version of nav2::LifecycleNode::ActivateInterfaces() and DeactivateInterfaces() to handle this.

So, we're thinking essentially the same thing it appears! I'm just thinking of this down the line once these 2x PRs are complete and we have the eq. for Service & Action Clients. I'm only looking to do that once we have all of them we can do in bulk so we don't end up in weird transient states in the main branch that aren't representative of the final state.