43 lines
1.2 KiB
C++
43 lines
1.2 KiB
C++
#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();
|
|
}
|