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

Contents

  1. Introduction
  2. Writing the code
    1. The code
    2. コードの説明
  3. Compiling the code
  4. あなたのコントローラーをプラグインとして登録

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_interface、pr2_mechanism_modelとpluginlibに依存する必要があります。 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 }

そして同じくディレクトリsrc とsrc/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

コードの説明

  • リアルタイムコントローラーを作るために、我々はpr2_controller_interfaceパッケージにあるcontroller::Controllerクラスから継承するクラスを作りました。このベースクラスのある特定のメソッドをオーバーロードしてください、そうすれば、あなたのコントローラーが状態を変えるとき、Mechanismコントロールがこれらのメソッドを呼び出すでしょう。あなたはinit, starting, update, stoppingメソッドをオーバーロードする必要があります

    • initメソッドはコントローラーがロードされる非リアルタイムで呼ばれるでしょう。

    • startingは、「updateの最初のコールの直前に、リアルタイムで1回呼ばれるでしょう。

    • コントローラーが稼働している間に、updateが（1000Hz）リアルタイムループから周期的に呼び出されます。

    • コントローラーがストップするときstoppingはリアルタイムで、最後のupdateコールの後に呼び出されます。

    ベースクラスでのメソッドの詳細な記述に関して、pr2_controller_interface package documentationをひと目見てください。

  • Jointへのアクセスが、 initメソッドの中に手渡されて、RobotState オブジェクトを通して提供されます。あなたは（名指しで）あなたがgetJointStateメソッドを使ってあなたのinitメソッドで欲するJointをルックアップするべきです。あなたはそれから戻ってきたJointStateから読み取って、そして書き込むかもしれません。どのようにシグナル/インフォメーションにアクセスするべきかについての詳細については同じくpr2_mechanism_modelパッケージドキュメンテーションを見てください。

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_managerは pluginlibをリンク、ロードとコントローラーをスタートすることを運営するために使うことができます。

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と呼ばれるマニフェスト、プラグイン記述ファイルを作る必要があります。フォーマットは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を教えるでしょう。

Wiki: ja/pr2_mechanism/Tutorials/Writing a realtime joint controller (last edited 2016-08-27 05:05:54 by Crescent)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0