Centralize path handler logic in controller server

nav2_behavior_tree/CMakeLists.txt

@@ -246,6 +246,9 @@ list(APPEND plugin_libs nav2_goal_checker_selector_bt_node)
 add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp)
 list(APPEND plugin_libs nav2_progress_checker_selector_bt_node)
 
+add_library(nav2_path_handler_selector_bt_node SHARED plugins/action/path_handler_selector_node.cpp)
+list(APPEND plugin_libs nav2_path_handler_selector_bt_node)
+
 add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp)
 list(APPEND plugin_libs nav2_goal_updated_controller_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp

@@ -96,6 +96,7 @@ class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
         BT::InputPort<std::string>("controller_id", ""),
         BT::InputPort<std::string>("goal_checker_id", ""),
         BT::InputPort<std::string>("progress_checker_id", ""),
+        BT::InputPort<std::string>("path_handler_id", ""),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The follow path error code"),
         BT::OutputPort<std::string>(

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp

@@ -0,0 +1,110 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// 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_BEHAVIOR_TREE__PLUGINS__ACTION__PATH_HANDLER_SELECTOR_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PATH_HANDLER_SELECTOR_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <chrono>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "behaviortree_cpp/action_node.h"
+
+#include "nav2_ros_common/lifecycle_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief The PathHandlerSelector behavior is used to switch the path handler
+ * of the controller server. It subscribes to a topic "path_handler_selector"
+ * to get the decision about what path_handler must be used. It is usually used before
+ * the FollowPath. The selected_path_handler output port is passed to path_handler_id
+ * input port of the FollowPath
+ * @note This is an Asynchronous node. It will re-initialize when halted.
+ */
+class PathHandlerSelector : public BT::SyncActionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::PathHandlerSelector
+   *
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param conf  BT node configuration
+   */
+  PathHandlerSelector(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>(
+        "default_path_handler",
+        "the default path_handler to use if there is not any external topic message received."),
+
+      BT::InputPort<std::string>(
+        "topic_name",
+        "path_handler_selector",
+        "the input topic name to select the path_handler"),
+
+      BT::OutputPort<std::string>(
+        "selected_path_handler",
+        "Selected path_handler by subscription")
+    };
+  }
+
+private:
+  /**
+   * @brief Function to read parameters and initialize class variables
+   */
+  void initialize();
+  /**
+   * @brief Function to create ROS interfaces
+   */
+  void createROSInterfaces();
+
+  /**
+   * @brief Function to perform some user-defined operation on tick
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief callback function for the path_handler_selector topic
+   *
+   * @param msg the message with the id of the path_handler_selector
+   */
+  void callbackPathHandlerSelect(const std_msgs::msg::String::SharedPtr msg);
+
+  nav2::Subscription<std_msgs::msg::String>::SharedPtr path_handler_selector_sub_;
+
+  std::string last_selected_path_handler_;
+
+  nav2::LifecycleNode::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+
+  std::string topic_name_;
+  std::chrono::milliseconds bt_loop_duration_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PATH_HANDLER_SELECTOR_NODE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -180,6 +180,7 @@
       <input_port name="path">Path to follow</input_port>
       <input_port name="goal_checker_id">Goal checker</input_port>
       <input_port name="progress_checker_id">Progress checker</input_port>
+      <input_port name="path_handler_id">Path handler</input_port>
       <input_port name="server_name">Action server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
       <output_port name="error_code_id">Follow Path error code</output_port>
@@ -257,6 +258,12 @@
       <output_port name="selected_progress_checker">Name of the selected progress checker received from the topic subscription</output_port>
     </Action>
 
+    <Action ID="PathHandlerSelector">
+      <input_port name="topic_name">Name of the topic to receive path handler selection commands</input_port>
+      <input_port name="default_path_handler">Default path handler of the controller selector</input_port>
+      <output_port name="selected_path_handler">Name of the selected path handler received from the topic subscription</output_port>
+    </Action>
+
     <Action ID="Spin">
       <input_port name="spin_dist">Spin distance</input_port>
       <input_port name="time_allowance">Allowed time for spinning</input_port>

nav2_behavior_tree/plugins/action/follow_path_action.cpp

@@ -34,6 +34,7 @@ void FollowPathAction::on_tick()
   getInput("controller_id", goal_.controller_id);
   getInput("goal_checker_id", goal_.goal_checker_id);
   getInput("progress_checker_id", goal_.progress_checker_id);
+  getInput("path_handler_id", goal_.path_handler_id);
 }
 
 BT::NodeStatus FollowPathAction::on_success()
@@ -101,6 +102,14 @@ void FollowPathAction::on_wait_for_result(
     goal_.progress_checker_id = new_progress_checker_id;
     goal_updated_ = true;
   }
+
+  std::string new_path_handler_id;
+  getInput("path_handler_id", new_path_handler_id);
+
+  if (goal_.path_handler_id != new_path_handler_id) {
+    goal_.path_handler_id = new_path_handler_id;
+    goal_updated_ = true;
+  }
 }
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/path_handler_selector_node.cpp

@@ -0,0 +1,104 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/path_handler_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+PathHandlerSelector::PathHandlerSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  initialize();
+  bt_loop_duration_ =
+    config().blackboard->template get<std::chrono::milliseconds>("bt_loop_duration");
+}
+
+void PathHandlerSelector::initialize()
+{
+  createROSInterfaces();
+}
+
+void PathHandlerSelector::createROSInterfaces()
+{
+  std::string topic_new;
+  getInput("topic_name", topic_new);
+  if (topic_new != topic_name_ || !path_handler_selector_sub_) {
+    topic_name_ = topic_new;
+    node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+    path_handler_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+      topic_name_,
+      std::bind(&PathHandlerSelector::callbackPathHandlerSelect, this, _1),
+      nav2::qos::LatchedSubscriptionQoS(),
+      callback_group_);
+  }
+}
+
+BT::NodeStatus PathHandlerSelector::tick()
+{
+  if (!BT::isStatusActive(status())) {
+    initialize();
+  }
+
+  callback_group_executor_.spin_all(bt_loop_duration_);
+
+  // This behavior always use the last selected path_handler received from the topic input.
+  // When no input is specified it uses the default path handler.
+  // If the default path handler is not specified then we work in "required path handler mode":
+  // In this mode, the behavior returns failure if the path handler selection is not received from
+  // the topic input.
+  if (last_selected_path_handler_.empty()) {
+    std::string default_path_handler;
+    getInput("default_path_handler", default_path_handler);
+    if (default_path_handler.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_path_handler_ = default_path_handler;
+    }
+  }
+
+  setOutput("selected_path_handler", last_selected_path_handler_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+PathHandlerSelector::callbackPathHandlerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_path_handler_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::PathHandlerSelector>("PathHandlerSelector");
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -91,6 +91,8 @@ plugin_add_test(test_goal_checker_selector_node test_goal_checker_selector_node.
 
 plugin_add_test(test_progress_checker_selector_node test_progress_checker_selector_node.cpp nav2_progress_checker_selector_bt_node)
 
+plugin_add_test(test_path_handler_selector_node test_path_handler_selector_node.cpp nav2_path_handler_selector_bt_node)
+
 plugin_add_test(test_toggle_collision_monitor_service
   test_toggle_collision_monitor_service.cpp
   nav2_toggle_collision_monitor_service_bt_node)