How can I record the robot trajectory and show it on the Android Device?

edit

This is the final method(subscrible the topic "/odom",then tranform the "odom frame" to the "map frame").I tested on a Android pad(xiao mi).Next is my code:

import android.util.Log;

import org.ros.android.view.visualization.Color;
import org.ros.android.view.visualization.VisualizationView;
import org.ros.android.view.visualization.layer.SubscriberLayer;
import org.ros.android.view.visualization.layer.TfLayer;
import org.ros.message.MessageListener;
import org.ros.namespace.GraphName;
import org.ros.node.ConnectedNode;
import org.ros.rosjava_geometry.FrameTransform;
import org.ros.rosjava_geometry.Transform;
import org.ros.rosjava_geometry.Vector3;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.FloatBuffer;
import java.util.ArrayList;

import javax.microedition.khronos.opengles.GL10;

import nav_msgs.Odometry;

/**
 * Created by tony on 16-12-8.
 */

public class TrajectoryOdomLayer extends SubscriberLayer<nav_msgs.odometry> implements TfLayer {

    private static final Color COLOR = Color.fromHexAndAlpha("ff0000", 1.0f);

    private static final float POINT_SIZE = 5.0f;

    private FloatBuffer trajectoryBuffer;

    private float[] vertexs;

    private ArrayList<float> arrayList = new ArrayList<float>();

    private boolean ready;

    private GraphName frame;

    private float lastX = 0.0f;
    private float lastY = 0.0f;
    private float lastZ = 0.0f;

    public TrajectoryOdomLayer(String topic) {
        this(GraphName.of(topic));
    }

    public TrajectoryOdomLayer(GraphName topic) {
        super(topic, Odometry._TYPE);
        ready = false;
    }

    @Override
    public GraphName getFrame() {
        return frame;
    }

    @Override
    public void draw(VisualizationView view, GL10 gl) {
        if (ready) {
            gl.glEnableClientState(GL10.GL_VERTEX_ARRAY);
            gl.glVertexPointer(3, GL10.GL_FLOAT, 0, trajectoryBuffer);
            COLOR.apply(gl);
            gl.glPointSize(POINT_SIZE);
            gl.glDrawArrays(GL10.GL_POINTS, 0, trajectoryBuffer.limit() / 3);
            gl.glDisableClientState(GL10.GL_VERTEX_ARRAY);
        }
    }

    @Override
    public void onStart(final VisualizationView view, final ConnectedNode connectedNode) {
        super.onStart(view, connectedNode);
        frame = GraphName.of("map");
        ByteBuffer goalVertexByteBuffer = ByteBuffer.allocateDirect(1024 * 1024);
        goalVertexByteBuffer.order(ByteOrder.nativeOrder());
        trajectoryBuffer = goalVertexByteBuffer.asFloatBuffer();

        getSubscriber().addMessageListener(new MessageListener<nav_msgs.odometry>() {
            @Override
            public void onNewMessage(nav_msgs.Odometry message) {
                Log.e("Trajectory", "onNewMessage");
                updateVertexBuffer(view, message);
                ready = true;
            }
        });
    }

    //转换odom frame到map frame
    private void updateVertexBuffer(VisualizationView view, nav_msgs.Odometry message) {
        int count = message.getHeader().getSeq();
        if (count % 5 == 0) {
            FrameTransform frameTransform = view.getFrameTransformTree().transform(GraphName.of(message.getHeader().getFrameId()), GraphName.of("map"));
            if (frameTransform != null) {
                Transform transform = Transform.fromPoseMessage(message.getPose().getPose());
                Vector3 vector3 = frameTransform.getTransform().multiply(transform).getTranslation();
                float x = (float) vector3.getX();
                float y = (float) vector3.getY();
                float z = (float) vector3.getZ();
                if (Math.sqrt(Math.pow(x - lastX, 2) + Math.pow(y - lastY, 2) + Math.pow(z - lastZ, 2)) > 0.02) {
                    arrayList.add(x);
                    arrayList.add(y);
                    arrayList.add(z);
                    vertexs = new float[arrayList.size()];
                    for (int i = 0; i < vertexs.length; i++) {
                        vertexs[i] = arrayList.get(i);
                    }
                    trajectoryBuffer.put(vertexs);
                    lastX = x;
                    lastY = y;
                    lastZ = z;
                }
            }

        }
        trajectoryBuffer.position(0);
    }
}