adding progress checker selector BT node (ros-navigation#4109)

Signed-off-by: enricosutera <[email protected]>

nav2_behavior_tree/CMakeLists.txt

@@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node)
 add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp)
 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_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/progress_checker_selector_node.hpp

@@ -0,0 +1,96 @@
+// Copyright (c) 2024 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.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_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 ProgressCheckerSelector behavior is used to switch the progress checker
+ * of the controller server. It subscribes to a topic "progress_checker_selector"
+ * to get the decision about what progress_checker must be used. It is usually used before of
+ * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id
+ * input port of the FollowPath
+ */
+class ProgressCheckerSelector : public BT::SyncActionNode
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector
+   *
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param conf  BT node configuration
+   */
+  ProgressCheckerSelector(
+    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_progress_checker",
+        "the default progress_checker to use if there is not any external topic message received."),
+
+      BT::InputPort<std::string>(
+        "topic_name",
+        "progress_checker_selector",
+        "the input topic name to select the progress_checker"),
+
+      BT::OutputPort<std::string>(
+        "selected_progress_checker",
+        "Selected progress_checker by subscription")
+    };
+  }
+
+private:
+  /**
+   * @brief Function to perform some user-defined operation on tick
+   */
+  BT::NodeStatus tick() override;
+
+  /**
+   * @brief callback function for the progress_checker_selector topic
+   *
+   * @param msg the message with the id of the progress_checker_selector
+   */
+  void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr progress_checker_selector_sub_;
+
+  std::string last_selected_progress_checker_;
+
+  rclcpp::Node::SharedPtr node_;
+
+  std::string topic_name_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -182,6 +182,12 @@
         <output_port name="selected_goal_checker">Name of the selected goal checker received from the topic subcription</output_port>
     </Action>
 
+    <Action ID="ProgressCheckerSelector">
+        <input_port name="topic_name">Name of the topic to receive progress checker selection commands</input_port>
+        <input_port name="default_progress_checker">Default progress checker of the controller selector</input_port>
+        <output_port name="selected_progress_checker">Name of the selected progress checker received from the topic subcription</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/progress_checker_selector_node.cpp

@@ -0,0 +1,81 @@
+// Copyright (c) 2024 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 <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+using std::placeholders::_1;
+
+ProgressCheckerSelector::ProgressCheckerSelector(
+  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_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  progress_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1));
+}
+
+BT::NodeStatus ProgressCheckerSelector::tick()
+{
+  rclcpp::spin_some(node_);
+
+  // This behavior always use the last selected progress checker received from the topic input.
+  // When no input is specified it uses the default goaprogressl checker.
+  // If the default progress checker is not specified then we work in
+  // "required progress checker mode": In this mode, the behavior returns failure if the progress
+  // checker selection is not received from the topic input.
+  if (last_selected_progress_checker_.empty()) {
+    std::string default_progress_checker;
+    getInput("default_progress_checker", default_progress_checker);
+    if (default_progress_checker.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_progress_checker_ = default_progress_checker;
+    }
+  }
+
+  setOutput("selected_progress_checker", last_selected_progress_checker_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_progress_checker_ = msg->data;
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<nav2_behavior_tree::ProgressCheckerSelector>("ProgressCheckerSelector");
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -107,3 +107,7 @@ ament_target_dependencies(test_smoother_selector_node ${dependencies})
 ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp)
 target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node)
 ament_target_dependencies(test_goal_checker_selector_node ${dependencies})
+
+ament_add_gtest(test_progress_checker_selector_node test_progress_checker_selector_node.cpp)
+target_link_libraries(test_progress_checker_selector_node nav2_progress_checker_selector_bt_node)
+ament_target_dependencies(test_progress_checker_selector_node ${dependencies})

nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp

@@ -0,0 +1,187 @@
+// Copyright (c) 2024 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 <gtest/gtest.h>
+
+#include <memory>
+#include <set>
+#include <string>
+
+#include "utils/test_action_server.hpp"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "std_msgs/msg/string.hpp"
+
+class ProgressCheckerSelectorTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("progress_checker_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("node", node_);
+
+    BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+        return std::make_unique<nav2_behavior_tree::ProgressCheckerSelector>(name, config);
+      };
+
+    factory_->registerBuilder<nav2_behavior_tree::ProgressCheckerSelector>(
+      "ProgressCheckerSelector",
+      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 ProgressCheckerSelectorTestFixture::node_ = nullptr;
+
+BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ProgressCheckerSelectorTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ProgressCheckerSelectorTestFixture::tree_ = nullptr;
+
+TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="SimpleProgressCheck" topic_name="progress_checker_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_progress_checker_result;
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+
+  EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck");
+
+  std_msgs::msg::String selected_progress_checker_cmd;
+
+  selected_progress_checker_cmd.data = "AngularProgressChecker";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto progress_checker_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>(
+    "progress_checker_selector_custom_topic_name", qos);
+
+  // publish a few updates of the selected_progress_checker
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    progress_checker_selector_pub->publish(selected_progress_checker_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check progress_checker updated
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+  EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result);
+}
+
+TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+          <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="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_progress_checker_result;
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+
+  EXPECT_EQ(selected_progress_checker_result, "GridBased");
+
+  std_msgs::msg::String selected_progress_checker_cmd;
+
+  selected_progress_checker_cmd.data = "RRT";
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  auto progress_checker_selector_pub =
+    node_->create_publisher<std_msgs::msg::String>("progress_checker_selector", qos);
+
+  // publish a few updates of the selected_progress_checker
+  auto start = node_->now();
+  while ((node_->now() - start).seconds() < 0.5) {
+    tree_->rootNode()->executeTick();
+    progress_checker_selector_pub->publish(selected_progress_checker_cmd);
+
+    rclcpp::spin_some(node_);
+  }
+
+  // check goal_checker updated
+  config_->blackboard->get("selected_progress_checker", selected_progress_checker_result);
+  EXPECT_EQ("RRT", selected_progress_checker_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;
+}