No.6-7:GazeboをROSに繋ぐ (ROS Control 編) - 九州工業大学 CIR-KIT Blog

gazebo_logo

検証日時

03/02/2015 (Mon)

概要

Gazeboのチュートリアル第六弾「Connect to ROS」。 ROSはロボットの制御フレームワークです。このチュートリアルでは、GazeboとROSのインターフェースについて説明します。今回はその「ROS control」編です。公式サイトを適当に翻訳しただけですので、あしからず。

レベル

INTERMEDIATE

環境


PC	:	Lenovo ThinkPad X240
Prosessor	:	Intel Core i7-4600U (2.10GHz, 4MB, 1600MHz)
RAM	:	PC3-12800 DDR3L (8GB)
OS	:	Ubuntu 14.04 LTS 64bit
Kernel	:	3.13.0-44-generic
Gazebo	:	Version 5.0.1

参考

GAZEBO Tutorial-Connect to ROS

チュートリアル : ROS Control

このチュートリアルでは、あなたのロボットのジョイントを動かすために、シミュレートされたコントローラを設定してゆきます。これにより、MoveIt!のようなプランナーのための正しいROSのインターフェースを提供することができるようになります。また、ROSの新しいスタンダードコントローラインターフェースであるros_controlパッケージを使用してゆきます。

`ros_control`について

次に進む前に、ros_controlドキュメントの概要を読んでおくことをおすすめします。 About ros_control

Gazeboと`ros_control`のデータフロー

Gazeboでロボットのコントローラをシミュレートすることで、ros_controlの使い方とシンプルなGazeboのプラグインアダプタの使い方を習得することができます。シミュレーション、ハードウェア、コントローラとトランスミッションの間の関係の概要は以下のようになっています。尚、以下の画像は公式サイトのものです。: Gazebo_ros_transmission

必要条件

このチュートリアルでは、これまでのチュートリアルで紹介したコンセプトの多くを再編してゆきます。プラグイン構成のための例として、GazeboにおけるURDFのチュートリアルの中で設定したRRBotを再び使用してゆきます。
すでにros_controlとros_controllersをインストールしていることを確認してください。また、それらの依存関係は、Gazebo_ros_packageをインストールするで説明しています。

使い方

URDFに`transmission`要素を追加する

あなたのロボットでros_controlを使用するには、URDFにいくつかの追加要素が必要です。<transmission>要素はリンクアクチュエータやジョイントで使用されます。正確なXMLフォーマットのためには、<transmission>の仕様をご覧ください。
実行途中のgazebo_ros_controlの目的のために、transmissionタグの中で唯一重要な情報は以下のとおりである。:

<joint name=""> : URDFにあるジョイントの名前と一致していなければなりません。
<type> ; transmissionのタイプです。現在では、transmission_interface/SimpleTransmissionのみが実行されます。(遠慮無く追加してください)
<hardwareInterface> : <actuator>タグでは、gazebo_ros_controlプラグインに姿勢、速度、実行インターフェースなどをロードするためのハードウェアインターフェースを知らせます。現在では、実行インターフェースのみが実行されます。(遠慮無く追加してください)

残りの名前と要素は現在は、無視されています。

`gazebo_ros_control`プラグインを追加する

transmissionタグへの追加では、正確にtransmissionタグを記述し、適用するハードウェアインターフェースとコントローラマネージャを読み込む、URDFにGazeboのプラグインを追加する必要があります。デフォルトのgazebo_ros_controlプラグインはシンプルですが、ros_controlとGazeboの間のカスタムのロボットハードウェアインターフェースを使えるようにするために、追加のプラグインを展開することができます。
デフォルトプラグインXMLをあなたのURDFに追加する必要があります。

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
  </plugin>
</gazebo>

gazebo_ros_controlの<plugin>タグには以下の子要素もあります。:

<robotNamespace> : プラグインの初期化に使用される名前空間です。デフォルトでは、URDF/SDFのロボット名となります。
<controlPeriod> : コントローラアップデートの周期です。デフォルトでは、Gazeboの手記です。
<robotParam> : パラメータサーバーのrobot_description(URDF)のロケーションです。デフォルトでは、/robot_descriptionです。
<robotSimType> : 使用されるカスタムのロボットシミュレーションインターフェースのpluginlibの名前です(詳しくは、続きをご覧ください)。デフォルトでは、DefaultRobotHWSimとなります。

デフォルトの`gazebo_ros_control`の挙動

デフォルトで、<robotSimType>タグなしだと、gazebo_ros_controlはURDF 外のros_control-basedコントローラと通信するのに必要なすべての情報を手に入れようとします。これは、ほとんどの場合十分なことで、少なくとも、始めの内は良いことです。
デフォルトの挙動では、以下のros_controlインターフェースを提供しています。:

hardware_interface::JointStateInterface
hardware_interface::EffortJointInterface
hardware_interface::VelocityJointInterface - not fully implemented

発展 : カスタムの`gazebo_ros_control`シミュレーションプラグイン

gazebo_ros_controlのGazeboプラグインは、より複雑なメカニズム(非線形バネ、リンクゲージ、etc…)のためのros_controlとGazebo間のカスタムインターフェースを実行するため、pluginlib-basedのインターフェースも提供しています。
そういったプラグインでは、シミュレートされているros_control hardware_interface::RobotHWを動かすgazebo_ros_control::RobotHWSimを継承していなければなりません。RobotHWSimでは、Gazeboシミュレータの中で、ジョイントプロパティを読み、命令するためのAPIレベルのアクセスを提供します。
それぞれのRobotHWSimのサブクラスはURDFモデルで定義されており、ロボットモデルが読み込まれた時に読み込まれます。例えば、以下のXMLはデフォルトプラグインを読み込みます。(<robotSimType>タグを使っていない時と同じ挙動です。):

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
</gazebo>

RRBotの例

Gazeboので動作させたいジョイントのために、以下<transmission>ブロックシミュレータに追加します。rrbot.xacroファイルを開くと、最後の方に、以下のようなソースコードを確認できるはずです。:

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1"/>
    <actuator name="motor1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint2"/>
    <actuator name="motor2">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

すべての<transmission>タグを読むrrbot.gazeboの中に、gazebo_ros_controlプラグインも確認できます。:

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/rrbot</robotNamespace>
  </plugin>
</gazebo>

`ros_controls`パッケージを作る

続いて、Gazeboと通信するros_contorlコントローラのためのconfigurationファイルとlaunchファイルを作る必要があります。

新しいパッケージを作る

cd ~/catkin_ws
catkin_create_pkg MYROBOT_control ros_control ros_controllers
cd MYROBOT_control
mkdir config
mkdir launch

`.yaml`のconfigファイルを作る

PIDゲインとコントローラ設定はroslaunchファイルを通じてparamサーバによって読み込まれるyamlファイルに保存しなければなりません。あなたのMYROBOT_controlパッケージのconfigフォルダで、以下のRRBotの例をあなたのロボットに適用してください。

rrbot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

これらのコントローラについては、次のセクションをご覧ください。

roslaunchファイルを作る

ros_controlコントローラを起動するためのroslaunchファイルを作ってください。launchフォルダで、MYROBOT/_contorol.launchファイルを作り、以下のRRBotの例をあなたのロボットに適用してください。:

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/rrbot" args="joint1_position_controller joint2_position_controller joint_state_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/rrbot/joint_states" />
  </node>

</launch>

説明

第一行目の、rosparamでは、パラメータサーバーにyamlのconfigurationファイル(次のセクションで説明します)を読み込むことでコントローラ設定を読み込んでいます。
controller_spawnerノードは、RRBotのための2つのジョイントポジションコントローラをros_controlコントローラマネージャのためのサービスコールを生成するpythonスクリプトを実行することで起動します。サービスコールでは、あなたの望むコントローラのコントローラマネージャを呼び出します。また、hardware_interfacesとすべてのジョイントの姿勢をpublishし、/joint_statesトピックを配信する三番目のコントローラも呼び出します。スポナーはroslaunchとの仕様のためのスクリプトを補助しているだけです。
最後の行は、joint_state_controllerからの/joint_statesメッセージをlistenするrobot_state_publisherノードを起動し、起動したら、座標変換を/tfへpublishします。これにより、Rvizでシミュレートされるロボットを他のタスクがしているのと同じように観察することができます。

roslaunchを使ってコントローラを起動する

以下のコマンドを実行し、ros_controlでコントロールされるRRBotをテストしてください。
RRBotシミュレーションを起動する。:

roslaunch rrbot_gazebo rrbot_world.launch

2つ目のlaunchファイルを実行し、2つのジョイントのためのコントローラを読み込んでください。

roslaunch rrbot_control rrbot_control.launch

尚、個々でエラーが出る場合は、ros_controlとros_controllerがインストールされていない可能性があります。*Indigoを使用している場合は、以下のようにしてこれらをインストールしましょう。:

sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controller

サービスコールをマニュアルで使う

rrbot_control.yamlファイルをパラメータサーバーで読み込んだら、サービスリクエストを通じてコントローラをマニュアルで読み込むことができます。いつものroslaunchと同じように、これらをインクルードします。:
コントローラを読み込む:

rosservice call /rrbot/controller_manager/load_controller "name: 'joint1_position_controller'"
rosservice call /rrbot/controller_manager/load_controller "name: 'joint2_position_controller'"

コントローラを起動する:

rosservice call /rrbot/controller_manager/switch_controller "{start_controllers: ['joint1_position_controller','joint2_position_controller'], stop_controllers: [], strictness: 2}"

コントローラを停止する:

rossevice call /rrbot/controller_manager/switch_controller "{start_controllers: [], stop_controllers: ['joint1_position_controller','joint2_position_controller'], strictness: 2}"

尚、これらひと通りの手順は、Gazeboとサービスを通じて通信する場合の例です。これを行うには、上記の通り、launchファイルにてrrbot_control.yamlを読み込んでおく必要があります。詳しくは、ROSのサービスとパラメータを理解するをご覧ください。

マニュアルでコマンドの例を送る

ジョイントコマンドをテストとして以下のコマンドで送ります。:

rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

もちろん、上記のコマンドを送る前に、以下のコマンドでシミュレーションとコントローラが起動していることが前提です。:

roslaunch rrbot_gazebo rrbot_world.launch
roslaunch rrbot_control rrbot_control.launch

コマンドを送ると、以下のように、２つのジョイントが一定の場所で静止するはずです。 rostopic-test-1

コマンドの送信のためのRQTを使う

このセクションでは、あなたのコントローラのパフォーマンスを視覚的に表示し、コントローラが持つであろうゲイン(一部にはPIDゲインも)やパラメータを調整するのを助けるツールについて見てゆきます。また、ROSのplugin-basedなユーザーインターフェースであるRQTを使います。なので、まずはそれがインストールされていることを確認しておいてください。
シミュレーションとros_controlを起動したまま、RQTを起動してください。:

rosrun rqt_gui rqt_gui

すると、以下のような画面が開いたはずです。 rqt_gui-1

コマンドpublisherを追加する

以下キャプチャ画面を参考にして、Publisherプラグインを追加し、あなたがpublishしたいそれぞれのコントローラを命令するドロップダウンボックスからトピックを選択してください。RRBotの場合、下記のコントローラを追加してください。:

/rrbot/joint1_position_controller/command

rqt_gui-2 rqt_gui-3

完了したら、緑色のプラスの形をしたボタンを押してください。
トピック名の左にあるチェックボックスをクリックし、トピックpublisherを有効にしてください。
できたら、rateの値を100に設定してください。(これはコマンドを送る周波数です。つまり、今回は100 [Hz]ということです)
続いて、データを生の状態で確認するために、トピックを実行してください。生データが再生されている途中で、join1のexpressionのラジアンの値を変えてみてください。ただし、 RRBotの例ではジョイントは連続であるので、joint limitsは存在しませんので、どんな値を入れても動きます。もしこのチュートリアルを行っているなら、RRBotをいろいろな位置に振ることができるようになるはずです。
続いて、同じ状態のボックスの中で、サイン波を使って自動で値を変えます。expressionの部分を以下のようにしてみてください。:

sin(i/100)

うまく行くと、以下の動画のようになっているはずです。

さらなる応用的なコントロールのために、ロボットの正確なexpressionにサイン波をpublishするよう構成することができます。:

sin(i/rate*speed)*diff + offset

ただし、変数については、以下のとおりです。:

i : RQTの時間変数
rate : 式が評価される周波数です。これはトピックpublisherの周波数と同じであるべきです。おすすめの値は100です。
speed : ジョイントをどれくらいの早さで動かすのかという指標です。1から始めるとゆっくりと動き始めます。
upper_limit と lower_limits : コントローラによってコントロールされる、ハードウェアのjoint limitsです。
diff = (upper_limit - lower_limit)/2
offset = upper_limit-diff

コントローラのパフォーマンスを閲覧する

先ほどと同じ手順で、プロットプラグインをRQTに追加し、上記で選んだトピックpublisherと同じトピックを追加します。:

/rrbot/joint1_position_controller/command/data

緑色の追加ボタンをクリックしてください。すると、サイン波がスクリーン上にプロットされるのが確認できるはずです。
スクリーンは以下のようになっているはずです。
また、他のデータもプロットしてみてください。例として、目標値との偏差をプロットするには、以下のトピックを追加します。:

/rrbot/joint1_position_controller/state/error

ここまでできたら、以下の様なグラフが表示されるはずです。尚、以下の画像では、プロットの表示範囲を時間軸方向に広げることで、見やすくしています。
rqt_gui-4

注意 : RQTのプロットプラグインは、起動した後、しばらく(>1 min)バグが発生することが確認されています。直線が描かれるだけのバグです。現在の解決方法は、プラグイン画面の右上にある、青いリフレッシュボタンを押すことです。

PIDゲインを調整する

最終的に、あなたのロボットに適用可能と考えられる、適切で独創的であり、統合されたPIDコントローラゲインのチューニングを行うために、dynamic reconfigureを使います。
これまでと同じように、以下の様なPIDゲインのコントロールウィンドウを表示するために、Dynamic ReconfigureプラグインをRQTに追加し、サブオプションを見るために、Expand Allをクリックしてください。コントローラにPIDを使うなら、pidオプションを使ってください。以下のように、5段階のスライダをクリックして、コントローラを調節します。コントローラのパフォーマンスが望みのものとなるまで、値を調整してください。

rqt_gui-5

RQTの図を保存するために、roslaunchを使う

RRBotのpre-configuredなRQTの図は以下のコマンドで簡単にlaunchすることができます。

roslaunch rrbot_control rrbot_rqt.launch

これは、あなたのロボットに対しても、テンプレートとして使うことができます。

RvizをGazeboシミュレーションにつなぐ

シミュレーション中のロボットにコマンドを送るために、ros_controlを使う時に、Gazeboからのロボットの姿勢を読むために、ros_controlジョイントステートコントローラを使用することもできます。良いシミュレータの前提的なアイデアはシミュレーションの中でするのと同じように、本物のハードウェア上で同じソフトウェアが使えるということです。そのスタートとなるのは、本物のハードウェアでも同じように使用する、Rviz上でシミュレートされるあなたのロボットを可視化することです。
これまで説明してきたような、joint_state_controllerを、rosparamやroslaunchファイルで起動する準備ができているとするなら、次のステップは、Rvizを起動することです。:

rosrun rviz rviz

起こりうるエラーを解決するために、Global OptionsにあるFixed Frameをworldに変更してください。
次にRobotModelディスプレイタイプをRvizに追加すると、以下のように、GazeboでシミュレートしているロボットがRvizでも見れるはずです！
尚、以下の例ではカメライメージとレーザースキャンのイメージを追加しています。

デモコード

このチュートリアルにあるRRBotで使用されているコードの例はgazebo_ros_demosレポジトリで使用可能です。

次のステップ

ROS Communicationというチュートリアルの中でGazeboとともに使うことのできるROSメッセージコールとサービスコールについて学んでください。