bt : Use RosActionNode instead and split out SleepBtNode

ros2_ws/src/work_bt/CMakeLists.txt

@@ -11,17 +11,22 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rover_interfaces REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}_interfaces
   "msg/WorkResult.msg"
   "action/DoWork.action"
+  "action/EmptyAction.action"
+  DEPENDENCIES std_msgs
 )
 
 add_executable (excavator_bt src/main.cpp)
-ament_target_dependencies(excavator_bt rclcpp rclcpp_action behaviortree_cpp)
+ament_target_dependencies(excavator_bt rclcpp rclcpp_action behaviortree_cpp behaviortree_ros2 rover_interfaces)
 rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME}_interfaces "rosidl_typesupport_cpp")
 target_link_libraries(excavator_bt "${cpp_typesupport_target}")
 

ros2_ws/src/work_bt/action/EmptyAction.action

@@ -0,0 +1,2 @@
+---
+---

ros2_ws/src/work_bt/package.xml

@@ -10,6 +10,8 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>behaviortree_cpp</depend>
+  <depend>behaviortree_ros2</depend>
+  <depend>rover_interfaces</depend>
 
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>

ros2_ws/src/work_bt/src/main.cpp

@@ -1,101 +1,85 @@
 #include <functional>
 #include <thread>
+#include <memory>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
 
 // #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
 
+#include "rover_interfaces/action/navigate_to_position.hpp"
 #include "work_bt/action/do_work.hpp"
+#include "work_bt/action/empty_action.hpp"
+
+#include "sleep_bt_node.hpp"
+#include "simple_ros_action_node.hpp"
 
 using DoWork = work_bt::action::DoWork;
+using EmptyAction = work_bt::action::EmptyAction;
+using NavigateToPosition = rover_interfaces::action::NavigateToPosition;
 using namespace std::placeholders;
 using namespace std::chrono_literals;
 
-class SleepBtNode : public BT::StatefulActionNode {
-public:
-  SleepBtNode(const std::string& name, const BT::NodeConfig& config, rclcpp::Node& node, std::chrono::milliseconds duration)
-      : BT::StatefulActionNode(name, config), node(node), duration(duration) {
-  }
-
-  BT::NodeStatus onStart() override {
-    start_time = std::chrono::system_clock::now();
-    return BT::NodeStatus::RUNNING;
-  }
-
-  BT::NodeStatus onRunning() override {
-    RCLCPP_INFO_THROTTLE(node.get_logger(), *node.get_clock(), 500, "Running %s", this->name().c_str());
-    if (std::chrono::system_clock::now() - start_time > duration) {
-      return BT::NodeStatus::SUCCESS;
-    } else {
-      return BT::NodeStatus::RUNNING;
-    }
-  }
-
-  void onHalted() override {}
-
-  /* Required implementation */
-  static BT::PortsList providedPorts()
-  {
-    return {};
-  }
-
-private:
-  rclcpp::Node& node;
-  std::chrono::system_clock::time_point start_time;
-  std::chrono::milliseconds duration;
-};
-
-class ExcavatorWork : public rclcpp::Node {
+/**
+ * @brief We do not use TreeExecutionServer because we have complex cancellation logic.
+ * We follow TreeExecutionServer in keeping a member Node instead of inheriting from Node
+ *
+ */
+class ExcavatorWork {
 public:
-  ExcavatorWork() : Node("excavator_bt") {
+  ExcavatorWork() : node(std::make_shared<rclcpp::Node>("excavator_bt")) {
     setup(factory);
-    this->declare_parameter("bt_xml", std::string("my_tree.xml"));
+    node->declare_parameter("bt_xml", std::string("my_tree.xml"));
 
     is_working = false;
     is_cancelling = false;
 
-    std::string bt_xml = this->get_parameter("bt_xml").as_string();
+    std::string bt_xml = node->get_parameter("bt_xml").as_string();
     factory.registerBehaviorTreeFromFile(bt_xml);
 
     bt_server = rclcpp_action::create_server<DoWork>(
-        this, "do_work", std::bind(&ExcavatorWork::handle_goal, this, _1, _2),
+        node, "do_work", std::bind(&ExcavatorWork::handle_goal, this, _1, _2),
         std::bind(&ExcavatorWork::handle_cancel, this, _1),
         std::bind(&ExcavatorWork::handle_accepted, this, _1)
     );
   }
 
+  std::shared_ptr<rclcpp::Node> get_node() {
+    return node;
+  }
+
 private:
   rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid,
                    std::shared_ptr<const DoWork::Goal> goal) {
     if (active_goal_handle) {
-      RCLCPP_WARN(this->get_logger(), "Received work goal but already working");
+      RCLCPP_WARN(node->get_logger(), "Received work goal but already working");
       return rclcpp_action::GoalResponse::REJECT;
     }
-    RCLCPP_INFO(this->get_logger(), "Received new work goal");
+    RCLCPP_INFO(node->get_logger(), "Received new work goal");
     return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
   }
   rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<DoWork>> goal_handle) {
     std::scoped_lock<std::mutex> lock(mtx);
-    RCLCPP_INFO(this->get_logger(), "Received cancel request");
+    RCLCPP_INFO(node->get_logger(), "Received cancel request");
     if (!active_goal_handle) {
-      RCLCPP_WARN(this->get_logger(), "No active goal, rejecting cancel request");
+      RCLCPP_WARN(node->get_logger(), "No active goal, rejecting cancel request");
       return rclcpp_action::CancelResponse::REJECT;
     } else if (active_goal_handle != goal_handle) {
-      RCLCPP_ERROR(this->get_logger(), "Received cancel request for different goal, this shouldn't happen...");
+      RCLCPP_ERROR(node->get_logger(), "Received cancel request for different goal, this shouldn't happen...");
       return rclcpp_action::CancelResponse::REJECT;
     } else if (is_cancelling) {
-      RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Already cancelling, accepting duplicate cancel request");
+      RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Already cancelling, accepting duplicate cancel request");
       return rclcpp_action::CancelResponse::ACCEPT;
     }
     assert(active_goal_handle == goal_handle);
     is_cancelling = true;
     std::thread(&ExcavatorWork::cancel, this).detach();
     return rclcpp_action::CancelResponse::ACCEPT;
   }
+
   void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<DoWork>> goal_handle) {
-    RCLCPP_INFO(this->get_logger(), "Start executing goal...");
+    RCLCPP_INFO(node->get_logger(), "Start executing goal...");
     std::scoped_lock<std::mutex> lock(mtx);
     active_goal_handle = goal_handle;
     is_working = true;
@@ -115,7 +99,7 @@ class ExcavatorWork : public rclcpp::Node {
           work_bt.haltTree();
           return;
         }
-        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Ticking work_bt");
+        RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Ticking work_bt");
         if (work_bt.tickOnce() != BT::NodeStatus::RUNNING) {
           return;
         }
@@ -134,7 +118,7 @@ class ExcavatorWork : public rclcpp::Node {
     while (rclcpp::ok()) {
       {
         std::scoped_lock<std::mutex> lock(mtx);
-        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Ticking stow_bt");
+        RCLCPP_INFO_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "Ticking stow_bt");
         if (stow_bt.tickOnce() != BT::NodeStatus::RUNNING) {
           is_cancelling = false;
           /* A successful cancel is treated as a succeeded goal */
@@ -153,6 +137,7 @@ class ExcavatorWork : public rclcpp::Node {
     active_goal_handle.reset();
   }
 
+  std::shared_ptr<rclcpp::Node> node;
   rclcpp_action::Server<DoWork>::SharedPtr bt_server;
   std::shared_ptr<rclcpp_action::ServerGoalHandle<DoWork>> active_goal_handle;
 
@@ -162,28 +147,39 @@ class ExcavatorWork : public rclcpp::Node {
 
   /********** BT Functions **********/
   BT::NodeStatus SendSuccess(BT::TreeNode &self) {
-    RCLCPP_INFO(this->get_logger(), "SendSuccess");
+    RCLCPP_INFO(node->get_logger(), "SendSuccess");
     send_result(work_bt::msg::WorkResult::SUCCESS);
     return BT::NodeStatus::SUCCESS;
   }
 
   BT::NodeStatus Fail(BT::TreeNode &self) {
-    RCLCPP_INFO(this->get_logger(), "Fail");
+    RCLCPP_INFO(node->get_logger(), "Fail");
     send_result(work_bt::msg::WorkResult::FAILURE);
     return BT::NodeStatus::FAILURE;
   }
 
   BT::NodeStatus CriticalFail(BT::TreeNode &self) {
-    RCLCPP_INFO(this->get_logger(), "CriticalFail");
+    RCLCPP_INFO(node->get_logger(), "CriticalFail");
     send_result(work_bt::msg::WorkResult::CRITICAL_FAILURE);
     return BT::NodeStatus::FAILURE;
   }
 
   void setup(BT::BehaviorTreeFactory& factory) {
-    factory.registerNodeType<SleepBtNode>("GoToWorksite", std::ref(*this), 5000ms);
-    factory.registerNodeType<SleepBtNode>("AssumeReadyPose", std::ref(*this), 2000ms);
-    factory.registerNodeType<SleepBtNode>("Excavate", std::ref(*this), 10000ms);
-    factory.registerNodeType<SleepBtNode>("Stow", std::ref(*this), 2000ms);
+    factory.registerNodeType<SimpleRosActionNode<NavigateToPosition>>("GoToWorksite", node, "navigate_to_position", []() {
+      auto goal = NavigateToPosition::Goal();
+      goal.target.position.latitude = 80.0;
+      goal.target.position.longitude = 80.0;
+      return goal;
+    });
+    factory.registerNodeType<SimpleRosActionNode<EmptyAction>>("AssumeReadyPose", node, "assume_ready_pose", []() {
+      return EmptyAction::Goal();
+    });
+    factory.registerNodeType<SimpleRosActionNode<EmptyAction>>("Excavate", node, "excavate", []() {
+      return EmptyAction::Goal();
+    });
+    factory.registerNodeType<SimpleRosActionNode<EmptyAction>>("Stow", node, "stow", []() {
+      return EmptyAction::Goal();
+    });
     factory.registerSimpleAction("SendSuccess", std::bind(&ExcavatorWork::SendSuccess, this, _1));
     factory.registerSimpleAction("Fail", std::bind(&ExcavatorWork::Fail, this, _1));
     factory.registerSimpleAction("CriticalFail", std::bind(&ExcavatorWork::CriticalFail, this, _1));
@@ -196,7 +192,8 @@ class ExcavatorWork : public rclcpp::Node {
 
 int main(int argc, char * argv[]) {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<ExcavatorWork>());
+  auto excavator_work = std::make_shared<ExcavatorWork>();
+  rclcpp::spin(excavator_work->get_node());
   rclcpp::shutdown();
   return 0;
 

ros2_ws/src/work_bt/src/simple_ros_action_node.hpp

@@ -0,0 +1,30 @@
+#pragma once
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "work_bt/action/bt_action_client.hpp"
+
+template<typename ActionType>
+class SimpleRosActionNode : public BT::RosActionNode<ActionType> {
+using RosActionNode = BT::RosActionNode<ActionType>;
+public:
+  SimpleRosActionNode(const std::string& name, const BT::NodeConfig& config, std::shared_ptr<rclcpp::Node> node, std::string action_server_name, std::function<typename ActionType::Goal()> goal_generator)
+      : RosActionNode(name, config, BT::RosNodeParams(node, action_server_name)), node(node), goal_generator(goal_generator) {
+  }
+
+  bool setGoal(typename RosActionNode::Goal& goal) override {
+    goal = goal_generator();
+    return true;
+  }
+
+  BT::NodeStatus onResultReceived(const typename RosActionNode::WrappedResult &result) override {
+    return BT::NodeStatus::SUCCESS;
+  }
+
+private:
+  std::shared_ptr<rclcpp::Node> node;
+  std::function<typename ActionType::Goal()> goal_generator;
+};