按需求，我们在使用cartographer的时候不单单需要保存map数据，还需要保存carto计算出来的机器人轨迹，今天就来写一个保存路径list的程序吧
数据格式我们先来看看carto输出的轨迹是什么格式的，如下
a@a:~$ rostopic info /trajectory_node_list Type: visualization_msgs/MarkerArray
然之后我发现网上关于这个数据格式的介绍还是比较少的，没办法了，只能自己仔细看看咯
使用rostopic echo看一下具体怎么呈现的，以下是一次消息格式
---
markers:
-
header:
seq: 0
stamp:
secs: 1658918845
nsecs: 318227513
frame_id: "map"
ns: "Trajectory 0"
id: 0
type: 4
action: 0
pose:
position:
x: 0.0
y: 0.0
z: 0.05
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
scale:
x: 0.07
y: 0.0
z: 0.0
color:
r: 0.20712989568710327
g: 0.11549998074769974
b: 0.7699999809265137
a: 1.0
lifetime:
secs: 0
nsecs: 0
frame_locked: False
points:
-
x: 0.0
y: 0.0
z: 0.0
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False
-
header:
seq: 0
stamp:
secs: 1658918845
nsecs: 318227513
frame_id: "map"
ns: "Trajectory 0"
id: 1
type: 4
action: 0
pose:
position:
x: 0.0
y: 0.0
z: 0.05
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
scale:
x: 0.07
y: 0.0
z: 0.0
color:
r: 0.20712989568710327
g: 0.11549998074769974
b: 0.7699999809265137
a: 0.5
lifetime:
secs: 0
nsecs: 0
frame_locked: False
points:
-
x: 0.0
y: 0.0
z: 0.0
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False
-
header:
seq: 0
stamp:
secs: 1658918845
nsecs: 318227513
frame_id: "map"
ns: "Trajectory 0"
id: 2
type: 4
action: 0
pose:
position:
x: 0.0
y: 0.0
z: 0.05
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
scale:
x: 0.07
y: 0.0
z: 0.0
color:
r: 0.20712989568710327
g: 0.11549998074769974
b: 0.7699999809265137
a: 0.25
lifetime:
secs: 0
nsecs: 0
frame_locked: False
points:
-
x: 0.0
y: 0.0
z: 0.0a
-
x: 0.0010985880336966897
y: -0.020813122900237884
z: 0.0
-
x: -0.002044120373747804
y: -0.0209120250600866
z: 0.0
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False
---
结合能查到的资料，发现data里面只有一个列表markers，列表长度是固定的，只有三个
那么现在搞清楚了数据格式，我们只需要写个程序将里面的信息进行提取
Code#!/usr/bin/python
# -*- coding: utf-8 -*-
import sys
import rospy
from visualization_msgs.msg import MarkerArray
import xlwt
import numpy as np
import sys
import matplotlib.pyplot as plt
def callback(data):
rospy.loginfo("read topic")
# 0 handle data这里第一步，处理一下数据格式啦，可以写短点的，懒得改了
save_points = []
list_points = data.markers[0].points
for point in list_points:
xyz = []
xyz.append(point.x)
xyz.append(point.y)
xyz.append(point.z)
save_points.append(xyz)
list_points = data.markers[1].points
for point in list_points:
xyz = []
xyz.append(point.x)
xyz.append(point.y)
xyz.append(point.z)
save_points.append(xyz)
list_points = data.markers[2].points
for point in list_points:
xyz = []
xyz.append(point.x)
xyz.append(point.y)
xyz.append(point.z)
save_points.append(xyz)
rospy.loginfo("len of points: {}".format(len(save_points)))
rospy.loginfo("saving")
# 1 save txt 这个地方保存txt文件
path_ = sys.path[0]
path_txt = "{}/out.txt".format(path_)
with open(path_txt, "w") as f:
f.write("xtytzr")
for i in save_points:
str_line = "{}t{}t{}r".format(i[0], i[1], i[2])
f.write(str_line)
f.close()
rospy.loginfo("finish save txtrpath:{}".format(path_txt))
# 2 save excel 保存excel咯
path_xls = "{}/out.xls".format(path_)
file_xls = xlwt.Workbook('encoding = utf-8')
sheet1 = file_xls.add_sheet('sheet1',cell_overwrite_ok=True)
sheet1.write(0, 0, "x")
sheet1.write(0, 1, "y")
sheet1.write(0, 2, "z")
for i in range(len(save_points)):
# print(xyz)
sheet1.write(i+1, 0, save_points[i][0])
sheet1.write(i+1, 1, save_points[i][1])
sheet1.write(i+1, 2, save_points[i][2])
file_xls.save(path_xls)
# data_write_csv(path_csv, save_points)
rospy.loginfo("finish save excelrpath:{}".format(path_xls))
# 3 save img 画图咯
rospy.loginfo("begin draw")
path_img = "{}/out.png".format(path_)
x_data = []
y_data = []
for i in save_points:
x_data.append(i[0])
y_data.append(i[1])
plt.plot(x_data,y_data)
ax = plt.gca()
ax.set_aspect(1)
rospy.loginfo("show img")
plt.show()
rospy.loginfo("finish save imgrpath:{}".format(path_img))
rospy.signal_shutdown("finish save!")
if __name__ == '__main__':
# cls_ = cls_save_path()
rospy.init_node('save_path')
rospy.Subscriber('/trajectory_node_list', MarkerArray, callback)
rospy.spin()
用的时候需要注意的目前有两个点
- 这个是用ros才能使用，其他没有的python包自行下载
- 路径那些自己改啦
