first commit

tf3_geometry_msgs/include/tf3_geometry_msgs/data_convert.h

@@ -0,0 +1,119 @@
+#ifndef DATA_CONVERT_H
+#define DATA_CONVERT_H
+
+#include <tf3/time.h>
+#include <tf3/compat.h>
+#include <tf3/LinearMath/Transform.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <robot/time.h>
+#include <cmath>
+
+namespace tf3
+{
+
+    robot::Time convertTime(const tf3::Time& time)
+    {
+        robot::Time time_tmp;
+        time_tmp.sec = time.sec;
+        time_tmp.nsec = time.nsec;
+        return time_tmp;
+    }
+
+    tf3::Time convertTime(const robot::Time& time)
+    {
+        tf3::Time time_tmp;
+        time_tmp.sec = time.sec;
+        time_tmp.nsec = time.nsec;
+        return time_tmp;
+    }
+
+    tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
+    {
+        tf3::Quaternion out(q.x,q.y,q.z,q.w);
+        return out;
+    }
+    
+    geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
+    {
+        return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
+    }
+
+    tf3::Transform convertToTransform(const tf3::TransformStampedMsg& msg)
+    {
+        tf3::Transform out;
+
+        out.setOrigin(tf3::Vector3(
+            msg.transform.translation.x,
+            msg.transform.translation.y,
+            msg.transform.translation.z
+        ));
+
+        tf3::Quaternion q(
+            msg.transform.rotation.x,
+            msg.transform.rotation.y,
+            msg.transform.rotation.z,
+            msg.transform.rotation.w
+        );
+        out.setBasis(tf3::Matrix3x3(q));
+
+        return out;
+    }
+
+    tf3::Transform convertToTransform(const geometry_msgs::TransformStamped& msg)
+    {
+        tf3::Transform out;
+
+        out.setOrigin(tf3::Vector3(
+            msg.transform.translation.x,
+            msg.transform.translation.y,
+            msg.transform.translation.z
+        ));
+
+        tf3::Quaternion q(
+            msg.transform.rotation.x,
+            msg.transform.rotation.y,
+            msg.transform.rotation.z,
+            msg.transform.rotation.w
+        );
+        out.setBasis(tf3::Matrix3x3(q));
+
+        return out;
+    }
+
+
+    tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
+    {
+        tf3::TransformStampedMsg out;
+        out.header.seq = msg.header.seq;
+        out.header.stamp = convertTime(msg.header.stamp);
+        out.header.frame_id = msg.header.frame_id;
+        out.child_frame_id = msg.child_frame_id;
+        out.transform.translation.x = msg.transform.translation.x;
+        out.transform.translation.y = msg.transform.translation.y;
+        out.transform.translation.z = msg.transform.translation.z;
+        out.transform.rotation.x = msg.transform.rotation.x; 
+        out.transform.rotation.y = msg.transform.rotation.y; 
+        out.transform.rotation.z = msg.transform.rotation.z; 
+        out.transform.rotation.w = msg.transform.rotation.w; 
+        
+        return out;
+    }
+    geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
+    {
+        geometry_msgs::TransformStamped out;
+        out.header.seq = msg.header.seq;
+        out.header.stamp = convertTime(msg.header.stamp);
+        out.header.frame_id = msg.header.frame_id;
+        out.child_frame_id = msg.child_frame_id;
+        out.transform.translation.x = msg.transform.translation.x;
+        out.transform.translation.y = msg.transform.translation.y;
+        out.transform.translation.z = msg.transform.translation.z;
+        out.transform.rotation.x = msg.transform.rotation.x; 
+        out.transform.rotation.y = msg.transform.rotation.y; 
+        out.transform.rotation.z = msg.transform.rotation.z; 
+        out.transform.rotation.w = msg.transform.rotation.w; 
+        return out;
+    }
+}
+
+#endif // DATA_CONVERT_H