系统ubuntu16.04 kinetic 20.04 noetic
想写一个小车寻迹程序,用rostopic echo odom/pose/pose/position > odom.txt ,保存的数据类型不好用,而且还要用到orientation数据,最好还是自己写一个程序吧程序如下,
#include "ros/ros.h" //ros需要的头文件
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"
//以下为串口通讯需要的头文件
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include <fstream>
/****************************************************************************/
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<<x_pre<<"\n"<<y_pre<<"\n"<<orientationx<<"\n"<<orientationy<<"\n"<<orientationz<<"\n"<<orientationw<<"\n"<<std::endl;
out.close();
cout << x_pre << endl;
cout << y_pre << endl;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "odom_record");//初始化串口节点
ros::NodeHandle n; //定义节点进程句柄
// ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题
ros::Subscriber odom_sub = n.subscribe("odom", 100, callback_odom);
//ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("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
空格
这么每七行一个记录。
亲测可用
完整代码在这里 odom_record.7z-Linux文档类资源-CSDN下载
至于对这个程序的调用,在下一篇博文里有。
评论(0)
您还未登录,请登录后发表或查看评论