adding an abstract BTNode for action cancellation

nav2_behavior_tree/CMakeLists.txt

@@ -57,6 +57,9 @@ list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node)
 add_library(nav2_compute_path_through_poses_action_bt_node SHARED plugins/action/compute_path_through_poses_action.cpp)
 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_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/bt_cancel_action_node.hpp

@@ -0,0 +1,166 @@
+// 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__BT_CANCEL_ACTION_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <chrono>
+
+#include "behaviortree_cpp_v3/action_node.h"
+#include "nav2_util/node_utils.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav2_behavior_tree/bt_conversions.hpp"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief Abstract class representing an action for cancelling BT node
+ * @tparam ActionT Type of action
+ */
+template<class ActionT>
+class BtCancelActionNode : public BT::ActionNodeBase
+{
+public:
+  /**
+   * @brief A nav2_behavior_tree::BtCancelActionNode constructor
+   * @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
+   */
+  BtCancelActionNode(
+    const std::string & xml_tag_name,
+    const std::string & action_name,
+    const BT::NodeConfiguration & conf)
+  : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
+  {
+    node_ = config().blackboard->template 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());
+
+    // Get the required items from the blackboard
+    server_timeout_ =
+      config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+    getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
+
+    std::string remapped_action_name;
+    if (getInput("server_name", remapped_action_name)) {
+      action_name_ = remapped_action_name;
+    }
+    createActionClient(action_name_);
+
+    // Give the derive class a chance to do any initialization
+    RCLCPP_DEBUG(
+      node_->get_logger(), "\"%s\" BtCancelActionNode initialized",
+      xml_tag_name.c_str());
+  }
+
+  BtCancelActionNode() = delete;
+
+  virtual ~BtCancelActionNode()
+  {
+  }
+
+  /**
+   * @brief Create instance of an action client
+   * @param action_name Action name to create client for
+   */
+  void createActionClient(const std::string & action_name)
+  {
+    // Now that we have the ROS node to use, create the action client for this BT action
+    action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name, callback_group_);
+
+    // Make sure the server is actually there before continuing
+    RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
+    action_client_->wait_for_action_server();
+  }
+
+  /**
+   * @brief Any subclass of BtCancelActionNode that accepts parameters must provide a
+   * providedPorts method and call providedBasicPorts in it.
+   * @param addition Additional ports to add to BT port list
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedBasicPorts(BT::PortsList addition)
+  {
+    BT::PortsList basic = {
+      BT::InputPort<std::string>("server_name", "Action server name"),
+      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+    };
+    basic.insert(addition.begin(), addition.end());
+
+    return basic;
+  }
+
+  void halt()
+  {
+  }
+
+  /**
+   * @brief Creates list of BT ports
+   * @return BT::PortsList Containing basic ports along with node-specific ports
+   */
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({});
+  }
+
+  /**
+   * @brief The main override required by a BT action
+   * @return BT::NodeStatus Status of tick execution
+   */
+  BT::NodeStatus tick() override
+  {
+    // setting the status to RUNNING to notify the BT Loggers (if any)
+    setStatus(BT::NodeStatus::RUNNING);
+
+    // Cancel all the goals specified before 10ms from current time
+    // to avoid async communication error
+
+    rclcpp::Time goal_expiry_time = node_->now() - std::chrono::milliseconds(10);
+
+    auto future_cancel = action_client_->async_cancel_goals_before(goal_expiry_time);
+
+    if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
+      rclcpp::FutureReturnCode::SUCCESS)
+    {
+      RCLCPP_ERROR(
+        node_->get_logger(),
+        "Failed to cancel the action server for %s", action_name_.c_str());
+      return BT::NodeStatus::FAILURE;
+    }
+    return BT::NodeStatus::SUCCESS;
+  }
+
+protected:
+  std::string action_name_;
+  typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
+
+  // The node that will be used for any ROS operations
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+
+  // The timeout value while waiting for response from a server when a
+  // new action goal is canceled
+  std::chrono::milliseconds server_timeout_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/controller_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__CONTROLLER_CANCEL_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "nav2_msgs/action/follow_path.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::FollowPath
+ */
+class ControllerCancel : public BtCancelActionNode<nav2_msgs::action::FollowPath>
+{
+public:
+  /**
+   * @brief A constructor for nav2_behavior_tree::FollowPathAction
+   * @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
+   */
+  ControllerCancel(
+    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__CONTROLLER_CANCEL_NODE_HPP_

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -13,6 +13,10 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
     </Action>
 
+    <Action ID="CancelControl">
+      <input_port name="service_name">Service name to cancel the controller server</input_port>
+    </Action>
+
     <Action ID="ClearEntireCostmap">
       <input_port name="service_name">Service name</input_port>
     </Action>

nav2_behavior_tree/plugins/action/controller_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/controller_cancel_node.hpp"
+
+namespace nav2_behavior_tree
+{
+
+ControllerCancel::ControllerCancel(
+  const std::string & xml_tag_name,
+  const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: BtCancelActionNode<nav2_msgs::action::FollowPath>(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::ControllerCancel>(
+        name, "follow_path", config);
+    };
+
+  factory.registerBuilder<nav2_behavior_tree::ControllerCancel>(
+    "CancelControl", builder);
+}

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -13,6 +13,10 @@ ament_add_gtest(test_action_wait_action test_wait_action.cpp)
 target_link_libraries(test_action_wait_action nav2_wait_action_bt_node)
 ament_target_dependencies(test_action_wait_action ${dependencies})
 
+ament_add_gtest(test_action_controller_cancel_action test_controller_cancel_node.cpp)
+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_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})