modelec-marcel-ROS/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp

#pragma once

#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <modelec_strat/action_executor.hpp>

namespace Modelec {
    class TakeMission : public Mission {
    public:
        TakeMission(const std::shared_ptr<NavigationHelper>& nav,
                      const std::shared_ptr<ActionExecutor>& action_executor);

        void Start(rclcpp::Node::SharedPtr node) override;
        void Update() override;
        void Clear() override;
        MissionStatus GetStatus() const override;
        std::string GetName() const override { return "Take"; }

    private:
        enum Step
        {
            GO_TO_TAKE,
            GO_TO_TAKE_CLOSE,
            DOWN,
            TAKE,
            UP,
            DONE,
        } step_;

        std::shared_ptr<BoxObstacle> closestBox;
        MissionStatus status_;
        std::shared_ptr<NavigationHelper> nav_;
        std::shared_ptr<ActionExecutor> action_executor_;
        rclcpp::Node::SharedPtr node_;

        rclcpp::Time go_timeout_;
        rclcpp::Time deploy_time_;
    };
}