编写自己的控制器

示例控制器包中的所有示例控制器都派生自 controller_interface::MultiInterfaceController 类，该类允许在一个控制器实例中声明多达四个接口。类的声明看起来如下所示：

class NameOfYourControllerClass : controller_interface::MultiInterfaceController <
                              my_mandatory_first_interface,
                              my_possible_second_interface,
                              my_possible_third_interface,
                              my_possible_fourth_interface> {
  bool init (hardware_interface::RobotHW* hw, ros::NodeHandle& nh);  // mandatory
  void update (const ros::Time& time, const ros::Duration& period);  // mandatory
  void starting (const ros::Time& time)   // optional
  void stopping (const ros::Time& time);  // optional
  ...
}

franka_hw 小节描述了可用的接口。

重要

请注意，命令接口的可声明组合受到限制，因为例如同时命令关节位置和笛卡尔位姿等没有意义。只读接口，如 JointStateInterface、FrankaStateInterface 或 FrankaModelInterface 始终可以声明并且不受限制。

命令接口的可能声明是：

`franka_hw::FrankaHW`	`franka_combinable_hw::FrankaCombinableHW`
所有可能的单一接口声明 `EffortJointInterface` +`PositionJointInterface` `EffortJointInterface` +`VelocityJointInterface` `EffortJointInterface` +`FrankaCartesianPoseInterface` `EffortJointInterface` +`FrankaCartesianVelocityInterface`	`EffortJointInterface` `EffortJointInterface` +`FrankaCartesianPoseInterface` `EffortJointInterface` +`FrankaCartesianVelocityInterface`

结合运动生成器接口提供 EffortJointInterface 背后的想法是向用户公开内部运动生成器。计算出的与运动生成器命令相对应的期望关节位姿在一个时间步后的机器人状态中可用。这种组合的一个用例是使用自己的关节层级扭矩控制器跟随笛卡尔轨迹。在这种情况下，将声明 EffortJointInterface + FrankaCartesianPoseInterface 的组合，将轨迹传输到 FrankaCartesianPoseInterface，并根据从机器人状态得到的期望关节位姿 (q_d) 计算关节层级扭矩命令。这允许使用机器人的内置逆运动学，而不必自己解决。

要实现功能齐全的控制器，必须至少实现继承的虚函数 init 和 update。初始化 - 例如启动位姿 - 应该在 starting 函数中完成，因为在重新启动控制器时会调用 starting，而在加载控制器时只调用一次 init。stopping 方法应包含与关闭相关的功能（如果需要）。

重要

在关闭控制器之前，始终给命令来温和地减速。使用速度接口时，不要在 stopping 时简单地命令速度为零。因为它可能在机器人仍在移动时被调用，所以它相当于命令速度跳跃导致非常高的关节层级扭矩。在这种情况下，保持相同的速度并停止控制器，比发送零值并让机器人处理减速更好。

控制器类必须通过 pluginlib 正确导出，这需要添加：

#include <pluginlib/class_list_macros.h>
// Implementation ..
PLUGINLIB_EXPORT_CLASS(name_of_your_controller_package::NameOfYourControllerClass,
                       controller_interface::ControllerBase)

在 .cpp 文件的末尾。此外，需要定义一个包含以下内容的 plugin.xml 文件：

<library path="lib/lib<name_of_your_controller_library>">
  <class name="name_of_your_controller_package/NameOfYourControllerClass"
         type="name_of_your_controller_package::NameOfYourControllerClass"
         base_class_type="controller_interface::ControllerBase">
    <description>
      Some text to describe what your controller is doing
    </description>
  </class>
</library>

通过添加导出：

<export>
  <controller_interface plugin="${prefix}/plugin.xml"/>
</export>

到 package.xml。此外，需要至少将控制器名称与控制器类型结合加载到 ROS 参数服务器。此外，可以包括需要的其他参数。一个示例 configuration.yaml 文件可能看起来如下所示：

your_custom_controller_name:
  type: name_of_your_controller_package/NameOfYourControllerClass
  additional_example_parameter: 0.0
  # ..

现在，可以使用 ROS control 中的 controller_spawner 节点或通过 hardware_manager 提供的服务调用来启动控制器。只要确保 controller_spawner 和 franka_control_node 在同一个命名空间中运行。有关更多详细信息，请查看 franka_example_controllers package 包或 ROS control 教程。