Verbose error messages in case of missing includes

Author: gleichdick

test_tf2/test/test_convert.cpp

@@ -41,13 +41,65 @@
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+
+
+// test of tf2 type traits
+static_assert(
+  !tf2::impl::MessageHasStdHeader<geometry_msgs::msg::Vector3>::value,
+  "MessageHasStdHeader traits error");
+static_assert(
+  tf2::impl::MessageHasStdHeader<geometry_msgs::msg::Vector3Stamped>::value,
+  "MessageHasStdHeader traits error");
+
+namespace
+{
+struct MyPODMessage
+{
+  int header;
+};
+
+template<typename Alloc>
+struct MyAllocMessage
+{
+  int header;
+};
+
+static_assert(
+  !tf2::impl::MessageHasStdHeader<MyPODMessage>::value,
+  "MessageHasStdHeader traits error");
+
+template<typename T>
+struct MyAllocator
+{
+  using value_type = T;
+  using size_type = unsigned int;
+  T * allocate(size_type);
+  void deallocate(T *, size_type);
+  template<class U>
+  struct rebind { typedef MyAllocator<U> other; };
+};
+using MyVec = geometry_msgs::msg::Vector3_<MyAllocator<void>>;
+using MyVecStamped = geometry_msgs::msg::Vector3Stamped_<MyAllocator<void>>;
+using MyMessage = MyAllocMessage<MyAllocator<void>>;
+
+static_assert(!tf2::impl::MessageHasStdHeader<MyVec>::value, "MessageHasStdHeader traits error");
+static_assert(
+  tf2::impl::MessageHasStdHeader<MyVecStamped>::value,
+  "MessageHasStdHeader traits error");
+
+static_assert(
+  !tf2::impl::MessageHasStdHeader<MyMessage>::value,
+  "MessageHasStdHeader traits error");
+}
+
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 
 TEST(tf2Convert, kdlToBullet)
 {
   double epsilon = 1e-9;
 
-  tf2::Stamped<btVector3> b(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame");
+  tf2::Stamped<btVector3> b(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame");
 
   tf2::Stamped<btVector3> b1 = b;
   tf2::Stamped<KDL::Vector> k1;
@@ -74,7 +126,7 @@ TEST(tf2Convert, kdlBulletROSConversions)
 {
   double epsilon = 1e-9;
 
-  tf2::Stamped<btVector3> b1(btVector3(1,2,3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4;
+  tf2::Stamped<btVector3> b1(btVector3(1, 2, 3), tf2::timeFromSec(0), "my_frame"), b2, b3, b4;
   geometry_msgs::msg::PointStamped r1, r2, r3;
   tf2::Stamped<KDL::Vector> k1, k2, k3;
 
@@ -101,7 +153,7 @@ TEST(tf2Convert, kdlBulletROSConversions)
 
 TEST(tf2Convert, ConvertTf2Quaternion)
 {
-  tf2::Quaternion tq(1,2,3,4);
+  tf2::Quaternion tq(1, 2, 3, 4);
   Eigen::Quaterniond eq;
   tf2::convert(tq, eq);
 
@@ -117,7 +169,7 @@ TEST(tf2Convert, PointVectorDefaultMessagetype)
   // as it can return a Vector3 or a Point for certain datatypes
   {
     // Bullet
-    const tf2::Stamped<btVector3> b1{btVector3{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame" };
+    const tf2::Stamped<btVector3> b1{btVector3{1.0, 3.0, 4.0}, tf2::get_now(), "my_frame"};
     const geometry_msgs::msg::PointStamped msg = tf2::toMsg(b1);
 
     EXPECT_EQ(msg.point.x, 1.0);
@@ -162,7 +214,7 @@ TEST(tf2Convert, PointVectorOtherMessagetype)
   {
     const tf2::Vector3 t1{2.0, 4.0, 5.0};
     geometry_msgs::msg::Point msg;
-    const geometry_msgs::msg::Point& msg2 = tf2::toMsg(t1, msg);
+    const geometry_msgs::msg::Point & msg2 = tf2::toMsg(t1, msg);
 
     // returned reference is second argument
     EXPECT_EQ(&msg2, &msg);
@@ -174,7 +226,7 @@ TEST(tf2Convert, PointVectorOtherMessagetype)
     // Eigen
     const Eigen::Vector3d e1{2.0, 4.0, 5.0};
     geometry_msgs::msg::Vector3 msg;
-    const geometry_msgs::msg::Vector3& msg2 = tf2::toMsg(e1, msg);
+    const geometry_msgs::msg::Vector3 & msg2 = tf2::toMsg(e1, msg);
 
     // returned reference is second argument
     EXPECT_EQ(&msg2, &msg);
@@ -286,7 +338,7 @@ TEST(TfEigenKdl, TestVector3dVector)
   EXPECT_EQ(eigen_v, eigen_v1);
 }
 
-int main(int argc, char** argv)
+int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();

tf2/include/tf2/impl/convert.h

@@ -32,14 +32,49 @@
 #define TF2__IMPL__CONVERT_H_
 
 #include <geometry_msgs/msg/quaternion.hpp>
+#include <std_msgs/msg/header.hpp>
 
 #include "../time.h"
 #include "forward.h"
 
+#include <type_traits>
+
+
 namespace tf2
 {
 namespace impl
 {
+
+/// Helper to always trigger \c static_assert s
+template<typename T>
+constexpr bool alwaysFalse = false;
+
+/** \brief Check whether a message is stamped and has a std_msgs::msg::Header member.
+ *
+ * It will be indicated with the static member valiable \c value .
+ * \tparam Message The message to check
+ */
+template<typename Message, typename = int>
+struct MessageHasStdHeader : std::false_type {};
+
+/** \brief Check whether a message is stamped and has a std_msgs::msg::Header member.
+ *
+ * It will be indicated with the static member valiable \c value .
+ * \tparam Message The message to check
+ */
+template<typename Message>
+class MessageHasStdHeader<Message, decltype(&Message::header, 0)>
+{
+  template<typename Alloc>
+  static std::true_type headerIsStdHeader(std_msgs::msg::Header_<Alloc>);
+  template<typename = void>
+  static std::false_type headerIsStdHeader(...);
+
+public:
+  /// true if Message has a member \c header of type std_msgs::msg::Header.
+  static constexpr bool value = decltype(headerIsStdHeader(std::declval<Message>().header))::value;
+};
+
 /**
  * \brief Mapping between Datatypes (like \c Vector3d ) and their default ROS Message types.
  *
@@ -52,6 +87,9 @@ namespace impl
 template<class Datatype, class>
 struct DefaultMessageForDatatype
 {
+  static_assert(
+    alwaysFalse<Datatype>, "No default message type defined, "
+    "please check your header file includes!");
   // using type = ...;
 };
 
@@ -73,6 +111,9 @@ struct DefaultMessageForDatatype
 template<class Datatype, class>
 struct DefaultTransformType
 {
+  static_assert(
+    alwaysFalse<Datatype>, "No default transform type defined, "
+    "please check your header file includes!");
   // using type = ...;
 };
 
@@ -100,6 +141,9 @@ struct DefaultTransformType
 template<class Datatype, class Message, class>
 struct ConversionImplementation
 {
+  static_assert(
+    alwaysFalse<Datatype>, "No Conversion Implementation available, "
+    "please check your header file includes!");
   // void toMsg(const Datatype&, Message&);
   // void fromMsg(const Message&, Datatype&);
 };
@@ -140,6 +184,9 @@ struct ConversionImplementation
 template<class StampedMessage>
 struct StampedMessageTraits
 {
+  static_assert(
+    alwaysFalse<StampedMessage>, "No traits for this stamped message type available, "
+    "please check your header file includes!");
   // using UntampedType = ...;
   // static UntampedType& accessMessage(StampedMsg &);
   // static UntampedType getMessage(StampedMsg const&);
@@ -172,6 +219,9 @@ struct StampedMessageTraits
 template<class UnstampedMessage>
 struct UnstampedMessageTraits
 {
+  static_assert(
+    alwaysFalse<UnstampedMessage>, "No traits for this message type available, "
+    "please check your header file includes!");
   // using StampedType = ...;
   // using StampedTypeWithCovariance = ...;
 };
@@ -364,41 +414,36 @@ inline void Converter<false, false>::convert(const A & a, B & b)
   fromMsg<>(toMsg<>(a), b);
 }
 
-template<typename T>
-using void_t = void;
-
 /**
  * \brief Default implementation for extracting timestamps and frame IDs.
  *
  * Both static member functions are for stamped ROS messages.
- * They are SFINAE'd out if T is not a stamped ROS message.
  *
  * \tparam T Arbitrary datatype
  */
 template<typename T, int>
 struct StampedAttributesHelper
 {
+  static_assert(
+    MessageHasStdHeader<T>::value,
+    "Trying to use default implementation for stamped message, "
+    "but the datatype does not have a Header member.");
+
   /**\brief Get the timestamp from data
-   * \param t The data input.
+   * \param[in] t The data input.
    * \return The timestamp associated with the data.
-   *
-   * The second parameter is needed to hide the default implementation if T is not a stamped ROS message.
    */
-  static tf2::TimePoint getTimestamp(
-    const T & t, void_t<typename StampedMessageTraits<T>::UntampedType> * = nullptr)
+  static tf2::TimePoint getTimestamp(const T & t)
   {
     tf2::TimePoint timestamp;
     tf2::fromMsg<>(t.header.stamp, timestamp);
     return timestamp;
   }
   /**\brief Get the frame_id from data
-   * \param t The data input.
+   * \param[in] t The data input.
    * \return The frame_id associated with the data.
-   *
-   * The second parameter is needed to hide the default implementation if T is not a stamped ROS message.
    */
-  static std::string getFrameId(
-    const T & t, void_t<typename StampedMessageTraits<T>::UntampedType> * = nullptr)
+  static std::string getFrameId(const T & t)
   {
     return t.header.frame_id;
   }