Gazebo多场景下的TurtleBot4键盘控制与真值轨迹记录
前言：
上一篇博客已经介绍了如何安装turtlebot4机器人包，运行机器人的ign gazebo仿真环境并加载turtlebot4机器人模型，接下来需要使用键盘控制机器人运动，在运动的过程中订阅tutlebot4机器人发布的/odom话题消息，保存为真值轨迹文件，并使用evo进行可视化（如果还未安装turtlebot4包，可以参考我上篇博客）
原理：
turtlrbot4的机器人的urdf文件里有一个差速控制插件diff，turtlrbot4机器人在仿真过程中通过该插件订阅/cmd_vel和发布/odom话题。而ros2的键盘控制节点能够发布/cmd_vel的话题，从而可以使用键盘控制机器人运动（当然也可以自己写一个节点发布cmd_vel话题）。发布的/odom话题数据，就是机器人的真实位姿
键盘控制：
输入以下指令启动机器人ign gazebo仿真
ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py待机器人完成初始化后，就可以使用键盘控制机器人运动了，输入指令：
ros2 run teleop_twist_keyboard teleop_twist_keyboard 就可以控制机器人运动。控制机器人走一段路径，如下图所示：
保存轨迹：
odom数据格式
启动机器人仿真ign gazebo仿真后，机器人会发布/odom话题数据，我们先来看看/odom话题数据格式，输入以下指令：
ros2 topic listros2 topic echo /odom通过以上指令，我们可以看到/odom话题的数据消息格式，如下图所示：
可以看到，话题数据是一个列表形式，我们只需要其中的position和orientation数据，即位置和姿态。
创建ros2节点
在turtlrbot4的工作空间turtlrbot4_ws/src中创建一个功能包，输入以下指令：
ros2 pkg create trajectory_save --build-type ament_python --dependencies rclpy创建功能包后，进入trajectory_save/trajectory_save文件路径下，创建node.py的文件，在这个文件中，我们编写代码实现订阅/odom话题数据，并保存为轨迹文件，代码如下：
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
import sys
class OdomSubscriber(Node):
def __init__(self):
super().__init__('odom_subscriber')
self.subscription = self.create_subscription(
Odometry,
'/odom',
self.odom_callback,
10
)
self.file = open('trajectory_real_odom.tum', 'w')
def odom_callback(self, msg):
# 提取时间戳、位置和四元数
timestamp_sec = msg.header.stamp.sec
timestamp_nsec = msg.header.stamp.nanosec
timestamp = timestamp_sec + timestamp_nsec * 1e-9
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
# 写入 TUM 文件格式
line = f"{timestamp} {position.x} {position.y} {position.z} "
f"{orientation.x} {orientation.y} {orientation.z} {orientation.w}
"
self.file.write(line)
def main(args=None):
rclpy.init(args=args)
odom_subscriber = OdomSubscriber()
try:
rclpy.spin(odom_subscriber)
except KeyboardInterrupt:
pass
finally:
odom_subscriber.file.close()
odom_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()同时在setup.py中修改配置，如图所示：
entry_points={
'console_scripts': [
"node = trajectory_save.node:main"
],
},进入turtlebot4_ws工作空间运行编译指令：
colcon build注意事项：
由于turtlrbot4的工作空间和该ros2节点的工作空间相同，我们可把/turtlrbot4_ws/install/setup.bash文件添加到～/.bashrc中，这样可以不用每次运行前在终端source环境变量。
运行以下指令打开~/.bashrc文件
vim ~/.bashrc添加环境如图所示：
# >>> fishros initialize >>>
source /opt/ros/humble/setup.bash
# <<< fishros initialize <<<
source /home/petermi/turtlebot4_ws/install/setup.bash #替换成你自己的路径
保存退出后，运行以下指令进行刷新
source ~/.bashrc打开任意一个终端，输入以下指令：
ros2 run trajectory_save node运行以上指令，就可以在该终端对应的文件路径下生成轨迹文件trajectory_real_odom.tum，如下图所示：
键盘控制运动同时保存轨迹文件
最终，我们只需要在ign gazebo环境中，同时启动键盘控制和录制轨迹，就可以得到机器人在运动过程中的真实轨迹文件
(1) 启动ign gazebo仿真环境
ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py(2) 启动键盘控制节点
ros2 run teleop_twist_keyboard teleop_twist_keyboard(3) 启动ros2节点
ros2 run trajectory_save node手动操作键盘控制机器人走一段路径后停止，如下图所示：
在与运行了ros2 run trajectory_save node指令的终端界面按ctrl+c结束录制，得到轨迹文件trajectory_real_odom.tum,并打开tum文件，如图所示：
evo画出轨迹
我这里已经提前下载好evo，如果想要下载，可以直接输入以下命令：
pip install evo --upgrade --no-binary evo -i https://pypi.tuna.tsinghua.edu.cn/simple
#用清华源加速pip安装
# 添加 ~/.local/bin 到 PATH
echo 'export PATH="$HOME/.local/bin:$PATH"' >> ~/.bashrc
source ~/.bashrc
这里要注意的就是evo的版本。最后就可以使用evo画出机器人的真实轨迹,输入指令如下：
evo_traj tum trajectory_real_odom.tum -p
机器人的真实轨迹如图所示：
补充：/sim_ground_truth_pose话题数据也是机器人的真实位姿，ign gazebo仿真中该话题发布的位姿数据比/odom话题数据更加准确，大家有兴趣的可以试一试。本人最近在学习slam，如果有不对的地方，恳请指正，谢谢
