Large Nav2 Node, Utils, and Interface Refactor

[SteveMacenski]

Closing #4888

This PR does a very large refactor of Nav2, its utilities, their structure, and how we interface with the ROS network. Note that if you want to do a review, focus on the nav2_ros_common package, the other changes are simply using the new API updates that this creates. That is what should be scrutinized, though I'm happy to have as much of a review as possible from anyone interested :-)

The aim of this work was to be able to expose:

• Action, Action Client, Service, and Clients' introspection for all such interfaces removing the boilerplate code for all instances in the application code
• Allow for QoS overrides of all publishers and subscribers by a standard parameter, removing the boilerplate code in the application code
• Audit and correct existing QoS profiles and configuration mistakes. Ensure that this cannot happen again by controlling globally the use of QoS in all instances
• Simplify interfaces for Nav2 developers and removing the need to specify extra information when using 'standard' topics and providing semantically meaningful QoS profiles to developers (i.e. StandardTopicQoS for reliable, volitile, queue size 10 topics, and LatchedTopicQoS for transient local queue size 1 topics, and SensorDataQoS for best effort, queue size 10 topics)
• Allow pub/sub to more easily expose things like matched callbacks, dropped message callback, etc for system debugging
• Clean up the code so that all main nav2 utilities can be addressed as MyObject<T>::SharedPtr rather than std::shared_ptr<MyObject<T>> for easier reading of users

In doing so, enough wrappers of ROS 2 interfaces justified the creation of a new nav2_ros_common package containing our specialized node and the various interface wrappers for the Actions, Services, Topics, etc and breaking it out of nav2_util. The new nav2::LifecycleNode also has factories for services, topics, etc using the same style API as ROS 2, but now returning Nav2 specific instances with our added utilities.

• create_client --> nav2::ServiceClient with introspection configuration
• create_service --> nav2::ServiceServer with introspection configuration
• create_publisher --> rclcpp_lifecycle::LifecyclePublisher with QoS override parameterization
• create_subscriber --> rclcpp::Subscriber with QoS override parameterization
• create_action_server --> nav2::SimpleActionServer with introspection configuration
• create_action_client --> rclcpp_action::Client with introspection configuration

So, in code using the Nav2 nodes, it will no longer return in some cases the rclcpp interface object but the new nav2 ones, requiring some migration of custom planners, controllers, behavior tree nodes, etc to use these interfaces. We do this globally to ensure that all ROS interfaces go through our factories in order to set the QoS defaults, QoS parameter overridability, and introspection status -- in addition to future possible modifications that are able to be applied to all ROS interfaces. For example, adding ros2_tracing tracepoints on key network events, new feature settings, etc.

Future work:

• Add ability to automatically activate managed objects (i.e. pub) if the node is already activated.
• Lifecycle Subscription, service server, client, action server, client? Auto-activate if already activated as well. nav2_ros_common interfaces for Services, Subscription, and Action to be lifecycle enabled #5298
• Nav2 Pub/Sub with logging or exposed callbacks for dropped messages, matched events, etc more easily used by applications
• Add nodethread spin as an optional spin type
• declare_or_get_param as a member & update code to use Update declares and gets to use new declare_or_get_param API #5299
• Create own Publisher, Subscriber, Action Client to control future changes and migration of users all at once?
• Separate latched publisher vs subscriber depth

---

General information

• nav2_util::LifecycleNode is now nav2::LifecycleNode
• The Service Client, Service Server, and Simple Action Server were also moved to the nav2:: namespace, but they should not be accessed directly. Use the create_* factories from the nav2::LifecycleNode
• There are now nav2::qos profiles for QoS to be used in the codebase for homologation and later easier changing: nav2::qos::StandardTopicQoS nav2::qos::LatchedTopicQoS and nav2::qos::SensorDataQoS.
• The APIs for create_* are very similar to the default ones, but slightly different to move the QoS profile specification below required information. When this is not specified the StandardTopicQoS is used (reliable, queue size 10). Only override this if you want another QoS type -- this is supposed to help make sure that things keep compatible and consistent. Avoid use of `SystemDefaultsQoS.

Plugin Migration

All plugins now use nav2::LifecycleNode instead of rclcpp_lifecycle::LifecycleNode or nav2_util::LifecycleNode. All must be updated to use this new API from planners and controllers to behavior tree nodes. Similarly, calls to the create_* factories will now point to the Nav2 objects, where applicable. See above for what those look like and below for a migration from the existing code usage to the new version in order to make use of the new features and API.

Service Client Migration Example

We no longer need to create the object manually, nor should we as it bypasses the lifecycle node factories that set introspection and possibly other future features. We can use the node now to do this instead of passing in a node and we don't need to specify the node type anymore as a template.

// If using rclcpp::Client<T> from previous create_client factor
main_client_ = node->create_client<SrvT>(main_srv_name_, rclcpp::SystemDefaultsQoS(), callback_group_); 

// If using nav2_util::ServiceClient<T> manually
main_client_ =
  std::make_shared<nav2_util::ServiceClient<SrvT>>(
  main_srv_name_,
  node,
  false /* Does not create and spin an internal executor*/);

To:

main_client_ = node->create_client<SrvT>(main_srv_name_, false  /* Does not create and spin an internal executor*/);

Service Server Migration Example

We need to include the rmw_request_id_t header now, so we have 3 placeholders

// If using previous create_service factory
  service_ = node->create_service<std_srvs::srv::Trigger>(
    std::string(node->get_name()) + "/" + getName() + "/reroute",
    std::bind(
      &ReroutingService::serviceCb, this,
      std::placeholders::_1, std::placeholders::_2));

// If using nav2_util::ServiceServer<T> manually
  service_ = std::make_shared<nav2_util::ServiceServer<std_srvs::srv::Trigger,
      std::shared_ptr<rclcpp_lifecycle::LifecycleNode>>>(
    std::string(node->get_name()) + "/" + getName() + "/reroute",
    node,
    std::bind(
      &ReroutingService::serviceCb, this,
      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

To

  service_ = node->create_service<std_srvs::srv::Trigger>(
    std::string(node->get_name()) + "/" + getName() + "/reroute",
    std::bind(&ReroutingService::serviceCb, this,
        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));

Action Server Migration

We can use the factory now from the node and the node is not required as an argument.

  compute_and_track_route_server_ = std::make_shared<ComputeAndTrackRouteServer>(
    node, "compute_and_track_route",
    std::bind(&RouteServer::computeAndTrackRoute, this),
    nullptr, std::chrono::milliseconds(500), true);

To

  compute_and_track_route_server_ = create_action_server<ComputeAndTrackRoute>(
    "compute_and_track_route",
    std::bind(&RouteServer::computeAndTrackRoute, this),
    nullptr, std::chrono::milliseconds(500), true);

Action Client Migration

We can use the node now and not need to specify all the nasty interfaces. We do that internally for you to clean up application code

  nav_to_pose_client_ = rclcpp_action::create_client<ClientT>(
    get_node_base_interface(),
    get_node_graph_interface(),
    get_node_logging_interface(),
    get_node_waitables_interface(),
    "navigate_to_pose", callback_group_);

To

  nav_to_pose_client_ = create_action_client<ClientT>(
    "navigate_to_pose", callback_group_);

Publisher Subscriber Migration

To migrate, the order of the arguments in the Subscription must change since the QoS profile is now optional. It is now (topic, callback, QoS) whereas QoS defaults to the StandardTopicQoS, which is the same as rclcpp::QoS for the moment.

Publishers that explicitly specify a QoS profile do not require changes, though if the constructor using depth is used, it must now specify a policy explicitly. Both are now nav2::Publisher and nav2::Subscription objects that today just typedef the rclcpp and rclcpp_lifecycle versions. In the future, more features will be added here like lifecycle support for the subscriptions, so its highly recommended as part of this migration to migrate the rclcpp:: to nav2:: as well so those updates are already easily available.

  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", 1);
  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan", rclcpp::QoS());

  data_sub_ = node->create_subscription<sensor_msgs::msg::Range>(
    source_topic, range_qos,
    std::bind(&Range::dataCallback, this, std::placeholders::_1));

To

  plan_publisher_ = create_publisher<nav_msgs::msg::Path>("plan");  // No QoS is required if using the StandardTopicQoS, else it can be provided

  data_sub_ = node->create_subscription<sensor_msgs::msg::Range>(
    source_topic,
    std::bind(&Range::dataCallback, this, std::placeholders::_1));  // QoS can be ommitted if using StandardTopicQoS, else it can be provided last

[Copilot]

Pull Request Overview

This PR refactors Nav2 and Opennav Docking packages to replace nav2_util and raw rclcpp_lifecycle::LifecycleNode usage with the new nav2_ros_common APIs, unifying QoS defaults and simplifying ROS interface creation.

• Replace all includes and types from nav2_util/rclcpp_lifecycle to nav2_ros_common and nav2::LifecycleNode
• Migrate service, action, publisher, and subscriber factories to create_* methods and async_call for services
• Update CMakeLists and package.xml to add nav2_ros_common dependency and include directories

Reviewed Changes

Copilot reviewed 300 out of 620 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
nav2_dwb_controller/dwb_core/trajectory_critic.hpp	Updated include and node pointer type to nav2::LifecycleNode
nav2_dwb_controller/dwb_core/publisher.hpp	Switched lifecycle callbacks and node type to nav2 namespace
nav2_dwb_controller/dwb_core/dwb_local_planner.hpp	Replaced node weak ptr type
nav2_dwb_controller/dwb_core/CMakeLists.txt	Added nav2_ros_common dependency and include dirs
nav2_docking/opennav_docking_core/.../*.hpp/.cpp/.xml/.CMakeLists.txt	Updated all docking core plugins to use nav2_ros_common APIs
nav2_docking/opennav_docking_bt/...	Updated BT test fixtures and CMake to nav2::LifecycleNode
nav2_docking/opennav_docking/...	Refactored server, utils, tests to new node and factory APIs
nav2_costmap_2d/...	Migrated costmap layers, filters, subscribers, publishers
nav2_costmap_2d/test/...	Converted all unit/integration tests to use nav2::LifecycleNode
nav2_costmap_2d/CMakeLists.txt & package.xml	Added nav2_ros_common dependency and include dirs

Comments suppressed due to low confidence (1)

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp:39

• The non-templated constructor for CostmapSubscriber(const nav2::LifecycleNode::WeakPtr&, const std::string&) is declared but its implementation was removed from the .cpp file. Either provide a definition or remove the declaration to avoid linker errors.

  CostmapSubscriber(

[SteveMacenski]

I re-reviewed the first ~250 files and going to step away for the day due to brain mush. Will pick back up tomorrow. I expect linting problems to cleanup but otherwise I think this should be ready minus my review comments to come.

[MarcoMatteoBassa]

I would default this to wait forever, or at least a bigger duration, to reduce the chance of errors in systems with a long boot-up times or other kind of delays.

[SteveMacenski]

Service clients shouldn't be invoking on startup - that's kind of the point on the lifecycle node. By the time this could be triggered, the node would need to be activated and ready to process data. I think this is reasonable to leave as-is?

[SteveMacenski]

FYI @MarcoMatteoBassa ros2/rclcpp#2876 this is what is causing the smac planner deadlock tests. When options.qos_overriding_options is non-empty for the publishers (also possibly subscribers?), then if ever reinitialized within a dynamic parameter callback it deadlocks.

[codecov]

Codecov Report

Attention: Patch coverage is 98.00000% with 10 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_amcl/src/amcl_node.cpp	97.14%	3 Missing ⚠️
nav2_costmap_2d/src/costmap_2d_markers.cpp	0.00%	2 Missing ⚠️
...av2_docking/opennav_docking/src/docking_server.cpp	91.66%	2 Missing ⚠️
nav2_bt_navigator/src/bt_navigator.cpp	80.00%	1 Missing ⚠️
...2_collision_monitor/src/collision_monitor_node.cpp	95.00%	1 Missing ⚠️
nav2_costmap_2d/plugins/static_layer.cpp	80.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...nclude/nav2_behavior_tree/behavior_tree_engine.hpp	100.00% <ø> (ø)
...tree/include/nav2_behavior_tree/bt_action_node.hpp	81.67% <100.00%> (+2.29%)	⬆️
...ee/include/nav2_behavior_tree/bt_action_server.hpp	83.33% <ø> (ø)
...clude/nav2_behavior_tree/bt_action_server_impl.hpp	89.77% <100.00%> (+0.17%)	⬆️
...clude/nav2_behavior_tree/bt_cancel_action_node.hpp	80.00% <100.00%> (ø)
...ree/include/nav2_behavior_tree/bt_service_node.hpp	84.05% <100.00%> (-0.23%)	⬇️
...avior_tree/include/nav2_behavior_tree/bt_utils.hpp	97.48% <ø> (ø)
...lugins/action/append_goal_pose_to_goals_action.hpp	100.00% <ø> (ø)
...ior_tree/plugins/action/assisted_teleop_action.hpp	100.00% <ø> (ø)
...ree/plugins/action/assisted_teleop_cancel_node.hpp	100.00% <ø> (ø)
... and 155 more

... and 65 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[MarcoMatteoBassa]

FYI @MarcoMatteoBassa ros2/rclcpp#2876 this is what is causing the smac planner deadlock tests. When options.qos_overriding_options is non-empty for the publishers (also possibly subscribers?), then if ever reinitialized within a dynamic parameter callback it deadlocks.

That is unfortunate :(

[SteveMacenski]

I actually filed a similar ticket awhile back and it seemed like it was resolved for subscribers. I'm also seeing that the tests don't fail anymore where the odom subscription (the subscriber that triggered the bug report) are failing, so it looks like subscriptions are working now. It seems to just be publishers.

Your original PR wanted the Subscriptions to be overridable, so that is still doable at this point in this PR as long as it wasn't for the planner server / smac planner. That is the only one I'm seeing a deadlock failure for in dynamic parameter updates. I could work around it by changing the API of the costmap publisher so it does not get reconstructed in the dynamic parameter callback, but I'd rather fix the core ROS 2 issue instead of just patching Nav2 above it.

[SteveMacenski]

Migration docs: ros-navigation/docs.nav2.org#712