diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 29e31c09d4c..860af8acbe9 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -45,7 +45,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( "~/candidate_trajectories", - [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + [&](const visualization_msgs::msg::MarkerArray::SharedPtr msg) {received_msg = *msg;}); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); @@ -109,7 +109,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( "~/candidate_trajectories", - [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + [&](const visualization_msgs::msg::MarkerArray::SharedPtr msg) {received_msg = *msg;}); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); @@ -142,7 +142,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) nav_msgs::msg::Path received_path; auto my_sub = node->create_subscription( "~/optimal_path", - [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + [&](const nav_msgs::msg::Path::SharedPtr msg) {received_path = *msg;}); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); diff --git a/nav2_ros_common/CMakeLists.txt b/nav2_ros_common/CMakeLists.txt index 4765454321d..f56c2c7cfc5 100644 --- a/nav2_ros_common/CMakeLists.txt +++ b/nav2_ros_common/CMakeLists.txt @@ -55,6 +55,15 @@ install(TARGETS ${PROJECT_NAME} ) ament_export_include_directories(include/${PROJECT_NAME}) +add_executable(test_lifecycle_sub test/test_lifecycle_sub.cpp) +ament_target_dependencies(test_lifecycle_sub + rclcpp + std_msgs + lifecycle_msgs + rclcpp_lifecycle +) +target_link_libraries(test_lifecycle_sub ${PROJECT_NAME}) +install(TARGETS test_lifecycle_sub DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp old mode 100644 new mode 100755 index 3f9f9db5131..ae1aba9d0aa --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -59,21 +59,17 @@ inline rclcpp::SubscriptionOptions createSubscriptionOptions( rclcpp::QOSLivelinessChangedCallbackType qos_liveliness_changed_callback = nullptr) { rclcpp::SubscriptionOptions options; - // Allow for all topics to have QoS overrides if (allow_parameter_qos_overrides) { options.qos_overriding_options = rclcpp::QosOverridingOptions( {rclcpp::QosPolicyKind::Depth, rclcpp::QosPolicyKind::Durability, rclcpp::QosPolicyKind::Reliability, rclcpp::QosPolicyKind::History}); } - // Set the callback group to use for this subscription, if given options.callback_group = callback_group_ptr; - // ROS 2 default logs this already options.event_callbacks.incompatible_qos_callback = requested_incompatible_qos_callback; options.event_callbacks.incompatible_type_callback = incompatible_qos_type_callback; - // Set the event callbacks if given, else log if (qos_message_lost_callback) { options.event_callbacks.message_lost_callback = qos_message_lost_callback; @@ -187,32 +183,45 @@ inline rclcpp::PublisherOptions createPublisherOptions( * @param node Node to create the subscription on * @param topic_name Name of topic * @param callback Callback function to handle incoming messages - * @param qos QoS settings for the subscription (default is nav2::qos::StandardTopicQoS()) + * @param qos QoS settings for the subscription (default is nav2:: qos::StandardTopicQoS()) * @param callback_group The callback group to use (if provided) - * @return A shared pointer to the created subscription + * @return A shared pointer to the created lifecycle-enabled subscription */ -template -typename nav2::Subscription::SharedPtr create_subscription( - const NodeT & node, +// Function 1: For LifecycleNode (with lifecycle checking) +template +typename nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, const std::string & topic_name, CallbackT && callback, const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) { - bool allow_parameter_qos_overrides = nav2::declare_or_get_parameter( - node, "allow_parameter_qos_overrides", true); - - auto params_interface = node->get_node_parameters_interface(); - auto topics_interface = node->get_node_topics_interface(); - return rclcpp::create_subscription( - params_interface, - topics_interface, - topic_name, - qos, - std::forward(callback), - createSubscriptionOptions(topic_name, allow_parameter_qos_overrides, callback_group)); + (void)callback_group; + auto wrapped_callback = [node, callback = std::forward(callback)] + (typename MessageT::SharedPtr msg) { + if (node->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + callback(msg); + } + }; + return node->rclcpp_lifecycle::LifecycleNode::template create_subscription( + topic_name, qos, wrapped_callback); } +// Function 2: For plain Node (no lifecycle, always active) +template +typename nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), + const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) +{ + (void)callback_group; + return node->template create_subscription( + topic_name, qos, std::forward(callback)); +} /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions * @param node Node to create the publisher on diff --git a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp old mode 100644 new mode 100755 index 3e67feec660..ff53ed7726f --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -27,6 +27,7 @@ #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "nav2_ros_common/node_thread.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" #include "rclcpp/rclcpp.hpp" #include "bondcpp/bond.hpp" #include "bond/msg/constants.hpp" @@ -145,11 +146,9 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode * @param callback Callback function to handle incoming messages * @param qos QoS settings for the subscription (default is nav2::qos::StandardTopicQoS()) * @param callback_group The callback group to use (if provided) - * @return A shared pointer to the created nav2::Subscription + * @return A shared pointer to the created nav2::LifecycleSubscription */ - template< - typename MessageT, - typename CallbackT> + template typename nav2::Subscription::SharedPtr create_subscription( const std::string & topic_name, @@ -369,6 +368,47 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode } } + // ADDED: New methods for lifecycle interface management + /** + * @brief Add a managed entity (publisher, subscription, etc.) to be lifecycle-managed + * @param entity Shared pointer to the managed entity + */ + void add_managed_entity(std::shared_ptr entity) + { + managed_entities_.push_back(entity); + } + + /** + * @brief Activate all managed interfaces in the correct order + * Order: Publishers -> Service Clients -> Action Clients -> + * Subscriptions -> Service Servers -> Action Servers + */ + void activateInterfaces() + { + RCLCPP_INFO(get_logger(), "Activating all managed interfaces for %s", get_name()); + for (auto & entity : managed_entities_) { + if (entity) { + entity->on_activate(); + } + } + } + + /** + * @brief Deactivate all managed interfaces in reverse order + * Order: Action Servers -> Service Servers -> Subscriptions -> + * Action Clients -> Service Clients -> Publishers + */ + void deactivateInterfaces() + { + RCLCPP_INFO(get_logger(), "Deactivating all managed interfaces for %s", get_name()); + // Deactivate in reverse order + for (auto it = managed_entities_.rbegin(); it != managed_entities_.rend(); ++it) { + if (*it) { + (*it)->on_deactivate(); + } + } + } + protected: /** * @brief Print notifications for lifecycle node @@ -427,6 +467,9 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode double bond_heartbeat_period{0.1}; rclcpp::TimerBase::SharedPtr autostart_timer_; + // ADDED: Vector to store all managed entities (publishers, subscriptions, etc.) + std::vector> managed_entities_; + private: /** * @brief Get the enable_lifecycle_services parameter value from NodeOptions diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp old mode 100644 new mode 100755 index b6b5bf64f0c..1dc99e2d2e7 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -17,15 +17,11 @@ #include #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" namespace nav2 { -/** - * @brief A ROS 2 subscription for Nav2 - * This is a convenience type alias to simplify the use of subscriptions in Nav2 - * which may be further built up on in the future with custom APIs. - */ template using Subscription = rclcpp::Subscription; diff --git a/nav2_ros_common/test/test_actions.cpp b/nav2_ros_common/test/test_actions.cpp index 5bda2e7b891..e023b5fd035 100644 --- a/nav2_ros_common/test/test_actions.cpp +++ b/nav2_ros_common/test/test_actions.cpp @@ -27,11 +27,11 @@ using GoalHandle = rclcpp_action::ServerGoalHandle; using std::placeholders::_1; using namespace std::chrono_literals; -class FibonacciServerNode : public rclcpp::Node +class FibonacciServerNode : public nav2::LifecycleNode { public: FibonacciServerNode() - : rclcpp::Node("fibonacci_server_node") + : nav2::LifecycleNode("fibonacci_server_node") { } @@ -48,27 +48,27 @@ class FibonacciServerNode : public rclcpp::Node deactivate_subs_ = create_subscription( "deactivate_server", - 1, - [this](std_msgs::msg::Empty::UniquePtr /*msg*/) { + [this](std_msgs::msg::Empty::SharedPtr /*msg*/) { RCLCPP_INFO(this->get_logger(), "Deactivating"); action_server_->deactivate(); - }); + }, + rclcpp::QoS(1)); activate_subs_ = create_subscription( "activate_server", - 1, - [this](std_msgs::msg::Empty::UniquePtr /*msg*/) { + [this](std_msgs::msg::Empty::SharedPtr /*msg*/) { RCLCPP_INFO(this->get_logger(), "Activating"); action_server_->activate(); - }); + }, + rclcpp::QoS(1)); omit_preempt_subs_ = create_subscription( "omit_preemption", - 1, - [this](std_msgs::msg::Empty::UniquePtr /*msg*/) { + [this](std_msgs::msg::Empty::SharedPtr /*msg*/) { RCLCPP_INFO(this->get_logger(), "Ignoring preemptions"); do_premptions_ = false; - }); + }, + rclcpp::QoS(1)); } void on_term() diff --git a/nav2_ros_common/test/test_lifecycle_sub.cpp b/nav2_ros_common/test/test_lifecycle_sub.cpp new file mode 100755 index 00000000000..38176529f17 --- /dev/null +++ b/nav2_ros_common/test/test_lifecycle_sub.cpp @@ -0,0 +1,76 @@ +// Copyright (c) 2025 Open Navigation LLC +// +// 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, +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "nav2_ros_common/lifecycle_node.hpp" + +class TestNode : public nav2::LifecycleNode +{ +public: + TestNode() + : nav2::LifecycleNode("test_lifecycle_sub") + { + RCLCPP_INFO(get_logger(), "Test node created"); + } + + void initialize() + { + RCLCPP_INFO(get_logger(), "Creating lifecycle subscription"); + + sub_ = create_subscription( + "test_topic", + [this](const std_msgs::msg::String::SharedPtr msg) { + RCLCPP_INFO(get_logger(), "Received: %s", msg->data.c_str()); + }); + + RCLCPP_INFO(get_logger(), "Subscription created"); + } + +protected: + nav2::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override + { + RCLCPP_INFO(get_logger(), "Configuring"); + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override + { + RCLCPP_INFO(get_logger(), "Activating - subscription will now process messages"); + return nav2::CallbackReturn::SUCCESS; + } + + nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override + { + RCLCPP_INFO(get_logger(), "Deactivating - subscription will ignore messages"); + return nav2::CallbackReturn::SUCCESS; + } + +private: + nav2::Subscription::SharedPtr sub_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + // Create node using shared_ptr + auto node = std::make_shared(); + + // Initialize when shared_ptr is created + node->initialize(); + + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +} diff --git a/nav2_route/test/test_operations.cpp b/nav2_route/test/test_operations.cpp index 3b35fea6b96..789391b6644 100644 --- a/nav2_route/test/test_operations.cpp +++ b/nav2_route/test/test_operations.cpp @@ -136,7 +136,7 @@ TEST(OperationsManagerTest, test_processing_speed_on_status) nav2_msgs::msg::SpeedLimit my_msg; auto sub = node->create_subscription( "speed_limit", - [&, this](nav2_msgs::msg::SpeedLimit msg) {got_msg = true; my_msg = msg;}); + [&, this](nav2_msgs::msg::SpeedLimit::SharedPtr msg) {got_msg = true; my_msg = *msg;}); Node node2; DirectionalEdge enter; diff --git a/nav2_route/test/test_path_converter.cpp b/nav2_route/test/test_path_converter.cpp index d8af7736cc8..cc2428435a2 100644 --- a/nav2_route/test/test_path_converter.cpp +++ b/nav2_route/test/test_path_converter.cpp @@ -39,7 +39,7 @@ TEST(PathConverterTest, test_path_converter_api) nav_msgs::msg::Path path_msg; auto sub = node->create_subscription( - "plan", [&, this](nav_msgs::msg::Path msg) {path_msg = msg;}); + "plan", [&, this](const nav_msgs::msg::Path::SharedPtr msg) {path_msg = *msg;}); PathConverter converter; converter.configure(node); diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index 0b716d20623..1ef1cabb869 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -46,7 +46,7 @@ TEST(TwistPublisher, Unstamped) geometry_msgs::msg::Twist sub_msg {}; auto my_sub = sub_node->create_subscription( "cmd_vel", - [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); + [&](const geometry_msgs::msg::Twist::SharedPtr msg) {sub_msg = *msg;}); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(sub_node->get_node_base_interface()); vel_publisher->publish(std::move(pub_msg)); @@ -86,7 +86,7 @@ TEST(TwistPublisher, Stamped) geometry_msgs::msg::TwistStamped sub_msg {}; auto my_sub = sub_node->create_subscription( "cmd_vel", - [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}); + [&](const geometry_msgs::msg::TwistStamped::SharedPtr msg) {sub_msg = *msg;}); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(sub_node->get_node_base_interface()); vel_publisher->publish(std::move(pub_msg)); diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp index f4043f899d9..3761eb00c9c 100644 --- a/nav2_util/test/test_twist_subscriber.cpp +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -34,8 +34,8 @@ TEST(TwistSubscriber, Unstamped) geometry_msgs::msg::TwistStamped sub_msg {}; auto vel_subscriber = std::make_unique( sub_node, "cmd_vel", - [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, - [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}, + [&](const geometry_msgs::msg::Twist::SharedPtr msg) {sub_msg.twist = *msg;}, + [&](const geometry_msgs::msg::TwistStamped::SharedPtr msg) {sub_msg = *msg;}, rclcpp::QoS(1) ); @@ -73,8 +73,8 @@ TEST(TwistSubscriber, Stamped) geometry_msgs::msg::TwistStamped sub_msg {}; auto vel_subscriber = std::make_unique( sub_node, "cmd_vel", - [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, - [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} + [&](const geometry_msgs::msg::Twist::SharedPtr msg) {sub_msg.twist = *msg;}, + [&](const geometry_msgs::msg::TwistStamped::SharedPtr msg) {sub_msg = *msg;} ); auto pub_node = std::make_shared("pub_node", "");