读取机器人移动轨迹并在RVIZ界面中显示-Toy模板网

这篇具有很好参考价值的文章主要介绍了读取机器人移动轨迹并在RVIZ界面中显示。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

前言

机器人在巡检过程中需要沿着固定路线执行任务，因此可以先把机器人的移动轨迹录制并保存下来，之后读取轨迹，方便后续操作。

一、准备

1.坐标系

巡检导航过程中，机器人需要确定好坐标系，以便进行定位与导航，在gazebo仿真下可以选择world坐标系，在实际使用中通常使用的是map坐标系，这里以map坐标系为例进行介绍。

2.ros下的路径消息格式

rosmsg show nav_msgs/Path 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

二、实现过程

1.轨迹保存

思路：使用/amcl_pose话题获取机器人当前的位置信息，用nav_msgs::Path格式的一个变量Path进行保存，控制机器人的运动，当机器人运动距离超过某一数值时，将当前位置pose加入该变量，直到机器人走完预设的路径。之后遍历Path中路径点保存输出CSV文本即可。

#include <fstream>
#include <tf/tf.h>
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
using namespace std;

ros::Subscriber robot_pose_sub_;
ros::Subscriber save_path_sub_ ;
nav_msgs::Path curr_trajectory_;
/*
* 计算两点间距离
*/
double calculateDistanceBetweenPose(const geometry_msgs::PoseStamped& pose1,const geometry_msgs::PoseStamped& pose2)
{
    double d_x = pose2.pose.position.x - pose1.pose.position.x;
    double d_y = pose2.pose.position.y - pose1.pose.position.y;
    return sqrt(d_x* d_x + d_y * d_y);
}
/*
* 保存路径
*/
void savePathToFile(string filename)
{
    ofstream File;
	//保存文本地址
    string filePathName;
    filePathName = "/home/name/path/"+ filename +".csv";

    File.open(filePathName.c_str(),ios::out|ios::trunc);
	//遍历存储路径容器，将路径四元数转为yaw角，写入文本
    for(int i=0;i<curr_trajectory_.poses.size();i++)
    {
        float x = curr_trajectory_.poses[i].pose.position.x;
        float y = curr_trajectory_.poses[i].pose.position.y;

        tf::Quaternion quat;
        tf::quaternionMsgToTF(curr_trajectory_.poses[i].pose.orientation,quat);
        double roll, pitch, yaw;
        tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
        float th = yaw;
        File<<x<<","<<y<<","<<th<<endl;
    }
    File.close();

}
/*
* 当前位置回调，将间距超过4cm的路径点加入容器
*/
void robotPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & curr_pose)
{
    geometry_msgs::PoseStamped current_pose;
    current_pose.header.frame_id = "map";
    current_pose.header.stamp = ros::Time::now();
    current_pose.pose.position = curr_pose->pose.pose.position;
    current_pose.pose.orientation = curr_pose->pose.pose.orientation;
    if(curr_trajectory_.poses.empty())
    {
        curr_trajectory_.poses.push_back(current_pose);
        return ;
    }
    int poses_num = curr_trajectory_.poses.size();
    double dis = calculateDistanceBetweenPose(curr_trajectory_.poses[poses_num - 1],current_pose);
    if(dis > 0.04)
        curr_trajectory_.poses.push_back(current_pose);
}
/*
*  接收路径保存指令，开始保存路径点
*/
void savePathCallback(const std_msgs::String::ConstPtr& msg)
{
    string str_msgs = msg->data.c_str();

    if(str_msgs.compare("end") == 0)
    {
        if(!curr_trajectory_.poses.empty())
        {
            string file_path = "aaa";
            savePathToFile(file_path.c_str());
            curr_trajectory_.poses.clear();
            cout<<"end1!"<<endl;
        }
        cout<<"end2!"<<endl;
    }
}
int main(int argc,char** argv)
{
    ros::init(argc,argv,"save_path_node");
    ros::NodeHandle nh_;

    robot_pose_sub_ = nh_.subscribe("/amcl_pose",1,robotPoseCallback);
    save_path_sub_ = nh_.subscribe("/start_end_record", 1,savePathCallback);
    curr_trajectory_.header.frame_id = "map";
    curr_trajectory_.header.stamp = ros::Time::now();
    ros::spin();
    return 0;
}

运行上述程序后，控制小车运动，当走完所需的路径后，需单独发送一个话题，从而启动路径保存

rostopic pub /start_end_record std_msgs/String "data: 'end'"

2.轨迹读取并显示

思路：读取CSV文本并分割，将路径点发布出去。
这里也发布了路径起点和终点位置。

#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/String.h>
#include <tf/tf.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <ros/time.h>
#include <istream>
#include <sstream>
#include <fstream>
#include <string>
#include <vector>
#include <tf/tf.h>
using namespace std;

/**
 * 字符串分割
*/
std::vector<std::string> split(const std::string &str, const std::string &pattern) {
    char *strc = new char[strlen(str.c_str()) + 1];
    strcpy(strc, str.c_str()); // string转换成C-string
    std::vector<std::string> res;
    char *temp = strtok(strc, pattern.c_str());
    while (temp != NULL) {
        res.push_back(std::string(temp));
        temp = strtok(NULL, pattern.c_str());
    }
    delete[] strc;
    return res;
}

int main(int argc,char** argv)
{
    ros::init(argc,argv,"path_pub");
    ros::NodeHandle n;
    ros::Rate r(10);
    ros::Publisher path_pub = n.advertise<nav_msgs::Path>("/path_pub",10);
    ros::Publisher path_end_pub = n.advertise<geometry_msgs::PointStamped>("/path_end_point",10);
    ros::Publisher path_start_pub = n.advertise<geometry_msgs::PointStamped>("/path_start_point",10);

    nav_msgs::Path nav_path;
    nav_path.header.frame_id= "/map";
    nav_path.header.stamp = ros::Time::now();
    geometry_msgs::PoseStamped path_pose;


   //读取CSV文件
    std::ifstream csv_data("path/aaa.csv",std::ios::in);
    if(!csv_data)
    {
        std::cout<<"open .csv failed"<<std::endl;
        ROS_ERROR(" .csv  doesn't exisit ");
        std::exit(1);
    }
    geometry_msgs::Quaternion quat;
    std::string line;
    int line_num = 0;
    std::vector<std::string> strbuf;

    while(ros::ok())
    {
        while(std::getline(csv_data,line))
        {
            // std::cout<<line<<std::endl;
            path_pose.header.frame_id = "/map";
            path_pose.header.stamp =ros::Time::now();
            path_pose.header.seq = 0;
            line_num++;
            strbuf = split(line, ",");
            path_pose.pose.position.x = atof(strbuf[0].c_str());
            path_pose.pose.position.y = atof(strbuf[1].c_str());
            path_pose.pose.position.z = 0.0;
            float yaw = atof(strbuf[2].c_str());

            quat = tf::createQuaternionMsgFromYaw(yaw);

            path_pose.pose.orientation.x = quat.x;
            path_pose.pose.orientation.y = quat.y;
            path_pose.pose.orientation.z = quat.z;
            path_pose.pose.orientation.w = quat.w;
            nav_path.poses.push_back(path_pose);
        }
        path_pub.publish(nav_path);
        ros::Duration(1).sleep();
        path_end_pub.publish(nav_path.poses[nav_path.poses.size()-1]);
        // std::cout<<"----2-----"<<std::endl;
        path_start_pub.publish(nav_path.poses[0]);

        ros::spinOnce();
        r.sleep();
    }

    return 0;

}