承接上一篇博客：
ROS机器人入门：机器人系统仿真【学习记录】——2-CSDN博客
以及第一篇博客：
ROS机器人入门：机器人系统仿真【学习记录】——1-CSDN博客
我们先前结束了（之前的博客中）：
1. 概述
2. URDF集成Rviz基本流程
3. URDF语法详解
4. URDF优化_xacro
5. Rviz中控制机器人模型运动
6. URDF集成Gazebo
下面让我们继续学习ROS机器人系统仿真！
让我们接着新的开始：
7. URDF、Gazebo与Rviz综合应用
关于URDF(Xacro)、Rviz 和 Gazebo 三者的关系，前面已有阐述: URDF 用于创建机器人模型、Rviz 可以显示机器人感知到的环境信息，Gazebo 用于仿真，可以模拟外界环境，以及机器人的一些传感器，如何在 Gazebo 中运行这些传感器，并显示这些传感器的数据(机器人的视角)呢？本节主要介绍的重点就是将三者结合:通过 Gazebo 模拟机器人的传感器，然后在 Rviz 中显示这些传感器感知到的数据。主要内容包括:
-
运动控制以及里程计信息显示
-
雷达信息仿真以及显示
-
摄像头信息仿真以及显示
-
kinect 信息仿真以及显示
7.1 机器人运动控制以及里程计信息显示
gazebo 中已经可以正常显示机器人模型了，那么如何像在 rviz 中一样控制机器人运动呢？在此，需要涉及到ros中的组件: ros_control。
1.ros_control 简介
场景:同一套 ROS 程序，如何部署在不同的机器人系统上，比如：开发阶段为了提高效率是在仿真平台上测试的，部署时又有不同的实体机器人平台，不同平台的实现是有差异的，如何保证 ROS 程序的可移植性？ROS 内置的解决方式是 ros_control。
ros_control:是一组软件包，它包含了控制器接口，控制器管理器，传输和硬件接口。ros_control 是一套机器人控制的中间件，是一套规范，不同的机器人平台只要按照这套规范实现，那么就可以保证 与ROS 程序兼容，通过这套规范，实现了一种可插拔的架构设计，大大提高了程序设计的效率与灵活性。
gazebo 已经实现了 ros_control 的相关接口，如果需要在 gazebo 中控制机器人运动，直接调用相关接口即可
2.运动控制实现流程(Gazebo)
承上，运动控制基本流程:
-
已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加传动装置以及控制器
-
将此文件集成进xacro文件
-
启动 Gazebo 并发布 /cmd_vel 消息控制机器人运动
2.1 为 joint 添加传动装置以及控制器
两轮差速配置
首先在car.xacro文件中添加如下代码段：
<!-- 运动控制 -->
<xacro:include filename="gazebo/move.xacro" />包含: 控制器以及传动配置的 xacro 文件
其次，launch文件沿用demo03_env.launch文件即可
下面继续在urdf文件夹中新建一个gazebo文件夹，在gazebo文件夹中创建一个move.launch文件，内容如下：
<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 传动实现:用于连接控制器与关节 -->
<xacro:macro name="joint_trans" params="joint_name">
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${joint_name}_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${joint_name}">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${joint_name}_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<!-- 每一个驱动轮都需要配置传动装置 -->
<xacro:joint_trans joint_name="left_wheel2base_link" />
<xacro:joint_trans joint_name="right_wheel2base_link" />
<!-- 控制器 -->
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<rosDebugLevel>Debug</rosDebugLevel>
<publishWheelTF>true</publishWheelTF>
<robotNamespace>/</robotNamespace>
<publishTf>1</publishTf>
<publishWheelJointState>true</publishWheelJointState>
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>left_wheel2base_link</leftJoint> <!-- 左轮 -->
<rightJoint>right_wheel2base_link</rightJoint> <!-- 右轮 -->
<wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 车轮间距 -->
<wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 -->
<broadcastTF>1</broadcastTF>
<wheelTorque>30</wheelTorque>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic> <!-- 运动控制话题 -->
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic> <!-- 里程计话题 -->
<robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐标系 -->
</plugin>
</gazebo>
</robot>下面source一下：
source ./devel/setup.bash
继续运行launch文件：
roslaunch urdf02_gazebo demo03_env.launch
可以发现如下显示：
再打开一个终端，查看一下ros的话题：
rostopic list
可以发现：
查看话题列表，会发现多了 /cmd_vel 然后发布 cmd_vel 消息控制即可使用命令控制(或者可以编写单独的节点控制)
下面我们试试利用键盘控制机器人运动，继续输入终端指令：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
遇到如下报错：
解决方法：
sudo apt-get install ros-noetic-teleop-twist-keyboard
再次执行：
conda activate hjx
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
发现终端出现如下界面：
按一下键盘中的 "I" 按键，发现小车向前运动：
想要将小车的运动停止，按 "K" 键即可。好啦~ 小车在 Gazebo 中已经正常运行起来了！
如果想要设置小车在gazebo中运动的线速度与角速度，可以运行如下指令：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.3 _turn:=0.5
可以发现，终端界面中的线速度与角速度变成了我们自定义的参数：
3.Rviz查看里程计信息
在 Gazebo 的仿真环境中，机器人的里程计信息以及运动朝向等信息是无法获取的，可以通过 Rviz 显示机器人的里程计信息以及运动朝向。
里程计: 机器人相对出发点坐标系的位姿状态(X 坐标 Y 坐标 Z坐标以及朝向)。
3.1启动 Rviz
在launch文件夹中新建一个demo04_sensor.launch文件：
demo04_sensor.launch 文件中的代码如下：
<launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" />
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" />
</launch>3.2 添加组件
执行 launch 文件后（前提），在 Rviz 中添加图示组件:
新开一个终端，source一下：source ./devel/setup.bash ，接着运行：
roslaunch urdf02_gazebo demo04_sensor.launch
可以发现：
下面我们查看里程计的可视化，将rviz中的Fixed Frame 设置为 odom , 在Add添加Odometry:
接着在Odometry中的Topic中设置为/odom，取消Covariance的"√"：
接着开启键盘控制节点，开启一个终端运行如下指令：
rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.3 _turn:=0.5
利用键盘控制小车在gazebo中运动，可以发现同时在rviz中可以实时地图形化显示odom：
7.2 雷达信息仿真以及显示
通过 Gazebo 模拟激光雷达传感器，并在 Rviz 中显示激光数据。
实现流程:
雷达仿真基本流程:
-
已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加雷达配置；
-
将此文件集成进xacro文件；
-
启动 Gazebo，使用 Rviz 显示雷达信息。
1.Gazebo 仿真雷达
1.1 新建 Xacro 文件，配置雷达传感器信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 雷达 -->
<gazebo reference="laser">
<sensor type="ray" name="rplidar">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>5.5</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-3</min_angle>
<max_angle>3</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>laser</frameName>
</plugin>
</sensor>
</gazebo>
</robot>在urdf文件夹中的gazebo文件夹中新建一个laser.xacro文件：
1.2 xacro 文件集成
将步骤1的 Xacro 文件集成进总的机器人模型文件，代码示例如下:
在car.xacro文件中添加：
<!-- laser -->
<xacro:include filename="gazebo/laser.xacro" />
1.3启动仿真环境
编写launch文件，启动gazebo，沿用之前的文件：
新开一个终端，source一下：source ./devel/setup.bash ，接着运行：
roslaunch urdf02_gazebo demo03_env.launch
发现遇到了报错：SpawnModel: Failure - model name mycar already exist.
解决方案：Failure - model name mrobot already exist._spawnmodel: failure - model name sr7e already exis-CSDN博客
重新运行一下launch文件，在gazebo中把小车模型删除，关闭，在重新运行launch文件就好了
正常的显示界面如下：
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
再新开一个窗口，source一下后执行：
roslaunch urdf02_gazebo demo04_sensor.launch
将rviz中的Fixed Frame 设置为 odom , 在Add添加LaserScan：
再设置LaserScan中的Topic为/sacn，将Size(m)设置为0.05，可以发现：
这些雷达传感器在rviz中所汇聚成的点，对应了机器人在gazebo仿真环境中的场景
7.3 摄像头信息仿真以及显示
通过 Gazebo 模拟摄像头传感器，并在 Rviz 中显示摄像头数据。
实现流程:
摄像头仿真基本流程:
-
已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加摄像头配置；
-
将此文件集成进xacro文件；
-
启动 Gazebo，使用 Rviz 显示摄像头信息。
1.Gazebo 仿真摄像头
1.1 新建 Xacro 文件，配置摄像头传感器信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<!-- 被引用的link -->
<gazebo reference="camera">
<!-- 类型设置为 camara -->
<sensor type="camera" name="camera_node">
<update_rate>30.0</update_rate> <!-- 更新频率 -->
<!-- 摄像头基本信息设置 -->
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<!-- 核心插件 -->
<plugin name="gazebo_camera" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>/camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>在urdf文件夹中的gazebo文件夹中新建一个camera.xacro文件：
1.2 xacro 文件集成
将步骤1的 Xacro 文件集成进总的机器人模型文件，代码示例如下:
在car.xacro文件中添加：
<!-- camera -->
<xacro:include filename="gazebo/camera.xacro" />
1.3启动仿真环境
编写launch文件，启动gazebo，沿用之前的文件：
新开一个终端，source一下：source ./devel/setup.bash ，接着运行：
roslaunch urdf02_gazebo demo03_env.launch
有遇到报错的话，需要重新运行一下launch文件，在gazebo中把小车模型删除，关闭，在重新运行launch文件，正常的终端提示如下:
再新开一个窗口，source一下后执行：
roslaunch urdf02_gazebo demo04_sensor.launch
将rviz中的Fixed Frame 设置为 odom , 在Add添加Camera：
再设置Camera中的Topic为/camera/image_raw，可以发现：
在rviz中可以观察到小车机器人在gazebo仿真环境中的相机视野（图片数据）
接着可以在gazebo仿真中运动小车，新开一个终端，执行指令（需要Tab补齐）：
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
设置angular中的z为0.5后回车执行，发现小车实时转动的的视角图片展现了出来：
7.4 kinect信息仿真以及显示
通过 Gazebo 模拟kinect摄像头，并在 Rviz 中显示kinect摄像头数据。
实现流程:
kinect摄像头仿真基本流程:
-
已经创建完毕的机器人模型，编写一个单独的 xacro 文件，为机器人模型添加kinect摄像头配置；
-
将此文件集成进xacro文件；
-
启动 Gazebo，使用 Rviz 显示kinect摄像头信息。
1.Gazebo仿真Kinect
1.1 新建 Xacro 文件，配置 kinetic传感器信息
<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro">
<gazebo reference="support">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${60.0*PI/180.0}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>support</frameName>
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
</plugin>
</sensor>
</gazebo>
</robot>在urdf文件夹中的gazebo文件夹中新建一个kinect.xacro文件：
1.2 xacro 文件集成
将步骤1的 Xacro 文件集成进总的机器人模型文件，代码示例如下:
<!-- kinect -->
<xacro:include filename="gazebo/kinect.xacro" />
1.3启动仿真环境
编写launch文件，启动gazebo，沿用之前的文件：
新开一个终端，source一下：source ./devel/setup.bash ，接着运行：
roslaunch urdf02_gazebo demo03_env.launch
再新开一个窗口，source一下后执行：
roslaunch urdf02_gazebo demo04_sensor.launch
将rviz中的Fixed Frame 设置为 odom , 在Add添加Camera. 再设置Camera中的Topic为/camera/depth/image_raw，接着可以在gazebo仿真中运动小车，新开一个终端，执行指令（需要Tab补齐）：
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
设置angular中的z为0.5后回车执行，发现小车实时转动的的视角中的深度信息展现了出来：
补充:kinect 点云数据显示
在kinect中也可以以点云的方式显示感知周围环境，在 rviz 中操作如下:
新开一个终端，source一下：source ./devel/setup.bash ，接着运行：
roslaunch urdf02_gazebo demo03_env.launch
再新开一个窗口，source一下后执行：
roslaunch urdf02_gazebo demo04_sensor.launch
将rviz中的Fixed Frame 设置为 odom , 在Add添加PointCloud2：
再设置PointCloud2中的Topic为/camera/depth/points，可以发现:
问题:在rviz中显示时错位。
原因:在kinect中图像数据与点云数据使用了两套坐标系统，且两套坐标系统位姿并不一致。
解决:
1.在插件中为kinect设置坐标系，修改配置文件的<frameName>标签内容：
<frameName>support_depth</frameName>
2.发布新设置的坐标系到kinect连杆的坐标变换关系，在启动rviz的launch中，添加:
<node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />
3.启动rviz，重新显示，发现问题得到了解决：
本博客主要介绍了ROS中仿真实现涉及的三大知识点:
- URDF(Xacro)
- Rviz
- Gazebo
URDF 是用于描述机器人模型的 xml 文件，可以使用不同的标签具代表不同含义，URDF 编写机器人模型代码冗余，xacro 可以优化 URDF 实现，代码实现更为精简、高效、易读。容易混淆的是Rviz与Gazebo，在此我们着重比较以下二者的区别:
rviz是三维可视化工具，强调把已有的数据可视化显示；
gazebo是三维物理仿真平台，强调的是创建一个虚拟的仿真环境。
rviz需要已有数据。
rviz提供了很多插件，这些插件可以显示图像、模型、路径等信息，但是前提都是这些数据已经以话题、参数的形式发布，rviz做的事情就是订阅这些数据，并完成可视化的渲染，让开发者更容易理解数据的意义。
gazebo不是显示工具，强调的是仿真，它不需要数据，而是创造数据。
我们可以在gazebo中免费创建一个机器人世界，不仅可以仿真机器人的运动功能，还可以仿真机器人的传感器数据。而这些数据就可以放到rviz中显示，所以使用gazebo的时候，经常也会和rviz配合使用。当我们手上没有机器人硬件或实验环境难以搭建时，仿真往往是非常有用的利器。
综上，如果你手上已经有机器人硬件平台，并且在上边可以完成需要的功能，用rviz应该就可以满足开发需求。
如果你手上没有机器人硬件，或者想在仿真环境中做一些算法、应用的测试，gazebo+rviz应该是你需要的。
另外，rviz配合其他功能包也可以建立一个简单的仿真环境，比如rviz+ArbotiX。
参考资料：
560
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
