ROS中在gazebo中模拟GPS数据

前面的文章写过了在robot_pose_ekf中添加GPS的方法，但是GPS在gazebo中使用会有两个问题：
1、GPS使用需要经过UTM包进行坐标转换，转换完成之后会出现一个GPS坐标很大的问题，有时候还会出现方向不对；
2、因为gazebo能知道绝对位置，所以有时候需要把GPS的数据（或者融合数据）与真实位置匹配，用UTM的数据我们算不出误差。

所以最近写了一个手动模拟GPS的数据的包，思路很简单，就是在GAZEBO的真实位置数据上加上一个随机数作为误差（不是高斯误差，太麻烦了）。
而如何获取gazebo的真实数据呢，使用turtlebot的时候，gazebo会自动发布一个精确的odom到base_footprint(或者是base_link,具体忘了)TF变换，而发布的topic就是/tf。

要注意这里有一个问题，使用robot_pose_ekf包的时候也会发布base_footprint的TF然后就会导致TF冲突，毕竟一个子节点不能有两个父节点，所以需要屏蔽gazebo的TF，方法见链接https://answers.ros.org/question/229722/how-to-stop-gazebo-publishing-tf/

屏蔽之后，gazebo的TF会以/gazebo_tf的topic发出（或其他名字，反正不是/tf），你去订阅这个topic就能收到所有turtlebot的坐标（gazebo的精确坐标），处理一下就是GPS数据了。

代码如下：
PS:代码应该是没有问题的，如果不能用思考一下，自己改改的，毕竟我自己已经用着了，如果发现什么BUG可以告诉我，谢谢。
PS2:反正我遇到过BUG，但是没有解决。

#include <iostream>
#include <vector>
#include <set>#include <map>
#include <string>
#include <cmath>
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <tf/transform_listener.h>using namespace std;class GazeboGpsSimulator {public:GazeboGpsSimulator(ros::NodeHandle nh_):nh(nh_) {ntpre = 0;ntnow = 0;sub = nh.subscribe("gazebo_tf", 1, &GazeboGpsSimulator::callback_tf, this);ros::spin();}
public:void callback_tf(const tf2_msgs::TFMessage::ConstPtr& msg);private:ros::NodeHandle nh;ros::Subscriber sub;vector<ros::Publisher> publist;vector<nav_msgs::Odometry> pose;int ntpre;int ntnow;tf::TransformListener listener;
};void GazeboGpsSimulator::callback_tf(const tf2_msgs::TFMessage::ConstPtr& msg)
{int idx = 0;//robot序号，从1开始int idxnum = 0;string robotName;for(int i = 0;i < msg->transforms.size();i++) {ntnow = msg->transforms[i].header.stamp.nsec;string frame_id = msg->transforms[i].header.frame_id;for(int j = 0;j < frame_id.size();j++) {if(frame_id[j] <= '9' && frame_id[j] >= '0') {idxnum = j;idx = frame_id[j] - '0';}}robotName = string(frame_id.begin(), frame_id.begin() + idxnum + 1);while(pose.size() < idx) {int size = pose.size();pose.push_back(nav_msgs::Odometry());ros::Publisher pubtemp = nh.advertise<nav_msgs::Odometry>("/robot" + to_string(size + 1) + "/gps_sim" , 1, this);publist.push_back(pubtemp);}nav_msgs::Odometry temp;temp.header.frame_id = "map";temp.pose.pose.position.x = msg->transforms[i].transform.translation.x + (rand() / double(RAND_MAX) - 0.5) / 2.5;temp.pose.pose.position.y = msg->transforms[i].transform.translation.y + (rand() / double(RAND_MAX) - 0.5) / 2.5;temp.pose.pose.position.z = msg->transforms[i].transform.translation.z;temp.pose.pose.orientation.x = msg->transforms[i].transform.rotation.x;temp.pose.pose.orientation.y = msg->transforms[i].transform.rotation.y;temp.pose.pose.orientation.z = msg->transforms[i].transform.rotation.z;temp.pose.pose.orientation.w = msg->transforms[i].transform.rotation.w;pose[idx - 1] = temp;if(((ntnow - ntpre) > 90000000) || ((ntnow - ntpre) < 0)) {ntpre = ntnow;for(int i = 0;i < pose.size();i++) {pose[i].header.stamp = ros::Time::now();pose[i].pose.covariance[0] = 0.0001;pose[i].pose.covariance[7] = 0.0001;pose[i].pose.covariance[14] = 0.0001;pose[i].pose.covariance[21] = 0.00000001;pose[i].pose.covariance[28] = 0.00000001;pose[i].pose.covariance[35] = 0.00000001;publist[i].publish(pose[i]);}}}}int main(int argc, char **argv)
{ros::init(argc, argv, "GazeboGpsSimulator");//vector<fd> friends;ros::NodeHandle nh;GazeboGpsSimulator robot1(nh);return 0;
}

ROS中在gazebo中模拟GPS数据相关推荐

STM32+果云GA6-GPRS/GSM模块+MQTT+HTTP协议连接中移OneNet上传GPS数据定位
一.环境介绍 MCU: STM32F103C8T6 GSM模块: GA6--果云开发软件: Keil5 完整源码下载: https://download.csdn.net/download/xi ...
android 生成kml,Android模拟GPS数据生成kml和nmea文件
在Android平台上开发可能没有真机的时候,我们通过DDMS中载入kml或nmea文件来模拟生产数据,使用方法参考新版ADT插件让Android开发更人性化一文,最终这两个文件加载到/data/ ...
android nmea 工具,Android模拟GPS数据生成kml和nmea文件
[IT168 技术文档]在Android平台上开发可能没有真机的时候,我们通过DDMS中载入KML或NMEA文件来模拟生产数据,使用方法参考新版ADT插件让Android开发更人性化一文,最终这两个文 ...
使用MATLAB绘制Gazebo中的仿真小车的运动轨迹
虽然,Gazebo中自带了绘图工具,当需要绘制一些简单的图像时,非常的方便,但是当需要绘制复杂的图像时,还是MATLAB更加合适一些. 当我们仅仅使用MATLAB以可视化图像的形式去观察G ...
Python出租车GPS数据的路网匹配(踩坑复盘)
目录项目背景 TransBigData简介数据预处理数据栅格化订单起讫点OD提取与聚合集计交互可视化总结 (基于小旭学长的TansBigData项目实践,此篇在转载小旭学长项目的基础上对遇 ...
Universal Robot——在Gazebo中模拟UR5机器人
今天的目标是在Gazebo安装我们的ur5机器人.Gazebo是一个流行的模拟工具,它不仅可以让你像在Rviz中一样可视化你的机器人,而且还内置了一个物理引擎.这样你就可以在更真实的环境中测试你的机器 ...
在ROS Kinetic和Gazebo 8中使用智能汽车仿真演示
在ROS Kinetic和Gazebo 8中使用智能汽车仿真演示智能车无人驾驶技术是目前人工智能和机器人技术的研究热点,有许多开源平台可以使我们零基础零成本入门无人驾驶技术.本文分享一下目前ROS官 ...
【ROS Gazebo专题】四、将Fetch机器人放到Gazebo中进行玩耍
Gazebo的了解请看我前两篇博客: [ROS学习笔记]二.Gazebo的使用上 [ROS学习笔记]三.Gazebo的使用下系统环境介绍: 操作系统:Ubuntu 14.04 LTS ROS版本:I ...
ROS(Robot Operating System)笔记 : 1.使用launch file在gazebo中生成urdf机器人
ROS(Robot Operating System) 1.使用launch file在gazebo中生成urdf机器人最近接触了ROS(Robot Operating System),发现单单学习 ...

ROS中在gazebo中模拟GPS数据

ROS中在gazebo中模拟GPS数据相关推荐

最新文章

热门文章