DriveToWaypointCommand.h

#pragma once
 
#include "../navtypes.h"
#include "../utils/time.h"
#include "../world_interface/data.h"
#include "CommandBase.h"
 
#include <chrono>
#include <optional>
 
namespace commands {

class DriveToWaypointCommand : CommandBase {
public:
    DriveToWaypointCommand(const navtypes::point_t& target, double thetaKP, double driveVel,
                           double slowDriveThresh, double doneThresh,
                           util::dseconds closeToTargetDur);

    void setState(const navtypes::pose_t& pose, robot::types::datatime_t time);

    command_t getOutput() override;

    bool isDone() override;
 
private:
    navtypes::point_t target;
    navtypes::pose_t pose;
    double thetaKP;
    double driveVel;
    double slowDriveThresh;
    double doneThresh;
    bool setStateCalledBeforeOutput;
    util::dseconds closeToTargetDur;
    std::optional<robot::types::datatime_t> lastRecordedTime;
    std::optional<robot::types::datatime_t> closeToTargetStartTime;
};
 
} // namespace commands