#!/usr/bin/env python
# -*- coding: UTF-8 -*-

# Copyright: 2016-2021 https://www.corvin.cn ROS小课堂
# Description: 从IIC接口中读取6DOF IMU模块的三轴加速度、角度、四元数，
#    然后组装成ROS中的IMU消息格式，发布到/imu_data话题中，这样有需要的
#    节点可以直接订阅该话题即可获取到imu当前的数据.
# Author: corvin
# History:
#    20211122:Initial this file.
#    20211203:增加imu_iic_bus,imu_iic_addr配置参数.
import rospy
import math

from std_msgs.msg import Float32
from sensor_msgs.msg import Imu
from iic_6dof_imu_data import MyIMU
from iic_6dof_imu.srv import GetYawData,GetYawDataResponse

degrees2rad = math.pi/180.0
yaw = 0.0

# ros server return Yaw data
def return_yaw_data(req):
    return GetYawDataResponse(yaw)

rospy.init_node("iic_6dof_imu_node")

# Get DIY config param
imu_iic_bus     = rospy.get_param('~iic_bus', 1)
imu_iic_addr    = rospy.get_param('~iic_addr', '0x50')
topic_pub_hz    = rospy.get_param('~topic_pub_hz', '100')
data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
link_name       = rospy.get_param('~link_name', 'base_imu_link')
yaw_topic_name  = rospy.get_param('~yaw_topic', 'yaw_topic')
pitch_topic_name= rospy.get_param('~pitch_topic', 'pitch_topic')
roll_topic_name = rospy.get_param('~roll_topic', 'roll_topic')
get_yaw_srv     = rospy.get_param('~get_yaw_service', '/iic_imu_service/getYawData_service')


#创建各话题发布者和自定义服务
data_pub  = rospy.Publisher(data_topic_name,  Imu,     queue_size=1)
yaw_pub   = rospy.Publisher(yaw_topic_name,   Float32, queue_size=1)
pitch_pub = rospy.Publisher(pitch_topic_name, Float32, queue_size=1)
roll_pub  = rospy.Publisher(roll_topic_name,  Float32, queue_size=1)
yaw_srv   = rospy.Service(get_yaw_srv, GetYawData, return_yaw_data)

#创建各话题发布的消息
imuMsg   = Imu()
yawMsg   = Float32()
pitchMsg = Float32()
rollMsg  = Float32()

#Orientation covariance estimation
imuMsg.orientation_covariance = [
0, 0, 0,
0, 0, 0,
0, 0, 0
]

#Angular velocity covariance estimation
imuMsg.angular_velocity_covariance = [
0, 0, 0,
0, 0, 0,
0, 0, 0
]

#linear acceleration covariance estimation
imuMsg.linear_acceleration_covariance = [
0, 0, 0,
0, 0, 0,
0, 0, 0
]

seq = 0
accel_factor = 9.806  #sensor accel g convert to m/s^2.

myIMU = MyIMU(imu_iic_bus, int(imu_iic_addr))
rate = rospy.Rate(topic_pub_hz)

rospy.loginfo("Now 6DOF IMU Module is working ...")
while not rospy.is_shutdown():
    myIMU.get_YPRAG()

    #rospy.loginfo("yaw:%f pitch:%f roll:%f", myIMU.raw_yaw,
    #              myIMU.raw_pitch, myIMU.raw_roll)
    yaw_deg = float(myIMU.raw_yaw)
    if yaw_deg >= 180.0:
        yaw_deg -= 360.0

    #rospy.loginfo("yaw_deg: %f", yaw_deg)
    yaw   = yaw_deg*degrees2rad
    pitch = float(myIMU.raw_pitch)*degrees2rad
    roll  = float(myIMU.raw_roll)*degrees2rad

    #填充话题消息内容,然后通过话题发布
    yawMsg.data   = yaw
    pitchMsg.data = pitch
    rollMsg.data  = roll
    yaw_pub.publish(yawMsg)
    pitch_pub.publish(pitchMsg)
    roll_pub.publish(rollMsg)

    #rospy.loginfo("acc_x:%f acc_y:%f acc_z:%f", myIMU.raw_ax,
    #              myIMU.raw_ay, myIMU.raw_az)
    imuMsg.linear_acceleration.x = -float(myIMU.raw_ax)*accel_factor
    imuMsg.linear_acceleration.y = -float(myIMU.raw_ay)*accel_factor
    imuMsg.linear_acceleration.z = -float(myIMU.raw_az)*accel_factor

    imuMsg.angular_velocity.x = float(myIMU.raw_gx)*degrees2rad
    imuMsg.angular_velocity.y = float(myIMU.raw_gy)*degrees2rad
    imuMsg.angular_velocity.z = float(myIMU.raw_gz)*degrees2rad

    # From IMU module get quatern param
    myIMU.get_quatern()
    imuMsg.orientation.x = myIMU.raw_q1
    imuMsg.orientation.y = myIMU.raw_q2
    imuMsg.orientation.z = myIMU.raw_q3
    imuMsg.orientation.w = myIMU.raw_q0

    imuMsg.header.stamp    = rospy.Time.now()
    imuMsg.header.frame_id = link_name
    imuMsg.header.seq = seq
    seq = seq + 1
    data_pub.publish(imuMsg)

    rate.sleep()