MissionControlTasks.h

#pragma once
 
#include "../utils/scheduler.h"
#include "../video/H264Encoder.h"
#include "../world_interface/data.h"
#include "../world_interface/world_interface.h"
#include "websocket/WebSocketServer.h"
 
#include <memory>
#include <mutex>
#include <optional>
#include <unordered_map>
#include <utility>
 
namespace net::mc::tasks {

class PowerRepeatTask : public util::PeriodicTask<> {
public:
    PowerRepeatTask();

    void setJointPower(robot::types::jointid_t id, double power);

    void setCmdVel(double steerVel, double xVel);

    void setTankCmdVel(double steerVel, double xVel);
 
    void start() override;
 
    void stop() override;
 
private:
    std::mutex _mutex;
    std::unordered_map<robot::types::jointid_t, double> _last_joint_power;
    // if not present, then there is no last requested drive power
    std::optional<std::pair<double, double>> _last_cmd_vel;
    bool _tank;
 
    void periodicTask();
};

class CameraStreamTask : public util::AsyncTask<> {
public:
    CameraStreamTask(websocket::SingleClientWSServer& server);

    void openStream(const robot::types::CameraID& cam, int fps);

    void closeStream(const robot::types::CameraID& cam);
 
protected:
    void task(std::unique_lock<std::mutex>& lock) override;
 
private:
    struct stream_data_t {
        std::shared_ptr<video::H264Encoder> encoder;
        std::shared_ptr<robot::types::CameraHandle> cam_handle;
        uint32_t frame_num;
 
        stream_data_t(std::shared_ptr<video::H264Encoder> encoder,
                      std::shared_ptr<robot::types::CameraHandle> cam_handle)
            : encoder(encoder), cam_handle(cam_handle), frame_num(0) {}
    };
 
 
    websocket::SingleClientWSServer& _server;
    std::mutex _mutex;
    std::unordered_map<robot::types::CameraID, stream_data_t> _open_streams;
};

class TelemReportTask : public util::PeriodicTask<> {
public:
    TelemReportTask(websocket::SingleClientWSServer& server);
 
private:
    websocket::SingleClientWSServer& _server;
 
    void sendTelemetry();
};

class ArmIKTask : public util::PeriodicTask<> {
public:
    ArmIKTask(websocket::SingleClientWSServer& server);
 
private:
    std::mutex _mutex;
    websocket::SingleClientWSServer& _server;
 
    void updateArmIK();
};
 
} // namespace net::mc::tasks