eigen_msg.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
32 
33 namespace tf {
34 
35 void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e)
36 {
37  e(0) = m.x;
38  e(1) = m.y;
39  e(2) = m.z;
40 }
41 
42 void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
43 {
44  m.x = e(0);
45  m.y = e(1);
46  m.z = e(2);
47 }
48 
49 namespace {
50  template<typename T>
51  void poseMsgToEigenImpl(const geometry_msgs::Pose &m, T &e)
52  {
53  e = Eigen::Translation3d(m.position.x,
54  m.position.y,
55  m.position.z) *
56  Eigen::Quaterniond(m.orientation.w,
57  m.orientation.x,
58  m.orientation.y,
59  m.orientation.z);
60  }
61 
62  template<typename T>
63  void poseEigenToMsgImpl(const T &e, geometry_msgs::Pose &m)
64  {
65  m.position.x = e.translation()[0];
66  m.position.y = e.translation()[1];
67  m.position.z = e.translation()[2];
68  Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
69  m.orientation.x = q.x();
70  m.orientation.y = q.y();
71  m.orientation.z = q.z();
72  m.orientation.w = q.w();
73  if (m.orientation.w < 0) {
74  m.orientation.x *= -1;
75  m.orientation.y *= -1;
76  m.orientation.z *= -1;
77  m.orientation.w *= -1;
78  }
79  }
80 
81  template<typename T>
82  void transformMsgToEigenImpl(const geometry_msgs::Transform &m, T &e)
83  {
84  e = Eigen::Translation3d(m.translation.x,
85  m.translation.y,
86  m.translation.z) *
87  Eigen::Quaterniond(m.rotation.w,
88  m.rotation.x,
89  m.rotation.y,
90  m.rotation.z);
91  }
92 
93  template<typename T>
94  void transformEigenToMsgImpl(const T &e, geometry_msgs::Transform &m)
95  {
96  m.translation.x = e.translation()[0];
97  m.translation.y = e.translation()[1];
98  m.translation.z = e.translation()[2];
99  Eigen::Quaterniond q = (Eigen::Quaterniond)e.linear();
100  m.rotation.x = q.x();
101  m.rotation.y = q.y();
102  m.rotation.z = q.z();
103  m.rotation.w = q.w();
104  if (m.rotation.w < 0) {
105  m.rotation.x *= -1;
106  m.rotation.y *= -1;
107  m.rotation.z *= -1;
108  m.rotation.w *= -1;
109  }
110  }
111 }
112 
113 void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
114 {
115  poseMsgToEigenImpl(m, e);
116 }
117 
118 void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e)
119 {
120  poseMsgToEigenImpl(m, e);
121 }
122 
123 void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
124 {
125  poseEigenToMsgImpl(e, m);
126 }
127 
128 void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m)
129 {
130  poseEigenToMsgImpl(e, m);
131 }
132 
133 void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
134 {
135  e = Eigen::Quaterniond(m.w, m.x, m.y, m.z);
136 }
137 
138 void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
139 {
140  m.x = e.x();
141  m.y = e.y();
142  m.z = e.z();
143  m.w = e.w();
144 }
145 
146 void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
147 {
148  transformMsgToEigenImpl(m, e);
149 }
150 
151 void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e)
152 {
153  transformMsgToEigenImpl(m, e);
154 }
155 
156 void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
157 {
158  transformEigenToMsgImpl(e, m);
159 }
160 
161 void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m)
162 {
163  transformEigenToMsgImpl(e, m);
164 }
165 
166 void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
167 {
168  e(0) = m.x;
169  e(1) = m.y;
170  e(2) = m.z;
171 }
172 
173 void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
174 {
175  m.x = e(0);
176  m.y = e(1);
177  m.z = e(2);
178 }
179 
180 void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix<double,6,1> &e)
181 {
182  e[0] = m.linear.x;
183  e[1] = m.linear.y;
184  e[2] = m.linear.z;
185  e[3] = m.angular.x;
186  e[4] = m.angular.y;
187  e[5] = m.angular.z;
188 }
189 
190 void twistEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Twist &m)
191 {
192  m.linear.x = e[0];
193  m.linear.y = e[1];
194  m.linear.z = e[2];
195  m.angular.x = e[3];
196  m.angular.y = e[4];
197  m.angular.z = e[5];
198 }
199 
200 void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix<double,6,1> &e)
201 {
202  e[0] = m.force.x;
203  e[1] = m.force.y;
204  e[2] = m.force.z;
205  e[3] = m.torque.x;
206  e[4] = m.torque.y;
207  e[5] = m.torque.z;
208 }
209 
210 void wrenchEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Wrench &m)
211 {
212  m.force.x = e[0];
213  m.force.y = e[1];
214  m.force.z = e[2];
215  m.torque.x = e[3];
216  m.torque.y = e[4];
217  m.torque.z = e[5];
218 }
219 
220 } // namespace
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Converts an Eigen Affine3d into a Pose message.
Definition: eigen_msg.cpp:123
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
Converts an Eigen Quaternion into a Quaternion message.
Definition: eigen_msg.cpp:138
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
Converts a Pose message into an Eigen Affine3d.
Definition: eigen_msg.cpp:113
void twistEigenToMsg(const Eigen::Matrix< double, 6, 1 > &e, geometry_msgs::Twist &m)
Converts an Eigen matrix into a Twist message.
Definition: eigen_msg.cpp:190
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
Converts an Eigen Affine3d into a Transform message.
Definition: eigen_msg.cpp:156
Definition: eigen_kdl.h:42
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
Converts a Vector message into an Eigen Vector.
Definition: eigen_msg.cpp:166
void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix< double, 6, 1 > &e)
Converts a Wrench message into an Eigen matrix.
Definition: eigen_msg.cpp:200
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
Converts an Eigen Vector into a Vector message.
Definition: eigen_msg.cpp:173
void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e)
Converts a Point message into an Eigen Vector.
Definition: eigen_msg.cpp:35
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
Converts an Eigen Vector into a Point message.
Definition: eigen_msg.cpp:42
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
Converts a Transform message into an Eigen Affine3d.
Definition: eigen_msg.cpp:146
void wrenchEigenToMsg(const Eigen::Matrix< double, 6, 1 > &e, geometry_msgs::Wrench &m)
Converts an Eigen matrix into a Wrench message.
Definition: eigen_msg.cpp:210
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
Converts a Quaternion message into an Eigen Quaternion.
Definition: eigen_msg.cpp:133
void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix< double, 6, 1 > &e)
Converts a Twist message into an Eigen matrix.
Definition: eigen_msg.cpp:180


eigen_conversions
Author(s): Stuart Glaser, Adam Leeper
autogenerated on Mon Feb 28 2022 22:26:17