(参考教程)（pr2_mechanism/Tutorials/Writing a realtime joint controller - ROS Wiki

Next Tutorial:Running a realtime joint controller

一、提要

ROS中controller它是一个关键的概念组，需要很多时间才能掌握；本教程教你如何在实时循环中编写关节空间控制器。在阅读本文之前，最好先阅读作者之前的文章作为准备知识：ROS-Control专题：PR六个概念01

二、结构介绍

要理解本教程和后续教程，您应该熟悉：

• pr2_controller_interface：它提供了一个代码接口，即如何调用您的代码。
• pr2_mechanism_model：尤其是 JointState，它提供访问关节位置传感器和关节扭矩命令。
• pr2_controller_manager：它通常加载/启动/停止并管理实时代码的执行。

三、定制ROS程序包

First, let's create a package where you'll build your controller. The package needs to depend on thepr2_controller_interface, thepr2_mechanism_modeland thepluginlib. Thecontroller interfacepackage contains the base class for all controllers, thepr2_mechanism_modelprovides access to the robot joints, and thepluginlibpackage allows us to add our own controller as a plugin into thecontroller manager

(1)首先，让我们创建一个包，你将在其中构建你的控制器。

(2)软件包需要依赖:

• pr2_controller_interface
• pr2_mechanism_model
• pluginlib。

(3)控制器接口包括所有控制器的基类，pr2_mechanism_model 访问机器人关节，pluginlib 允许我们在控制器管理器中添加我们的控制器作为插件。

$ roscd ros_pkg_tutorials $ roscreate-pkg my_controller_pkg pr2_controller_interface pr2_mechanism_model pluginlib $ roscd my_controller_pkg

让我们建立新包的所有依赖项：

$ rosmake

四、编写代码

现在，在我们的新包中创建目录 include，然后是 include/my_controller_pkg，启动编辑器，创建一个名字 include/my_controller_pkg/my_controller_file.h 复制并粘贴以下代码：

#include  #include   namespace my_controller_ns{  class MyControllerClass: public pr2_controller_interface::Controller { private:   pr2_mechanism_model::JointState* joint_state_;   double init_pos_;  public:   virtual bool init(pr2_mechanism_model::RobotState *robot,                    ros::NodeHandle &n);   virtual void starting();   virtual void update();   virtual void stopping(); }; } 

并创建一个目录 src 和一个名为 src/my_controller_file.cpp 文件。粘贴以下代码：

#include "my_controller_pkg/my_controller_file.h" #include   namespace my_controller_ns {   /// Controller initialization in non-realtime bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,                             ros::NodeHandle &n) {   std::string joint_name;   if (!n.getParam("joint_name", joint_name))   {     ROS_ERROR("No joint given in namespace: '%s')",               n.getNamespace().c_str());     return false;   }    joint_state_ = robot->getJointState(joint_name);   if (!joint_state_)   {     ROS_ERROR("MyController could not find joint named '%s'",               joint_name.c_str());     return false;   }   return true; }   /// Controller startup in realtime void MyControllerClass::starting() {   init_pos_ = joint_state_->position_; }   /// Controller update loop in realtime void MyControllerClass::update() {   double desired_pos = init_pos_   15 * sin(ros::Time::now().toSec());   double current_pos = joint_state_->position_;   joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos); }   /// Controller stopping in realtime void MyControllerClass::stopping() {} } // namespace 

五、代码原理

my_controller_ns是自己创建的一个realtime-controller类，我们继承controller::Controller基础类，建立自己的类别；基类controller::Controller在pr2_controller_interface包中；通过函数超载实现自己的功能，这些功能是Mechanism-Control检查状态变化后调用四个函数：

1）init() 2)starting 3)update 4)stopping()

• 非实时调用控制器时 init 方法。
• 当控制器启动时，starting 在第一次调用更新之前，将实时调用一次。
• 当控制器运行时，实时循环将定期(1万 Hz）调用更新。
• 最后一次更新调用后，当控制器停止时，停止实时调用。

详细说明基类中的方法，请查看pr2_controller_interface package documentation.(包文档)。

&bsp;     通过传递给 init 方法的 RobotState 对象提供对关节的访问。您应该使用 getJointState 方法在您的 init 方法中查找（按名称）您想要的关节。然后，您可以读取和写入返回的 JointState。有关如何访问信号/信息的详细信息，另请参阅 pr2_mechanism_model ( 包文档)。

六、编译代码

现在我们创建了代码，让我们先编译它。打开 CMakeLists.txt 文件，并在底部添加以下行：

rosbuild_add_library(my_controller_lib src/my_controller_file.cpp)

try to build your package:

$ make

如果一切顺利，您的 lib 文件夹中应该有一个名为 (lib)my_controller_lib.so 的库文件。

七、注册你的控制插件

        现在编译为库的代码需要在实时进程中运行。这种链接可以以两种方式发生：(a) 自动，当实时过程开始时。当该过程开始时，它将搜索并加载您的库。或者 (b) 在进程运行时手动进行。您可以明确要求该过程动态加载您的库。然而，对于这两种情况，都需要将代码指定为用于实时进程，即需要将其注册为插件。然后 pr2_controller_manager 可以使用插件库来管理控制器的链接、加载和启动。

        为了使控制器对 pr2_controller_manager、pluginlib 和实时进程可见，包含控制器的包必须将其导出为可加载类。首先要做的是从 src/my_controller_file.cpp 文件中调用插件注册宏。将以下行添加到该文件的底部：

/// Register controller to pluginlib
PLUGINLIB_DECLARE_CLASS(my_controller_pkg,MyControllerPlugin, 
                         my_controller_ns::MyControllerClass, 
                         pr2_controller_interface::Controller)

再次重新编译:

$ make

        接下来，要导出控制器，您还必须依赖 pr2_controller_interface 和 pluginlib 包，并在导出部分引用插件描述文件。在第 2 节的包创建过程中自动记录了依赖关系，但您需要在 <\package> 范围内将以下导出语句显式添加到 manifest.xml：

所有的manifest.xml 看起来像：

  ...
  
  
  ...
  
    
  
  ...

        最后，您需要创建插件描述文件，清单中称为 controller_plugins.xml。该格式在插件库文档中进行了描述。对于我们的控制器，我们需要在 controller_plugins.xml 中进行以下描述：

        现在，让我们确保控制器已正确配置为插件并且对 Mechanism Control 可见。检查插件描述文件是否对 rospack 可见：

$ rospack plugins --attrib=plugin pr2_controller_interface

您本地的 controller_plugins.xml 文件应该在此列表中。如果不是，那么您可能没有添加 pr2_controller_interface 作为包的依赖项。

现在您已准备好学习下一个教程，如何 to run your controller.