Utils.java
Go to the documentation of this file.
00001 package tfjava;
00002 /*
00003  * Copyright (c) 2011, Sjoerd van den Dries, Moritz Tenorth
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Technische Universiteit Eindhoven nor the
00015  *       names of its contributors may be used to endorse or promote products
00016  *       derived from this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 import ros.communication.Time;
00032 
00033 import javax.vecmath.Point3d;
00034 import javax.vecmath.Matrix4d;
00035 
00036 
00037 
00046 public class Utils {
00047     
00055         public static Time secondsToRosTime(double secs) {
00056             return new Time(secs);
00057         }
00058 
00063         public static double getTimeNowSecs() {
00064             return (double)Time.now().totalNsecs() / 1000000000;
00065         }
00066 
00067         
00074     public static double[] tfToPoseArray(StampedTransform tf) {
00075         Matrix4d m = tf.getMatrix4();
00076         double[] mArray = new double[] {m.m00, m.m01, m.m02, m.m03, m.m10, m.m11, m.m12, m.m13,
00077                                         m.m20, m.m21, m.m22, m.m23, m.m30, m.m31, m.m32, m.m33};
00078         return mArray;
00079     }
00080     
00081     
00090     public static StampedTransform poseArrayToTF(float[] p, String frameId, String childFrameId) {
00091 
00092         Matrix4d mat = new Matrix4d();
00093         for(int i=0;i<4;i++)
00094             for(int j=0;j<4;j++)
00095                 mat.setElement(i, j, p[4*i+j]);
00096         
00097         return new StampedTransform(mat, 
00098                                     secondsToRosTime(getTimeNowSecs()), 
00099                                     frameId, 
00100                                     childFrameId);
00101     }
00102 
00109     public static double[] stampedPoint3dToPointArray(Stamped<Point3d> stamped) {
00110         return  new double[] {stamped.data.x, stamped.data.y, stamped.data.z};
00111     }
00112     
00121     public static Stamped<Point3d> pointArrayToStampedPoint3d(float[] p, String frameID, double secs) {
00122 
00123         Stamped<Point3d> point = new Stamped<Point3d>();
00124         
00125         point.data = new Point3d();
00126         point.data.x = p[0];
00127         point.data.y = p[1];
00128         point.data.z = p[2];
00129         
00130         point.frameID = frameID;
00131         point.timeStamp = secondsToRosTime(secs);
00132         
00133         return point;
00134     }
00135     
00142     public static double[] stampedMatrix4dToPoseArray(Stamped<Matrix4d> stamped) {
00143         Matrix4d m = stamped.data;
00144                
00145         return new double[] {m.m00, m.m01, m.m02, m.m03, m.m10, m.m11, m.m12, m.m13,
00146                              m.m20, m.m21, m.m22, m.m23, m.m30, m.m31, m.m32, m.m33};
00147     }
00148 
00149     
00158     public static Stamped<Matrix4d> poseArrayToStampedMatrix4d(float[] m, String frameID, double secs) {
00159         
00160         Stamped<Matrix4d> mat = new Stamped<Matrix4d>();
00161         
00162         mat.data = new Matrix4d();
00163         for(int i=0;i<4;i++)
00164             for(int j=0;j<4;j++)
00165                 mat.data.setElement(i, j, m[4*i+j]);
00166         
00167         mat.frameID = frameID;
00168         mat.timeStamp = secondsToRosTime(secs);
00169         
00170         return mat;
00171     }
00172     
00178     public static Stamped<Matrix4d> getStampedIdentityMatrix4d() {
00179         
00180         Stamped<Matrix4d> res = new Stamped<Matrix4d>();
00181         res.data = new Matrix4d();
00182         res.data.setIdentity();
00183         return res;
00184         
00185     }
00186     
00192     public static Stamped<Point3d> getStampedPoint3d() {
00193         
00194         Stamped<Point3d> res = new Stamped<Point3d>();
00195         res.data = new Point3d();
00196         return res;
00197         
00198     }
00199     
00200 }


tfjava
Author(s): Sjoerd van den Dries
autogenerated on Sun Oct 5 2014 22:54:19