Android Open Source - base_controller Husky Odometry Status

From Project

License

The source code is released under:

Apache License

If you think the Android project base_controller listed in this page is inappropriate, such as containing malicious code/tools or violating the copyright, please email info at java2s dot com, thanks.

Java Source Code

package com.github.c77.base_driver.husky;
//from   w w  w.  j  av a2 s.  c o m
import com.github.c77.base_driver.AbstractOdometryStatus;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;

/**
 * @author jcerruti@creativa77.com (Julian Cerruti)
 */
public class HuskyOdometryStatus extends AbstractOdometryStatus {

    // TODO: Allow setting (and load from ROS param in node)
    private static final double WIDTH = 0.55;

    public HuskyOdometryStatus() {
        super(WIDTH);
    }

    public void update(byte[] encoderData) {
        if(encoderData.length != 13) {
            throw new RuntimeException("wrong size encoder data = " + encoderData.length);
        }

        // --------------------------------
        // Parse buffer into encoder travels and speeds
        // --------------------------------
        ByteBuffer buffer = ByteBuffer.wrap(encoderData);
        buffer.order(ByteOrder.LITTLE_ENDIAN);

        // Number of encoders
        // TODO: Verify it's two encoders
        byte nEncoders = buffer.get();
        // Left encoder travel
        int leftTravel = buffer.getInt(1);
        // Right encoder travel
        int rightTravel = buffer.getInt(5);
        // Left encoder speed
        short leftSpeed = buffer.getShort(9);
        // Right encoder speed
        short rightSpeed = buffer.getShort(11);

        // Update the current estimated pose
        calculateAndUpdate(leftTravel, rightTravel, rightSpeed, leftSpeed);
    }
}

Java Source Code List