松灵机器人Scout mini小车仿真指南

之前介绍了如何通过CAN TO USB串口实现用键盘控制小车移动。但是一直用小车测试缺乏安全性。而松灵官方贴心的为我们准备了gazebo仿真环境，提供了完整的仿真支持库，本文将介绍如何上手使用仿真。

官方仓库地址：https://github.com/agilexrobotics/ugv_gazebo_sim

1.仿真环境准备

整个功能包介绍

├── scout_control ：该文件夹是小车仿真控制器功能包
├── scout_description ：该文件夹是模型文件功能包
└── scout_gazebo_sim ：该文件夹是gazebo模拟功能包

安装相关功能包依赖

所采用的开发环境是Ubuntu18.04+ROS Melodic

下载所需要的功能包

sudo apt-get install ros-melodic-ros-control
sudo apt-get install ros-melodic-ros-controllers
sudo apt-get install ros-melodic-gazebo-ros
sudo apt-get install ros-melodic-gazebo-ros-control
sudo apt-get install ros-melodic-joint-state-publisher-gui 
sudo apt-get install ros-melodic-teleop-twist-keyboard 

新建工作空间并初始化

打开一个新终端，创建一个名为scout_ws的工作区，并初始化:

mkdir -p scout_ws/src
cd scout_ws/src
catkin_init_workspace

下载对应的仿真模型功能包。本文采用的是scout mini。需要将https://github.com/agilexrobotics/ugv_gazebo_sim/tree/master/scout下的三个文件复制到src目录中

git clone https://github.com/agilexrobotics/ugv_gazebo_sim/tree/master/scout

正常编译

cd ..
rosdep install --from-paths src --ignore-src -r -y
caktin_make

2.仿真小车验证

进入scout_ws工作空间，运行系列车仿真模型，display_xxxx.launch 为不同车型在rviz仿真展示。scout_xxxx.launch为不同车型在gazebo模拟环境中展示。

source devel/setup.bash
roslaunch scout_description display_scout_mini.launch 

source devel/setup.bash
roslaunch scout_gazebo_sim scout_empty_world.launch

如果想使用playgen地图，需要提取补全gazebo里面的models文件，不然会一直黑屏

3.添加自定义传感器

官方给定的只有小车底座，没有现成的传感器。在实际使用过程中，可以自定义一系列传感器。
scout系列车型的urdf描述文件位于scout_description/urdf中，在使用过程中，并未直接使用urdf进行描述，而是从xacro(xml xacro)描述文件中生成urdf信息。便于我们像编程一样复用urdf节点以及相关拆分组件等功能。

如果不了解xacro 可以参考xacro语法说明: http://wiki.ros.org/xarc

自定义简易摄像头

首先在scout_description/urdf中添加一个名为universal_sensor_adder.xacro的文件，其中保存内容如下:

<?xml version="1.0"?>
<!-- 
Author: AnthonySuen
Date: 2020-4-8
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="universal_sensor_adder">
    <!-- 用于生成一个新的传感器节点, 
    x_offset y_offset z_offset r p y对应于传感器和 base_link 的相对位置
    sensor_config 为传感器的配置信息
    sensor_plugin_config 为 gazebo 相关插件的配置内容 -->
    <xacro:macro name="add_sensor" params="sensor_name type x_offset y_offset z_offset r p y **sensor_config **sensor_plugin_config">
        <link name="sensor_${sensor_name}">
            <!-- 配置传感器视觉信息 -->
            <visual>
                <geometry>
                    <box size="0.03 0.05 0.05"/>
                </geometry>
                <material name="red">
                    <color rgba="1.0 0.0 0.0 1.0"/>
                </material>
            </visual>
            <!-- 配置碰撞体积, 为gazebo仿真使用 -->
            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <box size="0.03 0.05 0.05"/>
                </geometry>
            </collision>
            <!-- 配置惯性矩阵, 用于 gazebo 仿真,这里我假设传感器很轻很小 -->
            <inertial>
                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <mass value="1e-5"/>
                <inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
            </inertial>
        </link>
        <!-- 配置关节信息, 这里我默认传感器是固定在车上的 -->
        <joint name="sensor_${sensor_name}_joint" type="fixed">
            <parent link="base_link"/>
            <child link="sensor_${sensor_name}"/>
            <origin xyz="${x_offset} ${y_offset} ${z_offset}" rpy="${r} ${p} ${y}"/>
        </joint>
        <!-- gazebo 配置仿真配置部分 -->
        <gazebo reference="sensor_${sensor_name}">            <!-- 此部分要与 传感器 link 名称保持一致 -->
            <sensor name="${sensor_name}" type="${type}">
                <!-- 这里会插入传感器自身配置信息和相关插件的配置内容 -->
                <xacro:insert_block name="sensor_config"/>
                <xacro:insert_block name="sensor_plugin_config"/>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>

在使用过程中删除中文注释，xacro无法解析非ascii 字符！

编辑empty.urdf文件，修改内容如下：

<?xml version="1.0"?>
<!-- 
Author: AnthonySuen
Date: 2020-4-8
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sensors">

  <!-- 加载我们之前编写的文件，之后可以使用其中相关内容 -->
  <xacro:include filename="$(find scout_description)/urdf/universal_sensor_adder.xacro" />
  <!-- 使用我们编写的函数，填入相关参数 -->
  <xacro:add_sensor sensor_name="camera" type="camera" x_offset="0.1" y_offset="0.0" z_offset="0.085" r="0.0" p="0.0" y="0.0">
  <!-- 这部分会替换 <xacro:insert_block name="sensor_config"/> 中的内容 -->
    <sensor_config>
      <update_rate>30</update_rate>
      <camera name="general_camera">
        <image width="640" height="480" hfov="1.5708" format="RGB8" near="0.01" far="50.0"/>
      </camera>
    </sensor_config>
    <!-- 这部分会替换 <xacro:insert_block name="sensor_plugin_config"/> 中的内容 -->
    <sensor_plugin_config>
      <plugin name="general_camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>36.0</updateRate>
        <cameraName>sensor_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>sensor_camera</frameName> <!-- 需要与 传感器 link 名称保持一致 -->
        <hackBaseline>0.1</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor_plugin_config>
  </xacro:add_sensor>
</robot>

在使用过程中删除中文注释，xacro无法解析非ascii 字符！

之后再运行RIVZ查看修改后的模型

roslaunch scout_description display_mini_models.launch