写在前面
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 ..
把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})
show_path.cpp
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <string>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include "nav_msgs/Path.h"
#include <fstream>
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;
}
使用
./devel/lib/show_path/show_path_node ./orb_slam2.txt /trajectory1
打开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
...
...
...
参考
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