ロボットシミュレータ Gazebo を ROS開発で利用するための簡単な手順を記載します。ここでは Debian9 を利用します。個別にインストールすることもできますが、ROS を ros-melodic-desktop-full でインストールすれば gazebo もインストールされます。

URDF モデルの作成

rviz による URDF モデルの可視化

ROS ではロボットの 3D モデル情報などを Unified Robot Description Format (URDF) で記述します。インストール済みの urdf_tutorial を利用すると URDF を簡単に試せます。

$ rospack list | grep urdf_tutorial
urdf_tutorial /opt/ros/melodic/share/urdf_tutorial
$ rosls urdf_tutorial/urdf
01-myfirst.urdf  02-multipleshapes.urdf  03-origins.urdf  04-materials.urdf  05-visual.urdf  06-flexible.urdf  07-physics.urdf  08-macroed.urdf.xacro
$ rosls urdf_tutorial/launch
display.launch

rviz で URDF モデルを可視化できます。

roscd urdf_tutorial
DISPLAY=:0 roslaunch urdf_tutorial display.launch model:=urdf/01-myfirst.urdf

以下のようにしても同じです。中央ボタンのマウススクロールで拡大縮小、ドラッグで回転、Shift ドラッグで平行移動できます。

DISPLAY=:0 roslaunch urdf_tutorial display.launch model:='$(find urdf_tutorial)/urdf/01-myfirst.urdf'

display.launch は内部的に roscore、rviz、joint_state_publisher、robot_state_publisher を起動しています。

roscp urdf_tutorial display.launch .
cat display.launch

<launch>

  <arg name="model" default="$(find urdf_tutorial)/urdf/01-myfirst.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

COLLADA ファイルを利用した URDF モデル

URDF ファイルからは COLLADA データを参照できます。必須ではありませんがここでは ROS パッケージを追加して URDF ファイルと COLLADA ファイルを格納するディレクトリをそれぞれ作成します。ここでは OpenRAVE のサンプルとしてインストールされる mug1.dae を URDF から参照してみます。

$ ls src/mypkg/urdf/
my.urdf
$ ls src/mypkg/meshes/
mug1.dae

my.urdf

<?xml version="1.0"?>
<robot name="myrobot">
  <material name="blue">
    <color rgba="0 0 1.0 0.5"/>
  </material>
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
  </link>
  <link name="mug">
    <visual>
      <geometry>
        <mesh filename="package://mypkg/meshes/mug1.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </visual>
  </link>
  <joint name="base_to_mug" type="fixed">
    <parent link="base_link"/>
    <child link="mug"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>
</robot>

rviz で起動すると以下のようになります。base_link リンクから Y 軸方向に -0.22m、Z 軸方向に 0.25m 平行移動した mug リンクの座標系において、メッシュの位置を Z 軸方向に 0.1m 平行移動した箇所として記述しています。簡単のため回転要素 rpy (roll pitch yaw) はすべて 0 です。その他、複雑な URDF ファイルを記述するためのマクロ Xacro 等も利用できます。

DISPLAY=:0 roslaunch urdf_tutorial display.launch model:=urdf/my.urdf

Gazebo に URDF モデルを読み込む ROS 設定

gazebo_ros パッケージの empty_world.launch

Gazebo を ROS から利用するためには gazebo_ros パッケージを利用します。

$ rospack list | grep 'gazebo_ros '
gazebo_ros /opt/ros/melodic/share/gazebo_ros

gazebo_ros パッケージには launch ファイルが複数格納されていますが、すべては empty_world.launch を参照しています。empty_world.launch は ROS 連携に必要な設定が行われた状態で内部的に Gazebo の gzserver と gzclient を起動します。

$ roscp gazebo_ros empty_world.launch .
$ egrep '(gzserver|gzclient)' empty_world.launch
  <arg unless="$(arg debug)" name="script_type" value="gzserver"/>
    <node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen" args="$(arg command_arg3)"/>
$ dpkg -S `which gzserver`
gazebo9: /usr/bin/gzserver

world ファイル

Gazebo のシミュレーション環境は SDF 形式のファイルで設定できます。拡張子は world です。world ファイルでは環境に読み込む model を複数 include します。Gazebo には sun と ground_plane だけが存在する環境 empty.world ファイルがサンプルとして含まれています。

cat /usr/share/gazebo-9/worlds/empty.world 

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
  </world>
</sdf>

model ファイル

world ファイルで読み込む model は model.config で設定されます。SDF だけでなく URDF も model.config で指定できます。以下は SDF の例です。sun と ground_plane はサンプルとして Gazebo に含まれています。

ls /usr/share/gazebo-9/models/sun/
model.config  model.sdf

cat /usr/share/gazebo-9/models/sun/model.config
<?xml version="1.0"?>

<model>
  <name>Sun</name>
  <version>1.0</version>
  <sdf version="1.5">model.sdf</sdf>

  <author>
    <name>Nate Koenig</name>
    <email>nate@osrfoundation.org</email>
  </author>

  <description>
    A directional light for the sun.
  </description>
</model>

empty_world.launch の利用例

以下のように world ファイルを指定して Gazebo を起動できます。

DISPLAY=:0 roslaunch gazebo_ros empty_world.launch world_name:=worlds/empty.world

ground_plane と sun が読み込まれていることが World タブで確認できます。

インストールしたバージョンによっては SSL 証明書のエラーが出ます。

[Err] [REST.cc:205] Error in REST request
libcurl: (51) SSL: no alternative certificate subject name matches target host name 'api.ignitionfuel.org'

$ dpkg -S /usr/share/ignition/fuel_tools/config.yaml
libignition-fuel-tools:amd64: /usr/share/ignition/fuel_tools/config.yaml

~/.ignition/fuel/config.yaml の URL を新しい URL に変更することで解消します。

-    url: https://api.ignitionfuel.org
+    url: https://api.ignitionrobotics.org

独自に world ファイルと model ファイルを作成

ROS パッケージを新規に作成して world ファイルと model ファイルを格納することにします。

catkin_create_pkg myrobot_gazebo

world ファイルと model ファイルを格納するディレクトリを作成します。

cd myrobot_gazebo/
mkdir worlds
mkdir models

SDF サイトの Bitbucket からサンプルとして利用できる gazebo_models がダウンロードできます。今回作成する world ファイルではこれらの model を利用してみます。git ではなく mercurial レポジトリです。以下のコマンドでダウンロードできます。サイズが大きいため時間がかかります。

cd /tmp/
hg clone https://bitbucket.org/osrf/gazebo_models
cd -

myrobot_gazebo/models に必要なものをコピーします。model 内で自己参照している箇所があるため、ディレクトリ名を変更すると読み込めなくなります。例えば gas_station を gas_station2 とはできません。

cp -r /tmp/gazebo_models/sun models/
cp -r /tmp/gazebo_models/ground_plane models/
cp -r /tmp/gazebo_models/gas_station models/

これらを参照する world ファイルを作成します。

worlds/myrobot.world

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://gas_station</uri>
      <name>gas_station</name>
      <pose>-2.0 7.0 0 0 0 0</pose>
    </include>
  </world>
</sdf>

gazebo_ros パッケージの empty_world.launch で起動してみます。Gazebo が model ファイルを参照できるように GAZEBO_MODEL_PATH を設定します。world ファイルを参照できるように GAZEBO_RESOURCE_PATH を設定します。Gazebo は /usr/share/gazebo-9/models、~/.gazebo/models、GAZEBO_MODEL_PATH を model ディレクトリとして参照できます。

roscd myrobot_gazebo
export GAZEBO_MODEL_PATH=`pwd`/models:${GAZEBO_MODEL_PATH}
export GAZEBO_RESOURCE_PATH=`pwd`
DISPLAY=:0 roslaunch gazebo_ros empty_world.launch world_name:=worlds/myrobot.world

• esc → ドラッグで視点の平行移動、中央ボタンドラッグで視点移動による拡大縮小
• t → RGB の軸をドラッグして物体を平行移動
• r → RGB のリングをドラッグして物体を回転
• s → RGB の軸を Shift キーを押しながらドラッグして物体を拡大縮小

URDF ファイルの読み込み

URDF ファイルに対応する model.config を作成して Gazebo の world ファイルで読み込む方法もありますが、ここではそうではなく、gazebo_ros パッケージの spawn_model で world とは別に追加で読み込む方法を利用します。Gazebo で読み込める URDF のサンプルとして baxter をダウンロードします。

cd /tmp/
git clone --depth 1 https://github.com/RethinkRobotics/baxter_common.git
cd -

新規にパッケージとしてダウンロードした URDF をコピーします。

roscd myrobot_gazebo
cd ../
cp -r /tmp/baxter_common/baxter_description .

world と urdf を別々に起動する launch ファイルを、world を含む myrobot_gazebo パッケージ内に作成します。

roscd myrobot_gazebo
mkdir launch
vi launch/myrobot.launch

myrobot.launch

<launch>
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find myrobot_gazebo)/worlds/myrobot.world"/>
    <!-- more default parameters can be changed here -->
  </include>
  <!-- Spawn a robot into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find baxter_description)/urdf/baxter.urdf -urdf -z 1 -model baxter" />
</launch>

以下のようなコマンドで起動できます。

roscd myrobot_gazebo
export GAZEBO_MODEL_PATH=`pwd`/models:${GAZEBO_MODEL_PATH}
DISPLAY=:0 roslaunch myrobot_gazebo myrobot.launch

myrobot.launch 相当の処理は以下のコマンドでも行えます。

roscd myrobot_gazebo
export GAZEBO_MODEL_PATH=`pwd`/models:${GAZEBO_MODEL_PATH}
export GAZEBO_RESOURCE_PATH=`pwd`
DISPLAY=:0 roslaunch gazebo_ros empty_world.launch world_name:=worlds/myrobot.world

別ターミナル

DISPLAY=:0 rosrun gazebo_ros spawn_model -file `rospack find baxter_description`/urdf/baxter.urdf -urdf -z 1 -model baxter

package.xml で model パスを指定

package.xml を編集すると GAZEBO_MODEL_PATH を export せずに launch できるようになります。

roscd myrobot_gazebo
vi package.xml

<exec_depend>gazebo_ros</exec_depend>
<export>
  <gazebo_ros gazebo_model_path="${prefix}/models"/>
</export>

cd ../..
catkin_make

DISPLAY=:0 roslaunch myrobot_gazebo myrobot.launch

Gazebo で読み込める URDF の作成

新規に ROS パッケージを作成します。

catkin_create_pkg myrobot_description

本ページで作成した URDFファイルを用意します。

meshes/mug1.dae
urdf/myrobot.urdf

URDF ファイルは Gazebo シミュレーションのための十分な情報を持っていません。Gazebo が内部的に URDF を SDF に変換するために必要な情報 <inertial></inertial> を追記する必要があります。剛体力学などでも登場する inertia 慣性モーメントは、質量が物体の平行移動のしづらさを表現するのと同様に、物体の回転のしづらさのようなものを表現するパラメータです。単位は kg および m です。また SDF 変換のためには必要のない情報ですが、地面で着地するために <collision></collision> も追記します。

<?xml version="1.0"?>
<robot name="myrobot">
  <material name="blue">
    <color rgba="0 0 1.0 0.5"/>
  </material>
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
        ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0"/>
    </inertial>
  </link>
  <link name="mug">
    <visual>
      <geometry>
        <mesh filename="package://myrobot_description/meshes/mug1.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://myrobot_description/meshes/mug1.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </collision>
    <inertial>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
        ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0"/>
    </inertial>
  </link>
  <joint name="base_to_mug" type="fixed">
    <parent link="base_link"/>
    <child link="mug"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>
</robot>

環境にロボットを登場させてみます。inertial の origin を変更して link の重心をバランスの悪そうな箇所にずらすとロボットが転倒します。

roscd myrobot_gazebo
export GAZEBO_MODEL_PATH=`pwd`/models:${GAZEBO_MODEL_PATH}
DISPLAY=:0 roslaunch myrobot_gazebo myrobot_not_baxter.launch

読み込んだ URDF モデルを ROS から扱えるように設定

ROS には ros_control というメタパッケージがあります。ros_control メタパッケージにはロボットを制御するためのパッケージが含まれています。例えば joint_state_controller パッケージを利用するとロボットの関節値を取得できます。ros_control は ROS からの指令にもとづきロボットをフィードバック制御します。PID 制御などが可能です。ros_control の Gazebo プラグイン libgazebo_ros_control.so を利用すると、制御対象を Gazebo シミュレータ内のロボットに変更できます。ROS 側のプログラムを、シミュレーション時と実機使用時で変更する必要がないということになります。

myrobot_control パッケージの作成

ロボットを制御するために ros_control メタパッケージを利用することにします。ros_control メタパッケージが提供するパッケージ群から必要なものを依存先に選択して myrobot_control パッケージを作成します。ここでは rviz 利用時にも必要になる座標変換ライブラリ tf を使用することを想定して robot_state_publisher も依存先に追加します。

• controller_manager パッケージ → フィードバック制御システム全体を管理する機能を提供します
• joint_state_controller パッケージ → ロボットの関節値を取得するための機能を提供します
• robot_state_publisher パッケージ → controller_manager が管理する ros_control 側からロボットの状態を取得して ROS の座標変換ライブラリ tf で扱える情報に変換します

パッケージの作成コマンド例

catkin_create_pkg myrobot_control controller_manager joint_state_controller robot_state_publisher
cd myrobot_control
mkdir config
mkdir launch
vi config/myrobot_control.yaml
vi launch/myrobot_control.launch
cd ../..
catkin_make

ros_control の joint_state_controller はロボットの関節値情報を既定では /joint_states トピックにパブリッシュします。そのための設定を YAML で用意します。

myrobot_control.yaml

myrobot:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

controller_manager ROS ノードを arg に joint_state_controller を指定して起動します。更に /myrobot/joint_states をサブスクライブして /tf に tf 用の情報をパブリッシュする robot_state_publisher ROS ノードを起動します。

myrobot_control.launch

<launch>
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find myrobot_control)/config/myrobot_control.yaml" command="load"/>
  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/myrobot" args="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="/myrobot/joint_states" />
  </node>
</launch>

URDF を ROS 連携のために変更 (myrobot_description)

Gazebo シミュレーション環境内のロボットと ros_control の連携のために URDF を更新する必要があります。

• Joint が動くように fixed から continuous に変更します <joint name="base_to_mug" type="continuous">
• Gazebo プラグイン libgazebo_ros_control.so を利用するように <gazebo></gazebo> を設定します
• transmission タグを追記します

myrobot.urdf

<?xml version="1.0"?>
<robot name="myrobot">
  <material name="blue">
    <color rgba="0 0 1.0 0.5"/>
  </material>
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
        ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0"/>
    </inertial>
  </link>
  <link name="mug">
    <visual>
      <geometry>
        <mesh filename="package://myrobot_description/meshes/mug1.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://myrobot_description/meshes/mug1.dae"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.1"/>
    </collision>
    <inertial>
      <origin xyz="0 0 1" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia
        ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0"/>
    </inertial>
  </link>
  <joint name="base_to_mug" type="continuous">
    <parent link="base_link"/>
    <child link="mug"/>
    <origin xyz="0 -0.22 0.25"/>
  </joint>
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/myrobot</robotNamespace>
    </plugin>
  </gazebo>
  <transmission name="mytran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="base_to_mug">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="mymotor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</robot>

URDF モデルを ROS パラメータサーバに読み込むように変更 (myrobot_gazebo)

libgazebo_ros_control.so で URDF モデルの情報 robot_description が必要になるため ROS パラメータサーバに情報を読み込む必要があります。gazebo_ros の spawn_urdf で -file を利用していた場合は -param に変更します。

myrobot_gazebo/launch/myrobot_not_baxter.launch

<launch>
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find myrobot_gazebo)/worlds/myrobot.world"/>
    <!-- more default parameters can be changed here -->
  </include>
  <!-- Spawn a robot into Gazebo -->
  <param name="robot_description" textfile="$(find myrobot_description)/urdf/myrobot.urdf" />
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
        args="-urdf -model myrobot -param robot_description"/> />
</launch>

ROS ノードの起動

二つのパッケージの launch ファイルで ROS ノード群を起動します。

roscd myrobot_gazebo
export GAZEBO_MODEL_PATH=`pwd`/models:${GAZEBO_MODEL_PATH}
DISPLAY=:0 roslaunch myrobot_gazebo myrobot_not_baxter.launch
roslaunch myrobot_control myrobot_control.launch

/myrobot/joint_states および /tf トピックが作成されたことが確認できます。

rostopic hz /myrobot/joint_states
rostopic hz /tf

動作検証などでは ros_control の Gazebo プラグインが提供するサービスで Gazebo 内のロボットの pose (位置と姿勢)、速度、角速度を変更すると便利です。

rosservice call /gazebo/set_model_state '{
  model_state: {
    model_name: myrobot,
    pose: {
      position: { x: 0.0, y: 0.0, z: 1.0 },
      orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 }
    },
    twist: {
      linear: { x: 0.0, y: 0.0, z: 4.0 },
      angular: { x: 0.0, y: 0.0 , z: 0.0 }
    },
    reference_frame: world
}}'

• pose
  • 位置 position (x,y,z)
  • 姿勢 orientation (x,y,z,w)
    • 四元数です。
    • ロボット界隈では主に特異点の問題がない四元数で姿勢が表現されます。
    • OpenRAVE など w を最初に記載する場合と ROS のように最後に記載する場合があることに注意します。
    • その他にオイラー角で表現する方法があります。(roll, pitch, yaw) はオイラー角の一つです。
• twist
  • 並進速度、角速度

ROS ノードの関係を rqt_graphで可視化すると、/myrobot/joint_states トピックを robot_state_publisher ノードがサブスクライブしていることも分かります。

DISPLAY=:0 rqt_graph