Gazebo + ros_control

到目前为止，我们的机器人还是软软的趴在地上。本文中，我们将具体介绍插件gazebo_ros_control来让我们的机器人动起来。在Gazebo的仿真插件一文中，我们已经介绍了如何在SDF文件中插入插件，如何写一个Gazebo的插件，如何在Gazebo插件中使用ROS的API。这里我们将在URDF文件中插入Gazebo的插件，并通过ROS的环境控制机器人站在Gazebo的世界中。

ros_control是一个实现和管理机器人控制器的框架，它致力于提供一种具有实时性能的，与机器人无关的控制器设计方法。 ros_control源自于PR2机器人的控制器pr2_mechanism，但是ros_control却完全是robot-agnostic的。目前已经称为ROS中一种标准的控制器接口了。我们还有专门的文章介绍ros_control的细节，这里讲述如何在Gazebo环境中使用它。

The ros_control framework proides the capability to implement and manage robot controllers with a focus on both real-time performance and sharing of controllers in a robot-agnostic way.

1. 添加插件gazebo_ros_control

在上一节中，我们提到URDF专门定义了<gazebo>标签来描述在SDF格式中定义的属性。我们想在URDF文件中添加Gazebo的插件就需要用到这个标签了。根据我们的需要，在添加插件的时候可以指定为<robot>，<link>或者<joint>，只要将它写到<gazebo>中就可以了。

gazebo_ros_control是Gazebo的一个插件用来根据设定装载合适的硬件接口和控制器。这个实现非常简单，由于Gazebo的插件系统具有很强的扩展性，使得一些高级玩家可以在ros_control和Gazebo之间创建自己的机器人硬件接口。我们通过在URDF文件中写入如下的XML文本就可以添加gazebo_ros_control插件了，十分简单方便。

        <gazebo>
            <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
                <robotNamespace>/MYROBOT</robotNamespace>
            </plugin>
        </gazebo>

这里我们没有为标签<gazebo>添加属性reference，这样它就是对整个机器人<robot>的描述。gazebo_ros_control的<plugin>标签还可以通过如下的子标签指定一些参数:

<robotNamespace>: 用来告知插件其对象的ROS命名空间，默认是URDF或者SDF对应的机器人名称。
<contolPeriod>:控制器的更新周期，单位为秒，默认使用Gazebo的周期。
<robotParam>:在ROS的参数服务器上的机器人描述(URDF)路径，默认是'/robot_description'。
<robotSimType>:机器人仿真接口所使用的插件库名称，默认是'DefaultRobotHWSim'。

<robotNamespace>,<contolPeriod>,<robotParam>这几个标签都比较好理解。<robotSimType>描述了仿真接口的插件库名称。实际上gazebo_ros_control虽然是一个Gazebo的插件，它还是提供了一个插件接口用来配置Gazebo与ros_control之间个性化定制的接口。可以理解为一个插件的插件。借助这个接口，我们可以实现自己的控制器，只是需要继承自gazebo_ros_control::RobotHWSim。默认的插件库是'DefaultRobotHWSim'，提供了如下的ros_control接口：

hardware_interface::JointStateInterface
hardware_interface::EffortJointInterface
hardware_interface::VelocityJointInterface - 目前没有完全实现

2. 添加<transmission>标签

<transmission>是URDF中用来描述执行器和关节之间关系的扩展标签。通过它我们可以描述类似齿轮减速比以及并联结构的属性。通过它我们可以指定机器人的控制器接口，方便使用ros_control控制机器人。我们为左臂的肘关节添加如下的<transmission>标签：

        <transmission name="tran_left_upper_arm_to_left_elbow_y">
            <type>transmission_interface/SimpleTransmission</type>
            <joint name="left_upper_arm_to_left_elbow_y">
                <hardwareInterface>EffortJointInterface</hardwareInterface>
            </joint>
            <actuator name="act_left_upper_arm_to_left_elbow_y">
                <mechanicalReduction>1</mechanicalReduction>
            </actuator>
        </transmission>

对于每一个<transmission>标签都有一个"name"的属性，用来唯一的标识transmission，这里我根据自己的习惯以需要关联的joint的名称加上"tran_"前缀命名。它有三个子标签，分别描述了<transmission>的类型，以及关联的joint和执行器。

<type>描述了transmission的类型，对于Gazebo而言目前只实现了"transmission_interface/SimpleTransmission"这一个类型。 <joint>标签描述了关联的joint，它的name的属性用来指示需要关联的joint，它有一个hardwareinterface的子标签，用来描述硬件接口，因为我们将在Gazebo环境中使用，所以这里必须是"EffortJointInterface"。 <actuator>标签则声明了与之关联的执行器，同样需要以属性"name"给它一个唯一的名字，一般我们以joint名称加上"act_"前缀命名。它还有一个<mechanicalReduction>的标签，用来描述执行器到joint的齿轮减速比。修改后的机器人描述文件可以在这里下载。

3. 创建一个ros_controls包

接下来我们需要为ros_control控制器建立一个包，并为之创建一个配置文件和launch文件，分别存放在config和launch的子目录下，下面的指令用来创建包和相关子目录：

        $ cd ~/catkin_ws/src/
        $ catkin_create_pkg robot_control controller_manager joint_state_controller robot_state_publisher
        $ cd robot_control
        $ mkdir config
        $ mkdir launch

首先我们用yaml文件创建一个配置文件用来保存各个关节的PID参数。这些参数将通过launch文件加载到参数服务器上。下面在config子目录下创建GRobot_control.yaml，并写入如下的内容：

        GRobot:
            # Publish all joint states -----------------------------------
            joint_state_controller:
                type: joint_state_controller/JointStateController
                publish_rate: 50  
        
            # Position Controller -----------------------------------
            left_upper_arm_to_left_elbow_y_controller:
                type: effort_controllers/JointPositionController
                joint: left_upper_arm_to_left_elbow_y
                pid: {p: 100.0, i: 0.01, d:0.0}

关于这些控制器的细节我们将在后续的文章中介绍。接下来我们在launch子目录中创建一个GRobot_control.launch文件，用来加载刚才的配置文件并打开控制器。我们向launch文件中考入如下的内容：

        <launch>
            <rosparam file="$(find robot_control)/config/GRobot_control.yaml" command="load"/>
            <!-- load the controllers -->
            <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
                    output="screen" ns="/GRobot" args="left_upper_arm_to_left_elbow_y_controller"/>
            <!-- convert joint states to TF transforms for rviz, etc -->
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
                <remap from="/joint_states" to="/GRobot/joint_states" />
            </node>
        </launch>

在第2行中，我们导入了配置文件。第4,5行开启了控制器，其中属性"ns"指示了命名空间，"args"指示了需要装载的控制器。第7到9行则为了在类似rviz这样的节点中使用，而特意发布了关节状态用于广播TF变换。在下面的指令中，我们先通过上节创建的robot_gazebo包打开Gazebo世界，指令最后的'&'是Linux系统的一个特性告知系统后台运行指令。接着以相同的套路打开刚创建的launch文件加载控制器。此时，系统中就已经有了一个控制机器人左肘的控制器，我们可以通过rostopic发送消息来控制左肘转动。我们不断地运行第三行的指令，可以看到机器人的左肘在地上一遍又一遍的抽搐~~~

        $ roslaunch robot_gazebo robot_world.launch &
        $ roslaunch robot_control GRobot_control.launch &
        $ rostopic pub -1 /GRobot/left_upper_arm_to_left_elbow_y_controller/command std_msgs/Float64 "data: 0.0"
        publishing and latching message for 3.0 seconds

4. 总结

ros_control是ROS中一种比较常用的控制器管理和实现的框架。gazebo_ros_control是Gazebo的一个插件，用来根据设定装在合适的硬件接口和控制器。这些设定就是在URDF文件中的<transmission>标签中描述的内容，这是URDF格式针对ros_control专门设定的扩展。

本文中，我们以GRobot的左肘为例，设定了一个控制器，并可以通过rostopic发送消息控制机器人在地上抽搐。如法炮制，可以为GRobot的每一个关机都加上一个控制器，这样机器人就可以随意的在Gzabo的世界中抽搐了。修改之后的机器人描述文件、控制器配置文件和启动文件可以从这里获得：GRobot.gazebo.xacro, GRobot_control.yaml, GRobot_control.launch, GRobot_xacro_world.launch。