BT Service node & implementations

nav2_amcl/src/amcl_node.cpp

@@ -1202,7 +1202,7 @@ AmclNode::initPubSub()
 void
 AmclNode::initServices()
 {
-  global_loc_srv_ = create_service<std_srvs::srv::Empty>("global_localization",
+  global_loc_srv_ = create_service<std_srvs::srv::Empty>("reinitialize_global_localization",
       std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3));
 
   nomotion_update_srv_ = create_service<std_srvs::srv::Empty>("request_nomotion_update",

nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp

@@ -22,7 +22,6 @@
 #include "behaviortree_cpp/blackboard/blackboard_local.h"
 #include "behaviortree_cpp/bt_factory.h"
 #include "behaviortree_cpp/xml_parsing.h"
-#include "nav2_util/global_localization_service_client.hpp"
 
 namespace nav2_behavior_tree
 {
@@ -71,12 +70,7 @@ class BehaviorTreeEngine
 
 protected:
   // Methods used to register as (simple action) BT nodes
-  BT::NodeStatus globalLocalizationServiceRequest();
   BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node);
-  BT::NodeStatus clearEntirelyCostmapServiceRequest(BT::TreeNode & tree_node);
-
-  // Service clients
-  std::unique_ptr<nav2_util::GlobalLocalizationServiceClient> global_localization_client_;
 
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;

nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp

@@ -0,0 +1,122 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// 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_SERVICE_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
+
+#include <string>
+#include <memory>
+
+#include "behaviortree_cpp/action_node.h"
+#include "nav2_util/node_utils.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace nav2_behavior_tree
+{
+
+template<class ServiceT>
+class BtServiceNode : public BT::CoroActionNode
+{
+public:
+  BtServiceNode(
+    const std::string & service_node_name,
+    const BT::NodeParameters & params)
+  : BT::CoroActionNode(service_node_name, params), service_node_name_(service_node_name)
+  {
+  }
+
+  BtServiceNode() = delete;
+
+  virtual ~BtServiceNode()
+  {
+  }
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"service_name", "random_service_name"}};
+    return params;
+  }
+
+  // This is a callback from the BT library invoked after the node
+  // is created and after the blackboard has been set for the node
+  // by the library. It is the first opportunity for the node to
+  // access the blackboard. Derived classes do not override this method,
+  // but override on_init instead.
+  void onInit() final
+  {
+    node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+
+    // Get the required items from the blackboard
+    node_loop_timeout_ =
+      blackboard()->template get<std::chrono::milliseconds>("node_loop_timeout");
+
+    // Now that we have node_ to use, create the service client for this BT service
+    service_client_ = node_->create_client<ServiceT>(service_name_);
+
+    // Make sure the server is actually there before continuing
+    RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" service",
+      service_name_.c_str());
+    service_client_->wait_for_service();
+
+    RCLCPP_INFO(node_->get_logger(), "\"%s\" BtServiceNode initialized",
+      service_node_name_.c_str());
+
+    // Give user a chance for initialization and get the service name
+    on_init();
+  }
+
+  // The main override required by a BT service
+  BT::NodeStatus tick() override
+  {
+    on_tick();
+    auto future_result = service_client_->async_send_request(request_);
+
+    rclcpp::executor::FutureReturnCode rc;
+    rc = rclcpp::spin_until_future_complete(node_,
+        future_result, node_loop_timeout_);
+    if (rc != rclcpp::executor::FutureReturnCode::SUCCESS) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      return BT::NodeStatus::SUCCESS;
+    }
+  }
+
+  // Fill in service request with information if necessary
+  virtual void on_tick()
+  {
+    request_ = std::make_shared<typename ServiceT::Request>();
+  }
+
+  // Perform local initialization such as getting values from the blackboard
+  virtual void on_init()
+  {
+  }
+
+protected:
+  std::string service_name_, service_node_name_;
+  typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
+  std::shared_ptr<typename ServiceT::Request> request_;
+
+  // The node that will be used for any ROS operations
+  rclcpp::Node::SharedPtr node_;
+
+  // The timeout value while to use in the tick loop while waiting for
+  // a result from the server
+  std::chrono::milliseconds node_loop_timeout_;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/clear_costmap_service.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// 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__CLEAR_COSTMAP_SERVICE_HPP_
+#define NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
+
+#include <string>
+#include <memory>
+#include <cmath>
+
+#include "nav2_behavior_tree/bt_service_node.hpp"
+#include "nav2_msgs/srv/clear_entire_costmap.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  explicit ClearEntireCostmapService(
+    const std::string & service_node_name,
+    const BT::NodeParameters & params)
+  : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, params)
+  {
+    getParam<std::string>("service_name", service_name_);
+  }
+
+  void on_init() override
+  {
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/reinitialize_global_localization_service.hpp

@@ -0,0 +1,46 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// 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__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
+#define NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
+
+#include <string>
+#include <memory>
+#include <cmath>
+
+#include "nav2_behavior_tree/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
+{
+public:
+  explicit ReinitializeGlobalLocalizationService(
+    const std::string & service_node_name,
+    const BT::NodeParameters & params)
+  : BtServiceNode<std_srvs::srv::Empty>(service_node_name, params)
+  {
+    service_name_ = "reinitialize_global_localization";
+  }
+
+  void on_init() override
+  {
+  }
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -27,7 +27,8 @@
 #include "nav2_behavior_tree/rate_controller_node.hpp"
 #include "nav2_behavior_tree/recovery_node.hpp"
 #include "nav2_behavior_tree/spin_action.hpp"
-#include "nav2_util/clear_entirely_costmap_service_client.hpp"
+#include "nav2_behavior_tree/clear_costmap_service.hpp"
+#include "nav2_behavior_tree/reinitialize_global_localization_service.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
@@ -42,6 +43,9 @@ BehaviorTreeEngine::BehaviorTreeEngine()
   factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath");
   factory_.registerNodeType<nav2_behavior_tree::BackUpAction>("BackUp");
   factory_.registerNodeType<nav2_behavior_tree::SpinAction>("Spin");
+  factory_.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
+  factory_.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
+    "ReinitializeGlobalLocalization");
 
   // Register our custom condition nodes
   factory_.registerNodeType<nav2_behavior_tree::IsStuckCondition>("IsStuck");
@@ -56,17 +60,6 @@ BehaviorTreeEngine::BehaviorTreeEngine()
 
   // Register our custom control nodes
   factory_.registerNodeType<nav2_behavior_tree::RecoveryNode>("RecoveryNode");
-
-  // Register our simple action nodes
-  factory_.registerSimpleAction("globalLocalizationServiceRequest",
-    std::bind(&BehaviorTreeEngine::globalLocalizationServiceRequest, this));
-
-  factory_.registerSimpleAction("clearEntirelyCostmapServiceRequest",
-    std::bind(&BehaviorTreeEngine::clearEntirelyCostmapServiceRequest, this,
-    std::placeholders::_1));
-
-  global_localization_client_ =
-    std::make_unique<nav2_util::GlobalLocalizationServiceClient>("bt_navigator");
 }
 
 BtStatus
@@ -133,39 +126,11 @@ BehaviorTreeEngine::buildTreeFromText(std::string & xml_string, BT::Blackboard::
   return BT::buildTreeFromText(factory_, xml_string, blackboard);
 }
 
-BT::NodeStatus
-BehaviorTreeEngine::globalLocalizationServiceRequest()
-{
-  auto request = std::make_shared<std_srvs::srv::Empty::Request>();
-  auto response = std::make_shared<std_srvs::srv::Empty::Response>();
-
-  auto succeeded = global_localization_client_->invoke(request, response);
-  return succeeded ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
-}
-
 BT::NodeStatus
 BehaviorTreeEngine::initialPoseReceived(BT::TreeNode & tree_node)
 {
   auto initPoseReceived = tree_node.blackboard()->template get<bool>("initial_pose_received");
   return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus
-BehaviorTreeEngine::clearEntirelyCostmapServiceRequest(
-  BT::TreeNode & tree_node)
-{
-  std::string service_name = "/local_costmap/clear_entirely_local_costmap";
-  tree_node.getParam<std::string>("service_name", service_name);
-
-  nav2_util::ClearEntirelyCostmapServiceClient clear_entirely_costmap(service_name);
-  auto request = std::make_shared<nav2_msgs::srv::ClearEntireCostmap::Request>();
-  try {
-    clear_entirely_costmap.wait_for_service(std::chrono::seconds(3));
-    auto result = clear_entirely_costmap.invoke(request, std::chrono::seconds(3));
-    return BT::NodeStatus::SUCCESS;
-  } catch (std::runtime_error & e) {
-    return BT::NodeStatus::FAILURE;
-  }
-}
-
 }  // namespace nav2_behavior_tree

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -15,8 +15,8 @@
         <FollowPath path="${path}"/>
       </Sequence>
       <SequenceStar name="RecoveryActions">
-        <clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
-        <clearEntirelyCostmapServiceRequest service_name="/global_costmap/clear_entirely_global_costmap"/>
+        <ClearEntireCostmap service_name="/local_costmap/clear_entirely_local_costmap"/>
+        <ClearEntireCostmap service_name="/global_costmap/clear_entirely_global_costmap"/>
         <Spin/>
       </SequenceStar>
     </RecoveryNode>