Pose2D::Pose2D(const Pose2D& orig) : x(orig.getX()), y(orig.getY()), theta(orig.getTheta()) { }
Pose2D operator+(const Pose2D& op1, const Pose2D& op2) { Pose2D pose(op1.getX() + op2.getX(), op1.getY() + op2.getY(), op1.getTheta() + op2.getTheta()); return pose; }
float OmniDrivePositionController::getDistance(const Pose2D& actualPose, const Pose2D& goalPose) { return sqrt((actualPose.getX() - goalPose.getX()) * (actualPose.getX() - goalPose.getX()) + (actualPose.getY() - goalPose.getY()) * (actualPose.getY() - goalPose.getY())); }