adding Bt to cancel wait, spin and backup (#2856)

* adding Bt to cancel wait

* Adding cancel for spining recovery

* adding BT for cancelling the backup behavior

* adding cancel recoveries to param files for wider test coverage

* updating BT's XML

* updating XML nodes

nav2_behavior_tree/CMakeLists.txt

@@ -60,6 +60,15 @@ list(APPEND plugin_libs nav2_compute_path_through_poses_action_bt_node)
 add_library(nav2_controller_cancel_bt_node SHARED plugins/action/controller_cancel_node.cpp)
 list(APPEND plugin_libs nav2_controller_cancel_bt_node)
 
+add_library(nav2_wait_cancel_bt_node SHARED plugins/action/wait_cancel_node.cpp)
+list(APPEND plugin_libs nav2_wait_cancel_bt_node)
+
+add_library(nav2_spin_cancel_bt_node SHARED plugins/action/spin_cancel_node.cpp)
+list(APPEND plugin_libs nav2_spin_cancel_bt_node)
+
+add_library(nav2_back_up_cancel_bt_node SHARED plugins/action/back_up_cancel_node.cpp)
+list(APPEND plugin_libs nav2_back_up_cancel_bt_node)
+
 add_library(nav2_smooth_path_action_bt_node SHARED plugins/action/smooth_path_action.cpp)
 list(APPEND plugin_libs nav2_smooth_path_action_bt_node)
 

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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__BACK_UP_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_msgs/action/back_up.hpp"
+
+#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
+ */
+class BackUpCancel : public BtCancelActionNode<nav2_msgs::action::BackUp>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::BackUpAction
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  BackUpCancel(
+    const std::string & xml_tag_name,
+    const std::string & action_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 providedBasicPorts(
+      {
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_cancel_node.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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__SPIN_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_msgs/action/spin.hpp"
+
+#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
+ */
+class SpinCancel : public BtCancelActionNode<nav2_msgs::action::Spin>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::WaitAction
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  SpinCancel(
+    const std::string & xml_tag_name,
+    const std::string & action_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 providedBasicPorts(
+      {
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/wait_cancel_node.hpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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__WAIT_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_msgs/action/wait.hpp"
+
+#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
+ */
+class WaitCancel : public BtCancelActionNode<nav2_msgs::action::Wait>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::WaitAction
+   * @param xml_tag_name Name for the XML tag for this node
+   * @param action_name Action name this node creates a client for
+   * @param conf BT node configuration
+   */
+  WaitCancel(
+    const std::string & xml_tag_name,
+    const std::string & action_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 providedBasicPorts(
+      {
+      });
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -15,8 +15,24 @@
 
     <Action ID="CancelControl">
       <input_port name="service_name">Service name to cancel the controller server</input_port>
+      <input_port name="server_timeout">Server timeout</input_port>
     </Action>
-
+
+    <Action ID="CancelBackUp">
+      <input_port name="service_name">Service name to cancel the backup recovery</input_port>
+      <input_port name="server_timeout">Server timeout</input_port>
+    </Action>
+
+    <Action ID="CancelSpin">
+      <input_port name="service_name">Service name to cancel the spin recovery</input_port>
+      <input_port name="server_timeout">Server timeout</input_port>
+    </Action>
+
+    <Action ID="CancelWait">
+      <input_port name="service_name">Service name to cancel the wait recovery</input_port>
+      <input_port name="server_timeout">Server timeout</input_port>
+    </Action>
+
     <Action ID="ClearEntireCostmap">
       <input_port name="service_name">Service name</input_port>
     </Action>

nav2_behavior_tree/plugins/action/back_up_cancel_node.cpp

@@ -0,0 +1,47 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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/back_up_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+BackUpCancel::BackUpCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::BackUp>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::BackUpCancel>(
+        name, "backup", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::BackUpCancel>(
+    "CancelBackUp", builder);
+}

nav2_behavior_tree/plugins/action/spin_cancel_node.cpp

@@ -0,0 +1,47 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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/spin_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+SpinCancel::SpinCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::Spin>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::SpinCancel>(
+        name, "spin", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::SpinCancel>(
+    "CancelSpin", builder);
+}

nav2_behavior_tree/plugins/action/wait_cancel_node.cpp

@@ -0,0 +1,47 @@
+// Copyright (c) 2022 Neobotix GmbH
+//
+// 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/wait_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+WaitCancel::WaitCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
+{
+}
+
+}  // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder =
+    [](const std::string & name, const BT::NodeConfiguration & config)
+    {
+      return std::make_unique<nav2_behavior_tree::WaitCancel>(
+        name, "wait", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::WaitCancel>(
+    "CancelWait", builder);
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -19,6 +19,18 @@ ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node
 target_link_libraries(test_action_controller_cancel_action nav2_controller_cancel_bt_node)
 ament_target_dependencies(test_action_controller_cancel_action ${dependencies})
 
+ament_add_gtest(test_action_wait_cancel_action test_wait_cancel_node.cpp)
+target_link_libraries(test_action_wait_cancel_action nav2_wait_cancel_bt_node)
+ament_target_dependencies(test_action_wait_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_spin_cancel_action test_spin_cancel_node.cpp)
+target_link_libraries(test_action_spin_cancel_action nav2_spin_cancel_bt_node)
+ament_target_dependencies(test_action_spin_cancel_action ${dependencies})
+
+ament_add_gtest(test_action_back_up_cancel_action test_back_up_cancel_node.cpp)
+target_link_libraries(test_action_back_up_cancel_action nav2_back_up_cancel_bt_node)
+ament_target_dependencies(test_action_back_up_cancel_action ${dependencies})
+
 ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp)
 target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node)
 ament_target_dependencies(test_action_clear_costmap_service ${dependencies})