Enable Transform and Quaternion for bullet

Author: gleichdick

tf2_bullet/include/tf2_bullet/tf2_bullet.h

@@ -37,6 +37,7 @@
 #include <tf2/transform_datatypes.h>
 
 #include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
 
 #if (BT_BULLET_VERSION <= 282)
 // Suppress compilation warning on older versions of Bullet.
@@ -49,41 +50,6 @@ inline int bullet_btInfinityMask()
 
 namespace tf2
 {
-/** \brief Convert a timestamped transform to the equivalent Bullet data type.
- * \param t The transform to convert, as a geometry_msgs TransformedStamped message.
- * \return The transform message converted to a Bullet btTransform.
- */
-  inline
-  btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
-  {
-    return btTransform(
-      btQuaternion(
-        static_cast < float > (t.transform.rotation.x),
-        static_cast < float > (t.transform.rotation.y),
-        static_cast < float > (t.transform.rotation.z),
-        static_cast < float > (t.transform.rotation.w)),
-      btVector3(
-        static_cast < float > (t.transform.translation.x),
-        static_cast < float > (t.transform.translation.y),
-        static_cast < float > (t.transform.translation.z)));
-  }
-
-
-/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 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 Bullet btVector3 data type.
- * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template <>
-inline void doTransform(
-  const tf2::Stamped<btVector3> & t_in, tf2::Stamped<btVector3> & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
-{
-  t_out = tf2::Stamped<btVector3>(
-    transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id);
-}
-
 namespace impl
 {
 template <>
@@ -93,14 +59,26 @@ struct defaultMessage<btVector3>
 };
 
 template <>
-struct ImplDetails<btVector3, geometry_msgs::msg::Point>
+struct defaultMessage<btQuaternion>
+{
+  using type = geometry_msgs::msg::Quaternion;
+};
+
+template <>
+struct defaultMessage<btTransform>
+{
+  using type = geometry_msgs::msg::Transform;
+};
+
+template <class Message>
+struct BulletVectorImpl
 {
   /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message.
    * This function is a specialization of the toMsg template defined in tf2/convert.h
    * \param in The timestamped Bullet btVector3 to convert.
    * \return The vector converted to a PointStamped message.
    */
-  static void toMsg(const btVector3 & in, geometry_msgs::msg::Point & msg)
+  static void toMsg(const btVector3 & in, Message & msg)
   {
     msg.x = in[0];
     msg.y = in[1];
@@ -112,15 +90,93 @@ struct ImplDetails<btVector3, geometry_msgs::msg::Point>
    * \param msg The PointStamped message to convert.
    * \param out The point converted to a timestamped Bullet Vector3.
    */
-  static void fromMsg(const geometry_msgs::msg::Point & msg, btVector3 & out)
+  static void fromMsg(const Message & msg, btVector3 & out)
+  {
+    out[0] = msg.x;
+    out[1] = msg.y;
+    out[2] = msg.z;
+  }
+};
+
+template <>
+struct ImplDetails<btVector3, geometry_msgs::msg::Point>
+: BulletVectorImpl<geometry_msgs::msg::Point>
+{
+};
+
+template <>
+struct ImplDetails<btVector3, geometry_msgs::msg::Vector3>
+: BulletVectorImpl<geometry_msgs::msg::Vector3>
+{
+};
+
+template <>
+struct ImplDetails<btQuaternion, geometry_msgs::msg::Quaternion>
+{
+  static void toMsg(const btQuaternion & in, geometry_msgs::msg::Quaternion & msg)
+  {
+    msg.x = in[0];
+    msg.y = in[1];
+    msg.z = in[2];
+    msg.w = in[3];
+  }
+
+  static void fromMsg(const geometry_msgs::msg::Quaternion & msg, btQuaternion & out)
   {
     out[0] = msg.x;
     out[1] = msg.y;
     out[2] = msg.z;
+    out[3] = msg.w;
+    if (msg.w < 0) out *= -1;
+  }
+};
+
+template <>
+struct ImplDetails<btTransform, geometry_msgs::msg::Transform>
+{
+  static void fromMsg(geometry_msgs::msg::Transform const & in, btTransform & out)
+  {
+    btVector3 trans;
+    btQuaternion rot;
+    tf2::fromMsg<>(in.rotation, rot);
+    tf2::fromMsg<>(in.translation, trans);
+    out = btTransform(rot, trans);
+  }
+
+  static void toMsg(btTransform const & in, geometry_msgs::msg::Transform & out)
+  {
+    tf2::toMsg<>(in.getRotation(), out.rotation);
+    tf2::toMsg<>(in.getOrigin(), out.translation);
   }
 };
 }  // namespace impl
 
+/** \brief Convert a timestamped transform to the equivalent Bullet data type.
+ * \param t The transform to convert, as a geometry_msgs TransformedStamped message.
+ * \return The transform message converted to a Bullet btTransform.
+ */
+inline btTransform transformToBullet(const geometry_msgs::msg::TransformStamped & t)
+{
+  btTransform ans;
+  fromMsg<>(t.transform, ans);
+  return ans;
+}
+
+/** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 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 Bullet btVector3 data type.
+ * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template <>
+inline void doTransform(
+  const tf2::Stamped<btVector3> & t_in, tf2::Stamped<btVector3> & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  t_out = tf2::Stamped<btVector3>(
+    transformToBullet(transform) * t_in, transform.header.stamp, transform.header.frame_id);
+}
+
 /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type.
  * This function is a specialization of the doTransform template defined in tf2/convert.h
  * \param t_in The frame to transform, as a timestamped Bullet btTransform.

tf2_bullet/test/test_tf2_bullet.cpp

@@ -51,6 +51,22 @@ TEST(TfBullet, ConvertVector)
   EXPECT_EQ(v1, v2);
 }
 
+TEST(TfBullet, ConvertTransform)
+{
+  geometry_msgs::msg::Transform transform;
+  transform.rotation.x = 1.0;
+  transform.rotation.w = 0.0;
+  transform.translation.z = 2.0;
+
+  btTransform transform_bt;
+  tf2::convert(transform, transform_bt);
+  EXPECT_EQ(transform_bt.getRotation(), btQuaternion(1.0f, 0.0f, 0.0f, 0.0f));
+  EXPECT_EQ(transform_bt.getOrigin(), btVector3(0.0f, 0.0f, 2.0f));
+
+  geometry_msgs::msg::Transform transform2;
+  tf2::convert(transform_bt, transform2);
+  EXPECT_EQ(transform2, transform);
+}
 
 int main(int argc, char ** argv)
 {