ROS保存路径，将cartographer的路径点/trajectory_node_list话题数据保存为txt和excel文件，并画出轨迹图

前言

按需求，我们在使用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包自行下载
• 路径那些自己改啦