前言

系统环境: Ubuntu 18.04，硬盘安装。

ROS版本: melodic

深蓝学院（古月居）胡春旭ROS理论与实践笔记整理。

1. 优化物理仿真

1.1 xacro基本语法

  • 精简模型代码

    • 创建宏定义
    • 文件包括

  • 提供可编程接口

    • 常量
    • 变量
    • 数学计算
    • 条件语句

  • 常量定义与使用:

1
2
3
<xacro:property name="M_PI" value="3.14159" />

<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
  • 数学计算：
1
<origin xyz="0 ${(motor_length+wheel_length)/2} 0" rpy = "0 0 0" />
  • 宏定义：
1
2
3
<xacro:macro name = "name" params="A B C">
    ......
</xacro:macro>
  • 宏调用:
1
<name A="A_value" B="B_value" C="C_value" />
  • 文件包含：
1
<xacro:include filename="$(find mbot_description)/urdf/mbot_base_gazebo.xacro" />

1.2 ros_control框架

  • ros_control
    • ROS为开发者提供的机器人控制中间件
    • 包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等待
    • 可以帮助机器人应用功能包快速落地，提高开发效率。
ros control框架

其中：

  • 控制器管理器

提供一种通用的接口来管理不同的控制器。

  • 控制器

读取硬件状态，发布控制命令，完成每个joint的控制。

  • 硬件资源

为上下两层提供硬件资源的接口。

  • 机器人硬件抽象

机器人硬件抽象和硬件资源直接打交道，通过write和read方法完成硬件操作。

  • 真实机器人

执行接收到的命令

gazebo+ros_control实现仿真

  • ros_control里主要提供四种控制器：

    • joint_state_controller

    • joint_effort_controller

    • joint_position_controller

    • joint_velocity_controller

1.3 模型优化步骤

需要对URDF文件做优化。

  • 第一步：为link添加惯性参数和碰撞属性

比如下列代码，base_link的碰撞属性是一个圆柱体，惯性矩阵就是宏cylinder_inertial_matrix，返回的是一个3×33\times3的惯性矩阵。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
<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>

<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>   
    <cylinder_inertial_matrix  m="${base_mass}" r="${base_radius}" h="${base_length}" />
</link>
  • 第二步：为link添加gazebo标签

这里的<turnGravityOff>是对模型的“影子”base_footprint的重力设置为0。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<gazebo reference="${prefix}_wheel_link">
    <material>Gazebo/Gray</material>
</gazebo>

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

<gazebo reference="base_footprint">
    <turnGravityOff>false</turnGravityOff>
</gazebo>

<gazebo reference="base_link">
    <material>Gazebo/Blue</material>
</gazebo>
  • 第三步：为joint添加传动装置
    • SimpleTransmission: ROS提供的简单的传动装置类型
    • 实现了VelocityJointInterface接口
    • <joint>代表传动关节，<motor>代表电机
    • <mechanicalReduction>代表关节和电机之间的减速比。
1
2
3
4
5
6
7
8
9
10
11
<!-- 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>\
  • 第四步：添加gazebo控制器插件

libgazebo_ros_diff_drive.so:ROS提供的差速控制插件。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
<!-- controller -->
<gazebo>
    <plugin name="differential_drive_controller" 
            filename="libgazebo_ros_diff_drive.so">
        <rosDebugLevel>Debug</rosDebugLevel>
        <publishWheelTF>false</publishWheelTF>
        <robotNamespace>/</robotNamespace>
        <publishTf>1</publishTf>
        <publishWheelJointState>false</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>

其中，需要注意的是：

  1. <robotNamespace>：机器人的命名空间，"/"代表不设置命名空间
  2. <leftJoint>和<rightJoint>左右轮转动的关节joint。
  3. <wheelSeparation>和wheelDiameter：机器人模型的相关尺寸，在计算差速参数时需要用到。
  4. <commandTopic>：控制器订阅的速度控制指令，生成全局命名时需要结合<robotNamespace>中设置的命名空间。
  5. <odometryFrame>：里程计数据的参考坐标系，ROS中一般都命名为odom。
  • 第五步：编写launch文件：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
<launch>

    <!-- 设置launch文件的参数 -->
    <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="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 mbot_description)/urdf/mbot_gazebo.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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
<launch>

    <!-- 设置launch文件的参数 -->
    <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="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 mbot_description)/urdf/mbot_gazebo.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>

  • 第七步: 测试
1
$ roslaunch mbot_gazebo view_mbot_gazebo_empty_world.launch

2. 创建物理仿真环境

常用的两种方式:

  • 第一种方法: 直接添加环境模型(直接insert)
  • 第二种方法:使用Gazebo提供的小软件Building Editor

以一个之前保存的仿真环境为例：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mbot_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 mbot_description)/urdf/mbot_gazebo.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>

启动仿真环境：

1
$ roslaunch mbot_gazebo view_mbot_gazebo_play_ground.launch

编写键盘控制的脚本:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty

msg = """
Control mbot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0),
        'o':(1,-1),
        'j':(0,1),
        'l':(0,-1),
        'u':(1,1),
        ',':(-1,0),
        '.':(-1,1),
        'm':(-1,-1),
           }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = .2
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('mbot_teleop')
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

    x = 0
    th = 0
    status = 0
    count = 0
    acc = 0.1
    target_speed = 0
    target_turn = 0
    control_speed = 0
    control_turn = 0
    try:
        print msg
        print vels(speed,turn)
        while(1):
            key = getKey()
            # 运动控制方向键（1：正方向，-1负方向）
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                th = moveBindings[key][1]
                count = 0
            # 速度修改键
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]  # 线速度增加0.1倍
                turn = turn * speedBindings[key][1]    # 角速度增加0.1倍
                count = 0

                print vels(speed,turn)
                if (status == 14):
                    print msg
                status = (status + 1) % 15
            # 停止键
            elif key == ' ' or key == 'k' :
                x = 0
                th = 0
                control_speed = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    th = 0
                if (key == '\x03'):
                    break

            # 目标速度=速度值*方向值
            target_speed = speed * x
            target_turn = turn * th

            # 速度限位，防止速度增减过快
            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.02 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.02 )
            else:
                control_speed = target_speed

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            # 创建并发布twist消息
            twist = Twist()
            twist.linear.x = control_speed; 
            twist.linear.y = 0; 
            twist.linear.z = 0
            twist.angular.x = 0; 
            twist.angular.y = 0; 
            twist.angular.z = control_turn
            pub.publish(twist)

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

编写launch文件：

1
2
3
4
5
6
7
<launch>
  <node name="mbot_teleop" pkg="mbot_teleop" type="mbot_teleop.py" output="screen">
    <param name="scale_linear" value="0.1" type="double"/>
    <param name="scale_angular" value="0.4" type="double"/>
  </node>
</launch>

启动键盘控制：

1
$ roslaunch mbot_teleop mbot_teleop.launch

3. 传感器仿真

3.1 常规摄像头

写一个宏定义文件camera_gazebo.xacro：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
<?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">
                    <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>

    </xacro:macro>
</robot>

需要注意的是：

  1. <sensor>标签：描述传感器
    • type：传感器类型，camera
    • name：摄像头命名，自由设置
  2. <camera>标签：描述摄像头参数
    • 分辨率，编码格式，图像范围，噪音参数等
  3. <plugin>标签：加载摄像头仿真插件 libgazebo_ros_camera.so
    • 设置插件的命名空间、发布图像的话题、参考坐标系等

此时，在之前的base_link的宏里把base_link和camera连接起来：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <xacro:include filename="$(find mbot_description)/urdf/mbot_base_gazebo.xacro" />
    <xacro:include filename="$(find mbot_description)/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.10" />

    <!-- 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"/>

    <mbot_base_gazebo/>

</robot>

最后，写一个launch文件:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mbot_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 mbot_description)/urdf/mbot_with_camera_gazebo.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>

启动仿真环境并使用rqt小工具仿真:

1
2
3
$ roslaunch mbot_gazebo view_mbot_with_camera_gazebo.launch

$ rqt_image_view

左上角改成/camera/image_raw，这样就可以通过rqt小工具仿真虚拟摄像头。

3.2 RGB-D摄像头（kinect）

和camera类似，先写一个宏定义文件:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
<?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>
                    <mesh filename="package://mbot_description/meshes/kinect.dae" />
                </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>20.0</update_rate>
                <camera>
                    <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="kinect_${prefix}_controller" filename="libgazebo_ros_openni_kinect.so">
                    <cameraName>${prefix}</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>${prefix}_frame_optical</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>

    </xacro:macro>
</robot>

在之前的base_link的宏里把base_link和RGB-D连接起来：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

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

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

    <mbot_base_gazebo/>

</robot>

最后写一个launch文件:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mbot_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 mbot_description)/urdf/mbot_with_kinect_gazebo.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>

启动仿真环境：

1
$ roslaunch mbot_gazebo view_mbot_with_kinect_gazebo.launch

在rviz中显示：

1
$ rosrun rviz rviz

Add一个pointCloud2，一个robotmodel，topic选择/kinect/depth/points，fixed Frame改成odom，让小车动起来:

1
$ roslaunch mbot_teleop mbot_teleop.launch

深度图也会随运动改变。

3.3 激光雷达仿真

也和常规camera类似，先写一个宏定义文件:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mbot_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 mbot_description)/urdf/mbot_with_laser_gazebo.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>

再连接link：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

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

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

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

    <xacro:rplidar prefix="laser"/>

    <mbot_base_gazebo/>

</robot>

最后写launch文件:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<launch>

    <!-- 设置launch文件的参数 -->
    <arg name="world_name" value="$(find mbot_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 mbot_description)/urdf/mbot_with_laser_gazebo.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>

启动仿真环境：

1
roslaunch mbot_gazebo view_mbot_with_lase_gazebo.launch

在rviz中显示：

1
$ rosrun rviz rviz

Add一个LaserScan并且topic订阅/scan，add一个robotModel，再键盘控制小车:

1
$ roslaunch mbot_teleop mbot_teleop.launch

深度图也会随运动改变。