九州工業大学 CIR-KIT Blog

九工大自律移動ロボット製作プロジェクトCIR-KITの技術系ブログ

No.6-8:GazeboをROSに繋ぐ (ROS Communication 編)

gazebo_logo

検証日時

03/02/2015 (Mon)

概要

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

レベル

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 Communication

Gazeboはユーザにシミュレートされたworldの様々な角度を取得したり修正したりできるROSのAPIを提供しています。以下のセクションでは、シミュレーションのworldと物体を操るための効果について説明します。GazeboのためのROSメッセージとサービスの完全なリストはこのチュートリアルに書いてあります。

必要条件

これから説明する例を実行していきたいなら、RRBotのセットアップは、GazeboにおけるURDFのチュートリアルで述べているということを念頭に置いてください。このチュートリアルでは、いろいろな方法を使ってコーラの缶を蹴ります。
また、Gazeboが以下のコマンドですでにlaunchされていると仮定して話を進めます。:

1
2

roscore &
rosrun gazebo_ros gazebo

ただし、これから紹介するコマンドを打った後に、Gazeboを再起動する必要があるかもしれません。

gazeborosapi_pluginについて

gazeborosパッケージと共に配置されているgazeborosapi_pluginは、gazeboと呼ばれるROSのノードを初期化します。これは、これから説明する様なROSのインターフェースを供給するために、ROSコールバックスケジュラー(message passing)とGazeboのインターナショナルスケジュラーを統合しています。このROSのAPIはユーザーがROSを超えたシミュレーション環境の詳細を扱うのと同じように、環境のモデルの姿勢の内観とスポーンを行うことを可能にします。
また、このプラグインはgzserverによってのみ読み込まれます。

gazeborospaths_pluginについて

２つ目のgazeborospath_pluginという名前のプラグインはGazeboがROSリソースを探す(例えば、ROSパッケージのパス名を解決する)ことを可能にするgazebo rosパッケージの中で利用可能です。
このプラグインはgzserverとgzclientの両方によって読み込まれます。

Gazeboのpublishしたパラメータ

Parameters

名前 型 説明
/usesimtime Bool ROSがpublishされたROS timeのための/clockトピックを使うかどうか定義します

Gazeboは、もしシミュレーション時間が/usertimeパラメータに使用されるべきなら、他のアプリケーション(とりわけRviz)を定義するためにROSパラメータサーバーを使用します。これは、あなたがgazebo_rosを起動した時に、Gazeboが自動で設定すべきことです。
もし、ROSシステムをシミュレーション同期時間と共に提供するために、gazebo_rosがROSの/clockトピックをpublishしているなら、usesimtimeはtrueです。シミュレーション時間についてはROS C++ Timeをご覧ください。

値をクリックする

パラメータに何が設定されているか見るためには、以下のコマンドを実行してください。:

1

rosparam get /use_sim_time

Gazeboが購読するトピック

Topics

名前 型 説明
~/setlinkstate gazebomsgs/LinkState リンクの姿勢(pose/twist)を設定します。
~/setmodelstate gazebomsgs/ModelState モデルの姿勢(pose/twist)を設定します。

トピックを通じたシミュレーションにおけるモデルのPoseとTwist

Topicsはposeの設定動作が完了するのを待つことなく、迅速にモデルのposeとtwistを設定するのに使うことができます。そのようにするために、求められるmodel state messageを/gazebo/setmodelstateトピックにpublishします。例えば、topicsを通じてposeの設定をテストするために、オンラインデータベースからモデルをスポーンさせてコーラの缶をシミュレーションに追加します。:

1

rosrun gazebo_ros spawn_model -database coke_can -gazebo -model coke_can -y 1

そして/gazebo/setmodelstateトピックにpublishすることで、コーラの缶の姿勢を設定します。:

1

rostopic pub -r 20 /gazebo/set_model_state gazebo_msgs/ModelState '{model_name: coke_can, pose: { position: { x: 1, y: 0, z: 2 }, orientation: {x: 0, y: 0.491983115673, z: 0, w: 0.870604813099 } }, twist: { linear: { x: 0, y: 0, z: 0 }, angular: { x: 0, y: 0, z: 0}  }, reference_frame: world }'

すると以下のように、蹴ってくれと言わんばかりに(まあ、今から蹴るんですが)RRBotの前にコーラの缶が浮いているのを確認できるはずです。
ただし、以下のように、RRBotを追加するため、これまでに説明したroslaunchを使っています。:

1

roslaunch rrbot_gazebo rrbot_world.launch

rrbot-test-1

Gazeboがpuvlishするトピック

Topics

名前 型 説明
/clock rosgraphmsgs/Clock シミュレーション時間を/usertimeパラメータと共にpublishします。
~/linkstates gazebomsgs/LinkStates シミュレーション中のすべてのリンクの状態をpublishします。
~/modelstates gazebo_msgs/ModelStates シミュレーション中のすべてのモデルの状態をpublishします。

Topicを使ってモデルとリンクの姿勢を訂正する

Gazeboはworld座標系に関するシミュレーション中の物体のposeとtwistの情報を含む、/gazebo/linkstatesと/gazebo/modelstatesトピックをpublishしています。これらの挙動は、以下のコマンドを実行することで確認できます。

1

rostopic echo -n 1 /gazebo/model_states

もしくは

1

rostopic echo -n 1 /gazebo/link_states

繰り返すには、linkが慣性、外観、衝突プロパティの定義された剛体として定義される必要があります。一方、modelはリンクとジョイントの衝突要素として定義されます。modelの状態は標準的なlinkの状態であります。URDFが木構造で与えられており、モデルのカノニカルリンクは自身のルートリンクによって定義されます。

サービス : シミュレーション中のモデルの削除と作成

以下のサービスでは、シミュレーション中のモデルを削除したり作成したりできます。:

名前 型 説明
~/spawnurdfmodel gazebomsgs/SpawnModel Universal Robotic Description Format (URDF)をスポーンするために、このサービスを使います。
~/spawnsdfmodel gazebomsgs/SpawnModel Gazeboに記述されるSimulmulation Description Format (SDF)モデルをスポーンするために、このサービスを使います。
~/deletemodel gazebomsgs/DeleteModel シミュレーションからモデルを削除することができます。

モデルをスポーンする

spawnmodelと呼ばれるスクリプトはモデルのgazeborosによって呼び出されるスポーンサービスをコールするために提供されています。モデルをサービスコールを使ってスポーンするための最も実践的な方法はroslaunchファイルを使うことです。詳細は、roslaunchを使うというチュートリアルに書いてあります。URDFやSDFをGazeboに追加するためにspawn_modelを使う方法はたくさんあります。以下はそのうちのいくつかの例です。:
URDFをファイルからスポーンする - まず最初に.Xacroファイルを.xmlファイルに変換し、スポーンします。:

1
2

rosrun xacro xacro `rospack find rrbot_description`/urdf/rrbot.xacro >> `rospack find rrbot_description`/urdf/rrbot.xml
rosrun gazebo_ros spawn_model -file `rospack find rrbot_description`/urdf/rrbot.xml -urdf -y 1 -model rrbot1 -robot_namespace rrbot1

roslaunchとxacroを使ったパラメータサーバーからのURDF: roslaunchを使うをご覧ください。
ローカルモデルデータベースからのSDF:

1

rosrun gazebo_ros spawn_model -file `echo $GAZEBO_MODEL_PATH`/coke_can/model.sdf -sdf -model coke_can1 -y 0.2 -x -0.3

オンラインモデルデータベースからのSDF:

1

rosrun gazebo_ros spawn_model -database coke_can -sdf -model coke_can3 -y 2.2 -x -0.3

名前空間、trimeshプロパティ、ジョイントポジション、RPYオリエンテーションを含むspawn_modelのための利用可能な引数をすべて見るには、以下を実行してください。:

1

rosrun gazebo_ros spawn_model -h

モデルを削除する

すでにGazeboの中にあるモデルの削除は、物体に与えたモデルの名前を覚えている限りは簡単です。もし、RRBotを前のセクションで記述したように、rrbot1と名付けているなら、以下のコマンドでそれを削除できます。:

1

rosservice call gazebo/delete_model '{model_name: rrbot1}'

サービス : 状態とプロパティのセッティング

以下のサービスでは、シミュレーション中の物体やシミュレーションについての状態やプロパティの情報を設定することができます。:

名前 型 説明
~/setlinkproperties gazebomsgs/SetLinkProperties
~/setphysicsproperties gazebomsgs/SetPhysicsProperties
~/setmodelstate gazebomsgs/SetModelState
~/setmodelconfiguration gazebomsgs/SetModelConfiguration モデルのジョイントポジションをダイナミクスなしで設定できるようになります。
~/setjointproperties gazebomsgs/SetJointProperties
~/setlinkstate gazebomsgs/SetLinkState
~/setlinkstate gazebomsgs/LinkState
~/setmodelstate gazebomsgs/ModelState

モデルの姿勢を設定する例

/gazebo/setmodelstateサービスを使い、RRBotでコーラの缶を蹴ってみましょう !
もし、まだシミュレーションにコーラの缶を追加していないのなら、以下のコマンドを実行してください。:

1

rosrun gazebo_ros spawn_model -database coke_can -gazebo -model coke_can -y 1

これは、Gazeboもしくはオンラインモデルデータベース(インターネット接続が必要です)を通じて利用可能になるようパッケージされていなければなりません。尚、コーラの缶がどこにあっても問題はありません。
続いて、コーラの缶をRRBotのポジションに動かすためのサービスリクエストをコールしましょう。:

1

rosservice call /gazebo/set_model_state '{model_state: { model_name: coke_can, pose: { position: { x: 0.3, y: 0.2 ,z: 0 }, orientation: {x: 0, y: 0.491983115673, z: 0, w: 0.870604813099 } }, twist: { linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }'

すると、以下のようになるはずです。

rrobt-test-2

続いて、RRBotを以下のコマンドを使って動かしてみましょう。

1

rosservice call /gazebo/set_model_state '{model_state: { model_name: rrbot, pose: { position: { x: 1, y: 1 ,z: 10 }, orientation: {x: 0, y: 0.491983115673, z: 0, w: 0.870604813099 } }, twist: { linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 } } , reference_frame: world } }'

うまく行けば、コーラの缶がどこかに飛んでゆくでしょう。(゜Д゜)ノ”
うまいう行かなかったら、やり直してみましょう。これはチュートリアルなんですから。
ちなみに、以下の動画みたいになります。以下の動画では、缶が転がってます。私も何回かやりましたが、缶が気持ちよく飛んでゆくこともありましたw

< div class=“youtube-container”>

サービス : 状態とプロパティの取得

以下のサービスでは、シミュレーション中の物体やシミュレーションについての状態やプロパティの情報を取得することができます。:

名前 型 説明
~/getmodelproperties gazebomsgs/GetModelProperties シミュレーション中のモデルのプロパティを返します。
~/getmodelstate gazebomsgs/GetModelState シミュレーション中のモデルの状態を返します。
~/getworldproperties gazebomsgs/GetWorldProperties シミュレーションworldのプロパティを返します。
~/getjointproperties gazebomsgs/GetJointProperties シミュレーション中のジョイントのプロパティを返します。
~/getlinkproperties gazebomsgs/GetLinkProperties シミュレーション中のリンクのプロパティを返します。
~/getlinkstate gazebomsgs/GetLinkState シミュレーション中のリンクの状態を返します。
~/getphysicsproperties gazebomsgs/GetPhysicsProperties シミュレーション中で使用される物理エンジンのプロパティを返します。
~/linkstates gazebomsgs/LinkStates world座標の完全なリンクの状態をpublishします。
~/modelstates gazebo_msgs/ModelStates world座標の完全なモデルの状態をpublishします。

注意

Gazebo中のリンクの名前はmodelname::body_nameの名前表記にスコープされます。

モデルの状態を取得する例

さて、すでにコーラの缶を蹴ったことでしょう。今度は、それがどのくらい飛んだのか知りたいですね。これまでの例を使い(同じシミュレーションを実行して)、以下のサービスコールを使ってコーラの缶のposeとtwistを取得しましょう。:

1

rosservice call gazebo/get_model_state '{model_name: coke_can}'

この結果は、ロボットの蹴った具合によって変わりますが、以下のような感じになるはずです。:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

pose:
  position:
    x: -33.4741996929
    y: 1.72965811349
    z: 0.0715116701823
  orientation:
    x: 0.52144600413
    y: 0.478438115917
    z: -0.471063379854
    w: -0.526583636448
twist:
  linear:
    x: 5.74579911953e-07
    y: 0.000217124920081
    z: 9.67915822705e-06
  angular:
    x: -0.0029562892652
    y: 7.77750570372e-06
    z: 4.16767016512e-05
success: True

私のロボットは 33 [m]飛ばしました。あなたのはどうでしたか ?

シミュレーションworldとオブジェクトプロパティを取得する

以下のコマンドで、worldの中のモデルのリスト(ground_plane, coke cane, rrbot)を取得することができます。:

1

rosservice call gazebo/get_world_properties

すると、以下のような結果が得られるはずです。

1
2
3
4
5

sim_time: 59.62
model_names: ['ground_plane', 'rrbot', 'coke_can']
rendering_enabled: True
success: True
status_message: GetWorldProperties: got properties

また、以下のコマンドで特定のモデルの詳細を取得できます。

1

rosservice call gazebo/get_model_properties '{model_name: rrbot}'

すると、以下の様な結果が得られるはずです。

1
2
3
4
5
6
7
8
9

parent_model_name: ''
canonical_body_name: ''
body_names: ['link1', 'link2', 'link3']
geom_names: ['link1_collision', 'link2_collision', 'link3_collision', 'link3_collision_camera_link', 'link3_collision_hokuyo_link']
joint_names: ['fixed', 'joint1', 'joint2']
child_model_names: []
is_static: False
success: True
status_message: GetModelProperties: got properties

サービス : 力のコントロール

以下のサービスでは、シミュレーション中のボディやジョイントへの力とねじりを適用することができます。

名前 型 説明
~/applybodywrench gazebomsgs/ApplyBodyWrench シミュレーション中のボディにねじりを適用します。同じボディに適用されるすべてのアクティブなねじりは累積します。
~/applyjointeffort gazebomsgs/ApplyJointEffort シミュレーション中のジョイントへの力を適用します。同じジョイントに適用されるすべてのアクティブな力は累積します。
~/clearjointforces gazebomsgs/ClearJointForces ジョイントへ適用されている力をクリアします。
~/clearbodywrenches gazebomsgs/ClearBodyWrenches ボディへ適用されているねじりをクリアします。

ねじりをリンクに適用する

Gazeboのボディへのねじりの適用を説明するために、重力をなくして物体をスポーンさせてみましょう。すでにコーラの缶が追加されていることを確認してください。:

1

rosrun gazebo_ros spawn_model -database coke_can -gazebo -model coke_can -y 1

続いて、/gazebo/setphysicspropertiesサービスコールを送り、すべての軸の重力をなくしましょう。:

Then to turn off gravity send a service call to /gazebo/setphysicsproperties with no gravity in any of the axis:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

rosservice call /gazebo/set_physics_properties "
time_step: 0.001
max_update_rate: 1000.0
gravity: 
  x: 0.0
  y: 0.0
  z: 0.0
ode_config: 
  auto_disable_bodies: False
  sor_pgs_precon_iters: 0
  sor_pgs_iters: 50
  sor_pgs_w: 1.3
  sor_pgs_rms_error_tol: 0.0
  contact_surface_layer: 0.001
  contact_max_correcting_vel: 100.0
  cfm: 0.0
  erp: 0.2
  max_contacts: 20"

/gazebo/applybodywerchサービスを呼び、コーラの缶の原点に１秒間、0.01 [Nm]のトルクを適用しましょう。すると、コーラの缶が、X軸に対して正の方向にスピンするのが確認できるはずです。:

1

rosservice call /gazebo/apply_body_wrench '{body_name: "coke_can::link" , wrench: { torque: { x: 0.01, y: 0 , z: 0 } }, start_time: 10000000000, duration: 1000000000 }'

今度は、コーラの缶の原点に1秒間、-0.01 [Nm]のトルクを適用し、缶の回転を止めましょう。:

1

rosservice call /gazebo/apply_body_wrench '{body_name: "coke_can::link" , wrench: { torque: { x: -0.01, y: 0 , z: 0 } }, start_time: 10000000000, duration: 1000000000 }'

通常、負のトルクは、曖昧な定義であることが多いです。すべてのアクティブなねじりをボディに適用するには、以下のようなコマンドを実行してください。:

1

rosservice call /gazebo/clear_body_wrenches '{body_name: "coke_can::link"}'

シミュレーション中のジョイントに力を適用する

ジョイントにトルクを適用するために、/gazebo/applyjointeffortをコールしてください。:

1
2
3
4
5
6
7
8

rosservice call /gazebo/apply_joint_effort "joint_name: 'joint2'
effort: 10.0
start_time:
  secs: 0
  nsecs: 0
duration:
  secs: 10
  nsecs: 0"

するとリンクが回り始めるはずです。
特定のジョイントの力をクリアするためには、以下のコマンドをコールしてください。:

1

rosservice call /gazebo/clear_joint_forces '{joint_name: joint2}'

サービス : シミュレーションコントロール

以下のサービスでは、シミュレーション中の物理エンジンを一時停止したり解除したリアできます。:

名前 型 説明
~/pausephysics stdsrvs/Empty 物理エンジンのアップデートを一時停止します。
~/unpausephysics stdsrvs/Empty 物理エンジンのアップデートを再開します。
~/resetsimulation stdsrvs/Empty モデルのポーズをリセットします。
~/resetworld stdsrvs/Empty 時間を含むすべてのシミュレーションをリセットします。

物理エンジンのアップデートを一時停止する

飛んでゆくコーラの缶のナイスなスクリーンショットを撮りたいとしましょう。いや、そうだと言ってください。そんな時には、以下のコマンドをコールすることで、物理エンジンを一時停止することができます。:

1

rosservice call gazebo/pause_physics

シミュレーションが一時停止するとき、シミュレーション時間が停止し、物体も静的になります。しかし、Gazeboの内部アップデートループ(カスタムのダイナミクスプラグインアップデートなど)は依然として動いており、シミュレーション時間は変わっていません。これは、シミュレーションタイムをアップデートしないことで抑制されているのです。シミュレーションを再開するために、以下のコマンドをコールして物理エンジンを再開してください。:

1

rosservice call gazebo/unpause_physics

次のステップ

Gazeboのための独自のROSプラグインの作り方について学んでください。