[ros-users] [PATCH] add KDL::Wrench/geometry_msgs::Wrench tf…

Index: include/tf_conversions/tf_kdl.h

===================================================================
--- include/tf_conversions/tf_kdl.h    (revision 31694)
+++ include/tf_conversions/tf_kdl.h    (working copy)
@@ -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);

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

+/// Converts a Wrench message into a KDL Wrench
+void WrenchMsgToKDL(const geometry_msgs::Wrench &m, 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: src/tf_kdl.cpp
===================================================================
--- src/tf_kdl.cpp    (revision 31694)
+++ src/tf_kdl.cpp    (working copy)
@@ -108,6 +108,26 @@
     tf::poseTFToMsg(pose_tf,p);
   }

+  void WrenchKDLToMsg(const KDL::Wrench &w, geometry_msgs::Wrench &m)
+  {
+    m.force.x = w.force[0];
+    m.force.y = w.force[1];
+    m.force.z = w.force[2];
+    m.torque.x = w.torque[0];
+    m.torque.y = w.torque[1];
+    m.torque.z = w.torque[2];
+  }
+
+  void WrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &w)
+  {
+    w.force[0] = m.force.x;
+    w.force[1] = m.force.y;
+    w.force[2] = m.force.z;
+    w.torque[0] = m.torque.x;
+    w.torque[1] = m.torque.y;
+    w.torque[2] = m.torque.z;
+  }
+
   geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t)
   {
     geometry_msgs::Pose result;