$search
00001 /**************************************************************************** 00002 * 00003 * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $ 00004 * 00005 * This file is part of the ViSP software. 00006 * Copyright (C) 2005 - 2011 by INRIA. All rights reserved. 00007 * 00008 * This software is free software; you can redistribute it and/or 00009 * modify it under the terms of the GNU General Public License 00010 * ("GPL") version 2 as published by the Free Software Foundation. 00011 * See the file LICENSE.txt at the root directory of this source 00012 * distribution for additional information about the GNU GPL. 00013 * 00014 * For using ViSP with software that can not be combined with the GNU 00015 * GPL, please contact INRIA about acquiring a ViSP Professional 00016 * Edition License. 00017 * 00018 * See http://www.irisa.fr/lagadic/visp/visp.html for more information. 00019 * 00020 * This software was developed at: 00021 * INRIA Rennes - Bretagne Atlantique 00022 * Campus Universitaire de Beaulieu 00023 * 35042 Rennes Cedex 00024 * France 00025 * http://www.irisa.fr/lagadic 00026 * 00027 * If you have questions regarding the use of this file, please contact 00028 * INRIA at visp@inria.fr 00029 * 00030 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE 00031 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 00032 * 00033 * Contact visp@irisa.fr if any conditions of this licensing are 00034 * not clear to you. 00035 * 00036 * Description: 00037 * conversions between ROS and ViSP structures representing a 3D pose 00038 * 00039 * Authors: 00040 * Filip Novotny 00041 * 00042 * 00043 *****************************************************************************/ 00044 00049 #include "visp/vpConfig.h" 00050 #include "3dpose.h" 00051 #include <cmath> 00052 00053 #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1) 00054 #include <visp/vpQuaternionVector.h> 00055 #else 00056 #include "../vpQuaternionVector.h" 00057 #include <visp/vpRotationMatrix.h> 00058 #endif 00059 #include <visp/vpTranslationVector.h> 00060 00061 00062 namespace visp_bridge{ 00063 00064 #if VISP_VERSION_INT > (2<<16 | 6<<8 | 1) 00065 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Pose& pose){ 00066 vpHomogeneousMatrix mat; 00067 vpTranslationVector vec(pose.position.x,pose.position.y,pose.position.z); 00068 vpQuaternionVector q(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w); 00069 mat.buildFrom(vec,q); 00070 00071 return mat; 00072 } 00073 00074 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform& trans){ 00075 vpHomogeneousMatrix mat; 00076 vpTranslationVector vec(trans.translation.x,trans.translation.y,trans.translation.z); 00077 vpQuaternionVector q(trans.rotation.x,trans.rotation.y,trans.rotation.z,trans.rotation.w); 00078 mat.buildFrom(vec,q); 00079 00080 return mat; 00081 } 00082 00083 geometry_msgs::Transform toGeometryMsgsTransform(vpHomogeneousMatrix& mat){ 00084 geometry_msgs::Transform trans; 00085 vpQuaternionVector q; 00086 mat.extract(q); 00087 trans.rotation.x = q.x(); 00088 trans.rotation.y = q.y(); 00089 trans.rotation.z = q.z(); 00090 trans.rotation.w = q.w(); 00091 00092 00093 trans.translation.x = mat[0][3]; 00094 trans.translation.y = mat[1][3]; 00095 trans.translation.z = mat[2][3]; 00096 00097 return trans; 00098 } 00099 00100 geometry_msgs::Pose toGeometryMsgsPose(vpHomogeneousMatrix& mat){ 00101 geometry_msgs::Pose pose; 00102 00103 vpThetaUVector tu(mat); 00104 vpColVector u; 00105 double theta; 00106 tu.extract(theta, u); 00107 00108 theta *= 0.5; 00109 00110 double sinTheta_2 = sin(theta); 00111 00112 pose.orientation.x = u[0] * sinTheta_2; 00113 pose.orientation.y = u[1] * sinTheta_2; 00114 pose.orientation.z = u[2] * sinTheta_2; 00115 pose.orientation.w = cos(theta); 00116 00117 pose.position.x = mat[0][3]; 00118 pose.position.y = mat[1][3]; 00119 pose.position.z = mat[2][3]; 00120 00121 return pose; 00122 } 00123 00124 00125 #else 00126 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Transform& trans){ 00127 vpHomogeneousMatrix mat; 00128 vpTranslationVector vec(trans.translation.x,trans.translation.y,trans.translation.z); 00129 vpRotationMatrix rmat; 00130 00131 double a = trans.rotation.x; 00132 double b = trans.rotation.y; 00133 double c = trans.rotation.z; 00134 double d = trans.rotation.w; 00135 rmat[0][0] = a*a+b*b-c*c-d*d; 00136 rmat[0][1] = 2*b*c-2*a*d; 00137 rmat[0][2] = 2*a*c+2*b*d; 00138 00139 rmat[1][0] = 2*a*d+2*b*c; 00140 rmat[1][1] = a*a-b*b+c*c-d*d; 00141 rmat[1][2] = 2*c*d-2*a*b; 00142 00143 rmat[2][0] = 2*b*d-2*a*c; 00144 rmat[2][1] = 2*a*b+2*c*d; 00145 rmat[2][2] = a*a-b*b-c*c+d*d; 00146 00147 mat.buildFrom(vec,rmat); 00148 00149 return mat; 00150 } 00151 00152 geometry_msgs::Transform toGeometryMsgsTransform(vpHomogeneousMatrix& mat){ 00153 geometry_msgs::Transform trans; 00154 vpRotationMatrix rmat; 00155 mat.extract(rmat); 00156 vpQuaternionVector q(rmat); 00157 00158 trans.rotation.x = q.x(); 00159 trans.rotation.y = q.y(); 00160 trans.rotation.z = q.z(); 00161 trans.rotation.w = q.w(); 00162 00163 00164 trans.translation.x = mat[0][3]; 00165 trans.translation.y = mat[1][3]; 00166 trans.translation.z = mat[2][3]; 00167 00168 return trans; 00169 } 00170 00171 vpHomogeneousMatrix toVispHomogeneousMatrix(const geometry_msgs::Pose& pose){ 00172 vpHomogeneousMatrix mat; 00173 vpTranslationVector vec(pose.position.x,pose.position.y,pose.position.z); 00174 vpRotationMatrix rmat; 00175 00176 double a = pose.orientation.x; 00177 double b = pose.orientation.y; 00178 double c = pose.orientation.z; 00179 double d = pose.orientation.w; 00180 rmat[0][0] = a*a+b*b-c*c-d*d; 00181 rmat[0][1] = 2*b*c-2*a*d; 00182 rmat[0][2] = 2*a*c+2*b*d; 00183 00184 rmat[1][0] = 2*a*d+2*b*c; 00185 rmat[1][1] = a*a-b*b+c*c-d*d; 00186 rmat[1][2] = 2*c*d-2*a*b; 00187 00188 rmat[2][0] = 2*b*d-2*a*c; 00189 rmat[2][1] = 2*a*b+2*c*d; 00190 rmat[2][2] = a*a-b*b-c*c+d*d; 00191 00192 mat.buildFrom(vec,rmat); 00193 00194 return mat; 00195 } 00196 #endif 00197 }