From 5411947287e4752bb0de1dcc89ff8cc8d5f0fd50 Mon Sep 17 00:00:00 2001 From: "sewon.jeon" Date: Sun, 1 Mar 2026 16:40:37 +0900 Subject: [PATCH 1/4] feat: add LifecycleNode support to ManagedNitrosPublisher/Subscriber (fix #68) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Introduces `NitrosNodeInterfaces` — a `rclcpp::node_interfaces::NodeInterfaces` type alias — so that the entire NitrosPublisher/Subscriber stack accepts any node-like type (rclcpp::Node, rclcpp_lifecycle::LifecycleNode, or custom) without modification to downstream code. Key changes: - New `nitros_node_interfaces.hpp`: `NitrosNodeInterfaces` type alias + `MakeNitrosNodeInterfaces()` factory - `NitrosPublisherSubscriberBase`, `NitrosPublisher`, `NitrosSubscriber`: primary constructors accept `NitrosNodeInterfaces`; existing `rclcpp::Node &` constructors remain as backward-compatible delegating wrappers (no API break) - `NitrosFormatAgent` callbacks: use `rclcpp::create_publisher/subscription` free functions with `NitrosNodeInterfaces` instead of node member functions - `NitrosTypeManager`: adds `rclcpp::Logger`-based constructor, removing the mandatory `rclcpp::Node *` dependency - `ManagedNitrosPublisher/Subscriber`: template `` constructors accept any node pointer; `rclcpp::Node *` path continues to work unchanged - New lifecycle test: `NitrosEmptyLifecycleNode` composable component in `isaac_ros_managed_nitros/test/` demonstrates and validates LifecycleNode integration (requires GPU for full runtime test, mirrors existing tests) All linter checks (cpplint, flake8, uncrustify, pep257, lint_cmake, xmllint) pass. Runtime integration tests require GPU access, consistent with existing upstream tests. Co-Authored-By: Claude Sonnet 4.6 --- isaac_ros_managed_nitros/CMakeLists.txt | 24 +++ .../managed_nitros_publisher.hpp | 16 +- .../managed_nitros_subscriber.hpp | 16 +- isaac_ros_managed_nitros/package.xml | 3 + .../isaac_ros_nitros_lifecycle_test_pol.py | 97 ++++++++++++ .../test/src/nitros_empty_lifecycle_node.cpp | 90 +++++++++++ .../nitros_node_interfaces.hpp | 73 +++++++++ .../isaac_ros_nitros/nitros_publisher.hpp | 44 +++++- .../nitros_publisher_subscriber_base.hpp | 87 +++++++--- .../isaac_ros_nitros/nitros_subscriber.hpp | 30 +++- .../types/nitros_format_agent.hpp | 72 +++++---- .../types/nitros_type_manager.hpp | 7 +- isaac_ros_nitros/src/nitros_publisher.cpp | 149 ++++++++++++------ isaac_ros_nitros/src/nitros_subscriber.cpp | 122 +++++++++----- 14 files changed, 676 insertions(+), 154 deletions(-) create mode 100644 isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py create mode 100644 isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp create mode 100644 isaac_ros_nitros/include/isaac_ros_nitros/nitros_node_interfaces.hpp diff --git a/isaac_ros_managed_nitros/CMakeLists.txt b/isaac_ros_managed_nitros/CMakeLists.txt index 1ac64cc..a35cb00 100644 --- a/isaac_ros_managed_nitros/CMakeLists.txt +++ b/isaac_ros_managed_nitros/CMakeLists.txt @@ -59,6 +59,30 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + # NitrosEmptyLifecycleNode — validates ManagedNitrosPublisher with LifecycleNode (issue #68) + find_package(rclcpp_lifecycle REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + + add_library(nitros_empty_lifecycle_node SHARED + test/src/nitros_empty_lifecycle_node.cpp) + target_include_directories(nitros_empty_lifecycle_node PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include) + ament_target_dependencies(nitros_empty_lifecycle_node + isaac_ros_nitros + rclcpp_lifecycle + rclcpp_components) + set_target_properties(nitros_empty_lifecycle_node PROPERTIES + BUILD_WITH_INSTALL_RPATH TRUE + BUILD_RPATH_USE_ORIGIN TRUE + INSTALL_RPATH_USE_LINK_PATH TRUE) + rclcpp_components_register_nodes( + nitros_empty_lifecycle_node + "nvidia::isaac_ros::nitros::NitrosEmptyLifecycleNode") + install(TARGETS nitros_empty_lifecycle_node + DESTINATION lib) + + add_launch_test(test/isaac_ros_nitros_lifecycle_test_pol.py TIMEOUT "30") + endif() diff --git a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp index e31a683..4bb1f9c 100644 --- a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp +++ b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp @@ -33,6 +33,7 @@ #include "extensions/gxf_optimizer/core/optimizer.hpp" #include "extensions/gxf_optimizer/exporter/graph_types.hpp" +#include "isaac_ros_nitros/nitros_node_interfaces.hpp" #include "isaac_ros_nitros/nitros_publisher.hpp" #include "isaac_ros_nitros/types/nitros_type_manager.hpp" #include "isaac_ros_nitros/nitros_publisher_subscriber_group.hpp" @@ -51,15 +52,17 @@ template class ManagedNitrosPublisher { public: + template ManagedNitrosPublisher( - rclcpp::Node * node, + NodeT * node, const std::string & topic, const std::string & format, const NitrosDiagnosticsConfig & diagnostics_config = {}, const rclcpp::QoS qos = rclcpp::QoS(1)) - : node_{node}, + : node_ifaces_(MakeNitrosNodeInterfaces(*node)), context_{GetTypeAdapterNitrosContext()}, - nitros_type_manager_{std::make_shared(node_)} + nitros_type_manager_{std::make_shared( + node_ifaces_.get()->get_logger())} { nitros_type_manager_->registerSupportedType(); nitros_type_manager_->loadExtensions(format); @@ -74,13 +77,14 @@ class ManagedNitrosPublisher }; nitros_pub_ = std::make_shared( - *node_, GetTypeAdapterNitrosContext().getContext(), nitros_type_manager_, + node_ifaces_, GetTypeAdapterNitrosContext().getContext(), nitros_type_manager_, supported_data_formats, component_config, diagnostics_config); nitros_pub_->start(); RCLCPP_INFO( - node_->get_logger().get_child("ManagedNitrosPublisher"), + node_ifaces_.get()->get_logger().get_child( + "ManagedNitrosPublisher"), "Starting Managed Nitros Publisher"); } @@ -90,7 +94,7 @@ class ManagedNitrosPublisher } private: - rclcpp::Node * node_; + NitrosNodeInterfaces node_ifaces_; NitrosContext context_; std::shared_ptr nitros_type_manager_; std::shared_ptr nitros_pub_; diff --git a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp index c312b6c..db48958 100644 --- a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp +++ b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp @@ -23,6 +23,7 @@ #include #include "extensions/gxf_optimizer/core/optimizer.hpp" +#include "isaac_ros_nitros/nitros_node_interfaces.hpp" #include "isaac_ros_nitros/nitros_subscriber.hpp" #include "isaac_ros_nitros/types/nitros_type_manager.hpp" #include "rclcpp/rclcpp.hpp" @@ -39,15 +40,17 @@ template class ManagedNitrosSubscriber { public: + template explicit ManagedNitrosSubscriber( - rclcpp::Node * node, + NodeT * node, const std::string & topic_name, const std::string & format, std::function callback = nullptr, const NitrosDiagnosticsConfig & diagnostics_config = {}, const rclcpp::QoS qos = rclcpp::QoS(1)) - : node_{node}, topic_{topic_name}, - nitros_type_manager_{std::make_shared(node_)} + : node_ifaces_(MakeNitrosNodeInterfaces(*node)), topic_{topic_name}, + nitros_type_manager_{std::make_shared( + node_ifaces_.get()->get_logger())} { nitros_type_manager_->registerSupportedType(); nitros_type_manager_->loadExtensions(format); @@ -66,18 +69,19 @@ class ManagedNitrosSubscriber }; nitros_sub_ = std::make_shared( - *node_, GetTypeAdapterNitrosContext().getContext(), nitros_type_manager_, + node_ifaces_, GetTypeAdapterNitrosContext().getContext(), nitros_type_manager_, supported_data_formats, component_config, diagnostics_config); nitros_sub_->start(); RCLCPP_INFO( - node_->get_logger().get_child("ManagedNitrosSubscriber"), + node_ifaces_.get()->get_logger().get_child( + "ManagedNitrosSubscriber"), "Starting Managed Nitros Subscriber"); } private: - rclcpp::Node * node_; + NitrosNodeInterfaces node_ifaces_; std::string topic_; std::shared_ptr nitros_type_manager_; std::shared_ptr nitros_sub_; diff --git a/isaac_ros_managed_nitros/package.xml b/isaac_ros_managed_nitros/package.xml index 11661b3..a3fa547 100644 --- a/isaac_ros_managed_nitros/package.xml +++ b/isaac_ros_managed_nitros/package.xml @@ -37,6 +37,9 @@ license agreement from NVIDIA CORPORATION is strictly prohibited. ament_lint_auto ament_lint_common isaac_ros_test + rclcpp_lifecycle + launch_testing_ament_cmake + lifecycle_msgs ament_cmake diff --git a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py new file mode 100644 index 0000000..9d3086d --- /dev/null +++ b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py @@ -0,0 +1,97 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +""" +Launch test verifying that ManagedNitrosPublisher works inside a LifecycleNode. + +The test loads NitrosEmptyLifecycleNode in a component container, drives it +through the configure transition, and verifies the node reaches the 'inactive' +state without errors. +""" + +import time + +from isaac_ros_test import IsaacROSBaseTest +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import launch_testing +from lifecycle_msgs.msg import Transition +from lifecycle_msgs.srv import ChangeState +import pytest +import rclpy + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with NitrosEmptyLifecycleNode.""" + container = ComposableNodeContainer( + name='nitros_lifecycle_container', + namespace='', + package='rclcpp_components', + executable='component_container_mt', + composable_node_descriptions=[ + ComposableNode( + package='isaac_ros_managed_nitros', + plugin='nvidia::isaac_ros::nitros::NitrosEmptyLifecycleNode', + name='nitros_empty_lifecycle_node', + ), + ], + output='screen', + ) + + return generate_test_description.generate( + container, + launch_testing.actions.ReadyToTest(), + ) + + +generate_test_description.generate = lambda *actions: launch.LaunchDescription(list(actions)) + + +class TestNitrosLifecycle(IsaacROSBaseTest): + """Test that NitrosEmptyLifecycleNode transitions to 'inactive' state.""" + + package = 'isaac_ros_managed_nitros' + + def test_lifecycle_configure(self): + """Drive the lifecycle node through configure and verify success.""" + self.node.create_client( + ChangeState, + '/nitros_empty_lifecycle_node/change_state', + ) + change_state_client = self.node.create_client( + ChangeState, + '/nitros_empty_lifecycle_node/change_state', + ) + + # Wait for the service to be available + start = time.time() + available = change_state_client.wait_for_service(timeout_sec=10.0) + self.assertTrue( + available, + msg=f'ChangeState service not available after {time.time() - start:.1f}s', + ) + + # Send configure transition + request = ChangeState.Request() + request.transition.id = Transition.TRANSITION_CONFIGURE + future = change_state_client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + + self.assertIsNotNone(future.result(), 'ChangeState future returned None') + self.assertTrue(future.result().success, 'configure transition failed') diff --git a/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp b/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp new file mode 100644 index 0000000..e16fbed --- /dev/null +++ b/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include "isaac_ros_managed_nitros/managed_nitros_publisher.hpp" +#include "isaac_ros_nitros/types/nitros_empty.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace nitros +{ + +// Minimal lifecycle node that creates a ManagedNitrosPublisher in on_configure(). +// Validates that NitrosPublisher / ManagedNitrosPublisher work with LifecycleNode. +class NitrosEmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + explicit NitrosEmptyLifecycleNode(const rclcpp::NodeOptions & options) + : rclcpp_lifecycle::LifecycleNode("nitros_empty_lifecycle_node", options) + {} + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) override + { + nitros_pub_ = std::make_shared< + ManagedNitrosPublisher>( + this, + "lifecycle_pub_output", + nitros_empty::supported_type_name); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) override + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) override + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) override + { + nitros_pub_.reset(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State &) override + { + nitros_pub_.reset(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } + +private: + std::shared_ptr> nitros_pub_; +}; + +} // namespace nitros +} // namespace isaac_ros +} // namespace nvidia + +RCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::nitros::NitrosEmptyLifecycleNode) diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_node_interfaces.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_node_interfaces.hpp new file mode 100644 index 0000000..3e4b43c --- /dev/null +++ b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_node_interfaces.hpp @@ -0,0 +1,73 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_NITROS__NITROS_NODE_INTERFACES_HPP_ +#define ISAAC_ROS_NITROS__NITROS_NODE_INTERFACES_HPP_ + +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace nitros +{ + +/// @brief A "node-like" aggregate of the rclcpp node interfaces required by +/// NitrosPublisher / NitrosSubscriber. +/// +/// This type is constructible from any node-like object (rclcpp::Node, +/// rclcpp_lifecycle::LifecycleNode, or any type that provides the standard +/// get_node_xxx_interface() accessors). It can be passed directly to +/// rclcpp::create_publisher(), rclcpp::create_subscription(), +/// rclcpp::create_wall_timer(), and to the negotiated::NegotiatedPublisher / +/// NegotiatedSubscription template constructors. +using NitrosNodeInterfaces = rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, // get_name(), get_namespace(), + // create_callback_group() + rclcpp::node_interfaces::NodeClockInterface, // get_clock() + rclcpp::node_interfaces::NodeGraphInterface, // required by NegotiatedPublisher + rclcpp::node_interfaces::NodeLoggingInterface, // get_logger() + rclcpp::node_interfaces::NodeParametersInterface, // required by create_publisher/subscription + rclcpp::node_interfaces::NodeTimersInterface, // create_wall_timer() + rclcpp::node_interfaces::NodeTopicsInterface, // create_publisher / create_subscription + rclcpp::node_interfaces::NodeWaitablesInterface // add_waitable() +>; + +/// @brief Convenience factory that builds a NitrosNodeInterfaces from any node-like object. +/// +/// Accepts rclcpp::Node, rclcpp_lifecycle::LifecycleNode, or any type that +/// implements the standard rclcpp node interface accessors. +template +inline NitrosNodeInterfaces MakeNitrosNodeInterfaces(NodeT & node) +{ + return NitrosNodeInterfaces(node); +} + +} // namespace nitros +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_NITROS__NITROS_NODE_INTERFACES_HPP_ diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher.hpp index d5b1b3a..56a5809 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher.hpp @@ -57,7 +57,7 @@ class NitrosPublisherWaitable : public rclcpp::Waitable { public: NitrosPublisherWaitable( - rclcpp::Node & node, + std::string node_name, NitrosPublisher & nitros_publisher); size_t get_number_of_ready_guard_conditions() @@ -78,7 +78,7 @@ class NitrosPublisherWaitable : public rclcpp::Waitable void add_to_wait_set(rcl_wait_set_t * wait_set) override; private: - rclcpp::Node & node_; + std::string node_name_; // cached for NVTX profiling tags NitrosPublisher & nitros_publisher_; std::mutex guard_condition_mutex_; rclcpp::GuardCondition guard_condition_; @@ -92,7 +92,44 @@ class NitrosPublisherWaitable : public rclcpp::Waitable class NitrosPublisher : public NitrosPublisherSubscriberBase { public: - // Constructor + // Primary constructors (NitrosNodeInterfaces — accepts any node-like type) + NitrosPublisher( + NitrosNodeInterfaces node_ifaces, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const negotiated::NegotiatedPublisherOptions & negotiated_pub_options); + + NitrosPublisher( + NitrosNodeInterfaces node_ifaces, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const negotiated::NegotiatedPublisherOptions & negotiated_pub_options); + + NitrosPublisher( + NitrosNodeInterfaces node_ifaces, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const negotiated::NegotiatedPublisherOptions & negotiated_pub_options, + const NitrosDiagnosticsConfig & diagnostics_config); + + // Constructor for creating a publisher without an associated gxf egress port + NitrosPublisher( + NitrosNodeInterfaces node_ifaces, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const NitrosDiagnosticsConfig & diagnostics_config); + + // Backward-compatible constructors: delegate to NitrosNodeInterfaces versions NitrosPublisher( rclcpp::Node & node, std::shared_ptr nitros_type_manager, @@ -120,7 +157,6 @@ class NitrosPublisher : public NitrosPublisherSubscriberBase const negotiated::NegotiatedPublisherOptions & negotiated_pub_options, const NitrosDiagnosticsConfig & diagnostics_config); - // Constructor for creating a publisher without an associated gxf egress port NitrosPublisher( rclcpp::Node & node, const gxf_context_t context, diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp index 64cac67..0f444ac 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp @@ -26,16 +26,20 @@ #include #include #include +#include #include #include "extensions/gxf_optimizer/exporter/graph_types.hpp" #include "gxf/core/gxf.h" #include "gxf/std/timestamp.hpp" +#include "isaac_ros_nitros/nitros_node_interfaces.hpp" #include "isaac_ros_nitros/types/nitros_type_base.hpp" #include "isaac_ros_nitros/types/nitros_type_manager.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/key_value.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_timer.hpp" #include "rclcpp/rclcpp.hpp" namespace nvidia @@ -122,28 +126,49 @@ using NitrosPublisherSubscriberConfigMap = class NitrosPublisherSubscriberBase { public: - // Constructor + // Primary constructors (accept any node-like object via NitrosNodeInterfaces) NitrosPublisherSubscriberBase( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, const std::vector & supported_data_formats, const NitrosPublisherSubscriberConfig & config) - : node_(node), nitros_type_manager_(nitros_type_manager), + : node_ifaces_(std::move(node_ifaces)), nitros_type_manager_(nitros_type_manager), gxf_component_info_(gxf_component_info), supported_data_formats_(supported_data_formats), config_(config) {} NitrosPublisherSubscriberBase( - rclcpp::Node & node, const gxf_context_t context, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, const std::vector & supported_data_formats, const NitrosPublisherSubscriberConfig & config) - : node_(node), context_(context), + : node_ifaces_(std::move(node_ifaces)), context_(context), nitros_type_manager_(nitros_type_manager), gxf_component_info_(gxf_component_info), supported_data_formats_(supported_data_formats), config_(config) {} + // Backward-compatible constructors: delegate to NitrosNodeInterfaces versions + NitrosPublisherSubscriberBase( + rclcpp::Node & node, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config) + : NitrosPublisherSubscriberBase( + MakeNitrosNodeInterfaces(node), nitros_type_manager, + gxf_component_info, supported_data_formats, config) {} + + NitrosPublisherSubscriberBase( + rclcpp::Node & node, const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config) + : NitrosPublisherSubscriberBase( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, + gxf_component_info, supported_data_formats, config) {} + const NitrosPublisherSubscriberConfig getConfig() const {return config_;} // Getter for the GXF component info @@ -194,6 +219,24 @@ class NitrosPublisherSubscriberBase frame_id_map_ptr_ = frame_id_map_ptr; } + // Helper accessors (avoid repeating verbose node_ifaces_ lookups) + rclcpp::Logger get_node_logger() const + { + return node_ifaces_.get()->get_logger(); + } + const char * get_node_name() const + { + return node_ifaces_.get()->get_name(); + } + const char * get_node_namespace() const + { + return node_ifaces_.get()->get_namespace(); + } + std::shared_ptr get_node_clock() const + { + return node_ifaces_.get()->get_clock(); + } + uint64_t getTimestamp(NitrosTypeBase & base_msg) const { auto msg_entity = nvidia::gxf::Entity::Shared(context_, base_msg.handle); @@ -204,7 +247,7 @@ class NitrosPublisherSubscriberBase } } else { RCLCPP_FATAL( - node_.get_logger(), + get_node_logger(), "[NitrosPublisherSubscriberBase] Failed to resolve entity " "(eid=%ld) (error=%s)", base_msg.handle, GxfResultStr(msg_entity.error())); @@ -212,7 +255,7 @@ class NitrosPublisherSubscriberBase } RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisherSubscriberBase] Failed to get timestamp from a NITROS" " message (eid=%ld)", base_msg.handle); @@ -247,7 +290,7 @@ class NitrosPublisherSubscriberBase // Fix format security issue by using static format string RCLCPP_ERROR( - node_.get_logger(), + get_node_logger(), "[NitrosPublisherSubscriberBase] Specified compatible format \"%s\" was" " not listed in the supported format list: %s", config_.compatible_data_format.c_str(), @@ -283,7 +326,7 @@ class NitrosPublisherSubscriberBase // Increment jitter outlier count num_jitter_outliers_node_++; RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosDiagnostics Node Time]" " Difference of time between messages(%li) and expected time between" " messages(%i) is out of tolerance(%i) by %i for topic %s. Units " @@ -334,7 +377,7 @@ class NitrosPublisherSubscriberBase frames_dropped_since_last_pub_++; prev_drop_ts_ = clock_.now(); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosDiagnostics Message Timestamp]" " Difference of time between messages(%lu) and expected " "time between" @@ -380,7 +423,7 @@ class NitrosPublisherSubscriberBase num_non_increasing_msg_++; prev_noninc_msg_ts_ = clock_.now(); RCLCPP_WARN( - node_.get_logger(), + get_node_logger(), "[NitrosDiagnostics Message Timestamp Non Increasing]" " Message timestamp is not increasing. Current timestamp: " "%lu, Previous timestamp: %lu" @@ -429,7 +472,7 @@ class NitrosPublisherSubscriberBase } } - rclcpp::Time time_from_node = node_.get_clock()->now(); + rclcpp::Time time_from_node = get_node_clock()->now(); uint64_t ros_node_system_time_us = time_from_node.nanoseconds() / kMicrosecondsToNanoseconds; const uint64_t latency_wrt_current_timestamp_node_ms = @@ -574,8 +617,8 @@ class NitrosPublisherSubscriberBase { topic_name_ = config_.topic_name; diagnostic_msgs::msg::DiagnosticStatus topic_status; - topic_status.name = node_.get_name(); - topic_status.name.append(node_.get_namespace()); + topic_status.name = get_node_name(); + topic_status.name.append(get_node_namespace()); topic_status.name.append("/"); topic_status.name.append(config_.topic_name); topic_status.hardware_id = nvidiaID; @@ -605,18 +648,22 @@ class NitrosPublisherSubscriberBase outdated_msg_ = true; diagnostic_publisher_ = - node_.create_publisher( - "/diagnostics", 10); - diagnostics_publisher_timer_ = node_.create_wall_timer( + rclcpp::create_publisher( + node_ifaces_, "/diagnostics", rclcpp::QoS(10)); + diagnostics_publisher_timer_ = rclcpp::create_wall_timer( std::chrono::milliseconds( static_cast( 1000 / diagnostics_config_.diagnostics_publish_rate)), - [this]() -> void {publishDiagnostics();}); + [this]() -> void {publishDiagnostics();}, + nullptr, + node_ifaces_.get().get(), + node_ifaces_.get().get()); } protected: - // The parent ROS 2 node - rclcpp::Node & node_; + // Node interfaces — works with rclcpp::Node, rclcpp_lifecycle::LifecycleNode, or any + // type that satisfies the rclcpp node interface contracts. + NitrosNodeInterfaces node_ifaces_; // The parent GXF context gxf_context_t context_ = nullptr; diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp index 9c7a1f3..23e0c76 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp @@ -44,7 +44,35 @@ namespace nitros class NitrosSubscriber : public NitrosPublisherSubscriberBase { public: - // Constructor + // Primary constructors (NitrosNodeInterfaces — accepts any node-like type) + NitrosSubscriber( + NitrosNodeInterfaces node_ifaces, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const NitrosDiagnosticsConfig & diagnostics_config, + const bool use_callback_group = false); + + NitrosSubscriber( + NitrosNodeInterfaces node_ifaces, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config); + + // Constructor for creating a subscriber without an associated gxf ingress port + NitrosSubscriber( + NitrosNodeInterfaces node_ifaces, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const NitrosDiagnosticsConfig & diagnostics_config); + + // Backward-compatible constructors: delegate to NitrosNodeInterfaces versions NitrosSubscriber( rclcpp::Node & node, const gxf_context_t context, diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp index def988a..3ef3f0e 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp @@ -25,11 +25,14 @@ #include #include +#include "isaac_ros_nitros/nitros_node_interfaces.hpp" #include "isaac_ros_nitros/types/nitros_type_base.hpp" #include "negotiated/negotiated_publisher.hpp" #include "negotiated/negotiated_subscription.hpp" +#include "rclcpp/create_publisher.hpp" +#include "rclcpp/create_subscription.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/serialization.hpp" @@ -50,7 +53,7 @@ struct NitrosFormatCallbacks // Create a compatible publisher for T std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr & compatible_pub, const std::string & topic_name, const rclcpp::QoS & qos, @@ -60,7 +63,7 @@ struct NitrosFormatCallbacks // Add a compatible publisher of type T to a negotiated publisher std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_pub, std::shared_ptr compatible_pub, const double weight)> @@ -69,7 +72,7 @@ struct NitrosFormatCallbacks // Add T to a negotiated publisher as a supported format std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_pub, const double weight, const rclcpp::QoS & qos, @@ -79,7 +82,7 @@ struct NitrosFormatCallbacks // Publish a message of type T via a negotiated publisher std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_pub, NitrosTypeBase & base_msg)> negotiatedPublishCallback{nullptr}; @@ -87,7 +90,7 @@ struct NitrosFormatCallbacks // Publish a message of type T via a compatible publisher std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr compatible_pub, NitrosTypeBase & base_msg)> compatiblePublishCallback{nullptr}; @@ -97,7 +100,7 @@ struct NitrosFormatCallbacks // Create a compatible subscriber for T std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr & compatible_sub, const std::string & topic_name, const rclcpp::QoS & qos, @@ -109,7 +112,7 @@ struct NitrosFormatCallbacks // Remove a compatible subscriber of type T from a negotiated subscriber std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_sub, std::shared_ptr compatible_sub)> removeCompatibleSubscriberCallback {nullptr}; @@ -117,7 +120,7 @@ struct NitrosFormatCallbacks // Add a compatible subscriber of type T to a negotiated subscriber std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_sub, std::shared_ptr compatible_sub, const double weight)> @@ -126,7 +129,7 @@ struct NitrosFormatCallbacks // Add T to a negotiated subscriber as a supported format std::function< void( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_sub, const double weight, const rclcpp::QoS & qos, @@ -264,26 +267,28 @@ class NitrosFormatAgent // Publisher callbacks // Create a compatible publisher for T static void createCompatiblePublisherCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr & compatible_pub, const std::string & topic_name, const rclcpp::QoS & qos, const rclcpp::PublisherOptions & options) { - compatible_pub = node.create_publisher( + compatible_pub = rclcpp::create_publisher( + node_ifaces, topic_name, qos, options); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Created a compatible publisher (topic_name=%s)", compatible_pub->get_topic_name()); } // Add a compatible publisher of type T to a negotiated publisher static void addCompatiblePublisherCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_pub, std::shared_ptr compatible_pub, const double weight) @@ -296,14 +301,15 @@ class NitrosFormatAgent weight); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Added a compatible publisher (topic_name=%s) to a negotiated publisher", compatible_pub->get_topic_name()); } // Add T to a negotiated publisher as a supported format static void addPublisherSupportedFormatCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_pub, const double weight, const rclcpp::QoS & qos, @@ -315,14 +321,15 @@ class NitrosFormatAgent options); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Added a supported format \"%s\" to a negotiated publisher", T::supported_type_name.c_str()); } // Publish a message of type T via a negotiated publisher static void negotiatedPublishCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_pub, NitrosTypeBase & base_msg) { @@ -330,13 +337,14 @@ class NitrosFormatAgent negotiated_pub->publish(msg); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Published a message via the negotiated publisher"); } // Publish a message of type T via a compatible publisher static void compatiblePublishCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr compatible_pub, NitrosTypeBase & base_msg) { @@ -346,7 +354,8 @@ class NitrosFormatAgent cast_compatible_pub->publish(msg); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Published a message via the compatible publisher (topic_name=%s)", compatible_pub->get_topic_name()); } @@ -354,7 +363,7 @@ class NitrosFormatAgent // Subscriber callbacks // Create a compatible subscriber for T static void createCompatibleSubscriberCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr & compatible_sub, const std::string & topic_name, const rclcpp::QoS & qos, @@ -366,14 +375,16 @@ class NitrosFormatAgent &NitrosFormatAgent::subscriberCallback, std::placeholders::_1, subscriber_callback); - compatible_sub = node.create_subscription( + compatible_sub = rclcpp::create_subscription( + node_ifaces, topic_name, qos, internal_subscriber_callback, options); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Created a compatible subscriber (topic_name=%s, format_name=%s)", compatible_sub->get_topic_name(), T::supported_type_name.c_str()); @@ -381,7 +392,7 @@ class NitrosFormatAgent // Remove a compatible subscriber of type T from a negotiated subscriber static void removeCompatibleSubscriberCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_sub, std::shared_ptr compatible_sub) { @@ -391,14 +402,15 @@ class NitrosFormatAgent cast_compatible_sub, T::supported_type_name); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Removed a compatible subscriber (topic_name=%s) from a negotiated subscriber", compatible_sub->get_topic_name()); } // Add a compatible subscriber of type T to a negotiated subscriber static void addCompatibleSubscriberCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_sub, std::shared_ptr compatible_sub, const double weight) @@ -411,14 +423,15 @@ class NitrosFormatAgent weight); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Added a compatible subscriber (topic_name=%s) to a negotiated subscriber", compatible_sub->get_topic_name()); } // Add T to a negotiated subscriber as a supported format static void addSubscriberSupportedFormatCallback( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr negotiated_sub, const double weight, const rclcpp::QoS & qos, @@ -438,7 +451,8 @@ class NitrosFormatAgent options); RCLCPP_DEBUG( - node.get_logger().get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), + node_ifaces.get()->get_logger() + .get_child(LOGGER_SUFFIX).get_child(T::supported_type_name), "Added a supported format \"%s\" to a negotiated subscriber", T::supported_type_name.c_str()); } diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp index 6145a16..6112acb 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp @@ -49,6 +49,8 @@ class NitrosTypeManager { setNode(node); } + explicit NitrosTypeManager(rclcpp::Logger logger) + : logger_(logger) {} // Setter for node_ that is used to get ROS logger if set void setNode(const rclcpp::Node * node) @@ -169,12 +171,15 @@ class NitrosTypeManager if (node_ != nullptr) { return node_->get_logger(); } - return rclcpp::get_logger("NitrosTypeManager"); + return logger_; } // The associated ROS node (for logging purpose) const rclcpp::Node * node_ = nullptr; + // Fallback logger used when node_ is nullptr (e.g. when constructed from a Logger directly) + rclcpp::Logger logger_{rclcpp::get_logger("NitrosTypeManager")}; + // A map storing callback functions for each registered format std::map format_callback_map_; diff --git a/isaac_ros_nitros/src/nitros_publisher.cpp b/isaac_ros_nitros/src/nitros_publisher.cpp index a85550b..be12d39 100644 --- a/isaac_ros_nitros/src/nitros_publisher.cpp +++ b/isaac_ros_nitros/src/nitros_publisher.cpp @@ -36,10 +36,10 @@ namespace nitros { NitrosPublisherWaitable::NitrosPublisherWaitable( - rclcpp::Node & node, + std::string node_name, NitrosPublisher & nitros_publisher) : rclcpp::Waitable(), - node_(node), + node_name_(std::move(node_name)), nitros_publisher_(nitros_publisher) {} @@ -47,7 +47,7 @@ void NitrosPublisherWaitable::trigger() { std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosPublisherWaitable::trigger(" << + "[" << node_name_ << "] NitrosPublisherWaitable::trigger(" << nitros_publisher_.getConfig().topic_name << ")"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE); @@ -62,7 +62,7 @@ bool NitrosPublisherWaitable::is_ready(rcl_wait_set_t * wait_set) { std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosPublisherWaitable::is_ready(" << + "[" << node_name_ << "] NitrosPublisherWaitable::is_ready(" << nitros_publisher_.getConfig().topic_name << ")"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE); @@ -93,7 +93,7 @@ void NitrosPublisherWaitable::execute(std::shared_ptr & data) (void)data; // unused std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosPublisherWaitable::execute(" << + "[" << node_name_ << "] NitrosPublisherWaitable::execute(" << nitros_publisher_.getConfig().topic_name << ")"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_BLUE); @@ -115,14 +115,14 @@ void NitrosPublisherWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) } NitrosPublisher::NitrosPublisher( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, const std::vector & supported_data_formats, const NitrosPublisherSubscriberConfig & config, const negotiated::NegotiatedPublisherOptions & negotiated_pub_options) : NitrosPublisherSubscriberBase( - node, nitros_type_manager, gxf_component_info, supported_data_formats, config) + std::move(node_ifaces), nitros_type_manager, gxf_component_info, supported_data_formats, config) { // Bind the callback function to be used by a message relay in GXF graph gxf_message_relay_callback_func_ = std::bind( @@ -149,9 +149,9 @@ NitrosPublisher::NitrosPublisher( negotiated_pub_options; updated_negotiated_pub_options.negotiate_on_subscription_removal = false; - // Create a negotiated publisher object + // Create a negotiated publisher object (node_ifaces_ is node-like, works via template ctor) negotiated_pub_ = std::make_shared( - node_, + node_ifaces_, compatible_pub_->get_topic_name() + std::string("/nitros"), updated_negotiated_pub_options); @@ -173,7 +173,7 @@ NitrosPublisher::NitrosPublisher( } NitrosPublisher::NitrosPublisher( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, @@ -181,14 +181,14 @@ NitrosPublisher::NitrosPublisher( const NitrosPublisherSubscriberConfig & config, const negotiated::NegotiatedPublisherOptions & negotiated_pub_options) : NitrosPublisher( - node, nitros_type_manager, gxf_component_info, supported_data_formats, config, - negotiated_pub_options) + std::move(node_ifaces), nitros_type_manager, gxf_component_info, supported_data_formats, + config, negotiated_pub_options) { setContext(context); } NitrosPublisher::NitrosPublisher( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, @@ -197,8 +197,8 @@ NitrosPublisher::NitrosPublisher( const negotiated::NegotiatedPublisherOptions & negotiated_pub_options, const NitrosDiagnosticsConfig & diagnostics_config) : NitrosPublisher( - node, context, nitros_type_manager, gxf_component_info, supported_data_formats, config, - negotiated_pub_options) + std::move(node_ifaces), context, nitros_type_manager, gxf_component_info, + supported_data_formats, config, negotiated_pub_options) { diagnostics_config_ = diagnostics_config; @@ -213,17 +213,69 @@ NitrosPublisher::NitrosPublisher( } NitrosPublisher::NitrosPublisher( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const std::vector & supported_data_formats, const NitrosPublisherSubscriberConfig & config, const NitrosDiagnosticsConfig & diagnostics_config) : NitrosPublisher( - node, context, nitros_type_manager, {}, supported_data_formats, config, + std::move(node_ifaces), context, nitros_type_manager, {}, supported_data_formats, config, {}, diagnostics_config) {} +// Backward-compatible rclcpp::Node & constructors — delegate to NitrosNodeInterfaces versions +NitrosPublisher::NitrosPublisher( + rclcpp::Node & node, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const negotiated::NegotiatedPublisherOptions & negotiated_pub_options) +: NitrosPublisher( + MakeNitrosNodeInterfaces(node), nitros_type_manager, gxf_component_info, + supported_data_formats, config, negotiated_pub_options) +{} + +NitrosPublisher::NitrosPublisher( + rclcpp::Node & node, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const negotiated::NegotiatedPublisherOptions & negotiated_pub_options) +: NitrosPublisher( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, gxf_component_info, + supported_data_formats, config, negotiated_pub_options) +{} + +NitrosPublisher::NitrosPublisher( + rclcpp::Node & node, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const negotiated::NegotiatedPublisherOptions & negotiated_pub_options, + const NitrosDiagnosticsConfig & diagnostics_config) +: NitrosPublisher( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, gxf_component_info, + supported_data_formats, config, negotiated_pub_options, diagnostics_config) +{} + +NitrosPublisher::NitrosPublisher( + rclcpp::Node & node, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const NitrosDiagnosticsConfig & diagnostics_config) +: NitrosPublisher( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, supported_data_formats, + config, diagnostics_config) +{} + std::shared_ptr NitrosPublisher::getNegotiatedPublisher() { return negotiated_pub_; @@ -242,33 +294,33 @@ void NitrosPublisher::addSupportedDataFormat( "[NitrosPublisher] Could not identify the supported data foramt: " << "\"" << data_format.c_str() << "\""; RCLCPP_ERROR( - node_.get_logger(), error_msg.str().c_str()); + get_node_logger(), error_msg.str().c_str()); throw std::runtime_error(error_msg.str().c_str()); } if (data_format == config_.compatible_data_format) { nitros_type_manager_->getFormatCallbacks(data_format).addCompatiblePublisherCallback( - node_, + node_ifaces_, negotiated_pub_, compatible_pub_, weight ); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Added a compatible publisher: " "topic_name=\"%s\", data_format=\"%s\"", compatible_pub_->get_topic_name(), config_.compatible_data_format.c_str()); } else { nitros_type_manager_->getFormatCallbacks(data_format).addPublisherSupportedFormatCallback( - node_, + node_ifaces_, negotiated_pub_, weight, config_.qos, pub_options); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Added a supported data format: " "topic_name=\"%s\", data_format=\"%s\"", compatible_pub_->get_topic_name(), data_format.c_str()); @@ -293,13 +345,13 @@ void NitrosPublisher::createCompatiblePublisher() "[NitrosPublisher] Could not identify the compatible data foramt: " << "\"" << config_.compatible_data_format.c_str() << "\""; RCLCPP_ERROR( - node_.get_logger(), error_msg.str().c_str()); + get_node_logger(), error_msg.str().c_str()); throw std::runtime_error(error_msg.str().c_str()); } nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format). createCompatiblePublisherCallback( - node_, + node_ifaces_, compatible_pub_, config_.topic_name, config_.qos, @@ -317,17 +369,17 @@ void NitrosPublisher::postNegotiationCallback() if (!topics_info.success || topics_info.negotiated_topics.size() == 0) { negotiated_data_format_ = ""; RCLCPP_INFO( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Negotiation ended with no results"); RCLCPP_INFO( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Use only the compatible publisher: " "topic_name=\"%s\", data_format=\"%s\"", compatible_pub_->get_topic_name(), config_.compatible_data_format.c_str()); } else { negotiated_data_format_ = topics_info.negotiated_topics[0].supported_type_name; RCLCPP_INFO( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Use the negotiated data format: \"%s\"", negotiated_data_format_.c_str()); } @@ -353,18 +405,19 @@ void NitrosPublisher::setMessageRelayPointer(void * gxf_message_relay_ptr) void NitrosPublisher::startGxfVaultPeriodicPollingTimer() { // Set the Vault data polling timer - gxf_vault_periodic_polling_timer_ = node_.create_wall_timer( + gxf_vault_periodic_polling_timer_ = rclcpp::create_wall_timer( std::chrono::microseconds(100), - [this]() -> void { - extractMessagesFromGXF(); - }); + [this]() -> void {extractMessagesFromGXF();}, + nullptr, + node_ifaces_.get().get(), + node_ifaces_.get().get()); } void NitrosPublisher::enableNitrosPublisherWaitable() { - auto waitable_interfaces = node_.get_node_waitables_interface(); + auto waitable_interfaces = node_ifaces_.get(); waitable_ = std::make_shared( - node_, + get_node_name(), *this ); waitable_interfaces->add_waitable(waitable_, nullptr); @@ -374,7 +427,7 @@ void NitrosPublisher::extractMessagesFromGXF() { std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosPublisher::extractMessagesFromGXF(" << + "[" << get_node_name() << "] NitrosPublisher::extractMessagesFromGXF(" << config_.topic_name << ")"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_MAGENTA); @@ -388,7 +441,7 @@ void NitrosPublisher::extractMessagesFromGXF() auto msg_eids = gxf_vault_ptr_->store(100); if (msg_eids.size() > 0) { RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Obtained %ld messages from the vault", msg_eids.size()); } for (const auto msg_eid : msg_eids) { @@ -400,7 +453,7 @@ void NitrosPublisher::extractMessagesFromGXF() auto msg_eids = gxf_message_relay_ptr_->store(100); if (msg_eids.size() > 0) { RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Obtained %ld messages from the message relay for topic_name=\"%s\"", msg_eids.size(), config_.topic_name.c_str()); } @@ -415,7 +468,7 @@ void NitrosPublisher::extractMessagesFromGXF() void NitrosPublisher::gxfMessageRelayCallback() { - RCLCPP_DEBUG(node_.get_logger(), "[NitrosPublisher] gxfMessageRelayCallback"); + RCLCPP_DEBUG(get_node_logger(), "[NitrosPublisher] gxfMessageRelayCallback"); waitable_->trigger(); } @@ -426,7 +479,7 @@ void NitrosPublisher::publish(const int64_t handle) if (frame_id_map_ptr_->count(config_.frame_id_source_key) > 0) { frame_id = frame_id_map_ptr_->at(config_.frame_id_source_key); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Associating frame_id=\"%s\" to an Nitros-typed " "message (eid=%ld)", frame_id.c_str(), handle); } @@ -454,7 +507,7 @@ void NitrosPublisher::publish(const int64_t handle) void NitrosPublisher::publish(const int64_t handle, const std_msgs::msg::Header & ros_header) { RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Publishing an Nitros-typed message with timestamps updated (eid=%ld)", handle); @@ -480,7 +533,7 @@ void NitrosPublisher::publish(const int64_t handle, const std_msgs::msg::Header if (!is_timestamp_updated) { RCLCPP_ERROR( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Failed to update timestamps in a message entity (eid=%ld) as " "no Timestamp component was found", handle); @@ -494,7 +547,7 @@ void NitrosPublisher::publish( const std_msgs::msg::Header & ros_header) { RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Publishing an Nitros-typed message with timestamps updated (eid=%ld)", base_msg.handle); @@ -520,7 +573,7 @@ void NitrosPublisher::publish( if (!is_timestamp_updated) { RCLCPP_ERROR( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Failed to update timestamps in a message entity (eid=%ld) as " "no Timestamp component was found", base_msg.handle); @@ -534,7 +587,7 @@ void NitrosPublisher::publish(NitrosTypeBase & base_msg) { std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosPublisher::publish(" << + "[" << get_node_name() << "] NitrosPublisher::publish(" << config_.topic_name << ", t=" << getTimestamp(base_msg) << ")"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_PURPLE); @@ -545,13 +598,13 @@ void NitrosPublisher::publish(NitrosTypeBase & base_msg) { std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosPublisher::publish(" << + "[" << get_node_name() << "] NitrosPublisher::publish(" << config_.topic_name << ")::config_.callback"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_PURPLE); } RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Calling user-defined callback for an Nitros-typed " "message (eid=%ld)", base_msg.handle); config_.callback(context_, base_msg); @@ -566,12 +619,12 @@ void NitrosPublisher::publish(NitrosTypeBase & base_msg) } RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosPublisher] Publishing an Nitros-typed message (eid=%ld)", base_msg.handle); if (!negotiated_data_format_.empty()) { nitros_type_manager_->getFormatCallbacks(negotiated_data_format_).negotiatedPublishCallback( - node_, + node_ifaces_, negotiated_pub_, base_msg); } @@ -579,7 +632,7 @@ void NitrosPublisher::publish(NitrosTypeBase & base_msg) if (config_.compatible_data_format != negotiated_data_format_) { nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format). compatiblePublishCallback( - node_, + node_ifaces_, compatible_pub_, base_msg); } diff --git a/isaac_ros_nitros/src/nitros_subscriber.cpp b/isaac_ros_nitros/src/nitros_subscriber.cpp index d1f99ca..041cff5 100644 --- a/isaac_ros_nitros/src/nitros_subscriber.cpp +++ b/isaac_ros_nitros/src/nitros_subscriber.cpp @@ -35,7 +35,7 @@ constexpr uint32_t kGxfReceiverPushPollPeriodNs = 100000; // 100us constexpr uint32_t kGxfReceiverPushPollWarningPeriodLoopCount = 100000; // 10s NitrosSubscriber::NitrosSubscriber( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, @@ -44,7 +44,8 @@ NitrosSubscriber::NitrosSubscriber( const NitrosDiagnosticsConfig & diagnostics_config, const bool use_callback_group) : NitrosPublisherSubscriberBase( - node, context, nitros_type_manager, gxf_component_info, supported_data_formats, config), + std::move(node_ifaces), context, nitros_type_manager, + gxf_component_info, supported_data_formats, config), use_callback_group_{use_callback_group} { if (config_.type == NitrosPublisherSubscriberType::NOOP) { @@ -62,9 +63,11 @@ NitrosSubscriber::NitrosSubscriber( } if (use_callback_group_) { - callback_group_ = node_.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_ = + node_ifaces_.get()->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Created a MutuallyExclusive callback group"); } @@ -81,7 +84,7 @@ NitrosSubscriber::NitrosSubscriber( // Create a negotiated subscriber object negotiated_sub_ = std::make_shared( - node_, + node_ifaces_, compatible_sub_->get_topic_name() + std::string("/nitros")); // Add supported data formats (which also adds the compatible subscriber) @@ -95,29 +98,70 @@ NitrosSubscriber::NitrosSubscriber( } NitrosSubscriber::NitrosSubscriber( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const gxf::optimizer::ComponentInfo & gxf_component_info, const std::vector & supported_data_formats, const NitrosPublisherSubscriberConfig & config) : NitrosSubscriber( - node, context, nitros_type_manager, gxf_component_info, supported_data_formats, config, {}) + std::move(node_ifaces), context, nitros_type_manager, + gxf_component_info, supported_data_formats, config, {}) {} NitrosSubscriber::NitrosSubscriber( - rclcpp::Node & node, + NitrosNodeInterfaces node_ifaces, const gxf_context_t context, std::shared_ptr nitros_type_manager, const std::vector & supported_data_formats, const NitrosPublisherSubscriberConfig & config, const NitrosDiagnosticsConfig & diagnostics_config) : NitrosSubscriber( - node, context, nitros_type_manager, {}, supported_data_formats, config, diagnostics_config) + std::move(node_ifaces), context, nitros_type_manager, + {}, supported_data_formats, config, diagnostics_config) { use_gxf_receiver_ = false; } +// Backward-compatible rclcpp::Node & constructors — delegate to NitrosNodeInterfaces versions +NitrosSubscriber::NitrosSubscriber( + rclcpp::Node & node, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const NitrosDiagnosticsConfig & diagnostics_config, + const bool use_callback_group) +: NitrosSubscriber( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, + gxf_component_info, supported_data_formats, config, diagnostics_config, use_callback_group) +{} + +NitrosSubscriber::NitrosSubscriber( + rclcpp::Node & node, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const gxf::optimizer::ComponentInfo & gxf_component_info, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config) +: NitrosSubscriber( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, + gxf_component_info, supported_data_formats, config, {}) +{} + +NitrosSubscriber::NitrosSubscriber( + rclcpp::Node & node, + const gxf_context_t context, + std::shared_ptr nitros_type_manager, + const std::vector & supported_data_formats, + const NitrosPublisherSubscriberConfig & config, + const NitrosDiagnosticsConfig & diagnostics_config) +: NitrosSubscriber( + MakeNitrosNodeInterfaces(node), context, nitros_type_manager, + supported_data_formats, config, diagnostics_config) +{} + std::shared_ptr NitrosSubscriber::getNegotiatedSubscriber() { return negotiated_sub_; @@ -139,20 +183,20 @@ void NitrosSubscriber::addSupportedDataFormat( "[NitrosSubscriber] Could not identify the supported data foramt: " << "\"" << data_format.c_str() << "\""; RCLCPP_ERROR( - node_.get_logger(), error_msg.str().c_str()); + get_node_logger(), error_msg.str().c_str()); throw std::runtime_error(error_msg.str().c_str()); } if (data_format == config_.compatible_data_format) { nitros_type_manager_->getFormatCallbacks(data_format).addCompatibleSubscriberCallback( - node_, + node_ifaces_, negotiated_sub_, compatible_sub_, weight ); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Added a compatible subscriber: " "topic_name=\"%s\", data_format=\"%s\"", compatible_sub_->get_topic_name(), config_.compatible_data_format.c_str()); @@ -166,7 +210,7 @@ void NitrosSubscriber::addSupportedDataFormat( std::placeholders::_2); nitros_type_manager_->getFormatCallbacks(data_format).addSubscriberSupportedFormatCallback( - node_, + node_ifaces_, negotiated_sub_, weight, config_.qos, @@ -174,7 +218,7 @@ void NitrosSubscriber::addSupportedDataFormat( sub_options); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Added a supported data format: " "topic_name=\"%s\", data_format=\"%s\"", compatible_sub_->get_topic_name(), data_format.c_str()); @@ -207,7 +251,7 @@ void NitrosSubscriber::createCompatibleSubscriber() "[NitrosSubscriber] Could not identify the compatible data foramt: " << "\"" << config_.compatible_data_format.c_str() << "\""; RCLCPP_ERROR( - node_.get_logger(), error_msg.str().c_str()); + get_node_logger(), error_msg.str().c_str()); throw std::runtime_error(error_msg.str().c_str()); } @@ -221,7 +265,7 @@ void NitrosSubscriber::createCompatibleSubscriber() nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format). createCompatibleSubscriberCallback( - node_, + node_ifaces_, compatible_sub_, config_.topic_name, config_.qos, @@ -239,10 +283,10 @@ void NitrosSubscriber::postNegotiationCallback() auto topics_info = negotiated_sub_->get_negotiated_topics_info(); if (!topics_info.success || topics_info.negotiated_topics.size() == 0) { RCLCPP_INFO( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Negotiation ended with no results"); RCLCPP_INFO( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Use the compatible subscriber: " "topic_name=\"%s\", data_format=\"%s\"", compatible_sub_->get_topic_name(), config_.compatible_data_format.c_str()); @@ -251,7 +295,7 @@ void NitrosSubscriber::postNegotiationCallback() negotiated_data_format_ = topics_info.negotiated_topics[0].supported_type_name; RCLCPP_INFO( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Use the negotiated data format: \"%s\"", negotiated_data_format_.c_str()); @@ -259,12 +303,12 @@ void NitrosSubscriber::postNegotiationCallback() // Delete the compatible subscriber as we don't use it anymore nitros_type_manager_->getFormatCallbacks(config_.compatible_data_format). removeCompatibleSubscriberCallback( - node_, + node_ifaces_, negotiated_sub_, compatible_sub_); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Removed a compatible subscriber: " "topic_name=\"%s\", data_format=\"%s\"", compatible_sub_->get_topic_name(), config_.compatible_data_format.c_str()); @@ -283,7 +327,7 @@ void NitrosSubscriber::setReceiverPolicy(const size_t policy) { if (gxf_receiver_ptr_ == nullptr) { RCLCPP_ERROR( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "The underlying receiver pointer (\"%s\") is not set when setting its policy.", GenerateComponentKey(gxf_component_info_).c_str()); return; @@ -293,7 +337,7 @@ void NitrosSubscriber::setReceiverPolicy(const size_t policy) code = GxfParameterSetUInt64(context_, gxf_receiver_ptr_->cid(), "policy", policy); if (code != GXF_SUCCESS) { RCLCPP_ERROR( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "Failed to set policy for the underlying receiver (\"%s\"): %s", GenerateComponentKey(gxf_component_info_).c_str(), GxfResultStr(code)); @@ -301,7 +345,7 @@ void NitrosSubscriber::setReceiverPolicy(const size_t policy) } RCLCPP_DEBUG( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "Set policy to %zu for the underlying receiver (\"%s\")", policy, GenerateComponentKey(gxf_component_info_).c_str()); } @@ -311,7 +355,7 @@ void NitrosSubscriber::setReceiverCapacity(const size_t capacity) { if (gxf_receiver_ptr_ == nullptr) { RCLCPP_ERROR( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "The underlying receiver pointer (\"%s\") is not set when setting its capacity.", GenerateComponentKey(gxf_component_info_).c_str()); return; @@ -321,7 +365,7 @@ void NitrosSubscriber::setReceiverCapacity(const size_t capacity) code = GxfParameterSetUInt64(context_, gxf_receiver_ptr_->cid(), "capacity", capacity); if (code != GXF_SUCCESS) { RCLCPP_ERROR( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "Failed to set capacity for the underlying receiver (\"%s\"): %s", GenerateComponentKey(gxf_component_info_).c_str(), GxfResultStr(code)); @@ -329,7 +373,7 @@ void NitrosSubscriber::setReceiverCapacity(const size_t capacity) } RCLCPP_DEBUG( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "Set capacity to %zu for the underlying receiver (\"%s\")", capacity, GenerateComponentKey(gxf_component_info_).c_str()); } @@ -346,7 +390,7 @@ bool NitrosSubscriber::pushEntity(const int64_t eid, bool should_block) { if (loop_count > 0 && loop_count % kGxfReceiverPushPollWarningPeriodLoopCount == 0) { RCLCPP_WARN( - node_.get_logger().get_child(LOGGER_SUFFIX), + get_node_logger().get_child(LOGGER_SUFFIX), "%.1fs passed while waiting to push a message entity (eid=%ld) " "to the receiver %s", (loop_count * (kGxfReceiverPushPollPeriodNs / 1000000000.0)), @@ -358,7 +402,7 @@ bool NitrosSubscriber::pushEntity(const int64_t eid, bool should_block) code = GxfEntityGetStatus(context_, gxf_receiver_eid, &entity_status); if (code != GXF_SUCCESS) { RCLCPP_ERROR( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Failed to get the receiver entity (eid=%ld) status: %s", gxf_receiver_eid, GxfResultStr(code)); return false; @@ -371,7 +415,7 @@ bool NitrosSubscriber::pushEntity(const int64_t eid, bool should_block) GxfEntityNotifyEventType(context_, gxf_receiver_ptr_->eid(), GXF_EVENT_MESSAGE_SYNC); RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Pushed a message entity (eid=%ld) to " "the appliation", eid); @@ -384,7 +428,7 @@ void NitrosSubscriber::subscriberCallback( { std::stringstream nvtx_tag_name; nvtx_tag_name << - "[" << node_.get_name() << "] NitrosSubscriber::subscriberCallback(" << + "[" << get_node_name() << "] NitrosSubscriber::subscriberCallback(" << config_.topic_name << ", t=" << getTimestamp(msg_base) << ")"; nvtxRangePushWrapper(nvtx_tag_name.str().c_str(), CLR_PURPLE); @@ -399,21 +443,21 @@ void NitrosSubscriber::subscriberCallback( updateDiagnostics(getTimestamp(msg_base)); } - RCLCPP_DEBUG(node_.get_logger(), "[NitrosSubscriber] Received a Nitros-typed messgae"); - RCLCPP_DEBUG(node_.get_logger(), "[NitrosSubscriber] \teid: %ld", msg_base.handle); + RCLCPP_DEBUG(get_node_logger(), "[NitrosSubscriber] Received a Nitros-typed messgae"); + RCLCPP_DEBUG(get_node_logger(), "[NitrosSubscriber] \teid: %ld", msg_base.handle); RCLCPP_DEBUG( - node_.get_logger(), "[NitrosSubscriber] \tdata_format_name: %s", + get_node_logger(), "[NitrosSubscriber] \tdata_format_name: %s", data_format_name.c_str()); RCLCPP_DEBUG( - node_.get_logger(), "[NitrosSubscriber] \tmsg_base: %s", + get_node_logger(), "[NitrosSubscriber] \tmsg_base: %s", msg_base.data_format_name.c_str()); RCLCPP_DEBUG( - node_.get_logger(), "[NitrosSubscriber] \tReceiver's pointer: %p", + get_node_logger(), "[NitrosSubscriber] \tReceiver's pointer: %p", (void *)gxf_receiver_ptr_); if (use_gxf_receiver_ && (!is_gxf_running_ || gxf_receiver_ptr_ == nullptr)) { RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Received a message but the underlying GXF graph is" "not yet ready."); nvtxRangePopWrapper(); @@ -426,14 +470,14 @@ void NitrosSubscriber::subscriberCallback( (*frame_id_map_ptr_.get())[frame_id_source_key] = msg_base.frame_id; RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Updated frame_id=%s", (*frame_id_map_ptr_.get())[frame_id_source_key].c_str()); } if (config_.callback != nullptr) { RCLCPP_DEBUG( - node_.get_logger(), + get_node_logger(), "[NitrosSubscriber] Calling user-defined callback for an Nitros-typed " "message (eid=%ld)", msg_base.handle); config_.callback(context_, msg_base); From 378866429b4ae693a9aec92a0e6d7dcb0e1a7486 Mon Sep 17 00:00:00 2001 From: "sewon.jeon" Date: Sun, 1 Mar 2026 17:14:10 +0900 Subject: [PATCH 2/4] fixup: address code review feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Fix null callback crash: guard lambda in ManagedNitrosSubscriber before calling callback(view) when callback == nullptr (was silent UB) - Add ManagedNitrosSubscriber to lifecycle test node, exercising subscriber construction + cleanup in LifecycleNode context - Add test_lifecycle_configure_and_cleanup test method (configure → cleanup cycle verifies resource teardown path) - Refactor launch test to canonical IsaacROSBaseTest.generate_test_description pattern; remove duplicate create_client call; fix import order (flake8) - Update copyright years to 2022-2025 in all 7 modified files - Add NitrosEmptyView definition (via view factory macro) in test source to satisfy ManagedNitrosSubscriber::BaseType requirement Co-Authored-By: Claude Sonnet 4.6 --- .../managed_nitros_publisher.hpp | 2 +- .../managed_nitros_subscriber.hpp | 8 +- .../isaac_ros_nitros_lifecycle_test_pol.py | 77 ++++++++++++------- .../test/src/nitros_empty_lifecycle_node.cpp | 22 +++++- .../nitros_publisher_subscriber_base.hpp | 2 +- .../isaac_ros_nitros/nitros_subscriber.hpp | 2 +- .../types/nitros_format_agent.hpp | 2 +- .../types/nitros_type_manager.hpp | 2 +- isaac_ros_nitros/src/nitros_subscriber.cpp | 2 +- 9 files changed, 80 insertions(+), 39 deletions(-) diff --git a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp index 4bb1f9c..63accc1 100644 --- a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp +++ b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_publisher.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp index db48958..d571a06 100644 --- a/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp +++ b/isaac_ros_managed_nitros/include/isaac_ros_managed_nitros/managed_nitros_subscriber.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -63,8 +63,10 @@ class ManagedNitrosSubscriber .compatible_data_format = format, .topic_name = topic_name, .callback = [callback](const gxf_context_t, NitrosTypeBase & msg) -> void { - const NitrosMsgView view(*(static_cast(&msg))); - callback(view); + if (callback) { + const NitrosMsgView view(*(static_cast(&msg))); + callback(view); + } } }; diff --git a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py index 9d3086d..2b36f68 100644 --- a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py +++ b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py @@ -16,11 +16,11 @@ # SPDX-License-Identifier: Apache-2.0 """ -Launch test verifying that ManagedNitrosPublisher works inside a LifecycleNode. +Launch test verifying that ManagedNitrosPublisher/Subscriber work inside a LifecycleNode. -The test loads NitrosEmptyLifecycleNode in a component container, drives it -through the configure transition, and verifies the node reaches the 'inactive' -state without errors. +The test loads NitrosEmptyLifecycleNode in a component container, drives it through +the configure transition (which creates both a ManagedNitrosPublisher and a +ManagedNitrosSubscriber), and verifies the node transitions succeed. """ import time @@ -51,47 +51,68 @@ def generate_test_description(): name='nitros_empty_lifecycle_node', ), ], - output='screen', + output='both', ) - return generate_test_description.generate( + return TestNitrosLifecycle.generate_test_description([ container, - launch_testing.actions.ReadyToTest(), - ) - - -generate_test_description.generate = lambda *actions: launch.LaunchDescription(list(actions)) + launch.actions.TimerAction( + period=2.5, actions=[launch_testing.actions.ReadyToTest()]) + ]) class TestNitrosLifecycle(IsaacROSBaseTest): - """Test that NitrosEmptyLifecycleNode transitions to 'inactive' state.""" + """Test that NitrosEmptyLifecycleNode pub/sub construction works via lifecycle.""" package = 'isaac_ros_managed_nitros' - def test_lifecycle_configure(self): - """Drive the lifecycle node through configure and verify success.""" - self.node.create_client( + def _make_change_state_client(self): + """Return a ChangeState client for the lifecycle node.""" + return self.node.create_client( ChangeState, '/nitros_empty_lifecycle_node/change_state', ) - change_state_client = self.node.create_client( - ChangeState, - '/nitros_empty_lifecycle_node/change_state', + + def _send_transition(self, client, transition_id, timeout_sec=10.0): + """Send a lifecycle transition and return the result, or None on timeout.""" + available = client.wait_for_service(timeout_sec=timeout_sec) + if not available: + return None + request = ChangeState.Request() + request.transition.id = transition_id + future = client.call_async(request) + rclpy.spin_until_future_complete(self.node, future, timeout_sec=timeout_sec) + return future.result() + + def test_lifecycle_configure(self): + """Drive the lifecycle node through configure and verify success.""" + client = self._make_change_state_client() + + start = time.time() + self.assertTrue( + client.wait_for_service(timeout_sec=10.0), + msg=f'ChangeState service not available after {time.time() - start:.1f}s', ) - # Wait for the service to be available + result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) + + self.assertIsNotNone(result, 'ChangeState future returned None') + self.assertTrue(result.success, 'configure transition failed') + + def test_lifecycle_configure_and_cleanup(self): + """Configure then cleanup to verify pub/sub resource teardown.""" + client = self._make_change_state_client() + start = time.time() - available = change_state_client.wait_for_service(timeout_sec=10.0) self.assertTrue( - available, + client.wait_for_service(timeout_sec=10.0), msg=f'ChangeState service not available after {time.time() - start:.1f}s', ) - # Send configure transition - request = ChangeState.Request() - request.transition.id = Transition.TRANSITION_CONFIGURE - future = change_state_client.call_async(request) - rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) + configure_result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) + self.assertIsNotNone(configure_result, 'configure future returned None') + self.assertTrue(configure_result.success, 'configure transition failed') - self.assertIsNotNone(future.result(), 'ChangeState future returned None') - self.assertTrue(future.result().success, 'configure transition failed') + cleanup_result = self._send_transition(client, Transition.TRANSITION_CLEANUP) + self.assertIsNotNone(cleanup_result, 'cleanup future returned None') + self.assertTrue(cleanup_result.success, 'cleanup transition failed') diff --git a/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp b/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp index e16fbed..120bf10 100644 --- a/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp +++ b/isaac_ros_managed_nitros/test/src/nitros_empty_lifecycle_node.cpp @@ -19,7 +19,9 @@ #include #include "isaac_ros_managed_nitros/managed_nitros_publisher.hpp" +#include "isaac_ros_managed_nitros/managed_nitros_subscriber.hpp" #include "isaac_ros_nitros/types/nitros_empty.hpp" +#include "isaac_ros_nitros/types/nitros_type_view_factory.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" @@ -32,8 +34,15 @@ namespace isaac_ros namespace nitros { -// Minimal lifecycle node that creates a ManagedNitrosPublisher in on_configure(). -// Validates that NitrosPublisher / ManagedNitrosPublisher work with LifecycleNode. +// Minimal view wrapper for NitrosEmpty, required by ManagedNitrosSubscriber. +NITROS_TYPE_VIEW_FACTORY_BEGIN(NitrosEmpty) +NITROS_TYPE_VIEW_FACTORY_END(NitrosEmpty) + +void NitrosEmptyView::InitView() {} // NitrosEmpty carries no GXF components + +// Minimal lifecycle node that creates both a ManagedNitrosPublisher and a +// ManagedNitrosSubscriber in on_configure(). Validates that both types work +// correctly when owned by an rclcpp_lifecycle::LifecycleNode (issue #68). class NitrosEmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode { public: @@ -50,6 +59,12 @@ class NitrosEmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode "lifecycle_pub_output", nitros_empty::supported_type_name); + nitros_sub_ = std::make_shared< + ManagedNitrosSubscriber>( + this, + "lifecycle_sub_input", + nitros_empty::supported_type_name); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -69,6 +84,7 @@ class NitrosEmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode on_cleanup(const rclcpp_lifecycle::State &) override { nitros_pub_.reset(); + nitros_sub_.reset(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -76,11 +92,13 @@ class NitrosEmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode on_shutdown(const rclcpp_lifecycle::State &) override { nitros_pub_.reset(); + nitros_sub_.reset(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } private: std::shared_ptr> nitros_pub_; + std::shared_ptr> nitros_sub_; }; } // namespace nitros diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp index 0f444ac..9e25c97 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_publisher_subscriber_base.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp index 23e0c76..7b393cd 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/nitros_subscriber.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp index 3ef3f0e..fdc0717 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_format_agent.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp index 6112acb..11fc870 100644 --- a/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp +++ b/isaac_ros_nitros/include/isaac_ros_nitros/types/nitros_type_manager.hpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/isaac_ros_nitros/src/nitros_subscriber.cpp b/isaac_ros_nitros/src/nitros_subscriber.cpp index 041cff5..f28e8b5 100644 --- a/isaac_ros_nitros/src/nitros_subscriber.cpp +++ b/isaac_ros_nitros/src/nitros_subscriber.cpp @@ -1,5 +1,5 @@ // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES -// Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// Copyright (c) 2022-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From c9b9fbb97ea49d950ae718986989ca35ca3a8694 Mon Sep 17 00:00:00 2001 From: "sewon.jeon" Date: Sun, 1 Mar 2026 17:24:22 +0900 Subject: [PATCH 3/4] fixup: merge lifecycle tests into single ordered sequence MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Separating configure and cleanup into two test methods caused cross-test state leakage: the first method left the node in 'inactive', making the second method's configure call invalid (configure is only legal from 'unconfigured' in ROS 2 lifecycle semantics). Consolidate into one test_lifecycle_configure_and_cleanup that drives configure → cleanup → configure to verify both construction and teardown/re-creation of ManagedNitrosPublisher/Subscriber in a single deterministic sequence. Co-Authored-By: Claude Sonnet 4.6 --- .../isaac_ros_nitros_lifecycle_test_pol.py | 43 +++++++++++-------- 1 file changed, 25 insertions(+), 18 deletions(-) diff --git a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py index 2b36f68..c33b342 100644 --- a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py +++ b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py @@ -84,23 +84,23 @@ def _send_transition(self, client, transition_id, timeout_sec=10.0): rclpy.spin_until_future_complete(self.node, future, timeout_sec=timeout_sec) return future.result() - def test_lifecycle_configure(self): - """Drive the lifecycle node through configure and verify success.""" - client = self._make_change_state_client() - - start = time.time() - self.assertTrue( - client.wait_for_service(timeout_sec=10.0), - msg=f'ChangeState service not available after {time.time() - start:.1f}s', - ) - - result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) - - self.assertIsNotNone(result, 'ChangeState future returned None') - self.assertTrue(result.success, 'configure transition failed') - def test_lifecycle_configure_and_cleanup(self): - """Configure then cleanup to verify pub/sub resource teardown.""" + """ + Drive configure → cleanup → configure cycle in one test. + + A single test method avoids cross-test state leakage: both test methods + would share the same launched lifecycle node, so a standalone + 'test_configure' would leave the node in 'inactive', making a subsequent + 'configure' call invalid (configure is only valid from 'unconfigured'). + + Sequence: + 1. configure (unconfigured → inactive): constructs ManagedNitrosPublisher + and ManagedNitrosSubscriber via LifecycleNode + 2. cleanup (inactive → unconfigured): destroys pub/sub, verifies + on_cleanup() returns SUCCESS + 3. configure (unconfigured → inactive): re-constructs pub/sub, verifies + resources can be re-created after a cleanup + """ client = self._make_change_state_client() start = time.time() @@ -109,10 +109,17 @@ def test_lifecycle_configure_and_cleanup(self): msg=f'ChangeState service not available after {time.time() - start:.1f}s', ) + # Step 1: configure — creates publisher + subscriber configure_result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) - self.assertIsNotNone(configure_result, 'configure future returned None') - self.assertTrue(configure_result.success, 'configure transition failed') + self.assertIsNotNone(configure_result, 'configure (1st) future returned None') + self.assertTrue(configure_result.success, 'configure (1st) transition failed') + # Step 2: cleanup — destroys publisher + subscriber cleanup_result = self._send_transition(client, Transition.TRANSITION_CLEANUP) self.assertIsNotNone(cleanup_result, 'cleanup future returned None') self.assertTrue(cleanup_result.success, 'cleanup transition failed') + + # Step 3: configure again — verifies resources can be re-created + reconfigure_result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) + self.assertIsNotNone(reconfigure_result, 'configure (2nd) future returned None') + self.assertTrue(reconfigure_result.success, 'configure (2nd) transition failed') From e8f685fb224f16b7787a6d725683de1bf6965f12 Mon Sep 17 00:00:00 2001 From: "sewon.jeon" Date: Sun, 1 Mar 2026 17:45:21 +0900 Subject: [PATCH 4/4] test: extend lifecycle test to full state machine sequence MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Covers all five on_*() callbacks in one ordered test: configure → activate → deactivate → cleanup → configure → shutdown Previously only configure and cleanup were exercised. on_activate, on_deactivate, and on_shutdown had zero coverage. Co-Authored-By: Claude Sonnet 4.6 --- .../isaac_ros_nitros_lifecycle_test_pol.py | 76 ++++++++++++------- 1 file changed, 49 insertions(+), 27 deletions(-) diff --git a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py index c33b342..a8e3c70 100644 --- a/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py +++ b/isaac_ros_managed_nitros/test/isaac_ros_nitros_lifecycle_test_pol.py @@ -18,9 +18,13 @@ """ Launch test verifying that ManagedNitrosPublisher/Subscriber work inside a LifecycleNode. -The test loads NitrosEmptyLifecycleNode in a component container, drives it through -the configure transition (which creates both a ManagedNitrosPublisher and a -ManagedNitrosSubscriber), and verifies the node transitions succeed. +The test loads NitrosEmptyLifecycleNode in a component container and drives it through +the full lifecycle state machine: + + configure → activate → deactivate → cleanup → configure → shutdown + +This exercises every on_*() callback and verifies that ManagedNitrosPublisher and +ManagedNitrosSubscriber are constructed, used, and destroyed correctly at each stage. """ import time @@ -62,7 +66,7 @@ def generate_test_description(): class TestNitrosLifecycle(IsaacROSBaseTest): - """Test that NitrosEmptyLifecycleNode pub/sub construction works via lifecycle.""" + """Test the full lifecycle state machine of NitrosEmptyLifecycleNode.""" package = 'isaac_ros_managed_nitros' @@ -84,22 +88,25 @@ def _send_transition(self, client, transition_id, timeout_sec=10.0): rclpy.spin_until_future_complete(self.node, future, timeout_sec=timeout_sec) return future.result() - def test_lifecycle_configure_and_cleanup(self): + def test_lifecycle_full_sequence(self): """ - Drive configure → cleanup → configure cycle in one test. + Drive the complete lifecycle state machine in one ordered test. - A single test method avoids cross-test state leakage: both test methods - would share the same launched lifecycle node, so a standalone - 'test_configure' would leave the node in 'inactive', making a subsequent - 'configure' call invalid (configure is only valid from 'unconfigured'). + A single test method avoids cross-test state leakage (all test methods share + the same launched node; leaving the node in a non-unconfigured state would + make subsequent transitions invalid). Sequence: - 1. configure (unconfigured → inactive): constructs ManagedNitrosPublisher - and ManagedNitrosSubscriber via LifecycleNode - 2. cleanup (inactive → unconfigured): destroys pub/sub, verifies + 1. configure (unconfigured → inactive): constructs ManagedNitrosPublisher + and ManagedNitrosSubscriber + 2. activate (inactive → active): exercises on_activate() + 3. deactivate (active → inactive): exercises on_deactivate() + 4. cleanup (inactive → unconfigured): destroys pub/sub, verifies on_cleanup() returns SUCCESS - 3. configure (unconfigured → inactive): re-constructs pub/sub, verifies + 5. configure (unconfigured → inactive): re-constructs pub/sub, verifies resources can be re-created after a cleanup + 6. shutdown (inactive → finalized): destroys pub/sub via on_shutdown(), + verifies clean teardown from the inactive state """ client = self._make_change_state_client() @@ -110,16 +117,31 @@ def test_lifecycle_configure_and_cleanup(self): ) # Step 1: configure — creates publisher + subscriber - configure_result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) - self.assertIsNotNone(configure_result, 'configure (1st) future returned None') - self.assertTrue(configure_result.success, 'configure (1st) transition failed') - - # Step 2: cleanup — destroys publisher + subscriber - cleanup_result = self._send_transition(client, Transition.TRANSITION_CLEANUP) - self.assertIsNotNone(cleanup_result, 'cleanup future returned None') - self.assertTrue(cleanup_result.success, 'cleanup transition failed') - - # Step 3: configure again — verifies resources can be re-created - reconfigure_result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) - self.assertIsNotNone(reconfigure_result, 'configure (2nd) future returned None') - self.assertTrue(reconfigure_result.success, 'configure (2nd) transition failed') + result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) + self.assertIsNotNone(result, 'configure (1st) future returned None') + self.assertTrue(result.success, 'configure (1st) transition failed') + + # Step 2: activate — exercises on_activate() + result = self._send_transition(client, Transition.TRANSITION_ACTIVATE) + self.assertIsNotNone(result, 'activate future returned None') + self.assertTrue(result.success, 'activate transition failed') + + # Step 3: deactivate — exercises on_deactivate() + result = self._send_transition(client, Transition.TRANSITION_DEACTIVATE) + self.assertIsNotNone(result, 'deactivate future returned None') + self.assertTrue(result.success, 'deactivate transition failed') + + # Step 4: cleanup — destroys publisher + subscriber + result = self._send_transition(client, Transition.TRANSITION_CLEANUP) + self.assertIsNotNone(result, 'cleanup future returned None') + self.assertTrue(result.success, 'cleanup transition failed') + + # Step 5: configure again — verifies resources can be re-created after cleanup + result = self._send_transition(client, Transition.TRANSITION_CONFIGURE) + self.assertIsNotNone(result, 'configure (2nd) future returned None') + self.assertTrue(result.success, 'configure (2nd) transition failed') + + # Step 6: shutdown — destroys pub/sub via on_shutdown(), node enters finalized + result = self._send_transition(client, Transition.TRANSITION_INACTIVE_SHUTDOWN) + self.assertIsNotNone(result, 'shutdown future returned None') + self.assertTrue(result.success, 'shutdown transition failed')