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

【探索评估】记录小车运动轨迹，并在 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;
}