C++ (Cpp) Odometry::getPose2D Examples

Example #1

File: OmniDrivePositionController.cpp Project: BjoKaSH/BRICS_RN

const Odometry& OmniDrivePositionController::computeNewOdometry(const Odometry& actualOdometry, double elapsedTimeInSec) {


    KDL::Trajectory& trajectoryComposite = targetTrajectory.getTrajectory();

    this->actualOdometry = actualOdometry;
    computedOdometry = Odometry();

    if (trajectoryComposite.Duration() > 0 && !isTargetReached()) {
  
        actualTime = elapsedTimeInSec;

        KDL::Frame desiredPose = trajectoryComposite.Pos(actualTime);
        KDL::Twist desiredTwist = trajectoryComposite.Vel(actualTime);
        KDL::Frame initPose = trajectoryComposite.Pos(0);

        double r, p, y;
        desiredPose.M.GetRPY(r, p, y);

        Odometry desiredOdometryGlobal(Pose2D(desiredPose.p.x(),
                desiredPose.p.y(),
                actualOdometry.getPose2D().getTheta()),
                Twist2D(desiredTwist.vel.x(),
                desiredTwist.vel.y(),
                desiredTwist.rot.z()));

        Odometry desiredOdometryLocal;

        Odometry actualOdometryGlobal(actualOdometry);
        Odometry actualOdometryLocal;

        OmniDriveKinematicsModel omnidrive;
        omnidrive.convertToLocalReferenceFrame(desiredOdometryGlobal, desiredOdometryLocal);
        omnidrive.convertToLocalReferenceFrame(actualOdometryGlobal, actualOdometryLocal);

        double dPosX = desiredOdometryLocal.getPose2D().getX();
        double dPosY = desiredOdometryLocal.getPose2D().getY();
        double dPosTheta = y; //desiredOdometryLocal.getPose2D().getTheta();

        double dVelX = desiredOdometryLocal.getTwist2D().getX();
        double dVelY = desiredOdometryLocal.getTwist2D().getY();
        double dVelTheta = desiredOdometryLocal.getTwist2D().getTheta();

        double aPosX = actualOdometryLocal.getPose2D().getX();
        double aPosY = actualOdometryLocal.getPose2D().getY();
        double aPosTheta = actualOdometryLocal.getPose2D().getTheta();

        double positionXError = dPosX - aPosX;
        double positionYError = dPosY - aPosY;
        double positionThetaError = getShortestAngle(dPosTheta, aPosTheta);

        double gainPosX = gains.getPose2D().getX();
        double gainPosY = gains.getPose2D().getY();
        double gainPosTheta = gains.getPose2D().getTheta();

        double gainVelX = gains.getTwist2D().getX();
        double gainVelY = gains.getTwist2D().getY();
        double gainVelTheta = gains.getTwist2D().getTheta();

        double errorTheta = gainVelTheta * dVelTheta + gainPosTheta * positionThetaError;
        double errorX = gainVelX * dVelX + gainPosX * positionXError;
        double errorY = gainVelY * dVelY + gainPosY * positionYError;

        computedOdometry.setTwist2D(Twist2D(errorX, errorY, errorTheta));
    }

    return computedOdometry;
}