tf2_eigen uncrustify

Author: gleichdick

tf2_eigen/include/tf2_eigen/tf2_eigen.h

@@ -26,8 +26,8 @@
 
 /** \author Koji Terada, Bjarne von Horn */
 
-#ifndef TF2_EIGEN_H
-#define TF2_EIGEN_H
+#ifndef TF2_EIGEN__TF2_EIGEN_H_
+#define TF2_EIGEN__TF2_EIGEN_H_
 
 #include <tf2/convert.h>
 #include <tf2/transform_datatypes.h>
@@ -59,7 +59,8 @@ inline Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform &
  * \return The transform message converted to an Eigen Isometry3d transform.
  */
 inline
-Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::TransformStamped& t) {
+Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::TransformStamped & t)
+{
   return transformToEigen(t.transform);
 }
 
@@ -147,7 +148,6 @@ struct ImplDetails<Eigen::Vector3d, geometry_msgs::msg::Vector3>
 {
 };
 
-
 /// \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d.
 template<>
 struct DefaultTransformType<Eigen::Vector3d>
@@ -157,7 +157,7 @@ struct DefaultTransformType<Eigen::Vector3d>
 };
 
 /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d.
-template <>
+template<>
 struct defaultMessage<Eigen::Vector3d>
 {
   /// \brief Default return type of tf2::toMsg() for Eigen::Vector3d.
@@ -168,7 +168,6 @@ struct defaultMessage<Eigen::Vector3d>
 template<>
 struct ImplDetails<Eigen::Quaterniond, geometry_msgs::msg::Quaternion>
 {
-
   /** \brief Convert a Eigen Quaterniond type to a Quaternion message.
    * \param[in] in The Eigen Quaterniond to convert.
    * \param[out] msg The quaternion converted to a Quaterion message.
@@ -192,7 +191,7 @@ struct ImplDetails<Eigen::Quaterniond, geometry_msgs::msg::Quaternion>
 };
 
 /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond.
-template <>
+template<>
 struct defaultMessage<Eigen::Quaterniond>
 {
   /// \brief Default return type of tf2::toMsg() for Eigen::Quaterniond.
@@ -227,7 +226,7 @@ namespace impl
 /** \brief Pose conversion template for Eigen types.
  * \tparam T Eigen transform type.
  */
-template <typename T>
+template<typename T>
 struct EigenPoseImpl
 {
   /** \brief Convert a Eigen transform type to a Pose message.
@@ -238,7 +237,7 @@ struct EigenPoseImpl
   {
     tf2::toMsg<>(Eigen::Vector3d(in.translation()), msg.position);
     Eigen::Quaterniond q(in.linear());
-    if (q.w() < 0) q.coeffs() *= -1;
+    if (q.w() < 0) {q.coeffs() *= -1;}
     tf2::toMsg<>(q, msg.orientation);
   }
 
@@ -257,37 +256,37 @@ struct EigenPoseImpl
 };
 
 /// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Affine3d.
-template <>
+template<>
 struct ImplDetails<Eigen::Affine3d, geometry_msgs::msg::Pose>
-: public EigenPoseImpl<Eigen::Affine3d>
+  : public EigenPoseImpl<Eigen::Affine3d>
 {
 };
 
 /// \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Isometry3d.
-template <>
+template<>
 struct ImplDetails<Eigen::Isometry3d, geometry_msgs::msg::Pose>
-: public EigenPoseImpl<Eigen::Isometry3d>
+  : public EigenPoseImpl<Eigen::Isometry3d>
 {
 };
 
 /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
-template <>
+template<>
 struct defaultMessage<Eigen::Affine3d>
 {
   /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
   using type = geometry_msgs::msg::Pose;
 };
 
 /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
-template <>
+template<>
 struct defaultMessage<Eigen::Isometry3d>
 {
   /// \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
   using type = geometry_msgs::msg::Pose;
 };
 
 /// \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix<double, 6, 1>.
-template <>
+template<>
 struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Twist>
 {
   /** \brief Convert a Eigen 6x1 Matrix type to a Twist message.
@@ -320,21 +319,21 @@ struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Twist>
 };
 
 /// \brief Default return type of tf2::toMsg() for Eigen::Matrix<double, 6, 1>.
-template <>
+template<>
 struct defaultMessage<Eigen::Matrix<double, 6, 1>>
 {
   /// \brief Default return type of tf2::toMsg() for Eigen::Matrix<double, 6, 1>.
   using type = geometry_msgs::msg::Twist;
 };
 
 /// \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix<double, 6, 1>.
-template <>
+template<>
 struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Wrench>
 {
-   /** \brief Convert a Eigen 6x1 Matrix type to a Wrench message.
-   * \param[in] in The 6x1 Eigen Matrix to convert.
-   * \param[out] msg The Eigen Matrix converted to a Wrench message.
-   */
+  /** \brief Convert a Eigen 6x1 Matrix type to a Wrench message.
+  * \param[in] in The 6x1 Eigen Matrix to convert.
+  * \param[out] msg The Eigen Matrix converted to a Wrench message.
+  */
   static void toMsg(const Eigen::Matrix<double, 6, 1> & in, geometry_msgs::msg::Wrench & msg)
   {
     msg.force.x = in[0];
@@ -389,11 +388,11 @@ struct DefaultTransformType<Eigen::Isometry3d>
 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);
+  msg.x = in[0];
+  msg.y = in[1];
+  msg.z = in[2];
   return msg;
 }
 }  // namespace tf2
 
-#endif  // TF2_EIGEN_H
+#endif  // TF2_EIGEN__TF2_EIGEN_H_

tf2_eigen/test/tf2_eigen-test.cpp

@@ -1,6 +1,5 @@
 /*
- * Copyright (c) Koji Terada
- * All rights reserved.
+ * Copyright (c) Koji Terada. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -34,20 +33,26 @@
 
 // To get M_PI, especially on Windows.
 #include <math.h>
-#include <memory>
 
 #include <tf2_eigen/tf2_eigen.h>
 #include <gtest/gtest.h>
 #include <rclcpp/clock.hpp>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/buffer.h>
 #include <tf2/convert.h>
+
+
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <geometry_msgs/msg/quaternion_stamped.hpp>
 
+#include <memory>
+
+
 TEST(TfEigen, ConvertVector3dStamped)
 {
-  const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1,2,3), tf2::TimePoint(std::chrono::seconds(5)), "test");
+  const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1, 2, 3), tf2::TimePoint(
+      std::chrono::seconds(
+        5)), "test");
 
   tf2::Stamped<Eigen::Vector3d> v1;
   geometry_msgs::msg::PointStamped p1;
@@ -59,7 +64,7 @@ TEST(TfEigen, ConvertVector3dStamped)
 
 TEST(TfEigen, ConvertVector3d)
 {
-  const Eigen::Vector3d v(1,2,3);
+  const Eigen::Vector3d v(1, 2, 3);
 
   Eigen::Vector3d v1;
   geometry_msgs::msg::Point p1;
@@ -71,8 +76,12 @@ TEST(TfEigen, ConvertVector3d)
 
 TEST(TfEigen, ConvertAffine3dStamped)
 {
-  const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
-  const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, tf2::TimePoint(std::chrono::seconds(42)), "test_frame");
+  const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis<double>(
+      1,
+      Eigen::Vector3d::UnitX()));
+  const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, tf2::TimePoint(
+      std::chrono::seconds(
+        42)), "test_frame");
 
   tf2::Stamped<Eigen::Affine3d> v1;
   geometry_msgs::msg::PoseStamped p1;
@@ -87,7 +96,9 @@ TEST(TfEigen, ConvertAffine3dStamped)
 
 TEST(TfEigen, ConvertAffine3d)
 {
-  const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
+  const Eigen::Affine3d v(Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxis<double>(
+      1,
+      Eigen::Vector3d::UnitX()));
 
   Eigen::Affine3d v1;
   geometry_msgs::msg::Pose p1;
@@ -102,14 +113,20 @@ TEST(TfEigen, ConvertTransform)
 {
   Eigen::Matrix4d tm;
 
-  double alpha = M_PI/4.0;
-  double theta = M_PI/6.0;
-  double gamma = M_PI/12.0;
-
-  tm << cos(theta)*cos(gamma),-cos(theta)*sin(gamma),sin(theta), 1, //
-  cos(alpha)*sin(gamma)+sin(alpha)*sin(theta)*cos(gamma),cos(alpha)*cos(gamma)-sin(alpha)*sin(theta)*sin(gamma),-sin(alpha)*cos(theta), 2, //
-  sin(alpha)*sin(gamma)-cos(alpha)*sin(theta)*cos(gamma),cos(alpha)*sin(theta)*sin(gamma)+sin(alpha)*cos(gamma),cos(alpha)*cos(theta), 3, //
-  0, 0, 0, 1;
+  double alpha = M_PI / 4.0;
+  double theta = M_PI / 6.0;
+  double gamma = M_PI / 12.0;
+
+  tm << cos(theta) * cos(gamma), -cos(theta) * sin(gamma), sin(theta), 1,  //
+    //
+    cos(alpha) * sin(gamma) + sin(alpha) * sin(theta) * cos(gamma),  //
+    cos(alpha) * cos(gamma) - sin(alpha) * sin(theta) * sin(gamma),  //
+    -sin(alpha) * cos(theta), 2,                                     //
+    //
+    sin(alpha) * sin(gamma) - cos(alpha) * sin(theta) * cos(gamma),  //
+    cos(alpha) * sin(theta) * sin(gamma) + sin(alpha) * cos(gamma),  //
+    cos(alpha) * cos(theta), 3,                                      //
+    0, 0, 0, 1;
 
   Eigen::Affine3d T(tm);
 
@@ -251,24 +268,26 @@ TEST_F(EigenBufferTransform, Vector)
 
 TEST_F(EigenBufferTransform, Quaternion)
 {
-  using namespace Eigen;
   // rotated by -90° around y
   // 0, 0, -1
   // 0, 1, 0,
   // 1, 0, 0
-  const tf2::Stamped<Quaterniond> q1 {{Quaterniond(AngleAxisd(-0.5 * M_PI, Vector3d::UnitY()))},
+  const tf2::Stamped<Eigen::Quaterniond> q1 {{Eigen::Quaterniond(
+      Eigen::AngleAxisd(
+        -0.5 * M_PI,
+        Eigen::Vector3d::UnitY()))},
     tf2::toMsg(tf2::timeFromSec(2)), "A"};
 
   // simple api
-  const tf2::Stamped<Quaterniond> q_simple =
+  const tf2::Stamped<Eigen::Quaterniond> q_simple =
     tf_buffer->transform(q1, "B", tf2::durationFromSec(2.0));
   EXPECT_NEAR(q_simple.x(), 0.707107, EPS);
   EXPECT_NEAR(q_simple.y(), 0, EPS);
   EXPECT_NEAR(q_simple.z(), -0.707107, EPS);
   EXPECT_NEAR(q_simple.w(), 0, EPS);
 
   // advanced api
-  const tf2::Stamped<Quaterniond> q_advanced = tf_buffer->transform(
+  const tf2::Stamped<Eigen::Quaterniond> q_advanced = tf_buffer->transform(
     q1, "B", tf2::timeFromSec(2.0),
     "A", tf2::durationFromSec(3.0));
   EXPECT_NEAR(q_advanced.x(), 0.707107, EPS);