costmap_2d/include/costmap_2d/data_convert.h

#ifndef DATA_CONVERT_H
#define DATA_CONVERT_H

#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf3/utils.h>
#include <tf3/compat.h>
#include <robot/time.h>
#include <cmath>

namespace costmap_2d
{
    template <typename T>
    bool isEqual(const T& a, const T& b, double eps = 1e-5)
    {
        return std::fabs(a - b) < eps;
    }

    inline bool operator==(const geometry_msgs::Quaternion& a, const geometry_msgs::Quaternion& b)
    {
        return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z) && isEqual(a.w, b.w);
    }

    inline bool operator==(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
    {
        return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z);
    }

    inline bool operator==(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b)
    {
        return (a.position == b.position) && (a.orientation == b.orientation);
    }

    inline bool operator==(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b)
    {
        return (a.header.frame_id == b.header.frame_id) &&
            (a.pose == b.pose);
    }

    inline double getYaw(const geometry_msgs::Quaternion& q)
    {
        double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
        double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
        return std::atan2(siny_cosp, cosy_cosp);
    }

    inline geometry_msgs::Quaternion getQuaternion(const double& yaw)
    {
        geometry_msgs::Quaternion q;
        q.x = 0.0;
        q.y = 0.0;
        q.z = std::sin(yaw / 2.0);
        q.w = std::cos(yaw / 2.0);
        return q;
    }

    robot::Time convertTime(tf3::Time time)
    {
        robot::Time time_tmp;
        time_tmp.sec = time.sec;
        time_tmp.nsec = time.nsec;
        return time_tmp;
    }

    tf3::Time convertTime(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 convertTotf3Transform(const geometry_msgs::Transform& msg)
    {
        tf3::Quaternion q(
            msg.rotation.x,
            msg.rotation.y,
            msg.rotation.z,
            msg.rotation.w
        );

        tf3::Vector3 t(
            msg.translation.x,
            msg.translation.y,
            msg.translation.z
        );

        tf3::Transform tf(q, t);
        return tf;
    }

    tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
    {
        tf3::Quaternion q(
            msg.rotation.x,
            msg.rotation.y,
            msg.rotation.z,
            msg.rotation.w
        );

        tf3::Vector3 t(
            msg.translation.x,
            msg.translation.y,
            msg.translation.z
        );

        tf3::Transform tf(q, t);
        return tf;
    }

    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