Added Lifecycle Subscription wrapper

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<visualization_msgs::msg::MarkerArray>(
     "~/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<visualization_msgs::msg::MarkerArray>(
     "~/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<nav_msgs::msg::Path>(
     "~/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());

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)

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 MessageT, typename NodeT, typename CallbackT>
-typename nav2::Subscription<MessageT>::SharedPtr create_subscription(
-  const NodeT & node,
+// Function 1: For LifecycleNode (with lifecycle checking)
+template<typename MessageT, typename CallbackT>
+typename nav2::Subscription<MessageT>::SharedPtr
+create_subscription(
+  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & 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<MessageT, CallbackT>(
-    params_interface,
-    topics_interface,
-    topic_name,
-    qos,
-    std::forward<CallbackT>(callback),
-    createSubscriptionOptions(topic_name, allow_parameter_qos_overrides, callback_group));
+  (void)callback_group;
+  auto wrapped_callback = [node, callback = std::forward<CallbackT>(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<MessageT>(
+    topic_name, qos, wrapped_callback);
 }
 
+// Function 2: For plain Node (no lifecycle, always active)
+template<typename MessageT, typename CallbackT>
+typename nav2::Subscription<MessageT>::SharedPtr
+create_subscription(
+  const std::shared_ptr<rclcpp::Node> & 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<MessageT>(
+    topic_name, qos, std::forward<CallbackT>(callback));
+}
 /**
  * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions
  * @param node Node to create the publisher on

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 MessageT, typename CallbackT>
   typename nav2::Subscription<MessageT>::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<rclcpp_lifecycle::ManagedEntityInterface> 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<std::shared_ptr<rclcpp_lifecycle::ManagedEntityInterface>> managed_entities_;
+
 private:
   /**
    * @brief Get the enable_lifecycle_services parameter value from NodeOptions

nav2_ros_common/include/nav2_ros_common/subscription.hpp

@@ -17,15 +17,11 @@
 
 #include <memory>
 #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<typename MessageT>
 using Subscription = rclcpp::Subscription<MessageT>;
 

nav2_ros_common/test/test_actions.cpp

@@ -27,11 +27,11 @@ using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
 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<std_msgs::msg::Empty>(
       "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<std_msgs::msg::Empty>(
       "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<std_msgs::msg::Empty>(
       "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()

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 <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#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<std_msgs::msg::String>(
+      "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<std_msgs::msg::String>::SharedPtr sub_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  // Create node using shared_ptr
+  auto node = std::make_shared<TestNode>();
+
+  // Initialize when shared_ptr is created
+  node->initialize();
+
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+  return 0;
+}

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<nav2_msgs::msg::SpeedLimit>(
     "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;

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<nav_msgs::msg::Path>(
-    "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);

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<geometry_msgs::msg::Twist>(
     "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<geometry_msgs::msg::TwistStamped>(
     "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));