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;
+ }
}