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

2023-11-08 19:36

本文主要是介绍基于飞迪RTK/INS组合导航模组的里程计发布方法,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

文章目录

  • 概要
  • 解算过程
    • 获取初始化点
    • 经纬度坐标系转UTM
    • 计算航向角
    • 发布odom坐标
  • 完整代码

概要

这篇博客主要介绍,如何将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;
}

这篇关于基于飞迪RTK/INS组合导航模组的里程计发布方法的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!


原文地址:
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.chinasem.cn/article/372026

相关文章

Linux系统中查询JDK安装目录的几种常用方法

《Linux系统中查询JDK安装目录的几种常用方法》:本文主要介绍Linux系统中查询JDK安装目录的几种常用方法,方法分别是通过update-alternatives、Java命令、环境变量及目... 目录方法 1:通过update-alternatives查询(推荐)方法 2:检查所有已安装的 JDK方

SQL Server安装时候没有中文选项的解决方法

《SQLServer安装时候没有中文选项的解决方法》用户安装SQLServer时界面全英文,无中文选项,通过修改安装设置中的国家或地区为中文中国,重启安装程序后界面恢复中文,解决了问题,对SQLSe... 你是不是在安装SQL Server时候发现安装界面和别人不同,并且无论如何都没有中文选项?这个问题也

Java Thread中join方法使用举例详解

《JavaThread中join方法使用举例详解》JavaThread中join()方法主要是让调用改方法的thread完成run方法里面的东西后,在执行join()方法后面的代码,这篇文章主要介绍... 目录前言1.join()方法的定义和作用2.join()方法的三个重载版本3.join()方法的工作原

在MySQL中实现冷热数据分离的方法及使用场景底层原理解析

《在MySQL中实现冷热数据分离的方法及使用场景底层原理解析》MySQL冷热数据分离通过分表/分区策略、数据归档和索引优化,将频繁访问的热数据与冷数据分开存储,提升查询效率并降低存储成本,适用于高并发... 目录实现冷热数据分离1. 分表策略2. 使用分区表3. 数据归档与迁移在mysql中实现冷热数据分

Spring Boot从main方法到内嵌Tomcat的全过程(自动化流程)

《SpringBoot从main方法到内嵌Tomcat的全过程(自动化流程)》SpringBoot启动始于main方法,创建SpringApplication实例,初始化上下文,准备环境,刷新容器并... 目录1. 入口:main方法2. SpringApplication初始化2.1 构造阶段3. 运行阶

Olingo分析和实践之ODataImpl详细分析(重要方法详解)

《Olingo分析和实践之ODataImpl详细分析(重要方法详解)》ODataImpl.java是ApacheOlingoOData框架的核心工厂类,负责创建序列化器、反序列化器和处理器等组件,... 目录概述主要职责类结构与继承关系核心功能分析1. 序列化器管理2. 反序列化器管理3. 处理器管理重要方

Python错误AttributeError: 'NoneType' object has no attribute问题的彻底解决方法

《Python错误AttributeError:NoneTypeobjecthasnoattribute问题的彻底解决方法》在Python项目开发和调试过程中,经常会碰到这样一个异常信息... 目录问题背景与概述错误解读:AttributeError: 'NoneType' object has no at

postgresql使用UUID函数的方法

《postgresql使用UUID函数的方法》本文给大家介绍postgresql使用UUID函数的方法,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录PostgreSQL有两种生成uuid的方法。可以先通过sql查看是否已安装扩展函数,和可以安装的扩展函数

Java中Arrays类和Collections类常用方法示例详解

《Java中Arrays类和Collections类常用方法示例详解》本文总结了Java中Arrays和Collections类的常用方法,涵盖数组填充、排序、搜索、复制、列表转换等操作,帮助开发者高... 目录Arrays.fill()相关用法Arrays.toString()Arrays.sort()A

Nginx安全防护的多种方法

《Nginx安全防护的多种方法》在生产环境中,需要隐藏Nginx的版本号,以避免泄漏Nginx的版本,使攻击者不能针对特定版本进行攻击,下面就来介绍一下Nginx安全防护的方法,感兴趣的可以了解一下... 目录核心安全配置1.编译安装 Nginx2.隐藏版本号3.限制危险请求方法4.请求限制(CC攻击防御)