ros rviz显示orb-slam2保存的轨迹

写在前面

1、本文内容
ros rviz显示orb-slam2保存的轨迹

2、平台
ubuntu1804, ros melodic
3、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/126838142

代码

mkdir -p trajectory/src
cd trajectory/src
catkin_create_pkg show_path roscpp rospy tf nav_msgs
cd ..
1
2
3
4

把show_path.cpp放进src/show_path/src
在CMakeLists.txt中添加

add_executable(show_path_node src/imu_tf_pose.cpp)
target_link_libraries( show_path_node ${catkin_LIBRARIES})
1
2

show_path.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include "nav_msgs/Path.h"
#include 


ros::Publisher puber;


void PublishPath(std::string file_path)
{


    nav_msgs::Path path_msg;
    geometry_msgs::PoseStamped pose;

    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "/world";
    std::ifstream fin;
    fin.open(file_path,std::ios::in);
    if(!fin.is_open()){
        std::cout<<"can not open "<<file_path<<std::endl;
        return;
    }
    else{
            Eigen::Quaterniond Q_from_R;
            Eigen::Vector3d traj_node;
            while(!fin.eof()){
                double time,tx,ty,tz,qx,qy,qz,qw;
                fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
                pose.pose.position.x = tx;
                pose.pose.position.y = ty;
                pose.pose.position.z = tz;
                pose.pose.orientation.x = qx;
                pose.pose.orientation.y = qy;
                pose.pose.orientation.z = qz;
                pose.pose.orientation.w = qw;
                path_msg.poses.push_back(pose);
            }

    }

    path_msg.header.stamp = ros::Time::now();
    path_msg.header.frame_id = "/world";
    puber.publish(path_msg);
}

int main (int argc, char ** argv) {
    std::cout<<"argc: "<<argc<<std::endl;
    for(int i=0;i<argc;++i){
        std::cout<<argv[i]<<std::endl;
    }


    if(argc<2){
        std::cout<<"parameters:\n"
                        <<"file path including trajectory\n"
                        <<"topic name, default /trajectory"
                        <<std::endl;
        return 0;
    }
    std::string topic_name="/trajectory";
    
    std::string file_path=argv[1];
    if(argc>=3){
    topic_name=argv[2];
    std::cout<<"topic_name"<<topic_name<<std::endl;
    }
    ros::init(argc, argv, "path_publisher");
    
    ros::NodeHandle node;

    puber=node.advertise<nav_msgs::Path>(topic_name, 10);

    ros::Rate loop_rate(10);
    while(ros::ok()){
    PublishPath(file_path);
    }

    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
83
84
85
86

使用

./devel/lib/show_path/show_path_node ./orb_slam2.txt /trajectory1 
1

打开rviz，add, by topic，选择发布的trajectory1
把fixed frame改成world

其中，/orb_slam2.txt为orb-slam2生成的轨迹文件，如

1662709738.611801 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000
1662709738.944941 -0.0026136 0.0170262 -0.0045148 0.0095704 0.0072003 0.0153332 0.9998107
1662709739.544584 0.0069332 0.0336722 0.0215601 0.0175027 -0.0293207 0.0545010 0.9979296
1662709744.308815 0.0545398 0.0866963 0.1380486 -0.0544997 -0.1701589 0.1484294 0.9726481
1662709744.508729 0.0569852 0.0875546 0.1378780 -0.0624662 -0.1643448 0.1828232 0.9672975
1662709744.675315 0.0595775 0.0904402 0.1297402 -0.0711926 -0.1598502 0.2039685 0.9632115
1662709745.308262 0.0525226 0.0983748 0.1075200 -0.0962976 -0.1551851 0.2081083 0.9609034
1662709746.774192 0.0280566 0.0733101 0.1472074 -0.0261320 -0.1733477 0.1026057 0.9791526
...
...
...
1
2
3
4
5
6
7
8
9
10
11

参考

https://github.com/gaoxiang12/slambook2/blob/9382d9066029308ff40a539d64880d540135bfd6/ch3/examples/plotTrajectory.cpp
https://github.com/robosu12/imu_data_simulation/blob/master/src/vio_data_simulation/src/gener_alldata.cpp

完

--------------------------------------------------------------------------------------------202209