From 6d37648cd77f8a36fc1929d2131a941bc270beb2 Mon Sep 17 00:00:00 2001
From: Michael Jeronimo <michael.jeronimo@intel.com>
Date: Tue, 30 Oct 2018 16:11:45 -0700
Subject: [PATCH] Behavior tree for mission execution and BtNavigator (#227)

* Enable behavior trees and work on cancel capability
* Checkpoint working cancel with traces
* ExecuteMission behavior tree working w/ cancel
* A little clean-up
* Address cpplint and uncrustify issues
* Parameterize the loop rate for the behavior tree classes
* Build a parallel behavior tree for the NavToPose task
* Update the BT Navigator behavior tree to compute a path first before starting the parallel nodes
* Resolve how to handle BTpp dependency; Closes: #51
* Rebase to origin/master
* Inital port to new, updated repo for the behavior tree library
* Can't use shared_from_this in constructor; fix several linter issues
* Start on dynamically creating the behavior tree from an XML description
* Move a few files after rebasing
* Don't have to explicity specify dl dependency
* Working on execute mission BT
* Adapt the BtActionNode class to enable it to by dynamically created
* Checkpoint working version
* Another checkpoint for working version
* Clean up
* Enable the BtNavigator with the new behavior tree library
* Start hooking up params
* Adapt to new Behavior Tree library
* Pass position and orientation when using pose via rviz
* Missed a new file
* Re-enable getting pose from robot; was commented out for debugging
* Revert one of my changes for the BehaviorTree.CPP library now that it is fixed
* Back out the workaround for visibility warning now that it is fixed in the upstream library
* Rebase and cpplint/uncrustify
* Get rid of hardcoded starting position in compute_path_to_pose_action and address some linter issues
* Make the intent clearer for a couple variables names
* Fix a couple inconsistencies with the last checkin
---
 nav2_astar_planner/src/astar_planner.cpp      |  35 ++--
 nav2_bt_navigator/CMakeLists.txt              |   7 +-
 .../nav2_bt_navigator/bt_navigator.hpp        |  15 +-
 .../navigate_to_pose_behavior_tree.hpp        |  49 ++++++
 nav2_bt_navigator/package.xml                 |   4 +
 nav2_bt_navigator/src/bt_navigator.cpp        | 157 ++++++------------
 .../src/navigate_to_pose_behavior_tree.cpp    |  75 +++++++++
 .../dwa_controller.hpp                        |  45 +++++
 .../src/dwa_controller.cpp                    |  97 +++++++++++
 nav2_mission_executor/CMakeLists.txt          |   8 +-
 .../execute_mission_behavior_tree.hpp         |  49 ++++++
 .../mission_executor.hpp                      |  18 +-
 nav2_mission_executor/package.xml             |   3 +
 .../src/execute_mission_behavior_tree.cpp     |  74 +++++++++
 nav2_mission_executor/src/main.cpp            |   2 +-
 .../src/mission_executor.cpp                  | 126 ++++++--------
 nav2_robot/src/robot.cpp                      |   8 +-
 .../src/simple_navigator.cpp                  |  48 ++++--
 .../src/planning/planner_tester.cpp           |   5 +-
 nav2_tasks/CMakeLists.txt                     |  20 ++-
 .../nav2_tasks/behavior_tree_engine.hpp       |  51 ++++++
 .../include/nav2_tasks/bt_action_node.hpp     | 123 ++++++++++++++
 .../include/nav2_tasks/bt_conversions.hpp     |  76 +++++++++
 .../compute_path_to_pose_action.hpp           |  88 ++++++++++
 .../nav2_tasks/execute_mission_action.hpp     |  29 ++++
 .../include/nav2_tasks/follow_path_action.hpp |  43 +++++
 .../nav2_tasks/navigate_to_pose_action.hpp    |  66 ++++++++
 nav2_tasks/include/nav2_tasks/task_client.hpp |  15 +-
 nav2_tasks/include/nav2_tasks/task_server.hpp |   2 +
 nav2_tasks/package.xml                        |   2 +
 nav2_tasks/src/behavior_tree_engine.cpp       |  73 ++++++++
 tools/ros2_dependencies.repos                 |   4 +
 32 files changed, 1162 insertions(+), 255 deletions(-)
 create mode 100644 nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp
 create mode 100644 nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
 create mode 100644 nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp
 create mode 100644 nav2_controller/nav2_controller_example/src/dwa_controller.cpp
 create mode 100644 nav2_mission_executor/include/nav2_mission_executor/execute_mission_behavior_tree.hpp
 create mode 100644 nav2_mission_executor/src/execute_mission_behavior_tree.cpp
 create mode 100644 nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp
 create mode 100644 nav2_tasks/include/nav2_tasks/bt_action_node.hpp
 create mode 100644 nav2_tasks/include/nav2_tasks/bt_conversions.hpp
 create mode 100644 nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp
 create mode 100644 nav2_tasks/include/nav2_tasks/execute_mission_action.hpp
 create mode 100644 nav2_tasks/include/nav2_tasks/follow_path_action.hpp
 create mode 100644 nav2_tasks/include/nav2_tasks/navigate_to_pose_action.hpp
 create mode 100644 nav2_tasks/src/behavior_tree_engine.cpp

diff --git a/nav2_astar_planner/src/astar_planner.cpp b/nav2_astar_planner/src/astar_planner.cpp
index a786a676..10a49b66 100644
--- a/nav2_astar_planner/src/astar_planner.cpp
+++ b/nav2_astar_planner/src/astar_planner.cpp
@@ -14,6 +14,8 @@
 
 #include <string>
 #include <chrono>
+#include <ctime>
+#include <cstdlib>
 #include "nav2_astar_planner/astar_planner.hpp"
 
 using namespace std::chrono_literals;
@@ -25,43 +27,44 @@ namespace nav2_astar_planner
 AStarPlanner::AStarPlanner()
 : nav2_tasks::ComputePathToPoseTaskServer("ComputePathToPoseNode")
 {
-  RCLCPP_INFO(get_logger(), "Initializing AStarPlanner");
+  RCLCPP_INFO(get_logger(), "Initializing");
 }
 
 AStarPlanner::~AStarPlanner()
 {
-  RCLCPP_INFO(get_logger(), "Shutting down AStarPlanner");
+  RCLCPP_INFO(get_logger(), "Shutting down");
 }
 
 TaskStatus
 AStarPlanner::execute(const nav2_tasks::ComputePathToPoseCommand::SharedPtr command)
 {
-  RCLCPP_INFO(get_logger(), "AStarPlanner: Attempting to a find path from (%.2f, %.2f) to "
-    "(%.2f, %.2f).",command->start.position.x, command->start.position.y,
-    command->goal.position.x, command->goal.position.y);
+  RCLCPP_INFO(get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
+    "(%.2f, %.2f) with tolerance %.2f.",
+    command->start.position.x, command->start.position.y,
+    command->goal.position.x, command->goal.position.y,
+    command->tolerance);
+
+  unsigned int seed = std::time(nullptr);
+
+  // Let's use a random number of iterations (at least one)
+  int iterations = (rand_r(&seed) % 7) + 1;
 
   // Spin here for a bit to fake out some processing time
-  for (int i = 0; i < 10; i++) {
+  for (int i = 0; i < iterations; i++) {
     // Do a bit of the task
-    RCLCPP_DEBUG(get_logger(), "AStarPlanner::execute: doing work: %d", i);
-    std::this_thread::sleep_for(100ms);
-
-    // if (failed_to_plan)
-    // {
-    //   RCLCPP_WARN(get_logger(), "AStarPlanner: Failed to generate path.");
-    // }
+    std::this_thread::sleep_for(10ms);
 
     // Before we loop again to do more work, check if we've been canceled
     if (cancelRequested()) {
-      RCLCPP_INFO(get_logger(), "AStarPlanner: Cancelled global planning task.");
+      RCLCPP_INFO(get_logger(), "Canceled global planning task.");
       setCanceled();
       return TaskStatus::CANCELED;
     }
   }
 
   // We've successfully completed the task, so return the result
-  RCLCPP_INFO(get_logger(), "AStarPlanner: Successfully navigated to (%.2f, %.2f) with tolerance %.2f",
-    command->goal.position.x, command->goal.position.y, command->tolerance);
+  RCLCPP_INFO(get_logger(), "Successfully computed a path to (%.2f, %.2f)",
+    command->goal.position.x, command->goal.position.y);
 
   nav2_tasks::ComputePathToPoseResult result;
   setResult(result);
diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt
index 88a00233..28d4b8bf 100644
--- a/nav2_bt_navigator/CMakeLists.txt
+++ b/nav2_bt_navigator/CMakeLists.txt
@@ -15,6 +15,8 @@ find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(nav2_tasks REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(nav2_robot REQUIRED)
+find_package(behavior_tree_core REQUIRED)
 
 include_directories(
   include
@@ -30,6 +32,7 @@ set(library_name ${executable_name}_core)
 
 add_library(${library_name}
   src/bt_navigator.cpp
+  src/navigate_to_pose_behavior_tree.cpp
 )
 
 set(dependencies
@@ -37,7 +40,9 @@ set(dependencies
   std_msgs
   nav2_tasks
   nav2_msgs
-) 
+  nav2_robot
+  behavior_tree_core
+)
 
 ament_target_dependencies(${executable_name}
   ${dependencies}
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
index b2beeb12..fbefad6a 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
@@ -15,12 +15,8 @@
 #ifndef NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
 #define NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
 
-#include <string>
-#include <memory>
-#include "nav2_tasks/task_status.hpp"
 #include "nav2_tasks/navigate_to_pose_task.hpp"
-#include "nav2_tasks/compute_path_to_pose_task.hpp"
-#include "nav2_tasks/follow_path_task.hpp"
+#include "nav2_robot/robot.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -29,13 +25,12 @@ class BtNavigator : public nav2_tasks::NavigateToPoseTaskServer
 {
 public:
   BtNavigator();
-  ~BtNavigator();
 
-  nav2_tasks::TaskStatus execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr command);
+  nav2_tasks::TaskStatus execute(
+    const nav2_tasks::NavigateToPoseCommand::SharedPtr command) override;
 
-protected:
-  std::unique_ptr<nav2_tasks::ComputePathToPoseTaskClient> plannerTaskClient_;
-  std::unique_ptr<nav2_tasks::FollowPathTaskClient> controllerTaskClient_;
+private:
+  nav2_robot::Robot robot_;
 };
 
 }  // namespace nav2_bt_navigator
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp
new file mode 100644
index 00000000..036673f2
--- /dev/null
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp
@@ -0,0 +1,49 @@
+// 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_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_
+#define NAV2_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "behavior_tree_core/behavior_tree.h"
+#include "behavior_tree_core/bt_factory.h"
+#include "behavior_tree_core/xml_parsing.h"
+#include "nav2_tasks/navigate_to_pose_task.hpp"
+
+namespace nav2_bt_navigator
+{
+
+class NavigateToPoseBehaviorTree
+{
+public:
+  explicit NavigateToPoseBehaviorTree(rclcpp::Node::SharedPtr node);
+  NavigateToPoseBehaviorTree() = delete;
+
+  nav2_tasks::TaskStatus run(
+    const std::string & behavior_tree_xml,
+    std::function<bool()> cancelRequested,
+    std::chrono::milliseconds tree_tick_timeout = std::chrono::milliseconds(100));
+
+private:
+  // The ROS node to use for any task clients
+  rclcpp::Node::SharedPtr node_;
+
+  // A factory that will be used to dynamically construct the behavior tree
+  BT::BehaviorTreeFactory factory_;
+};
+
+}  // namespace nav2_bt_navigator
+
+#endif  // NAV2_BT_NAVIGATOR__NAVIGATE_TO_POSE_BEHAVIOR_TREE_HPP_
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
index b0fdf9b6..cf082589 100644
--- a/nav2_bt_navigator/package.xml
+++ b/nav2_bt_navigator/package.xml
@@ -12,10 +12,14 @@
   <build_depend>rclcpp</build_depend>
   <build_depend>nav2_tasks</build_depend>
   <build_depend>nav2_msgs</build_depend>
+  <build_depend>nav2_robot</build_depend>
+  <build_depend>behavior_tree_core</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>nav2_tasks</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>nav2_robot</exec_depend>
+  <exec_depend>behavior_tree_core</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 2b9b23c8..838569f1 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -12,133 +12,74 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
-#include <memory>
-#include <exception>
-#include <chrono>
+#include <sstream>
 #include "nav2_bt_navigator/bt_navigator.hpp"
+#include "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp"
 
-using namespace std::chrono_literals;
 using nav2_tasks::TaskStatus;
 
 namespace nav2_bt_navigator
 {
 
 BtNavigator::BtNavigator()
-: nav2_tasks::NavigateToPoseTaskServer("NavigateToPoseNode")
+: nav2_tasks::NavigateToPoseTaskServer("NavigateToPoseNode"),
+  robot_(this)
 {
-  RCLCPP_INFO(get_logger(), "Initializing BtNavigator");
-
-  plannerTaskClient_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(this);
-  controllerTaskClient_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(this);
-
-  if (!plannerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
-    RCLCPP_ERROR(get_logger(), "BtNavigator: Planner is not running");
-    throw std::runtime_error("BtNavigator: planner not running");
-  }
-
-  if (!controllerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
-    RCLCPP_ERROR(get_logger(), "BtNavigator: Controller is not running");
-    throw std::runtime_error("BtNavigator: controller not running");
-  }
-}
-
-BtNavigator::~BtNavigator()
-{
-  RCLCPP_INFO(get_logger(), "Shutting down BtNavigator");
 }
 
 TaskStatus
 BtNavigator::execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr command)
 {
-  RCLCPP_INFO(get_logger(), "BtNavigator: Received new navigation goal to (%.2f, %.2f).",
+  RCLCPP_INFO(get_logger(), "Start navigating to goal (%.2f, %.2f).",
     command->pose.position.x, command->pose.position.y);
 
-  // Compose the PathEndPoints message for Navigation
-  auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
-  // TODO(mjeronimo): get the starting pose from Localization (fake it out for now)
-  endpoints->start = command->pose;
-  endpoints->goal = command->pose;
-
-  RCLCPP_INFO(get_logger(), "BtNavigator: Getting a path from the planner.");
-  auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
-  plannerTaskClient_->sendCommand(endpoints);
-
-  // Loop until the subtasks are completed
-  for (;; ) {
-    // Check to see if this task (navigation) has been canceled. If so, cancel any child
-    // tasks and then cancel this task
-    if (cancelRequested()) {
-      RCLCPP_INFO(get_logger(), "BtNavigator: Task has been canceled.");
-      plannerTaskClient_->cancel();
-      setCanceled();
-      return TaskStatus::CANCELED;
-    }
-
-    // Check if the planning task has completed
-    TaskStatus status = plannerTaskClient_->waitForResult(path, 100ms);
-
-    switch (status) {
-      case TaskStatus::SUCCEEDED:
-        RCLCPP_INFO(get_logger(), "BtNavigator: Planning task completed.");
-        goto here;
+  // Get the current pose from the robot
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current;
 
-      case TaskStatus::FAILED:
-        return TaskStatus::FAILED;
-
-      case TaskStatus::RUNNING:
-        RCLCPP_DEBUG(get_logger(), "BtNavigator: Planning task still running");
-        break;
-
-      default:
-        RCLCPP_ERROR(get_logger(), "BtNavigator: Invalid status value.");
-        throw std::logic_error("BtNavigator::execute: invalid status value");
-    }
+  if (!robot_.getCurrentPose(current)) {
+    RCLCPP_ERROR(get_logger(), "Current robot pose is not available.");
+    return TaskStatus::FAILED;
   }
 
-here:
-  RCLCPP_INFO(get_logger(),
-    "BtNavigator: Sending the path to the controller to execute.");
-
-  controllerTaskClient_->sendCommand(path);
-
-  // Loop until the control task completes
-  for (;; ) {
-    // Check to see if this task (navigation) has been canceled. If so, cancel any child
-    // tasks and then cancel this task
-    if (cancelRequested()) {
-      RCLCPP_INFO(get_logger(), "BtNavigator: Task has been canceled.");
-      controllerTaskClient_->cancel();
-      setCanceled();
-      return TaskStatus::CANCELED;
-    }
-
-    // Check if the control task has completed
-    auto controlResult = std::make_shared<nav2_tasks::FollowPathResult>();
-    TaskStatus status = controllerTaskClient_->waitForResult(controlResult, 10ms);
-
-    switch (status) {
-      case TaskStatus::SUCCEEDED:
-        {
-          RCLCPP_INFO(get_logger(), "BtNavigator: Control task completed.");
-          nav2_tasks::NavigateToPoseResult navigationResult;
-          setResult(navigationResult);
-
-          return TaskStatus::SUCCEEDED;
-        }
-
-      case TaskStatus::FAILED:
-        return TaskStatus::FAILED;
-
-      case TaskStatus::RUNNING:
-        RCLCPP_INFO(get_logger(), "BtNavigator: Control task still running.");
-        break;
-
-      default:
-        RCLCPP_ERROR(get_logger(), "BtNavigator: Invalid status value.");
-        throw std::logic_error("BtNavigator::execute: invalid status value");
-    }
-  }
+  // Get a reference to the command pose for convenience
+  geometry_msgs::msg::Pose & goal = command->pose;
+
+  // Compose the args for the ComputePathToPose action
+  std::stringstream args;
+  args << "start_position=\"" <<
+    current->pose.pose.position.x << ";" << current->pose.pose.position.y << ";" <<
+    current->pose.pose.position.z << "\" " <<
+    "start_orientation=\"" <<
+    current->pose.pose.orientation.x << ";" << current->pose.pose.orientation.y << ";" <<
+    current->pose.pose.orientation.z << ";" << current->pose.pose.orientation.w << "\"" <<
+    "goal_position=\"" <<
+    goal.position.x << ";" << goal.position.y << ";" << goal.position.z << "\" " <<
+    "goal_orientation=\"" <<
+    goal.orientation.x << ";" << goal.orientation.y << ";" <<
+    goal.orientation.z << ";" << goal.orientation.w << "\"";
+
+  // Put it all together, trying to make the XML somewhat readable here
+  std::stringstream command_ss;
+  command_ss <<
+    R"(
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <SequenceStar name="root">
+      <ComputePathToPose )" << args.str() <<
+    R"(/>
+      <FollowPath />
+    </SequenceStar>
+  </BehaviorTree>
+</root>)";
+
+  RCLCPP_INFO(get_logger(), "Behavior tree XML: %s", command_ss.str().c_str());
+
+  // Create and run the behavior tree
+  NavigateToPoseBehaviorTree bt(shared_from_this());
+  TaskStatus result = bt.run(command_ss.str(), std::bind(&BtNavigator::cancelRequested, this));
+
+  RCLCPP_INFO(get_logger(), "Completed navigation: result: %d", result);
+  return result;
 }
 
 }  // namespace nav2_bt_navigator
diff --git a/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
new file mode 100644
index 00000000..3408635c
--- /dev/null
+++ b/nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp
@@ -0,0 +1,75 @@
+// 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.
+
+#include "nav2_bt_navigator/navigate_to_pose_behavior_tree.hpp"
+
+#include <memory>
+#include <string>
+#include <set>
+#include "behavior_tree_core/xml_parsing.h"
+#include "Blackboard/blackboard_local.h"
+#include "nav2_tasks/compute_path_to_pose_action.hpp"
+#include "nav2_tasks/follow_path_action.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_bt_navigator
+{
+
+NavigateToPoseBehaviorTree::NavigateToPoseBehaviorTree(rclcpp::Node::SharedPtr node)
+: node_(node)
+{
+  // 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");
+}
+
+nav2_tasks::TaskStatus
+NavigateToPoseBehaviorTree::run(
+  const std::string & behavior_tree_xml,
+  std::function<bool()> cancelRequested,
+  std::chrono::milliseconds tree_tick_timeout)
+{
+  // Create the blackboard that will be shared by all of the nodes in the tree
+  BT::Blackboard::Ptr blackboard = BT::Blackboard::create<BT::BlackboardLocal>();
+
+  // 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));
+
+  // The complete behavior tree that results from parsing the XML. When the tree goes
+  // out of scope, all the nodes are destroyed
+  BT::Tree tree = BT::buildTreeFromText(factory_, behavior_tree_xml, blackboard);
+
+  rclcpp::WallRate loop_rate(tree_tick_timeout);
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // Loop until something happens with ROS or the node completes w/ success or failure
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree.root_node->executeTick();
+
+    // Check if we've received a cancel message
+    if (cancelRequested()) {
+      tree.root_node->halt();
+      return nav2_tasks::TaskStatus::CANCELED;
+    }
+
+    loop_rate.sleep();
+  }
+
+  return (result == BT::NodeStatus::SUCCESS) ?
+         nav2_tasks::TaskStatus::SUCCEEDED : nav2_tasks::TaskStatus::FAILED;
+}
+
+}  // namespace nav2_bt_navigator
diff --git a/nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp b/nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp
new file mode 100644
index 00000000..33a52030
--- /dev/null
+++ b/nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp
@@ -0,0 +1,45 @@
+// 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_CONTROLLER_EXAMPLE__DWA_CONTROLLER_HPP_
+#define NAV2_CONTROLLER_EXAMPLE__DWA_CONTROLLER_HPP_
+
+#include <memory>
+#include <string>
+#include <memory>
+
+#include "nav2_tasks/follow_path_task.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace nav2_controller_example
+{
+
+class DwaController : public nav2_tasks::FollowPathTaskServer
+{
+public:
+  DwaController();
+  ~DwaController();
+
+  nav2_tasks::TaskStatus execute(const nav2_tasks::FollowPathCommand::SharedPtr path) override;
+
+protected:
+  void sendVelocity(double speed);
+
+  using CmdVel = geometry_msgs::msg::Twist;
+  std::shared_ptr<rclcpp::Publisher<CmdVel>> vel_pub_;
+};
+
+}  // namespace nav2_controller_example
+
+#endif  // NAV2_CONTROLLER_EXAMPLE__DWA_CONTROLLER_HPP_
diff --git a/nav2_controller/nav2_controller_example/src/dwa_controller.cpp b/nav2_controller/nav2_controller_example/src/dwa_controller.cpp
new file mode 100644
index 00000000..812ce5c3
--- /dev/null
+++ b/nav2_controller/nav2_controller_example/src/dwa_controller.cpp
@@ -0,0 +1,97 @@
+// 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.
+
+#include <string>
+#include <chrono>
+#include <ctime>
+#include <cstdlib>
+#include "nav2_controller_example/dwa_controller.hpp"
+
+using namespace std::chrono_literals;
+using nav2_tasks::TaskStatus;
+
+namespace nav2_controller_example
+{
+
+DwaController::DwaController()
+: nav2_tasks::FollowPathTaskServer("FollowPathNode")
+{
+  RCLCPP_INFO(get_logger(), "Initializing DwaController");
+  vel_pub_ = this->create_publisher<CmdVel>("/mobile_base/commands/velocity", 1);
+}
+
+DwaController::~DwaController()
+{
+  RCLCPP_INFO(get_logger(), "Shutting down DwaController");
+}
+
+TaskStatus
+DwaController::execute(const nav2_tasks::FollowPathCommand::SharedPtr command)
+{
+  if (command->poses.size() == 0) {
+    RCLCPP_WARN(get_logger(), "Received an empty path");
+  } else {
+    RCLCPP_INFO(get_logger(), "Received a new path to follow of"
+      " size %i ending at (%.2f, %.2f)", (int)command->poses.size(),
+      command->poses.back().position.x,
+      command->poses.back().position.y);
+  }
+
+  unsigned int seed = std::time(nullptr);
+
+  // Let's use a random number of iterations (from 10 to 30)
+  int iterations = (rand_r(&seed) % 20) + 10;
+
+  // Spin here for a bit to fake out some processing time
+  for (int i = 0; i < iterations; i++) {
+    // Do a bit of the task
+    const double cmd_vel = 0.1;
+    sendVelocity(cmd_vel);
+    std::this_thread::sleep_for(250ms);
+
+    // Before we loop again to do more work, check if we've been canceled
+    if (cancelRequested()) {
+      RCLCPP_INFO(get_logger(), "Follow task has been canceled");
+      setCanceled();
+      return TaskStatus::CANCELED;
+    }
+  }
+
+  // Stop the robot after having it move forward for a bit
+  sendVelocity(0);
+
+  // We've successfully completed the task, so return the result
+  RCLCPP_INFO(get_logger(), "Follow task has been completed");
+
+  nav2_tasks::FollowPathResult result;
+  setResult(result);
+
+  return TaskStatus::SUCCEEDED;
+}
+
+void DwaController::sendVelocity(double speed)
+{
+  RCLCPP_INFO(get_logger(), "Send velocity: %f", speed);
+
+  CmdVel v;
+  v.linear.x = speed;
+  v.linear.y = 0;
+  v.linear.z = 0;
+  v.angular.x = 0;
+  v.angular.y = 0;
+  v.angular.z = 0;
+  vel_pub_->publish(v);
+}
+
+}  // namespace nav2_controller_example
diff --git a/nav2_mission_executor/CMakeLists.txt b/nav2_mission_executor/CMakeLists.txt
index a00de159..f0713f1f 100644
--- a/nav2_mission_executor/CMakeLists.txt
+++ b/nav2_mission_executor/CMakeLists.txt
@@ -13,8 +13,9 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
-find_package(nav2_msgs REQUIRED)
 find_package(nav2_tasks REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(behavior_tree_core REQUIRED)
 
 include_directories(
   include
@@ -30,6 +31,7 @@ set(library_name ${executable_name}_core)
 
 add_library(${library_name}
   src/mission_executor.cpp
+  src/execute_mission_behavior_tree.cpp
 )
 
 set(dependencies
@@ -37,6 +39,7 @@ set(dependencies
   std_msgs
   nav2_tasks
   nav2_msgs
+  behavior_tree_core
 )
 
 ament_target_dependencies(${executable_name}
@@ -60,4 +63,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+ament_export_include_directories(include)
+
 ament_package()
+
diff --git a/nav2_mission_executor/include/nav2_mission_executor/execute_mission_behavior_tree.hpp b/nav2_mission_executor/include/nav2_mission_executor/execute_mission_behavior_tree.hpp
new file mode 100644
index 00000000..560db00b
--- /dev/null
+++ b/nav2_mission_executor/include/nav2_mission_executor/execute_mission_behavior_tree.hpp
@@ -0,0 +1,49 @@
+// 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_MISSION_EXECUTOR__EXECUTE_MISSION_BEHAVIOR_TREE_HPP_
+#define NAV2_MISSION_EXECUTOR__EXECUTE_MISSION_BEHAVIOR_TREE_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "behavior_tree_core/behavior_tree.h"
+#include "behavior_tree_core/bt_factory.h"
+#include "behavior_tree_core/xml_parsing.h"
+#include "nav2_tasks/execute_mission_task.hpp"
+
+namespace nav2_mission_executor
+{
+
+class ExecuteMissionBehaviorTree
+{
+public:
+  explicit ExecuteMissionBehaviorTree(rclcpp::Node::SharedPtr node);
+  ExecuteMissionBehaviorTree() = delete;
+
+  nav2_tasks::TaskStatus run(
+    const std::string & behavior_tree_xml,
+    std::function<bool()> cancelRequested,
+    std::chrono::milliseconds tree_tick_timeout = std::chrono::milliseconds(100));
+
+private:
+  // The ROS node to use for any task clients
+  rclcpp::Node::SharedPtr node_;
+
+  // A factory that will be used to dynamically construct the behavior tree
+  BT::BehaviorTreeFactory factory_;
+};
+
+}  // namespace nav2_mission_executor
+
+#endif  // NAV2_MISSION_EXECUTOR__EXECUTE_MISSION_BEHAVIOR_TREE_HPP_
diff --git a/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp b/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp
index d2de8b81..4f8abcca 100644
--- a/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp
+++ b/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp
@@ -15,38 +15,28 @@
 #ifndef NAV2_MISSION_EXECUTOR__MISSION_EXECUTOR_HPP_
 #define NAV2_MISSION_EXECUTOR__MISSION_EXECUTOR_HPP_
 
-#include <string>
-#include <memory>
-#include "nav2_tasks/task_status.hpp"
 #include "nav2_tasks/execute_mission_task.hpp"
-#include "nav2_tasks/navigate_to_pose_task.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
-namespace nav2_mission_execution
+namespace nav2_mission_executor
 {
 
 class MissionExecutor : public nav2_tasks::ExecuteMissionTaskServer
 {
 public:
   MissionExecutor();
-  ~MissionExecutor();
 
   nav2_tasks::TaskStatus execute(
     const nav2_tasks::ExecuteMissionCommand::SharedPtr command) override;
 
 private:
+  // For now, receive the move_base_simple/goal message from rviz and use this
+  // goal pose to create a mission plan, publishing the result
   void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
-
-  std::unique_ptr<nav2_tasks::NavigateToPoseTaskClient> navTaskClient_;
-
-  // For now, use the move_base_simple/goal topic (from rviz) for the goal pose
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
-  geometry_msgs::msg::PoseStamped::SharedPtr goal_pose_;
-
-  // Also, for now, publish a mission plan when receiving a goal pose from rviz
   rclcpp::Publisher<nav2_msgs::msg::MissionPlan>::SharedPtr plan_pub_;
 };
 
-}  // namespace nav2_mission_execution
+}  // namespace nav2_mission_executor
 
 #endif  // NAV2_MISSION_EXECUTOR__MISSION_EXECUTOR_HPP_
diff --git a/nav2_mission_executor/package.xml b/nav2_mission_executor/package.xml
index ebcf5951..c8f9c03e 100644
--- a/nav2_mission_executor/package.xml
+++ b/nav2_mission_executor/package.xml
@@ -13,10 +13,13 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>nav2_tasks</build_depend>
   <build_depend>nav2_msgs</build_depend>
+  <build_depend>behavior_tree_core</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
   <exec_depend>nav2_tasks</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>behavior_tree_core</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_mission_executor/src/execute_mission_behavior_tree.cpp b/nav2_mission_executor/src/execute_mission_behavior_tree.cpp
new file mode 100644
index 00000000..6ae395bc
--- /dev/null
+++ b/nav2_mission_executor/src/execute_mission_behavior_tree.cpp
@@ -0,0 +1,74 @@
+// 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.
+
+#include "nav2_mission_executor/execute_mission_behavior_tree.hpp"
+
+#include <memory>
+#include <string>
+#include <set>
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "behavior_tree_core/xml_parsing.h"
+#include "Blackboard/blackboard_local.h"
+#include "nav2_tasks/navigate_to_pose_action.hpp"
+#include "nav2_tasks/bt_conversions.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_mission_executor
+{
+
+ExecuteMissionBehaviorTree::ExecuteMissionBehaviorTree(rclcpp::Node::SharedPtr node)
+: node_(node)
+{
+  // Register our custom action nodes so that they can be included in XML description
+  factory_.registerNodeType<nav2_tasks::NavigateToPoseAction>("NavigateToPose");
+}
+
+nav2_tasks::TaskStatus ExecuteMissionBehaviorTree::run(
+  const std::string & behavior_tree_xml,
+  std::function<bool()> cancelRequested,
+  std::chrono::milliseconds tree_tick_timeout)
+{
+  // Create the blackboard that will be shared by all of the nodes in the tree
+  BT::Blackboard::Ptr blackboard = BT::Blackboard::create<BT::BlackboardLocal>();
+
+  // 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));
+
+  // The complete behavior tree that results from parsing the incoming XML. When the tree goes
+  // out of scope, all the nodes are destroyed
+  BT::Tree tree = BT::buildTreeFromText(factory_, behavior_tree_xml, blackboard);
+
+  rclcpp::WallRate loop_rate(tree_tick_timeout);
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // Loop until something happens with ROS or the node completes w/ success or failure
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree.root_node->executeTick();
+
+    // Check if we've received a cancel message
+    if (cancelRequested()) {
+      tree.root_node->halt();
+      return nav2_tasks::TaskStatus::CANCELED;
+    }
+
+    loop_rate.sleep();
+  }
+
+  return (result == BT::NodeStatus::SUCCESS) ?
+         nav2_tasks::TaskStatus::SUCCEEDED : nav2_tasks::TaskStatus::FAILED;
+}
+
+}  // namespace nav2_mission_executor
diff --git a/nav2_mission_executor/src/main.cpp b/nav2_mission_executor/src/main.cpp
index 05d8887b..f6062cc9 100644
--- a/nav2_mission_executor/src/main.cpp
+++ b/nav2_mission_executor/src/main.cpp
@@ -19,7 +19,7 @@
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<nav2_mission_execution::MissionExecutor>());
+  rclcpp::spin(std::make_shared<nav2_mission_executor::MissionExecutor>());
   rclcpp::shutdown();
 
   return 0;
diff --git a/nav2_mission_executor/src/mission_executor.cpp b/nav2_mission_executor/src/mission_executor.cpp
index bcefbbf6..55ee9ef0 100644
--- a/nav2_mission_executor/src/mission_executor.cpp
+++ b/nav2_mission_executor/src/mission_executor.cpp
@@ -12,28 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
-#include <memory>
-#include <exception>
 #include "nav2_mission_executor/mission_executor.hpp"
+#include "nav2_mission_executor/execute_mission_behavior_tree.hpp"
 
-using namespace std::chrono_literals;
 using nav2_tasks::TaskStatus;
 
-namespace nav2_mission_execution
+namespace nav2_mission_executor
 {
 
 MissionExecutor::MissionExecutor()
 : nav2_tasks::ExecuteMissionTaskServer("ExecuteMissionNode")
 {
-  RCLCPP_INFO(get_logger(), "Initializing MissionExecutor.");
-  navTaskClient_ = std::make_unique<nav2_tasks::NavigateToPoseTaskClient>(this);
-
-  if (!navTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
-    RCLCPP_ERROR(get_logger(), "MissionExecutor: NavigateToPoseTaskServer not running!");
-    throw std::runtime_error("MissionExecutor: NavigateToPoseTaskServer not running");
-  }
-
   goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("move_base_simple/goal",
       std::bind(&MissionExecutor::onGoalPoseReceived, this, std::placeholders::_1));
 
@@ -41,78 +30,57 @@ MissionExecutor::MissionExecutor()
     "ExecuteMissionTask_command");
 }
 
-MissionExecutor::~MissionExecutor()
+TaskStatus MissionExecutor::execute(const nav2_tasks::ExecuteMissionCommand::SharedPtr command)
 {
-  RCLCPP_INFO(get_logger(), "Shutting down MissionExecutor");
+  RCLCPP_INFO(get_logger(), "Executing mission plan: %s", command->mission_plan.c_str());
+
+  // Create and run the behavior tree for this mission
+  ExecuteMissionBehaviorTree bt(shared_from_this());
+  TaskStatus result =
+    bt.run(command->mission_plan, std::bind(&MissionExecutor::cancelRequested, this));
+
+  RCLCPP_INFO(get_logger(), "Completed mission execution: %d", result);
+  return result;
 }
 
-void
-MissionExecutor::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+// Besides receving a mission plan on the command topic, another way to initiate the mission
+// is to respond to the goal message sent from rviz. In this case, we'll receive the incoming
+// message, dynamically create a mission plan with a single NavigateToPose, then publish the plan
+void MissionExecutor::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
 {
-  goal_pose_ = msg;
-
+  RCLCPP_DEBUG(get_logger(), "Received goal pose message");
+
+  // Get a reference for convenience
+  geometry_msgs::msg::Pose & p = msg->pose;
+
+  // Compose the args for the NavigateToPose action
+  std::stringstream args;
+  args << "position=\"" <<
+    p.position.x << ";" << p.position.y << ";" << p.position.z << "\"" <<
+    " orientation=\"" <<
+    p.orientation.x << ";" << p.orientation.y << ";" << p.orientation.z << ";" <<
+    p.orientation.w << "\"";
+
+  // Put it all together, trying to make the XML somewhat readable here
+  std::stringstream command_ss;
+  command_ss <<
+    R"(
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <SequenceStar name="root">
+      <NavigateToPose )" << args.str() <<
+    R"(/>
+    </SequenceStar>
+  </BehaviorTree>
+</root>)";
+
+  RCLCPP_INFO(get_logger(), "Publishing a mission plan: %s", command_ss.str().c_str());
+
+  // Publish the mission plan (so that our own TaskServer picks it up)
   auto message = nav2_msgs::msg::MissionPlan();
-  message.mission_plan = "Hello, world!";
+  message.mission_plan = command_ss.str();
 
-  RCLCPP_INFO(this->get_logger(), "MissionExecutor: Publishing a new mission plan");
   plan_pub_->publish(message);
 }
 
-TaskStatus
-MissionExecutor::execute(const nav2_tasks::ExecuteMissionCommand::SharedPtr command)
-{
-  RCLCPP_INFO(get_logger(), "MissionExecutor: Executing task: %s from %f",
-    command->mission_plan.c_str(), command->header.stamp);
-
-  // TODO(mjeronimo): Validate the mission plan for syntax and semantics
-
-  // TODO(mjeronimo): Get the goal pose from the task in the mission plan. For now, we're
-  // using the one received from rviz via the move_base_simple/goal topic.
-  navTaskClient_->sendCommand(goal_pose_);
-
-  auto navResult = std::make_shared<nav2_tasks::NavigateToPoseResult>();
-
-  // Loop until navigation reaches a terminal state
-  for (;; ) {
-    // Check to see if this task (mission execution) has been canceled. If so,
-    // cancel the navigation task first and then cancel this task
-    if (cancelRequested()) {
-      RCLCPP_INFO(get_logger(), "MissionExecutor: Task %s has been canceled.",
-        command->mission_plan.c_str());
-      navTaskClient_->cancel();
-      setCanceled();
-      return TaskStatus::CANCELED;
-    }
-
-    // This task hasn't been canceled, so see if the navigation task has finished
-    TaskStatus status = navTaskClient_->waitForResult(navResult, 100ms);
-
-    switch (status) {
-      case TaskStatus::SUCCEEDED:
-        {
-          RCLCPP_INFO(get_logger(), "MissionExecutor: Mission task %s completed",
-            command->mission_plan.c_str());
-
-          // No data to return from this task, just an empty result message
-          nav2_tasks::ExecuteMissionResult result;
-          setResult(result);
-
-          return TaskStatus::SUCCEEDED;
-        }
-
-      case TaskStatus::FAILED:
-        RCLCPP_ERROR(get_logger(), "MissionExecutor::execute: navigation task failed");
-        return TaskStatus::FAILED;
-
-      case TaskStatus::RUNNING:
-        RCLCPP_INFO(get_logger(), "MissionExecutor: Current mission task still running.");
-        break;
-
-      default:
-        RCLCPP_ERROR(get_logger(), "MissionExecutor: Invalid status value.");
-        throw std::logic_error("MissionExecutor::execute: invalid status value");
-    }
-  }
-}
-
-}  // namespace nav2_mission_execution
+}  // namespace nav2_mission_executor
diff --git a/nav2_robot/src/robot.cpp b/nav2_robot/src/robot.cpp
index 2ecd9d9e..dc3791c8 100644
--- a/nav2_robot/src/robot.cpp
+++ b/nav2_robot/src/robot.cpp
@@ -25,7 +25,6 @@ Robot::Robot(rclcpp::Node * node)
 : node_(node), initial_pose_received_(false), initial_odom_received_(false)
 {
   // TODO(mhpanah): Topic names for pose and odom should should be configured with parameters
-
   pose_sub_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", std::bind(&Robot::onPoseReceived, this, std::placeholders::_1));
 
@@ -58,7 +57,8 @@ bool
 Robot::getCurrentPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & robot_pose)
 {
   if (!initial_pose_received_) {
-    RCLCPP_WARN(node_->get_logger(), "Can't return current pose: Initial pose not yet received.");
+    RCLCPP_WARN(node_->get_logger(),
+      "Robot: Can't return current pose: Initial pose not yet received.");
     return false;
   }
 
@@ -70,8 +70,8 @@ bool
 Robot::getCurrentVelocity(nav_msgs::msg::Odometry::SharedPtr & robot_velocity)
 {
   if (!initial_odom_received_) {
-    RCLCPP_WARN(node_->get_logger(), "Can't return current velocity: Initial odometry not yet"
-      " received.");
+    RCLCPP_WARN(node_->get_logger(),
+      "Robot: Can't return current velocity: Initial odometry not yet received.");
     return false;
   }
 
diff --git a/nav2_simple_navigator/src/simple_navigator.cpp b/nav2_simple_navigator/src/simple_navigator.cpp
index f69b4205..16f56b7b 100644
--- a/nav2_simple_navigator/src/simple_navigator.cpp
+++ b/nav2_simple_navigator/src/simple_navigator.cpp
@@ -31,14 +31,21 @@ SimpleNavigator::SimpleNavigator()
 {
   RCLCPP_INFO(get_logger(), "Initializing");
 
-  plannerTaskClient_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(this);
+  // TODO(mjeronimo): Once the TaskServer accepts a shared_ptr, this will change
+  // to use the shared_ptr passed in the constructor. For now, there's a problem
+  // because the SimpleNavigate *is* a ROS node and we can't use shared_from_this
+  // in the constructor.
+  auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
+
+  plannerTaskClient_ =
+    std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(temp_node);
 
   if (!plannerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
     RCLCPP_ERROR(get_logger(), "Global planner is not running");
     throw std::runtime_error("Global planner not running");
   }
 
-  controllerTaskClient_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(this);
+  controllerTaskClient_ = std::make_unique<nav2_tasks::FollowPathTaskClient>(temp_node);
 
   if (!controllerTaskClient_->waitForServer(nav2_tasks::defaultServerTimeout)) {
     RCLCPP_ERROR(get_logger(), "Controller is not running");
@@ -57,23 +64,30 @@ SimpleNavigator::execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr comm
   RCLCPP_INFO(get_logger(), "Begin navigating to (%.2f, %.2f)",
     command->pose.position.x, command->pose.position.y);
 
-  // Compose the PathEndPoints message for Navigation. The starting pose comes from
-  // localization, while the goal pose is from the incoming command
-  auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
-
+  // Get the current pose from the robot
   geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose;
-  if (robot_.getCurrentPose(current_pose)) {
-    RCLCPP_DEBUG(get_logger(), "got robot pose");
-    endpoints->start = current_pose->pose.pose;
-    endpoints->goal = command->pose;
-    endpoints->tolerance = 2.0;
-  } else {
+
+  if (!robot_.getCurrentPose(current_pose)) {
     // TODO(mhpanah): use either last known pose, current pose from odom, wait, or try again.
     RCLCPP_WARN(get_logger(), "Current robot pose is not available.");
     return TaskStatus::FAILED;
   }
 
-  RCLCPP_INFO(get_logger(), "Requesting path from the planner server.");
+  // Create a PathEndPoints message for the global planner
+  auto endpoints = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
+  endpoints->start = current_pose->pose.pose;
+  endpoints->goal = command->pose;
+  endpoints->tolerance = 2.0;
+
+  RCLCPP_DEBUG(get_logger(), "Getting the path from the planner");
+  RCLCPP_DEBUG(get_logger(), "goal.position.x: %f", endpoints->goal.position.x);
+  RCLCPP_DEBUG(get_logger(), "goal.position.y: %f", endpoints->goal.position.y);
+  RCLCPP_DEBUG(get_logger(), "goal.position.z: %f", endpoints->goal.position.z);
+  RCLCPP_DEBUG(get_logger(), "goal.orientation.x: %f", endpoints->goal.orientation.x);
+  RCLCPP_DEBUG(get_logger(), "goal.orientation.y: %f", endpoints->goal.orientation.y);
+  RCLCPP_DEBUG(get_logger(), "goal.orientation.z: %f", endpoints->goal.orientation.z);
+  RCLCPP_DEBUG(get_logger(), "goal.orientation.w: %f", endpoints->goal.orientation.w);
+
   auto path = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
   plannerTaskClient_->sendCommand(endpoints);
 
@@ -101,6 +115,10 @@ SimpleNavigator::execute(const nav2_tasks::NavigateToPoseCommand::SharedPtr comm
         RCLCPP_ERROR(get_logger(), "Planning task failed.");
         return TaskStatus::FAILED;
 
+      case TaskStatus::CANCELED:
+        RCLCPP_INFO(get_logger(), "Planning task canceled");
+        break;
+
       case TaskStatus::RUNNING:
         RCLCPP_DEBUG(get_logger(), "Planning task still running.");
         break;
@@ -157,6 +175,10 @@ planning_succeeded:
         RCLCPP_ERROR(get_logger(), "Control task failed.");
         return TaskStatus::FAILED;
 
+      case TaskStatus::CANCELED:
+        RCLCPP_INFO(get_logger(), "Control task canceled");
+        break;
+
       case TaskStatus::RUNNING:
         RCLCPP_DEBUG(get_logger(), "Control task still running");
         break;
diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp
index bf4f42e2..6304a5eb 100644
--- a/nav2_system_tests/src/planning/planner_tester.cpp
+++ b/nav2_system_tests/src/planning/planner_tester.cpp
@@ -39,7 +39,10 @@ PlannerTester::PlannerTester()
   RCLCPP_INFO(this->get_logger(), "PlannerTester::PlannerTester");
 
   // Our client used to invoke the services of the global planner (ComputePathToPose)
-  planner_client_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(this);
+
+  auto temp_node = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *) {});
+
+  planner_client_ = std::make_unique<nav2_tasks::ComputePathToPoseTaskClient>(temp_node);
 
   if (!planner_client_->waitForServer(nav2_tasks::defaultServerTimeout)) {
     RCLCPP_ERROR(this->get_logger(), "PlannerTester::PlannerTester: planner not running");
diff --git a/nav2_tasks/CMakeLists.txt b/nav2_tasks/CMakeLists.txt
index 231c9730..dc44ccab 100644
--- a/nav2_tasks/CMakeLists.txt
+++ b/nav2_tasks/CMakeLists.txt
@@ -17,17 +17,31 @@ find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(behavior_tree_core REQUIRED)
 
 include_directories(
   include
 )
 
-install(DIRECTORY include/
-  DESTINATION include/
+add_library(nav2_tasks STATIC
+  src/behavior_tree_engine.cpp
 )
 
-set(dependencies
+ament_target_dependencies(nav2_tasks
+  rclcpp
+  geometry_msgs
   nav2_msgs
+  behavior_tree_core
+)
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
 )
 
 if(BUILD_TESTING)
diff --git a/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp b/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp
new file mode 100644
index 00000000..8d143d54
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/behavior_tree_engine.hpp
@@ -0,0 +1,51 @@
+// 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__BEHAVIOR_TREE_ENGINE_HPP_
+#define NAV2_TASKS__BEHAVIOR_TREE_ENGINE_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "behavior_tree_core/behavior_tree.h"
+#include "behavior_tree_core/bt_factory.h"
+#include "behavior_tree_core/xml_parsing.h"
+#include "nav2_tasks/task_status.hpp"
+
+namespace nav2_tasks
+{
+
+class BehaviorTreeEngine
+{
+public:
+  explicit BehaviorTreeEngine(rclcpp::Node::SharedPtr node);
+  BehaviorTreeEngine() = delete;
+
+  TaskStatus run(
+    const std::string & behavior_tree_xml,
+    std::function<bool()> cancelRequested,
+    std::chrono::milliseconds loopTimeout = std::chrono::milliseconds(100));
+
+protected:
+  virtual void registerCustomActions() {}
+
+  // The ROS node to use for any task clients
+  rclcpp::Node::SharedPtr node_;
+
+  // A factory that will be used to dynamically construct the behavior tree
+  BT::BehaviorTreeFactory factory_;
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__BEHAVIOR_TREE_ENGINE_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/bt_action_node.hpp b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
new file mode 100644
index 00000000..aa571693
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/bt_action_node.hpp
@@ -0,0 +1,123 @@
+// 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__BT_ACTION_NODE_HPP_
+#define NAV2_TASKS__BT_ACTION_NODE_HPP_
+
+#include <string>
+#include <memory>
+#include <condition_variable>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_tasks/task_client.hpp"
+#include "behavior_tree_core/action_node.h"
+#include "behavior_tree_core/bt_factory.h"
+
+namespace nav2_tasks
+{
+
+template<class CommandMsg, class ResultMsg>
+class BtActionNode : public BT::ActionNode
+{
+public:
+  explicit BtActionNode(const std::string & action_name)
+  : BT::ActionNode(action_name), task_client_(nullptr)
+  {
+  }
+
+  BtActionNode(const std::string & action_name, const BT::NodeParameters & params)
+  : BT::ActionNode(action_name, params), task_client_(nullptr)
+  {
+  }
+
+  BtActionNode() = delete;
+
+  virtual ~BtActionNode()
+  {
+  }
+
+  BT::NodeStatus tick() override
+  {
+    if (task_client_ == nullptr) {
+      // Get the required items from the blackboard
+      node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+      node_loop_timeout_ = blackboard()->template get<std::chrono::milliseconds>("node_loop_timeout");
+
+      // Now that we have the ROS node to use, create the task client for this action
+      task_client_ = std::make_unique<nav2_tasks::TaskClient<CommandMsg, ResultMsg>>(node_);
+    }
+
+    task_client_->sendCommand(command_);
+
+    // Loop until the task has completed
+    while (!isHalted()) {
+      nav2_tasks::TaskStatus status = task_client_->waitForResult(result_, node_loop_timeout_);
+
+      switch (status) {
+        case nav2_tasks::TaskStatus::SUCCEEDED:
+          return BT::NodeStatus::SUCCESS;
+
+        case nav2_tasks::TaskStatus::FAILED:
+          return BT::NodeStatus::FAILURE;
+
+        case nav2_tasks::TaskStatus::CANCELED:
+          cv_cancel_.notify_one();
+          return BT::NodeStatus::IDLE;
+
+        case nav2_tasks::TaskStatus::RUNNING:
+          break;
+
+        default:
+          throw std::logic_error("BtActionNode::Tick: invalid status value");
+      }
+    }
+
+    return BT::NodeStatus::IDLE;
+  }
+
+  void halt() override
+  {
+    // Send a cancel message to the task server
+    task_client_->cancel();
+
+    // Then wait for the response before continuing
+    std::unique_lock<std::mutex> lock(cancel_mutex_);
+    cv_cancel_.wait(lock);
+  }
+
+protected:
+  typename std::unique_ptr<nav2_tasks::TaskClient<CommandMsg, ResultMsg>> task_client_;
+
+  // 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_;
+
+  typename CommandMsg::SharedPtr command_;
+  typename ResultMsg::SharedPtr result_;
+
+  typedef typename CommandMsg::SharedPtr CommandMsgPtr;
+  typedef typename ResultMsg::SharedPtr ResultMsgPtr;
+
+  // Allow for signaling receipt of the cancel message
+  std::mutex cancel_mutex_;
+  std::condition_variable cv_cancel_;
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__BT_ACTION_NODE_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/bt_conversions.hpp b/nav2_tasks/include/nav2_tasks/bt_conversions.hpp
new file mode 100644
index 00000000..fffc9b22
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/bt_conversions.hpp
@@ -0,0 +1,76 @@
+// 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__BT_CONVERSIONS_HPP_
+#define NAV2_TASKS__BT_CONVERSIONS_HPP_
+
+#include <string>
+#include "behavior_tree_core/behavior_tree.h"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+namespace BT
+{
+
+// The next two conversion functions are required to be defined by the BT library,
+// but are not actually called. TODO(mjeronimo): See if we can avoid these.
+
+template<>
+inline rclcpp::Node::SharedPtr convertFromString(const std::string & /*key*/)
+{
+  return nullptr;
+}
+
+template<>
+inline std::chrono::milliseconds convertFromString(const std::string & /*key*/)
+{
+  return std::chrono::milliseconds(0);
+}
+
+template<>
+inline geometry_msgs::msg::Point convertFromString(const std::string & key)
+{
+  // three real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() != 3) {
+    throw std::runtime_error("invalid number of fields for point attribute)");
+  } else {
+    geometry_msgs::msg::Point position;
+    position.x = BT::convertFromString<double>(parts[0]);
+    position.y = BT::convertFromString<double>(parts[1]);
+    position.z = BT::convertFromString<double>(parts[2]);
+    return position;
+  }
+}
+
+template<>
+inline geometry_msgs::msg::Quaternion convertFromString(const std::string & key)
+{
+  // three real numbers separated by semicolons
+  auto parts = BT::splitString(key, ';');
+  if (parts.size() != 4) {
+    throw std::runtime_error("invalid number of fields for orientation attribute)");
+  } else {
+    geometry_msgs::msg::Quaternion orientation;
+    orientation.x = BT::convertFromString<double>(parts[0]);
+    orientation.y = BT::convertFromString<double>(parts[1]);
+    orientation.z = BT::convertFromString<double>(parts[2]);
+    orientation.w = BT::convertFromString<double>(parts[3]);
+    return orientation;
+  }
+}
+
+}  // namespace BT
+
+#endif  // NAV2_TASKS__BT_CONVERSIONS_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp
new file mode 100644
index 00000000..1eb23b91
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_action.hpp
@@ -0,0 +1,88 @@
+// 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__COMPUTE_PATH_TO_POSE_ACTION_HPP_
+#define NAV2_TASKS__COMPUTE_PATH_TO_POSE_ACTION_HPP_
+
+#include <string>
+#include <memory>
+#include "nav2_tasks/bt_conversions.hpp"
+#include "nav2_tasks/bt_action_node.hpp"
+#include "nav2_tasks/compute_path_to_pose_task.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+namespace nav2_tasks
+{
+
+class ComputePathToPoseAction
+  : public BtActionNode<ComputePathToPoseCommand, ComputePathToPoseResult>
+{
+public:
+  ComputePathToPoseAction(const std::string & action_name, const BT::NodeParameters & params)
+  : BtActionNode<ComputePathToPoseCommand, ComputePathToPoseResult>(action_name, params)
+  {
+    // Get the starting pose information from the XML attributes
+
+    geometry_msgs::msg::Point start_position;
+    bool have_start_position =
+      getParam<geometry_msgs::msg::Point>("start_position", start_position);
+
+    geometry_msgs::msg::Quaternion start_orientation;
+    bool have_start_orientation =
+      getParam<geometry_msgs::msg::Quaternion>("start_orientation", start_orientation);
+
+    if (!have_start_position || !have_start_orientation) {
+      RCLCPP_ERROR(node_->get_logger(),
+        "ComputePathToPoseAction: starting position or orientation not provided");
+    }
+
+    // Get the ending pose information from the XML attributes
+
+    geometry_msgs::msg::Point goal_position;
+    bool have_goal_position = getParam<geometry_msgs::msg::Point>("goal_position", goal_position);
+
+    geometry_msgs::msg::Quaternion goal_orientation;
+    bool have_goal_orientation =
+      getParam<geometry_msgs::msg::Quaternion>("goal_orientation", goal_orientation);
+
+    if (!have_goal_position || !have_goal_orientation) {
+      RCLCPP_ERROR(node_->get_logger(),
+        "ComputePathToPoseAction: goal position or orientation not provided");
+    }
+
+    // Create the command message for this task
+    command_ = std::make_shared<nav2_tasks::ComputePathToPoseCommand>();
+    command_->start.position = start_position;
+    command_->start.orientation = start_orientation;
+    command_->goal.position = goal_position;
+    command_->goal.orientation = goal_orientation;
+
+    // Create the result message
+    result_ = std::make_shared<nav2_tasks::ComputePathToPoseResult>();
+  }
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {
+      {"start_position", "0;0;0"}, {"start_orientation", "0;0;0;0"},
+      {"goal_position", "0;0;0"}, {"goal_orientation", "0;0;0;0"}};
+    return params;
+  }
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__COMPUTE_PATH_TO_POSE_ACTION_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/execute_mission_action.hpp b/nav2_tasks/include/nav2_tasks/execute_mission_action.hpp
new file mode 100644
index 00000000..81ff5079
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/execute_mission_action.hpp
@@ -0,0 +1,29 @@
+// 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__EXECUTE_MISSION_ACTION_HPP_
+#define NAV2_TASKS__EXECUTE_MISSION_ACTION_HPP_
+
+#include "nav2_tasks/bt_action_node.hpp"
+#include "nav2_tasks/execute_mission_task.hpp"
+
+namespace nav2_tasks
+{
+
+using ExecuteMissionAction =
+  BtActionNode<ExecuteMissionCommand, ExecuteMissionResult>;
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__EXECUTE_MISSION_ACTION_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/follow_path_action.hpp b/nav2_tasks/include/nav2_tasks/follow_path_action.hpp
new file mode 100644
index 00000000..d612a519
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/follow_path_action.hpp
@@ -0,0 +1,43 @@
+// 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__FOLLOW_PATH_ACTION_HPP_
+#define NAV2_TASKS__FOLLOW_PATH_ACTION_HPP_
+
+#include <string>
+#include <memory>
+#include "nav2_tasks/bt_conversions.hpp"
+#include "nav2_tasks/bt_action_node.hpp"
+#include "nav2_tasks/follow_path_task.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+namespace nav2_tasks
+{
+
+class FollowPathAction : public BtActionNode<FollowPathCommand, FollowPathResult>
+{
+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>();
+    result_ = std::make_shared<nav2_tasks::FollowPathResult>();
+  }
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__FOLLOW_PATH_ACTION_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/navigate_to_pose_action.hpp b/nav2_tasks/include/nav2_tasks/navigate_to_pose_action.hpp
new file mode 100644
index 00000000..c0aaefe2
--- /dev/null
+++ b/nav2_tasks/include/nav2_tasks/navigate_to_pose_action.hpp
@@ -0,0 +1,66 @@
+// 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__NAVIGATE_TO_POSE_ACTION_HPP_
+#define NAV2_TASKS__NAVIGATE_TO_POSE_ACTION_HPP_
+
+#include <string>
+#include <memory>
+#include "nav2_tasks/bt_conversions.hpp"
+#include "nav2_tasks/bt_action_node.hpp"
+#include "nav2_tasks/navigate_to_pose_task.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+
+namespace nav2_tasks
+{
+
+class NavigateToPoseAction : public BtActionNode<NavigateToPoseCommand, NavigateToPoseResult>
+{
+public:
+  NavigateToPoseAction(const std::string & action_name, const BT::NodeParameters & params)
+  : BtActionNode<NavigateToPoseCommand, NavigateToPoseResult>(action_name, params)
+  {
+    // Use the position and orientation fields from the XML attributes
+    geometry_msgs::msg::Point position;
+    bool have_position = getParam<geometry_msgs::msg::Point>("position", position);
+
+    geometry_msgs::msg::Quaternion orientation;
+    bool have_orientation = getParam<geometry_msgs::msg::Quaternion>("orientation", orientation);
+
+    if (!have_position || !have_orientation) {
+      RCLCPP_ERROR(node_->get_logger(),
+        "NavigateToPoseAction: position or orientation not provided");
+    }
+
+    // Create the command message for this task
+    command_ = std::make_shared<nav2_tasks::NavigateToPoseCommand>();
+    command_->pose.position = position;
+    command_->pose.orientation = orientation;
+
+    // Create the result message
+    result_ = std::make_shared<nav2_tasks::NavigateToPoseResult>();
+  }
+
+  // Any BT node that accepts parameters must provide a requiredNodeParameters method
+  static const BT::NodeParameters & requiredNodeParameters()
+  {
+    static BT::NodeParameters params = {{"position", "0;0;0"}, {"orientation", "0;0;0;0"}};
+    return params;
+  }
+};
+
+}  // namespace nav2_tasks
+
+#endif  // NAV2_TASKS__NAVIGATE_TO_POSE_ACTION_HPP_
diff --git a/nav2_tasks/include/nav2_tasks/task_client.hpp b/nav2_tasks/include/nav2_tasks/task_client.hpp
index d4a0e878..27047c66 100644
--- a/nav2_tasks/include/nav2_tasks/task_client.hpp
+++ b/nav2_tasks/include/nav2_tasks/task_client.hpp
@@ -19,6 +19,8 @@
 #include <condition_variable>
 #include <string>
 #include <thread>
+#include <memory>
+
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/empty.hpp"
 #include "nav2_tasks/task_status.hpp"
@@ -35,9 +37,14 @@ template<class CommandMsg, class ResultMsg>
 class TaskClient
 {
 public:
-  explicit TaskClient(rclcpp::Node * node)
+  explicit TaskClient(rclcpp::Node::SharedPtr node)
   : node_(node)
   {
+    resultReceived_ = false;
+    statusReceived_ = false;
+
+    statusMsg_ = std::make_shared<StatusMsg>();
+
     std::string taskName = getTaskName<CommandMsg, ResultMsg>();
 
     // Create the publishers
@@ -81,7 +88,7 @@ public:
 
     // TODO(mjeronimo): Replace this with a legit way to wait for the server
     while (node_->count_subscribers(taskName) < 1) {
-      rclcpp::spin_some(node_->get_node_base_interface());
+      rclcpp::spin_some(node_);
 
       auto t1 = std::chrono::high_resolution_clock::now();
       auto elapsedTime = t1 - t0;
@@ -175,7 +182,7 @@ protected:
     cvResult_.notify_one();
   }
 
-  // Called when the TaskServer sends it status code (success or failure)
+  // Called when the TaskServer sends its status code (success or failure)
   void onStatusReceived(const StatusMsg::SharedPtr statusMsg)
   {
     {
@@ -188,7 +195,7 @@ protected:
   }
 
   // The TaskClient isn't itself a node, so needs to know which one to use
-  rclcpp::Node * node_;
+  rclcpp::Node::SharedPtr node_;
 
   // The client's publishers: the command and cancel messages
   typename rclcpp::Publisher<CommandMsg>::SharedPtr commandPub_;
diff --git a/nav2_tasks/include/nav2_tasks/task_server.hpp b/nav2_tasks/include/nav2_tasks/task_server.hpp
index ed60568c..a1a598c2 100644
--- a/nav2_tasks/include/nav2_tasks/task_server.hpp
+++ b/nav2_tasks/include/nav2_tasks/task_server.hpp
@@ -139,6 +139,8 @@ protected:
           // Or the canceled code
           statusMsg.result = nav2_msgs::msg::TaskStatus::CANCELED;
           statusPub_->publish(statusMsg);
+
+          cancelReceived_ = false;
         } else {
           throw std::logic_error("Unexpected status return from task");
         }
diff --git a/nav2_tasks/package.xml b/nav2_tasks/package.xml
index b7acbc6b..681f8693 100644
--- a/nav2_tasks/package.xml
+++ b/nav2_tasks/package.xml
@@ -15,6 +15,7 @@
   <build_depend>rosidl_default_generators</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav2_msgs</build_depend>
+  <build_depend>behavior_tree_core</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
@@ -22,6 +23,7 @@
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>behavior_tree_core</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_tasks/src/behavior_tree_engine.cpp b/nav2_tasks/src/behavior_tree_engine.cpp
new file mode 100644
index 00000000..2b48b543
--- /dev/null
+++ b/nav2_tasks/src/behavior_tree_engine.cpp
@@ -0,0 +1,73 @@
+// 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.
+
+#include "nav2_tasks/behavior_tree_engine.hpp"
+
+#include <memory>
+#include <string>
+#include <set>
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "Blackboard/blackboard_local.h"
+#include "nav2_tasks/navigate_to_pose_action.hpp"
+#include "nav2_tasks/bt_conversions.hpp"
+
+using namespace std::chrono_literals;
+
+namespace nav2_tasks
+{
+
+BehaviorTreeEngine::BehaviorTreeEngine(rclcpp::Node::SharedPtr node)
+: node_(node)
+{
+  // Register any custom action nodes so that they can be included in XML description
+  registerCustomActions();
+}
+
+TaskStatus BehaviorTreeEngine::run(
+  const std::string & behavior_tree_xml,
+  std::function<bool()> cancelRequested,
+  std::chrono::milliseconds loopTimeout)
+{
+  // Create the blackboard that will be shared by all of the nodes in the tree
+  BT::Blackboard::Ptr blackboard = BT::Blackboard::create<BT::BlackboardLocal>();
+
+  // Set a couple values that all of the action nodes expect/require
+  blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
+  blackboard->set<std::chrono::milliseconds>("tick_timeout", std::chrono::milliseconds(100));
+
+  // The complete behavior tree that results from parsing the incoming XML. When the tree goes
+  // out of scope, all the nodes are destroyed
+  BT::Tree tree = BT::buildTreeFromText(factory_, behavior_tree_xml, blackboard);
+
+  rclcpp::WallRate loopRate(loopTimeout);
+  BT::NodeStatus result = BT::NodeStatus::RUNNING;
+
+  // Loop until something happens with ROS or the node completes w/ success or failure
+  while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) {
+    result = tree.root_node->executeTick();
+
+    // Check if we've received a cancel message
+    if (cancelRequested()) {
+      tree.root_node->halt();
+      return TaskStatus::CANCELED;
+    }
+
+    loopRate.sleep();
+  }
+
+  return (result == BT::NodeStatus::SUCCESS) ?
+         TaskStatus::SUCCEEDED : TaskStatus::FAILED;
+}
+
+}  // namespace nav2_tasks
diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos
index 28462590..0da13e0f 100644
--- a/tools/ros2_dependencies.repos
+++ b/tools/ros2_dependencies.repos
@@ -19,3 +19,7 @@ repositories:
     type: git
     url: https://github.com/bpwilcox/xmlrpcpp.git
     version: master
+  behavior_tree_core:
+    type: git
+    url: https://github.com/mjeronimo/BehaviorTree.CPP
+    version: master
-- 
GitLab