public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3d(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quat4d(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3D(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quaternion(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }