固定路径保存并读取-优快云博客

本文链接：https://blog.youkuaiyun.com/xxxx2022D/article/details/140669074

在src中创建功能包

catkin_create_pkg 功能包名称 rospy roscpp std_msgs geometry_msgs

然后编写在功能包/src路径下编写两个cpp文件

保存路径save_road.cpp

#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 = "保存路径地址.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;
}

读取路径read_road.cpp

#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("读取的路径地址.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;
            */
            path_pose.pose.orientation.z = std::sin(atof(strbuf[1].c_str())/2.0);
            path_pose.pose.orientation.w = std::cos(atof(strbuf[1].c_str())/2.0);
            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;

}

然后要在功能包的CMakelists添加库链接

add_executable(save_road src/save_road.cpp)
target_link_libraries(save_road ${catkin_LIBRARIES})
add_executable(read_road src/read_road.cpp)
target_link_libraries(read_road ${catkin_LIBRARIES})

然后catkin_make

source devel/setup.bash

实操步骤
1.先打开导航

2.再开终端

rosrun road save_road

3.控制机器人移动

4.再开终端，发送话题，从而启动路径保存

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

这时候你查看文件夹会发现生成了相应的路径.csv文件

5.终止所有终端，重新打开导航

6.再开终端

rosrun road read_road

7.rviz左下角Add添加话题/path_pub

如图所示，之前走过的路径出现！！！！！