前言：

         承接上一篇博文本文将编写并记录上文中详细的工程项目，用于保存小车的运动路径，生成对应的csv，和加载所保存的路径到实际的Rviz中，本文将开源完整的工程项目，工程结构如下：

工程原码位于文章末尾：

路径存储：

waypoint_saver 用于存储 waypoint 的节点  

waypoint_saver.h 头h文件

#ifndef WAYPOINT_SAVER_WAYPOINT_SAVER_H_INCLUDED
#define WAYPOINT_SAVER_WAYPOINT_SAVER_H_INCLUDED

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <fstream>
#include <sstream>

class WaypointSaver
{
public:
    WaypointSaver(ros::NodeHandle nh,ros::No