读取存放在 .txt 文件中的七轴轨迹数据存放到 trajectory 变量中

# include <iostream>
# include <ros/ros.h>
# include <moveit/robot_trajectory/robot_trajectory.h>
# include <fstream>//1、包含头文件
# include <vector>//使用 vector 容器需要包含该头文件
# include <string>
# include <moveit/move_group_interface/move_group_interface.h>


using namespace std;

void LoadDatas(const string &trajectory_data_txt, vector<string> &tube)
{
    ifstream ifs;//2、创建流对象
    ifs.open(trajectory_data_txt,ios::in);
    //3、打开文件并判断是否打开成功
    if(!ifs.is_open())
    {
        cout << "Error opening file !" << endl;
        return;
    }
    //4、读取文件中的数据

    tube.reserve(5000);

    while(!ifs.eof())
    {
        string s;
        getline (ifs,s);
        if(!s.empty())
        {
            tube.push_back(s);
        }
    }
    //5、关闭文件
    ifs.close();
}

int main(int argc,char **argv)
{

    vector<string> tube;
    LoadDatas("trajectory.txt",tube);

    int num = tube.size();//返回的 size 是存储数据的行数（1155行）
    cout << "num = " << num << endl;

    // //直接顺序访问
    // cout << "直接顺序访问" << endl;
    // for(int i=0; i<10; i++)
    // {
    //     cout << tube[i] << endl;
    // }
    // //利用迭代器访问
    // cout << "利用迭代器访问" << endl;
    // vector<string>::iterator it;//声明一个迭代器来访问vector容器，作用：遍历或者指向vector容器的元素
    // for(it=tube.begin(); it!=tube.end(); it++)
    // {
    //     cout << *it <<endl;
    // }
    
    ros::init(argc,argv,"publish");//初始化节点
    ros::NodeHandle n;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // ros::Publisher tra_pub = n.advertise<moveit_msgs::RobotTrajectory>("/Trajectory_data",10);
    // ros::Rate loop_rate(10);

    moveit::planning_interface::MoveGroupInterface arm("xmate_arm");

    arm.setNamedTarget("home");
    arm.move();
    sleep(1);

    moveit_msgs::RobotTrajectory tra;
    tra.joint_trajectory.header.frame_id = "base_link";
    tra.joint_trajectory.joint_names = {"joint1","joint2","joint3","joint4","joint5","joint6","joint7"};
    
    int n_joints = tra.joint_trajectory.joint_names.size();
    cout << n_joints << endl;

    string p_s[7];
    string v_s[7];
    string a_s[7];
    string time_s;

    double p[7];
    double v[7];
    double a[7];
    double time;

    int points_num = (num-14-7)/27;

    tra.joint_trajectory.points.resize(points_num);//初始化 points 点数

    for(int i=0;i<points_num;i++)
    {
        tra.joint_trajectory.points[i].positions.resize(n_joints);//初始化 points 的 positions 数
        tra.joint_trajectory.points[i].velocities.resize(n_joints);
        tra.joint_trajectory.points[i].accelerations.resize(n_joints);

        for(int j=0;j<n_joints;j++)
        {
            p_s[j] = tube[16 + 27 * i + j].substr(22,15);
            p[j] = atof(p_s[j].c_str());
            tra.joint_trajectory.points[i].positions[j] = p[j];

            v_s[j] = tube[24 + 27 * i + j].substr(23,15);
            v[j] = atof(v_s[j].c_str());
            tra.joint_trajectory.points[i].velocities[j] = v[j];
            
            a_s[j] = tube[32 + 27 * i + j].substr(26,15);
            a[j] = atof(a_s[j].c_str());
            tra.joint_trajectory.points[i].accelerations[j] = a[j];
        }

        time_s = tube[40 + 27 * i].substr(23,15);
        time = atof(time_s.c_str());

        tra.joint_trajectory.points[i].time_from_start = ros::Duration().fromSec(time);
        // tra_pub.publish(tra.joint_trajectory.points[i]);
    }

    // cout << tra <<endl;

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory_ = tra;

    arm.execute(plan);
    sleep(1);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(1);


    ros::shutdown();
    // ros::spinOnce();
    return 0;
}