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

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

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>

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>

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>

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>

5.安装键盘控制器包

使用如下命令安装

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

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

a)启动.launch文件

  roslaunch gazebotest my_move_roboot.launch

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

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

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