DiffDriveKinematics.h

#pragma once
 
#include "../navtypes.h"
namespace kinematics {
struct wheelvel_t {
    double lVel;
    double rVel;
};

class DiffDriveKinematics {
public:
    enum class PreferredVelPreservation {
        Proportional,
        PreferXVel,
        PreferThetaVel,
    };

    explicit DiffDriveKinematics(double wheelBaseWidth);

    navtypes::pose_t wheelVelToRobotVel(double lVel, double rVel) const;

    wheelvel_t robotVelToWheelVel(double xVel, double thetaVel) const;

    navtypes::pose_t getLocalPoseUpdate(const wheelvel_t& wheelVel, double dt) const;

    navtypes::pose_t getPoseUpdate(const wheelvel_t& wheelVel, double heading,
                                   double dt) const;

    navtypes::pose_t getNextPose(const wheelvel_t& wheelVel, const navtypes::pose_t& pose,
                                 double dt) const;

    navtypes::pose_t ensureWithinWheelSpeedLimit(PreferredVelPreservation preferred,
                                                 double xVel, double thetaVel,
                                                 double maxWheelSpeed) const;
 
private:
    double wheelBaseWidth;
};
} // namespace kinematics