在本教程中，我们将通过模拟控制器来驱动机器人的关节。这将使我们能够为MoveIt等规划器提供正确的ROS接口！。我们将使用ros_control软件包。

与ros通讯

一、介绍
- 1、ros_control
- 2、ros_controllers
二、准备工作
- 1、在urdf中添加“元素
- 2、添加gazebo_ros_control插件
- 3、RRBot示例
三、创建 ros_controls包
- 1、创建新的功能包
- 2、创建.yaml配置文件
- 3、创建launch文件
四、使用roslaunch启动控制器
- 手动加载控制器
- 手动发送命令
五、使用RQT发送命令
- 1、添加命令推送者

一、介绍

1、ros_control

ros_control是一个功能包，它重写ros原有的pr2_mechanism包，使得它能够控制所有机器人，而并不仅止于PR2。该功能包将机器人执行器关节状态数据和期望状态作为输入。它使用一般的控制回路反馈机制（通常是PID控制器），来控制输出（通常为力矩）。
gazebo-ros教程（一）ros_control

2、ros_controllers

一个包含在ros_controllers中的可用控制器插件列表。主要实现的是PID控制，完成速度、位置等闭环控制，并将指令发出去，如果是仿真就发送至gazebo，如果是实体机器人，就发送给真实机器人硬件。你可以创建自己的控制插件，并且不限于以下所提及的。所有控制器都使用forward_command_controller将命令发送到硬件端的接口。

effort_controllers：通过一个期望扭矩/力矩，来控制关节。里面包含了一些接口： ①joint_effort_controller ②joint_position_controller ③joint_velocity_controller
joint_state_controller-读取所有关节位置。 ①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

本教程基于先前教程中的许多内容。我们以RRBot为示范模型，作为此处介绍的插件的示例。此外，确保已经安装了ros_control，ros_controllers及其依赖项。在命令行输入：

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

二、准备工作

1、在urdf中添加`<transmission>`元素

要想使用ros_control来控制机器人，需要先在URDF添加元素，该元素用于将执行器与关节绑定起来，这里给出一个示例，其余关节需要自行根据名称进行修改：

<transmission name="simple_trans">
	<type>transmission_interface/SimpleTransmission</type>
	<joint name="foo_joint">
		<hardwareInterface>EffortJointInterface</hardwareInterface>
	</joint>
	<actuator name="foo_motor">
 		<mechanicalReduction>50</mechanicalReduction>
		<hardwareInterface>EffortJointInterface</hardwareInterface>
	</actuator>
</transmission>

按照gazebo_ros_control的规范，需要注意以下几点：

<joint name=""> ：必须对应你在urdf文件中定义的关节名称
<type>-传输类型。该插件当前仅实现了“ transmission_interface /SimpleTransmission”，因此不要做更改
<hardwareInterface>-：在<actuator>和<joint>标签中的，告诉gazebo_ros_control插件要加载的硬件接口（位置，速度或力矩接口）。当前仅实现了EffortJointInterface这一功能，同样也不要随意更改

2、添加gazebo_ros_control插件

除了标签之外，还需要将一个Gazebo插件添加到URDF中，该插件实际上会解析标签并加载适当的硬件接口和控制器管理器。默认插件的XML格式如下，该段应该添加到URDF中：

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

gazebo_ros_control 标记还具有以下可选的子元素：

<robotNamespace>：用于当前插件实例化的ROS命名空间，默认为URDF / SDF中机器人的名称
<controlPeriod>：控制器的更新周期（以秒为单位），默认为gazebo的周期
<robotParam>：URDF文件在参数服务器上的位置，默认为’/ robot_description’
<robotSimType>：自定义的机器人仿真接口的pluginlib名称，默认为’DefaultRobotHWSim’。

3、RRBot示例

为rrbot的所有关节添加<transmission>元素，内如可以参考以下代码。注意，<hardwareInterface>必须在和标记中都包含，不能省略。打开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>

图片内容为上述代码。
gazebo-ros教程（一）ros_control 在rrbot.gazebo文件中，同样能看到添加的gazebo_ros_control插件，用于读取所有<transmission>标签：

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

图片内容为上述代码

gazebo-ros教程（一）ros_control

三、创建 ros_controls包

接下来，我们需要为ros_control控制器，创建一个配置文件和launch文件，使其能够与Gazebo进行交互。你能够在rrbot_control里面找到这一节的样例
gazebo-ros教程（一）ros_control

1、创建新的功能包

依次执行以下命令

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

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、创建launch文件

创建用于启动ros_control控制器的roslaunch文件。在启动文件夹中创建一个文件，以RRBot为例，MYROBOT_control/launch/MYROBOT_control.launch，内容如下：

<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上发布话题。该节点只是用于roslaunch的帮助程序脚本。最后一行开始一个robot_state_publisher节点，该节点监听来自joint_state_controller的/ joint_states消息，然后将转换发布到/ tf。这使您可以在Rviz中查看模拟的机器人以及执行其他任务。

四、使用roslaunch启动控制器

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

<code class="prism language-powershell has-numbering">roslaunch rrbot_gazebo rrbot_world<span class="token punctuation">.</span>launch
</code>

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

<code class="prism language-powershell has-numbering">roslaunch rrbot_control rrbot_control<span class="token punctuation">.</span>launch
</code>

手动加载控制器

该节步骤与roslaunch rrbot_control rrbot_control.launch等效

如果首次将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 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}"

手动发送命令

发送一个控制命令用于测试：

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"

五、使用RQT发送命令

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

rosrun rqt_gui rqt_gui

1、添加命令推送者

在RQT的“Plugins”菜单上，依次点击“Topics- >Messahe Publisher”，对于rrbot，我们选择话题/rrbot/joint1_position_controller/command 然后按右上角的加号按钮。通过选中话题名称左侧的复选框来启用推送者。将速率列设置为100（即我们发送命令的频率为100hz）。之后你应该能够看到以下画面：
gazebo-ros教程（一）ros_control 接下来，点击话题前面的三角展开，我们能够看到“data”行。在RRBot的情况下，因为关节是连续的，所以没有极限，因此任何弧度值都可以使用。如果你在本教程中使用的是rrbot。我们尝试修改expression栏，将其值修改为 $\sin(i / 100)$ ，我们能看到以下效果，如果你使用的自己的机器人模型，也可以尝试不同的控制命令。

i 是时间RQT中的变量

gazebo-ros教程（一）ros_control
还可以通过添加plot插件来可视化数据，这里就不具体展示了，有兴趣的小伙伴可以自己尝试以下工具栏上，依次点击Plugins-> Visualization ->Plot 添加Plot插件，然后在橙色选框选择你想要可视化的数据
gazebo-ros教程（一）ros_control