目录

一、提要

二、结构介绍

三、定制ROS程序包

四、代码编写

五、代码的原理

六、编译代码

七、注册你的控制插件

一、提要

       ROS中controller是个关键的概念群，掌握起来需要一大片时间；本教程教你如何编写一个可以在实时循环中执行的关节空间控制器。在阅读本文之前，最好是先看作者前面的文章做为预备知识： ROS-Control专题：PR2的六个概念【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 the pr2_controller_interface, the pr2_mechanism_model and the pluginlib. The controller interface package contains the base class for all controllers, the pr2_mechanism_model provides access to the robot joints, and the pluginlib package allows us to add our own controller as a plugin into the controller 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 <pr2_controller_interface/controller.h>
#include <pr2_mechanism_model/joint.h>

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 <pluginlib/class_list_macros.h>

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 将被实时调用一次，就在第一次调用更新之前。
• 当控制器运行时，实时循环会定期（1000 Hz）调用更新。
• 当控制器停止时，停止实时调用一次，就在最后一次更新调用之后。

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

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

六、编译代码

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

    rosbuild_add_library(my_controller_lib src/my_controller_file.cpp)

        进入你的包目录:

$ 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> <package> 范围内将以下导出语句显式添加到 manifest.xml：

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

所有的manifest.xml 看起来像：

<package>
  ...
  <depend package="pr2_controller_interface" />
  <depend package="pluginlib" />
  ...
  <export>
    <pr2_controller_interface plugin="${prefix}/controller_plugins.xml" />
  </export>
  ...
</package>

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

<library path="lib/libmy_controller_lib">
  <class name="my_controller_pkg/MyControllerPlugin" 
         type="my_controller_ns::MyControllerClass"           
         base_class_type="pr2_controller_interface::Controller" />
</library>

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

$ rospack plugins --attrib=plugin pr2_controller_interface

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

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

最后

以上就是怕黑宝马为你收集整理的ROS-Control专题：写一个实时关节控制器（ realtime joint controller）一、提要二、结构介绍三、定制ROS程序包四、代码编写五、代码的原理七、注册你的控制插件的全部内容，希望文章能够帮你解决ROS-Control专题：写一个实时关节控制器（ realtime joint controller）一、提要二、结构介绍三、定制ROS程序包四、代码编写五、代码的原理七、注册你的控制插件所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。