ROS中的/sensor_msgs/NavSatFix数据导入google earth pro显示轨迹

将ROS中的/sensor_msgs/NavSatFix数据导入google earth显示轨迹

1.获取GPS数据
将ros中发布的gps topic输出到文本中

// An highlighted block
rostopic echo -b <bagfile> -p <gpstopic_name> > <gpsData>

可以得到如下格式的数据:
2.数据格式转换
因为google earth支持的kml文件中需要导入longitude,latitude,altitude 格式的数据，所以需要对生成的数据文件进行处理，这里我使用python和正则表达式写了个小程序实现数据格式的转换,将gpsData转成gps_kml

// 将路径换成自己的data路径
import re
f_in = open(r'/home/nico/gpsData')
f_out = open(r'/home/nico/gps_kml','w')
a=f_in.readlines()
for line in a[1:]:
     m=re.match(r'^(\w*\,){6}([\d\-]*\.\d*)\,([\d\-]*\.\d*)\,([\d\-]*\.\d*)(\,\d*\.\d*){9}(\,\d*)$',line)
     lon=m.group(3)
     lat=m.group(2)
     alt=m.group(4)
     gpsData = lon+','+lat+','+alt+' '    
     f_out.write(gpsData)
f_in.close()
f_out.close()

3.将一个kml模板文件中的gps数据点替换成上面python脚本生成的gpsData中的数据
注意替换成自己数据时保证数据格式为lon,lat,alt lon,lat,alt lon,lat,alt lon,lat,alt …这样的格式，即经纬高之间用‘，’隔开，不可包含空格等符号，否则显示错误轨迹，gps点与点的三个坐标之间使用空格或换行符均可。

.kml 文件的内容如下

<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2" xmlns:gx="http://www.google.com/kml/ext/2.2" xmlns:kml="http://www.opengis.net/kml/2.2" xmlns:atom="http://www.w3.org/2005/Atom">
<Document>
	<name>Meu.kml</name>
	<Style id="s_ylw-pushpin_hl">
		<IconStyle>
			<scale>1.3</scale>
			<Icon>
				<href>http://maps.google.com/mapfiles/kml/pushpin/ylw-pushpin.png</href>
			</Icon>
			<hotSpot x="20" y="2" xunits="pixels" yunits="pixels"/>
		</IconStyle>
	</Style>
	<StyleMap id="m_ylw-pushpin">
		<Pair>
			<key>normal</key>
			<styleUrl>#s_ylw-pushpin</styleUrl>
		</Pair>
		<Pair>
			<key>highlight</key>
			<styleUrl>#s_ylw-pushpin_hl</styleUrl>
		</Pair>
	</StyleMap>
	<Style id="s_ylw-pushpin">
		<IconStyle>
			<scale>1.1</scale>
			<Icon>
				<href>http://maps.google.com/mapfiles/kml/pushpin/ylw-pushpin.png</href>
			</Icon>
			<hotSpot x="20" y="2" xunits="pixels" yunits="pixels"/>
		</IconStyle>
	</Style>
	<Placemark>
		<name>Meu</name>
		<styleUrl>#m_ylw-pushpin</styleUrl>
		<LineString>
			<tessellate>1</tessellate>
			<coordinates>
// add gpsData here

</coordinates>
		</LineString>
	</Placemark>
</Document>
</kml>

*4.最后直接用google earth pro打开此kml文件即可在地图上显示轨迹