Migration to BT.cpp v4 & new BT executor

rbs_bt_executor/src/TreeRunner.cpp

@@ -0,0 +1,43 @@
+#include <behaviortree_ros2/tree_execution_server.hpp>
+#include <behaviortree_cpp/loggers/bt_cout_logger.h>
+
+// #include <rclcpp/logging.hpp>
+// #include <std_msgs/msg/float32.hpp>
+
+class RbsBtExecutor : public BT::TreeExecutionServer {
+public:
+  RbsBtExecutor(const rclcpp::NodeOptions &options)
+      : TreeExecutionServer(options) {}
+
+  void onTreeCreated(BT::Tree &tree) override {
+    logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
+  }
+
+  std::optional<std::string>
+  onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override {
+
+    // RCLCPP_INFO(logger_cout_, "Tree completed with code: %d", status );
+
+    logger_cout_.reset();
+    return std::nullopt;
+  }
+
+private:
+  std::shared_ptr<BT::StdCoutLogger> logger_cout_;
+  // rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_;
+};
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+
+  rclcpp::NodeOptions options;
+  auto action_server = std::make_shared<RbsBtExecutor>(options);
+
+  rclcpp::executors::MultiThreadedExecutor exec(
+      rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
+  exec.add_node(action_server->node());
+  exec.spin();
+  exec.remove_node(action_server->node());
+
+  rclcpp::shutdown();
+}