前言

按需求，我们在使用cartographer的时候不单单需要保存map数据，还需要保存carto计算出来的机器人轨迹，今天就来写一个保存路径list的程序吧

数据格式

我们先来看看carto输出的轨迹是什么格式的，如下

a@a:~$ rostopic info /trajectory_node_list 
Type: visualization_msgs/MarkerArray
1
2

然之后我发现网上关于这个数据格式的介绍还是比较少的，没办法了，只能自己仔细看看咯

使用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
---
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140

结合能查到的资料，发现data里面只有一个列表markers，列表长度是固定的，只有三个对象，这个对象里面其他都不用看，我们只需要看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 = []
  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("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 保存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 = "{}/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!")
  


if __name__ == '__main__':
  # cls_ = cls_save_path()
  rospy.init_node('save_path')
  rospy.Subscriber('/trajectory_node_list', MarkerArray, callback)
  rospy.spin()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

用的时候需要注意的目前有两个点

• 这个是用ros才能使用，其他没有的python包自行下载
• 路径那些自己改啦