34 #ifndef EIGEN_MSG_CONVERSIONS_H
35 #define EIGEN_MSG_CONVERSIONS_H
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>
47 #include <Eigen/Geometry>
52 void pointMsgToEigen(
const geometry_msgs::Point &m, Eigen::Vector3d &e);
55 void pointEigenToMsg(
const Eigen::Vector3d &e, geometry_msgs::Point &m);
58 void poseMsgToEigen(
const geometry_msgs::Pose &m, Eigen::Affine3d &e);
61 void poseMsgToEigen(
const geometry_msgs::Pose &m, Eigen::Isometry3d &e);
64 void poseEigenToMsg(
const Eigen::Affine3d &e, geometry_msgs::Pose &m);
67 void poseEigenToMsg(
const Eigen::Isometry3d &e, geometry_msgs::Pose &m);
88 void twistMsgToEigen(
const geometry_msgs::Twist &m, Eigen::Matrix<double,6,1> &e);
91 void twistEigenToMsg(
const Eigen::Matrix<double,6,1> &e, geometry_msgs::Twist &m);
100 void wrenchMsgToEigen(
const geometry_msgs::Wrench &m, Eigen::Matrix<double,6,1> &e);
103 void wrenchEigenToMsg(
const Eigen::Matrix<double,6,1> &e, geometry_msgs::Wrench &m);
106 template <
class Derived>
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());
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);