A-LOAM保存轨迹，修改文件记录

参考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替换