【ROS】ROS入门笔记-Gazebo仿真与实物测试篇

前面我们学习了ROS的一些基础知识，现在我们来学习ROS环境下的Gazebo仿真。

本文所需的源码下载地址为

https://github.com/guyuehome/guyueclass

〇、写在前面

在第一篇笔记中我使用的是Ubuntu 20和ROS noetic，但为了方便学习和适应网上资料，我们在这里更改为Ubuntu 18和ROS melodic。在切换的过程中请注意以下几点：

1.rosdep问题

ROS melodic的安装有这么一步，会经常报错。

sudo rosdep init
rosdep update

主要原因是ROS服务器在国外，访问经常掉线。在这里我们参考了以下两个解决方案。

https://blog.csdn.net/Kenny_GuanHua/article/details/116845781

http://www.autolabor.com.cn/book/ROSTutorials/chapter1/12-roskai-fa-gong-ju-an-zhuang/124-an-zhuang-ros.html

具体方法如下（不保证长期有效）

网站https://ghproxy.com/支持Github的资源代理，非常好用，我们将用此代理加速rosdep对Github的访问，进而解决rosdep update失败问题。

我们要做的，是在rosdep的若干个脚本文件中，把https://ghproxy.com/添加到相应URL前面即可。

1. 首先修改rosdep下载资源的脚本文件：/usr/lib/python2.7/dist-packages/rosdep2/sources_list.py，把以下行添加到脚本中的download_rosdep_data()函数中，以应用代理服务：

# 打开sources_list.py
sudo gedit /usr/lib/python2.7/dist-packages/rosdep2/sources_list.py

# 在download_rosdep_data()函数中添加这一句
url="https://ghproxy.com/"+url

2. 然后，用同样的方法修改/usr/lib/python2.7/dist-packages/rosdistro/__init__.py里面的DEFAULT_INDEX_URL参数，如下：

# 打开__init__.py
sudo gedit /usr/lib/python2.7/dist-packages/rosdistro/__init__.py

# 将DEFAULT_INDEX_URL的网址前加上https://ghproxy.com/
DEFAULT_INDEX_URL = 'https://ghproxy.com/https://raw.githubusercontent.com/ros/rosdistro/master/index-v4.yaml'

3. 接着，以下4个文件中也使用了raw.githubusercontent.com网址，同样的方法把https://ghproxy.com/添加到网址前：

# 第36行
sudo gedit /usr/lib/python2.7/dist-packages/rosdep2/gbpdistro_support.py

# 第72行
sudo gedit /usr/lib/python2.7/dist-packages/rosdep2/sources_list.py

# 第39行
sudo gedit /usr/lib/python2.7/dist-packages/rosdep2/rep3.py

# 第68行 和 第119行
sudo gedit /usr/lib/python2.7/dist-packages/rosdistro/manifest_provider/github.py

4. 最后，在 /usr/lib/python2.7/dist-packages/rosdep2/gbpdistro_support.py 的第204行添加如下代码：

# 打开gbpdistro_support.py
sudo gedit /usr/lib/python2.7/dist-packages/rosdep2/gbpdistro_support.py

# 在第204行添加如下代码
gbpdistro_url = "https://ghproxy.com/" + gbpdistro_url

之后可正常执行

sudo rosdep init
rosdep update

2.Gazebo闪退问题

请输入如下进行测试

# 这将会打开Gazebo并显示三个简单的形状
roslaunch gazebo_ros shapes_world.launch

如果失败请运行如下，并重启终端。

echo "export SVGA_VGPU10=0" >> ~/.bashrc

---

一、优化物理仿真模型

1.使用xacro文件优化URDF模型

1.1 xacro简介

URDF模型是一种经典的物理模型，但其存在一些问题使其不适合在Gazebo中直接使用。这些问题包括：

• 模型冗长，重复较多
• 参数修改麻烦
• 没有参数计算功能
• …

xacro模型是适合在Gazebo中使用的模型，其特点包括：

• 精简模型代码
  • 创建宏定义
  • 文件包含
• 提供可编程接口
  • 常量
  • 变量
  • 数学计算
  • 条件语句

1.2 xacro的使用

常量定义

<xacro:property name="M_PI" value="3.1415926"/>
<xacro:property name="base_mass"   value="1" /> 
<xacro:property name="base_radius" value="0.20"/>
<xacro:property name="base_length" value="0.16"/>

常量使用与计算

<xacro:macro name="sphere_inertial_matrix" params="m r">
    <inertial>
        <mass value="${m}" />
        <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
            iyy="${2*m*r*r/5}" iyz="0" 
            izz="${2*m*r*r/5}" />
    </inertial>
</xacro:macro>

注意：所有数学运算都会被转换为浮点数计算以保证精度。

宏定义

<!-- Macro for robot caster -->
<xacro:macro name="caster" params="prefix reflect">
    <joint name="${prefix}_caster_joint" type="continuous">
        <origin xyz="${reflect*caster_joint_x} 0 ${-(base_length/2 + caster_radius)}" rpy="0 0 0"/>
        <parent link="base_link"/>
        <child link="${prefix}_caster_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="${prefix}_caster_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="${caster_radius}" />
            </geometry>
            <material name="black" />
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <geometry>
                <sphere radius="${caster_radius}" />
            </geometry>
        </collision>      
        <sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" />
    </link>

    <gazebo reference="${prefix}_caster_link">
        <material>Gazebo/Black</material>
    </gazebo>
</xacro:macro>

宏调用

<caster prefix="front" reflect="-1"/>
<caster prefix="back"  reflect="1"/>

文件包含

<xacro:include filename="$(find mbot_description)/urdf/mbot_base_gazebo.xacro" />

1.3 ros_control功能包

1.3.1 ros_control是什么

• ROS为开发者提供的机器人控制中间件
• 包含一些列控制器接口、传动接口、硬件接口等

应用 功能包 base_controller 控制器插件： - 不直接接触硬件 - 从抽象层请求资源 arm_controller 控制器插件 foo_controller 控制器插件 RobotHW 机器人硬件抽象： - 管理硬件资源 - 处理硬件冲突 机器人 速度控制 位置控制 力控制

ros_contral主要提供了四种控制器（Controllers）

• joint_state_controller 状态控制器
• joint_effort_controller 力控制器
• joint_position_controller 位置控制器
• joint_velocity_controller 速度控制器

2.完善机器人模型的物理仿真

2.1 为link添加惯性参数和碰撞属性

link标签一般包含三部分内容

• visual，可视化
• collision，碰撞检测
• cylinder_inertial_matrix，惯性矩阵

我们看一个例程

<link name="${prefix}_caster_link">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <sphere radius="${caster_radius}" />
        </geometry>
        <material name="black" />
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <sphere radius="${caster_radius}" />
        </geometry>
    </collision>      
    <sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" />
</link>

2.2 为link添加gazebo标签

主要是为了显示颜色。

<gazebo reference="${prefix}_wheel_link">
    <material>Gazebo/Gray</material>
</gazebo>

2.3 为joint添加传动装置

<transmission name="${prefix}_wheel_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="${prefix}_wheel_joint" >
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="${prefix}_wheel_joint_motor">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>

2.4 添加gazebo控制器插件

<!-- controller -->
<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_wheel_joint</leftJoint>
        <rightJoint>right_wheel_joint</rightJoint>
        
        # 机器人模型的相关尺寸
        <wheelSeparation>${wheel_joint_y*2}</wheelSeparation>
        <wheelDiameter>${2*wheel_radius}</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> 

2.5 在gazebo中加载机器人模型

使用launch文件加载模型

<launch>
	<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/mbot_gazebo.xacro'" />
	<arg name="gui" default="true" />

	<param name="robot_description" command="$(arg model)" />

    
	<param name="use_gui" value="$(arg gui)"/>

    
	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

	
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot.rviz" required="true" />

launch>

二.Gazebo仿真

1.下载模型

下载下列模型，并放置到~/.gazebo/models

https://github.com/osrf/gazebo_models

.gazebo目录需要在home目录下按Ctrl+h显示隐藏目录。

如果没有该目录请自行创建。

2 .初次运行Gazebo

准备工作

运行之前请将本章源码，即guyueclass-main/ros_basic/mbot_gazebo内的三个文件夹拷贝到我们的工作空间~/catkin_ws/src中。

工作空间的创建方法请参考本系列上一篇教程。

随后进入工作空间目录~/catkin_ws进行编译。

# 进入工作空间根目录
cd ~/catkin_ws

# 编译工作空间
catkin_make

打开Gazebo

打开一个新的终端

# 打开Gazebo运行空间
roslaunch mbot_gazebo view_mbot_gazebo_empty_world.launch

此时可看到Gazebo环境中出现一辆圆形小车。

控制小车

查看话题列表

rostopic list

可见/cmd_vel速度指令接口。

# 发布速度消息
rostopic pub /cmd_vel geometry_msgs/Twist "linear:
  x: 0.5
  y: 0.0
  z: 0.0
angular:
  x: 0.5
  y: 0.0
  z: 0.0" 

可见小车移动。

# 查看位置信息
rostopic echo /odom

此时可见终端打印小车位置信息。

3.在Gazebo中创建模型

Insert 直接添加

在Gazebo左侧栏中找到Insert，可直接添加已有的模型。

在File中的Save world as可以将当前环境保存下来。

Building Editor 绘制

点击Edit中的Building Editor，可自行绘制环境。

4.物理环境运动仿真

打开环境

# 打开Gazebo运行空间
roslaunch mbot_gazebo view_mbot_gazebo_play_ground.launch

此时可见有障碍物的环境。

键盘控制

# 启动键盘控制脚本
roslaunch mbot_teleop mbot_teleop.launch

此时可用键盘控制移动。

5.摄像头仿真

打开环境

# 打开Gazebo运行空间
roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch

查看话题列表

rostopic list

可见/camera摄像头接口。

使用rqt_image查看画面，选择自己想看的topic

rqt_image

6.深度相机仿真

打开环境

# 打开Gazebo运行空间
roslaunch mbot_gazebo view_mbot_with_kinect_gazebo.launch

查看话题列表

rostopic list

可见/kinect摄像头接口。

使用RVIZ查看画面

rosrun rviz rviz

坐标系选择odom

添加Pointcloud2和Robotmodel

7.激光雷达仿真

打开环境

# 打开Gazebo运行空间
roslaunch mbot_gazebo view_mbot_with_laser_gazebo.launch

查看话题列表

rostopic list

可见/scan激光雷达接口。

使用RVIZ查看画面

rosrun rviz rviz

坐标系选择odom

添加laderscan和Robotmodel

8.SLAM仿真

8.1 准备工作

1. 将源码拷贝到工作空间
2. 安装必要功能包

SLAM部分可能需要安装的功能包

# 点云转雷达
sudo apt-get install ros-melodic-depthimage-to-laserscan

cd ~/catkin_ws/src

# 编译所需
git clone https://github.com/ros-planning/navigation_msgs.git

# gmapping
sudo apt-get install ros-melodic-gmapping

cd ~/catkin_ws/src

# hector_slam
git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

# cartographer
sudo apt-get install ros-melodic-cartographer-*

# 保存图片所需
sudo apt-get install ros-melodic-map-server

# rtabmap
sudo apt-get install ros-melodic-rtabmap-ros

3. 编译

cd ~/catkin_ws

catkin_make

4. 打开环境

# 打开Gazebo运行空间
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

8.2 Gmapping SLAM

• 二维SLAM
• 常用、稳定
• 深度、IMU（可选）、里程计

激光雷达

# 激光雷达仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

# Gmapping Demo
roslaunch mbot_navigation gmapping_demo.launch

# 键盘控制脚本
roslaunch mbot_teleop mbot_teleop.launch

使用键盘控制小车，完成建图。

# 保存地图
rosrun map_server map_saver -f cloister_gmapping

生成.pgm和.yaml文件，默认保存位置为home目录。

深度相机

# 激光雷达仿真环境
roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch

# Gmapping Demo
roslaunch mbot_navigation gmapping_demo.launch

# 键盘控制脚本
roslaunch mbot_teleop mbot_teleop.launch

使用键盘控制小车，完成建图。

深度相机效果不好

• 需要较多的特征点
• 容易错位

8.3 Hector SLAM

• 激光雷达
• 不需要里程计
• 激光雷达刷新率要高
• 移动要慢

# 激光雷达仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

# Hector Demo
roslaunch mbot_navigation hector_demo.launch

# 键盘控制脚本
roslaunch mbot_teleop mbot_teleop.launch

要慢慢移动，不然极易漂移

# 保存地图
rosrun map_server map_saver -f hector

8.4 Cartographer SLAM

• 基于图网络
• 可选里程计

# 激光雷达仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

# Cartographer Demo
roslaunch mbot_navigation cartographer_demo.launch

# 键盘控制脚本
roslaunch mbot_teleop mbot_teleop.launch 

运行失败

错误信息：

[cartographer_node-1] process has died [pid 2992, exit code -6, cmd /opt/ros/melodic/lib/cartographer_ros/cartographer_node -configuration_directory /home/work/catkin_ws/src/mbot_navigation/config -configuration_basename lidar.lua scan:=scan __name:=cartographer_node __log:=/home/work/.ros/log/8fcd0372-f5b3-11eb-b110-000c295e8fa7/cartographer_node-1.log].
log file: /home/work/.ros/log/8fcd0372-f5b3-11eb-b110-000c295e8fa7/cartographer_node-1*.log

8.5 RTAB SLAM

• 三维SLAM

# 深度相机仿真环境
roslaunch mbot_gazebo mbot_kinect_nav_gazebo.launch

# Rtabmap Demo
roslaunch mbot_navigation rtabmap_demo.launch

# 键盘控制脚本
roslaunch mbot_teleop mbot_teleop.launch

使用键盘控制小车，完成建图。

查看建图结果。

rtabmap-databaseViewer ~/.ros/rtabmap.db

9.自主导航仿真

本部分代码在/guyueclass-main/planning&perception/mbot_navigation，请复制到工作空间中。

安装move_base导航框架

sudo apt-get install ros-melodic-navigation

配置move_base节点mbot_navigation/launch/move_base.launch

Gmapping建图仿真

# 打开激光雷达仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

# 打开Gmapping仿真环境
roslaunch mbot_navigation nav_cloister_demo.launch

# 运行定点移动程序
rosrun mbot_navigation move_test.py

# 运行随机移动程序
rosrun mbot_navigation exploring_random.py

Gmapping自主建图仿真

# 打开激光雷达仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch

# 运行自主建图仿真环境
roslaunch mbot_navigation exploring_slam_demo.launch

# 运行随机移动程序
rosrun mbot_navigation exploring_random.py