参考A-loam运行kitti及轨迹保存_aloam跑kitti数据集-CSDN博客
保存话题/aft_mapped,参考上述链接方法二
保存话题/laser_odom_to_init,参考链接方法二
在main函数中,添加代码
ros::Subscriber save_path= nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, path_save);
并将以下void放在main函数之前
void path_save(nav_msgs::Odometry laserOdometry ){
//保存轨迹,
std::ofstream pose1("/home/cxx/A-LOAM/laser_tum.txt", std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
pose1.precision(6);
static double timeStart = laserOdometry.header.stamp.toSec();
auto T1 =ros::Time().fromSec(timeStart) ;
pose1<< laserOdometry.header.stamp -T1<< " "
<< laserOdometry.pose.pose.position.x << " "
<< laserOdometry.pose.pose.position.y << " "
<< laserOdometry.pose.pose.position.z << " "
<< laserOdometry.pose.pose.orientation.x << " "
<< laserOdometry.pose.pose.orientation.y << " "
<< laserOdometry.pose.pose.orientation.z << " "
<< laserOdometry.pose.pose.orientation.w << std::endl;
pose1.close();
}
运行后在目录下生成两个对应轨迹的txt文件
关键是找到轨迹话题对应的laserOdometry替换