From a0d374f47756a4c7be73450eff7654ec47b23f0b Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 3 Jan 2026 06:23:25 +0000 Subject: [PATCH 01/37] nav2_ros_common: add lifecycle-managed Subscription wrapper Signed-off-by: lotusymt --- .../nav2_ros_common/interface_factories.hpp | 18 +-- .../include/nav2_ros_common/subscription.hpp | 142 +++++++++++++++++- nav2_ros_common/test/test_actions.cpp | 27 ++-- 3 files changed, 159 insertions(+), 28 deletions(-) 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..ab8d5fd07fd 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -190,7 +190,7 @@ inline rclcpp::PublisherOptions createPublisherOptions( * @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( @@ -201,17 +201,13 @@ typename nav2::Subscription::SharedPtr create_subscription( const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) { bool allow_parameter_qos_overrides = nav2::declare_or_get_parameter( - node, "allow_parameter_qos_overrides", true); + 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); + + return std::make_shared>( + node, topic_name, qos, std::forward(callback), options); } /** diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index b6b5bf64f0c..c9ed59dc7ff 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -15,19 +15,151 @@ #ifndef NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ #define NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ +#include #include +#include +#include + #include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" namespace nav2 { /** - * @brief A ROS 2 subscription for Nav2 - * This is a convenience type alias to simplify the use of subscriptions in Nav2 - * which may be further built up on in the future with custom APIs. - */ + * @class nav2::Subscription + * @brief A lifecycle-enabled ROS 2 subscription for Nav2 + * + * - Implements lifecycle transitions via SimpleManagedEntity + * - Gates user callbacks using is_activated() + * - For NON-lifecycle nodes (plain rclcpp::Node), it auto-activates so behavior matches old code. + */ template -using Subscription = rclcpp::Subscription; +class Subscription : public rclcpp_lifecycle::SimpleManagedEntity +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(Subscription) + + template + Subscription( + const NodeT & node, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && user_callback, + const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions{}) + : topic_name_(topic_name), + logger_(node->get_logger()), + should_log_(true) + { + auto wrapped_cb = make_wrapped_callback(std::forward(user_callback)); + + sub_ = node->template create_subscription( + topic_name_, qos, std::move(wrapped_cb), options); + + // If this is NOT a lifecycle node, there will be no lifecycle manager to call on_activate(). + // To keep old behavior (callbacks run immediately), we auto-activate. + auto maybe_lc = std::dynamic_pointer_cast(node); + if (!maybe_lc) { + this->on_activate(); + } + } + + void on_activate() override + { + rclcpp_lifecycle::SimpleManagedEntity::on_activate(); + // No need to touch should_log_ here (logging happens only when inactive). + } + + void on_deactivate() override + { + rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); + // New inactive period -> allow one warning again + should_log_.store(true); + } + +private: + template + auto make_wrapped_callback(CallbackU && user_callback) + { + using CB = std::decay_t; + using UniquePtr = typename MessageT::UniquePtr; + using SharedPtr = typename MessageT::SharedPtr; + using ConstSharedPtr = typename MessageT::ConstSharedPtr; + using MsgInfo = const rclcpp::MessageInfo &; + + // Use shared_ptr so the lambda stays COPIABLE (rclcpp often requires copiable callbacks) + auto cb_ptr = std::make_shared(std::forward(user_callback)); + + if constexpr (std::is_invocable_v) { + return [this, cb_ptr](UniquePtr msg) { + if (!this->is_activated()) { + log_subscription_not_enabled_once(); + return; + } + should_log_.store(true); + (*cb_ptr)(std::move(msg)); + }; + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](SharedPtr msg) { + if (!this->is_activated()) { + log_subscription_not_enabled_once(); + return; + } + should_log_.store(true); + (*cb_ptr)(std::move(msg)); + }; + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](ConstSharedPtr msg) { + if (!this->is_activated()) { + log_subscription_not_enabled_once(); + return; + } + should_log_.store(true); + (*cb_ptr)(std::move(msg)); + }; + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](UniquePtr msg, MsgInfo info) { + if (!this->is_activated()) { + log_subscription_not_enabled_once(); + return; + } + should_log_.store(true); + (*cb_ptr)(std::move(msg), info); + }; + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](SharedPtr msg, MsgInfo info) { + if (!this->is_activated()) { + log_subscription_not_enabled_once(); + return; + } + should_log_.store(true); + (*cb_ptr)(std::move(msg), info); + }; + } else { + static_assert( + sizeof(CB) == 0, + "nav2::Subscription: unsupported callback signature for this MessageT"); + } + } + + void log_subscription_not_enabled_once() + { + if (should_log_.exchange(false)) { + RCLCPP_WARN( + logger_, + "Subscription on [%s] is not enabled yet (node not activated). " + "Dropping messages until activation.", + topic_name_.c_str()); + } + } + + + typename rclcpp::Subscription::SharedPtr sub_; + std::string topic_name_; + rclcpp::Logger logger_; + std::atomic should_log_; +}; } // namespace nav2 diff --git a/nav2_ros_common/test/test_actions.cpp b/nav2_ros_common/test/test_actions.cpp index 5bda2e7b891..f6e74f27186 100644 --- a/nav2_ros_common/test/test_actions.cpp +++ b/nav2_ros_common/test/test_actions.cpp @@ -46,29 +46,32 @@ 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)); } void on_term() From 0a73e71295d9ed923d8ef04f4617716869c213fb Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 3 Jan 2026 21:58:36 +0000 Subject: [PATCH 02/37] modify alias Signed-off-by: lotusymt --- .../include/nav2_ros_common/subscription.hpp | 27 ++++++++++--------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index c9ed59dc7ff..d3558cf8f35 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -83,16 +83,17 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity auto make_wrapped_callback(CallbackU && user_callback) { using CB = std::decay_t; - using UniquePtr = typename MessageT::UniquePtr; - using SharedPtr = typename MessageT::SharedPtr; - using ConstSharedPtr = typename MessageT::ConstSharedPtr; + using MsgUniquePtr = typename MessageT::UniquePtr; + using MsgSharedPtr = typename MessageT::SharedPtr; + using MsgConstSharedPtr = typename MessageT::ConstSharedPtr; + using MsgInfo = const rclcpp::MessageInfo &; // Use shared_ptr so the lambda stays COPIABLE (rclcpp often requires copiable callbacks) auto cb_ptr = std::make_shared(std::forward(user_callback)); - if constexpr (std::is_invocable_v) { - return [this, cb_ptr](UniquePtr msg) { + if constexpr (std::is_invocable_v) { + return [this, cb_ptr](MsgUniquePtr msg) { if (!this->is_activated()) { log_subscription_not_enabled_once(); return; @@ -100,8 +101,8 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity should_log_.store(true); (*cb_ptr)(std::move(msg)); }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](SharedPtr msg) { + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](MsgSharedPtr msg) { if (!this->is_activated()) { log_subscription_not_enabled_once(); return; @@ -109,8 +110,8 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity should_log_.store(true); (*cb_ptr)(std::move(msg)); }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](ConstSharedPtr msg) { + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](MsgConstSharedPtr msg) { if (!this->is_activated()) { log_subscription_not_enabled_once(); return; @@ -118,8 +119,8 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity should_log_.store(true); (*cb_ptr)(std::move(msg)); }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](UniquePtr msg, MsgInfo info) { + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](MsgUniquePtr msg, MsgInfo info) { if (!this->is_activated()) { log_subscription_not_enabled_once(); return; @@ -127,8 +128,8 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity should_log_.store(true); (*cb_ptr)(std::move(msg), info); }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](SharedPtr msg, MsgInfo info) { + } else if constexpr (std::is_invocable_v) { + return [this, cb_ptr](MsgSharedPtr msg, MsgInfo info) { if (!this->is_activated()) { log_subscription_not_enabled_once(); return; From 9f5f9eb7277961f3905e75e6a96541b549509c85 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sun, 4 Jan 2026 07:00:50 +0000 Subject: [PATCH 03/37] fix bugs Signed-off-by: lotusymt --- .../include/nav2_ros_common/subscription.hpp | 44 +++++++++++-------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index d3558cf8f35..fc285167c52 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rclcpp/rclcpp.hpp" @@ -41,9 +42,10 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) + // Single unified constructor: (topic, qos, callback, options) template Subscription( - const NodeT & node, + const std::shared_ptr & node, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && user_callback, @@ -52,33 +54,40 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity logger_(node->get_logger()), should_log_(true) { - auto wrapped_cb = make_wrapped_callback(std::forward(user_callback)); - - sub_ = node->template create_subscription( - topic_name_, qos, std::move(wrapped_cb), options); - - // If this is NOT a lifecycle node, there will be no lifecycle manager to call on_activate(). - // To keep old behavior (callbacks run immediately), we auto-activate. - auto maybe_lc = std::dynamic_pointer_cast(node); - if (!maybe_lc) { - this->on_activate(); - } + init(node, qos, std::forward(user_callback), options); } void on_activate() override { rclcpp_lifecycle::SimpleManagedEntity::on_activate(); - // No need to touch should_log_ here (logging happens only when inactive). } void on_deactivate() override { rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); - // New inactive period -> allow one warning again should_log_.store(true); } private: + template + void init( + const std::shared_ptr & node, + const rclcpp::QoS & qos, + CallbackT && user_callback, + const rclcpp::SubscriptionOptions & options) + { + auto wrapped_cb = make_wrapped_callback(std::forward(user_callback)); + auto rcl_node = std::static_pointer_cast(node); + sub_ = rcl_node->template create_subscription( + topic_name_, qos, std::move(wrapped_cb), options); + + // Auto-activate for non-lifecycle nodes to preserve old behavior + auto maybe_lc = std::dynamic_pointer_cast(node); + if (!maybe_lc) { + this->on_activate(); + } + } + template auto make_wrapped_callback(CallbackU && user_callback) { @@ -86,10 +95,9 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity using MsgUniquePtr = typename MessageT::UniquePtr; using MsgSharedPtr = typename MessageT::SharedPtr; using MsgConstSharedPtr = typename MessageT::ConstSharedPtr; - using MsgInfo = const rclcpp::MessageInfo &; - // Use shared_ptr so the lambda stays COPIABLE (rclcpp often requires copiable callbacks) + // Keep callback copiable auto cb_ptr = std::make_shared(std::forward(user_callback)); if constexpr (std::is_invocable_v) { @@ -138,8 +146,7 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity (*cb_ptr)(std::move(msg), info); }; } else { - static_assert( - sizeof(CB) == 0, + static_assert(sizeof(CB) == 0, "nav2::Subscription: unsupported callback signature for this MessageT"); } } @@ -155,7 +162,6 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity } } - typename rclcpp::Subscription::SharedPtr sub_; std::string topic_name_; rclcpp::Logger logger_; From 7953c21ad95295618bb2044a14a9814e6ac948c5 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Mon, 5 Jan 2026 04:22:33 +0000 Subject: [PATCH 04/37] Fix lifecycle subscription tests and add header guard Signed-off-by: lotusymt --- .../include/nav2_ros_common/subscription.hpp | 113 +++++------------- 1 file changed, 32 insertions(+), 81 deletions(-) diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index fc285167c52..6bdfd52ffb6 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. @@ -18,31 +18,22 @@ #include #include #include -#include #include #include "rclcpp/rclcpp.hpp" +#include "rclcpp/any_subscription_callback.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/managed_entity.hpp" namespace nav2 { -/** - * @class nav2::Subscription - * @brief A lifecycle-enabled ROS 2 subscription for Nav2 - * - * - Implements lifecycle transitions via SimpleManagedEntity - * - Gates user callbacks using is_activated() - * - For NON-lifecycle nodes (plain rclcpp::Node), it auto-activates so behavior matches old code. - */ template class Subscription : public rclcpp_lifecycle::SimpleManagedEntity { public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) - // Single unified constructor: (topic, qos, callback, options) template Subscription( const std::shared_ptr & node, @@ -52,7 +43,8 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions{}) : topic_name_(topic_name), logger_(node->get_logger()), - should_log_(true) + should_log_(true), + any_cb_(std::allocator{}) { init(node, qos, std::forward(user_callback), options); } @@ -76,81 +68,37 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity CallbackT && user_callback, const rclcpp::SubscriptionOptions & options) { - auto wrapped_cb = make_wrapped_callback(std::forward(user_callback)); - auto rcl_node = std::static_pointer_cast(node); - sub_ = rcl_node->template create_subscription( - topic_name_, qos, std::move(wrapped_cb), options); - - // Auto-activate for non-lifecycle nodes to preserve old behavior + any_cb_.set(std::forward(user_callback)); + + auto wrapped_cb = + [this](typename MessageT::ConstSharedPtr msg, const rclcpp::MessageInfo & info) + { + if (!this->is_activated()) { + log_subscription_not_enabled_once(); + return; + } + should_log_.store(true); + any_cb_.dispatch_intra_process(msg, info); + }; + + auto params_if = node->get_node_parameters_interface(); + auto topics_if = node->get_node_topics_interface(); + + sub_ = rclcpp::create_subscription( + params_if, + topics_if, + topic_name_, + qos, + std::move(wrapped_cb), + options); + + // Preserve old behavior for non-lifecycle nodes auto maybe_lc = std::dynamic_pointer_cast(node); if (!maybe_lc) { this->on_activate(); } } - template - auto make_wrapped_callback(CallbackU && user_callback) - { - using CB = std::decay_t; - using MsgUniquePtr = typename MessageT::UniquePtr; - using MsgSharedPtr = typename MessageT::SharedPtr; - using MsgConstSharedPtr = typename MessageT::ConstSharedPtr; - using MsgInfo = const rclcpp::MessageInfo &; - - // Keep callback copiable - auto cb_ptr = std::make_shared(std::forward(user_callback)); - - if constexpr (std::is_invocable_v) { - return [this, cb_ptr](MsgUniquePtr msg) { - if (!this->is_activated()) { - log_subscription_not_enabled_once(); - return; - } - should_log_.store(true); - (*cb_ptr)(std::move(msg)); - }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](MsgSharedPtr msg) { - if (!this->is_activated()) { - log_subscription_not_enabled_once(); - return; - } - should_log_.store(true); - (*cb_ptr)(std::move(msg)); - }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](MsgConstSharedPtr msg) { - if (!this->is_activated()) { - log_subscription_not_enabled_once(); - return; - } - should_log_.store(true); - (*cb_ptr)(std::move(msg)); - }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](MsgUniquePtr msg, MsgInfo info) { - if (!this->is_activated()) { - log_subscription_not_enabled_once(); - return; - } - should_log_.store(true); - (*cb_ptr)(std::move(msg), info); - }; - } else if constexpr (std::is_invocable_v) { - return [this, cb_ptr](MsgSharedPtr msg, MsgInfo info) { - if (!this->is_activated()) { - log_subscription_not_enabled_once(); - return; - } - should_log_.store(true); - (*cb_ptr)(std::move(msg), info); - }; - } else { - static_assert(sizeof(CB) == 0, - "nav2::Subscription: unsupported callback signature for this MessageT"); - } - } - void log_subscription_not_enabled_once() { if (should_log_.exchange(false)) { @@ -166,6 +114,9 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity std::string topic_name_; rclcpp::Logger logger_; std::atomic should_log_; + + // Type-erased, “official rclcpp way” to store/dispatch callbacks + rclcpp::AnySubscriptionCallback> any_cb_; }; } // namespace nav2 From 5e5f949a81c307030f72dee7a34874d41ba44a5f Mon Sep 17 00:00:00 2001 From: lotusymt Date: Mon, 12 Jan 2026 08:56:17 +0000 Subject: [PATCH 05/37] WIP: draft subscription factory lifecycle handling (design not final) Signed-off-by: lotusymt --- .../test/collision_monitor_node_bag.cpp | 44 +- nav2_collision_monitor/test/sources_test.cpp | 4 + nav2_common/CMakeLists.txt | 3 + .../test/unit/binary_filter_test.cpp | 11 +- .../test/unit/speed_filter_test.cpp | 17 +- .../test/unit/test_vector_object_server.cpp | 2 +- .../nav2_ros_common/interface_factories.hpp | 80 +- .../nav2_ros_common/lifecycle_node.hpp | 185 +- .../include/nav2_ros_common/subscription.hpp | 47 +- nav2_route/graphs/aws_graph.geojson | 1792 +++++++++++++++-- nav2_route/graphs/sample_graph.geojson | 581 +++++- .../assisted_teleop_behavior_tester.cpp | 19 +- .../behaviors/wait/wait_behavior_tester.cpp | 8 +- .../behaviors/wait/wait_behavior_tester.hpp | 2 +- .../localization/test_localization_node.cpp | 10 +- 15 files changed, 2481 insertions(+), 324 deletions(-) diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp index ee74024fc1b..b1dfca6d351 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; @@ -72,21 +75,26 @@ class MetricsCatcher : public rclcpp::Node { // 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();} - }); + }, + 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();} - }); + }, + 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,14 +102,18 @@ 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 @@ -229,10 +241,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_; diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 89b0a792f92..a330d0079b0 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(); diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 786c9449f45..8d1e7be819e 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -6,6 +6,9 @@ find_package(ament_cmake_core REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(backward_ros REQUIRED) +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +include(nav2_package) + ament_python_install_package(nav2_common) ament_package( diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index e9f312e5ca0..6897a507dea 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -36,6 +36,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; @@ -106,9 +108,12 @@ class BinaryStateSubscriber : public rclcpp::Node explicit BinaryStateSubscriber(const std::string & binary_state_topic, bool default_state) : Node("binary_state_sub"), binary_state_updated_(false) { - subscriber_ = this->create_subscription( - binary_state_topic, rclcpp::QoS(10), - std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1)); + subscriber_ = nav2::interfaces::create_subscription( + shared_from_this(), + binary_state_topic, + std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1), + rclcpp::QoS(10)); + // Initialize with default state msg_.data = default_state; diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 5fab85bd046..f56d5ea7778 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -104,13 +104,16 @@ class MaskPublisher : public rclcpp::Node class SpeedLimitSubscriber : public rclcpp::Node { public: - explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) - : Node("speed_limit_sub"), speed_limit_updated_(false) - { - subscriber_ = this->create_subscription( - speed_limit_topic, rclcpp::QoS(10), - std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1)); - } +explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) +: Node("speed_limit_sub"), speed_limit_updated_(false) +{ + subscriber_ = nav2::interfaces::create_subscription( + shared_from_this(), + // ^^^ FIX: use Nav2 factory so the returned type is nav2::Subscription<...>::SharedPtr + speed_limit_topic, + std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1), + rclcpp::QoS(10)); +} void speedLimitCallback( const nav2_msgs::msg::SpeedLimit::ConstSharedPtr msg) 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..d54f1cbd3ec 100644 --- a/nav2_map_server/test/unit/test_vector_object_server.cpp +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -149,7 +149,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_; 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 ab8d5fd07fd..f6f7cd4664b 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -27,8 +27,16 @@ #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 +{ +// Forward declare nav2::LifecycleNode (defined in lifecycle_node.hpp) +class LifecycleNode; +} // namespace nav2 + namespace nav2 { @@ -184,7 +192,12 @@ 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 allocator-aware SubscriptionOptions. + * + * If the node is a nav2::LifecycleNode, the subscription will be registered as a managed + * entity so it participates in lifecycle transitions, and will be auto-activated if the + * node is already ACTIVE. + * * @param node Node to create the subscription on * @param topic_name Name of topic * @param callback Callback function to handle incoming messages @@ -192,23 +205,62 @@ inline rclcpp::PublisherOptions createPublisherOptions( * @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 NodeT & node, + template> + 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); + + auto options = createSubscriptionOptions( + topic_name, allow_parameter_qos_overrides, callback_group); + + auto sub = std::make_shared>( + node, + topic_name, + std::forward(callback), + qos, + options); + + return sub; + } + + +/** + * @brief Create a subscription on a nav2::LifecycleNode (managed-entity registration happens in the definition). + * + * NOTE: Declaration only here. Definition is in lifecycle_node.hpp to avoid circular includes. + */ +template> +typename nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, const std::string & topic_name, CallbackT && callback, const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), - const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) -{ - bool allow_parameter_qos_overrides = nav2::declare_or_get_parameter( - node, "allow_parameter_qos_overrides", true); + const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr); - auto options = createSubscriptionOptions( - topic_name, allow_parameter_qos_overrides, callback_group); - - return std::make_shared>( - node, topic_name, qos, std::forward(callback), options); -} +/** + * @brief Create a subscription on a base rclcpp_lifecycle::LifecycleNode. + * + * If the runtime type is nav2::LifecycleNode, it will be registered as a managed entity. + * Otherwise it behaves like the generic path (no registration). + * + * NOTE: Declaration only here. Definition is in lifecycle_node.hpp to avoid circular includes. + */ +template> +typename nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), + const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr); /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions 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..841c0377085 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,38 @@ #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 +{ +class LifecycleNode; +} + +namespace nav2::interfaces +{ + +template +typename nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + +template +typename nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + +} // namespace nav2::interfaces namespace nav2 { @@ -140,28 +170,32 @@ 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 - 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); - } +/** + * @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); + } + /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions @@ -452,6 +486,43 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp::TimerBase::SharedPtr autostart_timer_; private: + /** + * @brief Internal helper to register a managed entity with this LifecycleNode. + * + * This wraps the base class protected API `add_managed_entity()` in a way that + * keeps the base API protected, while allowing specific Nav2 factory overloads + * to register entities safely. + * + * @param node The nav2::LifecycleNode instance + * @param ent The managed entity to register + */ + static void register_managed_entity( + ::nav2::LifecycleNode & node, + std::weak_ptr ent) + { + node.add_managed_entity(std::move(ent)); + } + + + template + friend typename Subscription::SharedPtr + interfaces::create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + + template + friend typename Subscription::SharedPtr + interfaces::create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::CallbackGroup::SharedPtr & callback_group); + + /** * @brief Get the enable_lifecycle_services parameter value from NodeOptions * @param options NodeOptions to check for the parameter @@ -470,6 +541,76 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode } }; +namespace interfaces +{ + +template +typename ::nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr<::nav2::LifecycleNode> & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::CallbackGroup::SharedPtr & callback_group) +{ + bool allow_parameter_qos_overrides = + nav2::declare_or_get_parameter(node, "allow_parameter_qos_overrides", true); + + auto options = createSubscriptionOptions( + topic_name, allow_parameter_qos_overrides, callback_group); + + auto sub = std::make_shared<::nav2::Subscription>( + node, + topic_name, + std::forward(callback), + qos, + options); + + ::nav2::LifecycleNode::register_managed_entity(*node, sub); + + if (node->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + sub->on_activate(); + } + + return sub; +} + +template +typename ::nav2::Subscription::SharedPtr +create_subscription( + const std::shared_ptr & node, + const std::string & topic_name, + CallbackT && callback, + const rclcpp::QoS & qos, + const rclcpp::CallbackGroup::SharedPtr & callback_group) +{ + bool allow_parameter_qos_overrides = + nav2::declare_or_get_parameter(node, "allow_parameter_qos_overrides", true); + + auto options = createSubscriptionOptions( + topic_name, allow_parameter_qos_overrides, callback_group); + + auto sub = std::make_shared<::nav2::Subscription>( + node, + topic_name, + std::forward(callback), + qos, + options); + + if (auto nav2_lc = std::dynamic_pointer_cast<::nav2::LifecycleNode>(node)) { + ::nav2::LifecycleNode::register_managed_entity(*nav2_lc, sub); + + if (nav2_lc->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + sub->on_activate(); + } + } + + return sub; +} + +} // namespace nav2::interfaces + + } // namespace nav2 #endif // NAV2_ROS_COMMON__LIFECYCLE_NODE_HPP_ diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index 6bdfd52ffb6..f8727ec7ef9 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -22,29 +22,36 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/message_memory_strategy.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/managed_entity.hpp" +#include "nav2_ros_common/qos_profiles.hpp" + namespace nav2 { -template +template> class Subscription : public rclcpp_lifecycle::SimpleManagedEntity { public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) + using RclcppSub = rclcpp::Subscription; + using Options = rclcpp::SubscriptionOptionsWithAllocator; + using AnyCb = rclcpp::AnySubscriptionCallback; + template Subscription( const std::shared_ptr & node, const std::string & topic_name, - const rclcpp::QoS & qos, CallbackT && user_callback, - const rclcpp::SubscriptionOptions & options = rclcpp::SubscriptionOptions{}) + const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), + const Options & options = Options{}) : topic_name_(topic_name), logger_(node->get_logger()), should_log_(true), - any_cb_(std::allocator{}) + any_cb_(*options.get_allocator()) { init(node, qos, std::forward(user_callback), options); } @@ -52,6 +59,7 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity void on_activate() override { rclcpp_lifecycle::SimpleManagedEntity::on_activate(); + should_log_.store(true); } void on_deactivate() override @@ -60,13 +68,18 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity should_log_.store(true); } + const char * get_topic_name() const noexcept + { + return topic_name_.c_str(); + } + private: template void init( const std::shared_ptr & node, const rclcpp::QoS & qos, CallbackT && user_callback, - const rclcpp::SubscriptionOptions & options) + const Options & options) { any_cb_.set(std::forward(user_callback)); @@ -90,9 +103,10 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity topic_name_, qos, std::move(wrapped_cb), - options); + options, + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default()); - // Preserve old behavior for non-lifecycle nodes + // Legacy behavior: if this is NOT a lifecycle node, auto-activate immediately. auto maybe_lc = std::dynamic_pointer_cast(node); if (!maybe_lc) { this->on_activate(); @@ -101,22 +115,21 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity void log_subscription_not_enabled_once() { - if (should_log_.exchange(false)) { - RCLCPP_WARN( - logger_, - "Subscription on [%s] is not enabled yet (node not activated). " - "Dropping messages until activation.", - topic_name_.c_str()); + if (!should_log_.exchange(false)) { + return; } + + RCLCPP_WARN( + logger_, + "Trying to take messages on topic '%s', but the subscription is not activated. Dropping until activation.", + topic_name_.c_str()); } - typename rclcpp::Subscription::SharedPtr sub_; + typename RclcppSub::SharedPtr sub_; std::string topic_name_; rclcpp::Logger logger_; std::atomic should_log_; - - // Type-erased, “official rclcpp way” to store/dispatch callbacks - rclcpp::AnySubscriptionCallback> any_cb_; + AnyCb any_cb_; }; } // namespace nav2 diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 2e6a43af148..4c4fb3208ff 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,132 +1,1664 @@ { -"type": "FeatureCollection", -"name": "graph", -"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, -"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", -"features": [ -{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, -{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, -{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, -{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, -{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, -{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, -{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, -{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, -{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, -{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, -{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, -{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, -{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, -{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, -{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, -{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, -{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, -{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, -{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } -] + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, + "features": [ + { + "geometry": { + "coordinates": [ + -4.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -3.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 0.6499999761581421 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 5.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 8.649999618530273 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -7.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -7.763441562652588 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + -8.690509796142578 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -9.395081520080566 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 4.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 2.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -6.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -5.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 39 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 75, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 92, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 78, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 89, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 26, + "id": 102, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 119, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 120, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 79, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 88, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 24, + "id": 99, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 117, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 118, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 81, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 86, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 80, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 87, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 122, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 82, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 83, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 85, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 22, + "id": 96, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 84, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 76, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 91, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 28, + "id": 105, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 42, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 52, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 38, + "id": 54, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 43, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 51, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 115, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 44, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 50, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 59, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 111, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 45, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 49, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 108, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 46, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 48, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 63, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 71, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 62, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 17, + "id": 64, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 47, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 72, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 66, + "overridable": true, + "startid": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 65, + "overridable": true, + "startid": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 73, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 94, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 74, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 93, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 41, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 53, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 123, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 40, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 95, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 23, + "id": 97, + "overridable": true, + "startid": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 98, + "overridable": true, + "startid": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 25, + "id": 100, + "overridable": true, + "startid": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 101, + "overridable": true, + "startid": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 27, + "id": 103, + "overridable": true, + "startid": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 104, + "overridable": true, + "startid": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 29, + "id": 106, + "overridable": true, + "startid": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 107, + "overridable": true, + "startid": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 116, + "overridable": true, + "startid": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 112, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 113, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 114, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 109, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 110, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 77, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 90, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 121, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 60, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 70, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 14, + "id": 61, + "overridable": true, + "startid": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 57, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 67, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 58, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 69, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 39, + "id": 55, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 68, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 36, + "id": 56, + "overridable": true, + "startid": 39 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" } diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson index 162a1a88884..87a19b468ee 100644 --- a/nav2_route/graphs/sample_graph.geojson +++ b/nav2_route/graphs/sample_graph.geojson @@ -1,121 +1,504 @@ { - "type": "FeatureCollection", - "name": "graph", - "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, - "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, "features": [ { - "type": "Feature", - "properties": - { - "id": 0, + "geometry": { + "coordinates": [ + 0.0, + 0.0 + ], + "type": "Point" + }, + "properties": { "frame": "map", - "metadata": - { - "region": - { - "x_values": [1.0, -1.0, -1.0, 1.0], - "y_values": [1.0, 1.0, -1.0, -1.0], + "id": 0, + "metadata": { + "region": { "properties": { "class_type": "living_room", "number_of_lights": 10 - } + }, + "x_values": [ + 1.0, + -1.0, + -1.0, + 1.0 + ], + "y_values": [ + 1.0, + 1.0, + -1.0, + -1.0 + ] } }, - "operations": - { - "stop": - { - "type": "stop", - "trigger": "NODE", - "metadata": - { + "operations": { + "stop": { + "metadata": { "wait_for": 5.0 - } + }, + "trigger": "NODE", + "type": "stop" } } }, - "geometry": - { - "type": "Point", - "coordinates": [ 0.0, 0.0 ] - } - }, - { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, - { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, - { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, - { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, - { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, - { - "type": "Feature", - "properties": - { - "id": 9, - "startid": 0, - "endid": 1, - "overridable": false, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { "cost": 10.0, - "metadata": - { - "speed_limit": 0.85, - "aisle_number": 14 + "endid": 1, + "id": 9, + "metadata": { + "aisle_number": 14, + "speed_limit": 0.8500000238418579 }, - "operations": - { - "open_door": - { - "type": "open_door", - "trigger": "ON_ENTER", - "metadata": - { + "operations": { + "open_door": { + "metadata": { "door_id": 54, "service_name": "open_door" - } + }, + "trigger": "ON_ENTER", + "type": "open_door" }, - "take_picture": - { - "type": "take_picture", + "take_picture": { + "metadata": { + "resolution": [ + 1080, + 720 + ], + "type": "jpg" + }, "trigger": "ON_EXIT", - "metadata": - { - "type": "jpg", - "resolution": [1080, 720] - } + "type": "take_picture" } - } + }, + "overridable": false, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 13, + "overridable": true, + "startid": 0 }, - "geometry": - { - "type": "MultiLineString", - "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] - } - }, - { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } - - ] + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 10, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 11, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 15, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 12, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 17, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 14, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 19, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 23, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 16, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 20, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 21, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 25, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 18, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 22, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 27, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 24, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 29, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 26, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 30, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 31, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 28, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 32, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" } 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..80332b98597 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,7 @@ #include "assisted_teleop_behavior_tester.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_ros_common/interface_factories.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -59,15 +60,17 @@ 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)); - - filtered_vel_sub_ = node_->create_subscription( + subscription_ = nav2::interfaces::create_subscription( + node_, + "amcl_pose", + std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1), + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + 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::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); std::string costmap_topic = "/local_costmap/costmap_raw"; std::string footprint_topic = "/local_costmap/published_footprint"; 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..2fdfeba451b 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,11 @@ 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( + 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..23d4b1bb952 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,12 @@ 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( + 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_); } From ccd57eeeab862a94e045d686925fc8978ee5c733 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Mon, 12 Jan 2026 20:04:15 +0000 Subject: [PATCH 06/37] fix rviz Signed-off-by: lotusymt --- .../include/nav2_rviz_plugins/docking_panel.hpp | 8 ++++---- .../include/nav2_rviz_plugins/nav2_panel.hpp | 8 ++++---- .../include/nav2_rviz_plugins/route_tool.hpp | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) 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..b4f524f84a2 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -107,10 +107,10 @@ private Q_SLOTS: nav2::ActionClient::SharedPtr undock_client_; // 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_; + rclcpp::Subscription::SharedPtr docking_feedback_sub_; + rclcpp::Subscription::SharedPtr undocking_feedback_sub_; + rclcpp::Subscription::SharedPtr docking_goal_status_sub_; + rclcpp::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..784b9fe160e 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -146,13 +146,13 @@ private Q_SLOTS: nav_through_poses_action_client_; // Navigation action feedback subscribers - nav2::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr navigation_feedback_sub_; - nav2::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr nav_through_poses_feedback_sub_; - nav2::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr navigation_goal_status_sub_; - nav2::Subscription::SharedPtr + rclcpp::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..3ee6060f3f3 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/route_tool.hpp @@ -110,7 +110,7 @@ private Q_SLOTS: rclcpp::Publisher::SharedPtr graph_vis_publisher_; - nav2::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr clicked_point_subscription_; unsigned int next_node_id_ = 0; From eff1f93852c39e3a14bb772c6d776d1c29a05d9d Mon Sep 17 00:00:00 2001 From: lotusymt Date: Mon, 12 Jan 2026 21:14:50 +0000 Subject: [PATCH 07/37] fix cpp linter Signed-off-by: lotusymt --- .../test/collision_monitor_node_bag.cpp | 1 - .../test/unit/binary_filter_test.cpp | 2 +- .../test/unit/speed_filter_test.cpp | 10 ++--- .../test/test_following_server.py | 12 +++--- .../nav2_ros_common/interface_factories.hpp | 37 ++++++++++--------- .../nav2_ros_common/lifecycle_node.hpp | 28 +++++++------- .../include/nav2_ros_common/subscription.hpp | 3 +- .../assisted_teleop_behavior_tester.cpp | 5 ++- .../behaviors/wait/wait_behavior_tester.cpp | 3 +- .../localization/test_localization_node.cpp | 5 ++- 10 files changed, 55 insertions(+), 51 deletions(-) diff --git a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp index b1dfca6d351..78137441b80 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -113,7 +113,6 @@ class MetricsCatcher : public rclcpp::Node { last_action_ = msg.action_type; }, rclcpp::QoS(10)); - } // Spin until we passed the obstacle window (8s) + margin diff --git a/nav2_costmap_2d/test/unit/binary_filter_test.cpp b/nav2_costmap_2d/test/unit/binary_filter_test.cpp index 6897a507dea..88171bb8e12 100644 --- a/nav2_costmap_2d/test/unit/binary_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/binary_filter_test.cpp @@ -113,7 +113,7 @@ class BinaryStateSubscriber : public rclcpp::Node binary_state_topic, std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1), rclcpp::QoS(10)); - + // Initialize with default state msg_.data = default_state; diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index f56d5ea7778..08b1d6961c8 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -104,16 +104,16 @@ class MaskPublisher : public rclcpp::Node class SpeedLimitSubscriber : public rclcpp::Node { public: -explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) -: Node("speed_limit_sub"), speed_limit_updated_(false) -{ - subscriber_ = nav2::interfaces::create_subscription( + explicit SpeedLimitSubscriber(const std::string & speed_limit_topic) + : Node("speed_limit_sub"), speed_limit_updated_(false) + { + subscriber_ = nav2::interfaces::create_subscription( shared_from_this(), // ^^^ FIX: use Nav2 factory so the returned type is nav2::Subscription<...>::SharedPtr speed_limit_topic, std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1), rclcpp::QoS(10)); -} + } void speedLimitCallback( const nav2_msgs::msg::SpeedLimit::ConstSharedPtr msg) diff --git a/nav2_following/opennav_following/test/test_following_server.py b/nav2_following/opennav_following/test/test_following_server.py index 421582d017f..fa5f1acf8e5 100644 --- a/nav2_following/opennav_following/test/test_following_server.py +++ b/nav2_following/opennav_following/test/test_following_server.py @@ -27,7 +27,7 @@ import launch_testing import launch_testing.actions from lifecycle_msgs.srv import GetState -from nav2_msgs.action import FollowObject +from nav2_msgs.action import FollowObject # type: ignore[attr-defined] import pytest import rclpy from rclpy.action import ActionClient @@ -219,8 +219,8 @@ def at_distance_getter() -> bool: def wait_for_node_to_be_active(self, node_name: str, timeout_sec: float = 10.0) -> None: """Wait for a managed node to become active.""" - client: Client[GetState.Request, GetState.Response] = ( # type: ignore[name-defined] - self.node.create_client(GetState, f'{node_name}/get_state') # type: ignore[arg-type] + client: Client[GetState.Request, GetState.Response] = ( + self.node.create_client(GetState, f'{node_name}/get_state') ) if not client.wait_for_service(timeout_sec=2.0): self.fail(f'Service get_state for {node_name} not available.') @@ -281,7 +281,7 @@ def publish(self) -> None: def action_feedback_callback( self, - msg: FollowObject.Feedback # type: ignore[name-defined] + msg: FollowObject.Feedback ) -> None: # Force the following action to run a full recovery loop when # the robot is at distance @@ -300,9 +300,9 @@ def test_following_server(self) -> None: self.timer = self.node.create_timer(0.05, self.timer_callback) # Create action client - self.follow_action_client: ActionClient[ # type: ignore[name-defined] + self.follow_action_client: ActionClient[ FollowObject.Goal, FollowObject.Result, FollowObject.Feedback - ] = ActionClient(self.node, FollowObject, 'follow_object') # type: ignore[arg-type] + ] = ActionClient(self.node, FollowObject, 'follow_object') # Subscribe to command velocity self.node.create_subscription( 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 f6f7cd4664b..d2631b0eb2f 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -205,30 +205,31 @@ inline rclcpp::PublisherOptions createPublisherOptions( * @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 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); - - auto options = createSubscriptionOptions( +template> +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); + + auto options = createSubscriptionOptions( topic_name, allow_parameter_qos_overrides, callback_group); - - auto sub = std::make_shared>( + + auto sub = std::make_shared>( node, topic_name, std::forward(callback), qos, options); - - return sub; - } + + 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 841c0377085..a5f1d400ea0 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -180,22 +180,22 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode * @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( + 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); - } - + } + /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions @@ -496,13 +496,13 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode * @param node The nav2::LifecycleNode instance * @param ent The managed entity to register */ - static void register_managed_entity( + static void register_managed_entity( ::nav2::LifecycleNode & node, std::weak_ptr ent) { node.add_managed_entity(std::move(ent)); } - + template friend typename Subscription::SharedPtr @@ -608,7 +608,7 @@ create_subscription( return sub; } -} // namespace nav2::interfaces +} // namespace interfaces } // namespace nav2 diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index f8727ec7ef9..14f9fdd0b32 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -121,7 +121,8 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity RCLCPP_WARN( logger_, - "Trying to take messages on topic '%s', but the subscription is not activated. Dropping until activation.", + "Trying to take messages on topic '%s', but the subscription is not activated. " + "Dropping until activation.", topic_name_.c_str()); } 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 80332b98597..d82db066c00 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 @@ -60,12 +60,13 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() cmd_vel_pub_ = node_->create_publisher("cmd_vel_teleop", 10); - subscription_ = nav2::interfaces::create_subscription( + 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_ = nav2::interfaces::create_subscription( node_, "cmd_vel", 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 2fdfeba451b..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,7 +53,8 @@ WaitBehaviorTester::WaitBehaviorTester() publisher_ = node_->create_publisher("initialpose", 10); - subscription_ = nav2::interfaces::create_subscription( + subscription_ = nav2::interfaces::create_subscription< + geometry_msgs::msg::PoseWithCovarianceStamped>( node_, "amcl_pose", std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1), diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 23d4b1bb952..4540b75c145 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -42,12 +42,13 @@ class TestAmclPose : public ::testing::Test initial_pose_pub_ = node->create_publisher( "initialpose", rclcpp::SystemDefaultsQoS()); - subscription_ = nav2::interfaces::create_subscription( + 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_); } From 84b8b4559ba3fc11eb3c9db72453f043a3d25240 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 14 Jan 2026 19:38:34 +0000 Subject: [PATCH 08/37] subsscription2.0, imitating publisher Signed-off-by: lotusymt --- .../nav2_ros_common/interface_factories.hpp | 43 +---- .../nav2_ros_common/lifecycle_node.hpp | 161 +++--------------- .../include/nav2_ros_common/subscription.hpp | 31 +--- 3 files changed, 35 insertions(+), 200 deletions(-) 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 d2631b0eb2f..6ec5e4c4be7 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -194,9 +194,7 @@ inline rclcpp::PublisherOptions createPublisherOptions( /** * @brief Create a subscription to a topic using Nav2 QoS profiles and allocator-aware SubscriptionOptions. * - * If the node is a nav2::LifecycleNode, the subscription will be registered as a managed - * entity so it participates in lifecycle transitions, and will be auto-activated if the - * node is already ACTIVE. + * 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 @@ -228,41 +226,18 @@ create_subscription( qos, options); + // Auto-activate if the node is not a base lifecycle node + // Try to cast to lifecycle node - if it fails, it's a regular 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; } -/** - * @brief Create a subscription on a nav2::LifecycleNode (managed-entity registration happens in the definition). - * - * NOTE: Declaration only here. Definition is in lifecycle_node.hpp to avoid circular includes. - */ -template> -typename nav2::Subscription::SharedPtr -create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), - const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr); - -/** - * @brief Create a subscription on a base rclcpp_lifecycle::LifecycleNode. - * - * If the runtime type is nav2::LifecycleNode, it will be registered as a managed entity. - * Otherwise it behaves like the generic path (no registration). - * - * NOTE: Declaration only here. Definition is in lifecycle_node.hpp to avoid circular includes. - */ -template> -typename nav2::Subscription::SharedPtr -create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), - const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr); - /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions * @param node Node to create the publisher on diff --git a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp index a5f1d400ea0..c04095958d0 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -35,34 +35,6 @@ #include "nav2_ros_common/rate.hpp" #include "nav2_ros_common/subscription.hpp" -namespace nav2 -{ -class LifecycleNode; -} - -namespace nav2::interfaces -{ - -template -typename nav2::Subscription::SharedPtr -create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - -template -typename nav2::Subscription::SharedPtr -create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - -} // namespace nav2::interfaces - namespace nav2 { @@ -188,12 +160,27 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode 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); + bool allow_parameter_qos_overrides = + nav2::declare_or_get_parameter(shared_from_this(), "allow_parameter_qos_overrides", true); + auto options = nav2::interfaces::createSubscriptionOptions( + topic_name, allow_parameter_qos_overrides, callback_group); + + auto sub = std::make_shared>( + shared_from_this(), + topic_name, + std::forward(callback), + qos, + options); + this->add_managed_entity(sub); + + // Automatically activate the publisher if the node is already active + if (get_current_state().id() == + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + sub->on_activate(); + } + + return sub; } @@ -486,42 +473,6 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp::TimerBase::SharedPtr autostart_timer_; private: - /** - * @brief Internal helper to register a managed entity with this LifecycleNode. - * - * This wraps the base class protected API `add_managed_entity()` in a way that - * keeps the base API protected, while allowing specific Nav2 factory overloads - * to register entities safely. - * - * @param node The nav2::LifecycleNode instance - * @param ent The managed entity to register - */ - static void register_managed_entity( - ::nav2::LifecycleNode & node, - std::weak_ptr ent) - { - node.add_managed_entity(std::move(ent)); - } - - - template - friend typename Subscription::SharedPtr - interfaces::create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - - template - friend typename Subscription::SharedPtr - interfaces::create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::CallbackGroup::SharedPtr & callback_group); - /** * @brief Get the enable_lifecycle_services parameter value from NodeOptions @@ -541,76 +492,6 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode } }; -namespace interfaces -{ - -template -typename ::nav2::Subscription::SharedPtr -create_subscription( - const std::shared_ptr<::nav2::LifecycleNode> & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::CallbackGroup::SharedPtr & callback_group) -{ - bool allow_parameter_qos_overrides = - nav2::declare_or_get_parameter(node, "allow_parameter_qos_overrides", true); - - auto options = createSubscriptionOptions( - topic_name, allow_parameter_qos_overrides, callback_group); - - auto sub = std::make_shared<::nav2::Subscription>( - node, - topic_name, - std::forward(callback), - qos, - options); - - ::nav2::LifecycleNode::register_managed_entity(*node, sub); - - if (node->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - sub->on_activate(); - } - - return sub; -} - -template -typename ::nav2::Subscription::SharedPtr -create_subscription( - const std::shared_ptr & node, - const std::string & topic_name, - CallbackT && callback, - const rclcpp::QoS & qos, - const rclcpp::CallbackGroup::SharedPtr & callback_group) -{ - bool allow_parameter_qos_overrides = - nav2::declare_or_get_parameter(node, "allow_parameter_qos_overrides", true); - - auto options = createSubscriptionOptions( - topic_name, allow_parameter_qos_overrides, callback_group); - - auto sub = std::make_shared<::nav2::Subscription>( - node, - topic_name, - std::forward(callback), - qos, - options); - - if (auto nav2_lc = std::dynamic_pointer_cast<::nav2::LifecycleNode>(node)) { - ::nav2::LifecycleNode::register_managed_entity(*nav2_lc, sub); - - if (nav2_lc->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - sub->on_activate(); - } - } - - return sub; -} - -} // namespace interfaces - - } // namespace nav2 #endif // NAV2_ROS_COMMON__LIFECYCLE_NODE_HPP_ diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index 14f9fdd0b32..35a4d551d66 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -15,7 +15,6 @@ #ifndef NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ #define NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ -#include #include #include #include @@ -50,7 +49,6 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity const Options & options = Options{}) : topic_name_(topic_name), logger_(node->get_logger()), - should_log_(true), any_cb_(*options.get_allocator()) { init(node, qos, std::forward(user_callback), options); @@ -59,13 +57,11 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity void on_activate() override { rclcpp_lifecycle::SimpleManagedEntity::on_activate(); - should_log_.store(true); } void on_deactivate() override { rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); - should_log_.store(true); } const char * get_topic_name() const noexcept @@ -87,10 +83,13 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity [this](typename MessageT::ConstSharedPtr msg, const rclcpp::MessageInfo & info) { if (!this->is_activated()) { - log_subscription_not_enabled_once(); + RCLCPP_WARN_ONCE( + logger_, + "Trying to take messages on topic '%s', but the subscription is not activated. " + "Dropping until activation.", + topic_name_.c_str()); return; } - should_log_.store(true); any_cb_.dispatch_intra_process(msg, info); }; @@ -105,31 +104,11 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity std::move(wrapped_cb), options, rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default()); - - // Legacy behavior: if this is NOT a lifecycle node, auto-activate immediately. - auto maybe_lc = std::dynamic_pointer_cast(node); - if (!maybe_lc) { - this->on_activate(); - } - } - - void log_subscription_not_enabled_once() - { - if (!should_log_.exchange(false)) { - return; - } - - RCLCPP_WARN( - logger_, - "Trying to take messages on topic '%s', but the subscription is not activated. " - "Dropping until activation.", - topic_name_.c_str()); } typename RclcppSub::SharedPtr sub_; std::string topic_name_; rclcpp::Logger logger_; - std::atomic should_log_; AnyCb any_cb_; }; From 216d394ac378d4ecfc3cda4312f114023cf126f0 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 14 Jan 2026 19:42:43 +0000 Subject: [PATCH 09/37] fix cpp linter Signed-off-by: lotusymt --- nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 c04095958d0..823b61bc09b 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -161,7 +161,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) { bool allow_parameter_qos_overrides = - nav2::declare_or_get_parameter(shared_from_this(), "allow_parameter_qos_overrides", true); + nav2::declare_or_get_parameter(shared_from_this(), "allow_parameter_qos_overrides", true); auto options = nav2::interfaces::createSubscriptionOptions( topic_name, allow_parameter_qos_overrides, callback_group); @@ -179,7 +179,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode { sub->on_activate(); } - + return sub; } @@ -473,7 +473,6 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode rclcpp::TimerBase::SharedPtr autostart_timer_; private: - /** * @brief Get the enable_lifecycle_services parameter value from NodeOptions * @param options NodeOptions to check for the parameter From 020eab4845f3a9db95398485a0ae8323c07c4c90 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 14 Jan 2026 20:52:02 +0000 Subject: [PATCH 10/37] cleanup Signed-off-by: lotusymt --- nav2_bringup/launch/bringup_launch.py | 11 +++++------ .../launch/tb3_loopback_simulation_launch.py | 10 +++------- nav2_bringup/launch/tb3_simulation_launch.py | 1 - .../launch/tb4_loopback_simulation_launch.py | 10 +++------- nav2_bringup/launch/tb4_simulation_launch.py | 1 - .../test/test_following_server.py | 12 ++++++------ .../nav2_ros_common/interface_factories.hpp | 6 ------ .../include/nav2_ros_common/lifecycle_node.hpp | 15 +++------------ .../nav2_simple_commander/robot_navigator.py | 5 +---- .../src/error_codes/test_error_codes.py | 1 - .../src/waypoint_follower/test_case_launch.py | 7 ++++++- 11 files changed, 27 insertions(+), 52 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 8253dd7e3c1..7aedf9e0d93 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -43,7 +43,6 @@ def generate_launch_description() -> LaunchDescription: autostart = LaunchConfigAsBool('autostart') use_composition = LaunchConfigAsBool('use_composition') use_intra_process_comms = LaunchConfigAsBool('use_intra_process_comms') - container_name = LaunchConfiguration('container_name') use_respawn = LaunchConfigAsBool('use_respawn') log_level = LaunchConfiguration('log_level') use_localization = LaunchConfigAsBool('use_localization') @@ -166,7 +165,7 @@ def generate_launch_description() -> LaunchDescription: [ Node( condition=IfCondition(use_composition), - name=container_name, + name='nav2_container', namespace=namespace, package='rclcpp_components', executable='component_container_isolated', @@ -202,7 +201,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_intra_process_comms': use_intra_process_comms, 'use_respawn': use_respawn, - 'container_name': container_name, + 'container_name': 'nav2_container', }.items(), ), @@ -219,7 +218,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_intra_process_comms': use_intra_process_comms, 'use_respawn': use_respawn, - 'container_name': container_name, + 'container_name': 'nav2_container', }.items(), ), @@ -236,7 +235,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_intra_process_comms': use_intra_process_comms, 'use_respawn': use_respawn, - 'container_name': container_name, + 'container_name': 'nav2_container', }.items(), ), @@ -255,7 +254,7 @@ def generate_launch_description() -> LaunchDescription: 'use_respawn': use_respawn, 'use_keepout_zones': use_keepout_zones, 'use_speed_zones': use_speed_zones, - 'container_name': container_name, + 'container_name': 'nav2_container', }.items(), ), ] diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index b5af1cd9f8f..55a437d97ec 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -21,9 +21,9 @@ from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes, Node, SetParameter -from launch_ros.descriptions import ComposableNode, ParameterFile +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import LaunchConfigAsBool, RewrittenYaml @@ -48,7 +48,6 @@ def generate_launch_description() -> LaunchDescription: rviz_config_file = LaunchConfiguration('rviz_config_file') use_robot_state_pub = LaunchConfigAsBool('use_robot_state_pub') use_rviz = LaunchConfigAsBool('use_rviz') - container_name_full = (namespace, '/', 'nav2_container') remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -155,7 +154,6 @@ def generate_launch_description() -> LaunchDescription: 'use_localization': 'False', # Don't use SLAM, AMCL 'use_keepout_zones': 'False', 'use_speed_zones': 'False', - 'container_name': 'nav2_container', }.items(), ) @@ -178,7 +176,6 @@ def generate_launch_description() -> LaunchDescription: ) start_map_server = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', True), Node( @@ -253,7 +250,6 @@ def generate_launch_description() -> LaunchDescription: # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(start_map_server) - ld.add_action(start_composable_map_server) ld.add_action(loopback_sim_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 1d0e0c89bb5..4aafd371806 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -206,7 +206,6 @@ def generate_launch_description() -> LaunchDescription: 'use_respawn': use_respawn, 'use_keepout_zones': 'False', 'use_speed_zones': 'False', - 'container_name': 'nav2_container', }.items(), ) # The SDF file for the world is a xacro file because we wanted to diff --git a/nav2_bringup/launch/tb4_loopback_simulation_launch.py b/nav2_bringup/launch/tb4_loopback_simulation_launch.py index 5fd497d1feb..d423869957c 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation_launch.py @@ -21,9 +21,9 @@ from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes, Node, SetParameter -from launch_ros.descriptions import ComposableNode, ParameterFile +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node, SetParameter +from launch_ros.descriptions import ParameterFile from nav2_common.launch import LaunchConfigAsBool, RewrittenYaml @@ -48,7 +48,6 @@ def generate_launch_description() -> LaunchDescription: rviz_config_file = LaunchConfiguration('rviz_config_file') use_robot_state_pub = LaunchConfigAsBool('use_robot_state_pub') use_rviz = LaunchConfigAsBool('use_rviz') - container_name_full = (namespace, '/', 'nav2_container') remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -153,7 +152,6 @@ def generate_launch_description() -> LaunchDescription: 'use_keepout_zones': 'False', # Keepout zones not used in loopback simulation 'use_speed_zones': 'False', # Speed zones not used in loopback simulation 'use_localization': 'False', # Don't use SLAM, AMCL - 'container_name': 'nav2_container', }.items(), ) @@ -186,7 +184,6 @@ def generate_launch_description() -> LaunchDescription: ) start_map_server = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', True), Node( @@ -261,7 +258,6 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(start_robot_state_publisher_cmd) ld.add_action(static_publisher_cmd) ld.add_action(start_map_server) - ld.add_action(start_composable_map_server) ld.add_action(loopback_sim_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index ba5266ee64f..b6336f31f9a 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -247,7 +247,6 @@ def generate_launch_description() -> LaunchDescription: 'use_respawn': use_respawn, 'use_keepout_zones': use_keepout_zones, 'use_speed_zones': use_speed_zones, - 'container_name': 'nav2_container', }.items(), ) diff --git a/nav2_following/opennav_following/test/test_following_server.py b/nav2_following/opennav_following/test/test_following_server.py index fa5f1acf8e5..421582d017f 100644 --- a/nav2_following/opennav_following/test/test_following_server.py +++ b/nav2_following/opennav_following/test/test_following_server.py @@ -27,7 +27,7 @@ import launch_testing import launch_testing.actions from lifecycle_msgs.srv import GetState -from nav2_msgs.action import FollowObject # type: ignore[attr-defined] +from nav2_msgs.action import FollowObject import pytest import rclpy from rclpy.action import ActionClient @@ -219,8 +219,8 @@ def at_distance_getter() -> bool: def wait_for_node_to_be_active(self, node_name: str, timeout_sec: float = 10.0) -> None: """Wait for a managed node to become active.""" - client: Client[GetState.Request, GetState.Response] = ( - self.node.create_client(GetState, f'{node_name}/get_state') + client: Client[GetState.Request, GetState.Response] = ( # type: ignore[name-defined] + self.node.create_client(GetState, f'{node_name}/get_state') # type: ignore[arg-type] ) if not client.wait_for_service(timeout_sec=2.0): self.fail(f'Service get_state for {node_name} not available.') @@ -281,7 +281,7 @@ def publish(self) -> None: def action_feedback_callback( self, - msg: FollowObject.Feedback + msg: FollowObject.Feedback # type: ignore[name-defined] ) -> None: # Force the following action to run a full recovery loop when # the robot is at distance @@ -300,9 +300,9 @@ def test_following_server(self) -> None: self.timer = self.node.create_timer(0.05, self.timer_callback) # Create action client - self.follow_action_client: ActionClient[ + self.follow_action_client: ActionClient[ # type: ignore[name-defined] FollowObject.Goal, FollowObject.Result, FollowObject.Feedback - ] = ActionClient(self.node, FollowObject, 'follow_object') + ] = ActionClient(self.node, FollowObject, 'follow_object') # type: ignore[arg-type] # Subscribe to command velocity self.node.create_subscription( 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 6ec5e4c4be7..0e76bd55b71 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -31,12 +31,6 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "nav2_ros_common/rate.hpp" -namespace nav2 -{ -// Forward declare nav2::LifecycleNode (defined in lifecycle_node.hpp) -class LifecycleNode; -} // namespace nav2 - namespace nav2 { 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 823b61bc09b..2a81213e828 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -160,20 +160,11 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) { - bool allow_parameter_qos_overrides = - nav2::declare_or_get_parameter(shared_from_this(), "allow_parameter_qos_overrides", true); - auto options = nav2::interfaces::createSubscriptionOptions( - topic_name, allow_parameter_qos_overrides, callback_group); - - auto sub = std::make_shared>( - shared_from_this(), - topic_name, - std::forward(callback), - qos, - options); + auto sub = nav2::interfaces::create_subscription( + shared_from_this(), topic_name, std::forward(callback), qos, callback_group); this->add_managed_entity(sub); - // Automatically activate the publisher if the node is already active + // Automatically activate the subscription if the node is already active if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index be4bd9a3d8a..074c520e4de 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -525,8 +525,7 @@ def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]: return RunningTask.ASSISTED_TELEOP def followPath(self, path: Path, controller_id: str = '', - goal_checker_id: str = '', progress_checker_id: str = '', - path_handler_id: str = '') -> Optional[RunningTask]: + goal_checker_id: str = '') -> Optional[RunningTask]: self.clearPreviousState() """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -537,8 +536,6 @@ def followPath(self, path: Path, controller_id: str = '', goal_msg.path = path goal_msg.controller_id = controller_id goal_msg.goal_checker_id = goal_checker_id - goal_msg.progress_checker_id = progress_checker_id - goal_msg.path_handler_id = path_handler_id self.info('Executing path...') send_goal_future = self.follow_path_client.send_goal_async( diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index a99778b2e30..4b2d6b2919b 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -49,7 +49,6 @@ def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] goal_pose1 = goal_pose goal_pose1.pose.position.x += 0.25 - path.header.frame_id = 'map' path.poses.append(initial_pose) path.poses.append(goal_pose) path.poses.append(goal_pose1) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index 53d0f70f5bd..ebcd491c2ea 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -48,10 +48,15 @@ def generate_launch_description() -> LaunchDescription: ) params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + param_substitutions = { + 'controller_server.ros__parameters.FollowPath.prune_distance': '1.0', + 'controller_server.ros__parameters.FollowPath.forward_prune_distance': '1.0'} configured_params = RewrittenYaml( source_file=params_file, root_key='', - param_rewrites={}, + param_rewrites=param_substitutions, value_rewrites={ 'KEEPOUT_ZONE_ENABLED': 'False', 'SPEED_ZONE_ENABLED': 'False', From 7a7e7abea6ca89fc7ad36635d3114af48e9527bd Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 14 Jan 2026 21:22:16 +0000 Subject: [PATCH 11/37] Restore files to upstream/main Signed-off-by: lotusymt Signed-off-by: lotusymt --- Testing/Temporary/CTestCostData.txt | 1 + Testing/Temporary/LastTest.log | 3 + nav2_bringup/launch/bringup_launch.py | 11 +- .../launch/tb3_loopback_simulation_launch.py | 10 +- nav2_bringup/launch/tb3_simulation_launch.py | 1 + .../launch/tb4_loopback_simulation_launch.py | 10 +- nav2_bringup/launch/tb4_simulation_launch.py | 1 + nav2_route/graphs/aws_graph.geojson | 1792 ++--------------- nav2_route/graphs/sample_graph.geojson | 581 +----- .../nav2_simple_commander/robot_navigator.py | 5 +- 10 files changed, 259 insertions(+), 2156 deletions(-) create mode 100644 Testing/Temporary/CTestCostData.txt create mode 100644 Testing/Temporary/LastTest.log diff --git a/Testing/Temporary/CTestCostData.txt b/Testing/Temporary/CTestCostData.txt new file mode 100644 index 00000000000..ed97d539c09 --- /dev/null +++ b/Testing/Temporary/CTestCostData.txt @@ -0,0 +1 @@ +--- diff --git a/Testing/Temporary/LastTest.log b/Testing/Temporary/LastTest.log new file mode 100644 index 00000000000..fd05ba20ad1 --- /dev/null +++ b/Testing/Temporary/LastTest.log @@ -0,0 +1,3 @@ +Start testing: Oct 31 21:33 UTC +---------------------------------------------------------- +End testing: Oct 31 21:33 UTC diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 7aedf9e0d93..8253dd7e3c1 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -43,6 +43,7 @@ def generate_launch_description() -> LaunchDescription: autostart = LaunchConfigAsBool('autostart') use_composition = LaunchConfigAsBool('use_composition') use_intra_process_comms = LaunchConfigAsBool('use_intra_process_comms') + container_name = LaunchConfiguration('container_name') use_respawn = LaunchConfigAsBool('use_respawn') log_level = LaunchConfiguration('log_level') use_localization = LaunchConfigAsBool('use_localization') @@ -165,7 +166,7 @@ def generate_launch_description() -> LaunchDescription: [ Node( condition=IfCondition(use_composition), - name='nav2_container', + name=container_name, namespace=namespace, package='rclcpp_components', executable='component_container_isolated', @@ -201,7 +202,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_intra_process_comms': use_intra_process_comms, 'use_respawn': use_respawn, - 'container_name': 'nav2_container', + 'container_name': container_name, }.items(), ), @@ -218,7 +219,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_intra_process_comms': use_intra_process_comms, 'use_respawn': use_respawn, - 'container_name': 'nav2_container', + 'container_name': container_name, }.items(), ), @@ -235,7 +236,7 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_intra_process_comms': use_intra_process_comms, 'use_respawn': use_respawn, - 'container_name': 'nav2_container', + 'container_name': container_name, }.items(), ), @@ -254,7 +255,7 @@ def generate_launch_description() -> LaunchDescription: 'use_respawn': use_respawn, 'use_keepout_zones': use_keepout_zones, 'use_speed_zones': use_speed_zones, - 'container_name': 'nav2_container', + 'container_name': container_name, }.items(), ), ] diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 55a437d97ec..b5af1cd9f8f 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -21,9 +21,9 @@ from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node, SetParameter -from launch_ros.descriptions import ParameterFile +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import LaunchConfigAsBool, RewrittenYaml @@ -48,6 +48,7 @@ def generate_launch_description() -> LaunchDescription: rviz_config_file = LaunchConfiguration('rviz_config_file') use_robot_state_pub = LaunchConfigAsBool('use_robot_state_pub') use_rviz = LaunchConfigAsBool('use_rviz') + container_name_full = (namespace, '/', 'nav2_container') remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -154,6 +155,7 @@ def generate_launch_description() -> LaunchDescription: 'use_localization': 'False', # Don't use SLAM, AMCL 'use_keepout_zones': 'False', 'use_speed_zones': 'False', + 'container_name': 'nav2_container', }.items(), ) @@ -176,6 +178,7 @@ def generate_launch_description() -> LaunchDescription: ) start_map_server = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', True), Node( @@ -250,6 +253,7 @@ def generate_launch_description() -> LaunchDescription: # Add the actions to launch all of the navigation nodes ld.add_action(start_robot_state_publisher_cmd) ld.add_action(start_map_server) + ld.add_action(start_composable_map_server) ld.add_action(loopback_sim_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 4aafd371806..1d0e0c89bb5 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -206,6 +206,7 @@ def generate_launch_description() -> LaunchDescription: 'use_respawn': use_respawn, 'use_keepout_zones': 'False', 'use_speed_zones': 'False', + 'container_name': 'nav2_container', }.items(), ) # The SDF file for the world is a xacro file because we wanted to diff --git a/nav2_bringup/launch/tb4_loopback_simulation_launch.py b/nav2_bringup/launch/tb4_loopback_simulation_launch.py index d423869957c..5fd497d1feb 100644 --- a/nav2_bringup/launch/tb4_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb4_loopback_simulation_launch.py @@ -21,9 +21,9 @@ from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, LaunchConfiguration -from launch_ros.actions import Node, SetParameter -from launch_ros.descriptions import ParameterFile +from launch.substitutions import Command, LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, Node, SetParameter +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import LaunchConfigAsBool, RewrittenYaml @@ -48,6 +48,7 @@ def generate_launch_description() -> LaunchDescription: rviz_config_file = LaunchConfiguration('rviz_config_file') use_robot_state_pub = LaunchConfigAsBool('use_robot_state_pub') use_rviz = LaunchConfigAsBool('use_rviz') + container_name_full = (namespace, '/', 'nav2_container') remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -152,6 +153,7 @@ def generate_launch_description() -> LaunchDescription: 'use_keepout_zones': 'False', # Keepout zones not used in loopback simulation 'use_speed_zones': 'False', # Speed zones not used in loopback simulation 'use_localization': 'False', # Don't use SLAM, AMCL + 'container_name': 'nav2_container', }.items(), ) @@ -184,6 +186,7 @@ def generate_launch_description() -> LaunchDescription: ) start_map_server = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), actions=[ SetParameter('use_sim_time', True), Node( @@ -258,6 +261,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(start_robot_state_publisher_cmd) ld.add_action(static_publisher_cmd) ld.add_action(start_map_server) + ld.add_action(start_composable_map_server) ld.add_action(loopback_sim_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) diff --git a/nav2_bringup/launch/tb4_simulation_launch.py b/nav2_bringup/launch/tb4_simulation_launch.py index b6336f31f9a..ba5266ee64f 100644 --- a/nav2_bringup/launch/tb4_simulation_launch.py +++ b/nav2_bringup/launch/tb4_simulation_launch.py @@ -247,6 +247,7 @@ def generate_launch_description() -> LaunchDescription: 'use_respawn': use_respawn, 'use_keepout_zones': use_keepout_zones, 'use_speed_zones': use_speed_zones, + 'container_name': 'nav2_container', }.items(), ) diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 4c4fb3208ff..2e6a43af148 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,1664 +1,132 @@ { - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, - "features": [ - { - "geometry": { - "coordinates": [ - -4.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -3.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 0.6499999761581421 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 5.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 8.649999618530273 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -7.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -7.763441562652588 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - -8.690509796142578 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -9.395081520080566 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 4.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 2.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -6.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -5.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 39 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 75, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 92, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 78, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 89, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 26, - "id": 102, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 119, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 120, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 79, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 88, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 24, - "id": 99, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 117, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 118, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 81, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 86, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 80, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 87, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 122, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 82, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 83, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 85, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 22, - "id": 96, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 84, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 76, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 91, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 28, - "id": 105, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 42, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 52, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 38, - "id": 54, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 43, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 51, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 115, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 44, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 50, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 59, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 111, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 45, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 49, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 108, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 46, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 48, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 63, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 71, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 62, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 17, - "id": 64, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 47, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 72, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 66, - "overridable": true, - "startid": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 65, - "overridable": true, - "startid": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 73, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 94, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 74, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 93, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 41, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 53, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 123, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 40, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 95, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 23, - "id": 97, - "overridable": true, - "startid": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 98, - "overridable": true, - "startid": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 25, - "id": 100, - "overridable": true, - "startid": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 101, - "overridable": true, - "startid": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 27, - "id": 103, - "overridable": true, - "startid": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 104, - "overridable": true, - "startid": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 29, - "id": 106, - "overridable": true, - "startid": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 107, - "overridable": true, - "startid": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 116, - "overridable": true, - "startid": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 112, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 113, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 114, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 109, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 110, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 77, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 90, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 121, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 60, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 70, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 14, - "id": 61, - "overridable": true, - "startid": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 57, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 67, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 58, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 69, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 39, - "id": 55, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 68, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 36, - "id": 56, - "overridable": true, - "startid": 39 - }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" +"type": "FeatureCollection", +"name": "graph", +"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, +"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", +"features": [ +{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, +{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, +{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, +{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, +{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, +{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, +{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, +{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, +{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, +{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, +{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, +{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, +{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } +] } diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson index 87a19b468ee..162a1a88884 100644 --- a/nav2_route/graphs/sample_graph.geojson +++ b/nav2_route/graphs/sample_graph.geojson @@ -1,504 +1,121 @@ { - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", "features": [ { - "geometry": { - "coordinates": [ - 0.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", + "type": "Feature", + "properties": + { "id": 0, - "metadata": { - "region": { + "frame": "map", + "metadata": + { + "region": + { + "x_values": [1.0, -1.0, -1.0, 1.0], + "y_values": [1.0, 1.0, -1.0, -1.0], "properties": { "class_type": "living_room", "number_of_lights": 10 - }, - "x_values": [ - 1.0, - -1.0, - -1.0, - 1.0 - ], - "y_values": [ - 1.0, - 1.0, - -1.0, - -1.0 - ] + } } }, - "operations": { - "stop": { - "metadata": { - "wait_for": 5.0 - }, + "operations": + { + "stop": + { + "type": "stop", "trigger": "NODE", - "type": "stop" + "metadata": + { + "wait_for": 5.0 + } } } }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 10.0, - "endid": 1, + "geometry": + { + "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, + { + "type": "Feature", + "properties": + { "id": 9, - "metadata": { - "aisle_number": 14, - "speed_limit": 0.8500000238418579 + "startid": 0, + "endid": 1, + "overridable": false, + "cost": 10.0, + "metadata": + { + "speed_limit": 0.85, + "aisle_number": 14 }, - "operations": { - "open_door": { - "metadata": { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_ENTER", + "metadata": + { "door_id": 54, "service_name": "open_door" - }, - "trigger": "ON_ENTER", - "type": "open_door" + } }, - "take_picture": { - "metadata": { - "resolution": [ - 1080, - 720 - ], - "type": "jpg" - }, + "take_picture": + { + "type": "take_picture", "trigger": "ON_EXIT", - "type": "take_picture" + "metadata": + { + "type": "jpg", + "resolution": [1080, 720] + } } - }, - "overridable": false, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 13, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 10, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 11, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 15, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 12, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 17, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 14, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 19, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 23, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 16, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 20, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 21, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 25, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 18, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 22, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 27, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 24, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 29, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 26, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 30, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 31, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 28, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 32, - "overridable": true, - "startid": 8 + } }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + }, + { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } + + ] } diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 074c520e4de..be4bd9a3d8a 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -525,7 +525,8 @@ def assistedTeleop(self, time_allowance: int = 30) -> Optional[RunningTask]: return RunningTask.ASSISTED_TELEOP def followPath(self, path: Path, controller_id: str = '', - goal_checker_id: str = '') -> Optional[RunningTask]: + goal_checker_id: str = '', progress_checker_id: str = '', + path_handler_id: str = '') -> Optional[RunningTask]: self.clearPreviousState() """Send a `FollowPath` action request.""" self.debug("Waiting for 'FollowPath' action server") @@ -536,6 +537,8 @@ def followPath(self, path: Path, controller_id: str = '', goal_msg.path = path goal_msg.controller_id = controller_id goal_msg.goal_checker_id = goal_checker_id + goal_msg.progress_checker_id = progress_checker_id + goal_msg.path_handler_id = path_handler_id self.info('Executing path...') send_goal_future = self.follow_path_client.send_goal_async( From 9dd1d1b5dd4a62bfbd87eb851c4c321234b15c29 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 14 Jan 2026 21:27:55 +0000 Subject: [PATCH 12/37] Ignore CTest Testing/ artifacts Signed-off-by: lotusymt luiseyang36@gmail.com Signed-off-by: lotusymt --- .gitignore | 3 +++ Testing/Temporary/CTestCostData.txt | 1 - Testing/Temporary/LastTest.log | 3 --- 3 files changed, 3 insertions(+), 4 deletions(-) delete mode 100644 Testing/Temporary/CTestCostData.txt delete mode 100644 Testing/Temporary/LastTest.log 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/Testing/Temporary/CTestCostData.txt b/Testing/Temporary/CTestCostData.txt deleted file mode 100644 index ed97d539c09..00000000000 --- a/Testing/Temporary/CTestCostData.txt +++ /dev/null @@ -1 +0,0 @@ ---- diff --git a/Testing/Temporary/LastTest.log b/Testing/Temporary/LastTest.log deleted file mode 100644 index fd05ba20ad1..00000000000 --- a/Testing/Temporary/LastTest.log +++ /dev/null @@ -1,3 +0,0 @@ -Start testing: Oct 31 21:33 UTC ----------------------------------------------------------- -End testing: Oct 31 21:33 UTC From 8cb15944ed4bc33ca6db108387c5d4345d63af88 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 14 Jan 2026 21:32:01 +0000 Subject: [PATCH 13/37] revert unintended testfile changes Signed-off-by: lotusymt --- nav2_system_tests/src/error_codes/test_error_codes.py | 1 + .../src/waypoint_follower/test_case_launch.py | 7 +------ 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py index 4b2d6b2919b..a99778b2e30 100755 --- a/nav2_system_tests/src/error_codes/test_error_codes.py +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -49,6 +49,7 @@ def main(argv: list[str] = sys.argv[1:]): # type: ignore[no-untyped-def] goal_pose1 = goal_pose goal_pose1.pose.position.x += 0.25 + path.header.frame_id = 'map' path.poses.append(initial_pose) path.poses.append(goal_pose) path.poses.append(goal_pose1) diff --git a/nav2_system_tests/src/waypoint_follower/test_case_launch.py b/nav2_system_tests/src/waypoint_follower/test_case_launch.py index ebcd491c2ea..53d0f70f5bd 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_launch.py +++ b/nav2_system_tests/src/waypoint_follower/test_case_launch.py @@ -48,15 +48,10 @@ def generate_launch_description() -> LaunchDescription: ) params_file = os.path.join(nav2_bringup_dir, 'params/nav2_params.yaml') - - # Replace the `use_astar` setting on the params file - param_substitutions = { - 'controller_server.ros__parameters.FollowPath.prune_distance': '1.0', - 'controller_server.ros__parameters.FollowPath.forward_prune_distance': '1.0'} configured_params = RewrittenYaml( source_file=params_file, root_key='', - param_rewrites=param_substitutions, + param_rewrites={}, value_rewrites={ 'KEEPOUT_ZONE_ENABLED': 'False', 'SPEED_ZONE_ENABLED': 'False', From a9c12e9e4b6af5edcdef51c3ede73810ff11a931 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Thu, 15 Jan 2026 00:01:15 +0000 Subject: [PATCH 14/37] update subscription Signed-off-by: lotusymt --- .../include/nav2_ros_common/interface_factories.hpp | 7 +++---- nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) 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 0e76bd55b71..ed5214d4411 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -197,9 +197,8 @@ inline rclcpp::PublisherOptions createPublisherOptions( * @param callback_group The callback group to use (if provided) * @return A shared pointer to the created nav2::Subscription */ -template> -typename nav2::Subscription::SharedPtr +template +typename nav2::Subscription::SharedPtr create_subscription( const NodeT & node, const std::string & topic_name, @@ -213,7 +212,7 @@ create_subscription( auto options = createSubscriptionOptions( topic_name, allow_parameter_qos_overrides, callback_group); - auto sub = std::make_shared>( + auto sub = std::make_shared>( node, topic_name, std::forward(callback), 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 2a81213e828..c91eab2b858 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -160,7 +160,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), const rclcpp::CallbackGroup::SharedPtr & callback_group = nullptr) { - auto sub = nav2::interfaces::create_subscription( + auto sub = nav2::interfaces::create_subscription( shared_from_this(), topic_name, std::forward(callback), qos, callback_group); this->add_managed_entity(sub); From 7ecc46cfd958fa9e110a6ae2411ac25fc7f7b3f5 Mon Sep 17 00:00:00 2001 From: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> Date: Tue, 20 Jan 2026 14:22:16 -0800 Subject: [PATCH 15/37] Clean up CMakeLists.txt by removing unused lines Removed unnecessary CMake module path and include statement. Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> --- nav2_common/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/nav2_common/CMakeLists.txt b/nav2_common/CMakeLists.txt index 8d1e7be819e..786c9449f45 100644 --- a/nav2_common/CMakeLists.txt +++ b/nav2_common/CMakeLists.txt @@ -6,9 +6,6 @@ find_package(ament_cmake_core REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(backward_ros REQUIRED) -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -include(nav2_package) - ament_python_install_package(nav2_common) ament_package( From d3839fe0f35e5d0ac318649411fcc1c280a78e0a Mon Sep 17 00:00:00 2001 From: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> Date: Tue, 20 Jan 2026 14:55:14 -0800 Subject: [PATCH 16/37] Update nav2_ros_common/include/nav2_ros_common/interface_factories.hpp Co-authored-by: Steve Macenski Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> --- nav2_ros_common/include/nav2_ros_common/interface_factories.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ed5214d4411..6605642a079 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -186,7 +186,7 @@ inline rclcpp::PublisherOptions createPublisherOptions( } /** - * @brief Create a subscription to a topic using Nav2 QoS profiles and allocator-aware 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. * From b897c7be9f85131bd5824d5206a794609beb39a4 Mon Sep 17 00:00:00 2001 From: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> Date: Tue, 20 Jan 2026 15:35:28 -0800 Subject: [PATCH 17/37] Update nav2_ros_common/include/nav2_ros_common/interface_factories.hpp Co-authored-by: Steve Macenski Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> --- nav2_ros_common/include/nav2_ros_common/interface_factories.hpp | 1 - 1 file changed, 1 deletion(-) 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 6605642a079..8cd67c6e681 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -220,7 +220,6 @@ create_subscription( options); // Auto-activate if the node is not a base lifecycle node - // Try to cast to lifecycle node - if it fails, it's a regular node auto lc_node = std::dynamic_pointer_cast(node); if (!lc_node) { // Not a lifecycle node, auto-activate the subscription From ba3f2456a5e9f204649d93d78e56660f4dc44157 Mon Sep 17 00:00:00 2001 From: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> Date: Tue, 20 Jan 2026 16:11:33 -0800 Subject: [PATCH 18/37] Update nav2_ros_common/include/nav2_ros_common/subscription.hpp Co-authored-by: Steve Macenski Signed-off-by: Mengting Yang <87471734+Lotusymt@users.noreply.github.com> --- nav2_ros_common/include/nav2_ros_common/subscription.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index 35a4d551d66..1fd3895e381 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -69,7 +69,7 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity return topic_name_.c_str(); } -private: +protected: template void init( const std::shared_ptr & node, From 91640b9fb66a18dafca8bd4c59c5cf984865adb2 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Thu, 22 Jan 2026 04:32:42 +0000 Subject: [PATCH 19/37] fixes Signed-off-by: lotusymt --- nav2_costmap_2d/test/unit/speed_filter_test.cpp | 9 ++++----- .../nav2_ros_common/interface_factories.hpp | 1 - .../include/nav2_ros_common/subscription.hpp | 14 ++++++++++++-- .../include/nav2_rviz_plugins/docking_panel.hpp | 9 +++++---- .../include/nav2_rviz_plugins/nav2_panel.hpp | 9 +++++---- .../include/nav2_rviz_plugins/route_tool.hpp | 3 ++- nav2_rviz_plugins/src/docking_panel.cpp | 12 ++++++------ nav2_rviz_plugins/src/nav2_panel.cpp | 16 ++++++++-------- nav2_rviz_plugins/src/route_tool.cpp | 4 ++-- .../assisted_teleop_behavior_tester.cpp | 2 +- 10 files changed, 45 insertions(+), 34 deletions(-) diff --git a/nav2_costmap_2d/test/unit/speed_filter_test.cpp b/nav2_costmap_2d/test/unit/speed_filter_test.cpp index 08b1d6961c8..7b018af0274 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_filter_test.cpp @@ -24,6 +24,7 @@ #include "rclcpp/rclcpp.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" @@ -108,11 +109,9 @@ class SpeedLimitSubscriber : public rclcpp::Node : Node("speed_limit_sub"), speed_limit_updated_(false) { subscriber_ = nav2::interfaces::create_subscription( - shared_from_this(), - // ^^^ FIX: use Nav2 factory so the returned type is nav2::Subscription<...>::SharedPtr - speed_limit_topic, - std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1), - rclcpp::QoS(10)); + shared_from_this(), speed_limit_topic, + std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1), + rclcpp::QoS(10)); } void speedLimitCallback( 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 8cd67c6e681..91215456c53 100644 --- a/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp +++ b/nav2_ros_common/include/nav2_ros_common/interface_factories.hpp @@ -229,7 +229,6 @@ create_subscription( return sub; } - /** * @brief Create a publisher to a topic using Nav2 QoS profiles and PublisherOptions * @param node Node to create the publisher on diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index 1fd3895e381..8a42faa6c87 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -83,14 +83,23 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity [this](typename MessageT::ConstSharedPtr msg, const rclcpp::MessageInfo & info) { if (!this->is_activated()) { - RCLCPP_WARN_ONCE( + RCLCPP_WARN_THROTTLE( logger_, + *clock_, + 1000, // milliseconds -> 1Hz "Trying to take messages on topic '%s', but the subscription is not activated. " "Dropping until activation.", topic_name_.c_str()); return; } - any_cb_.dispatch_intra_process(msg, info); + // Use the appropriate dispatch method based on whether the message came from + // intra-process (composition) or inter-process (DDS/serialization) + if (info.get_rmw_message_info().from_intra_process) { + any_cb_.dispatch_intra_process(msg, info); + } else { + auto non_const_msg = std::make_shared(*msg); + any_cb_.dispatch(non_const_msg, info); + } }; auto params_if = node->get_node_parameters_interface(); @@ -109,6 +118,7 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity typename RclcppSub::SharedPtr sub_; std::string topic_name_; rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; AnyCb any_cb_; }; 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 b4f524f84a2..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" @@ -107,10 +108,10 @@ private Q_SLOTS: nav2::ActionClient::SharedPtr undock_client_; // Docking / Undocking action feedback subscribers - rclcpp::Subscription::SharedPtr docking_feedback_sub_; - rclcpp::Subscription::SharedPtr undocking_feedback_sub_; - rclcpp::Subscription::SharedPtr docking_goal_status_sub_; - rclcpp::Subscription::SharedPtr undocking_goal_status_sub_; + 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_; // 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 784b9fe160e..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" @@ -146,13 +147,13 @@ private Q_SLOTS: nav_through_poses_action_client_; // Navigation action feedback subscribers - rclcpp::Subscription::SharedPtr + nav2::Subscription::SharedPtr navigation_feedback_sub_; - rclcpp::Subscription::SharedPtr + nav2::Subscription::SharedPtr nav_through_poses_feedback_sub_; - rclcpp::Subscription::SharedPtr + nav2::Subscription::SharedPtr navigation_goal_status_sub_; - rclcpp::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 3ee6060f3f3..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" @@ -110,7 +111,7 @@ private Q_SLOTS: rclcpp::Publisher::SharedPtr graph_vis_publisher_; - rclcpp::Subscription::SharedPtr + nav2::Subscription::SharedPtr clicked_point_subscription_; unsigned int next_node_id_ = 0; diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 95e0bb516c7..4eea7143d1d 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -277,16 +277,16 @@ 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", + docking_feedback_sub_ = nav2::interfaces::create_subscription( + node, "dock_robot/_action/feedback", rclcpp::SystemDefaultsQoS(), [this](const Dock::Impl::FeedbackMessage::ConstSharedPtr & msg) { docking_feedback_indicator_->setText(getDockFeedbackLabel(msg->feedback)); }); // Create action goal status subscribers - docking_goal_status_sub_ = node->create_subscription( - "dock_robot/_action/status", + docking_goal_status_sub_ = nav2::interfaces::create_subscription( + node, "dock_robot/_action/status", rclcpp::SystemDefaultsQoS(), [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { docking_goal_status_indicator_->setText( @@ -297,8 +297,8 @@ void DockingPanel::onInitialize() } }); - undocking_goal_status_sub_ = node->create_subscription( - "undock_robot/_action/status", + undocking_goal_status_sub_ = nav2::interfaces::create_subscription( + node, "undock_robot/_action/status", rclcpp::SystemDefaultsQoS(), [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { docking_goal_status_indicator_->setText( diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index a0f18df1665..0e062a4245e 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -894,8 +894,8 @@ Nav2Panel::onInitialize() // create action feedback subscribers navigation_feedback_sub_ = - node->create_subscription( - "navigate_to_pose/_action/feedback", + nav2::interfaces::create_subscription( + node, "navigate_to_pose/_action/feedback", rclcpp::SystemDefaultsQoS(), [this](const nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage::ConstSharedPtr & msg) { if (stoi(nr_of_loops_->displayText().toStdString()) > 0) { @@ -919,8 +919,8 @@ Nav2Panel::onInitialize() } }); nav_through_poses_feedback_sub_ = - node->create_subscription( - "navigate_through_poses/_action/feedback", + nav2::interfaces::create_subscription( + node, "navigate_through_poses/_action/feedback", rclcpp::SystemDefaultsQoS(), [this](const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::ConstSharedPtr & msg) { @@ -928,8 +928,8 @@ Nav2Panel::onInitialize() }); // create action goal status subscribers - navigation_goal_status_sub_ = node->create_subscription( - "navigate_to_pose/_action/status", + navigation_goal_status_sub_ = nav2::interfaces::create_subscription( + node, "navigate_to_pose/_action/status", rclcpp::SystemDefaultsQoS(), [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { navigation_goal_status_indicator_->setText( @@ -947,8 +947,8 @@ Nav2Panel::onInitialize() navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel()); } }); - nav_through_poses_goal_status_sub_ = node->create_subscription( - "navigate_through_poses/_action/status", + nav_through_poses_goal_status_sub_ = nav2::interfaces::create_subscription( + node, "navigate_through_poses/_action/status", rclcpp::SystemDefaultsQoS(), [this](const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg) { navigation_goal_status_indicator_->setText( diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp index fa71310b8dd..0b0cfed2fe2 100644 --- a/nav2_rviz_plugins/src/route_tool.cpp +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -54,8 +54,8 @@ 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_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 d82db066c00..1b1c6489917 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 @@ -71,7 +71,7 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester() node_, "cmd_vel", std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1), - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + rclcpp::SystemDefaultsQoS()); std::string costmap_topic = "/local_costmap/costmap_raw"; std::string footprint_topic = "/local_costmap/published_footprint"; From 1e5519285f2a3adbe42107d1f5a2e6fca777f5d7 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Thu, 22 Jan 2026 06:23:53 +0000 Subject: [PATCH 20/37] fix param order Signed-off-by: lotusymt --- nav2_rviz_plugins/src/docking_panel.cpp | 12 ++++++------ nav2_rviz_plugins/src/nav2_panel.cpp | 16 ++++++++-------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 4eea7143d1d..3f0ed7064b8 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -279,15 +279,14 @@ void DockingPanel::onInitialize() // Create action feedback subscriber docking_feedback_sub_ = nav2::interfaces::create_subscription( node, "dock_robot/_action/feedback", - rclcpp::SystemDefaultsQoS(), [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_ = nav2::interfaces::create_subscription( node, "dock_robot/_action/status", - rclcpp::SystemDefaultsQoS(), [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 +294,16 @@ 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_ = nav2::interfaces::create_subscription( node, "undock_robot/_action/status", - rclcpp::SystemDefaultsQoS(), [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 0e062a4245e..d9641cc5363 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -896,7 +896,6 @@ Nav2Panel::onInitialize() navigation_feedback_sub_ = nav2::interfaces::create_subscription( node, "navigate_to_pose/_action/feedback", - rclcpp::SystemDefaultsQoS(), [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,20 @@ Nav2Panel::onInitialize() } else { navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel(msg->feedback)); } - }); + }, + rclcpp::SystemDefaultsQoS()); nav_through_poses_feedback_sub_ = nav2::interfaces::create_subscription( node, "navigate_through_poses/_action/feedback", - rclcpp::SystemDefaultsQoS(), [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_ = nav2::interfaces::create_subscription( node, "navigate_to_pose/_action/status", - rclcpp::SystemDefaultsQoS(), [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 +945,18 @@ Nav2Panel::onInitialize() loop_count_ = 0; navigation_feedback_indicator_->setText(getNavToPoseFeedbackLabel()); } - }); + }, + rclcpp::SystemDefaultsQoS()); nav_through_poses_goal_status_sub_ = nav2::interfaces::create_subscription( node, "navigate_through_poses/_action/status", - rclcpp::SystemDefaultsQoS(), [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 From 0013d84110aab6971aceaa65990107f958969b2e Mon Sep 17 00:00:00 2001 From: lotusymt Date: Thu, 22 Jan 2026 06:27:49 +0000 Subject: [PATCH 21/37] fix linter Signed-off-by: lotusymt --- nav2_rviz_plugins/src/docking_panel.cpp | 6 ++++-- nav2_rviz_plugins/src/nav2_panel.cpp | 9 ++++++--- nav2_rviz_plugins/src/route_tool.cpp | 3 ++- 3 files changed, 12 insertions(+), 6 deletions(-) diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 3f0ed7064b8..5ed1262c99c 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -285,7 +285,8 @@ void DockingPanel::onInitialize() rclcpp::SystemDefaultsQoS()); // Create action goal status subscribers - docking_goal_status_sub_ = nav2::interfaces::create_subscription( + 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( @@ -297,7 +298,8 @@ void DockingPanel::onInitialize() }, rclcpp::SystemDefaultsQoS()); - undocking_goal_status_sub_ = nav2::interfaces::create_subscription( + 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( diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index d9641cc5363..7c30a19c7b3 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -919,7 +919,8 @@ Nav2Panel::onInitialize() }, rclcpp::SystemDefaultsQoS()); nav_through_poses_feedback_sub_ = - nav2::interfaces::create_subscription( + 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) { @@ -928,7 +929,8 @@ Nav2Panel::onInitialize() rclcpp::SystemDefaultsQoS()); // create action goal status subscribers - navigation_goal_status_sub_ = nav2::interfaces::create_subscription( + 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( @@ -947,7 +949,8 @@ Nav2Panel::onInitialize() } }, rclcpp::SystemDefaultsQoS()); - nav_through_poses_goal_status_sub_ = nav2::interfaces::create_subscription( + 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( diff --git a/nav2_rviz_plugins/src/route_tool.cpp b/nav2_rviz_plugins/src/route_tool.cpp index 0b0cfed2fe2..be314aed3a4 100644 --- a/nav2_rviz_plugins/src/route_tool.cpp +++ b/nav2_rviz_plugins/src/route_tool.cpp @@ -54,7 +54,8 @@ void RouteTool::onInitialize(void) } auto node = ros_node_abstraction->get_raw_node(); - clicked_point_subscription_ = nav2::interfaces::create_subscription( + 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()); From fa7eb4c577d770d2270648e5b0c9346a7b0da2a8 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 31 Jan 2026 18:51:54 +0000 Subject: [PATCH 22/37] safe logic for templated node's subscription Signed-off-by: lotusymt --- nav2_controller/src/controller_server.cpp | 14 ++++++- .../nav2_costmap_2d/costmap_subscriber.hpp | 38 +++++++++++++------ .../nav2_costmap_2d/footprint_subscriber.hpp | 21 +++++++--- 3 files changed, 55 insertions(+), 18 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 8794ee04339..df74aa5d284 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -217,10 +217,17 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2::CallbackReturn -ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); + // Activate all managed entities (subscriptions, publishers created via + // create_subscription/create_publisher) before server-specific activation + auto ret = rclcpp_lifecycle::LifecycleNode::on_activate(state); + if (ret != nav2::CallbackReturn::SUCCESS) { + return ret; + } + const auto costmap_ros_state = costmap_ros_->activate(); if (costmap_ros_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { return nav2::CallbackReturn::FAILURE; @@ -256,7 +263,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } nav2::CallbackReturn -ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Deactivating"); @@ -281,6 +288,9 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) tracking_feedback_pub_->on_deactivate(); param_handler_->deactivate(); + // Deactivate all managed entities (subscriptions, publishers) last + rclcpp_lifecycle::LifecycleNode::on_deactivate(state); + // destroy bond connection destroyBond(); 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..77067a88687 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,31 @@ 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 node_base = std::shared_ptr(parent); + auto lc_node = std::dynamic_pointer_cast(node_base); + if (lc_node) { + costmap_sub_ = lc_node->create_subscription( + topic_name_, + std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1), + nav2::qos::LatchedSubscriptionQoS(3), callback_group); + + costmap_update_sub_ = lc_node->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); + } } /** 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..e4e7853c6e1 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,20 @@ 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 node_base = std::shared_ptr(parent); + auto lc_node = std::dynamic_pointer_cast(node_base); + if (lc_node) { + footprint_sub_ = lc_node->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)); + } } /** From 6c0174207206121aff0961232657451a17b0bdc9 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 4 Mar 2026 05:02:00 +0000 Subject: [PATCH 23/37] use handle message, bypass latched message, tests passed Signed-off-by: lotusymt --- .pre-commit-config.yaml | 4 +- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 4 +- nav2_amcl/src/amcl_node.cpp | 4 + .../action/test_check_stop_status_action.cpp | 1 + .../condition/test_is_battery_charging.cpp | 6 + .../plugins/condition/test_is_battery_low.cpp | 6 + .../decorator/test_goal_updater_node.cpp | 6 + .../decorator/test_speed_controller.cpp | 1 + .../plugins/assisted_teleop.hpp | 10 + nav2_behaviors/plugins/assisted_teleop.cpp | 14 + nav2_behaviors/src/behavior_server.cpp | 21 +- nav2_bt_navigator/src/bt_navigator.cpp | 4 + nav2_collision_monitor/CMakeLists.txt | 2 + .../include/nav2_collision_monitor/circle.hpp | 9 + .../nav2_collision_monitor/costmap.hpp | 3 + .../lifecycle_point_cloud_subscription.hpp | 77 + .../nav2_collision_monitor/pointcloud.hpp | 18 +- .../nav2_collision_monitor/polygon.hpp | 4 +- .../nav2_collision_monitor/polygon_source.hpp | 3 + .../include/nav2_collision_monitor/range.hpp | 3 + .../include/nav2_collision_monitor/scan.hpp | 3 + .../include/nav2_collision_monitor/source.hpp | 10 + nav2_collision_monitor/src/circle.cpp | 16 + .../src/collision_detector_node.cpp | 22 +- .../src/collision_monitor_node.cpp | 31 +- nav2_collision_monitor/src/costmap.cpp | 10 + .../lifecycle_point_cloud_subscription.cpp | 69 + nav2_collision_monitor/src/pointcloud.cpp | 27 +- nav2_collision_monitor/src/polygon.cpp | 12 + nav2_collision_monitor/src/polygon_source.cpp | 10 + nav2_collision_monitor/src/range.cpp | 10 + nav2_collision_monitor/src/scan.cpp | 10 + .../test/collision_detector_node_test.cpp | 26 +- .../test/collision_monitor_node_bag.cpp | 13 +- .../test/collision_monitor_node_test.cpp | 43 +- nav2_common/CMakeLists.txt | 1 + nav2_controller/src/controller_server.cpp | 19 +- .../nav2_core/waypoint_task_executor.hpp | 11 + .../costmap_filters/binary_filter.hpp | 4 + .../costmap_filters/costmap_filter.hpp | 12 + .../costmap_filters/keepout_filter.hpp | 4 + .../costmap_filters/speed_filter.hpp | 4 + .../nav2_costmap_2d/costmap_subscriber.hpp | 7 +- .../nav2_costmap_2d/footprint_subscriber.hpp | 17 +- .../plugins/costmap_filters/binary_filter.cpp | 22 + .../costmap_filters/costmap_filter.cpp | 2 + .../costmap_filters/keepout_filter.cpp | 22 + .../plugins/costmap_filters/speed_filter.cpp | 22 + nav2_costmap_2d/plugins/static_layer.cpp | 12 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 44 +- nav2_costmap_2d/src/footprint_subscriber.cpp | 10 + .../test/integration/inflation_tests.cpp | 1 + .../integration/plugin_container_tests.cpp | 1 + .../integration/test_costmap_2d_publisher.cpp | 8 + .../integration/test_costmap_subscriber.cpp | 5 + .../test_costmap_topic_collision_checker.cpp | 1 + .../test/unit/binary_filter_test.cpp | 43 +- .../test/unit/keepout_filter_test.cpp | 3 + .../test/unit/speed_filter_test.cpp | 39 +- .../opennav_docking/src/docking_server.cpp | 1 + .../src/simple_charging_dock.cpp | 19 + .../src/simple_non_charging_dock.cpp | 13 + .../opennav_docking/test/test_controller.cpp | 10 +- .../src/following_server.cpp | 3 + nav2_map_server/src/map_saver/main_cli.cpp | 2 + nav2_map_server/src/map_saver/map_saver.cpp | 4 +- nav2_map_server/test/unit/CMakeLists.txt | 1 + .../test/unit/test_vector_object_server.cpp | 42 +- .../test/unit/test_vector_object_shapes.cpp | 13 +- .../test/trajectory_visualizer_tests.cpp | 3 + .../include/nav2_ros_common/subscription.hpp | 159 +- nav2_ros_common/test/CMakeLists.txt | 21 + nav2_ros_common/test/test_actions.cpp | 3 + .../test/test_subscription_latched.cpp | 85 + nav2_route/graphs/aws_graph.geojson | 1792 +++++++++++++++-- nav2_route/graphs/sample_graph.geojson | 581 +++++- nav2_route/test/test_collision_operation.cpp | 4 + nav2_route/test/test_edge_scorers.cpp | 4 + nav2_route/test/test_operations.cpp | 1 + nav2_route/test/test_path_converter.cpp | 1 + nav2_smac_planner/test/test_smac_hybrid.cpp | 2 + nav2_smac_planner/test/test_smac_lattice.cpp | 2 + .../assisted_teleop_behavior_tester.cpp | 6 + .../src/planning/test_planner_plugins.cpp | 3 +- .../include/nav2_util/odometry_utils.hpp | 16 + .../include/nav2_util/twist_subscriber.hpp | 26 + nav2_util/test/test_odometry_utils.cpp | 1 + .../src/velocity_smoother.cpp | 18 +- .../test/test_velocity_smoother.cpp | 3 + .../plugins/input_at_waypoint.hpp | 11 + .../plugins/photo_at_waypoint.hpp | 10 + .../plugins/input_at_waypoint.cpp | 10 + .../plugins/photo_at_waypoint.cpp | 14 + .../src/waypoint_follower.cpp | 3 +- .../test/test_task_executors.cpp | 3 + 95 files changed, 3308 insertions(+), 408 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp create mode 100644 nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp create mode 100644 nav2_ros_common/test/test_subscription_latched.cpp 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..09a9da668c1 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -172,14 +172,22 @@ 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_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 +201,13 @@ BehaviorServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) (*iter)->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..d8d4acc863b 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -33,6 +33,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 +71,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..09cc6a914f9 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/lifecycle_point_cloud_subscription.hpp @@ -0,0 +1,77 @@ +// 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 used to create the transport (must outlive this subscription). + */ + explicit LifecyclePointCloudSubscription(rclcpp_lifecycle::LifecycleNode & 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 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..6b5e984562d 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -115,26 +115,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 +159,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..58aed87975e --- /dev/null +++ b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp @@ -0,0 +1,69 @@ +// 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" + +namespace nav2_collision_monitor +{ + +LifecyclePointCloudSubscription::LifecyclePointCloudSubscription( + rclcpp_lifecycle::LifecycleNode & node) +{ + pct_ = std::make_shared(node); +} + +LifecyclePointCloudSubscription::~LifecyclePointCloudSubscription() +{ + shutdown(); +} + +void LifecyclePointCloudSubscription::subscribe( + const std::string & topic, + const rclcpp::QoS & qos, + Callback callback, + const point_cloud_transport::TransportHints & transport_hints) +{ + auto gated = [this, cb = std::move(callback)]( + sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + if (is_activated()) { + cb(msg); + } + }; +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + data_sub_ = pct_->subscribe(topic, qos, gated, {}, &transport_hints); +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic pop +#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..a33c96dafce 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 78137441b80..1cba7112736 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_bag.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_bag.cpp @@ -72,14 +72,17 @@ 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_ = 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()); @@ -87,7 +90,7 @@ class MetricsCatcher : public rclcpp::Node { 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()); @@ -118,6 +121,11 @@ class MetricsCatcher : public rclcpp::Node { // 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_) { @@ -261,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_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 df74aa5d284..19ddf688bad 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -217,21 +217,16 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2::CallbackReturn -ControllerServer::on_activate(const rclcpp_lifecycle::State & state) +ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - // Activate all managed entities (subscriptions, publishers created via - // create_subscription/create_publisher) before server-specific activation - auto ret = rclcpp_lifecycle::LifecycleNode::on_activate(state); - if (ret != nav2::CallbackReturn::SUCCESS) { - return ret; - } - const auto costmap_ros_state = costmap_ros_->activate(); 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(); @@ -263,7 +258,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2::CallbackReturn -ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) +ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); @@ -281,16 +276,14 @@ 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(); tracking_feedback_pub_->on_deactivate(); param_handler_->deactivate(); - // Deactivate all managed entities (subscriptions, publishers) last - rclcpp_lifecycle::LifecycleNode::on_deactivate(state); - // destroy bond connection destroyBond(); 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 77067a88687..73c422a5dd0 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -56,15 +56,14 @@ class CostmapSubscriber // 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 node_base = std::shared_ptr(parent); - auto lc_node = std::dynamic_pointer_cast(node_base); + auto lc_node = std::dynamic_pointer_cast(parent); if (lc_node) { - costmap_sub_ = lc_node->create_subscription( + 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->create_subscription( + costmap_update_sub_ = lc_node->template create_subscription( topic_name_ + "_updates", std::bind(&CostmapSubscriber::costmapUpdateCallback, this, std::placeholders::_1), nav2::qos::LatchedSubscriptionQoS(), callback_group); 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 e4e7853c6e1..9c10f653169 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp @@ -51,10 +51,9 @@ class FootprintSubscriber { // 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 node_base = std::shared_ptr(parent); - auto lc_node = std::dynamic_pointer_cast(node_base); + auto lc_node = std::dynamic_pointer_cast(parent); if (lc_node) { - footprint_sub_ = lc_node->create_subscription( + footprint_sub_ = lc_node->template create_subscription( topic_name, std::bind(&FootprintSubscriber::footprint_callback, this, std::placeholders::_1), nav2::qos::StandardTopicQoS()); @@ -92,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..60353dbe98b 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_subscribe_transient_local_) { + 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..1a2e2f43327 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -95,6 +95,13 @@ class LayerSubscriber executor_thread_ = std::make_unique(executor_); } + void activate() + { + if (layer_sub_) { + layer_sub_->on_activate(); + } + } + ~LayerSubscriber() { executor_thread_.reset(); @@ -128,6 +135,7 @@ class CostmapRosTestFixture : public ::testing::Test costmap_lifecycle_node_->shared_from_this()); costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); + layer_subscriber_->activate(); } ~CostmapRosTestFixture() override diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 485d3b3eb6a..7c6f8ff2c46 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp @@ -53,6 +53,11 @@ class TestCostmapSubscriberShould : public ::testing::Test updateCostmapMsgCount = 0; updateCostmapRawMsgCount = 0; + dummyCostmapMsgSubscriber->on_activate(); + dummyCostmapRawMsgSubscriber->on_activate(); + dummyCostmapUpdateMsgSubscriber->on_activate(); + dummyCostmapRawUpdateMsgSubscriber->on_activate(); + costmapSubscriber = std::make_unique(node, topicName + "_raw"); 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 88171bb8e12..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" @@ -106,17 +108,23 @@ 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) + { + msg_.data = default_state; + } + + void initSubscription() { subscriber_ = nav2::interfaces::create_subscription( shared_from_this(), - binary_state_topic, + binary_state_topic_, std::bind(&BinaryStateSubscriber::binaryStateCallback, this, std::placeholders::_1), rclcpp::QoS(10)); + } - - // Initialize with default state - msg_.data = default_state; + void activate() + { + subscriber_->on_activate(); } void binaryStateCallback( @@ -142,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_; @@ -395,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(); @@ -729,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 7b018af0274..53348d8113b 100644 --- a/nav2_costmap_2d/test/unit/speed_filter_test.cpp +++ b/nav2_costmap_2d/test/unit/speed_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 "nav2_ros_common/interface_factories.hpp" #include "tf2_ros/buffer.hpp" @@ -106,14 +108,22 @@ 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_ = nav2::interfaces::create_subscription( - shared_from_this(), speed_limit_topic, + shared_from_this(), speed_limit_topic_, std::bind(&SpeedLimitSubscriber::speedLimitCallback, this, std::placeholders::_1), rclcpp::QoS(10)); } + void activate() + { + subscriber_->on_activate(); + } + void speedLimitCallback( const nav2_msgs::msg::SpeedLimit::ConstSharedPtr msg) { @@ -137,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_; @@ -370,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(); @@ -632,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_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..96a18ae08f7 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -197,7 +197,9 @@ bool MapSaver::saveMapTopicToFile( auto map_sub = create_subscription( map_topic_loc, mapCallback, map_qos, callback_group); - + if (!map_subscribe_transient_local_) { + 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_vector_object_server.cpp b/nav2_map_server/test/unit/test_vector_object_server.cpp index d54f1cbd3ec..1e6dbfcfcd2 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); @@ -183,6 +187,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 +532,7 @@ TEST_F(Tester, testObtainParams) setShapesParams( "01010101-0101-0101-0101-010101010101", "01010101-0101-0101-0101-010101010102"); - vo_server_->start(); + startVOServer(); verifyMap(true); @@ -594,7 +604,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 +686,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 +772,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 +895,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 +966,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 +1012,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 +1058,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 +1114,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 +1141,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 +1201,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 +1475,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 +1527,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/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index 8a42faa6c87..1a881b59136 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -15,13 +15,16 @@ #ifndef NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ #define NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ +#include #include #include #include #include "rclcpp/rclcpp.hpp" -#include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/get_message_type_support_handle.hpp" #include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/subscription_factory.hpp" +#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/managed_entity.hpp" @@ -30,15 +33,85 @@ namespace nav2 { +/// Subscription that gates message handling by an activation predicate. +/// Used as the actual rclcpp subscription held by nav2::Subscription; allows +/// overriding handle_message to check activation without wrapping the user callback. +template> +class LifecycleSubscription : public rclcpp::Subscription +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(LifecycleSubscription) + + using Base = rclcpp::Subscription; + + LifecycleSubscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rclcpp::QoS & qos, + rclcpp::AnySubscriptionCallback callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename Base::MessageMemoryStrategyType::SharedPtr message_memory_strategy, + std::shared_ptr + subscription_topic_statistics = nullptr) + : Base( + node_base, + type_support_handle, + topic_name, + qos, + callback, + options, + message_memory_strategy, + subscription_topic_statistics) + {} + + void set_activation_predicate(std::function predicate) + { + is_activated_ = std::move(predicate); + } + + void handle_message( + std::shared_ptr & message, + const rclcpp::MessageInfo & message_info) override + { + if (!is_activated_()) { + return; + } + Base::handle_message(message, message_info); + } + + void handle_serialized_message( + const std::shared_ptr & serialized_message, + const rclcpp::MessageInfo & message_info) override + { + if (!is_activated_()) { + return; + } + Base::handle_serialized_message(serialized_message, message_info); + } + + void handle_loaned_message( + void * loaned_message, + const rclcpp::MessageInfo & message_info) override + { + if (!is_activated_()) { + return; + } + Base::handle_loaned_message(loaned_message, message_info); + } + +private: + std::function is_activated_{[]() {return false;}}; +}; + template> class Subscription : public rclcpp_lifecycle::SimpleManagedEntity { public: RCLCPP_SMART_PTR_DEFINITIONS(Subscription) - using RclcppSub = rclcpp::Subscription; + using LifecycleSub = LifecycleSubscription; using Options = rclcpp::SubscriptionOptionsWithAllocator; - using AnyCb = rclcpp::AnySubscriptionCallback; template Subscription( @@ -47,9 +120,7 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity CallbackT && user_callback, const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), const Options & options = Options{}) - : topic_name_(topic_name), - logger_(node->get_logger()), - any_cb_(*options.get_allocator()) + : topic_name_(topic_name) { init(node, qos, std::forward(user_callback), options); } @@ -77,49 +148,47 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity CallbackT && user_callback, const Options & options) { - any_cb_.set(std::forward(user_callback)); - - auto wrapped_cb = - [this](typename MessageT::ConstSharedPtr msg, const rclcpp::MessageInfo & info) - { - if (!this->is_activated()) { - RCLCPP_WARN_THROTTLE( - logger_, - *clock_, - 1000, // milliseconds -> 1Hz - "Trying to take messages on topic '%s', but the subscription is not activated. " - "Dropping until activation.", - topic_name_.c_str()); - return; - } - // Use the appropriate dispatch method based on whether the message came from - // intra-process (composition) or inter-process (DDS/serialization) - if (info.get_rmw_message_info().from_intra_process) { - any_cb_.dispatch_intra_process(msg, info); - } else { - auto non_const_msg = std::make_shared(*msg); - any_cb_.dispatch(non_const_msg, info); - } - }; - - auto params_if = node->get_node_parameters_interface(); - auto topics_if = node->get_node_topics_interface(); + rclcpp::AnySubscriptionCallback any_cb(*options.get_allocator()); + any_cb.set(std::forward(user_callback)); - sub_ = rclcpp::create_subscription( - params_if, - topics_if, - topic_name_, - qos, - std::move(wrapped_cb), - options, - rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default()); + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + rclcpp_lifecycle::SimpleManagedEntity::on_activate(); + } + + auto topics_if = node->get_node_topics_interface(); + auto msg_mem_strat = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + + rclcpp::SubscriptionFactory factory{ + [options, msg_mem_strat, any_cb]( + rclcpp::node_interfaces::NodeBaseInterface * node_base_ptr, + const std::string & topic, + const rclcpp::QoS & actual_qos) -> rclcpp::SubscriptionBase::SharedPtr { + auto sub = LifecycleSub::make_shared( + node_base_ptr, + rclcpp::get_message_type_support_handle(), + topic, + actual_qos, + any_cb, + 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); + if (sub_) { + sub_->set_activation_predicate([this]() {return is_activated();}); + } } - typename RclcppSub::SharedPtr sub_; + typename LifecycleSub::SharedPtr sub_; std::string topic_name_; - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - AnyCb any_cb_; }; } // 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 f6e74f27186..1ea1d8ee073 100644 --- a/nav2_ros_common/test/test_actions.cpp +++ b/nav2_ros_common/test/test_actions.cpp @@ -72,6 +72,9 @@ class FibonacciServerNode : public rclcpp::Node 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..c66b3998d80 --- /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()); + // Transient subscription is activated on init. + + 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/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 2e6a43af148..4c4fb3208ff 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,132 +1,1664 @@ { -"type": "FeatureCollection", -"name": "graph", -"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, -"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", -"features": [ -{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, -{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, -{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, -{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, -{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, -{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, -{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, -{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, -{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, -{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, -{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, -{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, -{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, -{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, -{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, -{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, -{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, -{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, -{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, -{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, -{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, -{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, -{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, -{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, -{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, -{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } -] + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, + "features": [ + { + "geometry": { + "coordinates": [ + -4.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -3.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 0.6499999761581421 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 5.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + 8.649999618530273 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -7.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -5.872222900390625 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + -7.763441562652588 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -7.874690055847168 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + -8.690509796142578 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -9.395081520080566 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 3.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 6.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 4.650000095367432 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + 2.6500000953674316 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -6.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -6.0, + -9.350000381469727 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -2.0, + -4.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + -4.0, + -5.349999904632568 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -2.3499999046325684 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + -0.3499999940395355 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 4.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 6.0, + 1.649999976158142 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 39 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 75, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 92, + "overridable": true, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 78, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 89, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 26, + "id": 102, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 119, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 120, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 79, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 88, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 24, + "id": 99, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 117, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 118, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 81, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 86, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 80, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 87, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 122, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 82, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 83, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 85, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 22, + "id": 96, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 84, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 76, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 91, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 28, + "id": 105, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 42, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 52, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 38, + "id": 54, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 43, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 51, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 30, + "id": 115, + "overridable": true, + "startid": 9 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 44, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 50, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 59, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 111, + "overridable": true, + "startid": 10 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 45, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 10, + "id": 49, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 108, + "overridable": true, + "startid": 11 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 46, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 11, + "id": 48, + "overridable": true, + "startid": 12 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 63, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 71, + "overridable": true, + "startid": 13 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 62, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 17, + "id": 64, + "overridable": true, + "startid": 14 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 12, + "id": 47, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 72, + "overridable": true, + "startid": 15 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 66, + "overridable": true, + "startid": 16 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 16, + "id": 65, + "overridable": true, + "startid": 17 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 19, + "id": 73, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 15, + "id": 94, + "overridable": true, + "startid": 18 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 74, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 18, + "id": 93, + "overridable": true, + "startid": 19 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 41, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 21, + "id": 53, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 123, + "overridable": true, + "startid": 20 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 20, + "id": 40, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 95, + "overridable": true, + "startid": 21 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 23, + "id": 97, + "overridable": true, + "startid": 22 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 98, + "overridable": true, + "startid": 23 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 25, + "id": 100, + "overridable": true, + "startid": 24 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 101, + "overridable": true, + "startid": 25 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 27, + "id": 103, + "overridable": true, + "startid": 26 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 104, + "overridable": true, + "startid": 27 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 29, + "id": 106, + "overridable": true, + "startid": 28 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 107, + "overridable": true, + "startid": 29 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 116, + "overridable": true, + "startid": 30 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 31, + "id": 112, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 113, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 114, + "overridable": true, + "startid": 31 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 109, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 33, + "id": 110, + "overridable": true, + "startid": 32 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 77, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 90, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 32, + "id": 121, + "overridable": true, + "startid": 33 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 60, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 13, + "id": 70, + "overridable": true, + "startid": 34 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 14, + "id": 61, + "overridable": true, + "startid": 35 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 57, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 35, + "id": 67, + "overridable": true, + "startid": 36 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 9, + "id": 58, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 34, + "id": 69, + "overridable": true, + "startid": 37 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 39, + "id": 55, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 37, + "id": 68, + "overridable": true, + "startid": 38 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 36, + "id": 56, + "overridable": true, + "startid": 39 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" } diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson index 162a1a88884..87a19b468ee 100644 --- a/nav2_route/graphs/sample_graph.geojson +++ b/nav2_route/graphs/sample_graph.geojson @@ -1,121 +1,504 @@ { - "type": "FeatureCollection", - "name": "graph", - "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, - "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", + "crs": { + "properties": { + "name": "urn:ogc:def:crs:EPSG::3857" + }, + "type": "name" + }, "features": [ { - "type": "Feature", - "properties": - { - "id": 0, + "geometry": { + "coordinates": [ + 0.0, + 0.0 + ], + "type": "Point" + }, + "properties": { "frame": "map", - "metadata": - { - "region": - { - "x_values": [1.0, -1.0, -1.0, 1.0], - "y_values": [1.0, 1.0, -1.0, -1.0], + "id": 0, + "metadata": { + "region": { "properties": { "class_type": "living_room", "number_of_lights": 10 - } + }, + "x_values": [ + 1.0, + -1.0, + -1.0, + 1.0 + ], + "y_values": [ + 1.0, + 1.0, + -1.0, + -1.0 + ] } }, - "operations": - { - "stop": - { - "type": "stop", - "trigger": "NODE", - "metadata": - { + "operations": { + "stop": { + "metadata": { "wait_for": 5.0 - } + }, + "trigger": "NODE", + "type": "stop" } } }, - "geometry": - { - "type": "Point", - "coordinates": [ 0.0, 0.0 ] - } - }, - { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, - { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, - { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, - { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, - { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, - { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, - { - "type": "Feature", - "properties": - { - "id": 9, - "startid": 0, - "endid": 1, - "overridable": false, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 0.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 1.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 0.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 1.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "coordinates": [ + 2.0, + 2.0 + ], + "type": "Point" + }, + "properties": { + "frame": "map", + "id": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { "cost": 10.0, - "metadata": - { - "speed_limit": 0.85, - "aisle_number": 14 + "endid": 1, + "id": 9, + "metadata": { + "aisle_number": 14, + "speed_limit": 0.8500000238418579 }, - "operations": - { - "open_door": - { - "type": "open_door", - "trigger": "ON_ENTER", - "metadata": - { + "operations": { + "open_door": { + "metadata": { "door_id": 54, "service_name": "open_door" - } + }, + "trigger": "ON_ENTER", + "type": "open_door" }, - "take_picture": - { - "type": "take_picture", + "take_picture": { + "metadata": { + "resolution": [ + 1080, + 720 + ], + "type": "jpg" + }, "trigger": "ON_EXIT", - "metadata": - { - "type": "jpg", - "resolution": [1080, 720] - } + "type": "take_picture" } - } + }, + "overridable": false, + "startid": 0 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 13, + "overridable": true, + "startid": 0 }, - "geometry": - { - "type": "MultiLineString", - "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] - } - }, - { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, - { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } - - ] + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 10, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 11, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 15, + "overridable": true, + "startid": 1 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 12, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 17, + "overridable": true, + "startid": 2 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 0, + "id": 14, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 19, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 23, + "overridable": true, + "startid": 3 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 1, + "id": 16, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 20, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 21, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 25, + "overridable": true, + "startid": 4 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 2, + "id": 18, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 22, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 27, + "overridable": true, + "startid": 5 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 3, + "id": 24, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 29, + "overridable": true, + "startid": 6 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 4, + "id": 26, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 6, + "id": 30, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 8, + "id": 31, + "overridable": true, + "startid": 7 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 5, + "id": 28, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + }, + { + "geometry": { + "type": "MultiLineString" + }, + "properties": { + "cost": 0.0, + "endid": 7, + "id": 32, + "overridable": true, + "startid": 8 + }, + "type": "Feature" + } + ], + "name": "graph", + "type": "FeatureCollection" } 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_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 1b1c6489917..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 @@ -25,6 +25,7 @@ #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 @@ -129,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; } @@ -138,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/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/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 From d504958d46537cfa2b7fce68c6b05360ee3b1c44 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 4 Mar 2026 06:27:41 +0000 Subject: [PATCH 24/37] make cloud points compatible Signed-off-by: lotusymt --- nav2_collision_monitor/CMakeLists.txt | 5 ++ .../lifecycle_point_cloud_subscription.hpp | 6 ++- .../lifecycle_point_cloud_subscription.cpp | 53 +++++++++++++++---- nav2_collision_monitor/src/pointcloud.cpp | 2 +- 4 files changed, 53 insertions(+), 13 deletions(-) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index d8d4acc863b..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) 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 index 09cc6a914f9..f878d3bd59b 100644 --- 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 @@ -42,9 +42,10 @@ class LifecyclePointCloudSubscription : public rclcpp_lifecycle::SimpleManagedEn /** * @brief Construct and create the PointCloudTransport. - * @param node Lifecycle node used to create the transport (must outlive this subscription). + * @param node Lifecycle node shared pointer (PointCloudTransport requires Node::SharedPtr on ROS 2 Jazzy). */ - explicit LifecyclePointCloudSubscription(rclcpp_lifecycle::LifecycleNode & node); + explicit LifecyclePointCloudSubscription( + rclcpp_lifecycle::LifecycleNode::SharedPtr node); ~LifecyclePointCloudSubscription(); @@ -68,6 +69,7 @@ class LifecyclePointCloudSubscription : public rclcpp_lifecycle::SimpleManagedEn void shutdown(); private: + std::shared_ptr node_; std::shared_ptr pct_; point_cloud_transport::Subscriber data_sub_; }; diff --git a/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp index 58aed87975e..fe2c580ee68 100644 --- a/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp +++ b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp @@ -15,13 +15,43 @@ #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 & node) + rclcpp_lifecycle::LifecycleNode::SharedPtr node) +: node_(node) { - pct_ = std::make_shared(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() @@ -35,19 +65,22 @@ void LifecyclePointCloudSubscription::subscribe( Callback callback, const point_cloud_transport::TransportHints & transport_hints) { - auto gated = [this, cb = std::move(callback)]( + std::function gated = + [this, cb = std::move(callback)]( sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { if (is_activated()) { cb(msg); } }; -#if defined(__GNUC__) && !defined(__clang__) -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - data_sub_ = pct_->subscribe(topic, qos, gated, {}, &transport_hints); -#if defined(__GNUC__) && !defined(__clang__) -#pragma GCC diagnostic pop +#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 } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index a33c96dafce..d5c57f3d899 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -76,7 +76,7 @@ void PointCloud::configure() #if defined(RCLCPP_VERSION_MAJOR) && RCLCPP_VERSION_MAJOR >= 30 const point_cloud_transport::TransportHints hint(transport_type_); - data_sub_ = LifecyclePointCloudSubscription::make_shared(*node); + data_sub_ = LifecyclePointCloudSubscription::make_shared(node); data_sub_->subscribe( source_topic, nav2::qos::SensorDataQoS(), std::bind(&PointCloud::dataCallback, this, std::placeholders::_1), From 6d541817abe48ebcd609a9f44e25ac4c7bff925c Mon Sep 17 00:00:00 2001 From: lotusymt Date: Wed, 4 Mar 2026 08:13:56 +0000 Subject: [PATCH 25/37] fix bug Signed-off-by: lotusymt --- .../src/lifecycle_point_cloud_subscription.cpp | 3 ++- nav2_collision_monitor/test/sources_test.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp index fe2c580ee68..e03ebc8bd7c 100644 --- a/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp +++ b/nav2_collision_monitor/src/lifecycle_point_cloud_subscription.cpp @@ -47,7 +47,8 @@ LifecyclePointCloudSubscription::LifecyclePointCloudSubscription( 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)"); + "LifecyclePointCloudSubscription: node must be a rclcpp::Node " + "(LifecycleNode inheriting Node)"); } pct_ = std::make_shared( std::shared_ptr(node, node_ptr)); diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index a330d0079b0..023682048e1 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -505,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( @@ -909,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); From d09c3fa21deafbf9557513fa5fa3f1b39a9273a1 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Thu, 5 Mar 2026 02:07:12 +0000 Subject: [PATCH 26/37] fix generate result error Signed-off-by: lotusymt --- nav2_collision_monitor/src/collision_monitor_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 6b5e984562d..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; } } From 344fc948dd338dba7453271718bba1e3fce6c833 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Mon, 27 Apr 2026 20:28:35 -0700 Subject: [PATCH 27/37] nav2_ros_common: create subscription on activate, destroy on deactivate Per maintainer feedback on PR #5834 (SteveMacenski, 2026-03-13: "I'd say all"), redesign nav2::Subscription so the underlying rclcpp::Subscription is constructed in on_activate() and released in on_deactivate(), for all subscriptions (not just transient_local). The previous LifecycleSubscription override that gated handle_message() and the transient_local auto-activate hack are removed: while inactive, no DDS endpoint exists and no callback can fire. - subscription.hpp: drop LifecycleSubscription, store callback/qos/options and a weak ref to the topics interface; build the rclcpp::Subscription via SubscriptionFactory inside on_activate(); reset it in on_deactivate(). - test_subscription_latched: explicitly activate the subscription before publishing (the old "transient_local auto-activates at init" guarantee is gone by design). - Revert geojson churn in nav2_route/graphs/{aws_graph,sample_graph}.geojson (unrelated to this PR per maintainer review). Signed-off-by: lotusymt --- .../include/nav2_ros_common/subscription.hpp | 160 +- .../test/test_subscription_latched.cpp | 2 +- nav2_route/graphs/aws_graph.geojson | 1792 ++--------------- nav2_route/graphs/sample_graph.geojson | 581 +----- 4 files changed, 276 insertions(+), 2259 deletions(-) diff --git a/nav2_ros_common/include/nav2_ros_common/subscription.hpp b/nav2_ros_common/include/nav2_ros_common/subscription.hpp index 1a881b59136..5014ceddf4e 100644 --- a/nav2_ros_common/include/nav2_ros_common/subscription.hpp +++ b/nav2_ros_common/include/nav2_ros_common/subscription.hpp @@ -15,8 +15,8 @@ #ifndef NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ #define NAV2_ROS_COMMON__SUBSCRIPTION_HPP_ -#include #include +#include #include #include @@ -24,8 +24,6 @@ #include "rclcpp/get_message_type_support_handle.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/subscription_factory.hpp" -#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/managed_entity.hpp" #include "nav2_ros_common/qos_profiles.hpp" @@ -33,85 +31,21 @@ namespace nav2 { -/// Subscription that gates message handling by an activation predicate. -/// Used as the actual rclcpp subscription held by nav2::Subscription; allows -/// overriding handle_message to check activation without wrapping the user callback. -template> -class LifecycleSubscription : public rclcpp::Subscription -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(LifecycleSubscription) - - using Base = rclcpp::Subscription; - - LifecycleSubscription( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const rosidl_message_type_support_t & type_support_handle, - const std::string & topic_name, - const rclcpp::QoS & qos, - rclcpp::AnySubscriptionCallback callback, - const rclcpp::SubscriptionOptionsWithAllocator & options, - typename Base::MessageMemoryStrategyType::SharedPtr message_memory_strategy, - std::shared_ptr - subscription_topic_statistics = nullptr) - : Base( - node_base, - type_support_handle, - topic_name, - qos, - callback, - options, - message_memory_strategy, - subscription_topic_statistics) - {} - - void set_activation_predicate(std::function predicate) - { - is_activated_ = std::move(predicate); - } - - void handle_message( - std::shared_ptr & message, - const rclcpp::MessageInfo & message_info) override - { - if (!is_activated_()) { - return; - } - Base::handle_message(message, message_info); - } - - void handle_serialized_message( - const std::shared_ptr & serialized_message, - const rclcpp::MessageInfo & message_info) override - { - if (!is_activated_()) { - return; - } - Base::handle_serialized_message(serialized_message, message_info); - } - - void handle_loaned_message( - void * loaned_message, - const rclcpp::MessageInfo & message_info) override - { - if (!is_activated_()) { - return; - } - Base::handle_loaned_message(loaned_message, message_info); - } - -private: - std::function is_activated_{[]() {return false;}}; -}; - +/// 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 LifecycleSub = LifecycleSubscription; + using RclcppSub = rclcpp::Subscription; using Options = rclcpp::SubscriptionOptionsWithAllocator; + using AnyCallback = rclcpp::AnySubscriptionCallback; template Subscription( @@ -120,56 +54,42 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity CallbackT && user_callback, const rclcpp::QoS & qos = nav2::qos::StandardTopicQoS(), const Options & options = Options{}) - : topic_name_(topic_name) + : topic_name_(topic_name), + qos_(qos), + options_(options), + any_callback_(*options.get_allocator()), + topics_interface_(node->get_node_topics_interface()) { - init(node, qos, std::forward(user_callback), options); + any_callback_.set(std::forward(user_callback)); } void on_activate() override { rclcpp_lifecycle::SimpleManagedEntity::on_activate(); - } - - void on_deactivate() override - { - rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); - } - - const char * get_topic_name() const noexcept - { - return topic_name_.c_str(); - } - -protected: - template - void init( - const std::shared_ptr & node, - const rclcpp::QoS & qos, - CallbackT && user_callback, - const Options & options) - { - rclcpp::AnySubscriptionCallback any_cb(*options.get_allocator()); - any_cb.set(std::forward(user_callback)); - - if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { - 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 topics_if = node->get_node_topics_interface(); 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, any_cb]( + [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 = LifecycleSub::make_shared( + auto sub = RclcppSub::make_shared( node_base_ptr, rclcpp::get_message_type_support_handle(), topic, actual_qos, - any_cb, + callback, options, msg_mem_strat, nullptr); @@ -178,17 +98,29 @@ class Subscription : public rclcpp_lifecycle::SimpleManagedEntity } }; - auto sub_base = topics_if->create_subscription(topic_name_, factory, qos); - topics_if->add_subscription(sub_base, options.callback_group); + 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); + } - sub_ = std::dynamic_pointer_cast(sub_base); - if (sub_) { - sub_->set_activation_predicate([this]() {return is_activated();}); - } + void on_deactivate() override + { + rclcpp_lifecycle::SimpleManagedEntity::on_deactivate(); + sub_.reset(); } - typename LifecycleSub::SharedPtr sub_; + 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/test_subscription_latched.cpp b/nav2_ros_common/test/test_subscription_latched.cpp index c66b3998d80..d01971d5ae1 100644 --- a/nav2_ros_common/test/test_subscription_latched.cpp +++ b/nav2_ros_common/test/test_subscription_latched.cpp @@ -55,7 +55,7 @@ TEST(SubscriptionLatched, LatchedMessageReceivedAfterActivation) topic_name, callback, nav2::qos::LatchedSubscriptionQoS()); - // Transient subscription is activated on init. + subscription->on_activate(); auto publisher = node->create_publisher( topic_name, diff --git a/nav2_route/graphs/aws_graph.geojson b/nav2_route/graphs/aws_graph.geojson index 4c4fb3208ff..2e6a43af148 100644 --- a/nav2_route/graphs/aws_graph.geojson +++ b/nav2_route/graphs/aws_graph.geojson @@ -1,1664 +1,132 @@ { - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, - "features": [ - { - "geometry": { - "coordinates": [ - -4.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -3.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 0.6499999761581421 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 5.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - 8.649999618530273 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -7.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -5.872222900390625 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - -7.763441562652588 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -7.874690055847168 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - -8.690509796142578 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -9.395081520080566 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 3.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 6.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 4.650000095367432 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - 2.6500000953674316 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -6.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -6.0, - -9.350000381469727 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -2.0, - -4.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - -4.0, - -5.349999904632568 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -2.3499999046325684 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - -0.3499999940395355 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 4.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 6.0, - 1.649999976158142 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 39 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 75, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 92, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 78, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 89, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 26, - "id": 102, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 119, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 120, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 79, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 88, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 24, - "id": 99, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 117, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 118, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 81, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 86, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 80, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 87, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 122, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 82, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 83, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 85, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 22, - "id": 96, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 84, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 76, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 91, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 28, - "id": 105, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 42, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 52, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 38, - "id": 54, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 43, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 51, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 30, - "id": 115, - "overridable": true, - "startid": 9 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 44, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 50, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 59, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 111, - "overridable": true, - "startid": 10 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 45, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 10, - "id": 49, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 108, - "overridable": true, - "startid": 11 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 46, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 11, - "id": 48, - "overridable": true, - "startid": 12 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 63, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 71, - "overridable": true, - "startid": 13 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 62, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 17, - "id": 64, - "overridable": true, - "startid": 14 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 12, - "id": 47, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 72, - "overridable": true, - "startid": 15 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 66, - "overridable": true, - "startid": 16 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 16, - "id": 65, - "overridable": true, - "startid": 17 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 19, - "id": 73, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 15, - "id": 94, - "overridable": true, - "startid": 18 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 74, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 18, - "id": 93, - "overridable": true, - "startid": 19 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 41, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 21, - "id": 53, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 123, - "overridable": true, - "startid": 20 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 20, - "id": 40, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 95, - "overridable": true, - "startid": 21 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 23, - "id": 97, - "overridable": true, - "startid": 22 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 98, - "overridable": true, - "startid": 23 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 25, - "id": 100, - "overridable": true, - "startid": 24 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 101, - "overridable": true, - "startid": 25 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 27, - "id": 103, - "overridable": true, - "startid": 26 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 104, - "overridable": true, - "startid": 27 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 29, - "id": 106, - "overridable": true, - "startid": 28 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 107, - "overridable": true, - "startid": 29 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 116, - "overridable": true, - "startid": 30 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 31, - "id": 112, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 113, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 114, - "overridable": true, - "startid": 31 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 109, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 33, - "id": 110, - "overridable": true, - "startid": 32 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 77, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 90, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 32, - "id": 121, - "overridable": true, - "startid": 33 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 60, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 13, - "id": 70, - "overridable": true, - "startid": 34 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 14, - "id": 61, - "overridable": true, - "startid": 35 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 57, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 35, - "id": 67, - "overridable": true, - "startid": 36 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 9, - "id": 58, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 34, - "id": 69, - "overridable": true, - "startid": 37 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 39, - "id": 55, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 37, - "id": 68, - "overridable": true, - "startid": 38 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 36, - "id": 56, - "overridable": true, - "startid": 39 - }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" +"type": "FeatureCollection", +"name": "graph", +"crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, +"date_generated": "Wed Feb 22 05:41:45 PM EST 2025", +"features": [ +{ "type": "Feature", "properties": { "id": 0, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -3.35 ] } }, +{ "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 0.65 ] } }, +{ "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 5.65 ] } }, +{ "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, 8.65 ] } }, +{ "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -7.35 ] } }, +{ "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 9, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 10, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 11, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 12, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 13, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 14, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -5.87222299296573 ] } }, +{ "type": "Feature", "properties": { "id": 15, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, -7.763441762457957 ] } }, +{ "type": "Feature", "properties": { "id": 16, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 17, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -7.874689925369259 ] } }, +{ "type": "Feature", "properties": { "id": 18, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, -8.690509786718851 ] } }, +{ "type": "Feature", "properties": { "id": 19, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -9.395081485157126 ] } }, +{ "type": "Feature", "properties": { "id": 20, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 3.65 ] } }, +{ "type": "Feature", "properties": { "id": 21, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 22, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 6.65 ] } }, +{ "type": "Feature", "properties": { "id": 23, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 4.65 ] } }, +{ "type": "Feature", "properties": { "id": 24, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, 2.65 ] } }, +{ "type": "Feature", "properties": { "id": 25, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 26, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 27, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 28, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -6.35 ] } }, +{ "type": "Feature", "properties": { "id": 29, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -6.0, -9.35 ] } }, +{ "type": "Feature", "properties": { "id": 30, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 31, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 32, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -2.0, -4.35 ] } }, +{ "type": "Feature", "properties": { "id": 33, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ -4.0, -5.35 ] } }, +{ "type": "Feature", "properties": { "id": 34, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 35, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -2.35 ] } }, +{ "type": "Feature", "properties": { "id": 36, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 37, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, -0.35 ] } }, +{ "type": "Feature", "properties": { "id": 38, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 4.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 39, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 6.0, 1.65 ] } }, +{ "type": "Feature", "properties": { "id": 40, "startid": 21, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 41, "startid": 20, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 42, "startid": 8, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 43, "startid": 9, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 44, "startid": 10, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 45, "startid": 11, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 46, "startid": 12, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 47, "startid": 15, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 48, "startid": 12, "endid": 11 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -5.87222299296573 ], [ 1.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 49, "startid": 11, "endid": 10 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ 1.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 50, "startid": 10, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 51, "startid": 9, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ 1.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 52, "startid": 8, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 53, "startid": 20, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 3.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 54, "startid": 8, "endid": 38 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.65 ], [ 4.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 55, "startid": 38, "endid": 39 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 6.0, 1.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 56, "startid": 39, "endid": 36 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, 1.65 ], [ 6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 57, "startid": 36, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 58, "startid": 37, "endid": 9 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 1.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 59, "startid": 10, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 60, "startid": 34, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 61, "startid": 35, "endid": 14 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -2.35 ], [ 6.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 62, "startid": 14, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 63, "startid": 13, "endid": 12 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 1.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 64, "startid": 14, "endid": 17 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -5.87222299296573 ], [ 6.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 65, "startid": 17, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -7.874689925369259 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 66, "startid": 16, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -7.874689925369259 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 67, "startid": 36, "endid": 35 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 6.0, -0.35 ], [ 6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 68, "startid": 38, "endid": 37 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, 1.65 ], [ 4.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 69, "startid": 37, "endid": 34 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -0.35 ], [ 4.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 70, "startid": 34, "endid": 13 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -2.35 ], [ 4.0, -5.87222299296573 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 71, "startid": 13, "endid": 16 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 4.0, -5.87222299296573 ], [ 4.0, -7.874689925369259 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 72, "startid": 15, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -7.763441762457957 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 73, "startid": 18, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 74, "startid": 19, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 75, "startid": 0, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 76, "startid": 7, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 77, "startid": 33, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 78, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 79, "startid": 2, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 80, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 81, "startid": 3, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 82, "startid": 5, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 8.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 83, "startid": 5, "endid": 21 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ 1.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 84, "startid": 6, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 8.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 85, "startid": 5, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -4.0, 5.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 86, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 5.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 87, "startid": 4, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 88, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 89, "startid": 1, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 90, "startid": 33, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 91, "startid": 7, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -4.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 92, "startid": 0, "endid": 19 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -9.35 ], [ -2.0, -9.395081485157126 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 93, "startid": 19, "endid": 18 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -9.395081485157126 ], [ 0.0, -8.690509786718851 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 94, "startid": 18, "endid": 15 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, -8.690509786718851 ], [ 1.0, -7.763441762457957 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 95, "startid": 21, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 6.65 ], [ -4.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 96, "startid": 5, "endid": 22 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 6.65 ], [ -6.0, 6.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 97, "startid": 22, "endid": 23 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 6.65 ], [ -6.0, 4.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 98, "startid": 23, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 4.65 ], [ -4.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 99, "startid": 2, "endid": 24 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -6.0, 2.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 100, "startid": 24, "endid": 25 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, 2.65 ], [ -6.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 101, "startid": 25, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 102, "startid": 1, "endid": 26 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -6.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 103, "startid": 26, "endid": 27 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -2.35 ], [ -6.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 104, "startid": 27, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 105, "startid": 7, "endid": 28 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -7.35 ], [ -6.0, -6.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 106, "startid": 28, "endid": 29 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -6.35 ], [ -6.0, -9.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 107, "startid": 29, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -6.0, -9.35 ], [ -4.0, -7.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 108, "startid": 11, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -4.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 109, "startid": 32, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 110, "startid": 32, "endid": 33 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -4.35 ], [ -4.0, -5.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 111, "startid": 10, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -2.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 112, "startid": 31, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 113, "startid": 31, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 114, "startid": 31, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -2.35 ], [ -4.0, -3.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 115, "startid": 9, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, -0.35 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 116, "startid": 30, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -2.0, -0.35 ], [ -4.0, 0.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 117, "startid": 2, "endid": 30 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -0.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 118, "startid": 2, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 0.65 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 119, "startid": 1, "endid": 31 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -2.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 120, "startid": 1, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -3.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 121, "startid": 33, "endid": 32 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, -5.35 ], [ -2.0, -4.35 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 122, "startid": 4, "endid": 20 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ -4.0, 3.65 ], [ 1.0, 3.65 ] ] ] } }, +{ "type": "Feature", "properties": { "id": 123, "startid": 20, "endid": 4 }, "geometry": { "type": "LineString", "coordinates": [ [ [ 1.0, 3.65 ], [ -4.0, 3.65 ] ] ] } } +] } diff --git a/nav2_route/graphs/sample_graph.geojson b/nav2_route/graphs/sample_graph.geojson index 87a19b468ee..162a1a88884 100644 --- a/nav2_route/graphs/sample_graph.geojson +++ b/nav2_route/graphs/sample_graph.geojson @@ -1,504 +1,121 @@ { - "crs": { - "properties": { - "name": "urn:ogc:def:crs:EPSG::3857" - }, - "type": "name" - }, + "type": "FeatureCollection", + "name": "graph", + "crs": { "type": "name", "properties": { "name": "urn:ogc:def:crs:EPSG::3857" } }, + "date_generated": "Tue Feb 28 07:48:03 PM EST 2025", "features": [ { - "geometry": { - "coordinates": [ - 0.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", + "type": "Feature", + "properties": + { "id": 0, - "metadata": { - "region": { + "frame": "map", + "metadata": + { + "region": + { + "x_values": [1.0, -1.0, -1.0, 1.0], + "y_values": [1.0, 1.0, -1.0, -1.0], "properties": { "class_type": "living_room", "number_of_lights": 10 - }, - "x_values": [ - 1.0, - -1.0, - -1.0, - 1.0 - ], - "y_values": [ - 1.0, - 1.0, - -1.0, - -1.0 - ] + } } }, - "operations": { - "stop": { - "metadata": { - "wait_for": 5.0 - }, + "operations": + { + "stop": + { + "type": "stop", "trigger": "NODE", - "type": "stop" + "metadata": + { + "wait_for": 5.0 + } } } }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 0.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 1.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 0.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 1.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "coordinates": [ - 2.0, - 2.0 - ], - "type": "Point" - }, - "properties": { - "frame": "map", - "id": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 10.0, - "endid": 1, + "geometry": + { + "type": "Point", + "coordinates": [ 0.0, 0.0 ] + } + }, + { "type": "Feature", "properties": { "id": 1, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 2, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 0.0 ] } }, + { "type": "Feature", "properties": { "id": 3, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 4, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 5, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 1.0 ] } }, + { "type": "Feature", "properties": { "id": 6, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 0.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 7, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 1.0, 2.0 ] } }, + { "type": "Feature", "properties": { "id": 8, "frame": "map" }, "geometry": { "type": "Point", "coordinates": [ 2.0, 2.0 ] } }, + { + "type": "Feature", + "properties": + { "id": 9, - "metadata": { - "aisle_number": 14, - "speed_limit": 0.8500000238418579 + "startid": 0, + "endid": 1, + "overridable": false, + "cost": 10.0, + "metadata": + { + "speed_limit": 0.85, + "aisle_number": 14 }, - "operations": { - "open_door": { - "metadata": { + "operations": + { + "open_door": + { + "type": "open_door", + "trigger": "ON_ENTER", + "metadata": + { "door_id": 54, "service_name": "open_door" - }, - "trigger": "ON_ENTER", - "type": "open_door" + } }, - "take_picture": { - "metadata": { - "resolution": [ - 1080, - 720 - ], - "type": "jpg" - }, + "take_picture": + { + "type": "take_picture", "trigger": "ON_EXIT", - "type": "take_picture" + "metadata": + { + "type": "jpg", + "resolution": [1080, 720] + } } - }, - "overridable": false, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 13, - "overridable": true, - "startid": 0 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 10, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 11, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 15, - "overridable": true, - "startid": 1 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 12, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 17, - "overridable": true, - "startid": 2 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 0, - "id": 14, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 19, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 23, - "overridable": true, - "startid": 3 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 1, - "id": 16, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 20, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 21, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 25, - "overridable": true, - "startid": 4 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 2, - "id": 18, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 22, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 27, - "overridable": true, - "startid": 5 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 3, - "id": 24, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 29, - "overridable": true, - "startid": 6 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 4, - "id": 26, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 6, - "id": 30, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 8, - "id": 31, - "overridable": true, - "startid": 7 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 5, - "id": 28, - "overridable": true, - "startid": 8 - }, - "type": "Feature" - }, - { - "geometry": { - "type": "MultiLineString" - }, - "properties": { - "cost": 0.0, - "endid": 7, - "id": 32, - "overridable": true, - "startid": 8 + } }, - "type": "Feature" - } - ], - "name": "graph", - "type": "FeatureCollection" + "geometry": + { + "type": "MultiLineString", + "coordinates": [ [ [ 0.0, 0.0 ], [ 1.0, 0.0 ] ] ] + } + }, + { "type": "Feature", "properties": { "id": 10, "startid": 1, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 11, "startid": 1, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 12, "startid": 2, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 13, "startid": 0, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 0.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 14, "startid": 3, "endid": 0 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 15, "startid": 1, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 0.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 16, "startid": 4, "endid": 1 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 17, "startid": 2, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 0.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 18, "startid": 5, "endid": 2 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 0.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 19, "startid": 3, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 20, "startid": 4, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 21, "startid": 4, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 22, "startid": 5, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 23, "startid": 3, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 1.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 24, "startid": 6, "endid": 3 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 0.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 25, "startid": 4, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 1.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 26, "startid": 7, "endid": 4 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 1.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 27, "startid": 5, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 1.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 28, "startid": 8, "endid": 5 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 2.0, 1.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 29, "startid": 6, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 0.0, 2.0 ], [ 1.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 30, "startid": 7, "endid": 6 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 0.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 31, "startid": 7, "endid": 8 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 1.0, 2.0 ], [ 2.0, 2.0 ] ] ] } }, + { "type": "Feature", "properties": { "id": 32, "startid": 8, "endid": 7 }, "geometry": { "type": "MultiLineString", "coordinates": [ [ [ 2.0, 2.0 ], [ 1.0, 2.0 ] ] ] } } + + ] } From 75848e8ad669a33154473da564cf2b2ef71d9b78 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Tue, 28 Apr 2026 00:28:09 -0700 Subject: [PATCH 28/37] nav2_costmap_2d, nav2_map_server: align with create-on-activate subs PR #5834 redesigned nav2::Subscription to construct the underlying rclcpp::Subscription only at on_activate() and release it at on_deactivate() (no auto-activate-on-init for transient_local). Two production call sites still depended on the old behavior: * StaticLayer::activate() skipped on_activate() for transient_local map subscriptions, relying on them being created already-active. Now they are not, so the topic endpoint never appeared and Costmap2DROS hung in tests like test_collision_checker / inflation_tests / plugin_container_tests waiting for the latched /map message. * MapSaver::saveMapTopicToFile() had the same conditional skip for transient_local map subscriptions, which silently never delivered. Signed-off-by: lotusymt --- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- nav2_map_server/src/map_saver/map_saver.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 60353dbe98b..8702af71ee6 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -108,7 +108,7 @@ StaticLayer::onInitialize() void StaticLayer::activate() { - if (map_sub_ && !map_subscribe_transient_local_) { + if (map_sub_) { map_sub_->on_activate(); } if (map_update_sub_) { diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 96a18ae08f7..db6f0e83fcb 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -197,9 +197,7 @@ bool MapSaver::saveMapTopicToFile( auto map_sub = create_subscription( map_topic_loc, mapCallback, map_qos, callback_group); - if (!map_subscribe_transient_local_) { - map_sub->on_activate(); - } + 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()); From 7e73798a543d9cd7682da78162443c9c68905808 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Tue, 28 Apr 2026 00:28:23 -0700 Subject: [PATCH 29/37] nav2_costmap_2d, nav2_map_server: activate test subs after PR #5834 Tests that built nav2::Subscriptions via test fixtures were depending on the pre-redesign behavior where the underlying rclcpp::Subscription existed at construction. Now subscriptions exist only after on_activate(), which the test fixtures bypassed because they invoked the on_configure()/ on_activate() override callbacks directly rather than using the lifecycle state machine -- so the auto-activate-if-active branch in nav2::LifecycleNode::create_subscription never fired. * test_costmap_filter_info_server, test_vector_object_server: explicit subscription_->on_activate() after construction so the tester actually receives the latched message the server publishes on activation. * test_costmap_subscriber: drive the lifecycle node through configure() / activate() so its managed entities -- including those added internally by CostmapSubscriber via lc_node->create_subscription -- are activated together. Removes the redundant per-sub on_activate calls. * test_costmap_2d_publisher: defer LayerSubscriber's executor thread start until after the underlying subscription has been built in on_activate(); also activate the LayerSubscriber before activating the costmap, so the subscription is discovered before mapUpdateLoop publishes its first latched costmap message (avoids relying on transient_local replay, which is mid-rewrite in rmw_fastrtps_cpp 9.4.7). Signed-off-by: lotusymt --- .../test/integration/test_costmap_2d_publisher.cpp | 11 +++++++++-- .../test/integration/test_costmap_subscriber.cpp | 10 +++++----- .../test/unit/test_costmap_filter_info_server.cpp | 1 + .../test/unit/test_vector_object_server.cpp | 1 + 4 files changed, 16 insertions(+), 7 deletions(-) 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 1a2e2f43327..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,14 +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() @@ -134,8 +138,11 @@ 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()); - costmap_lifecycle_node_->on_activate(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()); } ~CostmapRosTestFixture() override diff --git a/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp b/nav2_costmap_2d/test/integration/test_costmap_subscriber.cpp index 7c6f8ff2c46..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 @@ -53,11 +58,6 @@ class TestCostmapSubscriberShould : public ::testing::Test updateCostmapMsgCount = 0; updateCostmapRawMsgCount = 0; - dummyCostmapMsgSubscriber->on_activate(); - dummyCostmapRawMsgSubscriber->on_activate(); - dummyCostmapUpdateMsgSubscriber->on_activate(); - dummyCostmapRawUpdateMsgSubscriber->on_activate(); - costmapSubscriber = std::make_unique(node, topicName + "_raw"); 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 1e6dbfcfcd2..d45bdcc215b 100644 --- a/nav2_map_server/test/unit/test_vector_object_server.cpp +++ b/nav2_map_server/test/unit/test_vector_object_server.cpp @@ -179,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()); From 990927f4b68eb067b8bd0441dacc30ac73bb7872 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Tue, 28 Apr 2026 00:28:34 -0700 Subject: [PATCH 30/37] nav2_util: break LifecycleServiceClient ctor wait loop on shutdown LifecycleServiceClient's constructor blocks on wait_for_service() in a loop. If the rcl context is invalidated mid-construction (e.g. the process receives SIGINT before the dependent service comes up), both wait_for_service() and Rate::sleep() return immediately, which spins the loop tight and prevents the process from exiting. Surfaced by test_costmap_subscriber_exec: after the gtest binary now exits in ~50ms (post-PR-#5834 fixes), the launch wrapper SIGINTs a lifecycle_manager whose constructor was still in this loop, and the ctest timeout fires while the manager refuses to die. Add an rclcpp::ok() check to the loop condition so context shutdown breaks construction cleanly. Signed-off-by: lotusymt --- nav2_util/include/nav2_util/lifecycle_service_client.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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()); From 2e3f88bb0c8338b2e225c6bc2d6a0ccd7b9fef79 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 2 May 2026 17:02:52 -0700 Subject: [PATCH 31/37] Re-trigger CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Previous algorithm_build hit CircleCI's 60-minute job timeout while still making progress (5/15 packages complete, 4 still building). All other CI signals — core_build, jazzy, kilted, pre-commit, linters, DCO — passed on the same code. Signed-off-by: lotusymt From 7b91f99d83b1b5d8f9185970da3c41468f178821 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 2 May 2026 18:11:30 -0700 Subject: [PATCH 32/37] Re-trigger CI (round 2) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Round 1 (#57229): algorithm_build hit 60-min timeout at 5/15 packages, likely cold caches. Round 2 (#57233): algorithm_build hit 60-min timeout at 13/15 packages (2 still building when killed) — caching from #57229 helped. This round expects to finish: caching from #57233 should push us under the limit. Also retries build-docker (jazzy)/(kilted), which both failed with GitHub-runner→archive.ubuntu.com connection timeouts (pure network infrastructure issue, unrelated to this PR's code). Signed-off-by: lotusymt From 73f7ff0ed0e02d0d9737961bfed4978049a0dbe3 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 2 May 2026 19:25:23 -0700 Subject: [PATCH 33/37] Re-trigger CI (round 3) Signed-off-by: lotusymt From d77b3c75cd430d47cb53bac6b2438c41037c9d13 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sat, 2 May 2026 21:40:34 -0700 Subject: [PATCH 34/37] nav2_ros_common, nav2_costmap_2d, nav2_route: activate subs created during ACTIVATING MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two related bugs surfaced in CI system tests after switching to the create-on-activate / destroy-on-deactivate Subscription wrapper: 1. nav2::LifecycleNode::create_{subscription,publisher} only auto-activated when the node was already in PRIMARY_STATE_ACTIVE. Costmap filters create their mask subscription lazily inside filterInfoCallback, which fires from the parent costmap node's internal executor *during* the on_activate transition (state == TRANSITION_STATE_ACTIVATING). The wrapper was registered as a managed entity but never received on_activate(), so the underlying rclcpp subscription was never created and the filter mask never arrived — failing test_keepout_filter, test_speed_filter_global, test_speed_filter_local. Extend the auto-activate predicate to also fire while the node is ACTIVATING. The wrapper's own on_activate() is idempotent so a later explicit activation by user code remains a no-op. 2. nav2_costmap_2d::CostmapSubscriber owns two nav2::Subscription wrappers that are added as managed entities of its LifecycleNode parent at construction time, but it exposed no way to activate them. RouteServer constructs a CostmapSubscriber in on_configure and then never wired it up — the costmap topic was never subscribed, leaving "No costmap yet received!" in test_route. Add on_activate()/on_deactivate() to CostmapSubscriber that fan out to the wrapped subscriptions, and call them from RouteServer's lifecycle callbacks. Signed-off-by: lotusymt --- .../nav2_costmap_2d/costmap_subscriber.hpp | 23 +++++++++++++++++++ .../nav2_ros_common/lifecycle_node.hpp | 18 ++++++++++----- nav2_route/src/route_server.cpp | 2 ++ 3 files changed, 37 insertions(+), 6 deletions(-) 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 73c422a5dd0..7c02ee408ee 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -85,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_ros_common/include/nav2_ros_common/lifecycle_node.hpp b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp index c91eab2b858..fa372086468 100644 --- a/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp +++ b/nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp @@ -164,9 +164,13 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode shared_from_this(), topic_name, std::forward(callback), qos, callback_group); this->add_managed_entity(sub); - // Automatically activate the subscription 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. + // 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(); } @@ -193,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_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 68f435c2fc2..e4b96dae3db 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -104,6 +104,7 @@ 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();} graph_vis_publisher_->on_activate(); graph_vis_publisher_->publish(utils::toMsg(graph_, route_frame_, this->now())); route_publisher_->on_activate(); @@ -117,6 +118,7 @@ RouteServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); compute_route_server_->deactivate(); compute_and_track_route_server_->deactivate(); + if (costmap_subscriber_) {costmap_subscriber_->on_deactivate();} graph_vis_publisher_->on_deactivate(); route_publisher_->on_deactivate(); destroyBond(); From 36462f45165f529ac1f9c6aca586e56f5f502639 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sun, 3 May 2026 00:03:07 -0700 Subject: [PATCH 35/37] nav2_behaviors, nav2_route: activate plugin-owned and behavior CostmapSubscribers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit After the create-on-activate Subscription wrapper, two more places were silently leaving lifecycle subscriptions unactivated: 1. nav2_behaviors::BehaviorServer creates local_costmap_sub_ and global_costmap_sub_ in on_configure (CostmapSubscribers wrapping nav2::Subscription) but only activated the *footprint* subscriptions in on_activate. Test assisted_teleop_behavior surfaced this as a stream of "Costmap is not available" errors. Activate (and symmetrically deactivate) the costmap subscribers. 2. nav2_route route_server's plugin-owned CostmapSubscribers (the ones CollisionMonitor and CostmapScorer allocate themselves when their costmap topic differs from the server topic) had no path to be activated — RouteServer only owns the shared subscriber. Surfaced as test_route's "Collision Monitor could not obtain a costmap from topic: local_costmap/costmap_raw". Add virtual activate()/deactivate() hooks to RouteOperation and EdgeCostFunction (default no-op), override them in CollisionMonitor and CostmapScorer to fan out to the subscriber they own (tracked via owns_costmap_subscriber_), and forward through OperationsManager, EdgeScorer, RouteTracker, and RoutePlanner so that RouteServer::on_activate/on_deactivate wakes the whole plugin chain. Signed-off-by: lotusymt --- nav2_behaviors/src/behavior_server.cpp | 12 ++++++++++++ nav2_route/include/nav2_route/edge_scorer.hpp | 12 ++++++++++++ .../nav2_route/interfaces/edge_cost_function.hpp | 12 ++++++++++++ .../nav2_route/interfaces/route_operation.hpp | 12 ++++++++++++ .../include/nav2_route/operations_manager.hpp | 12 ++++++++++++ .../edge_cost_functions/costmap_scorer.hpp | 12 ++++++++++++ .../route_operations/collision_monitor.hpp | 14 ++++++++++++++ nav2_route/include/nav2_route/route_planner.hpp | 10 ++++++++++ nav2_route/include/nav2_route/route_tracker.hpp | 10 ++++++++++ nav2_route/src/edge_scorer.cpp | 10 ++++++++++ nav2_route/src/operations_manager.cpp | 14 ++++++++++++++ .../edge_cost_functions/costmap_scorer.cpp | 15 +++++++++++++++ .../route_operations/collision_monitor.cpp | 15 +++++++++++++++ nav2_route/src/route_planner.cpp | 10 ++++++++++ nav2_route/src/route_server.cpp | 4 ++++ nav2_route/src/route_tracker.cpp | 10 ++++++++++ 16 files changed, 184 insertions(+) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 09a9da668c1..3e48e51d6bf 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -176,6 +176,12 @@ BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // 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(); } @@ -201,6 +207,12 @@ 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(); } 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..3da90f9560a 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -62,6 +62,16 @@ 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..319dd24713b 100644 --- a/nav2_route/src/operations_manager.cpp +++ b/nav2_route/src/operations_manager.cpp @@ -70,6 +70,20 @@ 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 e4b96dae3db..0b5aded01a5 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -105,6 +105,8 @@ RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) 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(); @@ -118,6 +120,8 @@ 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(); 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; From 25826b21ee657bb625d60ed8242d450604399d66 Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sun, 3 May 2026 00:05:59 -0700 Subject: [PATCH 36/37] nav2_route: expand for-loop bodies to satisfy uncrustify ament_uncrustify flagged the single-line for-loop bodies introduced in the previous commit. Expand them to a multi-line form. Signed-off-by: lotusymt --- nav2_route/src/edge_scorer.cpp | 8 ++++++-- nav2_route/src/operations_manager.cpp | 24 ++++++++++++++++++------ 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/nav2_route/src/edge_scorer.cpp b/nav2_route/src/edge_scorer.cpp index 3da90f9560a..cb14831b27f 100644 --- a/nav2_route/src/edge_scorer.cpp +++ b/nav2_route/src/edge_scorer.cpp @@ -64,12 +64,16 @@ EdgeScorer::EdgeScorer( void EdgeScorer::activate() { - for (auto & plugin : plugins_) {plugin->activate();} + for (auto & plugin : plugins_) { + plugin->activate(); + } } void EdgeScorer::deactivate() { - for (auto & plugin : plugins_) {plugin->deactivate();} + for (auto & plugin : plugins_) { + plugin->deactivate(); + } } bool EdgeScorer::score( diff --git a/nav2_route/src/operations_manager.cpp b/nav2_route/src/operations_manager.cpp index 319dd24713b..b7b09a11ada 100644 --- a/nav2_route/src/operations_manager.cpp +++ b/nav2_route/src/operations_manager.cpp @@ -72,16 +72,28 @@ 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();} + 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();} + for (auto & op : query_operations_) { + op->deactivate(); + } + for (auto & op : change_operations_) { + op->deactivate(); + } + for (auto & kv : graph_operations_) { + kv.second->deactivate(); + } } template From deedeeef17daf929253e691c999730a18e878d3e Mon Sep 17 00:00:00 2001 From: lotusymt Date: Sun, 3 May 2026 01:46:45 -0700 Subject: [PATCH 37/37] nav2_loopback_sim: activate lifecycle subscriptions and fix unit-test setup The create-on-activate Subscription wrapper left LoopbackSimulator's initial_pose_sub_ and cmd_vel_sub_ dormant: both are created in on_configure (state CONFIGURING) so the framework auto-activate doesn't fire, and on_activate didn't call on_activate() on them explicitly. Symptom: test_loopback_simulator's InitialPoseSetsMapToOdom / CmdVelMovesRobot / OdometryContainsTwist / RotationUpdatesYaw failed because the simulator never received either /initialpose or /cmd_vel. Add the missing on_activate/on_deactivate fan-out. ClockPublisherTest used a raw nav2::LifecycleNode that was never configured/activated, so subscriptions created via node_->create_subscription() now stay inactive and miss every /clock message. Drive the test node to ACTIVE in SetUp() so subscriptions auto-activate as designed. Signed-off-by: lotusymt --- nav2_loopback_sim/src/loopback_simulator.cpp | 15 +++++++++++++++ nav2_loopback_sim/test/test_clock_publisher.cpp | 4 ++++ 2 files changed, 19 insertions(+) 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