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

@@ -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);

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}

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

nav2_bt_navigator/include/nav2_bt_navigator/navigate_to_pose_behavior_tree.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_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_

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>

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

nav2_bt_navigator/src/navigate_to_pose_behavior_tree.cpp

+// 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

nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.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_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_

nav2_controller/nav2_controller_example/src/dwa_controller.cpp

+// 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

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()
+

nav2_mission_executor/include/nav2_mission_executor/execute_mission_behavior_tree.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_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_

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_

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>

nav2_mission_executor/src/execute_mission_behavior_tree.cpp

+// 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

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;

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

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;
   }
 

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;

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");

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)