Updated on: 2018-02-27
本セクションでは、移動型ロボットのURDF作成で作成したURDFを拡張して、Gazebo上のシミュレーションに必要な情報を追加します。
Gazeboは主にSDF(Simulation Description Format)というフォーマットでモデルやシミュレートされる世界を定義します。 でも、少しの情報の追加でURDFもGazeboで利用可能です。
URDFをシミュレータで利用するために、物理的なデータ及びアクチュエータの情報が必要です。
必要な物理的なデータは慣性の用法です。他の物理的なデータも利用可能ですが必須ではないのでここで定義しません。
衝突データも必要です。
前セクションで追加したので本セクションで利用するmobile_robot.urdfにすでにあります。
作成するURDFになければ、Gazeboに利用する前に衝突データも各リンクに追加しないとなりません。
物理的なデータはリンク毎に必要です。 そのリンクの質量と慣性モーメントを指定します。
質量はキログラムとして指定し、重心と友に指定します。 質量を指定するとき、その大きさに注意することが必要です。 小さすぎるとシミュレータの物理計算がおかしくなることがあります。 モデルの一部が消えたり、モデルが高速であちらこちら飛びたりします。 もしこのような現象を見かけたら、まずは質量を増やすことを試せばいいです。
慣性モーメントは下記のような行列として指定します。
1
2
3
ixx ixy ixz
ixy iyy iyz
ixz iyz izz
複製のデータが入っているので、6個の数字で指定できます。
慣性のデータはここの関数で計算できます。 MeshLabやSolidWorksのようなCADソフトウェアも慣性データの計算ができます。 MeshLabでの計算方法はここに参照してください。 下記ですでに計算した慣性データを利用します。
慣性データがない場合は、ディフォルトの単位行列になります。
これは0.1 m×0.1 m×0.1 m の600 kgの箱になるので、けして慣性データを指定しなくてはいけません。
計算したくなければ、ixxとiyyとizzを1e-3にしてixyとixzとiyzを0にすると悪くありません。
0.1 m×0.1 m×0.1 m の0.6 kgな箱になります。
inertial要素の原点を指定すれば、リンクの重心がその点になります。
下記のように、mobile_robot.urdfの各リンクに慣性データを追加します。
base_link:
1
2
3
4
5
6
7
8
9
10
<link name="base_link">
<!-- 省略 -->
<inertial>
<origin xyz="0 0 0.013" rpy="0 0 0"/>
<mass value="0.25"/>
<inertia ixx="0.0003350833" ixy="0" ixz="0"
iyy="0.000560417" iyz="0"
izz="0.000478833"/>
</inertial>
</link>
caster_support:
1
2
3
4
5
6
7
8
9
10
<link name="caster_support">
<!-- 省略 -->
<inertial>
<origin xyz="-0.009725 0 -0.00097398" rpy="0 0 0"/>
<mass value="0.1"/>
<inertia ixx="2.677264e-5" ixy="0" ixz="0"
iyy="7.476213e-5" iyz="0"
izz="5.655266e-5"/>
<kinertial>
</link>
caster_wheel:
1
2
3
4
5
6
7
8
9
<link name="caster_wheel">
<!-- 省略 -->
<inertial>
<mass value="0.1"/>
<inertia ixx="1.02083e-5" ixy="0" ixz="0"
iyy="1.02083e-5" iyz="0"
izz="2e-5"/>
</inertial>
</link>
right_wheel:
1
2
3
4
5
6
7
8
9
<link name="right_wheel">
<!-- 省略 -->
<inertial>
<mass value="0.1"/>
<inertia ixx="1.49333e-5" ixy="0" ixz="0"
iyy="1.49333e-5" iyz="0"
izz="2.88e-5"/>
</inertial>
</link>
left_wheel:
1
2
3
4
5
6
7
8
9
<link name="left_wheel">
<!-- 省略 -->
<inertial>
<mass value="0.1"/>
<inertia ixx="1.49333e-5" ixy="0" ixz="0"
iyy="1.49333e-5" iyz="0"
izz="2.88e-5"/>
</inertial>
</link>
laser:
1
2
3
4
5
6
7
8
9
<link name="laser">
<!-- 省略 -->
<inertial>
<mass value="0.6"/>
<inertia ixx="1e-3" ixy="0" ixz="0"
iyy="1e-3" iyz="0"
izz="1e-3"/>
</inertial>
</link>
これからGazeboを起動しロボットモデルを確認します。
Gazeboを起動してモデルのインスタンスを作成するlaunchファイルを作ります。
下記でmobile_robot_descriptionパッケージ内にlaunchファイルを作成します。
1
2
3
4
$ cd ~/urdf_ws/src/mobile_robot_description/
$ mkdir launch
$ cd launch
$ touch mobile_robot_simulation.launch
mobile_robot_simulation.launchをエディターで開いて、下記の内容を入力します。
1
2
3
4
5
6
7
<launch>
<param name="robot_description"
textfile="$(find mobile_robot_description)/urdf/mobile_robot.urdf"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch"/>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description -urdf -model mobilerobot"/>
</launch>
行ごとに内容を説明します。
<param name="robot_description" textfile="$(find mobile_robot_description)/urdf/mobile_robot.urdf"/>robot_descriptionはROSでよく利用されるパラメータです。
ロボットのURDFファイルを持ち、様々なノード等で利用されます。
例えば、rvizのロボット可視化の機能は自動的にrobot_descriptionを読み込みます。
MoveIt!やナビゲーションスタックも利用します。<include file="$(find gazebo_ros)/launch/empty_world.launch"/><node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model mobilerobot"/>robot_descriptionパラメータから取得します。roslaunchでlaunchファイルを起動します。
1
2
$ cd ~/urdf_ws
$ roslaunch mobile_robot_description mobile_robot_simulation.launch

Gazeboでモデルの様々な情報は見えます。 物理的なデータの確認に特に便利です。 「View」→「Wireframe」の選択の上で「View」→「Joints」を選択すると下記のようにロボットのジョイントが可視化できます。

「View」→「Center of mass」でリンク毎の重心が可視化できます。

そして、「View」→「Inertia」で慣性モメントが可視化できます。 紫の箱はリンクとかなり違う形やかなり違う大きさであれば、慣性データに間違いがあるかもしれません。

このソースは以下のURLでダウンロード可能です。
下記のように自分のワークスペースに入れて利用できます。
1
2
3
4
5
6
$ cd ~/urdf_ws/src
$ git clone https://github.com/gbiggs/rosjp_urdf_tutorial_packages
$ cd rosjp_urdf_tutorial_packages
$ git checkout mobile_robot_inertial_data
$ cd ~/urdf_ws
$ catkin_make
シミュレータでロボットモデルを見ることだけだとあんまり意味ありません。
rvizで同じことが可能だし、Gazeboよりrvizの方が軽いです。
ということで、シミュレータでロボットモデルを本物のロボットと同様に制御したりしたいです。
そうするために、シミュレータのロボットに本物のロボットと同じようなアクチュエータとセンサーを追加することが必要です。
シミュレータようのロボットモデルを制御可能にすることに2つの目的があります。
ジョイントの制御によってシミュレータ上でロボットを動かすことと、ジョイントの位置情報の出力によってrviz等でロボットの状態を可視化すること
アクチュエータの制御インターフェースを提供すること
インターフェースは本物のロボット(すなわちハードウェアドライバノード)と一緒にするとシステムの他のノードを変更せずシミュレータを利用するか本物のロボットを利用するか選択できます。 幸いでGazeboのアクチュエータはROSの標準なインターフェースに従います。
URDFにGazeboのみが利用する情報を追加するとき、<gazebo>要素の中に置きます。
この要素の中にGazeboとROSのインターアクションを可能にする様々なプラグインの設定を指定します。
様々なプラグインがあります。
利用可能なプラグインはここを参照してください。
今回のロボットは差動二輪駆動(differential drive)という移動方法を利用します。
制御するために、Gazeboが提供するdifferential_drive_controllerというプラグインをロードしてドライブウィールのジョイントにつなげることが必要です。
mobile_robot.urdfの下(robot要素の中)に下記のようにgazebo要素を追加します。
1
2
3
4
5
6
7
8
9
10
11
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<robotBaseFrame>base_link</robotBaseFrame>
<wheelSeparation>0.108</wheelSeparation>
<wheelDiameter>0.024</wheelDiameter>
<publishWheelJointState>true</publishWheelJointState>
<legacyMode>false</legacyMode>
</plugin>
</gazebo>
下記、行毎に説明します。
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">filenameは共有ライブラリのファイル名です。<leftJoint>left_wheel_joint</leftJoint><rightJoint>right_wheel_joint</rightJoint><robotBaseFrame>base_link</robotBaseFrame>rviz等でロボットの位置を可視化するときにこのフレームを利用します。<wheelSeparation>0.108</wheelSeparation><wheelDiameter>0.048</wheelDiameter><publishWheelJointState>true</publishWheelJointState><legacyMode>false</legacyMode>もう一度Gazeboを起動します。
1
2
$ cd ~/urdf_ws
$ roslaunch mobile_robot_description mobile_robot_simulation.launch
そして別の端末で下記の実行でプラグインがロードされてロボットは制御可能になったと確認できます。
1
2
3
4
5
6
7
$ rostopic info /cmd_vel
Type: geometry_msgs/Twist
Publishers: None
Subscribers:
* /gazebo (http://alnilam:43953/)
/cmd_velにコマンドを送るとロボットが動きます。
コマンドはgeometry_msgs/Twistメッセージ型を利用します。
1
2
$ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 0.2}}'
publishing and latching message for 3.0 seconds

さらに、teleop_twist_keyboardという操作用のノードを利用すればロボットが運転できます。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
$ sudo apt-get install ros-kinetic-teleop-twist-keyboard
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
currently: speed 0.5 turn 1
ロボットの前後は(i と (, で操作します。
回転は (j と (l で操作します。
(k で止めます。
速度の上下変更は (q と (z で行います。
このロボットは小さいので、操作する前に速度を0.1ぐらいに下げる方がおすすめです。
ロボットが動いている間に別の端末で出力されている情報の確認ができます。 Gazeboのプラグインはジョイントの位置情報と、まるで車輪回転センサーから計算したや中にIMUがあるようにロボットの位置情報を出力します。
1
2
3
4
5
6
7
8
9
10
11
12
$ rostopic echo /joint_states
header:
seq: 117259
stamp:
secs: 1172
nsecs: 948000000
frame_id: ''
name: [left_wheel_joint, right_wheel_joint]
position: [735.1906221749484, 206.91425624976398]
velocity: []
effort: []
---
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
$ rostopic echo /odom
header:
seq: 117558
stamp:
secs: 1175
nsecs: 938000000
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: -0.30114005659
y: 0.978111034157
z: 0.0449638364222
orientation:
x: 0.0294855243831
y: 0.0018045884977
z: -0.997695948223
w: 0.0610748901939
covariance: [1e-05, ..., 0.001]
twist:
twist:
linear:
x: 0.204723281336
y: 0.000434240639441
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.381388401658
covariance: [0.0, ..., 0.0]
---
物理的なシミュレーションは確かに行っていることはロボットを後ろに動かすことや急に止めることによって見えます。 今回のロボットはバランスがちゃんと設計しなかったため、かなり不安定です。 下記のように後ろに動くと前に引っくり返りそうになります!

上記の図にもうひとつの不思議なことがあります。
キャスターはボディーに中に入っています。
collision要素を定義したのに、なぜこうなるでしょう。
その理由は、Gazeboは同じロボット内のリンクの衝突チェックを行いません。
計算速のためですが、実はモデル作成に便利なことです。
ジョイントとジョイント間の接触等ロボット内に衝突を気にしない場合に、URDFにそのジョイントがぴったり機会のように合わせることは必要ありません。
逆にモデル(特にメッシュ)をより簡単にする方を優先にでき、シミュレータや可視化が扱うデータの量が減らせます。
Gazeboのプラグインが出力するジョイントの位置情報等は標準のROSのインターフェースを利用します。
よって、rvizでそのデータの可視化は可能です。
Gazeboが起動中、rvizも起動します。
rvizは必ずmobile_robot_descriptionパッケージがあるワークスペースから起動します。
しないとrvizはURDFに利用するポリゴンメッシュファイルが見つけられません。
端末1:
1
2
$ cd ~/urdf_ws
$ roslaunch mobile_robot_description mobile_robot_simulation.launch
端末2:
1
2
3
$ cd ~/urdf_ws
$ source devel/setup.bash
$ rviz
rvizでデータ可視化のセットアップを行います。
まずはfixed frameを指定します。
Fixed frameは、どのタスクフレームの原点にたいしてデータを表示するか指定します。
map、odom、base_linkなどはよく利用されているフレームです。
今回は地図がまだないので、ロボットのオドメトリの原点であるodomを利用します。
rvizの左上にFixed Frameオプションをodomに変更します。

つぎにロボットの可視化を追加します。
左下のAddボタンを押して、ダイアログのリストからRobotModelを選択しOKをクリックします。


画面でロボットが表示されます。
しかし前のrvizの利用と違って、ロボットのリンクがバラバラになりました。

理由は左側に見えます。
各ジョイントのTFデータがありません。
Gazeboはジョイントの位置情報を出力していますが、ロボットの現状を分かるためにrvizやナビゲーションの制御アルゴリズム等はこの情報ではなくてタスクフレームを利用します。
ジョイントの位置情報をタスクフレームに変更することが必要です。
ところで、本物のロボットにもこれは全く同じです。
ロボットのジョイント毎の位置情報を利用してTFデータを計算することは、robot_state_publisherという一般的に利用可能なノードで行います。
robot_state_publisherは/joint_statesに送信されるジョイントについてのデータと、robot_descriptionパラメータのURDFを利用してTFデータを計算します。
そして結果を/tfに送信します。
おかげでロボットによって変わるノードは/joint_statesに出力するノードだけです。
この便利なノードもURDFのおかげです。
Gazeboでは、同じノードが利用可能です。
しかし、今のURDFではGazeboはアクチュエータがあるジョイントのみの位置情報を出力します。
アクチュエータなしのキャスタージョイントとキャスターサポートジョイントの位置情報も出力しないとrvizの可視化でキャスターはまだおかしい位置に表示されます。
アクチュエータがないジョイントの位置情報を出力するために、Gazeboはjoint_state_publisherというプラグインを提供します。
利用するために下記をmobile_robot.urdfのgazebo要素の中に追加します。
1
2
3
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<jointName>caster_support_joint, caster_wheel_joint</jointName>
</plugin>
そしてジョイントの位置情報を利用するために、mobile_robot_simulation.launchにrobot_state_publisherノードの起動を追加します。
1
2
3
4
5
6
7
8
9
10
<launch>
<param name="robot_description"
textfile="$(find mobile_robot_description)/urdf/mobile_robot.urdf"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch"/>
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description -urdf -model mobilerobot"/>
<!---- ここから追加 ---->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<!---- ここまで追加 ---->
</launch>
シミュレータを再起動して、別の端末で/joint_statesを確認すると下記のようにキャスターのジョイントについてのメッセージもあります。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
$ rostopic echo -n 2 /joint_states
header:
seq: 4460
stamp:
secs: 22
nsecs: 655000000
frame_id: ''
name: [right_wheel_joint, left_wheel_joint]
position: [-0.17263927534774215, -0.14479137638759632]
velocity: []
effort: []
---
header:
seq: 4461
stamp:
secs: 22
nsecs: 655000000
frame_id: ''
name: [caster_support_joint, caster_wheel_joint]
position: [-0.00031824555801751586, -0.3779807032992206]
velocity: []
effort: []
---
rvizももう一度起動してセットアップすると、ロボットが正しく表示されます。
そして運転するとrvizで見えるロボットはシミュレータのロボットと同様に動きます。


このソースは以下のURLでダウンロード可能です。
下記のように自分のワークスペースに入れて利用できます。
1
2
3
4
5
6
$ cd ~/urdf_ws/src
$ git clone https://github.com/gbiggs/rosjp_urdf_tutorial_packages
$ cd rosjp_urdf_tutorial_packages
$ git checkout mobile_robot_simulated_actuator
$ cd ~/urdf_ws
$ catkin_make
アクチュエータがないモデルをGazeboで試すとき、少し置いたらロボットが移動したことを気づくかもしれません。 その理由は、前車輪は自由に曲がれることです。 本物のロボットにアクチュエータやブレーキが付いているから動けませんが、シミュレータのディフォルトはジョイントにアクチュエータがありません。 物理シミュレーションの計算にある小さなエラーのせいで、ロボットは微妙に動きます。
ロボットの動けるジョイントはそれでも動けないようにしたい場合(例えば車のブレーキをかけること)は、そのジョイントにアクチュエータを追加してそして制御しないという方法があります。 ジョイントを固定ジョイントに変更すると違って、実行中にGazeboのAPIでアクチュエータをなくすこと等が可能ので、ブレーキの開放のようなことがシミュレートできます。
アクチュエータを同様に、Gazeboにセンサーのシミュレーションが可能です。
URDFでセンサーのリンクをしてして、センサーのプラグインを追加するとGazeboはセンサーデータを生成します。
そしてそのセンサーデータはROSのトピックで送信されるから別のノードで利用可能です。
ここでmobile_robot.urdfにレーザーセンサーを追加します。
センサーのリンクはすでにURDFファイルに入れました。
laserと呼びました。
でも、Gazeboにとってこのリンクはただの箱です。
sensor要素の追加でlaserリンクはセンサーだとGazeboに示します。
下記をmobile_robot.urdfのrobot要素内に追加します。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
<gazebo reference="laser">
<sensor type="gpu_ray" name="laser">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="gpu_laser" filename="libgazebo_ros_gpu_laser.so">
<topicName>/scan</topicName>
<frameName>laser</frameName>
</plugin>
</sensor>
</gazebo>
下記に内容を説明します。
<gazebo reference="laser">gazebo要素はlaserというリンクについてです。
センサーはこのリンクに追加されます。<sensor type="gpu_ray" name="laser">gpu_rayという種類です。
他の利用可能なセンサー種類はここに参照してください。<pose>0 0 0 0 0 0</pose><visualize>false</visualize>trueにするとGazeboでレーザーが見えるようになります。<update_rate>40</update_rate><ray><scan><horizontal><samples>720</samples><resolution>1</resolution>1にすると全点を出力します。<min_angle>-1.570796</min_angle><max_angle>1.570796</max_angle><range><resolution>0.01</resolution><plugin name="gpu_laser" filename="libgazebo_ros_gpu_laser.so">gpu_laserはレーザーの計算をGPUで行います。<topicName>/scan</topicName>/scanトピックにレーザーデータを出力します。<frameName>laser</frameName>laserというタスクフレームの原点に対してデータを出力します。Gazeboを再起動したら、visualizeがfalseであれば最初は特に何も見ません。
レーザーセンサーも同じです。世界が空ので、距離の図ることはありません。
下記のようにGazeboで円筒を作成し、ロボットに前に置きましょう。


そしてrvizを起動して、レーザーデータの可視化を追加します。


ロボットや円筒を移動するとrvizで表示されているレーザーデータが変わります。


このソースは以下のURLでダウンロード可能です。
下記のように自分のワークスペースに入れて利用できます。
1
2
3
4
5
6
$ cd ~/urdf_ws/src
$ git clone https://github.com/gbiggs/rosjp_urdf_tutorial_packages
$ cd rosjp_urdf_tutorial_packages
$ git checkout mobile_robot_simulated_sensor
$ cd ~/urdf_ws
$ catkin_make