本文在Ubuntu18.04 + ROS melodic环境下完成，其他ROS版本类似。

在本教程中，我们将设置模拟控制器来驱动机器人的关节。这将使我们能够为MoveIt！等运动规划者提供正确的ROS接口。我们将使用ros_control功能包，这是ROS中用于控制器接口的新标准。

一、ros_control简介

在本教程的开头，我们还是有必要去学习一下ros_control的wiki官方文档。

ros_control功能包是对pr2_mechanism软件包的重写，以使控制器对PR2以外的所有机器人通用。

ros_control软件包将来自机器人执行器编码器的关节状态数据和输入设定点作为输入。它使用一般的控制回路反馈机制（通常是PID控制器）来控制发送到执行器的输出（通常是工作量）。对于没有联合位置，工作量等一一对应关系的物理机制，ros_control变得更加复杂，这些情况是使用传输来解决的。

1、Contorller

ros_controllers中包含的可用控制器插件列表如下，您也可以创建自己的应用程序，并且不限于以下列表。所有控制器都使用forward_command_controller将命令发送到硬件接口。

• effort_controllers：在关节上施加所需的力/扭矩。
• joint_effort_controller (力/扭矩输入直接输出到关节)
• joint_position_controller (位置输入进入PID控制器，该PID控制器向关节输出力/扭矩)
• joint_velocity_controller (速度输入进入PID控制器，该PID控制器向关节输出力/扭矩)
• joint_state_controller：将注册到hardware_interface::JointStateInterface的所有资源的状态发布到类型为sensor_msgs/JointState的topic。
• joint_state_controller
• position_controllers：一次设置一个或多个关节位置。
• joint_position_controller
• joint_group_position_controller
• velocity_controllers：一次设置一个或多个关节速度。
• joint_velocity_controller
• joint_group_velocity_controller
• joint_trajectory_controllers：附加功能，可为整个轨迹加花样。
• position_controller
• velocity_controller
• effort_controller
• position_velocity_controller
• position_velocity_acceleration_controller

查看源文件，以了解如何使用position_controller，velocity_controller等对joint_trajectory
_controller命名。

2、Hardware Interfaces

可用硬件接口（通过硬件资源管理器）的列表如下，ROS将硬件接口与上述ROS控制器之一结合使用，以向硬件发送和接收命令。 如果您的机器人的硬件接口尚不存在，那么您当然可以创建自己的硬件接口，并且不限于此列表：

• Joint Command Interface：硬件接口支持命令关节阵列。请注意，这些命令可以具有任何语义含义，只要它们都可以由单个double表示即可，它们不一定是控制力的命令。要指定此命令的含义，请参见派生类：
• Effort Joint Interface：用于指挥基于力的关节
• Velocity Joint Interface：用于指挥基于速度的关节
• Position Joint Interface：用于指挥基于位置的关节
• Joint State Interfaces：硬件接口支持读取命名关节数组的状态，每个关节都有一些位置，速度和作用力（力或扭矩）。
• Actuator State Interfaces：硬件接口支持读取命名的执行器数组的状态，每个执行器都有一定的位置，速度和作用力（力或扭矩）。
• Actuator Command Interfaces
• Effort Actuator Interface
• Velocity Actuator Interface
• Position Actuator Interface
• Force-torque sensor Interface
• IMU sensor Interface

另请参阅hardware_interface的C++ API和程序包wiki文档。

3、其他

Transmissions、Joint Limits等就不在此一一赘述，请大家自行学习：

https://link.zhihu.com/?target=https%3A//wiki.ros.org/ros_control

4、安装该功能包

$ sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers

二、ros_control和gazebo的数据流

可以使用ros_control和简单的gazebo插件适配器在gazebo中模拟机器人的控制器。仿真(simulation)，硬件(hardware)，控制器(controllers)和传输(transmissions)之间的关系概述如下所示：

先决条件：本教程基于先前教程中的许多概念。我们将再次使用urdf教程中设置的RRBot，作为此处介绍的插件的示例。确保已经按照安装说明中的说明安装了ros_control，ros_controllers及其依赖项。

三、在gazebo中的用法

1、将transmissions元素添加到urdf

要将ros_control与您的机器人一起使用，您需要向urdf添加一些其他元素。<transmission>元素用于将执行器链接到关节，有关精确的XML格式，请参见<transmission>规范。

以下列出其中的重要信息：

• <joint name = "">，该名称必须与urdf中的其他关节相对应。
• <type>，传输类型。 当前仅实现“ transmission_interface/SimpleTransmission”。
• <hardwareInterface>，在<actuator>和<joint>标记内时，它告诉gazebo_ros_control插件要加载什么硬件接口（位置，速度或力度接口）。当前仅实现力接口。

2、添加gazebo_ros_control插件

除了transmissions标签之外，还需要将一个gazebo插件添加到您的urdf中，该插件实际上会解析transmissions标签并加载适当的硬件接口和控制器管理器。默认情况下，gazebo_ros_
control插件非常简单，尽管它还可以通过其他插件体系结构进行扩展，以允许高级用户在ros_control和gazebo之间创建自己的自定义机器人硬件接口。

插件默认的XML代码应该添加到您的urdf中：

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

gazebo_ros_control <plugin>标签还具有以下可选的子元素：

• <robotNamespace>：用于此插件实例的ROS名称空间，在urdf/sdf中默认为机器人名称；
• <controlPeriod>：控制器更新的周期（以秒为单位），默认为gazebo的周期；
• <robotParam>：robot_description（urdf）在参数服务器上的位置，默认为'/ robot_description'
• <robotSimType>：要使用的自定义机器人sim接口的pluginlib名称（有关更多详细信息，请参见下文），默认为'DefaultRobotHWSim'。

3、默认的gazebo_ros_control行为

默认情况下，如果没有<robotSimType>标记，gazebo_ros_control将尝试从urdf中获取与基于ros_control的控制器接口所需的所有信息。在大多数情况下，这已足够，至少对于入门而言，是好的。

默认行为提供以下ros_control接口：

• hardware_interface::JointStateInterface
• hardware_interface::EffortJointInterface
• hardware_interface::VelocityJointInterface

4、进阶：自定义gazebo_ros_control仿真插件

gazebo_ros_control插件还提供了一个基于pluginlib的接口，用于在gazebo和ros_control之间实现自定义接口，以模拟更复杂的机制（非线性弹簧，连杆等）。

这些插件必须继承gazebo_ros_control::RobotHWSim，该插件实现了模拟的ros_control hardware_interface::RobotHW。RobotHWSim提供API级访问，以在gazebo仿真器中读取和命令关节属性。

相应的RobotHWSim子类在urdf模型中指定，并在加载机器人模型时加载。例如，以下XML将加载默认插件（与不使用<robotSimType>标记时相同）：

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

四、RRBot案例

我们希望gazebo启动的每个关节都添加一个类似于以下内容的<transmission>块。请注意，<hardwareInterface>必须同时包含在<joint>和<actuator>标记中。打开rrbot.xacro文件，在文件底部，您应该看到：

<transmission name="tran1">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint1">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor1">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator> 
</transmission>

<transmission name="tran2">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint2">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor2">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
</transmission>

您还将在rrbot.gazebo中看到gazebo_ros_control插件，该插件读取所有<transmission>标签：

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

五、创建一个ros_controls包

接下来，我们需要为与gazebo交互的ros_control控制器创建一个配置文件 (configuration file)与启动文件 (launch file)。

1、创建功能包

以catkin_ws工作区，MYROBOT_control功能包为例，名称可自定义。

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

完整功能包请在github中下载（推荐使用此包学习）：

https://link.zhihu.com/?target=https%3A//github.com/ros-simulation/gazebo_ros_demos/tree/kinetic-devel/rrbot_control

2、创建一个yaml配置文件

PID增益和控制器设置必须保存在yaml文件中，该文件通过roslaunch文件加载到参数服务器中。在MYROBOT _control功能包的config文件夹中，将以下RRBot示例作为MYROBOT_
control/config/rrbot_control.yaml应用于您的机器人：

rrbot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

有关这些控制器的更多详细信息，请参见下一部分。

3、创建一个roslaunch文件

创建用于启动ros_control控制器的roslaunch文件。在launch文件夹中，创建一个MYROBOT_control.launch文件，并将以下RRBot示例适应您的机器人：

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_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="/rrbot/joint_states" />
  </node>

</launch>

解析：

• 第一行“ rosparam”通过加载yaml配置文件（在下一节中讨论）将控制器设置加载到参数服务器。
• controller_spawner节点通过运行python脚本启动RRBot的两个关节位置控制器，该脚本对ros_control控制器管理器进行服务调用。服务调用告诉控制器管理器您想要哪个控制器。 它还加载了第三个控制器，该控制器使用hardware_interfaces发布所有关节的关节状态，并发布/joint_states上话题。
• 最后一行启动一个robot_state_publisher节点，该节点监听来自joint_state_controller的/ joint_states消息，然后将转换发布到/tf。 这使您可以在rviz中查看模拟的机器人以及执行其他任务。

4、使用roslaunch启动控制器

通过运行以下命令测试由ros_control控制的RRBot：

启动RRBot仿真：

$ roslaunch rrbot_gazebo rrbot_world.launch

通过运行第二个launch文件来加载两个关节的控制器：

$ roslaunch rrbot_control rrbot_control.launch

节点图如下所示（/joint_states负责发布所有关节的关节状态，/robot_state_publisher负责tf变换供rviz使用）：

六、手动使用服务调用

如果首先将rrbot_control.yaml文件加载到参数服务器，则可以通过服务请求手动加载控制器。 尽管我们通常更喜欢roslaunch，但我们将在此处提出以供参考：

先查看rosservice ($ rosservice list)，此时还未存在joint controller服务：

接着查看参数服务器中的参数 ($ rosparam list)，发现rrbot_control.yaml中的参数还未导入：

运行下方命令行导入参数文件：$ rosparam load rrbot_control.yaml

加载控制器：

$ rosservice call /rrbot/controller_manager/load_controller "name: 'joint1_position_controller'"
$ rosservice call /rrbot/controller_manager/load_controller "name: 'joint2_position_controller'"

再次查看rosservice中的服务，发现已经加载了joint1和joint2：

启动控制器：

$ rosservice call /rrbot/controller_manager/switch_controller "{start_controllers: ['joint1_position_controller','joint2_position_controller'], stop_controllers: [], strictness: 2}"

停止控制器：

$ rosservice call /rrbot/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['joint1_position_controller','joint2_position_controller'], strictness: 2}"

因上述过程繁琐，之后的操作均采用roslaunch方式启动控制器。

七、手动发送示例命令

向他们发送示例关节命令进行测试：

$ rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
$ rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

发送命令时，查看节点图：

gazebo中的机器人关节1位置发生的变化：

八、使用RQT发送命令

在本节中，我们将介绍一些工具，以帮助您可视化控制器的性能并调整控制器可能具有的任何增益/参数，尤其是PID增益。我们将使用rqt（ROS的基于插件的用户界面），因此请确保首先安装了rqt。

$ rosrun rqt_gui rqt_gui