【仿真】二、机器人SLAM

二、机器人SLAM

ROS机器人开发实践:链接: 源码链接

Gazebo仿真简介

<gazebo>标签是URDF模型中描述gazebo仿真时所需要的扩展属性。我们需要为每个必要的<link>、<joint>、<robot>设置<gazebo>标签。

1. 为link添加<gazebo>标签

针对机器人模型，需要对每一个link添加<gazebo>标签，包含的属性仅有material。material属性的作用与link里<visual>中material属性的作用相同，Gazebo无法通过<visual>中的material参数设置外观颜色，所以需要单独设置，否则默认情况下Gazebo中显示的模型全是灰白色。

link设置的代码如下，Gazebo/Black是Gazebo自带的属性

<link name="wheel_${lr}_link">
...
</link>

<gazebo reference="wheel_${lr}_link">
    <material>Gazebo/Black</material>
</gazebo>

2. 添加传动装置

我们的机器人模型是一个两轮差速驱动的机器人，通过调节两个轮子的速度比例，完成前进、转向、倒退等动作。火车跑得快，全靠车头带，在之前的模型中，并没有加入驱动机器人运动的动力源，这当然是仿真必不可少的部分。

传动系统（Transmission）可以将机器人的关节指令转换成执行器的控制信号。机器人每个需要运动的关节都需要配置相应的传动系统，可以在机器人的URDF模型文件中按照以下方法配置：

在Gazebo仿真环境中，<transmission> 元素是用于定义机器人模型中如何将动力从驱动器（如电机）传递到关节（如机器人轮子上的关节）的一种配置方式。这是ROS控制系统和Gazebo仿真之间的一个重要链接，特别是当使用 ros_control 插件时。<transmission> 的配置信息告诉 ros_control 如何在模拟环境中模拟各种硬件接口。代码如下：

<joint name="base_to_wheel_${lr}_joint" type="continuous">
...
</joint>

<transmission name="wheel_${lr}_joint_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_to_wheel_${lr}_joint" />
    <actuator name="wheel_${lr}_joint_motor">
        <hardwareInterface>VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
</transmission>

以上代码中，<joint name="">定义了将要绑定驱动器的joint，<type>标签声明了所使用的传动装置类型，<hardwareInterface>定义了硬件接口的类型，这里使用的是速度控制接口。

actuator name="wheel_${lr}_joint_motor"：定义驱动该关节的执行器（电机）的名称。mechanicalReduction：机械减速比设置为1，意味着电机输出与关节运动之间没有减速或增速，输出1:1传递。

• 上一章提到的rviz+ArbotiX也实现了控制小车运动，并没有使用到ros_control插件。ArbotiX 是一个常用于控制机器人的硬件和软件框架，在许多简单或资源有限的应用场景中进行机器人控制是完全足够的。

3. 添加Gazebo控制器插件

Gazebo插件可以根据插件的作用范围应用到URDF模型的<robot>、<link>、<joint>上，需要使用<gazebo>标签作为封装。

• 为<robot>元素添加插件

与其他的<gazebo>元素相同，如果<gazebo>元素中没有设置reference="x"属性，则默认应用于<robot>标签。

<gazebo>
    <plugin name="unique_name" filename="plugin_name.so">
        ... plugin parameters ...
    </plugin>
</gazebo>

name="unique_name"：指定插件的唯一名称，这个名称在同一个仿真中需要保持唯一。
filename="plugin_name.so"：指定插件的文件名，这个文件包含了插件的实现代码，通常是一个编译好的共享库（.so文件）

• 为<link>、<joint>标签添加插件

如果需要为<link>、<joint>标签添加插件，则需要设置<gazebo>标签中的reference="x"属性：

<gazebo reference="your_link_name">
    <plugin name=" unique_name " filename="plugin_name.so">
        ... plugin parameters ...
    </plugin>
</gazebo>

Gazebo已经提供了一个用于差速控制的插件libgazebo_ros_diff_drive.so，可以将其应用到现有的机器人模型上。给urdf文件加入如下插件声明：

<!-- 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>base_to_wheel_left_joint</leftJoint>
        <rightJoint>base_to_wheel_right_joint</rightJoint>
        <wheelSeparation>${base_link_radius*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>

在加载差速控制器插件的过程中，需要配置一系列参数，其中比较关键的参数如下。
<robotNamespace>：机器人的命名空间，插件所有数据的发布、订阅都在该命名空间下。
<leftJoint>和<rightJoint>：左右轮转动的关节joint，控制器插件最终需要控制这两个joint转动。
<wheelSeparation>和<wheelDiameter>：这是机器人模型的相关尺寸，在计算差速参数时需要用到。
<wheelAcceleration>：车轮转动的加速度。
<commandTopic>：控制器订阅的速度控制指令，ROS中一般都命名为cmd_vel，生成全局命名时需要结合<robotNamespace>中设置的命名空间。
<odometryFrame>：里程计数据的参考坐标系，ROS中一般都命名为odom。

具体流程分析如下：
在Gazebo仿真中，（1）首先加载和配置URDF模型。之后，
（2）加载Gazebo仿真插件，加载differential_drive_controller差速驱动控制器插件，它接收速度命令 (/cmd_vel)，设置插件的各种参数，并据此控制机器人左右两轮的速度，实现前进、后退和转向。

<rosDebugLevel>Debug</rosDebugLevel>设置ROS的调试级别为 “Debug”，这样可以输出更详细的调试信息，帮助开发者进行问题诊断。
<publishWheelTF>true</publishWheelTF>启用发布轮子的变换信息（TF）。
<robotNamespace>/</robotNamespace>设置机器人在ROS话题和服务中使用的命名空间，这里设置为根命名空间 “/”。
<publishTf>1</publishTf>启用发布机器人的TF信息。
<publishWheelJointState>true</publishWheelJointState>启用发布轮子关节的状态信息。
<alwaysOn>true</alwaysOn>确保控制器始终处于启动状态，不会因为缺乏命令或其他条件而关闭。
<updateRate>100.0</updateRate>设置控制器的更新频率为每秒100次，这定义了控制器处理输入和更新状态的频率。
<legacyMode>true</legacyMode>启用兼容模式，这通常用于确保控制器兼容旧版的ROS或Gazebo API。
<leftJoint>base_to_wheel_left_joint</leftJoint>
<rightJoint>base_to_wheel_right_joint</rightJoint>定义控制器应该控制的左右轮子的关节名称，确保控制器正确地连接到机器人的相应部分，即transmission传动系统。
<wheelSeparation>${base_link_radius*2}</wheelSeparation>设置两个轮子之间的距离。
<wheelDiameter>${2*wheel_radius}</wheelDiameter>设置轮子的直径，轮子半径的两倍。
<broadcastTF>1</broadcastTF>指示控制器发布TF变换信息。
<wheelTorque>30</wheelTorque>设置轮子的最大扭矩值。
<wheelAcceleration>1.8</wheelAcceleration>设置轮子的加速度。
<commandTopic>cmd_vel</commandTopic>定义控制器应该订阅的命令话题，用于接收速度控制命令。
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic> 定义里程计信息发布使用的坐标帧和话题名称。
<robotBaseFrame>base_footprint</robotBaseFrame>定义机器人的基础坐标帧。

之后，（3）启动仿真并进行控制，在启动Gazebo仿真环境时，所有在URDF模型文件中定义的插件都会被加载和初始化。控制器插件订阅来自ROS的 /cmd_vel 话题，此话题携带机器人的速度控制命令。之后，控制器插件根据接收到的速度指令调节机器人的实际行动，比如调整电机输出以改变轮速，从而使机器人在仿真环境中移动。
（4）控制器插件还可能负责发布机器人的状态信息，如里程计信息 (odom)，这些信息可以被导航和定位系统使用来估计机器人在环境中的位置。

Gazebo仿真1

launch文件

<launch>
    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mrobot_gazebo)/worlds/playground.world"/>
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
    <arg name="headless" default="false"/>
    <arg name="debug" default="false"/>

    <!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world_name)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="gui" value="$(arg gui)" />
        <arg name="paused" value="$(arg paused)"/>
        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
        <arg name="headless" value="$(arg headless)"/>
    </include>

    <!-- 加载机器人模型描述参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_gazebo)/urdf/mrobot.urdf.xacro'" /> 

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node> 

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"  output="screen" >
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
          args="-urdf -model mrobot -param robot_description"/> 

</launch>

以上launch文件主要做了两项工作：1）启动机器人的状态发布节点，同时加载带有Gazebo属性的机器人URDF模型。2）启动Gazebo，并且将机器人模型加载到Gazebo仿真环境中。

<include file="$(find gazebo_ros)/launch/empty_world.launch">用于包含一个预定义的launch文件,是Gazebo的空白世界启动文件。world_name载入指定世界文件，gui启动Gazebo的图形用户界面（GUI），paused指定Gazebo仿真是否应在启动时暂停，use_sim_time是否应使用仿真时间，headless指定Gazebo是否应该在无头模式下运行，即没有图形界面。

启动launch文件：roslaunch mrobot_gazebo view_mrobot_gazebo.launch
运动：roslaunch mrobot_teleop mrobot_teleop.launch

Gazebo仿真2

摄像头仿真

在之前rviz+ArbotiX搭建的机器人仿真环境中，机器人装配了多种传感器模型，但是这些模型并无法获取任何环境数据。Gazebo的强大之处还在于提供了一系列传感器插件，可以帮助我们仿真传感器数据，获取Gazebo虚拟环境中的传感信息

<gazebo reference="${prefix}_link">
    <material>Gazebo/Black</material>
</gazebo>

<gazebo reference="${prefix}_link">
    <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_link</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>

新的摄像头模型文件在模型描述部分没有变化，只需要加入两个标签。第一个标签用来设置摄像头模型在Gazebo中的material，与机器人模型的配置相似，只需要设置颜色参数。
重点是第二个设置摄像头插件的标签。在加载传感器插件时，需要使用标签来包含传感器的各种属性。例如现在使用的是摄像头传感器，需要设置type为camera，传感器的命名（name）可以自由设置；然后使用标签具体描述摄像头的参数，包括分辨率、编码格式、图像范围、噪音参数等；最后需要使用标签加载摄像头的插件libgazebo_ros_camera.so，同时设置插件的参数，包括命名空间、发布图像的话题、参考坐标系等。

gmapping功能包建图

安装：sudo apt-get install ros-noetic-gmapping 注意ros版本

# gampping_demo.launch
<launch>
    <include file="$(find mrobot_navigation)/launch/gmapping.launch"/>

    <!-- 启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/gmapping.rviz"/>

</launch>

1）启动之前创建的gmapping节点。2）启动rviz界面，查看传感器和地图构建的实时信息。节点参数的配置如下：

# gmapping.launch
<launch>
    <arg name="scan_topic" default="scan" />

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <param name="sigma" value="0.05"/>
        <param name="kernelSize" value="1"/>
        <param name="lstep" value="0.05"/>
        <param name="astep" value="0.05"/>
        <param name="iterations" value="5"/>
        <param name="lsigma" value="0.075"/>
        <param name="ogain" value="3.0"/>
        <param name="lskip" value="0"/>
        <param name="srr" value="0.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.0"/>
        <param name="delta" value="0.05"/>
        <param name="llsamplerange" value="0.01"/>
        <param name="llsamplestep" value="0.01"/>
        <param name="lasamplerange" value="0.005"/>
        <param name="lasamplestep" value="0.005"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

我们需要重点检查两个参数的输入设配置。1）里程计坐标系的设置，odom_frame参数需要和机器人本身的里程计坐标系一致。2）激光雷达的话题名，gmapping节点订阅的激光雷达话题名是“/scan”，如果与机器人发布的激光雷达话题名不一致，需要使用进行重映射

启动：
在终端中使用如下命令启动Gazebo仿真环境和gmapping节点。
roslaunch mrobot_gazebo mrobot_laser_nav_gazebo.launch
roslaunch mrobot_navigation gmapping.launch
现在启动键盘控制节点，让机器人动起来：
roslaunch mrobot_teleop mrobot_teleop.launch

可以看到机器人在rviz中同步显示，周围的红点（颜色可设置）是激光雷达传感器实时检测到的二维环境深度信息，并且根据当前的深度信息建立了部分已知环境的地图，呈现浅灰色。