Parallel planning and control (#362)

* Initial work on running planning and control in parallel
* Checkpoint a working version with lots of debugging traces
* Remove debugging traces
* Remove the temporary hooks
* Address lint issues
* Remove some debug output
* Fix a compile problem after taking out some debug code
* Read the BT XML from an external file
* Re-enable DWB code
* Address reviewer feedback

nav2_bt_navigator/behavior_trees/parallel.xml

+<!--
+  This Behavior Tree first computes a path using the global planner (ComputePathToPose).
+  Then, it runs two sub-branches in parallel. The first sub-branch is a FollowPath
+  operation (the local planner). In parallel, there is a rate controlled execution of
+  FollowPath (the global planner). Each time a new path is computed, the path update
+  is sent to the local planner. The right branch, which is the rate controlled
+  ComputePathToPose, always returns RUNNING. Because the Parallel node uses a
+  threshold of 1, whenever the FollowPath returns SUCCESS or FAILURE, the parallel
+  node will return this result.
+
+  The path endpoints (input to the global planner) and the resulting path (output of
+  the global planner and input to the local planner) are passed on the blackboard.
+
+  The rate at which the ComputePathToPose operation is invoked can be controlled with
+  the hz parameter to the RateController node.
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <SequenceStar name="root">
+      <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
+      <Parallel threshold_M="1">
+        <FollowPath path="${path}"/>
+        <Sequence>
+          <RateController hz="2">
+            <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
+          </RateController>
+          <UpdatePath/>
+        </Sequence>
+      </Parallel>
+    </SequenceStar>
+  </BehaviorTree>
+</root>

nav2_bt_navigator/behavior_trees/simple_sequential.xml

+<!--
+  This Behavior Tree does the bare minimum - it invokes ComputePathToPose to
+  compute a path using the global planner. It then invokes the local planner
+  (FollowPath) with this path. The path endpoints (input to the global planner)
+  and the resulting path (output of the global planner and input to the local
+  planner) are passed on the blackboard.
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <SequenceStar name="root">
+      <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
+      <FollowPath path="${path}"/>
+    </SequenceStar>
+  </BehaviorTree>
+</root>

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -15,6 +15,7 @@
 #ifndef NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
 #define NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
 
+#include <string>
 #include <memory>
 #include "nav2_tasks/navigate_to_pose_task.hpp"
 #include "nav2_robot/robot.hpp"
@@ -33,6 +34,7 @@ public:
 private:
   std::unique_ptr<nav2_robot::Robot> robot_;
   std::unique_ptr<nav2_tasks::NavigateToPoseTaskServer> task_server_;
+  std::string bt_xml_filename_;
 };
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp

@@ -15,8 +15,10 @@
 #ifndef NAV2_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_
 #define NAV2_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_
 
+#include <memory>
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_tasks/behavior_tree_engine.hpp"
+#include "nav2_tasks/follow_path_task.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -26,6 +28,11 @@ class NavigateToPoseBehaviorTree : public nav2_tasks::BehaviorTreeEngine
 public:
   explicit NavigateToPoseBehaviorTree(rclcpp::Node::SharedPtr node);
   NavigateToPoseBehaviorTree() = delete;
+
+private:
+  // Support for a BT SimpleActionNode that updates the FollowPath task
+  BT::NodeStatus updatePath(BT::TreeNode & tree_node);
+  std::unique_ptr<nav2_tasks::FollowPathTaskClient> follow_path_task_client_;
 };
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/src/bt_navigator.cpp

@@ -14,7 +14,8 @@
 
 #include <string>
 #include <memory>
-#include <sstream>
+#include <fstream>
+#include <streambuf>
 #include "nav2_bt_navigator/bt_navigator.hpp"
 #include "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp"
 #include "nav2_tasks/compute_path_to_pose_task.hpp"
@@ -27,7 +28,7 @@ namespace nav2_bt_navigator
 {
 
 BtNavigator::BtNavigator()
-: Node("NavigateToPoseNode")
+: Node("BtNavigator")
 {
   auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
 
@@ -55,35 +56,41 @@ BtNavigator::navigateToPose(const nav2_tasks::NavigateToPoseCommand::SharedPtr c
   // Create the blackboard that will be shared by all of the nodes in the tree
   BT::Blackboard::Ptr blackboard = BT::Blackboard::create<BT::BlackboardLocal>();
 
-  // Put together the PathEndPoints message for the ComputePathToPose
+  // Put together the PathEndPoints message for the ComputePathToPose task
   auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
   endpoints->start = current->pose.pose;
   endpoints->goal = command->pose;
   endpoints->tolerance = 2.0;  // TODO(mjeronimo): this will come in the command message
 
-  // The path returned from ComputePath and sent to FollowPath
+  // The path returned from ComputePath and sent to the FollowPath task
   auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
 
   // Set the shared data (commands/results)
   blackboard->set<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("endpoints", endpoints);
   blackboard->set<nav2_tasks::ComputePathToPoseResult::SharedPtr>("path", path);  // NOLINT
 
-  std::string xml_text =
-    R"(
-<root main_tree_to_execute="MainTree">
-  <BehaviorTree ID="MainTree">
-    <SequenceStar name="root">
-      <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
-      <FollowPath path="${path}"/>
-    </SequenceStar>
-  </BehaviorTree>
-</root>)";
+  // Get the filename to use from the parameter
+  get_parameter_or<std::string>("bt_xml_filename", bt_xml_filename_,
+    std::string("bt_navigator.xml"));
 
-  RCLCPP_INFO(get_logger(), "Behavior tree XML: %s", xml_text.c_str());
+  // Read the input BT XML file into a string
+  std::ifstream xml_file(bt_xml_filename_);
+
+  if (!xml_file.good()) {
+    RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename_.c_str());
+    return TaskStatus::FAILED;
+  }
+
+  std::string xml_string((std::istreambuf_iterator<char>(xml_file)),
+    std::istreambuf_iterator<char>());
+
+  RCLCPP_INFO(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename_.c_str());
+  RCLCPP_INFO(get_logger(), "Behavior Tree XML: %s", xml_string.c_str());
 
   // Create and run the behavior tree
   NavigateToPoseBehaviorTree bt(shared_from_this());
-  TaskStatus result = bt.run(blackboard, xml_text,
+
+  TaskStatus result = bt.run(blackboard, xml_string,
       std::bind(&nav2_tasks::NavigateToPoseTaskServer::cancelRequested, task_server_.get()));
 
   RCLCPP_INFO(get_logger(), "Completed navigation: result: %d", result);

nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp

@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <memory>
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp"
 #include "nav2_tasks/compute_path_to_pose_action.hpp"
 #include "nav2_tasks/follow_path_action.hpp"
+#include "nav2_tasks/rate_controller_node.hpp"
 
 using namespace std::chrono_literals;
 
@@ -28,6 +30,27 @@ NavigateToPoseBehaviorTree::NavigateToPoseBehaviorTree(rclcpp::Node::SharedPtr n
   // Register our custom action nodes so that they can be included in XML description
   factory_.registerNodeType<nav2_tasks::ComputePathToPoseAction>("ComputePathToPose");
   factory_.registerNodeType<nav2_tasks::FollowPathAction>("FollowPath");
+
+  // Register our custom decorator nodes
+  factory_.registerNodeType<nav2_tasks::RateController>("RateController");
+
+  // Register our Simple Action nodes
+  factory_.registerSimpleAction("UpdatePath",
+    std::bind(&NavigateToPoseBehaviorTree::updatePath, this, std::placeholders::_1));
+
+  // The parallel node is not yet registered in the BehaviorTree.CPP library
+  factory_.registerNodeType<BT::ParallelNode>("Parallel");
+
+  follow_path_task_client_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(node);
+}
+
+BT::NodeStatus NavigateToPoseBehaviorTree::updatePath(BT::TreeNode & tree_node)
+{
+  // Get the updated path from the blackboard and send to the FollowPath task server
+  auto path = tree_node.blackboard()->template get<nav2_tasks::ComputePathToPoseResult::SharedPtr>(
+    "path");
+  follow_path_task_client_->sendUpdate(path);
+  return BT::NodeStatus::RUNNING;
 }
 
 }  // namespace nav2_bt_navigator

nav2_dwb_controller/dwb_controller/src/dwb_controller.cpp

@@ -55,6 +55,26 @@ DwbController::~DwbController()
 TaskStatus
 DwbController::followPath(const nav2_tasks::FollowPathCommand::SharedPtr command)
 {
+#if 0
+  // TODO(mjeronimo): Integrate the following example code into the
+  // main loop below
+
+  while (true) {
+
+    ...
+
+    if (task_server_->updateRequested()) {
+      auto new_path = std::make_shared<nav2_tasks::FollowPathCommand>();
+      task_server_->getCommandUpdate(new_path);
+      task_server_->setUpdated();
+
+      // Update the target path
+    }
+
+    ...
+  }
+#endif
+
   RCLCPP_INFO(get_logger(), "Starting controller");
   try {
     auto path = nav_2d_utils::pathToPath2D(*command);

nav2_simple_navigator/src/simple_navigator.cpp

@@ -40,6 +40,7 @@ SimpleNavigator::SimpleNavigator()
   controller_client_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(temp_node);
 
   task_server_ = std::make_unique<nav2_tasks::NavigateToPoseTaskServer>(temp_node);
+
   task_server_->setExecuteCallback(
     std::bind(&SimpleNavigator::navigateToPose, this, std::placeholders::_1));
 }

nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp

@@ -36,7 +36,7 @@ public:
     BT::Blackboard::Ptr & blackboard,
     const std::string & behavior_tree_xml,
     std::function<bool()> cancelRequested,
-    std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(100));
+    std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(10));
 
 protected:
   // The ROS node to use for any task clients

nav2_tasks/include/nav2_tasks/bt_action_node.hpp

@@ -52,7 +52,7 @@ public:
   // as getting values from the shared blackboard. A BT node can't get values
   // from the blackboard in the constructor since the BT library doesn't set
   // the blackboard until *after* the tree is build
-  virtual void init() {}
+  virtual void onInit() {}
 
   BT::NodeStatus tick() override
   {
@@ -69,7 +69,7 @@ public:
       task_client_ = std::make_unique<nav2_tasks::TaskClient<CommandMsg, ResultMsg>>(node_);
 
       // Give the derived class a chance to do some initialization
-      init();
+      onInit();
       initialized_ = true;
     }
 

nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp

@@ -35,7 +35,7 @@ public:
   {
   }
 
-  void init() override
+  void onInit() override
   {
     command_ =
       blackboard()->template get<nav2_tasks::ComputePathToPoseCommand::SharedPtr>("endpoints");

nav2_tasks/include/nav2_tasks/follow_path_action.hpp

@@ -32,8 +32,12 @@ public:
   explicit FollowPathAction(const std::string & action_name)
   : BtActionNode<FollowPathCommand, FollowPathResult>(action_name)
   {
-    // Create the input and output messages
-    command_ = std::make_shared<nav2_tasks::FollowPathCommand>();
+  }
+
+  void onInit() override
+  {
+    // Set up the input and output messages
+    command_ = blackboard()->template get<nav2_tasks::ComputePathToPoseResult::SharedPtr>("path");
     result_ = std::make_shared<nav2_tasks::FollowPathResult>();
   }
 };

nav2_tasks/include/nav2_tasks/rate_controller_node.hpp

+// Copyright (c) 2018 Intel Corporation
+//
+// 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_TASKS__RATE_CONTROLLER_NODE_HPP_
+#define NAV2_TASKS__RATE_CONTROLLER_NODE_HPP_
+
+#include <string>
+#include <chrono>
+#include "behavior_tree_core/decorator_node.h"
+
+namespace nav2_tasks
+{
+
+class RateController : public BT::DecoratorNode
+{
+public:
+  RateController(const std::string & name, const BT::NodeParameters & params)
+  : BT::DecoratorNode(name, params)
+  {
+    unsigned int hz = 1;
+    getParam<unsigned int>("hz", hz);
+    period_ = 1.0 / hz;
+  }
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"hz", "10"}};
+    return params;
+  }
+
+private:
+  BT::NodeStatus tick() override;
+
+  std::chrono::time_point<std::chrono::high_resolution_clock> start_;
+  double period_;
+};
+
+inline BT::NodeStatus RateController::tick()
+{
+  if (status() == BT::NodeStatus::IDLE) {
+    // Reset the starting point since we're starting a new iteration of
+    // the rate controller (moving from IDLE to RUNNING)
+    start_ = std::chrono::high_resolution_clock::now();
+  }
+
+  setStatus(BT::NodeStatus::RUNNING);
+
+  // Determine how long its been since we've started this iteration
+  auto now = std::chrono::high_resolution_clock::now();
+  auto elapsed = now - start_;
+
+  // Now, get that in seconds
+  typedef std::chrono::duration<float> float_seconds;
+  auto seconds = std::chrono::duration_cast<float_seconds>(elapsed);
+
+  // If we've exceed the specified period, execute the child node
+  if (seconds.count() >= period_) {
+    const BT::NodeStatus child_state = child_node_->executeTick();
+
+    switch (child_state) {
+      case BT::NodeStatus::SUCCESS:
+        child_node_->setStatus(BT::NodeStatus::IDLE);
+        RCLCPP_DEBUG(rclcpp::get_logger("RateController"), "seconds.count: %lf", seconds.count());
+
+        // Reset the timer
+        start_ = std::chrono::high_resolution_clock::now();
+        return BT::NodeStatus::SUCCESS;
+
+      case BT::NodeStatus::RUNNING:
+        return BT::NodeStatus::RUNNING;
+
+      case BT::NodeStatus::FAILURE:
+      default:
+        // We'll try again next time
+        child_node_->setStatus(BT::NodeStatus::IDLE);
+        return BT::NodeStatus::RUNNING;
+    }
+  }
+
+  return status();
+}
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__RATE_CONTROLLER_NODE_HPP_

nav2_tasks/include/nav2_tasks/task_client.hpp

@@ -49,6 +49,7 @@ public:
 
     // Create the publishers
     commandPub_ = node_->create_publisher<CommandMsg>(taskName + "_command");
+    updatePub_ = node_->create_publisher<CommandMsg>(taskName + "_update");
     cancelPub_ = node_->create_publisher<CancelMsg>(taskName + "_cancel");
 
     // Create the subscribers
@@ -72,6 +73,13 @@ public:
     commandPub_->publish(msg);
   }
 
+  void sendUpdate(const typename CommandMsg::SharedPtr msg)
+  {
+    resultReceived_ = false;
+    statusReceived_ = false;
+    updatePub_->publish(msg);
+  }
+
   // An in-flight operation can be canceled
   void cancel()
   {
@@ -134,7 +142,10 @@ public:
           if (cvResult_.wait_for(lock, std::chrono::milliseconds(100),
             [&] {return resultReceived_ == true;}))
           {
-            result = resultMsg_;
+            // Copy only the data, not the pointer since the pointer may have been used in
+            // a BT blackboard
+            *result = *resultMsg_;
+
             resultReceived_ = false;
             return SUCCEEDED;
           }
@@ -199,6 +210,7 @@ protected:
 
   // The client's publishers: the command and cancel messages
   typename rclcpp::Publisher<CommandMsg>::SharedPtr commandPub_;
+  typename rclcpp::Publisher<CommandMsg>::SharedPtr updatePub_;
   rclcpp::Publisher<CancelMsg>::SharedPtr cancelPub_;
 
   // The client's subscriptions: result, feedback, and status

nav2_tasks/include/nav2_tasks/task_server.hpp

@@ -38,13 +38,18 @@ public:
   : node_(node),
     workerThread_(nullptr),
     commandReceived_(false),
+    updateReceived_(false),
     cancelReceived_(false),
     eptr_(nullptr)
   {
     std::string taskName = getTaskName<CommandMsg, ResultMsg>();
+
     commandSub_ = node_->create_subscription<CommandMsg>(taskName + "_command",
         std::bind(&TaskServer::onCommandReceived, this, std::placeholders::_1));
 
+    updateSub_ = node_->create_subscription<CommandMsg>(taskName + "_update",
+        std::bind(&TaskServer::onUpdateReceived, this, std::placeholders::_1));
+
     cancelSub_ = node_->create_subscription<std_msgs::msg::Empty>(taskName + "_cancel",
         std::bind(&TaskServer::onCancelReceived, this, std::placeholders::_1));
 
@@ -83,6 +88,21 @@ public:
     cancelReceived_ = false;
   }
 
+  void getCommandUpdate(typename CommandMsg::SharedPtr & command)
+  {
+    *command = *updateMsg_;
+  }
+
+  bool updateRequested()
+  {
+    return updateReceived_;
+  }
+
+  void setUpdated()
+  {
+    updateReceived_ = false;
+  }
+
   void setResult(const ResultMsg & result)
   {
     resultMsg_ = result;
@@ -99,6 +119,7 @@ protected:
   ExecuteCallback execute_callback_;
 
   typename CommandMsg::SharedPtr commandMsg_;
+  typename CommandMsg::SharedPtr updateMsg_;
   ResultMsg resultMsg_;
 
   // These messages are internal to the TaskClient implementation
@@ -187,10 +208,8 @@ protected:
   std::mutex commandMutex_;
   bool commandReceived_;
   std::condition_variable cvCommand_;
-
-  // Variables to handle the communication of the cancel request to the execute thread
+  std::atomic<bool> updateReceived_;
   std::atomic<bool> cancelReceived_;
-  std::condition_variable cvCancel_;
 
   // The callbacks for our subscribers
   void onCommandReceived(const typename CommandMsg::SharedPtr msg)
@@ -204,14 +223,20 @@ protected:
     cvCommand_.notify_one();
   }
 
+  void onUpdateReceived(const typename CommandMsg::SharedPtr msg)
+  {
+    updateMsg_ = msg;
+    updateReceived_ = true;
+  }
+
   void onCancelReceived(const CancelMsg::SharedPtr /*msg*/)
   {
     cancelReceived_ = true;
-    cvCancel_.notify_one();
   }
 
   // The subscribers: command and cancel
   typename rclcpp::Subscription<CommandMsg>::SharedPtr commandSub_;
+  typename rclcpp::Subscription<CommandMsg>::SharedPtr updateSub_;
   rclcpp::Subscription<CancelMsg>::SharedPtr cancelSub_;
 
   // The publishers for the result from this task

nav2_tasks/src/behavior_tree_engine.cpp

@@ -40,7 +40,7 @@ TaskStatus BehaviorTreeEngine::run(
 {
   // Set a couple values that all of the action nodes expect/require
   blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
-  blackboard->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(100)); // NOLINT
+  blackboard->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10));  // NOLINT
 
   // The complete behavior tree that results from parsing the incoming XML. When the tree goes
   // out of scope, all the nodes are destroyed