使用ROS读取tum格式轨迹文件并在RVIZ显示的实现过程

2024-02-01 15:10

本文主要是介绍使用ROS读取tum格式轨迹文件并在RVIZ显示的实现过程,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

内容

有一个轨迹文本文件,使用ROS系统读取该文件,并将轨迹在RVIZ界面绘制。

先用EVO工具将tum格式轨迹文件转为bag格式;

再创建ROS功能包,订阅bag文件中的话题数据,处理再发布新的轨迹话题,同时新建线程发布TF。

最后编译、运行,多个终端发布开启roscore、启动功能包、打开rviz、播放bag。

实现过程

tum转bag

使用EVO工具将tum格式轨迹文件转为bag格式:

evo_traj tum traj_1.txt traj_2.txt --save_as_bag

需要同时安装有 EVO和ROS。

完成转换后得到.bag格式文件,文件里面的话题类型为<geometry_msgs::PoseStamped>。

创建功能包

进入ROS工作空间的源代码文件夹:

cd ~/catkin_ws/src

 在src目录下创建一个新的包,例如showpath

catkin_create_pkg showpath

写入代码 

进入新创建的包目录:

cd showpath

 创建一个src文件夹,并进入该文件夹

mkdir src
cd src

创建一个C++源文件,例如read_tum_path.cpp,并使用文本编辑器打开它: 

touch read_tum_path.cpp
gedit read_tum_path.cpp

将以下代码复制粘贴到read_tum_path.cpp中:

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <thread>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
using namespace std;
ros::Publisher path_pub; 
nav_msgs::Path path;void gt_path_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{// 创建并填充nav_msgs::Path消息path.header = msg->header;path.header.frame_id = "camera_link";path.poses.push_back(*msg);cout<<"The program is reading data.stamp:"<<msg->header.stamp<<endl;// 发布nav_msgs::Path消息path_pub.publish(path);
}void publishTf()
{ros::NodeHandle nh;tf2_ros::TransformBroadcaster broadcaster;while (ros::ok()){// 创建并设置tf2变换消息geometry_msgs::TransformStamped transformStamped;transformStamped.header.frame_id = "base_link";transformStamped.child_frame_id = "camera_link";transformStamped.transform.translation.x = 0;transformStamped.transform.translation.y = 0.0;transformStamped.transform.translation.z = 0;transformStamped.transform.rotation.x = 0.0;transformStamped.transform.rotation.y = 0.0;transformStamped.transform.rotation.z = 0.0;transformStamped.transform.rotation.w = 1.0;ros::Rate rate(10.0);  // 发布频率为10HztransformStamped.header.stamp = ros::Time::now();broadcaster.sendTransform(transformStamped);// 发布频率适当调整ros::Rate(10).sleep();}
}int main(int argc, char** argv)
{ros::init(argc, argv, "tf_and_dataset_publisher");ros::NodeHandle nh;
cout<<"Program startup completed"<<endl;
ros::Subscriber sub_lidar = nh.subscribe("/gt_traj", 100,gt_path_callback);// 创建一个发布器,用于发布nav_msgs::Path消息path_pub = nh.advertise<nav_msgs::Path>("path_topic", 10);// 创建两个线程,分别用于发布tf变换和播放数据集// 创建线程,用于发布tf变换std::thread tf_thread(&publishTf);ros::spin();
cout<<"Program startup completed"<<endl;// 等待线程结束tf_thread.join();  return 0;
}

上述程序中,新建节点tf_and_dataset_publisher,接收/gt_traj话题,<geometry_msgs::PoseStamped>格式数据被回调函数处理后发布<nav_msgs::Path>类型的/path_topic话题。同时用std::thread新建线程发布tf变换,tf变换根据需要设定。

回到showpath包目录,并创建一个CMakeLists.txt文件,使用文本编辑器打开它: 

cd ..
touch CMakeLists.txt
gedit CMakeLists.txt

创建功能包时候已经创建有 CMakeLists.txt文件,删除新建就行。

将以下内容复制粘贴到CMakeLists.txt中:

cmake_minimum_required(VERSION 2.8.3)
set(CMAKE_CXX_STANDARD 11)project(showpath)find_package(catkin REQUIRED COMPONENTSroscpptf2_rosnav_msgsgeometry_msgs
)catkin_package(
)include_directories(${catkin_INCLUDE_DIRS}
)add_executable(read_tum_path src/read_tum_path.cpp)
target_link_libraries(read_tum_path ${catkin_LIBRARIES})

 保存并关闭CMakeLists.txt文件。

编译构建

返回到ROS工作空间的根目录,并构建该包: 

cd ~/catkin_ws
catkin_make

如果有报错,根据报错内容修改。

运行程序

 打开多个终端,分别运行ROS核心、showpath功能包、RVIZ界面、播放数据集。

roscore
rosrun showpath read_tum_path
rviz
rosbag play --pause xxx.bag

在rviz中添加一个Path显示器,并将其订阅到/path_topic话题,或者在“topic”中直接添加/path_topic话题,以显示轨迹的可视化效果。

 效果

这篇关于使用ROS读取tum格式轨迹文件并在RVIZ显示的实现过程的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/667628

相关文章

浅析Spring Security认证过程

类图 为了方便理解Spring Security认证流程,特意画了如下的类图,包含相关的核心认证类 概述 核心验证器 AuthenticationManager 该对象提供了认证方法的入口,接收一个Authentiaton对象作为参数; public interface AuthenticationManager {Authentication authenticate(Authenti

中文分词jieba库的使用与实景应用(一)

知识星球:https://articles.zsxq.com/id_fxvgc803qmr2.html 目录 一.定义: 精确模式(默认模式): 全模式: 搜索引擎模式: paddle 模式(基于深度学习的分词模式): 二 自定义词典 三.文本解析   调整词出现的频率 四. 关键词提取 A. 基于TF-IDF算法的关键词提取 B. 基于TextRank算法的关键词提取

使用SecondaryNameNode恢复NameNode的数据

1)需求: NameNode进程挂了并且存储的数据也丢失了,如何恢复NameNode 此种方式恢复的数据可能存在小部分数据的丢失。 2)故障模拟 (1)kill -9 NameNode进程 [lytfly@hadoop102 current]$ kill -9 19886 (2)删除NameNode存储的数据(/opt/module/hadoop-3.1.4/data/tmp/dfs/na

Hadoop数据压缩使用介绍

一、压缩原则 (1)运算密集型的Job,少用压缩 (2)IO密集型的Job,多用压缩 二、压缩算法比较 三、压缩位置选择 四、压缩参数配置 1)为了支持多种压缩/解压缩算法,Hadoop引入了编码/解码器 2)要在Hadoop中启用压缩,可以配置如下参数

作业提交过程之HDFSMapReduce

作业提交全过程详解 (1)作业提交 第1步:Client调用job.waitForCompletion方法,向整个集群提交MapReduce作业。 第2步:Client向RM申请一个作业id。 第3步:RM给Client返回该job资源的提交路径和作业id。 第4步:Client提交jar包、切片信息和配置文件到指定的资源提交路径。 第5步:Client提交完资源后,向RM申请运行MrAp

Makefile简明使用教程

文章目录 规则makefile文件的基本语法:加在命令前的特殊符号:.PHONY伪目标: Makefilev1 直观写法v2 加上中间过程v3 伪目标v4 变量 make 选项-f-n-C Make 是一种流行的构建工具,常用于将源代码转换成可执行文件或者其他形式的输出文件(如库文件、文档等)。Make 可以自动化地执行编译、链接等一系列操作。 规则 makefile文件

hdu1043(八数码问题,广搜 + hash(实现状态压缩) )

利用康拓展开将一个排列映射成一个自然数,然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu

第10章 中断和动态时钟显示

第10章 中断和动态时钟显示 从本章开始,按照书籍的划分,第10章开始就进入保护模式(Protected Mode)部分了,感觉从这里开始难度突然就增加了。 书中介绍了为什么有中断(Interrupt)的设计,中断的几种方式:外部硬件中断、内部中断和软中断。通过中断做了一个会走的时钟和屏幕上输入字符的程序。 我自己理解中断的一些作用: 为了更好的利用处理器的性能。协同快速和慢速设备一起工作

使用opencv优化图片(画面变清晰)

文章目录 需求影响照片清晰度的因素 实现降噪测试代码 锐化空间锐化Unsharp Masking频率域锐化对比测试 对比度增强常用算法对比测试 需求 对图像进行优化,使其看起来更清晰,同时保持尺寸不变,通常涉及到图像处理技术如锐化、降噪、对比度增强等 影响照片清晰度的因素 影响照片清晰度的因素有很多,主要可以从以下几个方面来分析 1. 拍摄设备 相机传感器:相机传

【C++】_list常用方法解析及模拟实现

相信自己的力量,只要对自己始终保持信心,尽自己最大努力去完成任何事,就算事情最终结果是失败了,努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一:什么是list? •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二:list常用接口 •🌰1.默认成员函数 🔥构造函数(⭐) 🔥析构函数 •🌰2.list对象