Reorder headers

Author: gleichdick

tf2/include/tf2/impl/convert.h

@@ -63,7 +63,7 @@ struct MessageHasStdHeader : std::false_type {};
  * \tparam Message The message to check
  */
 template<typename Message>
-class MessageHasStdHeader<Message, decltype(&Message::header, 0)>
+class MessageHasStdHeader<Message, decltype(static_cast<void>(Message::header), 0)>
 {
   template<typename Alloc>
   static std::true_type headerIsStdHeader(std_msgs::msg::Header_<Alloc>);

tf2_eigen/include/tf2_eigen/tf2_eigen.h

@@ -91,51 +91,9 @@ geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isometry3d &
 
 namespace impl
 {
-/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion.
- * \tparam mode Mode argument for Eigen::Transform template
- */
-template<int mode>
-struct EigenTransformImpl
-{
-  /** \brief Convert a Transform message to an Eigen type.
-   * \param[in] msg The message to convert.
-   * \param[out] out The converted message, as an Eigen type.
-   */
-  static void fromMsg(
-    const geometry_msgs::msg::Transform & msg, Eigen::Transform<double, 3, mode> & out)
-  {
-    Eigen::Quaterniond q;
-    Eigen::Vector3d v;
-    tf2::fromMsg<>(msg.rotation, q);
-    tf2::fromMsg<>(msg.translation, v);
-    out = Eigen::Translation3d(v) * q;
-  }
-
-  /** \brief Convert an Eigen Transform to a Transform message.
-   * \param[in] in The Eigen Transform to convert.
-   * \param[out] msg The converted Transform, as a message.
-   */
-  static void toMsg(
-    const Eigen::Transform<double, 3, mode> & in, geometry_msgs::msg::Transform & msg)
-  {
-    tf2::toMsg<>(Eigen::Quaterniond(in.linear()), msg.rotation);
-    tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.translation);
-  }
-};
-
-/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d.
-template<>
-struct ConversionImplementation<Eigen::Affine3d, geometry_msgs::msg::Transform>
-  : EigenTransformImpl<Eigen::Affine>
-{
-};
-
-/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d.
-template<>
-struct ConversionImplementation<Eigen::Isometry3d, geometry_msgs::msg::Transform>
-  : EigenTransformImpl<Eigen::Isometry>
-{
-};
+// ---------------------
+// Vector
+// ---------------------
 
 /// \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d.
 template<>
@@ -167,6 +125,11 @@ struct DefaultMessageForDatatype<Eigen::Vector3d>
   using type = geometry_msgs::msg::Point;
 };
 
+
+// ---------------------
+// Quaternion
+// ---------------------
+
 /// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond.
 template<>
 struct ConversionImplementation<Eigen::Quaterniond, geometry_msgs::msg::Quaternion>
@@ -200,51 +163,57 @@ struct DefaultMessageForDatatype<Eigen::Quaterniond>
   /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond.
   using type = geometry_msgs::msg::Quaternion;
 };
-}  // namespace impl
 
-/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h,
- * although it can not be used in tf2_ros::BufferInterface::transform because this
- * functions rely on the existence of a time stamp and a frame id in the type which should
- * get transformed.
- * \param t_in The vector to transform, as a Eigen Quaterniond data type.
- * \param t_out The transformed vector, as a Eigen Quaterniond data type.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
+
+// ---------------------
+// Transform
+// ---------------------
+
+/** \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion.
+ * \tparam mode Mode argument for Eigen::Transform template
  */
-template<>
-inline
-void doTransform(
-  const Eigen::Quaterniond & t_in,
-  Eigen::Quaterniond & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
+template<int mode>
+struct EigenTransformImpl
 {
-  Eigen::Quaterniond t;
-  fromMsg(transform.transform.rotation, t);
-  t_out = t * t_in;
-}
+  /** \brief Convert a Transform message to an Eigen type.
+   * \param[in] msg The message to convert.
+   * \param[out] out The converted message, as an Eigen type.
+   */
+  static void fromMsg(
+    const geometry_msgs::msg::Transform & msg, Eigen::Transform<double, 3, mode> & out)
+  {
+    Eigen::Quaterniond q;
+    Eigen::Vector3d v;
+    tf2::fromMsg<>(msg.rotation, q);
+    tf2::fromMsg<>(msg.translation, v);
+    out = Eigen::Translation3d(v) * q;
+  }
 
-/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type.
- * This function is a specialization of the doTransform template defined in tf2/convert.h.
- * \param t_in The vector to transform, as a timestamped Eigen Quaterniond data type.
- * \param t_out The transformed vector, as a timestamped Eigen Quaterniond data type.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
+  /** \brief Convert an Eigen Transform to a Transform message.
+   * \param[in] in The Eigen Transform to convert.
+   * \param[out] msg The converted Transform, as a message.
+   */
+  static void toMsg(
+    const Eigen::Transform<double, 3, mode> & in, geometry_msgs::msg::Transform & msg)
+  {
+    tf2::toMsg<>(Eigen::Quaterniond(in.linear()), msg.rotation);
+    tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.translation);
+  }
+};
+
+/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d.
 template<>
-inline
-void doTransform(
-  const tf2::Stamped<Eigen::Quaterniond> & t_in,
-  tf2::Stamped<Eigen::Quaterniond> & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
+struct ConversionImplementation<Eigen::Affine3d, geometry_msgs::msg::Transform>
+  : EigenTransformImpl<Eigen::Affine>
 {
-  t_out.frame_id_ = transform.header.frame_id;
-  tf2::fromMsg(transform.header.stamp, t_out.stamp_);
-  doTransform(
-    static_cast<const Eigen::Quaterniond &>(t_in),
-    static_cast<Eigen::Quaterniond &>(t_out), transform);
-}
+};
 
-namespace impl
+/// \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d.
+template<>
+struct ConversionImplementation<Eigen::Isometry3d, geometry_msgs::msg::Transform>
+  : EigenTransformImpl<Eigen::Isometry>
 {
+};
 
 /** \brief Pose conversion template for Eigen types.
  * \tparam T Eigen transform type.
@@ -308,6 +277,11 @@ struct DefaultMessageForDatatype<Eigen::Isometry3d>
   using type = geometry_msgs::msg::Pose;
 };
 
+
+// ---------------------
+// Twist
+// ---------------------
+
 /// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix<double, 6, 1>.
 template<>
 struct ConversionImplementation<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Twist>
@@ -349,6 +323,11 @@ struct DefaultMessageForDatatype<Eigen::Matrix<double, 6, 1>>
   using type = geometry_msgs::msg::Twist;
 };
 
+
+// ---------------------
+// Wrench
+// ---------------------
+
 /// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix<double, 6, 1>.
 template<>
 struct ConversionImplementation<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Wrench>
@@ -417,6 +396,27 @@ geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in)
   msg.z = in[2];
   return msg;
 }
+
+/** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h,
+ * although it can not be used in tf2_ros::BufferInterface::transform because this
+ * functions rely on the existence of a time stamp and a frame id in the type which should
+ * get transformed.
+ * \param[in] t_in The vector to transform, as a Eigen Quaterniond data type.
+ * \param[in,out] t_out The transformed vector, as a Eigen Quaterniond data type.
+ * \param[in] transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template<>
+inline
+void doTransform(
+  const Eigen::Quaterniond & t_in,
+  Eigen::Quaterniond & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  Eigen::Quaterniond t;
+  tf2::fromMsg<>(transform.transform.rotation, t);
+  t_out = Eigen::Quaterniond(t.toRotationMatrix() * t_in.toRotationMatrix());
+}
 }  // namespace tf2
 
 #endif  // TF2_EIGEN__TF2_EIGEN_H_