33 #include <Eigen/Geometry> 34 #include <geometry_msgs/QuaternionStamped.h> 35 #include <geometry_msgs/PointStamped.h> 36 #include <geometry_msgs/PoseStamped.h> 37 #include <geometry_msgs/Twist.h> 49 return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z)
50 * Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
69 geometry_msgs::TransformStamped t;
70 t.transform.translation.x = T.translation().x();
71 t.transform.translation.y = T.translation().y();
72 t.transform.translation.z = T.translation().z();
74 Eigen::Quaterniond q(T.linear());
75 t.transform.rotation.x = q.x();
76 t.transform.rotation.y = q.y();
77 t.transform.rotation.z = q.z();
78 t.transform.rotation.w = q.w();
90 geometry_msgs::TransformStamped t;
91 t.transform.translation.x = T.translation().x();
92 t.transform.translation.y = T.translation().y();
93 t.transform.translation.z = T.translation().z();
95 Eigen::Quaterniond q(T.rotation());
96 t.transform.rotation.x = q.x();
97 t.transform.rotation.y = q.y();
98 t.transform.rotation.z = q.z();
99 t.transform.rotation.w = q.w();
115 void doTransform(
const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out,
const geometry_msgs::TransformStamped& transform)
126 geometry_msgs::Point
toMsg(
const Eigen::Vector3d& in)
128 geometry_msgs::Point msg;
141 void fromMsg(
const geometry_msgs::Point& msg, Eigen::Vector3d& out)
154 geometry_msgs::Vector3&
toMsg(
const Eigen::Vector3d& in, geometry_msgs::Vector3& out)
168 void fromMsg(
const geometry_msgs::Vector3& msg, Eigen::Vector3d& out)
185 const geometry_msgs::TransformStamped& transform) {
187 transform.header.stamp,
188 transform.header.frame_id);
199 geometry_msgs::PointStamped msg;
200 msg.header.stamp = in.
stamp_;
202 msg.point =
toMsg(static_cast<const Eigen::Vector3d&>(in));
213 out.
stamp_ = msg.header.stamp;
215 fromMsg(msg.point, static_cast<Eigen::Vector3d&>(out));
230 Eigen::Affine3d& t_out,
231 const geometry_msgs::TransformStamped& transform) {
238 Eigen::Isometry3d& t_out,
239 const geometry_msgs::TransformStamped& transform) {
249 geometry_msgs::Quaternion
toMsg(
const Eigen::Quaterniond& in) {
250 geometry_msgs::Quaternion msg;
264 void fromMsg(
const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) {
265 out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
280 Eigen::Quaterniond& t_out,
281 const geometry_msgs::TransformStamped& transform) {
282 Eigen::Quaterniond t;
283 fromMsg(transform.transform.rotation, t);
284 t_out = t.inverse() * t_in * t;
294 geometry_msgs::QuaternionStamped msg;
295 msg.header.stamp = in.
stamp_;
297 msg.quaternion =
toMsg(static_cast<const Eigen::Quaterniond&>(in));
309 out.
stamp_ = msg.header.stamp;
310 fromMsg(msg.quaternion, static_cast<Eigen::Quaterniond&>(out));
323 const geometry_msgs::TransformStamped& transform) {
324 t_out.
frame_id_ = transform.header.frame_id;
325 t_out.
stamp_ = transform.header.stamp;
326 doTransform(static_cast<const Eigen::Quaterniond&>(t_in), static_cast<Eigen::Quaterniond&>(t_out), transform);
335 geometry_msgs::Pose
toMsg(
const Eigen::Affine3d& in) {
336 geometry_msgs::Pose msg;
337 msg.position.x = in.translation().x();
338 msg.position.y = in.translation().y();
339 msg.position.z = in.translation().z();
340 Eigen::Quaterniond q(in.linear());
341 msg.orientation.x = q.x();
342 msg.orientation.y = q.y();
343 msg.orientation.z = q.z();
344 msg.orientation.w = q.w();
345 if (msg.orientation.w < 0) {
346 msg.orientation.x *= -1;
347 msg.orientation.y *= -1;
348 msg.orientation.z *= -1;
349 msg.orientation.w *= -1;
360 geometry_msgs::Pose
toMsg(
const Eigen::Isometry3d& in) {
361 geometry_msgs::Pose msg;
362 msg.position.x = in.translation().x();
363 msg.position.y = in.translation().y();
364 msg.position.z = in.translation().z();
365 Eigen::Quaterniond q(in.linear());
366 msg.orientation.x = q.x();
367 msg.orientation.y = q.y();
368 msg.orientation.z = q.z();
369 msg.orientation.w = q.w();
370 if (msg.orientation.w < 0) {
371 msg.orientation.x *= -1;
372 msg.orientation.y *= -1;
373 msg.orientation.z *= -1;
374 msg.orientation.w *= -1;
385 void fromMsg(
const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
386 out = Eigen::Affine3d(
387 Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
388 Eigen::Quaterniond(msg.orientation.w,
400 void fromMsg(
const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
401 out = Eigen::Isometry3d(
402 Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
403 Eigen::Quaterniond(msg.orientation.w,
415 geometry_msgs::Twist
toMsg(
const Eigen::Matrix<double,6,1>& in) {
416 geometry_msgs::Twist msg;
417 msg.linear.x = in[0];
418 msg.linear.y = in[1];
419 msg.linear.z = in[2];
420 msg.angular.x = in[3];
421 msg.angular.y = in[4];
422 msg.angular.z = in[5];
432 void fromMsg(
const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
433 out[0] = msg.linear.x;
434 out[1] = msg.linear.y;
435 out[2] = msg.linear.z;
436 out[3] = msg.angular.x;
437 out[4] = msg.angular.y;
438 out[5] = msg.angular.z;
454 const geometry_msgs::TransformStamped& transform) {
471 const geometry_msgs::TransformStamped& transform) {
483 geometry_msgs::PoseStamped msg;
484 msg.header.stamp = in.
stamp_;
486 msg.pose =
toMsg(static_cast<const Eigen::Affine3d&>(in));
493 geometry_msgs::PoseStamped msg;
494 msg.header.stamp = in.
stamp_;
496 msg.pose =
toMsg(static_cast<const Eigen::Isometry3d&>(in));
508 out.
stamp_ = msg.header.stamp;
510 fromMsg(msg.pose, static_cast<Eigen::Affine3d&>(out));
516 out.
stamp_ = msg.header.stamp;
518 fromMsg(msg.pose, static_cast<Eigen::Isometry3d&>(out));
534 geometry_msgs::Pose
toMsg(
const Eigen::Affine3d& in) {
539 geometry_msgs::Pose
toMsg(
const Eigen::Isometry3d& in) {
544 void fromMsg(
const geometry_msgs::Point& msg, Eigen::Vector3d& out) {
549 geometry_msgs::Point
toMsg(
const Eigen::Vector3d& in) {
554 void fromMsg(
const geometry_msgs::Pose& msg, Eigen::Affine3d& out) {
559 void fromMsg(
const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) {
564 geometry_msgs::Quaternion
toMsg(
const Eigen::Quaterniond& in) {
569 void fromMsg(
const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) {
574 geometry_msgs::Twist
toMsg(
const Eigen::Matrix<double,6,1>& in) {
579 void fromMsg(
const geometry_msgs::Twist &msg, Eigen::Matrix<double,6,1>& out) {
585 #endif // TF2_EIGEN_H geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. ...
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
void fromMsg(const A &, B &b)
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
Convert a timestamped transform to the equivalent Eigen data type.