diff --git a/.gitignore b/.gitignore index bb346f49ba9..06d55d44341 100644 --- a/.gitignore +++ b/.gitignore @@ -70,3 +70,6 @@ Session.vim # Vim Temporary .netrwhist + +# CTest +Testing/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e8e1df2006d..f07d22ec3c4 100644 --- a/.pre-commit-config.yaml +++ b/.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. diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 97ba18138f0..3dae11cce02 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -164,7 +164,7 @@ class AmclNode : public nav2::LifecycleNode std::atomic first_map_received_{false}; amcl_hyp_t * initial_pose_hyp_; std::recursive_mutex mutex_; - nav2::Subscription::ConstSharedPtr map_sub_; + nav2::Subscription::SharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING struct Point2D { int32_t x; int32_t y; }; static std::vector free_space_indices; @@ -204,7 +204,7 @@ class AmclNode : public nav2::LifecycleNode * @brief Initialize pub subs of AMCL */ void initPubSub(); - nav2::Subscription::ConstSharedPtr + nav2::Subscription::SharedPtr initial_pose_sub_; nav2::Publisher::SharedPtr pose_pub_; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 6d6ae171d6d..41aa71723e4 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/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(); diff --git a/nav2_behavior_tree/test/plugins/action/test_check_stop_status_action.cpp b/nav2_behavior_tree/test/plugins/action/test_check_stop_status_action.cpp index 1201778a8e0..bde351970ea 100644 --- a/nav2_behavior_tree/test/plugins/action/test_check_stop_status_action.cpp +++ b/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("check_stop_status", *config_); + odom_smoother_->on_activate(); } void TearDown() diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp index 9733012e952..cbce4f7eba0 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp +++ b/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("test_is_battery_charging"); + node_->configure(); + node_->activate(); executor_ = std::make_shared(); executor_->add_node(node_->get_node_base_interface()); factory_ = std::make_shared(); @@ -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(); diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp index 86affc16424..c115221918c 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_battery_low.cpp +++ b/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("test_is_battery_low"); + node_->configure(); + node_->activate(); executor_ = std::make_shared(); executor_->add_node(node_->get_node_base_interface()); factory_ = std::make_shared(); @@ -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(); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 33cfeffc5e7..0677979b773 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/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("goal_updater_test_fixture"); + node_->configure(); + node_->activate(); factory_ = std::make_shared(); 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(); } diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 819e3465cb1..4b07b515c91 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/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("speed_controller", *config_); dummy_node_ = std::make_shared(); bt_node_->setChild(dummy_node_.get()); + odom_smoother_->on_activate(); } void TearDown() diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp index 8890dbfc886..5e4d0fb379b 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp @@ -66,6 +66,16 @@ class AssistedTeleop : public TimedBehavior */ 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 diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 7b59c616ed0..298a6a4e3a4 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -54,6 +54,20 @@ void AssistedTeleop::onConfigure() this, std::placeholders::_1)); } +void AssistedTeleop::activate() +{ + TimedBehavior::activate(); + vel_sub_->on_activate(); + preempt_teleop_sub_->on_activate(); +} + +void AssistedTeleop::deactivate() +{ + preempt_teleop_sub_->on_deactivate(); + vel_sub_->on_deactivate(); + TimedBehavior::deactivate(); +} + ResultStatus AssistedTeleop::onRun(const std::shared_ptr command) { preempt_teleop_ = false; diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index e4e7fa218d4..3e48e51d6bf 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/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>::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(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 95d3c7dcaa7..e1cacbbf9ea 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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(); diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index f20cfafd268..07891d40a3c 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/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 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index f9eb52e8023..717ed4f236c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/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 diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp index 346841b8805..f37cdfbce36 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/costmap.hpp +++ b/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. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp new file mode 100644 index 00000000000..f878d3bd59b --- /dev/null +++ b/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 +#include +#include + +#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; + + /** + * @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 node_; + std::shared_ptr pct_; + point_cloud_transport::Subscriber data_sub_; +}; + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__LIFECYCLE_POINT_CLOUD_SUBSCRIPTION_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index 1e9deebffa6..35fc927ed5b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -19,8 +19,14 @@ #include #include +#include "rclcpp/version.h" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "point_cloud_transport/point_cloud_transport.hpp" + +#if defined(RCLCPP_VERSION_MAJOR) && RCLCPP_VERSION_MAJOR >= 30 +#include "nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp" +#else +#include "nav2_ros_common/subscription.hpp" +#endif #include "nav2_collision_monitor/source.hpp" @@ -65,6 +71,9 @@ class PointCloud : public Source */ void configure(); + void activate() override; + void deactivate() override; + /** * @brief Adds latest data from pointcloud source to the data array. * @param curr_time Current node time for data interpolation @@ -110,10 +119,9 @@ class PointCloud : public Source // ----- Variables ----- - /// @brief PointCloud data subscriber - #if RCLCPP_VERSION_GTE(30, 0, 0) - std::shared_ptr pct_; - point_cloud_transport::Subscriber data_sub_; + /// @brief PointCloud data subscriber (lifecycle-managed; 30+ uses point_cloud_transport wrapper) + #if defined(RCLCPP_VERSION_MAJOR) && RCLCPP_VERSION_MAJOR >= 30 + LifecyclePointCloudSubscription::SharedPtr data_sub_; #else nav2::Subscription::SharedPtr data_sub_; #endif diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 64065a29058..ac2789c886d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -72,11 +72,11 @@ class Polygon /** * @brief Activates polygon lifecycle publisher */ - void activate(); + virtual void activate(); /** * @brief Deactivates polygon lifecycle publisher */ - void deactivate(); + virtual void deactivate(); /** * @brief Returns the name of polygon diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp index a25c60030e5..3b48431cbe3 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp @@ -65,6 +65,9 @@ class PolygonSource : public Source */ void configure(); + void activate() override; + void deactivate() override; + /** * @brief Adds latest data from polygon source to the data array. * @param curr_time Current node time for data interpolation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 763fe34d451..72dc960a81f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -64,6 +64,9 @@ class Range : public Source */ void configure(); + void activate() override; + void deactivate() override; + /** * @brief Adds latest data from range sensor to the data array. * @param curr_time Current node time for data interpolation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index ed941d19c49..4b994e918e6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -64,6 +64,9 @@ class Scan : public Source */ void configure(); + void activate() override; + void deactivate() override; + /** * @brief Adds latest data from laser scanner to the data array. * @param curr_time Current node time for data interpolation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 10bcdb094b0..93655fd70e0 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -95,6 +95,16 @@ class Source */ rclcpp::Duration getSourceTimeout() const; + /** + * @brief Activates the source's subscription(s). Call from node on_activate. + */ + virtual void activate() {} + + /** + * @brief Deactivates the source's subscription(s). Call from node on_deactivate. + */ + virtual void deactivate() {} + protected: /** * @brief Source configuration routine. diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index ccefe3d71bb..4d316830972 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -147,6 +147,22 @@ void Circle::createSubscription(std::string & polygon_sub_topic) } } +void Circle::activate() +{ + if (radius_sub_) { + radius_sub_->on_activate(); + } + Polygon::activate(); +} + +void Circle::deactivate() +{ + if (radius_sub_) { + radius_sub_->on_deactivate(); + } + Polygon::deactivate(); +} + void Circle::updatePolygon(double radius) { // Update circle radius diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 5912b0ea7e0..4591160f68c 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -72,21 +72,24 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - // Activating lifecycle publisher - state_pub_->on_activate(); - collision_points_marker_pub_->on_activate(); - - // Activating polygons + // 1. Activate all input subscriptions first (sources, then polygons) + for (std::shared_ptr source : sources_) { + source->activate(); + } for (std::shared_ptr polygon : polygons_) { polygon->activate(); } - // Creating timer + // 2. Activate publishers + state_pub_->on_activate(); + collision_points_marker_pub_->on_activate(); + + // 3. Create timer timer_ = this->create_timer( std::chrono::duration{1.0 / frequency_}, std::bind(&CollisionDetector::process, this)); - // Creating bond connection + // 4. Creating bond connection last createBond(); return nav2::CallbackReturn::SUCCESS; @@ -109,6 +112,11 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) polygon->deactivate(); } + // Deactivating sources + for (std::shared_ptr source : sources_) { + source->deactivate(); + } + // Destroying bond connection destroyBond(); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index e1f5d0e154f..1a32774a319 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -63,6 +63,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { on_cleanup(state); + on_shutdown(state); return nav2::CallbackReturn::FAILURE; } @@ -95,6 +96,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); on_cleanup(state); + on_shutdown(state); return nav2::CallbackReturn::FAILURE; } } @@ -115,26 +117,29 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - // Activating lifecycle publisher + // 1. Activate all input subscriptions first (before any publishers or timers) + cmd_vel_in_sub_->on_activate(); + for (std::shared_ptr source : sources_) { + source->activate(); + } + for (std::shared_ptr polygon : polygons_) { + polygon->activate(); + } + + // 2. Activate publishers cmd_vel_out_pub_->on_activate(); if (state_pub_) { state_pub_->on_activate(); } collision_points_marker_pub_->on_activate(); - // Activating polygons - for (std::shared_ptr polygon : polygons_) { - polygon->activate(); - } - - // Since polygons are being published when cmd_vel_in appears, - // we need to publish polygons first time to display them at startup + // 3. Publish polygons first time to display them at startup publishPolygons(); - // Activating main worker + // 4. Activate main worker process_active_ = true; - // Creating bond connection + // 5. Creating bond connection last createBond(); return nav2::CallbackReturn::SUCCESS; @@ -156,7 +161,13 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) polygon->deactivate(); } - // Deactivating lifecycle publishers + // Deactivating sources + for (std::shared_ptr source : sources_) { + source->deactivate(); + } + + // Deactivating lifecycle publishers and subscribers + cmd_vel_in_sub_->on_deactivate(); cmd_vel_out_pub_->on_deactivate(); if (state_pub_) { state_pub_->on_deactivate(); diff --git a/nav2_collision_monitor/src/costmap.cpp b/nav2_collision_monitor/src/costmap.cpp index befff7e4ca0..6b2034c5dad 100644 --- a/nav2_collision_monitor/src/costmap.cpp +++ b/nav2_collision_monitor/src/costmap.cpp @@ -64,6 +64,16 @@ void CostmapSource::configure() nav2::qos::StandardTopicQoS()); } +void CostmapSource::activate() +{ + data_sub_->on_activate(); +} + +void CostmapSource::deactivate() +{ + data_sub_->on_deactivate(); +} + bool CostmapSource::getData( const rclcpp::Time & curr_time, std::vector & data) diff --git a/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp new file mode 100644 index 00000000000..e03ebc8bd7c --- /dev/null +++ b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp @@ -0,0 +1,103 @@ +// 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. + +#include "nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp" + +#include "rclcpp/node.hpp" +#if defined(POINT_CLOUD_TRANSPORT_SUBSCRIBE_USE_QOS) +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#endif + +namespace nav2_collision_monitor +{ + +LifecyclePointCloudSubscription::LifecyclePointCloudSubscription( + rclcpp_lifecycle::LifecycleNode::SharedPtr node) +: node_(node) +{ +#if defined(POINT_CLOUD_TRANSPORT_SUBSCRIBE_USE_QOS) + // Rolling: LifecycleNode does not inherit Node; use node interfaces + pct_ = std::make_shared( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface, + rclcpp::node_interfaces::NodeLoggingInterface>( + node->get_node_base_interface(), + node->get_node_parameters_interface(), + node->get_node_topics_interface(), + node->get_node_logging_interface())); +#else + // Jazzy/Kilted: PointCloudTransport(rclcpp::Node::SharedPtr) + rclcpp::Node * node_ptr = dynamic_cast(node.get()); + if (!node_ptr) { + throw std::invalid_argument( + "LifecyclePointCloudSubscription: node must be a rclcpp::Node " + "(LifecycleNode inheriting Node)"); + } + pct_ = std::make_shared( + std::shared_ptr(node, node_ptr)); +#endif +} + +LifecyclePointCloudSubscription::~LifecyclePointCloudSubscription() +{ + shutdown(); +} + +void LifecyclePointCloudSubscription::subscribe( + const std::string & topic, + const rclcpp::QoS & qos, + Callback callback, + const point_cloud_transport::TransportHints & transport_hints) +{ + std::function gated = + [this, cb = std::move(callback)]( + sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + if (is_activated()) { + cb(msg); + } + }; +#if defined(POINT_CLOUD_TRANSPORT_SUBSCRIBE_USE_QOS) + data_sub_ = pct_->subscribe(topic, qos, gated, std::shared_ptr(), &transport_hints); +#else + data_sub_ = pct_->subscribe( + topic, + qos.get_rmw_qos_profile(), + gated, + std::shared_ptr(), + &transport_hints); +#endif +} + +void LifecyclePointCloudSubscription::on_activate() +{ + rclcpp_lifecycle::SimpleManagedEntity::on_activate(); +} + +void LifecyclePointCloudSubscription::on_deactivate() +{ + rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); +} + +void LifecyclePointCloudSubscription::shutdown() +{ + data_sub_.shutdown(); +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 9d3b9bce442..d5c57f3d899 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -45,11 +45,12 @@ PointCloud::PointCloud( PointCloud::~PointCloud() { RCLCPP_INFO(logger_, "[%s]: Destroying PointCloud", source_name_.c_str()); - #if RCLCPP_VERSION_GTE(30, 0, 0) - data_sub_.shutdown(); - #else - data_sub_.reset(); + #if defined(RCLCPP_VERSION_MAJOR) && RCLCPP_VERSION_MAJOR >= 30 + if (data_sub_) { + data_sub_->shutdown(); + } #endif + data_sub_.reset(); auto node = node_.lock(); if (post_set_params_handler_ && node) { node->remove_post_set_parameters_callback(post_set_params_handler_.get()); @@ -73,13 +74,13 @@ void PointCloud::configure() getParameters(source_topic); - #if RCLCPP_VERSION_GTE(30, 0, 0) + #if defined(RCLCPP_VERSION_MAJOR) && RCLCPP_VERSION_MAJOR >= 30 const point_cloud_transport::TransportHints hint(transport_type_); - pct_ = std::make_shared(*node); - data_sub_ = pct_->subscribe( + data_sub_ = LifecyclePointCloudSubscription::make_shared(node); + data_sub_->subscribe( source_topic, nav2::qos::SensorDataQoS(), std::bind(&PointCloud::dataCallback, this, std::placeholders::_1), - {}, &hint + hint ); #else data_sub_ = node->create_subscription( @@ -99,6 +100,16 @@ void PointCloud::configure() this, std::placeholders::_1)); } +void PointCloud::activate() +{ + data_sub_->on_activate(); +} + +void PointCloud::deactivate() +{ + data_sub_->on_deactivate(); +} + bool PointCloud::getData( const rclcpp::Time & curr_time, std::vector & data) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 554672e5022..9d0e2cf507c 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -125,6 +125,12 @@ void Polygon::activate() { resetTriggerState(); + if (polygon_sub_) { + polygon_sub_->on_activate(); + } + if (footprint_sub_) { + footprint_sub_->on_activate(); + } if (visualize_) { polygon_pub_->on_activate(); } @@ -132,6 +138,12 @@ void Polygon::activate() void Polygon::deactivate() { + if (polygon_sub_) { + polygon_sub_->on_deactivate(); + } + if (footprint_sub_) { + footprint_sub_->on_deactivate(); + } if (visualize_) { polygon_pub_->on_deactivate(); } diff --git a/nav2_collision_monitor/src/polygon_source.cpp b/nav2_collision_monitor/src/polygon_source.cpp index 1483d76df05..227dfc8916d 100644 --- a/nav2_collision_monitor/src/polygon_source.cpp +++ b/nav2_collision_monitor/src/polygon_source.cpp @@ -65,6 +65,16 @@ void PolygonSource::configure() nav2::qos::SensorDataQoS()); } +void PolygonSource::activate() +{ + data_sub_->on_activate(); +} + +void PolygonSource::deactivate() +{ + data_sub_->on_deactivate(); +} + bool PolygonSource::getData( const rclcpp::Time & curr_time, std::vector & data) diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 6df4d27c513..8147631d01d 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -67,6 +67,16 @@ void Range::configure() nav2::qos::SensorDataQoS()); } +void Range::activate() +{ + data_sub_->on_activate(); +} + +void Range::deactivate() +{ + data_sub_->on_deactivate(); +} + bool Range::getData( const rclcpp::Time & curr_time, std::vector & data) diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index ce5fa7c89ec..49c09f88557 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -66,6 +66,16 @@ void Scan::configure() nav2::qos::SensorDataQoS()); } +void Scan::activate() +{ + data_sub_->on_activate(); +} + +void Scan::deactivate() +{ + data_sub_->on_deactivate(); +} + bool Scan::getData( const rclcpp::Time & curr_time, std::vector & data) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 024485b62c9..c7f1651f573 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -139,6 +139,9 @@ class Tester : public ::testing::Test // Setting TF chains void sendTransforms(const rclcpp::Time & stamp); + // Start node and activate test subscriptions (so test receives output topics) + void startDetector(); + // Publish robot footprint void publishFootprint(const double radius, const rclcpp::Time & stamp); @@ -378,6 +381,13 @@ void Tester::setVectors( cd_->declare_parameter("observation_sources", rclcpp::ParameterValue(sources)); } +void Tester::startDetector() +{ + cd_->start(); + state_sub_->on_activate(); + collision_points_marker_sub_->on_activate(); +} + void Tester::sendTransforms(const rclcpp::Time & stamp) { std::shared_ptr tf_broadcaster = @@ -652,7 +662,7 @@ TEST_F(Tester, testProcessActive) setVectors({"DetectionRegion"}, {SCAN_NAME}); // Configure and activate Collision Detector node - cd_->start(); + startDetector(); // ... and check that state is published ASSERT_TRUE(waitState(300ms)); @@ -672,7 +682,7 @@ TEST_F(Tester, testPolygonDetection) setVectors({"DetectionRegion"}, {RANGE_NAME}); // Start Collision Detector node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); @@ -701,7 +711,7 @@ TEST_F(Tester, testCircleDetection) setVectors({"DetectionRegion"}, {RANGE_NAME}); // Start Collision Detector node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); @@ -730,7 +740,7 @@ TEST_F(Tester, testScanDetection) setVectors({"DetectionRegion"}, {SCAN_NAME}); // Start Collision Detector node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); @@ -759,7 +769,7 @@ TEST_F(Tester, testPointcloudDetection) setVectors({"DetectionRegion"}, {POINTCLOUD_NAME}); // Start Collision Detector node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); @@ -788,7 +798,7 @@ TEST_F(Tester, testPolygonSourceDetection) setVectors({"DetectionRegion"}, {POLYGON_NAME}); // Start Collision Detector node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); @@ -817,7 +827,7 @@ TEST_F(Tester, testCostmapDetection) setVectors({"DetectionRegion"}, {COSTMAP_NAME}); // Start Collision Detector node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); @@ -845,7 +855,7 @@ TEST_F(Tester, testCollisionPointsMarkers) setVectors({}, {SCAN_NAME}); // Start Collision Monitor node - cd_->start(); + startDetector(); // Share TF sendTransforms(curr_time); diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp index ee74024fc1b..1cba7112736 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -29,6 +29,9 @@ #include "nav2_msgs/msg/collision_monitor_state.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "rosgraph_msgs/msg/clock.hpp" +#include "nav2_ros_common/subscription.hpp" +#include "nav2_ros_common/interface_factories.hpp" + using nav2_msgs::msg::CollisionMonitorState; @@ -69,24 +72,32 @@ class MetricsCatcher : public rclcpp::Node { min_hold_pct_ = this->declare_parameter("min_hold_pct", 0.90); max_time_to_resume_ = this->declare_parameter("max_time_to_resume", 0.6); max_false_stop_pct_ = this->declare_parameter("max_false_stop_pct", 0.05); + } + void initialize_subscriptions() + { // We only start collecting after we have seen *both* /clock and /local_costmap // This avoids counting "startup zero cmd" as real data. - cm_sub_ = this->create_subscription( - "/local_costmap/costmap", rclcpp::QoS(1).reliable().durability_volatile(), + cm_sub_ = nav2::interfaces::create_subscription( + shared_from_this(), + "/local_costmap/costmap", [this](const nav2_msgs::msg::Costmap &){ - if (!got_costmap_) {got_costmap_ = true; cm_sub_.reset();} - }); + if (!got_costmap_) {got_costmap_ = true;} + }, + rclcpp::QoS(1).reliable().durability_volatile()); - clock_sub_ = this->create_subscription( - "/clock", rclcpp::QoS(1).best_effort().durability_volatile(), + clock_sub_ = nav2::interfaces::create_subscription( + shared_from_this(), + "/clock", [this](const rosgraph_msgs::msg::Clock &){ - if (!got_clock_) {got_clock_ = true; clock_sub_.reset();} - }); + if (!got_clock_) {got_clock_ = true;} + }, + rclcpp::QoS(1).best_effort().durability_volatile()); // This is the *output* we evaluate. In the launch file you can remap it. - cmd_sub_ = this->create_subscription( - "/cmd_vel", rclcpp::QoS(60), + cmd_sub_ = nav2::interfaces::create_subscription( + shared_from_this(), + "/cmd_vel", [this](const geometry_msgs::msg::Twist & msg){ if (!got_clock_ || !got_costmap_) { // don't collect before system is “live” @@ -94,19 +105,27 @@ class MetricsCatcher : public rclcpp::Node { } const double t = this->now().seconds(); samples_.push_back(Sample{t, msg.linear.x, last_action_}); - }); + }, + rclcpp::QoS(60)); // Optional: subscribe to CM state to help debugging (not used in asserts) - state_sub_ = this->create_subscription( - "/collision_state", rclcpp::QoS(10), + state_sub_ = nav2::interfaces::create_subscription( + shared_from_this(), + "/collision_state", [this](const CollisionMonitorState & msg){ last_action_ = msg.action_type; - }); + }, + rclcpp::QoS(10)); } // Spin until we passed the obstacle window (8s) + margin void run_and_collect(rclcpp::executors::SingleThreadedExecutor & exec, double margin_s = 2.0) { + if (cm_sub_) {cm_sub_->on_activate();} + if (clock_sub_) {clock_sub_->on_activate();} + if (cmd_sub_) {cmd_sub_->on_activate();} + if (state_sub_) {state_sub_->on_activate();} + // First, up to 5s WALL-time to see /clock auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); while (rclcpp::ok() && !got_clock_) { @@ -229,10 +248,10 @@ class MetricsCatcher : public rclcpp::Node { private: // Subscriptions - rclcpp::Subscription::SharedPtr cm_sub_; - rclcpp::Subscription::SharedPtr cmd_sub_; - rclcpp::Subscription::SharedPtr state_sub_; - rclcpp::Subscription::SharedPtr clock_sub_; + nav2::Subscription::SharedPtr cm_sub_; + nav2::Subscription::SharedPtr cmd_sub_; + nav2::Subscription::SharedPtr state_sub_; + nav2::Subscription::SharedPtr clock_sub_; // Buffers/state std::vector samples_; @@ -250,6 +269,7 @@ TEST(CollisionMonitorNodeBag, TrajectoryAndMetrics) { rclcpp::init(0, nullptr); auto node = std::make_shared(); + node->initialize_subscriptions(); rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node); diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index b4042e310e7..aa839cb4261 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -170,6 +170,9 @@ class Tester : public ::testing::Test // Publish robot footprint void publishFootprint(const double radius, const rclcpp::Time & stamp); + // Start node and activate test subscriptions (so test receives output topics) + void startMonitor(); + // Main topic/data working routines void publishScan(const double dist, const rclcpp::Time & stamp); void publishPointCloud(const double dist, const rclcpp::Time & stamp); @@ -494,6 +497,16 @@ void Tester::setGlobalHeightParams(const std::string & source_name, const double cm_->set_parameter(rclcpp::Parameter(source_name + ".min_height", min_height)); } +void Tester::startMonitor() +{ + cm_->start(); + // Activate test subscriptions so they receive output topics + // (lifecycle-managed subs need activate) + cmd_vel_out_sub_->on_activate(); + action_state_sub_->on_activate(); + collision_points_marker_sub_->on_activate(); +} + void Tester::sendTransforms(const rclcpp::Time & stamp) { std::shared_ptr tf_broadcaster = @@ -813,7 +826,7 @@ TEST_F(Tester, testToggleService) cm_->set_parameter(rclcpp::Parameter("enabled", false)); // Start collision monitor node - cm_->start(); + startMonitor(); ASSERT_FALSE(cm_->isEnabled()); auto request = std::make_shared(); @@ -852,7 +865,7 @@ TEST_F(Tester, testProcessStopSlowdownLimit) setVectors({"Limit", "SlowDown", "Stop"}, {SCAN_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -939,7 +952,7 @@ TEST_F(Tester, testPolygonSource) setVectors({"Limit", "SlowDown", "Stop"}, {POLYGON_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1026,7 +1039,7 @@ TEST_F(Tester, testProcessApproach) setVectors({"Approach"}, {SCAN_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1095,7 +1108,7 @@ TEST_F(Tester, testProcessApproachRotation) setVectors({"Approach"}, {RANGE_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Publish robot footprint publishFootprint(1.0, curr_time); @@ -1169,7 +1182,7 @@ TEST_F(Tester, testCrossOver) setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1237,7 +1250,7 @@ TEST_F(Tester, testSourceTimeout) setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1276,7 +1289,7 @@ TEST_F(Tester, testSourceTimeoutOverride) cm_->set_parameter(rclcpp::Parameter("source_timeout", 0.0)); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1315,7 +1328,7 @@ TEST_F(Tester, testCeasePublishZeroVel) setVectors({"Stop", "Approach"}, {SCAN_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1385,7 +1398,7 @@ TEST_F(Tester, testPolygonNotEnabled) setVectors({"Stop"}, {SCAN_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Check that robot stops when polygon is enabled rclcpp::Time curr_time = cm_->now(); @@ -1440,7 +1453,7 @@ TEST_F(Tester, testSourceNotEnabled) setVectors({"Stop"}, {SCAN_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Check that robot stops when source is enabled rclcpp::Time curr_time = cm_->now(); @@ -1562,7 +1575,7 @@ TEST_F(Tester, testCollisionPointsMarkers) setVectors({}, {SCAN_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF sendTransforms(curr_time); @@ -1597,7 +1610,7 @@ TEST_F(Tester, testVelocityPolygonStop) rclcpp::Time curr_time = cm_->now(); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Check that robot stops when source is enabled sendTransforms(curr_time); @@ -1669,7 +1682,7 @@ TEST_F(Tester, testVelocityPolygonStopGlobalHeight) rclcpp::Time curr_time = cm_->now(); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Check that robot stops when source is enabled sendTransforms(curr_time); @@ -1723,7 +1736,7 @@ TEST_F(Tester, testSourceAssociatedToPolygon) setVectors({"StopOnRangeSource", "SlowdownOnAllSources"}, {SCAN_NAME, RANGE_NAME}); // Start Collision Monitor node - cm_->start(); + startMonitor(); // Share TF rclcpp::Time curr_time = cm_->now(); diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 89b0a792f92..023682048e1 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -456,6 +456,8 @@ Tester::Tester() test_node_ = std::make_shared(); executor_ = std::make_shared(); executor_->add_node(test_node_->get_node_base_interface()); + test_node_->configure(); + test_node_->activate(); tf_buffer_ = std::make_shared(test_node_->get_clock()); tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model @@ -470,6 +472,8 @@ Tester::~Tester() polygon_.reset(); costmap_.reset(); + test_node_->deactivate(); + test_node_->cleanup(); test_node_.reset(); tf_listener_.reset(); @@ -501,6 +505,7 @@ void Tester::createSources(const bool base_shift_correction) BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); pointcloud_->configure(); + pointcloud_->activate(); // Create Range object test_node_->declare_parameter( @@ -905,6 +910,7 @@ TEST_F(Tester, testPointCloudMinRange) BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT, true); pointcloud_->configure(); + pointcloud_->activate(); sendTransforms(curr_time); diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 786c9449f45..1225375c98b 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -25,6 +25,7 @@ install( if(BUILD_TESTING) find_package(ament_cmake_test REQUIRED) find_package(ament_cmake_pytest REQUIRED) + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/nav2_package.cmake) nav2_add_pytest_test(test_rewritten_yaml test/test_rewritten_yaml.py ) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8794ee04339..19ddf688bad 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -225,6 +225,8 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2::CallbackReturn::FAILURE; } + odom_sub_->on_activate(); + speed_limit_sub_->on_activate(); ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -274,7 +276,8 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ costmap_ros_->deactivate(); - + odom_sub_->on_deactivate(); + speed_limit_sub_->on_deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); transformed_plan_pub_->on_deactivate(); diff --git a/nav2_core/include/nav2_core/waypoint_task_executor.hpp b/nav2_core/include/nav2_core/waypoint_task_executor.hpp index 3f735583b03..6bd9ec7cd73 100644 --- a/nav2_core/include/nav2_core/waypoint_task_executor.hpp +++ b/nav2_core/include/nav2_core/waypoint_task_executor.hpp @@ -63,6 +63,17 @@ class WaypointTaskExecutor */ virtual bool processAtWaypoint( const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) = 0; + + /** + * @brief Activate the plugin (e.g. lifecycle subscriptions). Call when the waypoint follower + * node is activated. Override in plugins that use lifecycle-managed subscriptions. + */ + virtual void on_activate() {} + + /** + * @brief Deactivate the plugin. Call when the waypoint follower node is deactivated. + */ + virtual void on_deactivate() {} }; } // namespace nav2_core #endif // NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp index 02ab054ad97..12e00e2c39b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp @@ -86,6 +86,10 @@ class BinaryFilter : public CostmapFilter */ bool isActive(); +protected: + void activateSubscriptions() override; + void deactivateSubscriptions() override; + private: /** * @brief Callback for the filter information diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 88fbb617a8f..834037ca489 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -160,6 +160,18 @@ class CostmapFilter : public Layer virtual void resetFilter() = 0; protected: + /** + * @brief Activate lifecycle-managed subscriptions (filter info, filter mask). + * Override in derived filters to call on_activate() on filter_info_sub_ and mask_sub_. + */ + virtual void activateSubscriptions() {} + + /** + * @brief Deactivate lifecycle-managed subscriptions before resetFilter() clears them. + * Override in derived filters to call on_deactivate() on filter_info_sub_ and mask_sub_. + */ + virtual void deactivateSubscriptions() {} + /** * @brief Costmap filter enabling/disabling callback * @param request_header Service request header diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index 9930c2b73a7..d9fb78d36bd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -100,6 +100,10 @@ class KeepoutFilter : public CostmapFilter */ bool isActive(); +protected: + void activateSubscriptions() override; + void deactivateSubscriptions() override; + private: /** * @brief Callback for the filter information diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index abd31800b58..a51594ea8fe 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -86,6 +86,10 @@ class SpeedFilter : public CostmapFilter */ bool isActive(); +protected: + void activateSubscriptions() override; + void deactivateSubscriptions() override; + private: /** * @brief Callback for the filter information diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index 1202fa0d968..7c02ee408ee 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -25,7 +25,9 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_msgs/msg/costmap.hpp" #include "nav2_msgs/msg/costmap_update.hpp" +#include "nav2_ros_common/interface_factories.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/qos_profiles.hpp" namespace nav2_costmap_2d { @@ -52,17 +54,30 @@ class CostmapSubscriber { logger_ = parent->get_logger(); - // Could be using a user rclcpp::Node, so need to use the Nav2 factory to create the - // subscription to convert nav2::LifecycleNode, rclcpp::Node or rclcpp_lifecycle::LifecycleNode - costmap_sub_ = nav2::interfaces::create_subscription( - parent, topic_name_, - std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS(3), callback_group); - - costmap_update_sub_ = nav2::interfaces::create_subscription( - parent, topic_name_ + "_updates", - std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS(), callback_group); + // Use lifecycle node's create_subscription when parent is a LifecycleNode so the + // subscription is added to managed entities and deactivated with the node + auto lc_node = std::dynamic_pointer_cast(parent); + if (lc_node) { + costmap_sub_ = lc_node->template create_subscription( + topic_name_, + std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS(3), callback_group); + + costmap_update_sub_ = lc_node->template create_subscription( + topic_name_ + "_updates", + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS(), callback_group); + } else { + costmap_sub_ = nav2::interfaces::create_subscription( + parent, topic_name_, + std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS(3), callback_group); + + costmap_update_sub_ = nav2::interfaces::create_subscription( + parent, topic_name_ + "_updates", + std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS(), callback_group); + } } /** @@ -70,6 +85,29 @@ class CostmapSubscriber */ ~CostmapSubscriber() {} + /** + * @brief Activate the underlying lifecycle-managed subscriptions so that + * the costmap and its updates start being received. Owners with a + * LifecycleNode parent must call this from their on_activate(); when + * constructed against a non-lifecycle node the subscriptions are already + * active and this is a no-op. + */ + void on_activate() + { + if (costmap_sub_) {costmap_sub_->on_activate();} + if (costmap_update_sub_) {costmap_update_sub_->on_activate();} + } + + /** + * @brief Deactivate the underlying lifecycle-managed subscriptions, tearing + * down the rclcpp endpoints. Symmetric counterpart to on_activate(). + */ + void on_deactivate() + { + if (costmap_sub_) {costmap_sub_->on_deactivate();} + if (costmap_update_sub_) {costmap_update_sub_->on_deactivate();} + } + /** * @brief Get current costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp index 2ea89b0b2ec..9c10f653169 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -20,7 +20,9 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/footprint.hpp" +#include "nav2_ros_common/interface_factories.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/qos_profiles.hpp" #include "nav2_util/robot_utils.hpp" namespace nav2_costmap_2d @@ -47,11 +49,19 @@ class FootprintSubscriber robot_base_frame_(robot_base_frame), transform_tolerance_(transform_tolerance) { - // Could be using a user rclcpp::Node, so need to use the Nav2 factory to create the - // subscription to convert nav2::LifecycleNode, rclcpp::Node or rclcpp_lifecycle::LifecycleNode - footprint_sub_ = nav2::interfaces::create_subscription( - parent, topic_name, - std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); + // Use lifecycle node's create_subscription when parent is a LifecycleNode so the + // subscription is added to managed entities and deactivated with the node + auto lc_node = std::dynamic_pointer_cast(parent); + if (lc_node) { + footprint_sub_ = lc_node->template create_subscription( + topic_name, + std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1), + nav2::qos::StandardTopicQoS()); + } else { + footprint_sub_ = nav2::interfaces::create_subscription( + parent, topic_name, + std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1)); + } } /** @@ -81,6 +91,18 @@ class FootprintSubscriber std::vector & footprint, std_msgs::msg::Header & footprint_header); + /** + * @brief Activate the subscription (for lifecycle-managed nav2::Subscription). + * Call from node on_activate when using a lifecycle node. + */ + void on_activate(); + + /** + * @brief Deactivate the subscription. + * Call from node on_deactivate when using a lifecycle node. + */ + void on_deactivate(); + protected: /** * @brief Callback to process new footprint updates. diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index c319df7e005..169e3c36bc9 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -224,6 +224,28 @@ void BinaryFilter::process( } } +void BinaryFilter::activateSubscriptions() +{ + std::lock_guard guard(*getMutex()); + if (filter_info_sub_) { + filter_info_sub_->on_activate(); + } + if (mask_sub_) { + mask_sub_->on_activate(); + } +} + +void BinaryFilter::deactivateSubscriptions() +{ + std::lock_guard guard(*getMutex()); + if (filter_info_sub_) { + filter_info_sub_->on_deactivate(); + } + if (mask_sub_) { + mask_sub_->on_deactivate(); + } +} + void BinaryFilter::resetFilter() { std::lock_guard guard(*getMutex()); diff --git a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp index 0d3a96ba114..e6f76325f3f 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp @@ -93,10 +93,12 @@ void CostmapFilter::onInitialize() void CostmapFilter::activate() { initializeFilter(filter_info_topic_); + activateSubscriptions(); } void CostmapFilter::deactivate() { + deactivateSubscriptions(); resetFilter(); } diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index bb21956ad13..9df3d1462eb 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -397,6 +397,28 @@ void KeepoutFilter::process( last_pose_lethal_ = is_pose_lethal_; } +void KeepoutFilter::activateSubscriptions() +{ + std::lock_guard guard(*getMutex()); + if (filter_info_sub_) { + filter_info_sub_->on_activate(); + } + if (mask_sub_) { + mask_sub_->on_activate(); + } +} + +void KeepoutFilter::deactivateSubscriptions() +{ + std::lock_guard guard(*getMutex()); + if (filter_info_sub_) { + filter_info_sub_->on_deactivate(); + } + if (mask_sub_) { + mask_sub_->on_deactivate(); + } +} + void KeepoutFilter::resetFilter() { std::lock_guard guard(*getMutex()); diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index 8495413ea9b..0a55a6ea050 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -265,6 +265,28 @@ void SpeedFilter::process( } } +void SpeedFilter::activateSubscriptions() +{ + std::lock_guard guard(*getMutex()); + if (filter_info_sub_) { + filter_info_sub_->on_activate(); + } + if (mask_sub_) { + mask_sub_->on_activate(); + } +} + +void SpeedFilter::deactivateSubscriptions() +{ + std::lock_guard guard(*getMutex()); + if (filter_info_sub_) { + filter_info_sub_->on_deactivate(); + } + if (mask_sub_) { + mask_sub_->on_deactivate(); + } +} + void SpeedFilter::resetFilter() { std::lock_guard guard(*getMutex()); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index e73bbd5d89a..8702af71ee6 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -108,6 +108,12 @@ StaticLayer::onInitialize() void StaticLayer::activate() { + if (map_sub_) { + map_sub_->on_activate(); + } + if (map_update_sub_) { + map_update_sub_->on_activate(); + } auto node = node_.lock(); // Add callback for dynamic parameters post_set_params_handler_ = node->add_post_set_parameters_callback( @@ -123,6 +129,12 @@ StaticLayer::activate() void StaticLayer::deactivate() { + if (map_sub_) { + map_sub_->on_deactivate(); + } + if (map_update_sub_) { + map_update_sub_->on_deactivate(); + } auto node = node_.lock(); if (post_set_params_handler_ && node) { node->remove_post_set_parameters_callback(post_set_params_handler_.get()); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 4ef7ea3096f..c772ea5b5b7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -266,11 +266,24 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - // First, make sure that the transform between the robot base frame - // and the global frame is available + // 1. Activate input subscriptions first + if (subscribe_to_stamped_footprint_) { + footprint_stamped_sub_->on_activate(); + } else { + footprint_sub_->on_activate(); + } + if (parameter_sub_) { + parameter_sub_->on_activate(); + } - std::string tf_error; + // 2. Activate publishers + footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } + std::string tf_error; RCLCPP_INFO(get_logger(), "Checking transform"); rclcpp::Rate r(2); const auto initial_transform_timeout = rclcpp::Duration::from_seconds( @@ -285,7 +298,6 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) " to become available, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); - // Check timeout if (now() > initial_transform_timeout_point) { RCLCPP_ERROR( get_logger(), @@ -296,27 +308,15 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) return nav2::CallbackReturn::FAILURE; } - // The error string will accumulate and errors will typically be the same, so the last - // will do for the warning above. Reset the string here to avoid accumulation tf_error.clear(); r.sleep(); } - // Activate publishers - footprint_pub_->on_activate(); - costmap_publisher_->on_activate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_activate(); - } - - // Create a thread to handle updating the map stopped_ = true; // to active plugins stop_updates_ = false; map_update_thread_shutdown_ = false; map_update_thread_ = std::make_unique( std::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency_)); - start(); // Add callback for dynamic parameters @@ -349,12 +349,19 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) map_update_thread_->join(); } - footprint_pub_->on_deactivate(); costmap_publisher_->on_deactivate(); - for (auto & layer_pub : layer_publishers_) { layer_pub->on_deactivate(); } + footprint_pub_->on_deactivate(); + if (subscribe_to_stamped_footprint_) { + footprint_stamped_sub_->on_deactivate(); + } else { + footprint_sub_->on_deactivate(); + } + if (parameter_sub_) { + parameter_sub_->on_deactivate(); + } return nav2::CallbackReturn::SUCCESS; } @@ -376,6 +383,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) tf_buffer_.reset(); footprint_sub_.reset(); + footprint_stamped_sub_.reset(); footprint_pub_.reset(); return nav2::CallbackReturn::SUCCESS; diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp index 7195aed1446..9c6b9400652 100644 --- a/nav2_costmap_2d/src/footprint_subscriber.cpp +++ b/nav2_costmap_2d/src/footprint_subscriber.cpp @@ -72,6 +72,16 @@ FootprintSubscriber::getFootprintInRobotFrame( return true; } +void FootprintSubscriber::on_activate() +{ + footprint_sub_->on_activate(); +} + +void FootprintSubscriber::on_deactivate() +{ + footprint_sub_->on_deactivate(); +} + void FootprintSubscriber::footprint_callback( const geometry_msgs::msg::PolygonStamped::ConstSharedPtr & msg) diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index 7e96965b95c..3a9498fbfcb 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -101,6 +101,7 @@ std::vector TestNode::setRadii( void TestNode::waitForMap(std::shared_ptr & slayer) { + slayer->activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node_->get_node_base_interface()); while (!slayer->isCurrent()) { diff --git a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp index 284e7a83347..8f1293c030f 100644 --- a/nav2_costmap_2d/test/integration/plugin_container_tests.cpp +++ b/nav2_costmap_2d/test/integration/plugin_container_tests.cpp @@ -95,6 +95,7 @@ class TestNode : public ::testing::Test void waitForMap(std::shared_ptr & slayer) { + slayer->activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node_->get_node_base_interface()); while (!slayer->isCurrent()) { diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 614e571f764..41ce5192bfc 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -92,7 +92,18 @@ class LayerSubscriber executor_ = std::make_shared(); executor_->add_callback_group(callback_group_, node->get_node_base_interface()); - executor_thread_ = std::make_unique(executor_); + } + + void activate() + { + if (layer_sub_) { + // Build the underlying rclcpp::Subscription before the executor starts + // spinning, so it observes the entity from its first wait_for_work(). + layer_sub_->on_activate(); + } + if (!executor_thread_) { + executor_thread_ = std::make_unique(executor_); + } } ~LayerSubscriber() @@ -127,6 +138,10 @@ class CostmapRosTestFixture : public ::testing::Test layer_subscriber_ = std::make_shared( costmap_lifecycle_node_->shared_from_this()); costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); + // Activate the subscriber before the costmap so the underlying + // rclcpp::Subscription exists (per PR #5834 create-on-activate design) + // by the time mapUpdateLoop starts publishing. + layer_subscriber_->activate(); costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); } diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 485d3b3eb6a..5d15ec7ca85 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -44,6 +44,11 @@ class TestCostmapSubscriberShould : public ::testing::Test topicName + "_raw_updates", std::bind( &TestCostmapSubscriberShould::costmapRawUpdateCallback, this, std::placeholders::_1)); + + // Transition the node to active so all managed nav2::Subscriptions + // (including those created by CostmapSubscriber later) auto-activate. + node->configure(); + node->activate(); } void SetUp() override diff --git a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index 8205c9f8781..b9c4d3b4f39 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -134,6 +134,7 @@ class TestCollisionChecker : public nav2::LifecycleNode // Add Static Layer std::shared_ptr slayer = nullptr; addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer, callback_group_); + slayer->activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(this->get_node_base_interface()); while (!slayer->isCurrent()) { diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index e9f312e5ca0..8fb147b7999 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -23,6 +23,8 @@ #include #include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" @@ -36,6 +38,8 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "nav2_costmap_2d/costmap_filters/binary_filter.hpp" +#include "nav2_ros_common/interface_factories.hpp" + using namespace std::chrono_literals; @@ -104,16 +108,25 @@ class BinaryStateSubscriber : public rclcpp::Node { public: explicit BinaryStateSubscriber(const std::string & binary_state_topic, bool default_state) - : Node("binary_state_sub"), binary_state_updated_(false) + : Node("binary_state_sub"), binary_state_topic_(binary_state_topic), binary_state_updated_(false) { - subscriber_ = this->create_subscription( - binary_state_topic, rclcpp::QoS(10), - std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1)); - - // Initialize with default state msg_.data = default_state; } + void initSubscription() + { + subscriber_ = nav2::interfaces::create_subscription( + shared_from_this(), + binary_state_topic_, + std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1), + rclcpp::QoS(10)); + } + + void activate() + { + subscriber_->on_activate(); + } + void binaryStateCallback( const std_msgs::msg::Bool::ConstSharedPtr & msg) { @@ -137,6 +150,7 @@ class BinaryStateSubscriber : public rclcpp::Node } private: + std::string binary_state_topic_; nav2::Subscription::SharedPtr subscriber_; std_msgs::msg::Bool msg_; bool binary_state_updated_; @@ -390,9 +404,15 @@ bool TestNode::createBinaryFilter(const std::string & global_frame, double flip_ binary_state_subscriber_ = std::make_shared(BINARY_STATE_TOPIC, default_state_); + binary_state_subscriber_->initSubscription(); + binary_state_subscriber_->activate(); binary_state_subscriber_executor_.add_node(binary_state_subscriber_); node_executor_.add_node(node_->get_node_base_interface()); + // Activate lifecycle so the costmap_filter_info subscription can receive messages + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + // Wait until mask will be received by BinaryFilter const std::chrono::nanoseconds timeout = 500ms; rclcpp::Time start_time = node_->now(); @@ -724,7 +744,23 @@ void TestNode::reset() mask_publisher_.reset(); binary_state_subscriber_.reset(); binary_filter_.reset(); - node_.reset(); + + // Shut down lifecycle node properly to avoid destructor warning + if (node_) { + const auto state = node_->get_current_state().id(); + if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } else if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } else if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } + node_.reset(); + } + tf_listener_.reset(); tf_broadcaster_.reset(); tf_buffer_.reset(); diff --git a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp index f6078c52cc0..19d703892ec 100644 --- a/nav2_costmap_2d/test/unit/keepout_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/keepout_filter_test.cpp @@ -230,6 +230,9 @@ void TestNode::createKeepoutFilter(const std::string & global_frame) keepout_filter_->initialize(&layers, std::string(FILTER_NAME), tf_buffer_.get(), node_, nullptr); keepout_filter_->initializeFilter(INFO_TOPIC); + node_->configure(); + node_->activate(); + // Wait until mask will be received by KeepoutFilter rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node_->get_node_base_interface()); diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 5fab85bd046..53348d8113b 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -23,7 +23,10 @@ #include #include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/interface_factories.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" #include "tf2_ros/transform_broadcaster.hpp" @@ -105,11 +108,20 @@ class SpeedLimitSubscriber : public rclcpp::Node { public: explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) - : Node("speed_limit_sub"), speed_limit_updated_(false) + : Node("speed_limit_sub"), speed_limit_topic_(speed_limit_topic), speed_limit_updated_(false) + {} + + void initSubscription() { - subscriber_ = this->create_subscription( - speed_limit_topic, rclcpp::QoS(10), - std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1)); + subscriber_ = nav2::interfaces::create_subscription( + shared_from_this(), speed_limit_topic_, + std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1), + rclcpp::QoS(10)); + } + + void activate() + { + subscriber_->on_activate(); } void speedLimitCallback( @@ -135,6 +147,7 @@ class SpeedLimitSubscriber : public rclcpp::Node } private: + std::string speed_limit_topic_; nav2::Subscription::SharedPtr subscriber_; nav2_msgs::msg::SpeedLimit::ConstSharedPtr msg_; bool speed_limit_updated_; @@ -368,9 +381,15 @@ bool TestNode::createSpeedFilter(const std::string & global_frame) speed_filter_->initializeFilter(INFO_TOPIC); speed_limit_subscriber_ = std::make_shared(SPEED_LIMIT_TOPIC); + speed_limit_subscriber_->initSubscription(); + speed_limit_subscriber_->activate(); speed_limit_subscriber_executor_.add_node(speed_limit_subscriber_); node_executor_.add_node(node_->get_node_base_interface()); + // Activate lifecycle so the costmap_filter_info subscription can receive messages + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); + // Wait until mask will be received by SpeedFilter const std::chrono::nanoseconds timeout = 500ms; rclcpp::Time start_time = node_->now(); @@ -630,7 +649,23 @@ void TestNode::reset() mask_publisher_.reset(); speed_limit_subscriber_.reset(); speed_filter_.reset(); - node_.reset(); + + // Shut down lifecycle node properly to avoid destructor warning + if (node_) { + const auto state = node_->get_current_state().id(); + if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } else if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } else if (state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + node_->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } + node_.reset(); + } + tf_listener_.reset(); tf_broadcaster_.reset(); tf_buffer_.reset(); diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 7825f6a8a3f..b13eeb6f9e7 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -84,6 +84,7 @@ DockingServer::on_activate(const rclcpp_lifecycle::State & /*state*/) dock_db_->activate(); navigator_->activate(); vel_publisher_->on_activate(); + odom_sub_->on_activate(); docking_action_server_->activate(); undocking_action_server_->activate(); param_handler_->activate(); diff --git a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp index e9f67f27202..fe54c50c47a 100644 --- a/nav2_docking/opennav_docking/src/simple_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_charging_dock.cpp @@ -380,6 +380,7 @@ bool SimpleChargingDock::startDetectionProcess() initial_pose_received_ = true; }, nav2::qos::StandardTopicQoS()); + dock_pose_sub_->on_activate(); } detection_active_ = true; @@ -434,10 +435,28 @@ void SimpleChargingDock::activate() dock_pose_pub_->on_activate(); filtered_dock_pose_pub_->on_activate(); staging_pose_pub_->on_activate(); + if (dock_pose_sub_) { + dock_pose_sub_->on_activate(); + } + if (battery_sub_) { + battery_sub_->on_activate(); + } + if (joint_state_sub_) { + joint_state_sub_->on_activate(); + } } void SimpleChargingDock::deactivate() { + if (dock_pose_sub_) { + dock_pose_sub_->on_deactivate(); + } + if (battery_sub_) { + battery_sub_->on_deactivate(); + } + if (joint_state_sub_) { + joint_state_sub_->on_deactivate(); + } stopDetectionProcess(); dock_pose_pub_->on_deactivate(); filtered_dock_pose_pub_->on_deactivate(); diff --git a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp index b8c79fca639..4bab483c01e 100644 --- a/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp +++ b/nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp @@ -338,6 +338,7 @@ bool SimpleNonChargingDock::startDetectionProcess() initial_pose_received_ = true; }, nav2::qos::StandardTopicQoS()); + dock_pose_sub_->on_activate(); } detection_active_ = true; @@ -392,10 +393,22 @@ void SimpleNonChargingDock::activate() dock_pose_pub_->on_activate(); filtered_dock_pose_pub_->on_activate(); staging_pose_pub_->on_activate(); + if (dock_pose_sub_) { + dock_pose_sub_->on_activate(); + } + if (joint_state_sub_) { + joint_state_sub_->on_activate(); + } } void SimpleNonChargingDock::deactivate() { + if (dock_pose_sub_) { + dock_pose_sub_->on_deactivate(); + } + if (joint_state_sub_) { + joint_state_sub_->on_deactivate(); + } stopDetectionProcess(); dock_pose_pub_->on_deactivate(); filtered_dock_pose_pub_->on_deactivate(); diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp index be2587f7bc5..8c7384c53b7 100644 --- a/nav2_docking/opennav_docking/test/test_controller.cpp +++ b/nav2_docking/opennav_docking/test/test_controller.cpp @@ -87,13 +87,13 @@ class TestCollisionChecker : public nav2::LifecycleNode nav2::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(this->get_logger(), "Activating"); - costmap_pub_->on_activate(); return nav2::CallbackReturn::SUCCESS; } nav2::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(this->get_logger(), "Deactivating"); + footprint_pub_->on_deactivate(); costmap_pub_->on_deactivate(); costmap_.reset(); return nav2::CallbackReturn::SUCCESS; @@ -296,6 +296,8 @@ TEST(ControllerTests, CollisionCheckerDockForward) { auto controller = std::make_unique( node, tf, "test_base_frame", "test_base_frame"); + node->configure(); + node->activate(); collision_tester->configure(); collision_tester->activate(); auto executor_thread = std::make_unique(node->get_node_base_interface()); @@ -362,6 +364,8 @@ TEST(ControllerTests, CollisionCheckerDockBackward) { auto controller = std::make_unique( node, tf, "test_base_frame", "test_base_frame"); + node->configure(); + node->activate(); collision_tester->configure(); collision_tester->activate(); auto executor_thread = std::make_unique(node->get_node_base_interface()); @@ -428,6 +432,8 @@ TEST(ControllerTests, CollisionCheckerUndockBackward) { auto controller = std::make_unique( node, tf, "test_base_frame", "test_base_frame"); + node->configure(); + node->activate(); collision_tester->configure(); collision_tester->activate(); auto executor_thread = std::make_unique(node->get_node_base_interface()); @@ -502,6 +508,8 @@ TEST(ControllerTests, CollisionCheckerUndockForward) { auto controller = std::make_unique( node, tf, "test_base_frame", "test_base_frame"); + node->configure(); + node->activate(); collision_tester->configure(); collision_tester->activate(); auto executor_thread = std::make_unique(node->get_node_base_interface()); diff --git a/nav2_following/opennav_following/src/following_server.cpp b/nav2_following/opennav_following/src/following_server.cpp index db32dfe7ec5..7154d173556 100644 --- a/nav2_following/opennav_following/src/following_server.cpp +++ b/nav2_following/opennav_following/src/following_server.cpp @@ -95,6 +95,7 @@ FollowingServer::on_activate(const rclcpp_lifecycle::State & /*state*/) tf2_listener_ = std::make_unique(*tf2_buffer_, this, true); vel_publisher_->on_activate(); + odom_sub_->on_activate(); filtered_dynamic_pose_pub_->on_activate(); following_action_server_->activate(); param_handler_->activate(); @@ -111,6 +112,7 @@ FollowingServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating %s", get_name()); following_action_server_->deactivate(); + odom_sub_->on_deactivate(); vel_publisher_->on_deactivate(); filtered_dynamic_pose_pub_->on_deactivate(); param_handler_->deactivate(); @@ -224,6 +226,7 @@ void FollowingServer::followObject() detected_dynamic_pose_ = *pose; }, nav2::qos::StandardTopicQoS(1)); // Only want the most recent pose + dynamic_pose_sub_->on_activate(); param_handler_->getMutex().lock(); } } else { diff --git a/nav2_loopback_sim/src/loopback_simulator.cpp b/nav2_loopback_sim/src/loopback_simulator.cpp index 856c32ab4fe..2c26fc149e4 100644 --- a/nav2_loopback_sim/src/loopback_simulator.cpp +++ b/nav2_loopback_sim/src/loopback_simulator.cpp @@ -118,6 +118,15 @@ LoopbackSimulator::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + // Activate lifecycle-managed subscriptions created in on_configure(); under + // the create-on-activate wrapper they would otherwise stay dormant. + if (initial_pose_sub_) { + initial_pose_sub_->on_activate(); + } + if (cmd_vel_sub_) { + cmd_vel_sub_->on_activate(); + } + odom_pub_->on_activate(); if (scan_pub_) { scan_pub_->on_activate(); @@ -167,6 +176,12 @@ LoopbackSimulator::on_deactivate(const rclcpp_lifecycle::State & /*state*/) if (scan_pub_) { scan_pub_->on_deactivate(); } + if (initial_pose_sub_) { + initial_pose_sub_->on_deactivate(); + } + if (cmd_vel_sub_) { + cmd_vel_sub_->on_deactivate(); + } has_initial_pose_ = false; curr_cmd_vel_.reset(); diff --git a/nav2_loopback_sim/test/test_clock_publisher.cpp b/nav2_loopback_sim/test/test_clock_publisher.cpp index 640af984e99..51dd8ff882e 100644 --- a/nav2_loopback_sim/test/test_clock_publisher.cpp +++ b/nav2_loopback_sim/test/test_clock_publisher.cpp @@ -40,6 +40,10 @@ class ClockPublisherTest : public ::testing::Test node_ = std::make_shared("clock_test_node"); executor_ = std::make_shared(); executor_->add_node(node_->get_node_base_interface()); + // Drive the lifecycle to ACTIVE so create_subscription() in each test + // auto-activates under the nav2::Subscription wrapper. + node_->configure(); + node_->activate(); } void TearDown() override diff --git a/nav2_map_server/src/map_saver/main_cli.cpp b/nav2_map_server/src/map_saver/main_cli.cpp index 48d53450606..ef155a8e190 100644 --- a/nav2_map_server/src/map_saver/main_cli.cpp +++ b/nav2_map_server/src/map_saver/main_cli.cpp @@ -21,6 +21,7 @@ #include "nav2_map_server/map_mode.hpp" #include "nav2_map_server/map_saver.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "rclcpp/rclcpp.hpp" using namespace nav2_map_server; // NOLINT @@ -165,6 +166,7 @@ int main(int argc, char ** argv) try { auto map_saver = std::make_shared(); map_saver->on_configure(rclcpp_lifecycle::State()); + map_saver->on_activate(rclcpp_lifecycle::State()); if (map_saver->saveMapTopicToFile(map_topic, save_parameters)) { retcode = 0; } else { diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 0302bca2e93..db6f0e83fcb 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -197,7 +197,7 @@ bool MapSaver::saveMapTopicToFile( auto map_sub = create_subscription( map_topic_loc, mapCallback, map_qos, callback_group); - + map_sub->on_activate(); // Create SingleThreadedExecutor to spin map_sub in callback_group rclcpp::executors::SingleThreadedExecutor executor; executor.add_callback_group(callback_group, get_node_base_interface()); diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index 42c8c20c29c..1f53853088a 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -28,6 +28,7 @@ target_link_libraries(test_vector_object_shapes ${vo_server_dependencies}) target_link_libraries(test_vector_object_shapes ${vo_library_name} + ${lifecycle_msgs_TARGETS} ) # test_vector_object_server unit test diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index e46d1409c2c..b37a53c0cb5 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -84,6 +84,7 @@ class InfoServerTester : public ::testing::Test FILTER_INFO_TOPIC, std::bind(&InfoServerTester::infoCallback, this, std::placeholders::_1), nav2::qos::LatchedSubscriptionQoS()); + subscription_->on_activate(); } ~InfoServerTester() diff --git a/nav2_map_server/test/unit/test_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp index 863dd2f219e..d45bdcc215b 100644 --- a/nav2_map_server/test/unit/test_vector_object_server.cpp +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -132,6 +132,10 @@ class Tester : public ::testing::Test bool waitMap(const std::chrono::nanoseconds & timeout); void verifyMap(bool is_circle, double poly_x_end = 1.0, double circle_cx = 3.0); + /** Start server and activate map subscription (subscription is not auto-activated when + * lifecycle callbacks are invoked directly instead of via trigger_transition). */ + void startVOServer(); + void comparePolygonObjects( nav2_msgs::msg::PolygonObject::SharedPtr p1, nav2_msgs::msg::PolygonObject::SharedPtr p2); @@ -149,7 +153,7 @@ class Tester : public ::testing::Test nav2::ServiceClient::SharedPtr remove_shapes_client_; // Output map subscriber - rclcpp::Subscription::SharedPtr vo_map_sub_; + nav2::Subscription::SharedPtr vo_map_sub_; // Output map published by VectorObjectServer nav_msgs::msg::OccupancyGrid::ConstSharedPtr map_; @@ -175,6 +179,7 @@ Tester::Tester() vo_map_sub_ = vo_server_->create_subscription( "vo_map", std::bind(&Tester::mapCallback, this, std::placeholders::_1), nav2::qos::LatchedSubscriptionQoS()); + vo_map_sub_->on_activate(); // Transform buffer and listener initialization tf_buffer_ = std::make_shared(vo_server_->get_clock()); @@ -183,6 +188,12 @@ Tester::Tester() executor_.add_node(vo_server_->get_node_base_interface()); } +void Tester::startVOServer() +{ + vo_server_->start(); + // vo_map_sub_ uses transient QoS and is activated on init +} + Tester::~Tester() { vo_map_sub_.reset(); @@ -522,7 +533,7 @@ TEST_F(Tester, testObtainParams) setShapesParams( "01010101-0101-0101-0101-010101010101", "01010101-0101-0101-0101-010101010102"); - vo_server_->start(); + startVOServer(); verifyMap(true); @@ -594,7 +605,7 @@ TEST_F(Tester, testIncorrectCircleParams) TEST_F(Tester, testAddShapes) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); @@ -676,7 +687,7 @@ TEST_F(Tester, testAddShapes) TEST_F(Tester, testRemoveShapes) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); @@ -762,7 +773,7 @@ TEST_F(Tester, testRemoveShapes) TEST_F(Tester, testAddIncorrectShapes) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); @@ -885,7 +896,7 @@ TEST_F(Tester, testAddIncorrectShapes) TEST_F(Tester, testRemoveIncorrectShapes) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); @@ -956,7 +967,7 @@ TEST_F(Tester, testOverlayMax) rclcpp::Parameter( "overlay_type", static_cast(nav2_map_server::OverlayType::OVERLAY_MAX))); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); @@ -1002,7 +1013,7 @@ TEST_F(Tester, testOverlayMin) rclcpp::Parameter( "overlay_type", static_cast(nav2_map_server::OverlayType::OVERLAY_MIN))); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); @@ -1048,7 +1059,7 @@ TEST_F(Tester, testOverlaySeq) rclcpp::Parameter( "overlay_type", static_cast(nav2_map_server::OverlayType::OVERLAY_SEQ))); - vo_server_->start(); + startVOServer(); // Sequentially add polygon and then circle overlapped with it on map auto add_shapes_msg = std::make_shared(); @@ -1104,7 +1115,7 @@ TEST_F(Tester, testOverlayUnknown) setVOServerParams(); vo_server_->set_parameter( rclcpp::Parameter("overlay_type", static_cast(1000))); - vo_server_->start(); + startVOServer(); // Try to add polygon on map auto add_shapes_msg = std::make_shared(); @@ -1131,7 +1142,7 @@ TEST_F(Tester, testOverlayUnknown) TEST_F(Tester, testShapesMove) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Wait for the initial map to be received to not get it in later updates const std::chrono::nanoseconds timeout = @@ -1191,7 +1202,7 @@ TEST_F(Tester, testShapesMove) TEST_F(Tester, testSwitchDynamicStatic) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Wait for the initial map to be received to not get it in later updates const std::chrono::nanoseconds timeout = @@ -1465,7 +1476,7 @@ TEST_F(Tester, testEnforceGlobalFrameIdYamlShapesValid) TEST_F(Tester, switchProcessMap) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Wait for the initial map to be received to not get it in later updates const std::chrono::nanoseconds timeout = @@ -1517,25 +1528,27 @@ TEST_F(Tester, switchProcessMap) TEST_F(Tester, testIncorrectMapBoundaries) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Set min_x > max_x EXPECT_THROW(vo_server_->updateMap(1.0, 0.1, 0.1, 1.0), std::runtime_error); // Set min_y > max_y EXPECT_THROW(vo_server_->updateMap(0.1, 1.0, 1.0, 0.1), std::runtime_error); + + vo_server_->stop(); } TEST_F(Tester, testIncorrectMapBoundariesWhenNoShapes) { setVOServerParams(); - vo_server_->start(); + startVOServer(); double min_x, min_y, max_x, max_y; EXPECT_THROW(vo_server_->getMapBoundaries(min_x, min_y, max_x, max_y), std::runtime_error); } TEST_F(Tester, testShapeOutsideMap) { setVOServerParams(); - vo_server_->start(); + startVOServer(); // Add polygon and circle on map auto add_shapes_msg = std::make_shared(); diff --git a/nav2_map_server/test/unit/test_vector_object_shapes.cpp b/nav2_map_server/test/unit/test_vector_object_shapes.cpp index 43a6f43a58d..942e50defb2 100644 --- a/nav2_map_server/test/unit/test_vector_object_shapes.cpp +++ b/nav2_map_server/test/unit/test_vector_object_shapes.cpp @@ -22,6 +22,8 @@ #include #include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" #include "geometry_msgs/msg/point32.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -129,7 +131,16 @@ Tester::~Tester() polygon_.reset(); circle_.reset(); - node_.reset(); + // Shut down the lifecycle node properly to avoid "Node still in state(1) in destructor" warning + if (node_) { + if (node_->get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + node_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } + node_.reset(); + } tf_listener_.reset(); tf_buffer_.reset(); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 1f76dff5b0a..77ad64c6edf 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -48,6 +48,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory) auto my_sub = node->create_subscription( "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + my_sub->on_activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); @@ -112,6 +113,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) auto my_sub = node->create_subscription( "~/candidate_trajectories", [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;}); + my_sub->on_activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); @@ -149,6 +151,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath) auto my_sub = node->create_subscription( "~/optimal_path", [&](const nav_msgs::msg::Path msg) {received_path = msg;}); + my_sub->on_activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); diff --git a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp index bea95e7d56b..91215456c53 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -27,6 +27,8 @@ #include "nav2_ros_common/subscription.hpp" #include "nav2_ros_common/action_client.hpp" #include "rclcpp_action/client.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_ros_common/rate.hpp" namespace nav2 @@ -184,34 +186,47 @@ inline rclcpp::PublisherOptions createPublisherOptions( } /** - * @brief Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions + * @brief Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions. + * + * If the node is not a lifecycle node, the subscription will be auto-activated. + * * @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 callback_group The callback group to use (if provided) - * @return A shared pointer to the created subscription + * @return A shared pointer to the created nav2::Subscription */ template -typename nav2::Subscription::SharedPtr create_subscription( +typename nav2::Subscription::SharedPtr +create_subscription( const NodeT & 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); + 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)); + auto options = createSubscriptionOptions( + topic_name, allow_parameter_qos_overrides, callback_group); + + auto sub = std::make_shared>( + node, + topic_name, + std::forward(callback), + qos, + options); + + // Auto-activate if the node is not a base lifecycle node + auto lc_node = std::dynamic_pointer_cast(node); + if (!lc_node) { + // Not a lifecycle node, auto-activate the subscription + sub->on_activate(); + } + + return sub; } /** diff --git a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp index 26c7f9e2cf2..fa372086468 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -30,8 +30,10 @@ #include "rclcpp/rclcpp.hpp" #include "bondcpp/bond.hpp" #include "bond/msg/constants.hpp" +#include "nav2_ros_common/qos_profiles.hpp" #include "nav2_ros_common/interface_factories.hpp" #include "nav2_ros_common/rate.hpp" +#include "nav2_ros_common/subscription.hpp" namespace nav2 { @@ -140,29 +142,43 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode default_value, parameter_descriptor); } - /** - * @brief Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions - * @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 callback_group The callback group to use (if provided) - * @return A shared pointer to the created nav2::Subscription - */ - template< - typename MessageT, - typename CallbackT> - typename nav2::Subscription::SharedPtr +/** + * @brief Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions. + * The resulting subscription is managed/activated through the Nav2 interface factory. + * + * @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 callback_group The callback group to use (if provided) + * @return A shared pointer to the created nav2::Subscription + */ + template> + typename nav2::Subscription::SharedPtr create_subscription( const std::string & topic_name, CallbackT && callback, const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) { - return nav2::interfaces::create_subscription( - shared_from_this(), topic_name, - std::forward(callback), qos, callback_group); + auto sub = nav2::interfaces::create_subscription( + shared_from_this(), topic_name, std::forward(callback), qos, callback_group); + this->add_managed_entity(sub); + + // Auto-activate when the node is ACTIVE, or in the middle of ACTIVATING. + // The latter covers subscriptions created from callbacks that fire + // synchronously inside on_activate (e.g. transient_local replay during a + // newly-activated parent subscription's first spin). + auto state_id = get_current_state().id(); + if (state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || + state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) + { + sub->on_activate(); + } + + return sub; } + /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions * @param topic_name Name of topic @@ -181,9 +197,11 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode shared_from_this(), topic_name, qos, callback_group); this->add_managed_entity(pub); - // Automatically activate the publisher if the node is already active - if (get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + // Auto-activate when the node is ACTIVE, or in the middle of ACTIVATING + // (i.e. a publisher created from a callback running inside on_activate). + auto state_id = get_current_state().id(); + if (state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || + state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) { pub->on_activate(); } diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index b6b5bf64f0c..5014ceddf4e 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2025 Open Navigation LLC +// Copyright (c) 2026 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. @@ -16,18 +16,112 @@ #define NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ #include +#include +#include +#include + #include "rclcpp/rclcpp.hpp" +#include "rclcpp/get_message_type_support_handle.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" + +#include "nav2_ros_common/qos_profiles.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; +/// Lifecycle-managed subscription wrapper. +/// +/// The underlying rclcpp::Subscription is not constructed until on_activate() +/// is called, and is destroyed on on_deactivate(). This means the topic +/// endpoint is not discoverable on the ROS graph and no callbacks fire +/// until the wrapper is activated. +template> +class Subscription : public rclcpp_lifecycle::SimpleManagedEntity +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Subscription) + + using RclcppSub = rclcpp::Subscription; + using Options = rclcpp::SubscriptionOptionsWithAllocator; + using AnyCallback = rclcpp::AnySubscriptionCallback; + + template + Subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && user_callback, + const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), + const Options & options = Options{}) + : topic_name_(topic_name), + qos_(qos), + options_(options), + any_callback_(*options.get_allocator()), + topics_interface_(node->get_node_topics_interface()) + { + any_callback_.set(std::forward(user_callback)); + } + + void on_activate() override + { + rclcpp_lifecycle::SimpleManagedEntity::on_activate(); + if (sub_) { + return; + } + auto topics_if = topics_interface_.lock(); + if (!topics_if) { + throw std::runtime_error( + "nav2::Subscription: node topics interface expired before activation"); + } + + auto msg_mem_strat = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + auto options = options_; + auto callback = any_callback_; + rclcpp::SubscriptionFactory factory{ + [options, msg_mem_strat, callback]( + rclcpp::node_interfaces::NodeBaseInterface * node_base_ptr, + const std::string & topic, + const rclcpp::QoS & actual_qos) -> rclcpp::SubscriptionBase::SharedPtr { + auto sub = RclcppSub::make_shared( + node_base_ptr, + rclcpp::get_message_type_support_handle(), + topic, + actual_qos, + callback, + options, + msg_mem_strat, + nullptr); + sub->post_init_setup(node_base_ptr, actual_qos, options); + return std::dynamic_pointer_cast(sub); + } + }; + + auto sub_base = topics_if->create_subscription(topic_name_, factory, qos_); + topics_if->add_subscription(sub_base, options_.callback_group); + sub_ = std::dynamic_pointer_cast(sub_base); + } + + void on_deactivate() override + { + rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); + sub_.reset(); + } + + const char * get_topic_name() const noexcept + { + return topic_name_.c_str(); + } + +protected: + std::string topic_name_; + rclcpp::QoS qos_; + Options options_; + AnyCallback any_callback_; + std::weak_ptr topics_interface_; + typename RclcppSub::SharedPtr sub_; +}; } // namespace nav2 diff --git a/nav2_ros_common/test/CMakeLists.txt b/nav2_ros_common/test/CMakeLists.txt index a8acb1d67c2..321ee740fe3 100644 --- a/nav2_ros_common/test/CMakeLists.txt +++ b/nav2_ros_common/test/CMakeLists.txt @@ -104,6 +104,27 @@ target_link_libraries(test_lifecycle_node pluginlib::pluginlib ) +nav2_add_gtest(test_subscription_latched test_subscription_latched.cpp) +target_include_directories(test_subscription_latched PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../include +) +target_link_libraries(test_subscription_latched + bondcpp::bondcpp + ${geometry_msgs_TARGETS} + ${lifecycle_msgs_TARGETS} + ${nav2_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${rcl_interfaces_TARGETS} + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} + pluginlib::pluginlib + ${std_msgs_TARGETS} +) + nav2_add_gtest(test_validation_messages test_validation_messages.cpp) target_include_directories(test_validation_messages PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../include diff --git a/nav2_ros_common/test/test_actions.cpp b/nav2_ros_common/test/test_actions.cpp index 5bda2e7b891..1ea1d8ee073 100644 --- a/nav2_ros_common/test/test_actions.cpp +++ b/nav2_ros_common/test/test_actions.cpp @@ -46,29 +46,35 @@ class FibonacciServerNode : public rclcpp::Node "fibonacci", std::bind(&FibonacciServerNode::execute, this)); - deactivate_subs_ = create_subscription( - "deactivate_server", - 1, + deactivate_subs_ = nav2::interfaces::create_subscription( + shared_from_this(), + "deactivate_server", [this](std_msgs::msg::Empty::UniquePtr /*msg*/) { RCLCPP_INFO(this->get_logger(), "Deactivating"); action_server_->deactivate(); - }); + }, + rclcpp::QoS(1)); - activate_subs_ = create_subscription( - "activate_server", - 1, + activate_subs_ = nav2::interfaces::create_subscription( + shared_from_this(), + "activate_server", [this](std_msgs::msg::Empty::UniquePtr /*msg*/) { RCLCPP_INFO(this->get_logger(), "Activating"); action_server_->activate(); - }); + }, + rclcpp::QoS(1)); - omit_preempt_subs_ = create_subscription( - "omit_preemption", - 1, + omit_preempt_subs_ = nav2::interfaces::create_subscription( + shared_from_this(), + "omit_preemption", [this](std_msgs::msg::Empty::UniquePtr /*msg*/) { RCLCPP_INFO(this->get_logger(), "Ignoring preemptions"); do_premptions_ = false; - }); + }, + rclcpp::QoS(1)); + deactivate_subs_->on_activate(); + activate_subs_->on_activate(); + omit_preempt_subs_->on_activate(); } void on_term() diff --git a/nav2_ros_common/test/test_subscription_latched.cpp b/nav2_ros_common/test/test_subscription_latched.cpp new file mode 100644 index 00000000000..d01971d5ae1 --- /dev/null +++ b/nav2_ros_common/test/test_subscription_latched.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2026 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, +// 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. + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +#include "nav2_ros_common/qos_profiles.hpp" +#include "nav2_ros_common/subscription.hpp" + +using namespace std::chrono_literals; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SubscriptionLatched, LatchedMessageReceivedAfterActivation) +{ + const std::string topic_name = "test_latched_topic"; + constexpr int kExpectedData = 42; + + auto node = std::make_shared("test_subscription_latched_node"); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + + std::promise received_promise; + std::future received_future = received_promise.get_future(); + auto callback = [&received_promise](std_msgs::msg::Int32::ConstSharedPtr msg) { + received_promise.set_value(msg->data); + }; + + auto subscription = std::make_shared>( + node, + topic_name, + callback, + nav2::qos::LatchedSubscriptionQoS()); + subscription->on_activate(); + + auto publisher = node->create_publisher( + topic_name, + nav2::qos::LatchedPublisherQoS()); + + std_msgs::msg::Int32 msg; + msg.data = kExpectedData; + publisher->publish(msg); + + // Allow discovery and latched delivery + for (int i = 0; i < 20; ++i) { + executor.spin_some(50ms); + std::this_thread::sleep_for(10ms); + } + + + // Spin until callback invoked or timeout (2s) + auto status = received_future.wait_for(2s); + for (int i = 0; i < 100 && status != std::future_status::ready; ++i) { + executor.spin_some(20ms); + status = received_future.wait_for(20ms); + } + + ASSERT_EQ(status, std::future_status::ready) + << "Latched message was not received after subscription activation"; + EXPECT_EQ(received_future.get(), kExpectedData); +} diff --git a/nav2_route/include/nav2_route/edge_scorer.hpp b/nav2_route/include/nav2_route/edge_scorer.hpp index 7b6534b50c4..e0dd2a0036d 100644 --- a/nav2_route/include/nav2_route/edge_scorer.hpp +++ b/nav2_route/include/nav2_route/edge_scorer.hpp @@ -58,6 +58,18 @@ class EdgeScorer */ ~EdgeScorer() = default; + /** + * @brief Activate loaded edge cost function plugins so that any + * lifecycle resources they own (e.g. plugin-private CostmapSubscriber) + * are brought up. Forwarded from RouteServer::on_activate(). + */ + void activate(); + + /** + * @brief Deactivate loaded edge cost function plugins. + */ + void deactivate(); + /** * @brief Score the edge with the set of plugins * @param edge Ptr to edge for scoring diff --git a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp index e45980f3a45..81801a4ae18 100644 --- a/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp +++ b/nav2_route/include/nav2_route/interfaces/edge_cost_function.hpp @@ -60,6 +60,18 @@ class EdgeCostFunction std::shared_ptr costmap_subscriber, const std::string & name) = 0; + /** + * @brief Activate any lifecycle-managed resources owned by the plugin + * (e.g. CostmapSubscribers it created in configure()). Called by + * RouteServer on_activate. Default no-op. + */ + virtual void activate() {} + + /** + * @brief Deactivate the resources activated in activate(). Default no-op. + */ + virtual void deactivate() {} + /** * @brief Main scoring plugin API * @param edge The edge pointer to score, which has access to the diff --git a/nav2_route/include/nav2_route/interfaces/route_operation.hpp b/nav2_route/include/nav2_route/interfaces/route_operation.hpp index 4a5ba2affdc..5c39e1e11b2 100644 --- a/nav2_route/include/nav2_route/interfaces/route_operation.hpp +++ b/nav2_route/include/nav2_route/interfaces/route_operation.hpp @@ -88,6 +88,18 @@ class RouteOperation std::shared_ptr costmap_subscriber, const std::string & name) = 0; + /** + * @brief Activate any lifecycle-managed resources owned by the plugin + * (e.g. CostmapSubscribers it created itself in configure()). Called by + * RouteServer on_activate. Default no-op. + */ + virtual void activate() {} + + /** + * @brief Deactivate the resources activated in activate(). Default no-op. + */ + virtual void deactivate() {} + /** * @brief An API to get the name of a particular operation for triggering, query * or logging diff --git a/nav2_route/include/nav2_route/operations_manager.hpp b/nav2_route/include/nav2_route/operations_manager.hpp index 11f6edda21a..126898c4236 100644 --- a/nav2_route/include/nav2_route/operations_manager.hpp +++ b/nav2_route/include/nav2_route/operations_manager.hpp @@ -53,6 +53,18 @@ class OperationsManager */ ~OperationsManager() = default; + /** + * @brief Activate all loaded route operation plugins (forwarding from + * RouteServer::on_activate). Plugins that own lifecycle resources + * (e.g. their own CostmapSubscriber) bring them up here. + */ + void activate(); + + /** + * @brief Deactivate all loaded route operation plugins. + */ + void deactivate(); + /** * @brief Finds the set of operations stored in the graph to trigger at this transition * @param node Node to check diff --git a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp index 74375187cda..830feb099c4 100644 --- a/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp +++ b/nav2_route/include/nav2_route/plugins/edge_cost_functions/costmap_scorer.hpp @@ -54,6 +54,16 @@ class CostmapScorer : public EdgeCostFunction std::shared_ptr costmap_subscriber, const std::string & name) override; + /** + * @brief Activate the plugin-owned CostmapSubscriber if any. + */ + void activate() override; + + /** + * @brief Deactivate the plugin-owned CostmapSubscriber if any. + */ + void deactivate() override; + /** * @brief Main scoring plugin API * @param edge The edge pointer to score, which has access to the @@ -86,6 +96,8 @@ class CostmapScorer : public EdgeCostFunction std::shared_ptr costmap_subscriber_; std::shared_ptr costmap_{nullptr}; unsigned int check_resolution_ {1u}; + // True when this plugin allocated its own CostmapSubscriber in configure(). + bool owns_costmap_subscriber_{false}; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp b/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp index 0b392e49969..a7cbea4df1a 100644 --- a/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp +++ b/nav2_route/include/nav2_route/plugins/route_operations/collision_monitor.hpp @@ -66,6 +66,16 @@ class CollisionMonitor : public RouteOperation std::shared_ptr costmap_subscriber, const std::string & name) override; + /** + * @brief Activate the plugin-owned CostmapSubscriber if any. + */ + void activate() override; + + /** + * @brief Deactivate the plugin-owned CostmapSubscriber if any. + */ + void deactivate() override; + /** * @brief Get name of the plugin for parameter scope mapping * @return Name @@ -150,6 +160,10 @@ class CollisionMonitor : public RouteOperation unsigned int check_resolution_{1u}; std::shared_ptr costmap_subscriber_; std::shared_ptr costmap_{nullptr}; + // True when this plugin allocated its own CostmapSubscriber in configure(). + // The shared route-server subscriber is activated/deactivated by RouteServer, + // so we only fan out activate/deactivate to a subscriber we own. + bool owns_costmap_subscriber_{false}; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/route_planner.hpp b/nav2_route/include/nav2_route/route_planner.hpp index cd0c978bc90..a405e911c45 100644 --- a/nav2_route/include/nav2_route/route_planner.hpp +++ b/nav2_route/include/nav2_route/route_planner.hpp @@ -61,6 +61,16 @@ class RoutePlanner const std::shared_ptr tf_buffer, const std::shared_ptr costmap_subscriber); + /** + * @brief Activate edge scoring plugins (forwarded from RouteServer). + */ + void activate(); + + /** + * @brief Deactivate edge scoring plugins. + */ + void deactivate(); + /** * @brief Find the route from start to goal on the graph * @param graph Graph to search diff --git a/nav2_route/include/nav2_route/route_tracker.hpp b/nav2_route/include/nav2_route/route_tracker.hpp index 90e999c83fb..ca6d403b28c 100644 --- a/nav2_route/include/nav2_route/route_tracker.hpp +++ b/nav2_route/include/nav2_route/route_tracker.hpp @@ -64,6 +64,16 @@ class RouteTracker const std::string & route_frame, const std::string & base_frame); + /** + * @brief Activate route operation plugins (forwarded from RouteServer). + */ + void activate(); + + /** + * @brief Deactivate route operation plugins. + */ + void deactivate(); + /** * @brief Determine if a node is to be considered achieved at the current position * @param pose Current robot pose to query diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index d3513d21ebd..cb14831b27f 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -62,6 +62,20 @@ EdgeScorer::EdgeScorer( } } +void EdgeScorer::activate() +{ + for (auto & plugin : plugins_) { + plugin->activate(); + } +} + +void EdgeScorer::deactivate() +{ + for (auto & plugin : plugins_) { + plugin->deactivate(); + } +} + bool EdgeScorer::score( const EdgePtr edge, const RouteRequest & route_request, const EdgeType & edge_type, float & total_score) diff --git a/nav2_route/src/operations_manager.cpp b/nav2_route/src/operations_manager.cpp index d24d36594a4..b7b09a11ada 100644 --- a/nav2_route/src/operations_manager.cpp +++ b/nav2_route/src/operations_manager.cpp @@ -70,6 +70,32 @@ OperationsManager::OperationsManager( } } +void OperationsManager::activate() +{ + for (auto & op : query_operations_) { + op->activate(); + } + for (auto & op : change_operations_) { + op->activate(); + } + for (auto & kv : graph_operations_) { + kv.second->activate(); + } +} + +void OperationsManager::deactivate() +{ + for (auto & op : query_operations_) { + op->deactivate(); + } + for (auto & op : change_operations_) { + op->deactivate(); + } + for (auto & kv : graph_operations_) { + kv.second->deactivate(); + } +} + template void OperationsManager::findGraphOperationsToProcess( T & obj, const OperationTrigger & trigger, diff --git a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp index 8c645d94810..898d75daca2 100644 --- a/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp +++ b/nav2_route/src/plugins/edge_cost_functions/costmap_scorer.cpp @@ -57,6 +57,7 @@ void CostmapScorer::configure( if (costmap_topic != server_costmap_topic) { costmap_subscriber_ = std::make_shared( node, costmap_topic); + owns_costmap_subscriber_ = true; RCLCPP_INFO( node->get_logger(), "Using costmap topic: %s instead of server costmap topic: %s for CostmapScorer.", @@ -70,6 +71,20 @@ void CostmapScorer::configure( node->declare_or_get_parameter(getName() + ".weight", 1.0)); } +void CostmapScorer::activate() +{ + if (owns_costmap_subscriber_ && costmap_subscriber_) { + costmap_subscriber_->on_activate(); + } +} + +void CostmapScorer::deactivate() +{ + if (owns_costmap_subscriber_ && costmap_subscriber_) { + costmap_subscriber_->on_deactivate(); + } +} + void CostmapScorer::prepare() { try { diff --git a/nav2_route/src/plugins/route_operations/collision_monitor.cpp b/nav2_route/src/plugins/route_operations/collision_monitor.cpp index 4a3cf6a20de..1aad46faa9a 100644 --- a/nav2_route/src/plugins/route_operations/collision_monitor.cpp +++ b/nav2_route/src/plugins/route_operations/collision_monitor.cpp @@ -41,6 +41,7 @@ void CollisionMonitor::configure( "Using costmap topic: %s instead of server costmap topic: %s for CollisionMonitor.", costmap_topic.c_str(), server_costmap_topic.c_str()); costmap_subscriber_ = std::make_shared(node, costmap_topic); + owns_costmap_subscriber_ = true; topic_ = costmap_topic; } else { costmap_subscriber_ = costmap_subscriber; @@ -69,6 +70,20 @@ void CollisionMonitor::configure( } } +void CollisionMonitor::activate() +{ + if (owns_costmap_subscriber_ && costmap_subscriber_) { + costmap_subscriber_->on_activate(); + } +} + +void CollisionMonitor::deactivate() +{ + if (owns_costmap_subscriber_ && costmap_subscriber_) { + costmap_subscriber_->on_deactivate(); + } +} + void CollisionMonitor::getCostmap() { try { diff --git a/nav2_route/src/route_planner.cpp b/nav2_route/src/route_planner.cpp index 592b14917e3..9f104291931 100644 --- a/nav2_route/src/route_planner.cpp +++ b/nav2_route/src/route_planner.cpp @@ -38,6 +38,16 @@ void RoutePlanner::configure( edge_scorer_ = std::make_unique(node, tf_buffer, costmap_subscriber); } +void RoutePlanner::activate() +{ + if (edge_scorer_) {edge_scorer_->activate();} +} + +void RoutePlanner::deactivate() +{ + if (edge_scorer_) {edge_scorer_->deactivate();} +} + Route RoutePlanner::findRoute( Graph & graph, unsigned int start_index, unsigned int goal_index, const std::vector & blocked_ids, diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 68f435c2fc2..0b5aded01a5 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -104,6 +104,9 @@ RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); compute_route_server_->activate(); compute_and_track_route_server_->activate(); + if (costmap_subscriber_) {costmap_subscriber_->on_activate();} + if (route_planner_) {route_planner_->activate();} + if (route_tracker_) {route_tracker_->activate();} graph_vis_publisher_->on_activate(); graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); route_publisher_->on_activate(); @@ -117,6 +120,9 @@ RouteServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); compute_route_server_->deactivate(); compute_and_track_route_server_->deactivate(); + if (route_tracker_) {route_tracker_->deactivate();} + if (route_planner_) {route_planner_->deactivate();} + if (costmap_subscriber_) {costmap_subscriber_->on_deactivate();} graph_vis_publisher_->on_deactivate(); route_publisher_->on_deactivate(); destroyBond(); diff --git a/nav2_route/src/route_tracker.cpp b/nav2_route/src/route_tracker.cpp index 07a41cc0e31..50a1f65fa82 100644 --- a/nav2_route/src/route_tracker.cpp +++ b/nav2_route/src/route_tracker.cpp @@ -45,6 +45,16 @@ void RouteTracker::configure( operations_manager_ = std::make_unique(node, costmap_subscriber); } +void RouteTracker::activate() +{ + if (operations_manager_) {operations_manager_->activate();} +} + +void RouteTracker::deactivate() +{ + if (operations_manager_) {operations_manager_->deactivate();} +} + geometry_msgs::msg::PoseStamped RouteTracker::getRobotPose() { geometry_msgs::msg::PoseStamped pose; diff --git a/nav2_route/test/test_collision_operation.cpp b/nav2_route/test/test_collision_operation.cpp index 3dcc99b5465..a085fc37ae0 100644 --- a/nav2_route/test/test_collision_operation.cpp +++ b/nav2_route/test/test_collision_operation.cpp @@ -163,6 +163,10 @@ TEST(TestCollisionMonitor, test_costmap_apis) { auto node = std::make_shared("test"); node->declare_parameter("costmap_topic", rclcpp::ParameterValue("dummy_topic")); + node->declare_parameter("name.costmap_topic", + rclcpp::ParameterValue("local_costmap/costmap_raw")); + node->configure(); + node->activate(); auto node_thread = std::make_unique(node); std::shared_ptr costmap_subscriber; CollisionMonitorWrapper monitor; diff --git a/nav2_route/test/test_edge_scorers.cpp b/nav2_route/test/test_edge_scorers.cpp index 8c16ced8306..f0a0b63c20d 100644 --- a/nav2_route/test/test_edge_scorers.cpp +++ b/nav2_route/test/test_edge_scorers.cpp @@ -213,6 +213,8 @@ TEST(EdgeScorersTest, test_costmap_scoring) // Test Penalty scorer plugin loading + penalizing on metadata values auto node = std::make_shared("edge_scorer_test"); node->declare_parameter("costmap_topic", "dummy_topic"); + node->configure(); + node->activate(); auto node_thread = std::make_unique(node); std::shared_ptr tf_buffer; @@ -328,6 +330,8 @@ TEST(EdgeScorersTest, test_costmap_scoring_alt_profile) // Test Penalty scorer plugin loading + penalizing on metadata values auto node = std::make_shared("edge_scorer_test"); node->declare_parameter("costmap_topic", "dummy_costmap/costmap_raw"); + node->configure(); + node->activate(); auto node_thread = std::make_unique(node); std::shared_ptr tf_buffer; diff --git a/nav2_route/test/test_operations.cpp b/nav2_route/test/test_operations.cpp index 3b35fea6b96..0fadaf853d6 100644 --- a/nav2_route/test/test_operations.cpp +++ b/nav2_route/test/test_operations.cpp @@ -137,6 +137,7 @@ TEST(OperationsManagerTest, test_processing_speed_on_status) auto sub = node->create_subscription( "speed_limit", [&, this](nav2_msgs::msg::SpeedLimit msg) {got_msg = true; my_msg = msg;}); + sub->on_activate(); Node node2; DirectionalEdge enter; diff --git a/nav2_route/test/test_path_converter.cpp b/nav2_route/test/test_path_converter.cpp index d8af7736cc8..43af47ca12e 100644 --- a/nav2_route/test/test_path_converter.cpp +++ b/nav2_route/test/test_path_converter.cpp @@ -40,6 +40,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;}); + sub->on_activate(); PathConverter converter; converter.configure(node); diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index b425a307368..f18b1ef6683 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -29,6 +29,7 @@ #include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_ros_common/interface_factories.hpp" #include "rviz_common/panel.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -109,8 +110,8 @@ private Q_SLOTS: // Docking / Undocking action feedback subscribers nav2::Subscription::SharedPtr docking_feedback_sub_; nav2::Subscription::SharedPtr undocking_feedback_sub_; - nav2::Subscription::SharedPtr docking_goal_status_sub_; - nav2::Subscription::SharedPtr undocking_goal_status_sub_; + nav2::Subscription::SharedPtr docking_goal_status_sub_; + nav2::Subscription::SharedPtr undocking_goal_status_sub_; // Goal related state DockGoalHandle::SharedPtr dock_goal_handle_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 22ee63e7a38..4577199719b 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -32,6 +32,7 @@ #include "nav2_rviz_plugins/ros_action_qevent.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_ros_common/interface_factories.hpp" #include "rviz_common/panel.hpp" #include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -150,9 +151,9 @@ private Q_SLOTS: navigation_feedback_sub_; nav2::Subscription::SharedPtr nav_through_poses_feedback_sub_; - nav2::Subscription::SharedPtr + nav2::Subscription::SharedPtr navigation_goal_status_sub_; - nav2::Subscription::SharedPtr + nav2::Subscription::SharedPtr nav_through_poses_goal_status_sub_; // Tf's for initial pose diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp index c6c46c3266e..5402609cc91 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp @@ -27,6 +27,7 @@ #include "nav2_route/types.hpp" #include "nav2_route/utils.hpp" #include "rclcpp/rclcpp.hpp" +#include "nav2_ros_common/interface_factories.hpp" #include "rviz_common/panel.hpp" #include "std_msgs/msg/int16.hpp" #include "std_msgs/msg/string.hpp" diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 95e0bb516c7..5ed1262c99c 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -277,17 +277,17 @@ void DockingPanel::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); // Create action feedback subscriber - docking_feedback_sub_ = node->create_subscription( - "dock_robot/_action/feedback", - rclcpp::SystemDefaultsQoS(), + docking_feedback_sub_ = nav2::interfaces::create_subscription( + node, "dock_robot/_action/feedback", [this](const Dock::Impl::FeedbackMessage::ConstSharedPtr & msg) { docking_feedback_indicator_->setText(getDockFeedbackLabel(msg->feedback)); - }); + }, + rclcpp::SystemDefaultsQoS()); // Create action goal status subscribers - docking_goal_status_sub_ = node->create_subscription( - "dock_robot/_action/status", - rclcpp::SystemDefaultsQoS(), + docking_goal_status_sub_ = + nav2::interfaces::create_subscription( + node, "dock_robot/_action/status", [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); @@ -295,15 +295,17 @@ void DockingPanel::onInitialize() if (msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { docking_feedback_indicator_->setText(getDockFeedbackLabel()); } - }); + }, + rclcpp::SystemDefaultsQoS()); - undocking_goal_status_sub_ = node->create_subscription( - "undock_robot/_action/status", - rclcpp::SystemDefaultsQoS(), + undocking_goal_status_sub_ = + nav2::interfaces::create_subscription( + node, "undock_robot/_action/status", [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { docking_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); - }); + }, + rclcpp::SystemDefaultsQoS()); } void DockingPanel::startThread() diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index a0f18df1665..7c30a19c7b3 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -894,9 +894,8 @@ Nav2Panel::onInitialize() // create action feedback subscribers navigation_feedback_sub_ = - node->create_subscription( - "navigate_to_pose/_action/feedback", - rclcpp::SystemDefaultsQoS(), + nav2::interfaces::create_subscription( + node, "navigate_to_pose/_action/feedback", [this](const nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage::ConstSharedPtr & msg) { if (stoi(nr_of_loops_->displayText().toStdString()) > 0) { if (goal_index_ == 0 && !loop_counter_stop_) { @@ -917,20 +916,22 @@ Nav2Panel::onInitialize() } else { navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel(msg->feedback)); } - }); + }, + rclcpp::SystemDefaultsQoS()); nav_through_poses_feedback_sub_ = - node->create_subscription( - "navigate_through_poses/_action/feedback", - rclcpp::SystemDefaultsQoS(), + nav2::interfaces::create_subscription< + nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>( + node, "navigate_through_poses/_action/feedback", [this](const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::ConstSharedPtr & msg) { navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel(msg->feedback)); - }); + }, + rclcpp::SystemDefaultsQoS()); // create action goal status subscribers - navigation_goal_status_sub_ = node->create_subscription( - "navigate_to_pose/_action/status", - rclcpp::SystemDefaultsQoS(), + navigation_goal_status_sub_ = + nav2::interfaces::create_subscription( + node, "navigate_to_pose/_action/status", [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { navigation_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); @@ -946,17 +947,19 @@ Nav2Panel::onInitialize() loop_count_ = 0; navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel()); } - }); - nav_through_poses_goal_status_sub_ = node->create_subscription( - "navigate_through_poses/_action/status", - rclcpp::SystemDefaultsQoS(), + }, + rclcpp::SystemDefaultsQoS()); + nav_through_poses_goal_status_sub_ = + nav2::interfaces::create_subscription( + node, "navigate_through_poses/_action/status", [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { navigation_goal_status_indicator_->setText( nav2_rviz_plugins::getGoalStatusLabel("Feedback", msg->status_list.back().status)); if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) { navigation_feedback_indicator_->setText(getNavThroughPosesFeedbackLabel()); } - }); + }, + rclcpp::SystemDefaultsQoS()); } void diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp index fa71310b8dd..be314aed3a4 100644 --- a/nav2_rviz_plugins/src/route_tool.cpp +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -54,8 +54,9 @@ void RouteTool::onInitialize(void) } auto node = ros_node_abstraction->get_raw_node(); - clicked_point_subscription_ = node->create_subscription( - "clicked_point", 1, [this](const geometry_msgs::msg::PointStamped::ConstSharedPtr & msg) { + clicked_point_subscription_ = + nav2::interfaces::create_subscription( + node, "clicked_point", [this](const geometry_msgs::msg::PointStamped::ConstSharedPtr & msg) { ui_->add_field_1->setText(std::to_string(msg->point.x).c_str()); ui_->add_field_2->setText(std::to_string(msg->point.y).c_str()); ui_->edit_field_1->setText(std::to_string(msg->point.x).c_str()); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 6497677243d..26423a3738f 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -104,6 +104,8 @@ TEST(SmacTest, test_smac_se2) unsmoothed_plan_received = true; received_unsmoothed_plan = msg; }); + expansions_sub->on_activate(); + unsmoothed_plan_sub->on_activate(); auto dummy_cancel_checker = []() { return false; diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index 612b47a0b66..d36f295bcc7 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -116,6 +116,8 @@ TEST(SmacTest, test_smac_lattice) unsmoothed_plan_received = true; received_unsmoothed_plan = msg; }); + expansions_sub->on_activate(); + unsmoothed_plan_sub->on_activate(); auto dummy_cancel_checker = []() { return false; diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp index 6dfd78caed2..2dec5cca096 100644 --- a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp @@ -24,6 +24,8 @@ #include "assisted_teleop_behavior_tester.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_ros_common/interface_factories.hpp" +#include "nav2_ros_common/qos_profiles.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -59,15 +61,18 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() cmd_vel_pub_ = node_->create_publisher("cmd_vel_teleop", 10); - subscription_ = node_->create_subscription( - "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + subscription_ = nav2::interfaces::create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + node_, + "amcl_pose", + std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1), + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - filtered_vel_sub_ = node_->create_subscription( + filtered_vel_sub_ = nav2::interfaces::create_subscription( + node_, "cmd_vel", - rclcpp::SystemDefaultsQoS(), - std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1)); - + std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1), + rclcpp::SystemDefaultsQoS()); std::string costmap_topic = "/local_costmap/costmap_raw"; std::string footprint_topic = "/local_costmap/published_footprint"; @@ -125,6 +130,9 @@ void AssistedTeleopBehaviorTester::activate() return; } + filtered_vel_sub_->on_activate(); + footprint_sub_->on_activate(); + RCLCPP_INFO(this->node_->get_logger(), "Assisted Teleop action server is ready"); is_active_ = true; } @@ -134,6 +142,8 @@ void AssistedTeleopBehaviorTester::deactivate() if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); } + filtered_vel_sub_->on_deactivate(); + footprint_sub_->on_deactivate(); is_active_ = false; } diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index 8ac24efa7ec..6925a9c52fd 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -53,9 +53,12 @@ WaitBehaviorTester::WaitBehaviorTester() publisher_ = node_->create_publisher("initialpose", 10); - subscription_ = node_->create_subscription( - "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); + subscription_ = nav2::interfaces::create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + node_, + "amcl_pose", + std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1), + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); } WaitBehaviorTester::~WaitBehaviorTester() diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp index 4df39c42aff..dec55f813f4 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -31,7 +31,7 @@ #include "nav2_ros_common/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" - +#include "nav2_ros_common/interface_factories.hpp" #include "tf2/utils.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 7cf7b5b35c4..4540b75c145 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -19,6 +19,7 @@ #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_ros_common/interface_factories.hpp" using namespace std::chrono_literals; @@ -41,9 +42,13 @@ class TestAmclPose : public ::testing::Test initial_pose_pub_ = node->create_publisher( "initialpose", rclcpp::SystemDefaultsQoS()); - subscription_ = node->create_subscription( - "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&TestAmclPose::amcl_pose_callback, this, std::placeholders::_1)); + subscription_ = nav2::interfaces::create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( + node, + "amcl_pose", + std::bind(&TestAmclPose::amcl_pose_callback, this, std::placeholders::_1), + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + initial_pose_pub_->publish(testPose_); } diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index 6abec7058ad..60d9e97e2be 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -158,8 +158,9 @@ TEST(testPluginMap, Failures) rclcpp_lifecycle::State state; obj->declare_parameter("expected_planner_frequency", rclcpp::ParameterValue(100000.0)); obj->onConfigure(state); - obj->create_subscription( + auto plan_sub = obj->create_subscription( "plan", callback); + plan_sub->on_activate(); geometry_msgs::msg::PoseStamped start; geometry_msgs::msg::PoseStamped goal; diff --git a/nav2_util/include/nav2_util/lifecycle_service_client.hpp b/nav2_util/include/nav2_util/lifecycle_service_client.hpp index 024b4d8eb29..dd1fe11f4a4 100644 --- a/nav2_util/include/nav2_util/lifecycle_service_client.hpp +++ b/nav2_util/include/nav2_util/lifecycle_service_client.hpp @@ -44,9 +44,10 @@ class LifecycleServiceClient get_state_(lifecycle_node_name + "/get_state", parent_node, true /*creates and spins an internal executor*/) { - // Block until server is up + // Block until server is up, but bail out if the context is shutting down + // so we don't spin forever after SIGINT during construction. rclcpp::Rate r(20); - while (!get_state_.wait_for_service(2s)) { + while (rclcpp::ok() && !get_state_.wait_for_service(2s)) { RCLCPP_INFO( parent_node->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str()); diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 910e6c7238c..6a9d8d5d27b 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -139,6 +139,22 @@ class OdomSmoother return twist_stamped; } + /** + * @brief Activate the odometry subscription. Call when the node is activated. + */ + void on_activate() + { + odom_sub_->on_activate(); + } + + /** + * @brief Deactivate the odometry subscription. Call when the node is deactivated. + */ + void on_deactivate() + { + odom_sub_->on_deactivate(); + } + protected: /** * @brief Callback of odometry subscriber to process diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index c65ac526d08..77c36a0cb56 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -132,6 +132,32 @@ class TwistSubscriber } } + /** + * @brief Activate the subscription (calls nav2::Subscription::on_activate). + * Call at the same lifecycle point as publisher on_activate() (e.g. in tests). + */ + void on_activate() + { + if (twist_stamped_sub_) { + twist_stamped_sub_->on_activate(); + } else if (twist_sub_) { + twist_sub_->on_activate(); + } + } + + /** + * @brief Deactivate the subscription (calls nav2::Subscription::on_deactivate). + * Call at the same lifecycle point as publisher on_deactivate(). + */ + void on_deactivate() + { + if (twist_stamped_sub_) { + twist_stamped_sub_->on_deactivate(); + } else if (twist_sub_) { + twist_sub_->on_deactivate(); + } + } + protected: //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel bool is_stamped_{true}; diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index eaac1c1d23d..5f7790de495 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -66,6 +66,7 @@ TEST(OdometryUtils, test_smoothed_velocity) odom_pub->on_activate(); nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); + odom_smoother.on_activate(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); nav_msgs::msg::Odometry odom_msg; diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index ba046d47eb0..3f72e254957 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -165,13 +165,23 @@ nav2::CallbackReturn VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Activating"); + + // 1. Activate all input subscriptions first + cmd_sub_->on_activate(); + if (odom_smoother_) { + odom_smoother_->on_activate(); + } + + // 2. Activate publishers smoothed_cmd_pub_->on_activate(); + + // 3. Start timer double timer_duration_ms = 1000.0 / smoothing_frequency_; timer_ = this->create_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); - // Add callback for dynamic parameters + // 4. Add callback for dynamic parameters auto node = shared_from_this(); post_set_params_handler_ = node->add_post_set_parameters_callback( std::bind( @@ -182,7 +192,7 @@ VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) &VelocitySmoother::validateParameterUpdatesCallback, this, std::placeholders::_1)); - // create bond connection + // 5. Create bond connection last createBond(); return nav2::CallbackReturn::SUCCESS; } @@ -196,6 +206,10 @@ VelocitySmoother::on_deactivate(const rclcpp_lifecycle::State &) timer_.reset(); } smoothed_cmd_pub_->on_deactivate(); + cmd_sub_->on_deactivate(); + if (odom_smoother_) { + odom_smoother_->on_deactivate(); + } auto node = shared_from_this(); if (post_set_params_handler_ && node) { diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index c07e125b662..7cd963ec24e 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -88,6 +88,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer6dof) }, [&](geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { linear_vels.push_back(msg->twist.linear.x); }); + subscription.on_activate(); // subscription created after node activate() // Send a velocity command auto cmd = std::make_shared(); @@ -146,6 +147,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer) }, [&](geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { linear_vels.push_back(msg->twist.linear.x); }); + subscription.on_activate(); // subscription created after node activate() // Send a velocity command auto cmd = std::make_shared(); @@ -201,6 +203,7 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) }, [&](geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { linear_vels.push_back(msg->twist.linear.x); }); + subscription.on_activate(); // subscription created after node activate() auto odom_pub = smoother->create_publisher("odom"); odom_pub->on_activate(); diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp index a28567b17a0..93bda78cc38 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/input_at_waypoint.hpp @@ -55,6 +55,17 @@ class InputAtWaypoint : public nav2_core::WaypointTaskExecutor const nav2::LifecycleNode::WeakPtr & parent, const std::string & plugin_name); + /** + * @brief Activate the subscription. Call at same lifecycle point as publisher on_activate() + * (e.g. from WaypointFollower::on_activate or in tests). + */ + void on_activate() override; + + /** + * @brief Deactivate the subscription. + */ + void on_deactivate() override; + /** * @brief Processor * @param curr_pose current pose of the robot diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 485fc7157ee..e1e3dbf92ef 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -58,6 +58,16 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor const nav2::LifecycleNode::WeakPtr & parent, const std::string & plugin_name); + /** + * @brief Activate the subscription. Call at same lifecycle point as publisher on_activate() + * (e.g. from WaypointFollower::on_activate or in tests). + */ + void on_activate() override; + + /** + * @brief Deactivate the subscription. + */ + void on_deactivate() override; /** * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint diff --git a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp index a12d850e4f8..b8321f78a07 100644 --- a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp @@ -67,6 +67,16 @@ void InputAtWaypoint::initialize( input_topic, std::bind(&InputAtWaypoint::Cb, this, _1)); } +void InputAtWaypoint::on_activate() +{ + subscription_->on_activate(); +} + +void InputAtWaypoint::on_deactivate() +{ + subscription_->on_deactivate(); +} + void InputAtWaypoint::Cb(const std_msgs::msg::Empty::ConstSharedPtr & /*msg*/) { std::lock_guard lock(mutex_); diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp index 5368302f5aa..fd2bed73d17 100644 --- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp @@ -89,6 +89,20 @@ void PhotoAtWaypoint::initialize( } } +void PhotoAtWaypoint::on_activate() +{ + if (camera_image_subscriber_) { + camera_image_subscriber_->on_activate(); + } +} + +void PhotoAtWaypoint::on_deactivate() +{ + if (camera_image_subscriber_) { + camera_image_subscriber_->on_deactivate(); + } +} + bool PhotoAtWaypoint::processAtWaypoint( const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index) { diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index b60f39cbce9..8969d57b8ca 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -99,12 +99,12 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + waypoint_task_executor_->on_activate(); xyz_action_server_->activate(); gps_action_server_->activate(); // create bond connection createBond(); - return nav2::CallbackReturn::SUCCESS; } @@ -113,6 +113,7 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); + waypoint_task_executor_->on_deactivate(); xyz_action_server_->deactivate(); gps_action_server_->deactivate(); // destroy bond connection diff --git a/nav2_waypoint_follower/test/test_task_executors.cpp b/nav2_waypoint_follower/test/test_task_executors.cpp index acb929c8aa8..309104d5e8b 100644 --- a/nav2_waypoint_follower/test/test_task_executors.cpp +++ b/nav2_waypoint_follower/test/test_task_executors.cpp @@ -76,6 +76,7 @@ TEST(WaypointFollowerTest, InputAtWaypoint) new nav2_waypoint_follower::InputAtWaypoint ); iaw->initialize(node, std::string("IAW")); + iaw->on_activate(); executor.add_node(node->shared_from_this()->get_node_base_interface()); auto start_time = node->now(); @@ -139,6 +140,8 @@ TEST(WaypointFollowerTest, PhotoAtWaypoint) new nav2_waypoint_follower::PhotoAtWaypoint ); paw->initialize(node, std::string("PAW")); + // Caller (test): activate subscription so it receives messages (same as pub->on_activate()). + paw->on_activate(); executor.add_node(node->shared_from_this()->get_node_base_interface()); // no images, throws because can't write