LGSVL中GPS、Odom使用过程

首先要知道这两个传感器发布的ROS消息格式：

在开发之前，需要在ROS环境中安装这两个msg。

sudo apt install ros-melodic-nmea-msgs
sudo apt install ros-melodic-nav-msgs

GPS Device Sensor

发布的格式是GPGGA格式，需要经纬度信息的可以直接从字符串中取。

GPS Odometer Sensor

这里的position指的是UTM坐标系下的xyz，orientation是在UTM坐标系下的四元数，也是车辆的姿态。

如果需要车辆的航向角Heading，可以使用以下方法获取：
四元数->欧拉角->航向角

        tf::Quaternion quat;
        tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, quat);
        double roll, pitch, yaw;   
        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
        heading = -1 * (yaw - PI / 2) / PI * 180.0;
        if(heading<0)
        {
            heading = heading + 360.0;
        }