ROS探索总结-31.ros_control

本文转载自古月居，原作者古月，原文链接：https://www.guyuehome.com/890。

ROS探索总结-31.ros_control

说明：

• 介绍ros_control框架和使用

ROS中提供了丰富的机器人应用：SLAM、导航、MoveIt……但是你可能一直有一个疑问，这些功能包到底应该怎么样用到我们的机器人上，也就是说在应用和实际机器人或者机器人仿真器之间，缺少一个连接两者的东西。

ros_control就是ROS为用户提供的应用与机器人之间的中间件，包含一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等等，可以帮助机器人应用快速落地，提高开发效率。

总体框架

上图是ros_control的总体框架，可以看到正对不同类型的控制器（底盘、机械臂等），ros_control可以提供多种类型的控制器，但是这些控制器的接口各不相同，为了提高代码的复用率，ros_control还提供一个硬件的抽象层。

硬件抽象层负责机器人硬件资源的管理，而controller从抽象层请求资源即可，并不直接接触硬件。

上图是ros_control的数据流图，可以更加清晰的看到每个层次包含的功能：

• Controller Manager：每个机器人可能有多个controller，所以这里有一个控制器管理器的概念，提供一种通用的接口来管理不同的controller。controller manager的输入就是ROS上层应用的输出。

• Controller：controller可以完成每个joint的控制，请求下层的硬件资源，并且提供了PID控制器，读取硬件资源接口中的状态，在发布控制命令。

• Hardware Rescource：为上下两层提供硬件资源的接口。

• RobotHW：硬件抽象层和硬件直接打交道，通过write和read方法来完成硬件的操作，这一层也包含关节限位、力矩转换、状态转换等功能。

• Real Robot：实际的机器人上也需要有自己的嵌入式控制器，接收到命令后需要反映到执行器上，比如接收到位置1的命令后，那就需要让执行器快速、稳定的到达位置1。

Controllers

ros_controllers这个功能包提供了已有的一些controllers：

当然，我们也可以根据自己的需求，创建需要的controller，然后通过controller来管理自己创建的controller，可以参考https://github.com/ros-controls/ros_control/wiki/controller_interface

Hardware Interface

Hardware Interface是controller和RobotHw沟通的接口，基本上和controllers的种类是对应的，同样可以自己创建需要的接口，可以参考：https://github.com/ros-controls/ros_control/wiki/hardware_interface

Transmissions

Transmissions就是机器人的传动系统，机器人每个需要运动的关节都需要配置相应的Transmission，可以通过代码完成https://github.com/ros-controls/ros_control/wiki/transmission_interface，但大部分情况下，都会在URDF文件中直接添加（http://ros.org/wiki/urdf/XML/Transmission）：

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

Joint Limits

Joint Limits是硬件抽象层中的一块，维护一个关节限位的数据结构，这些限位数据可以从机器人的URDF文件中加载，也可以ROS的参数服务器上加载（先用YAML配置文件导入ROS parameter server），这些限位数据不仅包含关节速度、位置、加速度、加加速度、力矩等方面的限位，还包含安全作用的位置软限位、速度边界（k_v）和位置边界（k_p）等等。

我们来看一个URDF中设置Joint Limits的例子：

    <joint name="$foo_joint" type="revolute">

  <!-- other joint description elements -->

 

  <!-- Joint limits -->

  <limit lower="0.0"

         upper="1.0"

         effort="10.0"

         velocity="5.0" />

 

  <!-- Soft limits -->

  <safety_controller k_position="100"

                     k_velocity="10"

                     soft_lower_limit="0.1"

                     soft_upper_limit="0.9" /> 

</joint>

还有一些参数需要通过YAML配置文件先加载到参数服务器中，YAML文件的格式如下：

    joint_limits:

  foo_joint:

    has_position_limits: true

    min_position: 0.0

    max_position: 1.0

    has_velocity_limits: true

    max_velocity: 2.0

    has_acceleration_limits: true

    max_acceleration: 5.0

    has_jerk_limits: true

    max_jerk: 100.0

    has_effort_limits: true

    max_effort: 5.0

  bar_joint:

    has_position_limits: false # Continuous joint

    has_velocity_limits: true

    max_velocity: 4.0

另外，我们还可以在代码中使用joint_limits_interface来加载和设置关节的限位参数，可以参考： joint_limits_interface

controller manager

controller manager提供了一种多controller控制的机制，可以加载、开始运行、停止运行、卸载不同的controller，并且提供了多种工具来完成这些操作。

• 命令行工具

命令行的格式为：

    $ rosrun controller_manager controller_manager <command> <controller_name>

支持的 <command> ：

- load: load a controller (construct and initialize)
- unload: unload a controller (destruct)
- start: start a controller
- stop: stop a controller
- spawn: load and start a controller
- kill: stop and unload a controller

如果想要查看某个controller的状态，可以使用下边的命令：

    $ rosrun controller_manager controller_manager <command>

支持的<command> ：

- list: list all the controllers in the order they are executed, and give the state of  each controller
- list-types: list all the controller types the controller manager knows about. If your controller is not in this list, you won't be able to spawn it.
- reload-libraries: Reloads all the controller libraries that are available as plugins. This is convenient when you are developing a controller and you want to test your new controller code, without restarting the robot every time. This does not restart controllers which were running before.
- reload-libraries --restore: Reloads all the controller libraries that are available as plugins and restores all controllers to their original state.

但是很多时候我们需要控制的controller有很多，比如六轴机器人，至少有六个controller，这时也可以使用“spawner ”这个命令来一次控制多个controller：

$ rosrun controller_manager spawner [--stopped] name1 name2 name3

上边的命令可以自动加载、启动controller，如果加上–stopped参数，那么contrller则只会被加载，但是并不会开始运行。如果想要停止一系列controller，但是不需要卸载，还需要运行的话，可以使用下边的命令：

$ rosrun controller_manager unspawner name1 name2 name3

• launch工具

在launch文件中，同样可以通过运行controller_manager包的命令，来加载和启动一系列controller：

     <launch>

   <node pkg="controller_manager"

         type="spawner"

         args="controller_name1 controller_name2" /> 

 </launch>

上边的launch文件会加载并启动controllers，如果只需要加载：

    <launch>

  <node pkg="controller_manager"

    type="spawner"

    args="--stopped controller_name1 controller_name2" />

</launch>

• 可视化工具rqt_controller_manager

controller_manager还提供了可视化工具rqt_controller_manager，安装rosrun rqt_controller_manager rqt_controller_manager，直接使用下边的命令打开：

$ rosrun rqt_controller_manager rqt_controller_manager

不过目前在indigo ros版本里面，这个工具貌似有问题，找不到executable。

案例分析

gazebo的tutorials里边提供了gazebo_ros_control的教程，用到一个两个关节的机械臂作为案例，我们就来分析一下这个案例中都是怎样落实上边这些概念的。源码可以在这里找到。

首先来看一张gazebo结合ros_control的架构图，其实和上边的数据流图差别不大。

• Transmissions

rrbot有两个关节，每个关节都有一个传动装置，所以应该有两个Transmissions，在rrbot的URDF文件rrbot.xacro文件中，我们可以找到这两个Transmissions：

      <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可以识别
标签，还需要家在一个gazebo的ros_control插件：

    <gazebo>

  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

    <robotNamespace>/rrbot</robotNamespace>

  </plugin>

</gazebo>

• controller和controller_manager

首先通过一个YAML文件rrbot_control.yaml来声明我们所需要的controller，以及对应的参数：

    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}

其中还需要包含一个joint_state_controller，来控制发布每个关节的实时状态。

然后使用launch文件rrbot_control.launch，运行controller_manager中的spawner，加载并运行这些上边这些controller：

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

这个例程没有涉及到Joint Limits。通过下边命令就可以在gazebo中启动rrbot并且开始控制：

$ roslaunch rrbot_gazebo rrbot_world.launch

$ roslaunch rrbot_control rrbot_control.launch

也可以手动调用完成controller的加载：

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

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

启动controller：

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

停止controller：

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