ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
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);
}
}
2 | No.2 Revision |
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);
}
}
3 | No.3 Revision |
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);
}
}
4 | No.4 Revision |
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);
}
}