ROS 读取csv文件，Rviz显示路径path

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseArray.h>
#include <fstream>
#include <string>
#include <cstdlib>
#include <ros/package.h>
#include "tf/tf.h"
#include <iostream>
#include <nav_msgs/Path.h>

void getPose(std::string s, double *v)
{
    int p = 0;
    int q = 0;
    for (int i = 0; i < s.size(); i++)
    {
        if (s[i] == ',' || i == s.size() - 1)
        {
            char tab2[16];
            strcpy(tab2, s.substr(p, i - p).c_str());
            v[q] = std::strtod(tab2, NULL);
            p = i + 1;
            q++;
        }
    }
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "moveit_cartesian_demo");
    ros::NodeHandle n;
    ros::Publisher path_pub = n.advertise<geometry_msgs::PoseArray>("trajectory", 10, true);
      ros::Publisher state_pub_ = n.advertise<nav_msgs::Path>("gps_path", 10);
    ros::AsyncSpinner spinner(1);
    spinner.start();
    sleep(1);
    std::ifstream f(ros::package::getPath("probot_demo") + "/paths/" + "cubeOri.csv");
    //f.open(ros::package::find(plan_pkg)+"/paths/path.csv"); //ros::package::find(plan_pkg)
    if (!f.is_open())
    {
        ROS_ERROR("failed to open file");
        return 0;
    }
    std::string line;
    std::vector<geometry_msgs::Pose> waypoints;
    double scale = 100;
    int count = -1;
    nav_msgs::Path ros_path_;

    geometry_msgs::PoseArray track;
    track.header.stamp = ros::Time::now();
    track.header.frame_id = "/ebot_base";
    while (std::getline(f, line))
    {
        std::cout << "Hello w000orld!!!" << std::endl;

        count++;
        double pose[6];
        getPose(line, pose);
        tf::Quaternion q = tf::createQuaternionFromRPY(pose[3], pose[4], pose[5]);
        geometry_msgs::Pose pose1;

        geometry_msgs::PoseStamped pose2;
        ros_path_.header.frame_id = "/ebot_base";
        ros_path_.header.stamp = ros::Time::now();  
        pose2.header = ros_path_.header;

        pose2.pose.position.x = pose[0];
        pose2.pose.position.y = pose[1];
        pose2.pose.position.z = pose[2];
        pose1.orientation.x =0;
        pose1.orientation.y = 0;
        pose1.orientation.z = 0;
        pose1.orientation.w = 1;
        pose1.position.x = pose[0];
        pose1.position.y = pose[1];
        pose1.position.z =pose[2];
        std::cout << "path_pose1.position.x" << pose1.position.x <<std::endl;

        ROS_INFO("Adding waypoint x,y,z =  [%.2f, %0.2f, %0.2f]", pose1.position.x, pose1.position.y, pose1.position.z);
    
            waypoints.push_back(pose1);
            track.poses.push_back(pose1);
            ros_path_.poses.push_back(pose2);

    }

    sleep(1);
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        path_pub.publish(track);
        state_pub_.publish(ros_path_);
    }
    return 0;
}