Feature: Add doTransform for Wrench messages (ros#476)

* Add doTransform for Wrench and WrenchStamped for c++

tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp

@@ -48,6 +48,8 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/wrench.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <kdl/frames.hpp>
 
 #include <array>
@@ -151,8 +153,8 @@ std::string getFrameId(const geometry_msgs::msg::Vector3Stamped & t)
 
 /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector 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 Vector3 message.
- * \param t_out The transformed vector, as a timestamped Vector3 message.
+ * \param t_in The vector to transform, as a timestamped Vector3Stamped message.
+ * \param t_out The transformed vector, as a timestamped Vector3Stamped message.
  * \param transform The timestamped transform to apply, as a TransformStamped message.
  */
 template<>
@@ -162,11 +164,7 @@ void doTransform(
   geometry_msgs::msg::Vector3Stamped & t_out,
   const geometry_msgs::msg::TransformStamped & transform)
 {
-  KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(
-    t_in.vector.x, t_in.vector.y, t_in.vector.z);
-  t_out.vector.x = v_out[0];
-  t_out.vector.y = v_out[1];
-  t_out.vector.z = v_out[2];
+  doTransform(t_in.vector, t_out.vector, transform);
   t_out.header.stamp = transform.header.stamp;
   t_out.header.frame_id = transform.header.frame_id;
 }
@@ -898,6 +896,17 @@ geometry_msgs::msg::Transform toMsg(const tf2::Transform & in)
   return out;
 }
 
+/** \brief Convert a tf2 Transform type to its equivalent geometry_msgs representation.
+ * \param in A tf2 Transform object.
+ * \param out The Transform converted to a geometry_msgs message type.
+ */
+inline
+/** This section is about converting */
+void toMsg(const tf2::Transform & in, geometry_msgs::msg::Transform & out)
+{
+  out = toMsg(in);
+}
+
 /** \brief Convert a Transform message to its equivalent tf2 representation.
  * This function is a specialization of the toMsg template defined in tf2/convert.h.
  * \param in A Transform message type.
@@ -1134,6 +1143,88 @@ void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out)
     tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
 }
 
+/**********************/
+/*** WrenchStamped ****/
+/**********************/
+
+/** \brief Extract a timestamp from the header of a Wrench message.
+ * This function is a specialization of the getTimestamp template defined in tf2/convert.h.
+ * \param t WrenchStamped message to extract the timestamp from.
+ * \return The timestamp of the message.
+ */
+template<>
+inline
+tf2::TimePoint getTimestamp(const geometry_msgs::msg::WrenchStamped & t)
+{
+  return tf2_ros::fromMsg(t.header.stamp);
+}
+
+/** \brief Extract a frame ID from the header of a Wrench message.
+ * This function is a specialization of the getFrameId template defined in tf2/convert.h.
+ * \param t WrenchStamped message to extract the frame ID from.
+ * \return A string containing the frame ID of the message.
+ */
+template<>
+inline
+std::string getFrameId(const geometry_msgs::msg::WrenchStamped & t) {return t.header.frame_id;}
+
+
+/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Wrench type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The wrench to transform, as a Wrench message.
+ * \param t_out The transformed wrench, as a Wrench message.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template<>
+inline
+void doTransform(
+  const geometry_msgs::msg::Wrench & t_in, geometry_msgs::msg::Wrench & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  doTransform(t_in.force, t_out.force, transform);
+  doTransform(t_in.torque, t_out.torque, transform);
+}
+
+/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs WrenchStamped type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The wrench to transform, as a timestamped Wrench message.
+ * \param t_out The transformed wrench, as a timestamped Wrench message.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template<>
+inline
+void doTransform(
+  const geometry_msgs::msg::WrenchStamped & t_in,
+  geometry_msgs::msg::WrenchStamped & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  doTransform(t_in.wrench, t_out.wrench, transform);
+  t_out.header.stamp = transform.header.stamp;
+  t_out.header.frame_id = transform.header.frame_id;
+}
+
+/** \brief Trivial "conversion" function for Wrench message type.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param in A WrenchStamped message.
+ * \return The input argument.
+ */
+inline
+geometry_msgs::msg::WrenchStamped toMsg(const geometry_msgs::msg::WrenchStamped & in)
+{
+  return in;
+}
+
+/** \brief Trivial "conversion" function for Wrench message type.
+ * This function is a specialization of the toMsg template defined in tf2/convert.h.
+ * \param msg A WrenchStamped message.
+ * \param out The input argument.
+ */
+inline
+void fromMsg(const geometry_msgs::msg::WrenchStamped & msg, geometry_msgs::msg::WrenchStamped & out)
+{
+  out = msg;
+}
+
 }  // namespace tf2
 
 #endif  // TF2_GEOMETRY_MSGS__TF2_GEOMETRY_MSGS_HPP_

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

@@ -538,6 +538,31 @@ TEST(TfGeometry, Transform)
   }
 }
 
+TEST(TfGeometry, Wrench)
+{
+  geometry_msgs::msg::Wrench v1, res;
+  v1.force.x = 2;
+  v1.force.y = 1;
+  v1.force.z = 3;
+  v1.torque.x = 2;
+  v1.torque.y = 1;
+  v1.torque.z = 3;
+
+  geometry_msgs::msg::TransformStamped trafo;
+  trafo.transform.translation.x = -1;
+  trafo.transform.translation.y = 2;
+  trafo.transform.translation.z = -3;
+  trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), -M_PI / 2.0));
+
+  tf2::doTransform(v1, res, trafo);
+  EXPECT_NEAR(res.force.x, 1, EPS);
+  EXPECT_NEAR(res.force.y, -2, EPS);
+  EXPECT_NEAR(res.force.z, 3, EPS);
+
+  EXPECT_NEAR(res.torque.x, 1, EPS);
+  EXPECT_NEAR(res.torque.y, -2, EPS);
+  EXPECT_NEAR(res.torque.z, 3, EPS);
+}
 
 int main(int argc, char ** argv)
 {