Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version
   Now ,I solve the problem by this method.
   Subscrible the "/odom" topic, and record the useful position(x,y,z).Compare the current position(x,y,z) to the last position(x,y,z). Why? Because  the "/odom" is Non-stop broadcasting.
   But I find the other problem.At first,the map frame is match to the odom frame,but as time went by,the trajectory is offset. Here is my code:

public class TrajectoryOdomLayer extends SubscriberLayer<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) {
            Log.e("TrajectoryDraw", "draw");
            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(VisualizationView view, ConnectedNode connectedNode) {
        super.onStart(view, connectedNode);

        ByteBuffer goalVertexByteBuffer = ByteBuffer.allocateDirect(1024 * 1024);
        goalVertexByteBuffer.order(ByteOrder.nativeOrder());
        trajectoryBuffer = goalVertexByteBuffer.asFloatBuffer();

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

    private void updateVertexBuffer(Odometry message) {

        int count = message.getHeader().getSeq();
        if (count % 5 == 0) {
            float x = (float) message.getPose().getPose().getPosition().getX();
            float y = (float) message.getPose().getPose().getPosition().getY();
            float z = (float) message.getPose().getPose().getPosition().getZ();

            frame = GraphName.of("map");
//            frame = GraphName.of(message.getChildFrameId());
            if (Math.sqrt(Math.pow(x - lastX, 2) + Math.pow(y - lastY, 2) + Math.pow(z - lastZ, 2)) > 0.05) {
                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);
    }
}
 
   Now ,I solve the problem by this method.
   Subscrible the "/odom" topic, and record the useful position(x,y,z).Compare the current position(x,y,z) to the last position(x,y,z). Why? Because  the "/odom" is Non-stop broadcasting.
   But I find the other problem.At first,the map frame is match to the odom frame,but as time went by,the trajectory is offset. Here is my code:

public class TrajectoryOdomLayer extends SubscriberLayer<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) {
            Log.e("TrajectoryDraw", "draw");
            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(VisualizationView view, ConnectedNode connectedNode) {
        super.onStart(view, connectedNode);

        ByteBuffer goalVertexByteBuffer = ByteBuffer.allocateDirect(1024 * 1024);
        goalVertexByteBuffer.order(ByteOrder.nativeOrder());
        trajectoryBuffer = goalVertexByteBuffer.asFloatBuffer();

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

    private void updateVertexBuffer(Odometry message) {

        int count = message.getHeader().getSeq();
        if (count % 5 == 0) {
            float x = (float) message.getPose().getPose().getPosition().getX();
            float y = (float) message.getPose().getPose().getPosition().getY();
            float z = (float) message.getPose().getPose().getPosition().getZ();

            frame = GraphName.of("map");
//            frame = GraphName.of(message.getChildFrameId());
            if (Math.sqrt(Math.pow(x - lastX, 2) + Math.pow(y - lastY, 2) + Math.pow(z - lastZ, 2)) > 0.05) {
                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);
    }
}
 
 

Now ,I solve the problem by this method. Subscrible the "/odom" topic, and record the useful position(x,y,z).Compare the current position(x,y,z) to the last position(x,y,z). Why? Because the "/odom" is Non-stop broadcasting. But I find the other problem.At first,the map frame is match to the odom frame,but as time went by,the trajectory is offset. Here is my code:

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 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<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) {
            Log.e("TrajectoryDraw", "draw");
            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(VisualizationView view, ConnectedNode connectedNode) {
        super.onStart(view, connectedNode);

        ByteBuffer goalVertexByteBuffer = ByteBuffer.allocateDirect(1024 * 1024);
        goalVertexByteBuffer.order(ByteOrder.nativeOrder());
        trajectoryBuffer = goalVertexByteBuffer.asFloatBuffer();

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

    private void updateVertexBuffer(Odometry message) {

        int count = message.getHeader().getSeq();
        if (count % 5 == 0) {
            float x = (float) message.getPose().getPose().getPosition().getX();
            float y = (float) message.getPose().getPose().getPosition().getY();
            float z = (float) message.getPose().getPose().getPosition().getZ();

            frame = GraphName.of("map");
//            frame = GraphName.of(message.getChildFrameId());
            if (Math.sqrt(Math.pow(x - lastX, 2) + Math.pow(y - lastY, 2) + Math.pow(z - lastZ, 2)) > 0.05) {
                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);
    }
}
  

Now ,I solve This is the problem by this method. Subscrible final method(subscrible the "/odom" topic, and record topic "/odom",then tranform the useful position(x,y,z).Compare the current position(x,y,z) "odom frame" to the last position(x,y,z). Why? Because the "/odom" is Non-stop broadcasting. But I find the other problem.At first,the map frame is match to the odom frame,but as time went by,the trajectory is offset. Here "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<odometry> 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) {
            Log.e("TrajectoryDraw", "draw");
            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(VisualizationView 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<odometry>() MessageListener<nav_msgs.odometry>() {
            @Override
            public void onNewMessage(final Odometry odometry) onNewMessage(nav_msgs.Odometry message) {
                Log.e("Trajectory", "onNewMessage");
                updateVertexBuffer(odometry);
updateVertexBuffer(view, message);
                ready = true;
            }
        });
    }

    //转换odom frame到map frame
    private void updateVertexBuffer(Odometry 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) message.getPose().getPose().getPosition().getX();
vector3.getX();
                float y = (float) message.getPose().getPose().getPosition().getY();
vector3.getY();
                float z = (float) message.getPose().getPose().getPosition().getZ();

            frame = GraphName.of("map");
//            frame = GraphName.of(message.getChildFrameId());
vector3.getZ();
                if (Math.sqrt(Math.pow(x - lastX, 2) + Math.pow(y - lastY, 2) + Math.pow(z - lastZ, 2)) > 0.05) {
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);
    }
}