ROS 中订阅odom消息并按行保存到txt中

  系统ubuntu16.04  kinetic

想写一个小车寻迹程序，用rostopic echo odom/pose/pose/position > odom.txt  ，保存的数据类型不好用，而且还要用到orientation数据，最好还是自己写一个程序吧程序如下，

#include "ros/ros.h"  //ros需要的头文件
#include 
#include 
#include "std_msgs/String.h"
//以下为串口通讯需要的头文件
#include         
#include 
#include 
#include 
#include 
#include 

/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;

//float x_pre,y_pre,z_pre;
 float x_pre=0.0f;
float y_pre=0.0f;
/************************************************************/

void callback_odom(const nav_msgs::Odometry::ConstPtr& odom)//订阅/odom主题回调函数
{


     
   // geometry_msgs::PoseStamped this_pose_stamped;
	float x,y,z;
	//float x_pre,y_pre,z_pre;

      float orientationx,orientationy,orientationz,orientationw;
	x = odom -> pose.pose.position.x;
	y = odom -> pose.pose.position.y;
	//z = odom -> pose.pose.position.z;
	orientationx=odom -> pose.pose.orientation.x;
	orientationy=odom -> pose.pose.orientation.y;
	orientationz=odom -> pose.pose.orientation.z;
	orientationw=odom -> pose.pose.orientation.w;
if(sqrt((x-x_pre)*(x-x_pre)+(y-y_pre)*(y-y_pre)) >= 1)
{
	x_pre=x;
	y_pre=y;
 	std::ofstream out("/home/~/point.txt",std::ios::app);
//注意这里是你自己的地址
 	out<("odom", 20); //定义要发布/odom主题


		    //里程计位置数据：x,y,z,方向
		  //  odom.pose.pose.position.x = position_x;     
		  //  odom.pose.pose.position.y = position_y;
		 //   odom.pose.pose.position.z = 0.0;
		//    odom.pose.pose.orientation = odom_quat;       

    ros::spin();
    return 0;
}

std::ofstream out("/home/~/point.txt",std::ios::app);   这里面的路径中～ 是你自己的地址，最后程序运行后，当发布odom消息后，能在home根目录下有一个point.txt文件，内容为

position.x

position.y

orientation.x

orientation.y

orientation.z

orientation.w

空格

这么每七行一个记录。

亲测可用

完整代码在这里 https://download.csdn.net/download/weixin_42638797/20298369

至于对这个程序的调用，在下一篇博文里有。