diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 224e8498e1..1767bdcf32 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -183,6 +183,9 @@ 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) +add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp) +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) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp new file mode 100644 index 0000000000..f9bb293d26 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -0,0 +1,99 @@ +// 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__SMOOTHER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The SmootherSelector behavior is used to switch the smoother + * that will be used by the smoother server. It subscribes to a topic "smoother_selector" + * to get the decision about what smoother must be used. It is usually used before of + * the FollowPath. The selected_smoother output port is passed to smoother_id + * input port of the FollowPath + */ +class SmootherSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::SmootherSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + SmootherSelector( + 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( + "default_smoother", + "the default smoother to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "smoother_selector", + "the input topic name to select the smoother"), + + BT::OutputPort( + "selected_smoother", + "Selected smoother by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the smoother_selector topic + * + * @param msg the message with the id of the smoother_selector + */ + void callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr smoother_selector_sub_; + + std::string last_selected_smoother_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 62a5f1f482..67b97662af 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -162,6 +162,12 @@ Name of the selected controller received from the topic subcription + + Name of the topic to receive smoother selection commands + Default smoother of the smoother selector + Name of the selected smoother received from the topic subcription + + Name of the topic to receive goal checker selection commands Default goal checker of the controller selector diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp new file mode 100644 index 0000000000..0a84062e08 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -0,0 +1,92 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// Copyright (c) 2022 Owen Hooper +// +// 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 "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +SmootherSelector::SmootherSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + smoother_selector_sub_ = node_->create_subscription( + topic_name_, + qos, + std::bind(&SmootherSelector::callbackSmootherSelect, this, _1), + sub_option); +} + +BT::NodeStatus SmootherSelector::tick() +{ + callback_group_executor_.spin_some(); + + // This behavior always use the last selected smoother received from the topic input. + // When no input is specified it uses the default smoother. + // If the default smoother is not specified then we work in "required smoother mode": + // In this mode, the behavior returns failure if the smoother selection is not received from + // the topic input. + if (last_selected_smoother_.empty()) { + std::string default_smoother; + getInput("default_smoother", default_smoother); + if (default_smoother.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_smoother_ = default_smoother; + } + } + + setOutput("selected_smoother", last_selected_smoother_); + + return BT::NodeStatus::SUCCESS; +} + +void +SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_smoother_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SmootherSelector"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 0b7cae1a6c..c640e82424 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -100,6 +100,10 @@ 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}) +ament_add_gtest(test_smoother_selector_node test_smoother_selector_node.cpp) +target_link_libraries(test_smoother_selector_node nav2_smoother_selector_bt_node) +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}) diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp new file mode 100644 index 0000000000..c59cac1b2c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -0,0 +1,187 @@ +// 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 + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class SmootherSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("smoother_selector_test_fixture"); + factory_ = std::make_shared(); + + 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(name, config); + }; + + factory_->registerBuilder( + "SmootherSelector", + 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 factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr SmootherSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::factory_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::tree_ = nullptr; + +TEST_F(SmootherSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(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_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "DWB"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "DWC"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector_custom_topic_name", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("DWC", selected_smoother_result); +} + +TEST_F(SmootherSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(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_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "GridBased"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("RRT", selected_smoother_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; +}