SLAM ROS显示轨迹_ros显示轨迹点

#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

using namespace std;
using namespace Eigen;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "trajectory");
    ros::NodeHandle node;
    tf::TransformBroadcaster br;

    // tf file
    vector<double> pose;
    vector<vector<double>> poses;
    ifstream  fin("/home/lzf/catkin_ws/src/slam/file/trajectory.txt");
    double time, tx, ty, tz, qx, qy, qz, qw;
    if (!fin)
    {
        cout << "cannot open file";
    }
    else
    {
        
        while (!fin.eof())
        {
            /* code */  
            fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
            pose.push_back(time); 
            pose.push_back(tx); 
            pose.push_back(ty); 
            pose.push_back(tz);
            pose.push_back(qx); 
            pose.push_back(qy); 
            pose.push_back(qz); 
            pose.push_back(qw);
            poses.push_back(pose);
            // ROS_INFO("t  %lf  %lf  %lf  \n",  tx, ty, tz);
            // ROS_INFO("v  %lf  %lf  %lf  \n",  pose[1], pose[2], pose[3]);
            pose.clear();
        }
    }
    cout << "pose nums" << poses.size() << endl;

    tf::Transform transform;
    vector<double> tmp;
    ros::Rate rate(10);
    double implify = 1.0;

    for (int i=0; i < poses.size(); i++)
    {
        tmp = poses[i];
        cout << tmp[5] << tmp[6] <<tmp[7] << endl;
    }
    
    while (ros::ok())
    {
        tmp = poses[0];
        transform.setOrigin(tf::Vector3(implify*tmp[1], implify*tmp[2], implify*tmp[3]));
        // tf::Quaternion q(implify*tmp[4], implify*tmp[5], implify*tmp[6], implify*tmp[7]);
        // transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", to_string(0)));
        for (int i=1; i < poses.size(); i++)
        {
            transform.setOrigin(tf::Vector3(implify*(poses[i][1] - poses[i - 1][1]), implify*(poses[i][2] - poses[i - 1][2]), implify*(poses[i][3] - poses[i - 1][3])));
            Matrix3d T1 = Quaterniond(implify*poses[i - 1][4], implify*poses[i - 1][5], implify*poses[i - 1][6], implify*poses[i - 1][7]).matrix();
            Matrix3d T2 = Quaterniond(implify*poses[i][4], implify*poses[i][5], implify*poses[i][6], implify*poses[i][7]).matrix();
            Matrix3d T3 = T1.inverse() * T2;
            Quaterniond eq = Quaterniond(T3);
            ROS_INFO("%lf  %lf  %lf  %lf", eq.x(), eq.y(), eq.z(), eq.w());
            tf::Quaternion q(eq.x(), eq.y(), eq.z(), eq.w());
            transform.setRotation(q);
            br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), to_string(i - 1), to_string(i)));
        }
        rate.sleep();
    }
    
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

trajectory.txt