Ejemplo n.º 1
0
Pose2D::Pose2D(const Pose2D& orig) : x(orig.getX()), y(orig.getY()), theta(orig.getTheta()) {
}
Ejemplo n.º 2
0
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()));
}