Controller selector

nav2_behavior_tree/CMakeLists.txt

@@ -129,6 +129,9 @@ list(APPEND plugin_libs nav2_single_trigger_bt_node)
 add_library(nav2_planner_selector_bt_node SHARED plugins/action/planner_selector_node.cpp)
 list(APPEND plugin_libs nav2_planner_selector_bt_node)
 
+add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp)
+list(APPEND plugin_libs nav2_controller_selector_bt_node)
+
 foreach(bt_plugin ${plugin_libs})
   ament_target_dependencies(${bt_plugin} ${dependencies})
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

nav2_behavior_tree/README.md

@@ -85,6 +85,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 | ClearCostmapExceptRegion | Action | Invokes the ClearCostmapExceptRegion ROS2 service server of costmap_2d_ros. It will clear all the costmap apart a square area centered on the robot of side size equal to the `reset_distance` input port. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_except_local_costmap` or `global_costmap/clear_except_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. |
 | ClearCostmapAroundRobot | Action | Invokes the ClearCostmapAroundRobot ROS2 service server of costmap_2d_ros. It will only clear the costmap on a square area centered on the robot of side size equal to the `reset_distance` input port. The service address needs to be specified using the `service_name` input port, typically `local_costmap/clear_around_local_costmap` or `global_costmap/clear_around_global_costmap`. This action is used in nav2 Behavior Trees as a recovery behavior. |
 | PlannerSelector | Action | The PlannerSelector behavior is used to switch the planner that will be used by the planner server. It subscribes to a topic "planner_selector" to get the decision about what planner must be used. It is usually used before of the ComputePathToPoseAction. The selected_planner output port is passed to planner_id input port of the ComputePathToPoseAction.
+| ControllerSelector | Action | The ControllerSelector behavior is used to switch the controller that will be used by the controller server. It subscribes to a topic "controller_selector" to get the decision about what controller must be used. It is usually used before of the ComputePathToPoseAction. The selected_controller output port is passed to controller_id input port of the ComputePathToPoseAction.
 
 
 For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/bt_basics/

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_selector_node.hpp

@@ -0,0 +1,97 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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__CONTROLLER_SELECTOR_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief The ControllerSelector behavior is used to switch the controller
+ * that will be used by the controller server. It subscribes to a topic "controller_selector"
+ * to get the decision about what controller must be used. It is usually used before of
+ * the FollowPath. The selected_controller output port is passed to controller_id
+ * input port of the FollowPath
+ */
+class ControllerSelector : public BT::SyncActionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::ControllerSelector
+   *
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param conf  BT node configuration
+   */
+  ControllerSelector(
+    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_controller",
+        "the default controller to use if there is not any external topic message received."),
+
+      BT::InputPort<std::string>(
+        "topic_name",
+        "controller_selector",
+        "the input topic name to select the controller"),
+
+      BT::OutputPort<std::string>(
+        "selected_controller",
+        "Selected controller by subscription")
+    };
+  }
+
+private:
+  /**
+   * @brief Function to perform some user-defined operation on tick
+   */
+  BT::NodeStatus tick();
+
+  /**
+   * @brief callback function for the controller_selector topic
+   *
+   * @param msg the message with the id of the controller_selector
+   */
+  void callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr controller_selector_sub_;
+
+  std::string last_selected_controller_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  std::string topic_name_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -38,6 +38,12 @@
         <output_port name="selected_planner">Name of the selected planner received from the topic subcription</output_port>
     </Action>
 
+    <Action ID="ControllerSelector">
+        <input_port name="topic_name">Name of the topic to receive controller selection commands</input_port>
+        <input_port name="default_controller">Default controller of the controller selector</input_port>
+        <output_port name="selected_controller">Name of the selected controller received from the topic subcription</output_port>
+    </Action>
+
     <Action ID="RandomCrawl"/>
 
     <Action ID="Spin">

nav2_behavior_tree/plugins/action/controller_selector_node.cpp

@@ -0,0 +1,68 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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/controller_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+ControllerSelector::ControllerSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+  getInput("topic_name", topic_name_);
+
+  controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_, 1, std::bind(&ControllerSelector::callbackControllerSelect, this, _1));
+}
+
+BT::NodeStatus ControllerSelector::tick()
+{
+  rclcpp::spin_some(node_);
+
+  if (last_selected_controller_.empty()) {
+    getInput("default_controller", last_selected_controller_);
+  }
+
+  setOutput("selected_controller", last_selected_controller_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+ControllerSelector::callbackControllerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_controller_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::ControllerSelector>("ControllerSelector");
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -37,3 +37,7 @@ ament_target_dependencies(test_truncate_path_action ${dependencies})
 ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp)
 target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node)
 ament_target_dependencies(test_planner_selector_node ${dependencies})
+
+ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp)
+target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node)
+ament_target_dependencies(test_controller_selector_node ${dependencies})

nav2_behavior_tree/test/plugins/action/test_controller_selector_node.cpp

@@ -0,0 +1,181 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Pablo Iñigo Blasco
+//
+// 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 <gtest/gtest.h>
+
+#include <memory>
+#include <set>
+#include <string>
+
+#include "../../test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class ControllerSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("controller_selector_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::ControllerSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ControllerSelector>(
+      "ControllerSelector",
+      builder);
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    factory_.reset();
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ControllerSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ControllerSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ControllerSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(ControllerSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ControllerSelector selected_controller="{selected_controller}" default_controller="DWB" topic_name="controller_selector_custom_topic_name"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_controller_result;
+  config_->blackboard->get("selected_controller", selected_controller_result);
+
+  EXPECT_EQ(selected_controller_result, "DWB");
+
+  std_msgs::msg::String selected_controller_cmd;
+
+  selected_controller_cmd.data = "DWC";
+
+  auto controller_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("controller_selector_custom_topic_name", 10);
+
+  // publish a few updates of the selected_controller
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    controller_selector_pub->publish(selected_controller_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check controller updated
+  config_->blackboard->get("selected_controller", selected_controller_result);
+  EXPECT_EQ("DWC", selected_controller_result);
+}
+
+TEST_F(ControllerSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ControllerSelector selected_controller="{selected_controller}" default_controller="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // check default value
+  std::string selected_controller_result;
+  config_->blackboard->get("selected_controller", selected_controller_result);
+
+  EXPECT_EQ(selected_controller_result, "GridBased");
+
+  std_msgs::msg::String selected_controller_cmd;
+
+  selected_controller_cmd.data = "RRT";
+
+  auto controller_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("controller_selector", 10);
+
+  // publish a few updates of the selected_controller
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    controller_selector_pub->publish(selected_controller_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check controller updated
+  config_->blackboard->get("selected_controller", selected_controller_result);
+  EXPECT_EQ("RRT", selected_controller_result);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+
+  return all_successful;
+}

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -78,6 +78,7 @@ bt_navigator:
     - nav2_distance_traveled_condition_bt_node
     - nav2_single_trigger_bt_node
     - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
 
 bt_navigator_rclcpp_node:
   ros__parameters: