eigen_msg.h
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 /*
31  * Author: Stuart Glaser
32  */
33 
34 #ifndef EIGEN_MSG_CONVERSIONS_H
35 #define EIGEN_MSG_CONVERSIONS_H
36 
37 #include <std_msgs/Float64MultiArray.h>
38 #include <geometry_msgs/Point.h>
39 #include <geometry_msgs/Pose.h>
40 #include <geometry_msgs/Quaternion.h>
41 #include <geometry_msgs/Transform.h>
42 #include <geometry_msgs/Twist.h>
43 #include <geometry_msgs/Vector3.h>
44 #include <geometry_msgs/Wrench.h>
45 
46 #include <Eigen/Core>
47 #include <Eigen/Geometry>
48 
49 namespace tf {
50 
52 void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e);
53 
55 void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m);
56 
58 void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
59 
61 void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Isometry3d &e);
62 
64 void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m);
65 
67 void poseEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Pose &m);
68 
70 void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e);
71 
73 void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m);
74 
76 void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e);
77 
79 void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Isometry3d &e);
80 
82 void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m);
83 
85 void transformEigenToMsg(const Eigen::Isometry3d &e, geometry_msgs::Transform &m);
86 
88 void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix<double,6,1> &e);
89 
91 void twistEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Twist &m);
92 
94 void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e);
95 
97 void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m);
98 
100 void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix<double,6,1> &e);
101 
103 void wrenchEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Wrench &m);
104 
106 template <class Derived>
107 void matrixEigenToMsg(const Eigen::MatrixBase<Derived> &e, std_msgs::Float64MultiArray &m)
108 {
109  if (m.layout.dim.size() != 2)
110  m.layout.dim.resize(2);
111  m.layout.dim[0].stride = e.rows() * e.cols();
112  m.layout.dim[0].size = e.rows();
113  m.layout.dim[1].stride = e.cols();
114  m.layout.dim[1].size = e.cols();
115  if ((int)m.data.size() != e.size())
116  m.data.resize(e.size());
117  int ii = 0;
118  for (int i = 0; i < e.rows(); ++i)
119  for (int j = 0; j < e.cols(); ++j)
120  m.data[ii++] = e.coeff(i, j);
121 }
122 
123 } // namespace
124 
125 #endif
tf::poseMsgToEigen
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
Converts a Pose message into an Eigen Affine3d.
Definition: eigen_msg.cpp:113
tf::twistEigenToMsg
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
tf::wrenchMsgToEigen
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
tf::matrixEigenToMsg
void matrixEigenToMsg(const Eigen::MatrixBase< Derived > &e, std_msgs::Float64MultiArray &m)
Converts an Eigen matrix into a Float64MultiArray message.
Definition: eigen_msg.h:107
tf::quaternionMsgToEigen
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
Converts a Quaternion message into an Eigen Quaternion.
Definition: eigen_msg.cpp:133
tf::pointMsgToEigen
void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e)
Converts a Point message into an Eigen Vector.
Definition: eigen_msg.cpp:35
tf::vectorEigenToMsg
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
Converts an Eigen Vector into a Vector message.
Definition: eigen_msg.cpp:173
tf::pointEigenToMsg
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
Converts an Eigen Vector into a Point message.
Definition: eigen_msg.cpp:42
tf::vectorMsgToEigen
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
Converts a Vector message into an Eigen Vector.
Definition: eigen_msg.cpp:166
tf::poseEigenToMsg
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Converts an Eigen Affine3d into a Pose message.
Definition: eigen_msg.cpp:123
tf::transformMsgToEigen
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
Converts a Transform message into an Eigen Affine3d.
Definition: eigen_msg.cpp:146
tf::twistMsgToEigen
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
tf::wrenchEigenToMsg
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
tf::quaternionEigenToMsg
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
Converts an Eigen Quaternion into a Quaternion message.
Definition: eigen_msg.cpp:138
tf
Definition: eigen_kdl.h:42
tf::transformEigenToMsg
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
Converts an Eigen Affine3d into a Transform message.
Definition: eigen_msg.cpp:156


eigen_conversions
Author(s): Stuart Glaser, Adam Leeper
autogenerated on Sat Aug 19 2023 02:38:05