//
// Created by bastian on 26.03.22.
//

#ifndef BUILD_ROBOTMOVE_H
#define BUILD_ROBOTMOVE_H

#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "MoveConnection.h"

namespace BT {

    class RobotMove : public StatefulActionNode {
    private:
        std::shared_ptr<MoveConnection> connection;
        NodeStatus asyncResult = NodeStatus::FAILURE;

    public:
        RobotMove(const std::string &name, const BT::NodeConfiguration &config,
                  std::shared_ptr<MoveConnection> connection);

        NodeStatus onStart() override;

        NodeStatus onRunning() override;

        void onHalted() override;

        static PortsList providedPorts();
    };
}

#endif //BUILD_ROBOTMOVE_H
