前言
按需求,我们在使用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
,列表长度是不定的,最开始只有三个<class 'visualization_msgs.msg._Marker.Marker'>
对象,这个对象里面其他都不用看,我们只需要看points
,这个就是存放path的列表,points
列表中每一个元素都代表一个point,包括了x,y,z
那么现在搞清楚了数据格式,我们只需要写个程序将里面的信息进行提取
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 = []
for marker in data.markers:
points = marker.points
for point in points:
xyz =[]
xyz.append(point.x)
xyz.append(point.y)
xyz.append(point.z)
save_points.append(xyz)
# 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
path_ = sys.path[0]
path_txt = "{}/out.txt".format(path_)
with open(path_txt, "w") as f:
f.write("x\ty\tz\r")
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 txt\rpath:{}".format(path_txt))
# 2 save 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 excel\rpath:{}".format(path_xls))
# 3 save img
rospy.loginfo("begin draw")
path_img = r"{}/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 img\rpath:{}".format(path_img))
rospy.signal_shutdown("finish save!")
plt.savefig(fname=path_img)
print(0000000)
if __name__ == '__main__':
# cls_ = cls_save_path()
rospy.init_node('save_path')
rospy.Subscriber('/trajectory_node_list', MarkerArray, callback)
rospy.spin()
用的时候需要注意的目前有两个点
- 这个是用ros才能使用,其他没有的python包自行下载
- 路径那些自己改啦