（三）ROS中控制机器人运动的实现（在gazebo中显示）_skid_steer_drive_controller

ROS中控制机器人运动主要流程如下
新建模型—>加载驱动插件—>启动模型和节点->显示
有了机器人模型还不行，在Gazebo中仿真需要控制模型。模型中有轮子，需要控制轮子转动，控制轮子转动就需要加载驱动。

步骤：
（1）编辑机器人模型
（2）编辑gazebo属性，加载连接器控制驱动插件
（3）编辑gazebo属性，加载ros控制插件
（4）编辑.launch文件
（5）安装键盘控制器包
（6）运行.launch和键盘控制器程序，让小车动起来。

注意：xacro文件可以包含机器人模型的描述如<joint>标签,同样可以包含物理属性的描述如<gazebo >标签，可以将机器人模型标签项放在.xacro文件中，<gazebo >标签单独放在.gazebo文件中，然后在.xacro文件中使用#include命令将.gazebo文件加载进来。为了方便起见，本文将<gazebo >属性都放在一个.xacro文件中。
首先是要创建包，配置环境变量等操作，就不详述了。
然后的（1）（2）（3）步骤都是通过编辑.xacro文件实现。
在工程目录下的urdf文件夹下新建my_move_robot.xacro文件。

1.编辑机器人模型

在my_move_robot.xacro文件下加入以下代码，代码包含的元素有车身，关节，轮子，
其中车身通过固定关节连接，包括三个标签
a)车身
<link name="base_footprint">，
<joint name="base_footprint_joint" type="fixed">

<link name="base_link">
1

b)轮子<link name="wheel_1">
c)轮子和机身的连接<joint name="base_to_wheel1" type="continuous">

my_move_robot.xacro文件如下

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" 
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
    name="robot1_xacro">


    <xacro:property name="length_wheel" value="0.05" />
    <xacro:property name="radius_wheel" value="0.05" />
    <xacro:macro name="default_inertial" params="mass">
               <inertial>
                       <mass value="${mass}" />
                       <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                                iyy="1.0" iyz="0.0"
                                izz="1.0" />
               </inertial>
    </xacro:macro>

    <link name="base_footprint">
        <visual>
            <geometry>
                    <box size="0.001 0.001 0.001"/>
                </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
        </visual>
        <xacro:default_inertial mass="0.0001"/>
    </link>
    <joint name="base_footprint_joint" type="fixed">
        <origin xyz="0 0 0" />
        <parent link="base_footprint" />
        <child link="base_link" />
    </joint>


    <link name="base_link">
        <visual>
            <geometry>
                    <box size="0.2 .3 .1"/>
                </geometry>
            <origin rpy="0 0 1.54" xyz="0 0 0.05"/>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                    <box size="0.2 .3 0.1"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="10"/>
    </link>

    <link name="wheel_1">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
            <!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
    </link>

    <link name="wheel_2">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
            <!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>

    </link>

    <link name="wheel_3">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
            <!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->

            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>
    </link>

    <link name="wheel_4">
        <visual>
                <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
                </geometry>
        <!--    <origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
            <origin rpy="0 0 0" xyz="0 0 0" />
            <material name="black"/>
        </visual>
        <collision>
            <geometry>
                    <cylinder length="${length_wheel}" radius="${radius_wheel}"/>
            </geometry>
        </collision>
        <xacro:default_inertial mass="1"/>

    </link>



 <joint name="base_to_wheel1" type="continuous">
   <parent link="base_link"/>
   <child link="wheel_1"/>
   <origin rpy="1.5707 0 0" xyz="0.1 0.15 0"/>
   <axis xyz="0 0 1" />
 </joint>

 <joint name="base_to_wheel2" type="continuous">
   <axis xyz="0 0 1" />
   <anchor xyz="0 0 0" />
   <limit effort="100" velocity="100" />
   <parent link="base_link"/>
   <child link="wheel_2"/>
   <origin rpy="1.5707 0 0" xyz="-0.1 0.15 0"/>
</joint>

 <joint name="base_to_wheel3" type="continuous">
   <parent link="base_link"/>
   <axis xyz="0 0 1" />
   <child link="wheel_3"/>
   <origin rpy="1.5707 0 0" xyz="0.1 -0.15 0"/>
 </joint>

 <joint name="base_to_wheel4" type="continuous">
   <parent link="base_link"/>
   <axis xyz="0 0 1" />
   <child link="wheel_4"/>
   <origin rpy="1.5707 0 0" xyz="-0.1 -0.15 0"/>
 </joint>
</robot>
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
151
152
153
154
155
156
157
158
159

2.编辑gazebo属性，加载连接器控制驱动插件

在文件my_move_robot.xacro中加入连接器控制驱动插件代码，内容很简单，加载插件的动态链接库。

<!-- Drive controller -->
<gazebo>
  <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
    <updateRate>100.0</updateRate>
    <robotNamespace></robotNamespace>
    <leftFrontJoint>base_to_wheel1</leftFrontJoint>
    <rightFrontJoint>base_to_wheel3</rightFrontJoint>
    <leftRearJoint>base_to_wheel2</leftRearJoint>
    <rightRearJoint>base_to_wheel4</rightRearJoint>
    <wheelSeparation>4</wheelSeparation>
    <wheelDiameter>0.1</wheelDiameter>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <robotBaseFrame>base_footprint</robotBaseFrame>
    <odometryFrame>odom</odometryFrame>
    <torque>1</torque>
    <topicName>cmd_vel</topicName>
    <broadcastTF>1</broadcastTF>
  </plugin>
</gazebo>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

3.编辑gazebo属性，加载ros控制插件

在文件my_move_robot.xacro中加入ros控制插件代码，如下

<!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/robot</robotNamespace>

    </plugin>
  </gazebo>
1
2
3
4
5
6
7

4.编辑.launch文件

文件很简单，内容包括加载一个空的地图，加载机器人模型，启动一个urdf_spawner服务节点。注意$(find gazebotest)表示我的工程包的文件目录。

<?xml version="1.0"?>
<launch>
 <include file="$(find gazebo_ros)/launch/empty_world.launch">
  </include>
  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
         command="$(find xacro)/xacro.py '$(find gazebotest)/urdf/my_move_robot.xacro'" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model robot1 -param robot_description -z 0.05"/>

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

5.安装键盘控制器包

使用如下命令安装

 sudo apt-get install ros-hydro-teleop-twist-keyboard
 rosstack profile
 rospack profile
1
2
3

6.启动并运行，让小车动起来

a)启动.launch文件

  roslaunch gazebotest my_move_roboot.launch
1

其中gazebotest为当前包的名字，此包要在ROS_PACK_PATH环境变量中。
my_move_roboot.launch为脚本，相当于一系列控制台命令
b)启动键盘控制器

rosrun teleop_twist_keyboard teleop_twist_keyboard.py
1

使用键盘控制可以看到小车在移动

在控制台输入rqt_graph，可以看到节点图如下