[ros-users] [patch] add wrench to tf_conversion.

Hi all,

I needed KDL::Wrench to geometry_msgs::Wrench conversion for my Force/Torque
sensor driver, so I added it to tf_conversion.
Is it possible that it would be included in next cturtle release ?

Pozdrawiam
Konrad Banachowicz
Index: tf_conversions/include/tf_conversions/tf_kdl.h

===================================================================
--- tf_conversions/include/tf_conversions/tf_kdl.h    (wersja 34192)
+++ tf_conversions/include/tf_conversions/tf_kdl.h    (kopia robocza)
@@ -34,6 +34,7 @@
 #include "kdl/frames.hpp"
 #include "geometry_msgs/Twist.h"
 #include "geometry_msgs/Pose.h"
+#include "geometry_msgs/Wrench.h"

namespace tf
{
@@ -67,8 +68,13 @@
/// Converts a KDL Frame into a Pose message
void PoseKDLToMsg(const KDL::Frame &t, geometry_msgs::Pose &p);

+/// Convert a KDL Wrench into Wrench message
+void WrenchKDLToMsg(const KDL::Wrench &w, geometry_msgs::Wrench &wm);

+// Convert a Wrench message into KDL Wrench
+void WrenchMsgToKDL(const geometry_msgs::Wrench &wm, KDL::Wrench &w);

+
 /* DEPRECATED FUNCTIONS */
 /// Starting from a Pose from A to B, apply a Twist with reference frame A and reference point B, during a time t.
 geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t)  __attribute__((deprecated));
Index: tf_conversions/src/tf_kdl.cpp
===================================================================
--- tf_conversions/src/tf_kdl.cpp    (wersja 34192)
+++ tf_conversions/src/tf_kdl.cpp    (kopia robocza)
@@ -120,4 +120,26 @@
     PoseKDLToMsg(kdl_pose,result);
     return result;
   }
+
+  void WrenchKDLToMsg(const KDL::Wrench &w, geometry_msgs::Wrench &wm)
+  {
+    wm.force.x = w[0];
+    wm.force.y = w[1];
+    wm.force.z = w[2];
+
+    wm.torque.x = w[3];
+    wm.torque.y = w[4];
+    wm.torque.z = w[5];
+  }
+
+  void WrenchMsgToKDL(const geometry_msgs::Wrench &wm, KDL::Wrench &w)
+  {
+    w[0] = wm.force.x;
+    w[1] = wm.force.y;
+    w[2] = wm.force.z;
+
+    w[3] = wm.torque.x;
+    w[4] = wm.torque.y;
+    w[5] = wm.torque.z;
+  }
 }