Implemented smoother selector bt node (ros-navigation#3283)

* Implemented smoother selector bt node

Signed-off-by: Owen Hooper <[email protected]>

* updated copyright in modified file

Signed-off-by: Owen Hooper <[email protected]>

Signed-off-by: Owen Hooper <[email protected]>

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)
 

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 <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 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<std::string>(
+        "default_smoother",
+        "the default smoother to use if there is not any external topic message received."),
+
+      BT::InputPort<std::string>(
+        "topic_name",
+        "smoother_selector",
+        "the input topic name to select the smoother"),
+
+      BT::OutputPort<std::string>(
+        "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<std_msgs::msg::String>::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_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -166,6 +166,12 @@
         <output_port name="selected_controller">Name of the selected controller received from the topic subcription</output_port>
     </Action>
 
+    <Action ID="SmootherSelector">
+        <input_port name="topic_name">Name of the topic to receive smoother selection commands</input_port>
+        <input_port name="default_smoother">Default smoother of the smoother selector</input_port>
+        <output_port name="selected_smoother">Name of the selected smoother received from the topic subcription</output_port>
+    </Action>
+
     <Action ID="GoalCheckerSelector">
         <input_port name="topic_name">Name of the topic to receive goal checker selection commands</input_port>
         <input_port name="default_goal_checker">Default goal checker of the controller selector</input_port>

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 <string>
+#include <memory>
+
+#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<rclcpp::Node::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());
+
+  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<std_msgs::msg::String>(
+    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<nav2_behavior_tree::SmootherSelector>("SmootherSelector");
+}

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})