基于飞迪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 <ros/ros.h>
#include "turtlesim/Pose.h"
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <geodesy/utm.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <math.h>
#include <message_filters/synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <boost/thread/thread.hpp>
#include <iostream>//全局变量
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;
}