51 void poseMsgToEigenImpl(
const geometry_msgs::Pose &m, T &e)
53 e = Eigen::Translation3d(m.position.x,
56 Eigen::Quaterniond(m.orientation.w,
63 void poseEigenToMsgImpl(
const T &e, geometry_msgs::Pose &m)
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;
82 void transformMsgToEigenImpl(
const geometry_msgs::Transform &m, T &e)
84 e = Eigen::Translation3d(m.translation.x,
87 Eigen::Quaterniond(m.rotation.w,
94 void transformEigenToMsgImpl(
const T &e, geometry_msgs::Transform &m)
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) {
115 poseMsgToEigenImpl(m, e);
120 poseMsgToEigenImpl(m, e);
125 poseEigenToMsgImpl(e, m);
130 poseEigenToMsgImpl(e, m);
135 e = Eigen::Quaterniond(m.w, m.x, m.y, m.z);
148 transformMsgToEigenImpl(m, e);
153 transformMsgToEigenImpl(m, e);
158 transformEigenToMsgImpl(e, m);
163 transformEigenToMsgImpl(e, m);
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
Converts an Eigen Affine3d into a Pose message.
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
Converts an Eigen Quaternion into a Quaternion message.
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
Converts a Pose message into an Eigen Affine3d.
void twistEigenToMsg(const Eigen::Matrix< double, 6, 1 > &e, geometry_msgs::Twist &m)
Converts an Eigen matrix into a Twist message.
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
Converts an Eigen Affine3d into a Transform message.
void vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e)
Converts a Vector message into an Eigen Vector.
void wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix< double, 6, 1 > &e)
Converts a Wrench message into an Eigen matrix.
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
Converts an Eigen Vector into a Vector message.
void pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e)
Converts a Point message into an Eigen Vector.
void pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m)
Converts an Eigen Vector into a Point message.
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
Converts a Transform message into an Eigen Affine3d.
void wrenchEigenToMsg(const Eigen::Matrix< double, 6, 1 > &e, geometry_msgs::Wrench &m)
Converts an Eigen matrix into a Wrench message.
void quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e)
Converts a Quaternion message into an Eigen Quaternion.
void twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix< double, 6, 1 > &e)
Converts a Twist message into an Eigen matrix.