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

原创

已于 2022-08-15 16:42:40 修改 · 2.4k 阅读

27 ·

CC 4.0 BY-SA版权

文章标签：

#python #人工智能 #自动驾驶

于 2022-08-05 14:49:56 首次发布

前言

按需求，我们在使用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