[ros2学习笔记]-如何给模型添加控制器（驱动模型）

参考这篇帖子

1.添加控制器依赖

确定在CMakeLists.txt中添加：

find_package(urdf REQUIRED)
find_package(controller_manager REQUIRED)
find_package(joint_state_controller REQUIRED)
find_package(robot_state_publisher REQUIRED)

确定在package.xml中添加：

<build_depend>urdf</build_depend>
<build_depend>controller_manager</build_depend>
<build_depend>joint_state_controller</build_depend>
<build_depend>robot_state_publisher</build_depend>

<build_export_depend>urdf</build_export_depend>
<build_export_depend>controller_manager</build_export_depend>
<build_export_depend>joint_state_controller</build_export_depend>
<build_export_depend>robot_state_publisher</build_export_depend>

<exec_depend>urdf</exec_depend>
<exec_depend>controller_manager</exec_depend>
<exec_depend>joint_state_controller</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>

2.在机器人模型中添加transmissions元素

查看之前的URDF文件：

  <m_link_box name="${link_00_name}"
           origin_rpy="0 0 0" origin_xyz="0 0 0"
           mass="1024"
           ixx="170.667" ixy="0" ixz="0"
           iyy="170.667" iyz="0"
           izz="170.667"
           size="1 1 1" />

  <m_joint name="${link_00_name}__${link_01_name}" type="revolute"
           axis_xyz="0 0 1"
           origin_rpy="0 0 0" origin_xyz="0 0 0.5"
           parent="${link_00_name}" child="${link_01_name}" />

  <transmission name="trans_${link_00_name}__${link_01_name}">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="${link_00_name}__${link_01_name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_${link_00_name}__${link_01_name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <m_link_cylinder name="${link_01_name}"
           origin_rpy="0 0 0" origin_xyz="0 0 0.2"
           mass="157.633"
           ixx="13.235" ixy="0" ixz="0"
           iyy="13.235" iyz="0"
           izz="9.655"
           length="0.4" radius="0.35" />

  <m_joint name="${link_01_name}__${link_02_name}" type="revolute"
           axis_xyz="0 1 0"
           origin_rpy="0 0 0" origin_xyz="0 0 0.4"
           parent="${link_01_name}" child="${link_02_name}" />

  <transmission name="trans_${link_01_name}__${link_02_name}">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="${link_01_name}__${link_02_name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_${link_01_name}__${link_02_name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <m_link_cylinder name="${link_02_name}"
           origin_rpy="0 0 0" origin_xyz="0 0 0.4"
           mass="57.906"
           ixx="12.679" ixy="0" ixz="0"
           iyy="12.679" iyz="0"
           izz="0.651"
           radius="0.15" length="0.8" />

对每一个关节都需要写一个transmission元素。
在该例子中我们定义每个关节为一个SimpleTransmission类型。
也要在joint中指定相应关节，关节名字需要和要驱动的关节匹配。
关节相应硬件接口也需要指定，在该例中我们指定为：

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

还需要定义actuator元素，如下：

    <actuator name="motor_${link_01_name}__${link_02_name}">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
    </actuator>

3.在URDF模型中建立gazebo控制器

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

4.描述控制器YAML文件

除了进行上述操作，还需要定义控制器变量（PID）参数。
创建一个新的控制器描述文件：~/simulation_ws/src/mrm_description/config/joints.yaml
内容如下：

# 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: base_link__link_01
  pid: {p: 2000.0, i: 100, d: 500.0}
joint2_position_controller:
  type: effort_controllers/JointPositionController
  joint: link_01__link_02
  pid: {p: 50000.0, i: 100, d: 2000.0}

5.在launch文件中添加控制器

        <!-- Load controllers -->
        <rosparam command="load" file="$(find mrm_description)/config/joints.yaml" />

        <node name="controller_spawner" pkg="controller_manager" type="spawner"
              respawn="false" output="screen" ns="/mrm"
              args="--namespace=/mrm
              joint_state_controller
              joint1_position_controller
              joint2_position_controller
              --timeout 60">
        </node>