Nav2 講座 5: .pgm ファイルを .stl ファイルに変換し、Gazebo simulation に使用する

ROS
2025.04.06

ROS 2 に関して筆者は以下の本で学びました。ROS1 との比較を含めかなりわかりやすく ROS 2 について書かれており、おすすめの一冊です。

Amazon.co.jp
amzn.to

前回の記事で、自分で作成した map 上でロボットを動かすシミュレーションに成功しました。

Nav2 講座 4: 自分で作成したマップを使って TurtleBot シミュレーションをする #ROS 2 #nav2
ROS 2 に関して筆者は以下の本で学びました。ROS1 との比較を含めかなりわかりやすく ROS 2 について書かれており、おすすめの一冊です。前回の記事で、nav2_bringup を模倣した custom_nav...
www.remma.net
2025.03.02

ただ、この方法の問題点として、作成した map によっては .world ファイルの容量が重くなってしまい（試しに作成した .world ファイルは 10,000 行程度）、 Gazebo で開くのに時間がかかることが挙げられます。この問題を解消するため、.pgm ファイルを .stl ファイルに変換し、.stl ファイルを Gazebo で開けるようにします。

過去記事

PC 環境

  • Thinkpad X1 Intel Core i7
  • Ubuntu 24.04

とりあえず実行してみたい人向け

とりあえず動かしてみたい方は以下を実行してみてください。

事前準備

事前準備として以下を実行してください。

$ git clone git@github.com:remmaTech12/ros2_nav2.git
$ cd ros2_nav2
$ git checkout article_8211
$ docker compose up -d --build
$ xhost +
$ docker exec -it ros2_nav2 bash
/workspace$ export TURTLEBOT3_MODEL=waffle
/workspace$ export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models
/workspace$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py  ## Takes about 5 min.

実行

以下により、.stl ファイルをベースとした map 上でのシミュレーションができます。

$ docker exec -it ros2_nav2 bash
/workspace$ export TURTLEBOT3_MODEL=waffle
/workspace$ export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models
/workspace$ colcon build
/workspace$ source install/setup.bash
/workspace$ ros2 launch custom_nav2_bringup tb3_simulation_launch.py headless:=False
RViz と Gazebo の表示

.pgm ファイルから .stl ファイルへの変換

.pgm ファイルから .stl ファイルに変換する python スクリプト convert_pgm_to_stl.py を ChatGPT に書いてもらいました。

import numpy as np
from PIL import Image
import trimesh
from trimesh.creation import box
from trimesh.scene import Scene

# Parameters
input_file = "maze.pgm"
output_file = "maze.stl"
resolution = 0.05
wall_height = 1.0

# Load image
image = Image.open(input_file).convert("L")
image = np.array(image, dtype=np.uint8)
height, width = image.shape

scene = Scene()

# Threshold for black (wall)
black_threshold = 128

for y in range(height):
    for x in range(width):
        if image[y, x] < black_threshold:
            px = (x - width / 2) * resolution
            py = (height / 2 - y) * resolution
            pz = wall_height / 2

            wall = box(extents=[resolution, resolution, wall_height])
            wall.apply_translation([px, py, pz])
            scene.add_geometry(wall)

scene.export(output_file)
print(output_file + " created!")

スクリプトの簡単な説明

入力・出力ファイルを変更する場合は、こちらを変更してください。

# Parameters
input_file = "maze.pgm"
output_file = "maze.stl"

画素が黒い場合のみ壁を作ります。

        if image[y, x] < black_threshold:

以下は画像座標系から Gazebo 座標系（メートル単位）への座標変換です。

            px = (x - width / 2) * resolution
            py = (height / 2 - y) * resolution

変換実行

python スクリプトを実行し、.stl ファイルを生成します。

$ python3 convert_pgm_to_stl.py 
maze.stl created!

変換後の .stl ファイルは FreeCAD で表示できます。

.stl ファイルを FreeCAD で描画

.world ファイル

.world ファイルは以下です。

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="maze">

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <gui fullscreen="0">
      <camera name="user_camera">
        <pose frame="">0 0 10 0 1.570796 0</pose>
        <view_controller>orbit</view_controller>
        <projection_type>perspective</projection_type>
      </camera>
    </gui>

    <model name="maze">
      <static>true</static>
      <pose>0 0 0 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <mesh>
              <uri>file:///workspace/src/custom_nav2_bringup/maps/maze.stl</uri>
            </mesh>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <mesh>
              <uri>file:///workspace/src/custom_nav2_bringup/maps/maze.stl</uri>
            </mesh>
          </geometry>
        </visual>
      </link>
    </model>

  </world>
</sdf>

前回記事でもともと 10,000 行くらいあったファイルが 43 行になり、とてもすっきりしました。

別の .stl ファイルを使いたい場合は以下の部分を変更してください。collision（物理シミュレーション用）と visual（見た目描画用）の両方を変更することに注意してください。

 <uri>file:///workspace/src/custom_nav2_bringup/maps/maze.stl</uri>

.launch ファイル

.launch ファイルの変更は非常にシンプルで、今回は maze.world に代わって maze_stl.world という .world ファイルを使用するので一行変更するだけです。

-        default_value=os.path.join(bringup_dir, 'worlds', 'maze.world'),
+        default_value=os.path.join(bringup_dir, 'worlds', 'maze_stl.world'),

実行コマンド

実行する場合はこちらを参照ください。

まとめ

.stl ファイルを使うことで .world ファイルの記述がとてもすっきりしました！！

コメント