[Documentation] [TitleIndex] [WordIndex

Note: This tutorial assumes that you have completed the previous tutorials: ROS tutorials.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a realtime joint controller

Description: このチュートリアルはあなたに ja/pr2_controller_managerのリアルタイムループで実行されることができるJointスペースコントローラーを書く方法を教えます

Tutorial Level: BEGINNER

Next Tutorial: Running a realtime joint controller

Introduction

後続のチュートリアルを理解するために、あなたは以下のドキュメンテーションを知っている必要があります:

  1. ja/pr2_controller_interface, それはコードインタフェースを提供する、すなわちあなたのコードが呼び出されるであろう方法

  2. ja/pr2_mechanism_modelと特にJointState、それはJointトルクコマンドとJoint位置センサーへのアクセスを提供します。

  3. ja/pr2_controller_manager, それはロード/スタート/ストップ、 リアルタイムコードの実行を通常管理します。

Writing the code

controllers_1.png

最初に、あなたがあなたのコントローラーを作るであろうパッケージを作成しましょう。 パッケージはpr2_controller_interfacepr2_mechanism_modelpluginlibに依存する必要があります。 controller interfaceパッケージはすべてのコントローラーに対するベースクラスを含みます、pr2_mechanism_modelはロボットJointへのアクセスを提供します、そしてpluginlibパッケージは我々がcontroller managerの中に我々自身のコントローラーをプラグインとして加えることを可能にします

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

そして我々のすでに新しいパッケージのすべての依存をビルドしましょう:

$ rosmake

The code

今、我々の新しいパッケージの中に、includeディレクトリを作成し、それからあなたのエディタをたち上げてinclude/my_controller_pkg,include/my_controller_pkg/my_controller_file.hと呼ばれるファイルを作成して、そして、このファイルの中に対して次のコードをコピーペーストします:

   1 #include <pr2_controller_interface/controller.h>
   2 #include <pr2_mechanism_model/joint.h>
   3 
   4 namespace my_controller_ns{
   5 
   6 class MyControllerClass: public pr2_controller_interface::Controller
   7 {
   8 private:
   9   pr2_mechanism_model::JointState* joint_state_;
  10   double init_pos_;
  11 
  12 public:
  13   virtual bool init(pr2_mechanism_model::RobotState *robot,
  14                    ros::NodeHandle &n);
  15   virtual void starting();
  16   virtual void update();
  17   virtual void stopping();
  18 };
  19 } 

そして同じくディレクトリsrcsrc/my_controller_file.cppと呼ばれるファイルを作ってください。このファイルの中に次のコードをコピーペーストします:

   1 #include "my_controller_pkg/my_controller_file.h"
   2 #include <pluginlib/class_list_macros.h>
   3 
   4 namespace my_controller_ns {
   5 
   6 
   7 /// Controller initialization in non-realtime
   8 bool MyControllerClass::init(pr2_mechanism_model::RobotState *robot,
   9                             ros::NodeHandle &n)
  10 {
  11   std::string joint_name;
  12   if (!n.getParam("joint_name", joint_name))
  13   {
  14     ROS_ERROR("No joint given in namespace: '%s')",
  15               n.getNamespace().c_str());
  16     return false;
  17   }
  18 
  19   joint_state_ = robot->getJointState(joint_name);
  20   if (!joint_state_)
  21   {
  22     ROS_ERROR("MyController could not find joint named '%s'",
  23               joint_name.c_str());
  24     return false;
  25   }
  26   return true;
  27 }
  28 
  29 
  30 /// Controller startup in realtime
  31 void MyControllerClass::starting()
  32 {
  33   init_pos_ = joint_state_->position_;
  34 }
  35 
  36 
  37 /// Controller update loop in realtime
  38 void MyControllerClass::update()
  39 {
  40   double desired_pos = init_pos_ + 15 * sin(ros::Time::now().toSec());
  41   double current_pos = joint_state_->position_;
  42   joint_state_->commanded_effort_ = -10 * (current_pos - desired_pos);
  43 }
  44 
  45 
  46 /// Controller stopping in realtime
  47 void MyControllerClass::stopping()
  48 {}
  49 } // namespace
  50 

コードの説明

Compiling the code

我々が作ったコードを今、まずコンパイルしましょう。 CMakeLists.txtファイルを開いて、そして一番下に次のラインを加えてください:

rosbuild_add_library(my_controller_lib src/my_controller_file.cpp)

そして、あなたのパッケージをビルドしてください:

$ make

もしすべてがうまくいったなら、あなたはあなたのlibフォルダーに(lib)my_controller_lib.soと呼ばれるライブラリファイルを持っているべきです。

あなたのコントローラーをプラグインとして登録

controllers_2.png 今ライブラリとして編集されたコードはリアルタイムプロセスの中に走る必要があります。この連結は2つの方法で起こることができます:(a)自動的に、リアルタイムとしてプロセスがスタートします。プロセスがスタートするとき、それはあなたのライブラリを捜して、そしてロードするでしょう。あるいは(b)手作業で、プロセスが走っているとき、あなたは明示的にプロセスにその場その場であなたのライブラリをロードするように求めることができます。しかしながら、両方のケースに関して、コードはリアルタイムプロセスとして定められて取っておかれる必要があります、すなわち、それはプラグインとして登録される必要があります。それからpr2_controller_managerpluginlibをリンク、ロードとコントローラーをスタートすることを運営するために使うことができます。

pr2_controller_managerpluginlibとリアルタイムプロセスにコントローラーを可視にするために、コントローラーを含んでいるパッケージはそれをロード可能なクラスとして書き出さなくてはなりません。最初にすることはあなたの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_interfacepluginlibパッケージに頼って、そしてエクスポートのセクションでプラグイン記述ファイルを参照しなくてはなりません。依存は第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と呼ばれるマニフェスト、プラグイン記述ファイルを作る必要があります。フォーマットはpluginlib ドキュメンテーションで記述されます。我々のコントローラーのために我々は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 をあなたのパッケージの依存として追加しませんでした。

あなたが次のチュートリアルのために準備ができている今、あなたに方法 run your controllerを教えるでしょう。


2019-12-07 12:42