はじめに

こんにちは！今回はGazebo上で「床に経路を描く」方法を紹介します。
この記事では、Pythonを使ってBézier曲線を計算し、その結果を基にGazeboのworldファイルを生成する方法を説明します。

結果として、Gazeboのシミュレーション環境において、上から見たときに経路が視覚的に確認できるようになります。
ROSやGazeboに触れたばかりの方でも実践できるよう、簡単な解説を交えながら進めていきます。

この記事の目次

対象読者
実現したいこと
想定環境
Step1: Gazebo上にBézier曲線の経路を生成
Step2: Gazebo上で確認
応用例
おわりに

対象読者

GazeboやROSのシミュレーション環境を触っている方
自律移動ロボットの経路計画を視覚化してみたい方
Pythonでファイルの自動生成をしてみたい方

実現したいこと

Bézier曲線の制御点をPythonコードで指定し、Gazeboの床に経路を描画します。本記事における経路は、薄い直方体をつなげることで成り立っています。

完成イメージは以下の通りです。

想定環境

Ubuntu 20.04 LTS
ROS Noetic
Gazebo11
Python 3.11.9

Step1:Gazebo上にBézier曲線の経路を生成

以下のPythonコードを用いてmain関数内で(Bézier曲線の制御点、プロット数、直方体のサイズ、視点位置、出力ファイル名)を指定して、Gazebo上にBézier曲線の経路を生成できます。

generate_bezier_box.py

import math


def bezier_curve(control_points, num_points):
    """Bézier曲線を生成する"""
    def bernstein_poly(i, n, t):
        """バーンスタイン多項式"""
        return math.comb(n, i) * (t ** i) * ((1 - t) ** (n - i))

    n = len(control_points) - 1
    curve = []
    prev_x, prev_y = None, None  # 直前の点を保存するための変数

    for t in [i / (num_points - 1) for i in range(num_points)]:
        x, y = 0.0, 0.0
        for i, (px, py) in enumerate(control_points):
            b = bernstein_poly(i, n, t)
            x += b * px
            y += b * py

        if prev_x is None and prev_y is None:  # 最初の点の場合
            theta = math.atan2(control_points[1][1] - control_points[0][1],
                                control_points[1][0] - control_points[0][0])
        else:  # 2点目以降の点の場合
            theta = math.atan2(y - prev_y, x - prev_x)

        curve.append((x, y, theta))
        prev_x, prev_y = x, y  # 現在の点を次のループのために保存

    return curve

def offset_position(x, y, th, offset):
    """指定したオフセットで座標を調整"""
    x_offset = x + offset * math.cos(th)
    y_offset = y + offset * math.sin(th)
    return x_offset, y_offset

MODEL_TEMPLATE = """
    <model name='unit_box_{index}'>
      <static>true</static>
      <pose>{x} {y} 0 0 0 {th}</pose>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.000005 0.000005 0.000005</size>
            </box>
          </geometry>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size>{length} {width} {height}</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Green</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
"""

WORLD_TEMPLATE = """<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    {models}
    <gui>
      <camera name="user_camera">
        <pose>{view_point_x} {view_point_y} {view_point_z} 0 1.57 1.57 0</pose>
      </camera>
    </gui>
  </world>
</sdf>
"""

def generate_models(coordinates, length, width, height):
    """座標リストからモデルを生成"""
    models = []
    for index, (x, y, th) in enumerate(coordinates):
        if index == 0:
            offset = 0.25 * length
            x_offset, y_offset = offset_position(x, y, th, offset)
            obj_length = 0.5 * length
        elif index == len(coordinates) - 1:
            offset = -0.25 * length
            x_offset, y_offset = offset_position(x, y, th, offset)
            obj_length = 0.5 * length
        else:
            x_offset, y_offset = x, y
            obj_length = length
        models.append(MODEL_TEMPLATE.format(index=index, x=x_offset, y=y_offset, th=th, length=obj_length, width=width, height=height))
    return "\n".join(models)

def create_world_file(output_filename, models, view_point_x, view_point_y, view_point_z):
    """worldファイルを生成"""
    with open(output_filename, mode='w') as file:
        file.write(WORLD_TEMPLATE.format(models=models, view_point_x=view_point_x, view_point_y=view_point_y, view_point_z=view_point_z))

def main():
    # Bézier曲線の制御点
    control_points = [(0, 0), (0, 8), (8, 8), (8, 0)]

    # 経路用に使う直方体の数
    num_points = 100

    # 経路用に使う直方体のパラメータ
    length = 0.24
    width = 0.05
    height = 0.0005

    # 視点位置
    view_point_x = 4
    view_point_y = 3
    view_point_z = 12

    # Bézier曲線を生成
    coordinates = bezier_curve(control_points, num_points)

    # モデルを生成
    models = generate_models(coordinates, length, width, height)

    # worldファイルを生成
    filename = "bezier_box.world"
    create_world_file(filename, models, view_point_x, view_point_y, view_point_z)

    print(f"Worldファイルが生成されました: {filename}")

if __name__ == "__main__":
    main()

詳細についてこれから説明していきます。

Step1.1: Bézier曲線の計算

まずは、指定されたBézier曲線の制御点(control_points)とGazebo上に定義する物体の個数(num_points)から、Bézier曲線の座標と接線方位角$(x,y,\theta)$を計算する関数を準備します。

def bezier_curve(control_points, num_points):
    """Bézier曲線を生成する"""
    def bernstein_poly(i, n, t):
        """バーンスタイン多項式"""
        return math.comb(n, i) * (t ** i) * ((1 - t) ** (n - i))

    n = len(control_points) - 1
    curve = []
    prev_x, prev_y = None, None  # 直前の点を保存するための変数

    for t in [i / (num_points - 1) for i in range(num_points)]:
        x, y = 0.0, 0.0
        for i, (px, py) in enumerate(control_points):
            b = bernstein_poly(i, n, t)
            x += b * px
            y += b * py

        if prev_x is None and prev_y is None:  # 最初の点の場合
            if len(control_points) > 1:  # 次の制御点が存在する場合
                theta = math.atan2(control_points[1][1] - control_points[0][1],
                                   control_points[1][0] - control_points[0][0])
            else:
                theta = 0.0
        else:  # 2点目以降の点の場合
            theta = math.atan2(y - prev_y, x - prev_x)

        curve.append((x, y, theta))
        prev_x, prev_y = x, y  # 現在の点を次のループのために保存

    return curve

Step1.2: worldファイルのテンプレートを定義

次に、計算したBézier曲線に沿ってGazeboの床部分に直方体を配置するワールドファイルのテンプレートを作成します。

import math


def offset_position(x, y, th, offset):
    """指定したオフセットで座標を調整"""
    x_offset = x + offset * math.cos(th)
    y_offset = y + offset * math.sin(th)
    return x_offset, y_offset

MODEL_TEMPLATE = """
    <model name='unit_box_{index}'>
      <static>true</static>
      <pose>{x} {y} 0 0 0 {th}</pose>
      <link name='link'>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.000005 0.000005 0.000005</size>
            </box>
          </geometry>
        </collision>
        <visual name='visual'>
          <geometry>
            <box>
              <size>{length} {width} {height}</size>
            </box>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Green</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
      </link>
    </model>
"""

WORLD_TEMPLATE = """<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    {models}
    <gui>
      <camera name="user_camera">
        <pose>{view_point_x} {view_point_y} {view_point_z} 0 1.57 1.57 0</pose>
      </camera>
    </gui>
  </world>
</sdf>
"""

def generate_models(coordinates, length, width, height):
    """座標リストからモデルを生成"""
    models = []
    for index, (x, y, th) in enumerate(coordinates):
        if index == 0:
            offset = 0.25 * length
            x_offset, y_offset = offset_position(x, y, th, offset)
            obj_length = 0.5 * length
        elif index == len(coordinates) - 1:
            offset = -0.25 * length
            x_offset, y_offset = offset_position(x, y, th, offset)
            obj_length = 0.5 * length
        else:
            x_offset, y_offset = x, y
            obj_length = length
        models.append(MODEL_TEMPLATE.format(index=index, x=x_offset, y=y_offset, th=th, length=obj_length, width=width, height=height))
    return "\n".join(models)

offset_position：直方体の中心を調整するための関数
直方体はbezier_curve関数で導出された$(x,y,\theta)$を用いて配置されます。しかし直方体の中心が$(x,y)$に相当するため最初と最後の直方体は経路からはみ出てしまいます。offset_position関数ははみ出ないように直方体の中心を調整するための関数です。
$ $
MODEL_TEMPLATE：worldファイルにおける直方体一つのかたまり
- 何かと衝突しても動かない(staticタグ)
- 位置
- 衝突判定に利用されるサイズ
- 見た目上のサイズ
- 色
  $ $
WORLD_TEMPLATE：worldファイルにおける基礎的な設定
- 視点の位置
  $ $
generate_models：指定された物体の位置・角度のリスト(coordinates), サイズ(length, width, height)から、Gazebo上に生成する経路のモデルを作成する関数

Step1.3: worldファイルを生成

出力するファイル名、worldファイルのモデル、視点を指定してworldファイルを作成する関数

def create_world_file(output_filename, models, view_point_x, view_point_y, view_point_z):
    """worldファイルを生成"""
    with open(output_filename, mode='w') as file:
        file.write(WORLD_TEMPLATE.format(models=models, view_point_x=view_point_x, view_point_y=view_point_y, view_point_z=view_point_z))

Step2: Gazebo上で確認

ターミナルで、bezier_box.worldがあるディレクトリに移動し以下のコマンドを実行して、生成されたワールドファイルをGazeboで立ち上げます。

$ gazebo bezier_box.world

シミュレーション画面が表示され、床に描かれた経路が確認できるはずです。

応用例

この方法を応用することで、以下のようなことも実現可能です。

経路に基づくロボットの自律移動シミュレーション
ロボットが経路を正確にたどる制御アルゴリズムのテスト
障害物を避けるルートの可視化

おわりに

今回はGazeboの床に経路を描画する方法を解説しました。
Pythonスクリプトで簡単にワールドファイルを生成し、シミュレーション環境に反映させる方法が理解できたと思います。

皆さんのシミュレーション環境で役立てていただけるとうれしいです！
不明点や改善案があれば、ぜひコメントで教えてください。

最後までお読みいただきありがとうございました！