world_interface.h

#pragma once
 
#include "../gps/gps_util.h"
#include "../kinematics/DiffDriveKinematics.h"
#include "../navtypes.h"
#include "../camera/Camera.h"
#include "../camera/CameraParams.h"
#include "../network/websocket/WebSocketServer.h"
#include "data.h"
#include "motor/base_motor.h"
 
#include <array>
#include <optional>
#include <unordered_set>

namespace robot {
 
namespace types {
class CameraHandle {
public:
    CameraHandle(std::shared_ptr<cam::Camera> cam) : camera(cam) {}
private:
    std::shared_ptr<cam::Camera> camera;
};
}

enum class WorldInterface {
    real,
    sim2d,
    sim3d,
    noop
};

extern const WorldInterface WORLD_INTERFACE;
 
// TODO: add documentation
const kinematics::DiffDriveKinematics& driveKinematics();

void world_interface_init(
    std::optional<std::reference_wrapper<net::websocket::SingleClientWSServer>> wsServer,
    bool initOnlyMotors = false);

std::shared_ptr<robot::base_motor> getMotor(robot::types::motorid_t motor);

void emergencyStop();

bool isEmergencyStopped();

std::unordered_set<types::CameraID> getCameras();

std::shared_ptr<types::CameraHandle> openCamera(types::CameraID camID);

bool hasNewCameraFrame(types::CameraID camera, uint32_t oldFrameNum);

types::DataPoint<types::CameraFrame> readCamera(types::CameraID camera);

std::optional<cam::CameraParams> getCameraIntrinsicParams(types::CameraID camera);

std::optional<cv::Mat> getCameraExtrinsicParams(types::CameraID camera);

types::landmarks_t readLandmarks();

bool gpsHasFix();

types::DataPoint<navtypes::point_t> readGPS();

types::DataPoint<double> readIMUHeading();

types::DataPoint<Eigen::Quaterniond> readIMU();

types::DataPoint<navtypes::pose_t> getTruePose();

std::optional<navtypes::point_t> gpsToMeters(const navtypes::gpscoords_t& coord);

void setIndicator(types::indication_t signal);

void setMotorPower(robot::types::motorid_t motor, double power);

void setMotorPos(robot::types::motorid_t motor, int32_t targetPos);

void setMotorVel(robot::types::motorid_t motor, int32_t targetVel);

types::DataPoint<int32_t> getMotorPos(robot::types::motorid_t motor);
 
void setServoPos(robot::types::servoid_t servo, int32_t position);
void setRequestedStepperTurnAngle(robot::types::stepperid_t stepper, int16_t angle);
 
using callbackid_t = unsigned long long;
 
callbackid_t addLimitSwitchCallback(
    robot::types::motorid_t motor,
    const std::function<void(
        robot::types::motorid_t motor,
        robot::types::DataPoint<robot::types::LimitSwitchData> limitSwitchData)>& callback);
 
void removeLimitSwitchCallback(callbackid_t id);

template <unsigned long int N>
types::DataPoint<navtypes::Vectord<N>>
getMotorPositionsRad(const std::array<types::motorid_t, N>& motors) {
    navtypes::Vectord<N> motorPositions;
    std::optional<types::datatime_t> timestamp;
    for (size_t i = 0; i < motors.size(); i++) {
        types::DataPoint<int32_t> positionDataPoint = robot::getMotorPos(motors[i]);
        if (positionDataPoint.isValid()) {
            if (!timestamp.has_value() || timestamp > positionDataPoint.getTime()) {
                timestamp = positionDataPoint.getTime();
            }
            double position = static_cast<double>(positionDataPoint.getData());
            position *= M_PI / 180.0 / 1000.0;
            motorPositions(i) = position;
        } else {
            return {};
        }
    }
    return types::DataPoint<navtypes::Vectord<N>>(timestamp.value(), motorPositions);
}
 
} // namespace robot
 
namespace gps {
robot::types::DataPoint<navtypes::gpscoords_t> readGPSCoords();
} // namespace gps