conversions for Eigen::Isometry3d in ROS 2

[vmayoral]

This commits fetch ros/geometry2#335 into ROS 2 and addresses the requests from moveit/moveit2#32 originated at moveit/moveit2#12

[davetcoleman]

@rhaschke please review since i believe this was originally yours

[rhaschke]

I'm not in ROS2, so I cannot judge the corresponding changes. The Eigen stuff looks reasonable.

[rhaschke]

If that's possible, flip the quaternion in a single line operation, instead of manually doing it below.

Suggested change

	Eigen::Quaterniond q(in.linear());
	Eigen::Quaterniond q(in.linear());
	if (q.w() < 0)
	q *= 1.0;

[tfoote]

I'm not sure that we want to be doing another operation if we can avoid it. Every extra operation is potentially a loss of precision as well as costing resources.

The negative w values are completely valid quaternions. I'd rather not just normalize it here. This breaks the expectation that fromMsg(toMsg(...)) will return the same value.

[davetcoleman]

closes moveit/moveit2#32

[vmayoral]

#90 has been merged but this PR still remains. @ahcorde, how do you want to proceed about this? AFAIK, these changes are needed to compile http://github.com/acutronicrobotics/moveit2

[clalancette]

@vmayoral The thing to do here is to rebase this on top of the current ros2 branch. Then this will be much easier to review. Thanks!

[vmayoral]

@vmayoral The thing to do here is to rebase this on top of the current ros2 branch. Then this will be much easier to review. Thanks!

Thanks @clalancette, I was expecting @ahcorde to take care of it since he's ensuring everything's aligned for the Dashing release but I went ahead and did it myself. @ahcorde can you have a look yourself at the changes as well please?

[tfoote]

This rebase looks like it has ended up with a lot of duplicate already merged content in the diff. It's over 1000 lines of diff and includes 44 commits several of which are in the last release or two including things like the changelogs making this diff indecipherable. Which are the commits in this branch that are adding new content to be reviewed?

I pulled it and computed the diff to look like this:

$ git diff ros2
diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h
index 8f8fd2e..9213915 100644
--- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h
+++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h
@@ -50,7 +50,7 @@ namespace tf2
  */
  inline
  Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform& t) {
-   return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z)
+ return Eigen::Isometry3d(Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z)
                         * Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
 }
 
@@ -224,12 +224,12 @@ void doTransform(const Eigen::Isometry3d& t_in,
  */
 inline
 geometry_msgs::msg::Quaternion toMsg(const Eigen::Quaterniond& in) {
- geometry_msgs::msg::Quaternion msg;
- msg.w = in.w();
- msg.x = in.x();
- msg.y = in.y();
- msg.z = in.z();
- return msg;
+  geometry_msgs::msg::Quaternion msg;
+  msg.w = in.w();
+  msg.x = in.x();
+  msg.y = in.y();
+  msg.z = in.z();
+  return msg;
 }
 
 /** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type.
@@ -341,22 +341,6 @@ geometry_msgs::msg::Pose toMsg(const Eigen::Isometry3d& in) {
   return msg;
 }
 
-/** \brief Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3.
- * This function is a specialization of the toMsg template defined in tf2/convert.h.
- * The function was renamed to toMsg2 to avoid overloading conflicts with definitions above.
- *
- * \param in The Eigen::Vector3d to convert.
- * \return The Eigen::Vector3d converted to a geometry_msgs::msg::Vector3 message.
- */
-inline
-geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d& in) {
-  geometry_msgs::msg::Vector3 msg;
-  msg.x = in(0);
-  msg.y = in(1);
-  msg.z = in(2);
-  return msg;
-}
-
 /** \brief Convert a Pose message transform type to a Eigen Affine3d.
  * This function is a specialization of the toMsg template defined in tf2/convert.h.
  * \param msg The Pose message to convert.

[dirk-thomas]

@vmayoral Friendly ping.

[vmayoral]

That's right @tfoote, sorry. Thanks for the ping @dirk-thomas.

Following at #113.