nav2_ros_common: add lifecycle-managed Subscription wrapper

.gitignore

@@ -70,3 +70,6 @@ Session.vim
 
 # Vim Temporary
 .netrwhist
+
+# CTest
+Testing/

.pre-commit-config.yaml

@@ -80,10 +80,10 @@ repos:
         entry: ament_uncrustify
     -   id: ament_xmllint
         name: ament_xmllint
-        description: Check XML markup using xmllint.
+        description: Check XML markup using xmllint (well-formedness only, no schema fetch).
         language: system
         types: [xml]
-        entry: ament_xmllint
+        entry: python3 tools/xmllint_wellformed_only
     -   id: ament_flake8
         name: ament_flake8
         description: Check Python code style using flake8.

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -164,7 +164,7 @@ class AmclNode : public nav2::LifecycleNode
   std::atomic<bool> first_map_received_{false};
   amcl_hyp_t * initial_pose_hyp_;
   std::recursive_mutex mutex_;
-  nav2::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
+  nav2::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
   struct Point2D { int32_t x; int32_t y; };
   static std::vector<Point2D> free_space_indices;
@@ -204,7 +204,7 @@ class AmclNode : public nav2::LifecycleNode
    * @brief Initialize pub subs of AMCL
    */
   void initPubSub();
-  nav2::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
+  nav2::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     initial_pose_sub_;
   nav2::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     pose_pub_;

nav2_amcl/src/amcl_node.cpp

@@ -100,6 +100,8 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Activating");
 
   // Lifecycle publishers must be explicitly activated
+  map_sub_->on_activate();
+  initial_pose_sub_->on_activate();
   pose_pub_->on_activate();
   particle_cloud_pub_->on_activate();
 
@@ -178,6 +180,8 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   active_ = false;
 
   // Lifecycle publishers must be explicitly deactivated
+  map_sub_->on_deactivate();
+  initial_pose_sub_->on_deactivate();
   pose_pub_->on_deactivate();
   particle_cloud_pub_->on_deactivate();
 

nav2_behavior_tree/test/plugins/action/test_check_stop_status_action.cpp

@@ -38,6 +38,7 @@ class CheckStopStatusTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
     std::chrono::milliseconds duration = 100ms;
     config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms";
     bt_node_ = std::make_shared<nav2_behavior_tree::CheckStopStatus>("check_stop_status", *config_);
+    odom_smoother_->on_activate();
   }
 
   void TearDown()

nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp

@@ -29,6 +29,8 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("test_is_battery_charging");
+    node_->configure();
+    node_->activate();
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     executor_->add_node(node_->get_node_base_interface());
     factory_ = std::make_shared<BT::BehaviorTreeFactory>();
@@ -58,6 +60,10 @@ class IsBatteryChargingConditionTestFixture : public ::testing::Test
     delete config_;
     config_ = nullptr;
     battery_pub_.reset();
+    if (node_) {
+      node_->deactivate();
+      node_->cleanup();
+    }
     node_.reset();
     factory_.reset();
     executor_.reset();

nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp

@@ -30,6 +30,8 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("test_is_battery_low");
+    node_->configure();
+    node_->activate();
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
     executor_->add_node(node_->get_node_base_interface());
     factory_ = std::make_shared<BT::BehaviorTreeFactory>();
@@ -59,6 +61,10 @@ class IsBatteryLowConditionTestFixture : public ::testing::Test
     delete config_;
     config_ = nullptr;
     battery_pub_.reset();
+    if (node_) {
+      node_->deactivate();
+      node_->cleanup();
+    }
     node_.reset();
     factory_.reset();
     executor_.reset();

nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp

@@ -33,6 +33,8 @@ class GoalUpdaterTestFixture : public ::testing::Test
   static void SetUpTestCase()
   {
     node_ = std::make_shared<nav2::LifecycleNode>("goal_updater_test_fixture");
+    node_->configure();
+    node_->activate();
     factory_ = std::make_shared<BT::BehaviorTreeFactory>();
 
     config_ = new BT::NodeConfiguration();
@@ -62,6 +64,10 @@ class GoalUpdaterTestFixture : public ::testing::Test
   {
     delete config_;
     config_ = nullptr;
+    if (node_) {
+      node_->deactivate();
+      node_->cleanup();
+    }
     node_.reset();
     factory_.reset();
   }

nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp

@@ -55,6 +55,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi
     bt_node_ = std::make_shared<nav2_behavior_tree::SpeedController>("speed_controller", *config_);
     dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>();
     bt_node_->setChild(dummy_node_.get());
+    odom_smoother_->on_activate();
   }
 
   void TearDown()

nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp

@@ -66,6 +66,16 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
    */
   CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
 
+  /**
+   * @brief Activate behavior (and lifecycle-managed subscriptions e.g. preempt_teleop)
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate behavior (and lifecycle-managed subscriptions)
+   */
+  void deactivate() override;
+
 protected:
   /**
    * @brief Configuration of behavior action

nav2_behaviors/plugins/assisted_teleop.cpp

@@ -54,6 +54,20 @@ void AssistedTeleop::onConfigure()
       this, std::placeholders::_1));
 }
 
+void AssistedTeleop::activate()
+{
+  TimedBehavior<AssistedTeleopAction>::activate();
+  vel_sub_->on_activate();
+  preempt_teleop_sub_->on_activate();
+}
+
+void AssistedTeleop::deactivate()
+{
+  preempt_teleop_sub_->on_deactivate();
+  vel_sub_->on_deactivate();
+  TimedBehavior<AssistedTeleopAction>::deactivate();
+}
+
 ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAction::Goal> command)
 {
   preempt_teleop_ = false;

nav2_behaviors/src/behavior_server.cpp

@@ -172,14 +172,28 @@ nav2::CallbackReturn
 BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
+
+  // Activate lifecycle-managed subscriptions so they receive /local_costmap/published_footprint
+  // and /global_costmap/published_footprint (and costmaps). Without this, footprint is never
+  // received and behaviors like AssistedTeleop wait forever for the collision checker.
+  if (local_costmap_sub_) {
+    local_costmap_sub_->on_activate();
+  }
+  if (global_costmap_sub_) {
+    global_costmap_sub_->on_activate();
+  }
+  if (local_footprint_sub_) {
+    local_footprint_sub_->on_activate();
+  }
+  if (global_footprint_sub_) {
+    global_footprint_sub_->on_activate();
+  }
+
   std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
   for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) {
     (*iter)->activate();
   }
-
-  // create bond connection
   createBond();
-
   return nav2::CallbackReturn::SUCCESS;
 }
 
@@ -193,6 +207,19 @@ BehaviorServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
     (*iter)->deactivate();
   }
 
+  if (local_costmap_sub_) {
+    local_costmap_sub_->on_deactivate();
+  }
+  if (global_costmap_sub_) {
+    global_costmap_sub_->on_deactivate();
+  }
+  if (local_footprint_sub_) {
+    local_footprint_sub_->on_deactivate();
+  }
+  if (global_footprint_sub_) {
+    global_footprint_sub_->on_deactivate();
+  }
+
   // destroy bond connection
   destroyBond();
 

nav2_bt_navigator/src/bt_navigator.cpp

@@ -133,6 +133,8 @@ nav2::CallbackReturn
 BtNavigator::on_activate(const rclcpp_lifecycle::State & state)
 {
   RCLCPP_INFO(get_logger(), "Activating");
+
+  odom_smoother_->on_activate();
   for (size_t i = 0; i != navigators_.size(); i++) {
     if (!navigators_[i]->on_activate()) {
       on_deactivate(state);
@@ -156,6 +158,8 @@ BtNavigator::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
     }
   }
 
+  odom_smoother_->on_deactivate();
+
   // destroy bond connection
   destroyBond();
 

nav2_collision_monitor/CMakeLists.txt

@@ -19,6 +19,11 @@ find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(point_cloud_transport REQUIRED)
 
+# Rolling point_cloud_transport prefers subscribe(..., rclcpp::QoS, ...); Jazzy/Kilted use rmw_qos_profile_t
+if(DEFINED ENV{ROS_DISTRO} AND "$ENV{ROS_DISTRO}" STREQUAL "rolling")
+  add_compile_definitions(POINT_CLOUD_TRANSPORT_SUBSCRIBE_USE_QOS)
+endif()
+
 nav2_package()
 
 set(monitor_executable_name collision_monitor)
@@ -33,6 +38,7 @@ add_library(${monitor_library_name} SHARED
   src/circle.cpp
   src/source.cpp
   src/scan.cpp
+  src/lifecycle_point_cloud_subscription.cpp
   src/pointcloud.cpp
   src/polygon_source.cpp
   src/range.cpp
@@ -70,6 +76,7 @@ add_library(${detector_library_name} SHARED
   src/circle.cpp
   src/source.cpp
   src/scan.cpp
+  src/lifecycle_point_cloud_subscription.cpp
   src/pointcloud.cpp
   src/polygon_source.cpp
   src/costmap.cpp

nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp

@@ -74,6 +74,15 @@ class Circle : public Polygon
    */
   bool isShapeSet() override;
 
+  /**
+   * @brief Activates circle subscription(s)
+   */
+  void activate() override;
+  /**
+   * @brief Deactivates circle subscription(s)
+   */
+  void deactivate() override;
+
 protected:
   /**
    * @brief Supporting routine obtaining polygon-specific ROS-parameters

nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp

@@ -80,6 +80,9 @@ class CostmapSource : public Source
    */
   void configure();
 
+  void activate() override;
+  void deactivate() override;
+
   /**
    * @brief Produce current obstacle points from the latest costmap.
    * @param curr_time Current time used for staleness checks and TF queries.

nav2_collision_monitor/include/nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) 2025 Angsa Robotics
+// Copyright (c) 2025 lotusymt
+//
+// 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.
+
+#ifndef NAV2_COLLISION_MONITOR__LIFECYCLE_POINT_CLOUD_SUBSCRIPTION_HPP_
+#define NAV2_COLLISION_MONITOR__LIFECYCLE_POINT_CLOUD_SUBSCRIPTION_HPP_
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "point_cloud_transport/point_cloud_transport.hpp"
+
+namespace nav2_collision_monitor
+{
+
+/**
+ * @brief Lifecycle-managed subscription that wraps point_cloud_transport::Subscriber.
+ *
+ * Gates message delivery on activation state (on_activate/on_deactivate), so it can be
+ * used like nav2::Subscription when using point_cloud_transport for transport hints.
+ */
+class LifecyclePointCloudSubscription : public rclcpp_lifecycle::SimpleManagedEntity
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(LifecyclePointCloudSubscription)
+
+  using Callback = std::function<void(sensor_msgs::msg::PointCloud2::ConstSharedPtr)>;
+
+  /**
+   * @brief Construct and create the PointCloudTransport.
+   * @param node Lifecycle node shared pointer (PointCloudTransport requires Node::SharedPtr on ROS 2 Jazzy).
+   */
+  explicit LifecyclePointCloudSubscription(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node);
+
+  ~LifecyclePointCloudSubscription();
+
+  /**
+   * @brief Subscribe to a topic with transport hints; callback is only invoked when activated.
+   * @param topic Topic name
+   * @param qos QoS profile
+   * @param callback User callback (invoked only when this entity is activated)
+   * @param transport_hints Transport hints for point_cloud_transport
+   */
+  void subscribe(
+    const std::string & topic,
+    const rclcpp::QoS & qos,
+    Callback callback,
+    const point_cloud_transport::TransportHints & transport_hints);
+
+  void on_activate() override;
+  void on_deactivate() override;
+
+  /** @brief Unsubscribe and release the underlying subscriber. */
+  void shutdown();
+
+private:
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  std::shared_ptr<point_cloud_transport::PointCloudTransport> pct_;
+  point_cloud_transport::Subscriber data_sub_;
+};
+
+}  // namespace nav2_collision_monitor
+
+#endif  // NAV2_COLLISION_MONITOR__LIFECYCLE_POINT_CLOUD_SUBSCRIPTION_HPP_