100 lines
3.0 KiB
C++
100 lines
3.0 KiB
C++
#ifndef DATA_CONVERT_H
|
|
#define DATA_CONVERT_H
|
|
|
|
#include <geometry_msgs/TransformStamped.h>
|
|
#include <tf2/utils.h>
|
|
#include <tf2/compat.h>
|
|
#include <robot/time.h>
|
|
|
|
namespace costmap_2d
|
|
{
|
|
robot::Time convertTime(tf2::Time time)
|
|
{
|
|
robot::Time time_tmp;
|
|
time_tmp.sec = time.sec;
|
|
time_tmp.nsec = time.nsec;
|
|
return time_tmp;
|
|
}
|
|
|
|
tf2::Time convertTime(robot::Time time)
|
|
{
|
|
tf2::Time time_tmp;
|
|
time_tmp.sec = time.sec;
|
|
time_tmp.nsec = time.nsec;
|
|
return time_tmp;
|
|
}
|
|
|
|
|
|
tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
|
|
{
|
|
tf2::Quaternion q(
|
|
msg.rotation.x,
|
|
msg.rotation.y,
|
|
msg.rotation.z,
|
|
msg.rotation.w
|
|
);
|
|
|
|
tf2::Vector3 t(
|
|
msg.translation.x,
|
|
msg.translation.y,
|
|
msg.translation.z
|
|
);
|
|
|
|
tf2::Transform tf(q, t);
|
|
return tf;
|
|
}
|
|
|
|
tf2::Transform convertToTf2Transform(const tf2::TransformMsg& msg)
|
|
{
|
|
tf2::Quaternion q(
|
|
msg.rotation.x,
|
|
msg.rotation.y,
|
|
msg.rotation.z,
|
|
msg.rotation.w
|
|
);
|
|
|
|
tf2::Vector3 t(
|
|
msg.translation.x,
|
|
msg.translation.y,
|
|
msg.translation.z
|
|
);
|
|
|
|
tf2::Transform tf(q, t);
|
|
return tf;
|
|
}
|
|
|
|
tf2::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
|
|
{
|
|
tf2::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 tf2::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
|