ROS2进阶第二章 -- 使用Gazebo构建机器人仿真平台 -- 控制差速轮式机器人移动

概述：
上一篇博客：ROS2进阶第一章 – 从头开始构建一个可视化的差速轮式机器人模型 – 学习URDF机器人建模与xacro优化，我们使用urdf和xacro搞了一个差速轮式机器人的外形建模，并使用 rviz 可视化查看。

本文是此系列的第二章，我们将使用gazebo仿真环境创建一个世界，并将上一节制作的机器人加载到仿真环境中，在通过键盘控制节点来控制小车移动，并通过rviz实时察看 camera，kinect和lidar三种传感器的仿真效果。

参考资料

1. ROS2入门21讲——学习笔记（二）常用工具部分15-21讲中的第18讲
2. ROS2高效进阶 – ROS2高级组件之gazebo11学习 – 模型库和编辑模型
3. Gazebo官方教程
4. ros2-control介绍
5. 欢迎阅读ros2_control文档！
6. ROS高效进阶第三章 – 以差速轮式机器人为例，使用Gazebo构建机器人仿真平台

1. 绘制 my_house空间模型

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python mbot_gazebo --dependencies urdf xacro gazebo_plugins gazebo_ros gazebo_ros_control geometry_msgs rclcpp rclpy

cd mbot_gazebo 
mkdir -p config launch meshes scripts urdf/sensor worlds rviz

（2）启动gazebo，绘制my_house空间模型，并保存 my_house.sdf文件。绘制模型的过程可以参考：ROS2高效进阶 – ROS2高级组件之gazebo11学习 – 模型库和编辑模型

查看保存好的模型：在模型的保存路径下打开终端，运行下面的命令

gazebo --verbose <world_file>

gazebo --verbose my_house.sdf

下面是我刚刚绘制的世界

2 为机器人模型添加物理属性和差速控制器插件

（1）在 mbot_gazebo 的 urdf 目录内，创建机器人模型文件

cd ~/ros2_ws/src/mbot_gazebo 
touch mbot_base_gazebo.xacro mbot_gazebo.xacro 
touch sensors/camera_gazebo.xacro sensors/kinect_gazebo.xacro sensors/lidar_gazebo.xacro

（2）转动惯量和惯性矩阵：

转动惯量：Moment of Inertia，在物理学中，用于描述物体绕特定轴线旋转时的惯性大小，也就是物体抵抗旋转运动的能力。具体而言，对于某个轴线，离该轴线越远的物体质量对转动惯量的贡献越大。
惯性矩阵：描述物体关于某一点的转动惯量的3x3的对称矩阵。在ROS中，这个矩阵的形式如下：

    | ixx  ixy  ixz |
    | ixy  iyy  iyz |
    | ixz  iyz  izz |

对角线上的元素（ixx, iyy, izz）分别表示物体绕x轴、y轴和z轴的主转动惯量。非对角元素（ixy, ixz, iyz）表示物体关于不同轴之间的耦合作用，也就是说他们表示当物体同时绕多个轴旋转时存在的影响，这些被称为非对角转动惯量。
对于均匀的几何体（如长方体、圆柱体、球体等），它们的主轴通常与物体的对称轴相沿。在这种情况下，如果我们选择这些对称轴作为参考系（坐标系），那么非对角元素（也就是耦合项）将会消失，所以设为0。如果，物体旋转轴并不完全沿着这些主轴，那么就需要计算和考虑非对角元素了。

（3）圆柱体的惯性矩阵：m是物体质量，r是圆柱半径，h是圆柱高度

ixx：圆柱体绕x轴的转动惯量，I = m * (3 * r^2 + h^2) / 12
iyy：圆柱体绕y轴的转动惯量，与绕x轴相同
izz：圆柱体绕z轴的转动惯量，I = m * r^2 / 2
ixy, ixz, iyz均为0,表示非对角转动惯量为0。

（4）球体的惯性矩阵：m是物体质量，r是圆柱半径

ixx/iyy/izz：球体绕x,y,z轴的转动惯量公式：I = 2 * m * r^2 / 5
ixy, ixz, iyz均为0,表示非对角转动惯量为0。

（5）惯性矩阵（inertial matrix）和碰撞参数（collision properties）

在 ROS2进阶第一章 – 从头开始构建一个可视化的差速轮式机器人模型 – 学习URDF机器人建模与xacro优化 中，我们讲了一个刚体 link，有外观和物理两大属性，其中物理属性又包括：惯性矩阵（inertial matrix）和碰撞参数（collision properties）。
以差速轮式机器人主导轮为例，其惯性矩阵为：

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

以差速轮式机器人万向轮为例，其惯性矩阵为：

    <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>

至于刚体的碰撞参数，一般与外观一致，以底盘base_link为例，其外观，惯性矩阵和碰撞参数为：

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>
            <collision>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
            </collision>   
            <xacro:cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
        </link>

（6）刚体link如果要在gazebo上显示，必须使用gazebo标签，可以添加颜色，或者关闭重力：

    <gazebo reference="${prefix}_wheel_link">
        <material>Gazebo/White</material>
    </gazebo>
    // base_footprint是影子link，无重力
    <gazebo reference="base_footprint">
        <turnGravityOff>false</turnGravityOff>
    </gazebo>

（7）ros2_control框架

一个机器人实现运动，无论是自主运动还是被动控制运动，在应用程序与底层执行机构之间，需要一个控制中间件，其能对各种底层硬件，无论是真实的还是仿真的，进行抽象，并提供多种控制器，而这就是ros提供的ros_control。ros_control以插件的形式，提供了多种控制器，本文的差速轮式机器人使用的是差速控制器插件。
ros2_control框架的架构如下图所示：
上半部分是控制者（控制器插件）：不直接接触硬件，从抽象层请求资源
下半部分是机器人的硬件：管理硬件资源，处理硬件冲突

（8）差速控制器

为差速轮式机器人的两个主动轮，添加传动装置，这里添加的是两个速度控制接口。

        <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>

然后为差速轮式机器人添加差速控制器插件:

        <gazebo>
            <plugin name="differential_drive_controller" 
                    filename="libgazebo_ros_diff_drive.so">                
                  <update_rate>30</update_rate>
                  <left_joint>left_wheel_joint</left_joint>
                  <right_joint>right_wheel_joint</right_joint>
                  <wheel_separation>${wheel_joint_y*2}</wheel_separation>
                  <wheel_diameter>${2*wheel_radius}</wheel_diameter>
                  <max_wheel_torque>20</max_wheel_torque>
                  <max_wheel_acceleration>1.0</max_wheel_acceleration>
                  <command_topic>cmd_vel</command_topic>
                  <publish_odom>true</publish_odom>
                  <publish_odom_tf>true</publish_odom_tf>
                  <publish_wheel_tf>true</publish_wheel_tf>
                  <odometry_topic>odom</odometry_topic>
                  <odometry_frame>odom</odometry_frame>
                  <robot_base_frame>base_footprint</robot_base_frame>
                  <odometry_source>1</odometry_source>
            </plugin>
        </gazebo> 

（9）完整内容如下：mbot_base_gazebo.xacro

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <!-- PROPERTY LIST -->
    <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:property name="pillar_length" value="0.22"/>
    <xacro:property name="pillar_radius" value="0.03"/>

    <xacro:property name="wheel_mass"   value="0.2" />
    <xacro:property name="wheel_radius" value="0.06"/>
    <xacro:property name="wheel_length" value="0.025"/>
    <xacro:property name="wheel_joint_y" value="0.19"/>
    <xacro:property name="wheel_joint_z" value="0.05"/>

    <xacro:property name="caster_mass"    value="0.2" /> 
    <xacro:property name="caster_radius"  value="0.015"/> <!-- wheel_radius - ( base_length/2 - wheel_joint_z) -->
    <xacro:property name="caster_joint_x" value="0.18"/>

    <!-- Defining the colors used in this robot -->
    <material name="yellow">
        <color rgba="1 0.4 0 1"/>
    </material>
    <material name="black">
        <color rgba="0 0 0 0.95"/>
    </material>
    <material name="gray">
        <color rgba="0.75 0.75 0.75 1"/>
    </material>
    <material name="red">
        <color rgba="1 0 0 1"/>
    </material>
    
    <!-- Macro for inertia matrix -->
    <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>

    <xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

    <!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <origin xyz="0 ${reflect*wheel_joint_y} ${-wheel_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
                <material name="gray" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
            </collision>
            <xacro:cylinder_inertial_matrix  m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
        </link>

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

        <!-- Transmission is important to link the joints and the controller -->
        <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>
    </xacro:macro>

    <!-- Macro for robot caster -->
    <xacro:macro name="caster" params="prefix reflect">
        <joint name="${prefix}_caster_joint" type="fixed">
            <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"/>
        </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>      
            <xacro:sphere_inertial_matrix  m="${caster_mass}" r="${caster_radius}" />
        </link>

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

    <xacro:macro name="mbot_base_gazebo">
        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>
        <gazebo reference="base_footprint">
            <turnGravityOff>false</turnGravityOff>
        </gazebo>

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${base_length/2 + caster_radius*2}" rpy="0 0 0" />        
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
                <material name="yellow" />
            </visual>
            <collision>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="${base_length}" radius="${base_radius}"/>
                </geometry>
            </collision>   
            <xacro:cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
        </link>

        <link name="base_link_upper">
            <visual>
                <origin xyz=" 0 0 ${base_length}" rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.05" radius="${base_radius}" />
                </geometry>
                <material name="red" />
            </visual>
            <collision>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.05" radius="${base_radius}"/>
                </geometry>
            </collision>   
            <xacro:cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
        </link>

            <!-- Attach base_link_upper to base_link -->
        <joint name="base_link_to_upper" type="fixed">
            <origin xyz="0 0 ${base_length}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="base_link_upper"/>
        </joint>

            <!-- Macro for pillar -->
        <xacro:macro name="pillar" params="x y">
            <link name="pillar_${x}_${y}">
                <visual>
                    <origin xyz="${x} ${y} ${base_length/2}" rpy="0 0 0"/>
                    <geometry>
                        <cylinder radius="${pillar_radius/2}" length="${pillar_length}" />
                    </geometry>
                    <material name="black" />
                </visual>
                <collision>
                    <origin xyz=" 0 0 0" rpy="0 0 0" />
                    <geometry>
                        <cylinder length="${pillar_radius/2}" radius="${pillar_length}"/>
                    </geometry>
                </collision>   
                <!-- <xacro:cylinder_inertial_matrix  m="${base_length}" r="${pillar_radius/2}}" h="${pillar_length}" /> -->
            </link>
            <joint name="pillar_${x}_${y}_joint" type="fixed">
                <origin xyz="${x} ${y} ${base_length/1.5}" rpy="0 0 0"/>
                <parent link="base_link"/>
                <child link="pillar_${x}_${y}"/>
            </joint>
        </xacro:macro>

        <!-- Pillars connecting upper and lower base -->
        <xacro:pillar x="0.05" y="0.05"/>
        <xacro:pillar x="-0.05" y="0.05"/>
        <xacro:pillar x="-0.05" y="-0.05"/>
        <xacro:pillar x="0.05" y="-0.05"/>

        <gazebo reference="base_link">
            <material>Gazebo/Blue</material>
        </gazebo>

        <xacro:wheel prefix="left"  reflect="1"/>
        <xacro:wheel prefix="right" reflect="-1"/>

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

        <!-- controller -->
        <gazebo>
            <plugin name="differential_drive_controller" 
                    filename="libgazebo_ros_diff_drive.so">                
                  <update_rate>30</update_rate>
                  <left_joint>left_wheel_joint</left_joint>
                  <right_joint>right_wheel_joint</right_joint>
                  <wheel_separation>${wheel_joint_y*2}</wheel_separation>
                  <wheel_diameter>${2*wheel_radius}</wheel_diameter>
                  <max_wheel_torque>20</max_wheel_torque>
                  <max_wheel_acceleration>1.0</max_wheel_acceleration>
                  <command_topic>cmd_vel</command_topic>
                  <publish_odom>true</publish_odom>
                  <publish_odom_tf>true</publish_odom_tf>
                  <publish_wheel_tf>true</publish_wheel_tf>
                  <odometry_topic>odom</odometry_topic>
                  <odometry_frame>odom</odometry_frame>
                  <robot_base_frame>base_footprint</robot_base_frame>
                  <odometry_source>1</odometry_source>
            </plugin>
        </gazebo> 
    </xacro:macro>

</robot>

在mbot_gazebo.xacro中复制下面的代码
这样引用可以使代码更整洁，且后期添加部件也很方便

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

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

    <xacro:mbot_base_gazebo/>

</robot>

3 在gazebo中显示模型，并使用rviz查看三种传感器仿真效果

（1）创建launch文件

cd ~/ros2_ws/src/mbot_gazebo 
touch launch/mbot.launch.py  launch/load_urdf_into_gazebo.launch.py

（2）mbot.launch.py

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node

import xacro


def generate_launch_description():

    # Check if we're told to use sim time
    use_sim_time = LaunchConfiguration('use_sim_time')

    # Process the URDF file
    pkg_path = os.path.join(get_package_share_directory('mbot_gazebo'))
    xacro_file = os.path.join(pkg_path,'urdf','mbot_gazebo.xacro')
    robot_description_config = xacro.process_file(xacro_file)
    
    # Create a robot_state_publisher node
    params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params]
    )


    # Launch!
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use sim time if true'),

        node_robot_state_publisher
    ])

(3) load_urdf_into_gazebo.launch.py

import os

from ament_index_python.packages import get_package_share_directory


from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():


    # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
    # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!

    package_name='mbot_gazebo' #<--- CHANGE ME
    world_file_path = 'worlds/my_house.sdf'
    
    pkg_path = os.path.join(get_package_share_directory(package_name))
    world_path = os.path.join(pkg_path, world_file_path)  
    
    # Pose where we want to spawn the robot
    spawn_x_val = '0.0'
    spawn_y_val = '0.0'
    spawn_z_val = '0.0'
    spawn_yaw_val = '0.0'
  
    mbot = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory(package_name),'launch','mbot.launch.py'
                )]), launch_arguments={'use_sim_time': 'true', 'world':world_path}.items()
    )

    # Include the Gazebo launch file, provided by the gazebo_ros package
    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
             )

    # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
                        arguments=['-topic', 'robot_description',
                                   '-entity', 'mbot',
                                   '-x', spawn_x_val,
                                   '-y', spawn_y_val,
                                   '-z', spawn_z_val,
                                   '-Y', spawn_yaw_val],
                        output='screen')



    # Launch them all!
    return LaunchDescription([
        mbot,
        gazebo,
        spawn_entity,
    ])

(4) 修改setup.py文件

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'mbot_gazebo'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
        (os.path.join('share', package_name, 'urdf'), glob(os.path.join('urdf', '*.*'))),
        (os.path.join('share', package_name, 'meshes'), glob(os.path.join('meshes', '*.*'))),
        (os.path.join('share', package_name, 'urdf/sensors'), glob(os.path.join('urdf/sensors', '*.*'))),
        (os.path.join('share', package_name, 'worlds'), glob(os.path.join('worlds', '*.*'))),   
        (os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*.rviz'))),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='lll',
    maintainer_email='lll@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

（5） 编译并运行launch程序

cd ~/ros2_ws/
colcon build --packages-select mbot_gazebo
source install/setup.bash 
ros2 launch mbot_gazebo load_urdf_into_gazebo.launch.py

可以看到我们已经将模型加载了进来，但此时还没有相机和雷达，接下面我们将相机和雷达也添加进来

（6）添加相机和雷达

在camera_gazebo.xacro中添加代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="camera">

    <xacro:macro name="usb_camera" params="prefix:=camera">
        <!-- Create laser reference frame -->
        <link name="${prefix}_link">
            <inertial>
                <mass value="0.1" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
                <material name="black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.01 0.04 0.04" />
                </geometry>
            </collision>
        </link>
        <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">
				    <ros>
				        <!-- <namespace>stereo</namespace> -->
				        <remapping>~/image_raw:=image_raw</remapping>
				        <remapping>~/camera_info:=camera_info</remapping>
				    </ros>
				    <camera_name>${prefix}</camera_name>
				    <frame_name>${prefix}_link</frame_name>
				    <hack_baseline>0.2</hack_baseline>
                </plugin>
            </sensor>
        </gazebo>

    </xacro:macro>
</robot>

在kinect_gazebo.xacro中添加下面的代码：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kinect_camera">

    <xacro:macro name="kinect_camera" params="prefix:=camera">
        <!-- Create kinect reference frame -->
        <!-- Add mesh for kinect -->
        <link name="${prefix}_link">
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
                <geometry>
                    <box size="0.15 0.04 0.04" />
                </geometry>
            </visual>
            <collision>
                <geometry>
                    <box size="0.07 0.3 0.09"/>
                </geometry>
            </collision>
        </link>

        <joint name="${prefix}_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
            <parent link="${prefix}_link"/>
            <child link="${prefix}_frame_optical"/>
        </joint>

        <link name="${prefix}_frame_optical"/>

        <gazebo reference="${prefix}_link">
            <sensor type="depth" name="${prefix}">
                <always_on>true</always_on>
                <update_rate>15.0</update_rate>
                <pose>0 0 0 0 0 0</pose>
                <camera name="kinect">
                    <horizontal_fov>${60.0*M_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="${prefix}_controller" filename="libgazebo_ros_camera.so">
		 		 	<ros>
						<!-- <namespace>${prefix}</namespace> -->
						<remapping>${prefix}/image_raw:=rgb/image_raw</remapping>
						<remapping>${prefix}/image_depth:=depth/image_raw</remapping>
						<remapping>${prefix}/camera_info:=rgb/camera_info</remapping>
						<remapping>${prefix}/camera_info_depth:=depth/camera_info</remapping>
						<remapping>${prefix}/points:=depth/points</remapping>
					</ros>
					<camera_name>${prefix}</camera_name>
					<frame_name>${prefix}_frame_optical</frame_name>
				    <hack_baseline>0.07</hack_baseline>
				    <min_depth>0.001</min_depth>
				    <max_depth>300.0</max_depth>
                </plugin>
            </sensor>
        </gazebo>

    </xacro:macro>
</robot>

在lidar_gazebo.xacro中添加下面的代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="laser">

    <xacro:macro name="rplidar" params="prefix:=laser">
        <!-- Create laser reference frame -->
        <link name="${prefix}_link">
            <inertial>
                <mass value="0.1" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.01" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
                <origin xyz=" 0 0 0 " rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.05" radius="0.05"/>
                </geometry>
                <material name="black"/>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <cylinder length="0.06" radius="0.05"/>
                </geometry>
            </collision>
        </link>
        <gazebo reference="${prefix}_link">
            <material>Gazebo/Black</material>
        </gazebo>

        <gazebo reference="${prefix}_link">
            <sensor type="ray" name="rplidar">
                <update_rate>20</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_ray_sensor.so">
		      <ros>
			<namespace>/</namespace>
			<remapping>~/out:=scan</remapping>
		      </ros>
		      <output_type>sensor_msgs/LaserScan</output_type>
                </plugin>
            </sensor>
        </gazebo>

    </xacro:macro>
</robot>

在rgbd_gazebo.xacro中添加下面的代码

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="rgbd_camera">

    <xacro:macro name="rgbd_camera" params="prefix:=camera">
        <!-- Create rgbd reference frame -->
        <!-- Add mesh for rgbd -->
        <link name="${prefix}_link">
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}"/>
                <geometry>
                    <box size="0.15 0.04 0.04" />
                </geometry>
            </visual>
            <collision>
                <geometry>
                    <box size="0.07 0.3 0.09"/>
                </geometry>
            </collision>
        </link>

        <joint name="${prefix}_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
            <parent link="${prefix}_link"/>
            <child link="${prefix}_frame_optical"/>
        </joint>

        <link name="${prefix}_frame_optical"/>

        <gazebo reference="${prefix}_link">
            <sensor type="depth" name="${prefix}">
                <always_on>true</always_on>
                <update_rate>15.0</update_rate>
                <pose>0 0 0 0 0 0</pose>
                <camera name="rgbd">
                    <horizontal_fov>${60.0*M_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="${prefix}_controller" filename="libgazebo_ros_camera.so">
		 		 	<ros>
						<!-- <namespace>${prefix}</namespace> -->
						<remapping>${prefix}/image_raw:=rgb/image_raw</remapping>
						<remapping>${prefix}/image_depth:=depth/image_raw</remapping>
						<remapping>${prefix}/camera_info:=rgb/camera_info</remapping>
						<remapping>${prefix}/camera_info_depth:=depth/camera_info</remapping>
						<remapping>${prefix}/points:=depth/points</remapping>
					</ros>
					<camera_name>${prefix}</camera_name>
					<frame_name>${prefix}_frame_optical</frame_name>
				    <hack_baseline>0.07</hack_baseline>
				    <min_depth>0.001</min_depth>
				    <max_depth>300.0</max_depth>
                </plugin>
            </sensor>
        </gazebo>

    </xacro:macro>
</robot>

新建mbot_with_kinect_laser_gazebo.xacro文件并在其中添加下面的代码

<?xml version="1.0"?>
<robot name="mbot" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find mbot_gazebo)/urdf/my_mbot_base_gazebo.xacro" />
    <xacro:include filename="$(find mbot_gazebo)/urdf/sensors/kinect_gazebo.xacro" />
    <xacro:include filename="$(find mbot_gazebo)/urdf/sensors/lidar_gazebo.xacro" />
    <xacro:include filename="$(find mbot_gazebo)/urdf/sensors/camera_gazebo.xacro" />

    <xacro:property name="camera_offset_x" value="0.17" />
    <xacro:property name="camera_offset_y" value="0" />
    <xacro:property name="camera_offset_z" value="0.38" />

    <xacro:property name="lidar_offset_x" value="0" />
    <xacro:property name="lidar_offset_y" value="0" />
    <xacro:property name="lidar_offset_z" value="0.105" />


    <xacro:property name="kinect_offset_x" value="0.15" />
    <xacro:property name="kinect_offset_y" value="0" />
    <xacro:property name="kinect_offset_z" value="0.11" />

    <!-- kinect -->
    <joint name="kinect_joint" type="fixed">
        <origin xyz="${kinect_offset_x} ${kinect_offset_y} ${kinect_offset_z}" rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="kinect_link"/>
    </joint>

    <xacro:kinect_camera prefix="kinect"/>

    <joint name="lidar_joint" type="fixed">
        <origin xyz="${lidar_offset_x} ${lidar_offset_y} ${lidar_offset_z}" rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="laser_link"/>
    </joint>

        <!-- Camera -->
    <joint name="camera_joint" type="fixed">
        <origin xyz="${camera_offset_x} ${camera_offset_y} ${camera_offset_z}" rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="camera_link"/>
    </joint>

    <xacro:usb_camera prefix="camera"/>

    <xacro:rplidar prefix="laser"/>

    <xacro:mbot_base_gazebo/>

</robot>

在mbot.launch.py中的第20行改为

    xacro_file = os.path.join(pkg_path,'urdf','mbot_with_kinect_laser_gazebo.xacro')

然后编译并运行

cd ~/ros2_ws/
colcon build --packages-select mbot_gazebo
source install/setup.bash 
ros2 launch mbot_gazebo load_urdf_into_gazebo.launch.py

运行结果如图所示

然后我们运行rviz2

ros2 run rviz2 rviz2

启动成功后，在左侧Displays窗口中点击“Add”，找到Image显示项，OK确认后就可以加入显示列表啦，然后配置好该显示项订阅的图像话题，就可以顺利看到机器人的摄像头图像啦。

同样的流程，点击Add，添加PointCloud2，设置订阅的点云话题，还要配置Rviz的参考系是odom，就可以看到点云数据啦，每一个点都是由xyz位置和rgb颜色组成。

点击Add，选择Laserscan，然后配置订阅的话题名，rviz的固定坐标系依然是odom，此时就可以看到激光点啦。

topic列表：

4 编写机器人运动控制程序，遥控机器人运动

下载键盘控制节点teleop_twist_keyboard

cd ~/ros2_ws/
colcon build --packages-select teleop_twist_keyboard
source install/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard

运行结果如下图，已经可以通过键盘控制自己的小车移动了。

5 总结

至此我们有了一个完整的差速轮式机器人，可以使用键盘控制它，在rviz实时查看它，但是它自己没有感知规划能力，后续我们将添加语音控制的节点和视觉目标检测的功能。