基于飞迪RTK/INS组合导航模组的里程计发布方法

概要

这篇博客主要介绍，如何将GPS_fix、磁偏角转成odom信息。
PS:官方的驱动包中是自带odom信息，但是对于原点的定义尚未找到出处，故自己另外写了一套发布odom信息。

解算过程

获取初始化点

第一个获取的GPS_fix点为初始点

initPose.latitude = gpsFix->latitude;
initPose.longitude = gpsFix->longitude;
initPose.altitude = 0;
init = true;

经纬度坐标系转UTM

/*原点经纬度转UTM*/
geographic_msgs::GeoPoint gpInit;
gpInit.latitude = initPose.latitude;
gpInit.longitude = initPose.longitude;
geodesy::UTMPoint ptInit(gpInit);
initX = ptInit.easting;
initY = ptInit.northing;

计算航向角

记得减去当地的磁偏角，在这个网站进行查询。

tf::Quaternion qua;
tf::quaternionMsgToTF(odomMsg->pose.pose.orientation, qua);
double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
tf::Matrix3x3(qua).getRPY(roll, pitch, yaw); //进行转换
yaw = yaw - 0.5 * M_PI + MagDec / 180.0 * M_PI;

发布odom坐标

  /***publish gps_odom**/
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.pose.pose.position.x = fixX - initX;
odom.pose.pose.position.y = fixY - initY;
odom.pose.pose.orientation.x=qua.x();
odom.pose.pose.orientation.y=qua.y();
odom.pose.pose.orientation.z=qua.z();
odom.pose.pose.orientation.w=qua.w();        
gpsOdomPub.publish(odom);

完整代码

#include 
#include "turtlesim/Pose.h"
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//全局变量
static double EARTH_RADIUS = 6378.137;//地球半径

class OdomPublisher
{
    public:
        OdomPublisher();
        void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gpsFix,
                const nav_msgs::Odometry::ConstPtr& odomMsg);
        double rad(double d);

    private:
        ros::Publisher state_pub_, smallCarPub, gpsOdomPub;
        geometry_msgs::PolygonStamped carPolygon;
        nav_msgs::Path ros_path_;
        ros::NodeHandle n, nhPrivate;
        message_filters::Subscriber<sensor_msgs::NavSatFix> *subGPS;
        message_filters::Subscriber<nav_msgs::Odometry> *subOdom;
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, nav_msgs::Odometry> syncPolicy;
        message_filters::Synchronizer<syncPolicy> *sync;
        bool init;
        struct my_pose
        {
            double latitude;
            double longitude;
            double altitude;
        };
        my_pose initPose, fixPose;
        double initX, initY, MagDec;
        std::string gpsFixTopic, gpsOdomTopic, gpsOdomPubTopic;
};

OdomPublisher::OdomPublisher():nhPrivate("~")
{    
    ROS_INFO("Initialization");
    nhPrivate.param("gpsFixTopic", gpsFixTopic, std::string("/gps/fix"));
    nhPrivate.param("gpsOdomTopic", gpsOdomTopic, std::string("/odom"));
    nhPrivate.param("gpsOdomPubTopic", gpsOdomPubTopic, std::string("/gps_fix/odom"));
    nhPrivate.getParam("MagDec", MagDec);

    subGPS = new message_filters::Subscriber<sensor_msgs::NavSatFix> (n, gpsFixTopic.c_str(), 1);
    subOdom = new message_filters::Subscriber<nav_msgs::Odometry> (n, gpsOdomTopic.c_str(), 1);
    sync = new message_filters::Synchronizer<syncPolicy> (syncPolicy(10), *subGPS, *subOdom);
    sync->registerCallback(boost::bind(&OdomPublisher::gpsCallback, this, _1, _2));

    state_pub_ = n.advertise<nav_msgs::Path>("/trajectory/gps", 10);
    smallCarPub = n.advertise<geometry_msgs::PolygonStamped>("/trajectory/car", 10);
    gpsOdomPub = n.advertise<nav_msgs::Odometry>(gpsOdomPubTopic.c_str(),10);			
    init = false;
}

//角度制转弧度制
double OdomPublisher::rad(double d) 
{
	return d * 3.1415926 / 180.0;
}

void OdomPublisher::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gpsFix,
                const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    // ROS_INFO_STREAM("Starting to work!!!");
    // std::cout << "Starting to work!!!" << std::endl;
    //初始化
    if(!init)
    {
        initPose.latitude = gpsFix->latitude;
        initPose.longitude = gpsFix->longitude;
        initPose.altitude = 0;
        init = true;
        /*原点经纬度转UTM*/
        geographic_msgs::GeoPoint gpInit;
        gpInit.latitude = initPose.latitude;
        gpInit.longitude = initPose.longitude;
        geodesy::UTMPoint ptInit(gpInit);
        initX = ptInit.easting;
	    initY = ptInit.northing;
    }
    else
    {
        geographic_msgs::GeoPoint gp;
        gp.latitude = gpsFix->latitude;
        gp.longitude = gpsFix->longitude;
        geodesy::UTMPoint pt(gp);
        double fixX = pt.easting;
        double fixY = pt.northing;

        tf::Quaternion qua;
        tf::quaternionMsgToTF(odomMsg->pose.pose.orientation, qua);
        double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
        tf::Matrix3x3(qua).getRPY(roll, pitch, yaw); //进行转换
        yaw = yaw - 0.5 * M_PI + MagDec / 180.0 * M_PI;

        qua.setRPY(0,0,yaw);
        // ROS_INFO("After optmized, yaw is %f", yaw);

        /***publish gps_odom**/
        nav_msgs::Odometry odom;
        odom.header.stamp = ros::Time::now();
        odom.header.frame_id = "odom";
        odom.pose.pose.position.x = fixX - initX;
        odom.pose.pose.position.y = fixY - initY;
        odom.pose.pose.orientation.x=qua.x();
	    odom.pose.pose.orientation.y=qua.y();
	    odom.pose.pose.orientation.z=qua.z();
	    odom.pose.pose.orientation.w=qua.w();        
        gpsOdomPub.publish(odom);

        ros_path_.header.frame_id = "odom";
        ros_path_.header.stamp = ros::Time::now();  

        geometry_msgs::PoseStamped pose;
        pose.header = ros_path_.header;

        pose.pose.position = odom.pose.pose.position;
        
        ros_path_.poses.push_back(pose);
        state_pub_.publish(ros_path_);
    }
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"gps_fix");
    OdomPublisher op;
    ros::spin();
    return 0;
}