예제 #1
0
파일: Pose2D.cpp 프로젝트: BjoKaSH/BRICS_RN
Pose2D::Pose2D(const Pose2D& orig) : x(orig.getX()), y(orig.getY()), theta(orig.getTheta()) {
}
예제 #2
0
파일: Pose2D.cpp 프로젝트: BjoKaSH/BRICS_RN
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()));
}