TurtleBot3在ROS2 humble中的仿真解析之仿真、遥控与智控

在TurtleBot3的机器人建模完成后，接下来就需要进行仿真。机器人建模之后，机器人的部件、关节的物理参数就确定了，但是还不能使用安装在其上的各种传感器来感知世界。所谓仿真就是要模仿真实环境中对象/机器人的行为，其主要目的，就是要让机器人通过摄像头、激光雷达、毫米波雷达、超声波雷达、惯性测量单元等传感器感知世界，从而对感知到的世界做出适当反应。

由此可知，仿真之前需要对机器人运行的外部环境建模，在turtlebot3开源代码中提供几个世界模型，其存放在以下目录中：

~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/worlds

本文以文件名为turtlebot3_world.world的世界模型为例进行讲解，将其替换为不同世界模型文件，就变成了不同仿真：empty地图仿真、house地图仿真等等。

在ROS2中进行仿真的主要工具是gazebo，本文将利用gazebo就机器人的仿真环境搭建，仿真环境中的机器人的遥控，以及机器人在仿真环境的智控等诸多方面进行代码级深入剖析。

本文从以下三方面进行讲解：

一、仿真环境部署

二、机器人的遥控

三、机器人的智控

关于在ROS2 humble中TurtleBot3机器人建模，可参考下文：

(29条消息) TurtleBot3在ROS2 humble中的仿真解析之Fake Node_数据绿洲的博客-CSDN博客

一、仿真环境部署

1. 部署

部署命令：

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

部署内容包括gazebo节点、robot_state_publish节点、机器人实体相关节点（差速轮、IMU、LiDAR、Camera、关节等）。

2. 代码解析

目录结构：

部署相关的文件有部署文件、仿真定义文件、世界模型文件等。

主部署脚本：

文件位置：

~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_world.launch.py

部署脚本中包含：

- 通过gazebo_ros包部署gazebo节点

- 部署robot_state_publisher节点

- 通过gazebo_ros包spawn_entity服务部署新机器人实体节点

turtlebot3_gazebo包提供许多其他机器人部署脚本，其所在目录为：

~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/launch

部署gazebo节点

部署robot_state_publisher节点

robot_state_publisher节点部署：

脚本文件：

~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/launch/robot_state_publisher.launch.py

通过现有的robot_state_pubslisher包中的robot_state_pubslisher进行部署。

部署机器人实体节点：

机器人实体部署脚本文件：

~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py

 

机器人实体节点部署是利用gazebo_ros包中spawn_entity服务来产生新的机器人实体节点，其主要利用仿真定义文件(sdf: simulation definition file)来定制机器人的部件、关节和插件。注意此文件中使用了如下四个gazebo提供的插件，这四个插件提供了标准化的fake node的所有功能 (参见(30条消息) TurtleBot3在ROS2 humble中的仿真解析之Fake Node_数据绿洲的博客-CSDN博客)，还包括激光雷达模块、惯性测量单元模块、差速驱动轮和关节发布相关功能。

- 差速驱动轮插件：libgazebo_ros_diff_drive.so

- 关节状态发布插件：libgazebo_ros_joint_state_publish.so

- 惯性测量单元插件：libgazebo_ros_imu_sensor.so

- 激光雷达插件：libgazebo_ros_ray_sensor.so

3. 部署的结果

通过上述部署脚本的部署，由机器人模型、世界模型和传感器构成的仿真环境呈现如下：

相应的节点/话题图如下图（为了让话题的订阅者和发布者更加完整的呈现，在此先将作为/cmd_vel话题的发布者的teleop_keyboard加入其中）：

二、机器人的遥控

由前面的讲解可知，对机器人发布的命令是通过写/cmd_vel话题来完成的，故对仿真机器人遥控，可以通过ROS2提供的turtlebot3_teleop包中的teleop_keyboard节点来进行（该节点就是发布命令到/cmd_vel话题）。

其命令如下：

ros2 run turtlebot3_teleop teleop_keyboard

当然，你也可以写一个自己的遥控节点，但是功能都一样，即通过向/cmd_vel话题写入速度命令，为机器人行走提供目标线速度和目标角速度。

teleop_keyboard节点能够手动操控机器人沿着命令指定的方向运动，操控界面如下图所示：

 操控机器人的结果直接反应到gazebo界面进行可视化，如下图：

三、机器人的智控

仿真机器人的智控就是设计一个遥控节点，不仅要提供机器人行走的目标线速度和目标角速度，而且要使机器人在运动过程中不要与环境中的物体发生碰撞。这就需要涉及到环境感知，检测机器人周边的障碍物，从而选择一个不发生碰撞的运动命令。相较于遥控操作是源于人通过感知机器人周边环境而利用键盘发布指令指挥、控制机器人运动不同，智控是机器人通过安装在其上的LiDAR传感器来感知环境，直接指挥、控制机器人。

turtlebot3开源代码中，实现机器人简单智控的节点是：turtlebot3_gazebo包中的turtlebot3_drive节点。

1. turtlebot3_drive节点启动命令

ros2 run turtlebot3_gazebo turtlebot3_drive

2. 代码解析

目录结构：

turtlebot3_drive节点是一个ROS2的C++客户端，从rcl库中继承而来，其实现包含一个头文件和.cpp实现文件。

头文件：

位置： ~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/include/turtlebot3_gazebo/turtlebot3_drive.hpp

头文件定义如下:

其定义了一个Turtlebot3Drive类，该类从ROS2的C++客户库rclcpp::Node类继承而来。

.cpp实现文件:

位置:

~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/src/turtlebot3_drive.cpp

==>构造函数

构造函数主要完成对象的初始化，包括LiDAR数据初始化，机器人位姿初始化，将要用到的所有话题发布器和订阅器对象的创建，以及用于话题发布的定时器的创建等。

订阅器的创建：

LiDAR传感器扫描周边环境、收集数据，并发布到话题/scan中，差速轮传感器发布测量信息到/odom话题。这两个传感器的实现由gazebo中的三方插件（libgazebo_ros_diff_drive.so，libgazebo_ros_joint_ray_sensor.so）提供，在机器人的仿真定义文件（model.sdf）中定义，参见“部署机器人实体节点：”小节。

发布器的创建：

更新定时器创建：

==>接收LiDAR数据的回调函数：

由此可见，机器人只检测其正前方和左右前方30度角的障碍物，具有一定局限性。

==>接收差速轮的测量数据回调函数：

机器人位姿信息也只保存了，偏航角yaw。

==>更新定时器超时回调函数：

该回调函数中实现了一个简单的状态机转移FSM程序，其中实现了4个状态：GET_TB3_DIRECTION，TB3_DRIVE_FORWARD，TB3_RIGHT_TURN，TB3_LEFT_TURN。其中GET_TB3_DIRECTION是基础规划状态，其他3个状态为执行状态。由此可知，每个机器人运动命令的发送需要两次定时器超时来完成的，共计20ms。

更新定时器超时2次，就会触发对机器人的操作命令发送，即向/cmd_vel话题写命令，写命令话题的代码如下：

3. 结果展示

智控机器人的工作情况如下图所示，其能自动检测周边环境的障碍物，并作出规避碰撞的运动规划。

ros2-20221124智控机器人

该智控机器人部署后节点/话题图如下图所示，与遥控机器人不同的是话题/cmd_vel由turtlebot3_drive_node节点来发布，而不是teleop_keyboard节点。