#!/usr/bin/env python

# Visualize compass_msgs/Azimuth messages as a pose.
# Author: Martin Pecka
# SPDX-License-Identifier: BSD-3-Clause
# SPDX-FileCopyrightText: Czech Technical University in Prague

from math import radians

import rospy
from tf.transformations import quaternion_from_euler, quaternion_inverse

from compass_msgs.msg import Azimuth
from geometry_msgs.msg import PoseWithCovarianceStamped


def cb(msg):
    """Process the incoming message.

    :param Azimuth msg: The azimuth message.
    """
    pose = PoseWithCovarianceStamped()
    pose.header = msg.header
    azimuth_rad = msg.azimuth
    if msg.unit == Azimuth.UNIT_DEG:
        azimuth_rad = radians(azimuth_rad)
    q = quaternion_inverse(quaternion_from_euler(0, 0, azimuth_rad))
    pose.pose.pose.orientation.x = q[0]
    pose.pose.pose.orientation.y = q[1]
    pose.pose.pose.orientation.z = q[2]
    pose.pose.pose.orientation.w = q[3]
    pose.pose.covariance[-1] = msg.variance
    pub.publish(pose)


if __name__ == '__main__':
    rospy.init_node('visualize_azimuth')

    pub = rospy.Publisher('~azimuth_vis', PoseWithCovarianceStamped, queue_size=10)
    sub = rospy.Subscriber('~azimuth', Azimuth, cb, queue_size=10)

    rospy.spin()
