[3/7] Complete rework: bullet

gleichdick · gleichdick · commit 26de98d67c06 · 2022-12-16T22:12:14.000+01:00
diff --git a/tf2_bullet/CMakeLists.txt b/tf2_bullet/CMakeLists.txt
@@ -33,13 +33,16 @@ install(TARGETS tf2_bullet EXPORT export_tf2_bullet)
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_cppcheck REQUIRED)
   list(APPEND AMENT_LINT_AUTO_EXCLUDE
     ament_cmake_cppcheck
+    ament_cmake_uncrustify
   )
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_uncrustify REQUIRED)
+  find_package(ament_cmake_cppcheck REQUIRED)
   ament_lint_auto_find_test_dependencies()
-  ament_cppcheck(LANGUAGE c++)
+  ament_cppcheck(LANGUAGE "c++")
+  ament_uncrustify(LANGUAGE "c++")
 
   find_package(ament_cmake_gtest REQUIRED)
   ament_add_gtest(test_bullet test/test_tf2_bullet.cpp)
diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp b/tf2_bullet/include/tf2_bullet/tf2_bullet.hpp
@@ -26,18 +26,18 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-/** \author Wim Meeussen */
+/** \author Wim Meeussen, Bjarne von Horn */
 
 #ifndef TF2_BULLET__TF2_BULLET_HPP_
 #define TF2_BULLET__TF2_BULLET_HPP_
 
-#include <iostream>
-
 #include "tf2/convert.h"
+#include "tf2/transform_datatypes.h"
 #include "LinearMath/btQuaternion.h"
 #include "LinearMath/btScalar.h"
 #include "LinearMath/btTransform.h"
 #include "geometry_msgs/msg/point_stamped.hpp"
+#include "geometry_msgs/msg/vector3.hpp"
 #include "tf2_ros/buffer_interface.h"
 
 #if (BT_BULLET_VERSION <= 282)
@@ -51,96 +51,119 @@ 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)
+namespace impl
 {
-  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 Default return type of tf2::toMsg() for btVector3.
+template<>
+struct DefaultMessageForDatatype<btVector3>
+{
+  /// \brief Default return type of tf2::toMsg() for btVector3.
+  using type = geometry_msgs::msg::Point;
+};
 
+/// \brief Default return type of tf2::toMsg() for btQuaternion.
+template<>
+struct DefaultMessageForDatatype<btQuaternion>
+{
+  /// \brief Default return type of tf2::toMsg() for btQuaternion.
+  using type = geometry_msgs::msg::Quaternion;
+};
 
-/** \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)
+/// \brief Default return type of tf2::toMsg() for btTransform.
+template<>
+struct DefaultMessageForDatatype<btTransform>
 {
-  t_out =
-    tf2::Stamped<btVector3>(
-    transformToBullet(transform) * t_in,
-    tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
-}
+  /// \brief Default return type of tf2::toMsg() for btTransform.
+  using type = geometry_msgs::msg::Transform;
+};
 
-/** \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.
- */
-inline
-geometry_msgs::msg::PointStamped toMsg(const tf2::Stamped<btVector3> & in)
+/// \brief Conversion implementation for geometry_msgs::msg::Point and btVector3.
+template<>
+struct ConversionImplementation<btVector3, geometry_msgs::msg::Point>
+  : DefaultVectorConversionImplementation<btVector3, geometry_msgs::msg::Point, btScalar>
 {
-  geometry_msgs::msg::PointStamped msg;
-  msg.header.stamp = tf2_ros::toMsg(in.stamp_);
-  msg.header.frame_id = in.frame_id_;
-  msg.point.x = in[0];
-  msg.point.y = in[1];
-  msg.point.z = in[2];
-  return msg;
-}
+};
 
-/** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type.
- * This function is a specialization of the fromMsg template defined in tf2/convert.h
- * \param msg The PointStamped message to convert.
- * \param out The point converted to a timestamped Bullet Vector3.
- */
-inline
-void fromMsg(const geometry_msgs::msg::PointStamped & msg, tf2::Stamped<btVector3> & out)
+/// \brief Conversion implementation for geometry_msgs::msg::Vector3 and btVector3.
+template<>
+struct ConversionImplementation<btVector3, geometry_msgs::msg::Vector3>
+  : DefaultVectorConversionImplementation<btVector3, geometry_msgs::msg::Vector3, btScalar>
 {
-  out.stamp_ = tf2_ros::fromMsg(msg.header.stamp);
-  out.frame_id_ = msg.header.frame_id;
-  out[0] = static_cast<float>(msg.point.x);
-  out[1] = static_cast<float>(msg.point.y);
-  out[2] = static_cast<float>(msg.point.z);
-}
+};
 
+/// \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond.
+template<>
+struct ConversionImplementation<btQuaternion, geometry_msgs::msg::Quaternion>
+  : DefaultQuaternionConversionImplementation<btQuaternion, btScalar> {};
 
-/** \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.
- * \param t_out The transformed frame, as a timestamped Bullet btTransform.
- * \param transform The timestamped transform to apply, as a TransformStamped message.
- */
-template< >
-inline
-void doTransform(
-  const tf2::Stamped<btTransform> & t_in, tf2::Stamped<btTransform> & t_out,
-  const geometry_msgs::msg::TransformStamped & transform)
+/// \brief Conversion implementation for geometry_msgs::msg::Transform and btTransform.
+template<>
+struct ConversionImplementation<btTransform, geometry_msgs::msg::Transform>
 {
-  t_out =
-    tf2::Stamped<btTransform>(
-    transformToBullet(transform) * t_in,
-    tf2_ros::fromMsg(transform.header.stamp), transform.header.frame_id);
-}
+  /**
+   * \brief Convert a Transform message to a btTransform type.
+   * \param[in] in The message to convert.
+   * \param[out] out The converted message, as a btTransform type.
+   */
+  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);
+  }
+
+  /**
+   * \brief Convert a btTransform to a Transform message.
+   * \param[in] in The btTransform to convert.
+   * \param[out] out The converted Transform, as a message.
+   */
+  static void toMsg(btTransform const & in, geometry_msgs::msg::Transform & out)
+  {
+    tf2::toMsg<>(in.getRotation(), out.rotation);
+    tf2::toMsg<>(in.getOrigin(), out.translation);
+  }
+};
+
+/// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
+template<>
+struct DefaultTransformType<btTransform>
+{
+  /// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
+  using type = btTransform;
+};
+
+/// \brief Default Type for automatic tf2::doTransform() implementation for btVector3.
+template<>
+struct DefaultTransformType<btVector3>
+{
+  /// \brief Default Type for automatic tf2::doTransform() implementation for btVector3.
+  using type = btTransform;
+};
 
+/// \brief Default Type for automatic tf2::doTransform() implementation for btQuaternion.
+template<>
+struct DefaultTransformType<btQuaternion>
+{
+  /// \brief Default Type for automatic tf2::doTransform() implementation for btTransform.
+  using type = btTransform;
+};
+}  // namespace impl
 
+/**
+ * \brief Convert a timestamped transform to the equivalent Bullet data type.
+ * \param[in] t The transform to convert, as a geometry_msgs TransformedStamped message.
+ * \return The transform message converted to a Bullet btTransform.
+ */
+[[deprecated("Please use tf2::fromMsg()")]]
+inline btTransform transformToBullet(
+  const geometry_msgs::msg::TransformStamped & t)
+{
+  btTransform ans;
+  fromMsg<>(t.transform, ans);
+  return ans;
+}
 }  // namespace tf2
 
 #endif  // TF2_BULLET__TF2_BULLET_HPP_
diff --git a/tf2_bullet/test/test_tf2_bullet.cpp b/tf2_bullet/test/test_tf2_bullet.cpp
@@ -49,6 +49,24 @@ 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, 0, 0, 0));
+  // vector has 4 entries, set last one to 0 to make comparsion more stable
+  transform_bt.getOrigin().setW(0);
+  EXPECT_EQ(transform_bt.getOrigin(), btVector3(0, 0, 2));
+
+  geometry_msgs::msg::Transform transform2;
+  tf2::convert(transform_bt, transform2);
+  EXPECT_EQ(transform2, transform);
+}
 
 int main(int argc, char ** argv)
 {
