【探索评估】记录小车运动轨迹，并在 rviz 上显示

本文主要是参考的博客：ROS 移动机器人运动轨迹记录，并发布在rviz上
但是由于订阅的话题不一样，在这里记录一下

在进行自主探索的时候将小车移动过程中的消息记录在 txt 或是 csv 文件中

在此记录的是小车的 /odom 消息，并且只记录其中的 x,y,w 的值，存储格式为每组数据存一行，如下：

记录轨迹的代码如下：

/***** 实现路径读取 *****/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <string> 
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include <fstream> //文件读入读出
#include <ctime>

using namespace std;
 
void callback_path(const nav_msgs::Odometry::ConstPtr& msg )
{ 
      //outfile用法同cout,存储形式 1 2 3 
      ofstream outfile;
      outfile.setf(ios::fixed, ios::floatfield);
      outfile.precision(2);
      outfile.open("/xxx/data/explore_lite_1.txt",std::ios::app);
      outfile<<msg->pose.pose.position.x<<" "<<msg->pose.pose.position.y<<" "<<msg->pose.pose.orientation.w<<endl;

      outfile.close();
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "explore_record");
    ros::NodeHandle n;
    ros::Subscriber explore_sub = n.subscribe("/odom", 100, callback_path);
    ros::spin();
    return 0;
}

选择你所需要记录的话题，并在回调中以自定义的方式存储数据格式，这里是用空格隔开的，同样也可以使用逗号隔开。

接下来是将 txt 或是 csv 文件中的数据读出，并在rviz中显示，代码如下：
（注意这里每行数据以空格分割！若以逗号分割则Stringsplit函数的第二个参数要做相应的修改）

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include "std_msgs/String.h"
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <string>        
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include <fstream> //文件读入读出
#include <ctime>
#include <cstdlib> //exit
#include <vector>
#include <array>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>

using namespace std;         
geometry_msgs::PoseStamped pose;
geometry_msgs::PoseStamped p;
nav_msgs::Path path;
int global_status;

array<double,3> Stringsplit(string str,const char split)
{
      array<double,3> arr;
      int n{};
    istringstream iss(str);    // 输入流
    string token;            // 接收缓冲区
    while (getline(iss, token, split))    // 以split为分隔符
    {
            arr[n] = stod(token);
            n++;
    }
      return arr;
}

void result_cb(const move_base_msgs::MoveBaseActionResult::ConstPtr& msg)
{
      actionlib_msgs::GoalStatus status;
      status = msg->status;
      global_status = status.status;
}

int main(int argc,char** argv)
{
      ros::init(argc,argv,"read_path");
      ros::NodeHandle n;
      ros::Publisher pub_path = n.advertise<nav_msgs::Path>("read_path",1000);
      ros::Publisher follow_path = n.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",10);
      ros::Subscriber result_sub = n.subscribe("/move_base/result",10,result_cb);
      
      ifstream file;
      file.open("/xxx/data/explore_lite.txt");

      string str;
      vector<array<double,3>> path_;
      while(getline(file,str))
      {
            auto val = Stringsplit(str,' ');
            path_.emplace_back(val);
      }
      file.close();

      ros::Rate r(1);
      path.header.frame_id = "map";
      path.header.stamp = ros::Time::now();
      for(const auto& n : path_)
      {
            pose.header.frame_id = "map";
            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = n[0];
            pose.pose.position.y = n[1];
            pose.pose.position.z = 0;
            pose.pose.orientation.w = n[2];
            pose.pose.orientation.x = 0;
            pose.pose.orientation.y = 0;
            pose.pose.orientation.z = 0;

            path.poses.emplace_back(pose);
            ROS_INFO("( x:%0.6f ,y:%0.6f ,w:%0.6f)",n[0] ,n[1] ,n[2] );
      }

      while(ros::ok())
      {
            pub_path.publish(path);
            ros::spinOnce();  
            r.sleep();
      }
      return 0;
}

在 rviz 上同时显示 /map 话题和轨迹（frame_id 都为 map），显示的效果如下：
（轨迹的话题为：/read_path）

参考博客：ROS 移动机器人运动轨迹记录，并发布在rviz上
非常感谢！！