Hi all!
Its your Friendly Neighborhood Navigator here with a big Nav2 announcement.
We’ve recently merged in a large, mega-refactoring PR that impacts essentially every server, plugin, test, and user extension imaginable. Because of this, I think this is worth a special announcement to highlight this change, why it was done, and how users should migrate any custom task servers, plugins, or developer workflows.
Starting in Rolling today and Lyrical next year, all task servers now derive from a new nav2::LifecycleNode
class as part of the new nav2_ros_common
package. This package contains ROS abstractions such as LifecycleNode, Simple Action Server, Service Client, Service Server, Node Utilities and more that can be used in any ROS 2 user project, but shared among all Nav2 projects. This is a standalone, header only project that doesn’t depend on other Nav2 packages and can be used in user applications if they wish to leverage our ROS 2 interface wrappers and utilities.
Some of these capabilities were previously in the nav2_util
package that should be familiar to many of you, but others are brand new.
The new nav2::LifecycleNode
has its own create_publisher
, create_subscription
, create_service
, etc interface factories that return Nav2 objects rather than rclcpp
base types. This lets us design better software in Nav2 that leverages shared capabilities and configurations without putting all the details into the application code. For example, with the new Service and Action Introspection capability since Kilted, every Service Client, Service Server, Action Client, and Action server would need to have a block of boilerplate in the application code. Now, this can be defined once in the nav2_ros_common
package factory and users can simple create_client(...)
and have this handled for them. Similarly with topic QoS overrides and other advanced configurations we can add over time.
rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;
if (!node_parameters_interface->has_parameter("service_introspection_mode")) {
node_parameters_interface->declare_parameter(
"service_introspection_mode", rclcpp::ParameterValue("disabled"));
}
std::string service_introspection_mode =
node_parameters_interface->get_parameter("service_introspection_mode").as_string();
if (service_introspection_mode == "metadata") {
introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;
} else if (service_introspection_mode == "contents") {
introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;
}
ros_interface->configure_introspection(clock, rclcpp::ServicesQoS(), introspection_state);
Additionally, these create nav2::
types of ROS interfaces that come with nice simplified APIs to remove boilerplate code in applications. For example, the nav2::ServiceClient
has an invoke()
method that will send the async request and spin over the server’s node or an internal executor to obtain a result without having to specify all this code into your application:
auto now = clock_->now();
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
throw std::runtime_error(
service_name_ + " service client: interrupted while waiting for service");
}
RCLCPP_INFO(
logger_, "%s service client: waiting for service to appear...",
service_name_.c_str());
if (clock_->now() - now > wait_for_service_timeout) {
throw std::runtime_error(
service_name_ + " service client: timed out waiting for service");
}
}
RCLCPP_DEBUG(
logger_, "%s service client: send async request",
service_name_.c_str());
auto future_result = client_->async_send_request(request);
if (spin_until_complete(future_result, timeout) != rclcpp::FutureReturnCode::SUCCESS) {
// Pending request must be manually cleaned up if execution is interrupted or timed out
client_->remove_pending_request(future_result);
throw std::runtime_error(service_name_ + " service client: async_send_request failed");
}
auto response = future_result.get();
Now you can just do client_->invoke(request, timeout);
and obtain the same thing. Yay!
The various create_*
factories returned by the Nav2 LifecycleNode are listed below. Some of these existed before and should be familiar to Nav2 developers, some are new.
- create_client –> nav2::ServiceClient
- create_service –> nav2::ServiceServer
- create_publisher –> nav2::Publisher
- create_subscriber –> nav2::Subscriber
- create_action_server –> nav2::SimpleActionServer
- create_action_client –> nav2::ActionClient
Notably, the nav2::Publisher
, nav2::Subscription
, and nav2::ActionClient
are simply typedefs of the rclcpp_lifecycle
and rclcpp
varieties. However, over time we have features planned that will require non-trivial implementations of these objects so as part of a migration, you should update these as well so that when the type becomes more complex, your system is already supporting it.
The others are objects with their own ease of use APIs like the SimpleActionServer
, ServiceClient
and/or contain key configurations abstracted away from the application code. Thus, it is important to now use the factories even if you already use a nav2_util::ServiceClient
or nav2_util::SimpleActionServer
so that these exposed capabilities and utilities are included in your application. No longer should you make_shared<nav2::ServiceClient>(...)
but now node->create_client(...)
.
We also now have our own internal QoS profiles which are globally used in the stack:
nav2::qos::StandardTopicQoS
nav2::qos::LatchedPublisherQoS
nav2::qos::LatchedSubscriptionQoS
nav2::qos::SensorDataQoS
These provide us with two major benefits. One is an explicit policy set in the code that can be changed in 1 line and impact all publishers or subscriptions within the stack for testing and refinement. Second is simplifying the QoS from users, whereas we can add contextual profiles within Nav2 / autonomy stacks without needing them to be general purpose for all of ROS users (or manually populating rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable())
repeatedly for what is commonly known as Latched
in ROS 1). We also allow all of our new create_*
factories to not specify QoS profiles and default use the StandardTopicQoS
when not otherwise specified, which further simplifies application code from unnecessary middleware details for most on-CPU compute tasks.
Since this is a big change requiring an update to any custom plugins and task servers that use the nav2_util::LifecycleNode
, I have provided a migration guide in docs.nav2.org for this update and link the PR that migrates Nav2’s own servers as reference.
If you have any questions or concerns, please respond here and happy to chat! I know this is a very large and unusually breaking update, but it:
- Allows us easier adjustment of QoS profiles used globally in the stack from a single location
- Allows us to parameterize and expose advanced features like QoS overrides and service/action introspection without polluting application code
- Obtain Nav2-specific utilities designed to make services, actions, and pub/sub easier for users without as much boilerplate or manual calls,
spin_until_future_complete
and error checking - Add additional features across the stack, such as tracing or new advanced features easily
- Future-proof the migration so that additional features (lifecycle subscribers, new publisher features, etc) don’t require such a large migration effort in the future
I think these benefits outweigh the one-time migration cost for custom algorithms in user stacks. As always if you need professional support Open Navigation is happy to provide those if you reach out at info@opennav.org – but you should not require them for this migration.
Happy (or at least not too begrudgingly ) refactoring,
Steve Macenski
Open Navigation