/****************************************************************
 * Copyright:2016-2021 www.corvin.cn ROS小课堂
 * Description:使用串口来读取IMU模块的数据,并通过ROS话题
 *   topic将数据发布出来.芯片中默认读取出来的加速度数据
 *   单位是g,需要将其转换为ROS中加速度规定的m/s2才能发布.
 * Author: corvin
 * History:
 *   20200403: init this file.
 *   20200406:增加发布yaw数据的话题.
 *   20200703:修改三轴加速度数据正负,按照ROS规定沿着各轴
 *     正方向时数据为正,否则为负.单位m/s2。
 *   20210317:去掉从配置文件中读取的停止位,数据位这些无法
 *     修改的配置参数,因为都是固定的,无需自己修改配置.
 *   20210318:增加订阅话题,话题消息格式Empty,可以将yaw角度归零.
 *     增加使用service方式可将yaw角度归零.
 *   20210319:增加使用service方式可以修改imu模块串口通信波特率.
 *     注意修改了波特率后,需要将IMU模块短开使用新波特率连接.
****************************************************************/
#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_imu_hat_6dof/setYawZero.h"
#include "serial_imu_hat_6dof/setBaudRate.h"


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

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

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

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

    std::string link_name;
    std::string imu_topic;
    std::string temp_topic;
    std::string yaw_pub_topic;
    std::string yaw_zero_topic;
    std::string yaw_zero_service;
    std::string baud_update_service;

    int pub_hz = 0;
    float degree2Rad = 3.1415926/180.0;
    float acc_factor = 9.806;

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

    //从yaml配置文件中读取配置参数
    ros::param::get("~imu_dev",   imu_dev);
    ros::param::get("~baud_rate", baud);
    ros::param::get("~pub_hz",    pub_hz);
    ros::param::get("~link_name", link_name);
    ros::param::get("~pub_data_topic", imu_topic);
    ros::param::get("~pub_temp_topic", temp_topic);
    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("~baud_update_srv", baud_update_service);

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

    //定义两个服务,分别是更新波特率和yaw角度归零
    ros::ServiceServer baudSrv = handle.advertiseService(baud_update_service, setBaudService);
    ros::ServiceServer yawSrv  = handle.advertiseService(yaw_zero_service, yawZeroService);

    ros::Subscriber yaw_zero_sub = handle.subscribe(yaw_zero_topic, 1, yawZeroCallback);
    ros::Publisher imu_pub = handle.advertise<sensor_msgs::Imu>(imu_topic, 1);
    ros::Publisher yaw_pub = handle.advertise<std_msgs::Float32>(yaw_pub_topic, 1);
    ros::Rate loop_rate(pub_hz);

    sensor_msgs::Imu imu_msg;
    imu_msg.header.frame_id = link_name;
    std_msgs::Float32 yaw_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;

        //publish yaw data to topic
        yaw_msg.data = yaw;
        yaw_pub.publish(yaw_msg);

        //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);
        ros::spinOnce();
        loop_rate.sleep();
    }

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

