/*****************************************************************
 * Copyright:2016-2021 www.corvin.cn ROS小课堂
 * Description:使用串口来读取IMU的数据,并通过ROS话题topic将数据发布出来.
 *   芯片中默认读取出来的加速度数据单位是g,需要将其转换为ROS中加速度规定的
 *   m/s2才能发布.
 * Author: corvin
 * History:
 *   20211122: init this file.
*****************************************************************/
#include <ros/ros.h>
#include <tf/tf.h>
#include <imu_data.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Empty.h>
#include "serial_6dof_imu/setYawZero.h"
#include "serial_6dof_imu/setBaudRate.h"
#include "serial_6dof_imu/getYawData.h"


static float g_yawData;  //全局变量,存储当前yaw值,可以通过服务来得到该值

/**********************************************************
 * Description:将yaw角度归零的话题回调函数,只要往话题中
 *   发布一条Empty消息,即可将yaw角度归零.
 *********************************************************/
void yawZeroCallback(const std_msgs::Empty::ConstPtr& msg)
{
    makeYawZero();
}

/******************************************************************
 * Description:使用service方式来进行yaw角度归零,这里是服务的回调
 *   函数,当有客户端发送yaw归零的服务时,自动调用该函数,如果正确
 *   执行了yaw归零,则response反馈为0,如果为其他负数则表明yaw归零
 *   命令执行失败.
 *****************************************************************/
bool yawZeroService(serial_6dof_imu::setYawZero::Request &req,
                    serial_6dof_imu::setYawZero::Response &res)
{
    res.status = makeYawZero();
    return true;
}

/********************************************************************
 * Description:使用服务调用方式来修改与IMU模块通信的波特率,这里根据
 *   客户端发送过来的request来决定修改的波特率,请求内容对应的波特率:
 *   请求发送1,修改波特率为9600;
 *   请求发送2,修改波特率为57600;
 *   请求发送3,修改波特率为115200;
 *   当修改IMU模块的波特率成功后函数会返回0,若返回其他负值,则表示
 *   修改波特率失败,可以重新调用服务来修改波特率.
 *******************************************************************/
bool setBaudService(serial_6dof_imu::setBaudRate::Request &req,
                    serial_6dof_imu::setBaudRate::Response &res)
{
    res.status = setIMUBaudRate(req.index);
    return true;
}

/**********************************************************************
 * Description:通过service服务调用方式来获取到yaw角度信息.
 **********************************************************************/
bool getYawDataService(serial_6dof_imu::getYawData::Request &req,
                       serial_6dof_imu::getYawData::Response &res)
{
    res.yaw = g_yawData;
    return true;
}

int main(int argc, char **argv)
{
    float yaw, pitch, roll;
    std::string imu_dev;
    int baud = 0;

    std::string imu_link_name;
    std::string imu_topic_name;
    std::string yaw_pub_topic;
    std::string pitch_pub_topic;
    std::string roll_pub_topic;
    std::string yaw_zero_topic;
    std::string yaw_zero_service;
    std::string baud_update_service;
    std::string yaw_data_service;

    int pub_topic_hz = 0;  //话题发布imu数据的频率
    float degree2Rad = 3.1415926/180.0;
    float acc_factor = 9.806;

    ros::init(argc, argv, "imu_data_pub_node");
    ros::NodeHandle handle;

    //launch文件中加载yaml配置文件，然后从配置文件中读取参数
    ros::param::get("~imu_dev",          imu_dev);
    ros::param::get("~baud_rate",        baud);
    ros::param::get("~imu_link_name",    imu_link_name);
    ros::param::get("~pub_topic_hz",     pub_topic_hz);
    ros::param::get("~pub_data_topic",   imu_topic_name);
    ros::param::get("~yaw_zero_topic",   yaw_zero_topic);
    ros::param::get("~yaw_zero_service", yaw_zero_service);
    ros::param::get("~yaw_pub_topic",    yaw_pub_topic);
    ros::param::get("~pitch_pub_topic",  pitch_pub_topic);
    ros::param::get("~roll_pub_topic",   roll_pub_topic);
    ros::param::get("~baud_update_srv",  baud_update_service);
    ros::param::get("~yaw_data_srv",     yaw_data_service);

    //初始化imu模块串口,根据设备号和波特率建立连接
    int ret = initSerialPort(imu_dev.c_str(), baud);
    if(ret < 0) //通过串口连接IMU模块失败
    {
        ROS_ERROR("init 6DOF IMU module serial port error !");
        closeSerialPort();
        return -1;
    }
    ROS_INFO("Now 6DOF IMU module is working...");

    //定义四个服务,分别是更新波特率,yaw角度归零和获取yaw当前角度
    ros::ServiceServer setBaudSrv = handle.advertiseService(baud_update_service, setBaudService);
    ros::ServiceServer setyawSrv  = handle.advertiseService(yaw_zero_service,  yawZeroService);
    ros::ServiceServer getYawSrv  = handle.advertiseService(yaw_data_service,  getYawDataService);

    ros::Subscriber yawZeroSub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
    ros::Publisher imu_pub   = handle.advertise<sensor_msgs::Imu>(imu_topic_name, 2);
    ros::Publisher yaw_pub   = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 2);
    ros::Publisher pitch_pub = handle.advertise<std_msgs::Float32>(pitch_pub_topic, 2);
    ros::Publisher roll_pub  = handle.advertise<std_msgs::Float32>(roll_pub_topic, 2);
    ros::Rate loop_rate(pub_topic_hz);

    sensor_msgs::Imu imu_msg;
    imu_msg.header.frame_id = imu_link_name;
    std_msgs::Float32 yaw_msg;
    std_msgs::Float32 pitch_msg;
    std_msgs::Float32 roll_msg;
    while(ros::ok())
    {
        if(getImuData() < 0)
            break;

        imu_msg.header.stamp = ros::Time::now();
        roll  = getAngle(0)*degree2Rad;
        pitch = getAngle(1)*degree2Rad;
        yaw   = getAngle(2)*degree2Rad;
        if(yaw >= 3.1415926)
            yaw -= 6.2831852;

        g_yawData = yaw; //获取yaw值,可以通过服务来得到该值
        yaw_msg.data   = yaw;
        pitch_msg.data = pitch;
        roll_msg.data  = roll;
        yaw_pub.publish(yaw_msg);     //将yaw值通过话题发布出去
        pitch_pub.publish(pitch_msg); //将pitch值通过话题发布出去
        roll_pub.publish(roll_msg);   //将roll值通过话题发布出去

        //ROS_INFO("yaw:%f pitch:%f roll:%f",yaw, pitch, roll);
        imu_msg.orientation.x = getQuat(1);
        imu_msg.orientation.y = getQuat(2);
        imu_msg.orientation.z = getQuat(3);
        imu_msg.orientation.w = getQuat(0);
        imu_msg.orientation_covariance = {0.0025, 0, 0,
                                          0, 0.0025, 0,
                                          0, 0, 0.0025};

        imu_msg.angular_velocity.x = getAngular(0)*degree2Rad;
        imu_msg.angular_velocity.y = getAngular(1)*degree2Rad;
        imu_msg.angular_velocity.z = getAngular(2)*degree2Rad;
        imu_msg.angular_velocity_covariance = {0.02, 0, 0,
                                               0, 0.02, 0,
                                               0, 0, 0.02};

        //ROS_INFO("acc_x:%f acc_y:%f acc_z:%f",getAcc(0), getAcc(1), getAcc(2));
        imu_msg.linear_acceleration.x = -getAcc(0)*acc_factor;
        imu_msg.linear_acceleration.y = -getAcc(1)*acc_factor;
        imu_msg.linear_acceleration.z = -getAcc(2)*acc_factor;
        imu_msg.linear_acceleration_covariance = {0.04, 0, 0,
                                                  0, 0.04, 0,
                                                  0, 0, 0.04};

        imu_pub.publish(imu_msg); //将imu数据通过话题发布出去
        ros::spinOnce();
        loop_rate.sleep();
    }

    closeSerialPort(); //关闭与IMU模块的串口连接
    return 0;
}