1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@bebebefu(べべべふ)

Open-RMF + Gazebo Classic でカチャカ（Kachaka）の棚搬送をシミュレーションをしてみた

1
Last updated at Posted at 2026-06-06

1. はじめに

カチャカ (Kachaka)というロボットを開発とかで使っている中で、これの動きをシミュレーションできないかなあと考えていました。まあそういったリポジトリもあるのですが、棚を切り離してどこかにおけるというところを再現したいなと思いまして。色々試してみたというのがこの記事の内容になります。

本記事では、Open-RMF (Open Robotics Middleware Framework) のシミュレーション環境 (Gazebo Classic) に カチャカ (Kachaka) の模擬体を持ち込むまでを、ステップバイステップでまとめます。なお、実施したことの概要は以下の３つです。

  1. ロボットの見た目を Kachaka に差し替える
    RMF で動く中身は TinyRobot (slotcar) のまま、見た目 (visual) だけを Kachaka の STL に置換します。

  2. 3 段棚 (キャスター付き) を配置する
    STL を Blender で結合・加工して 3 段棚のモデルをGazebo モデルとして登録し、Traffic Editor で配置します。

  3. 棚の取り付け / 切り離しを再現する
    Open-RMF のタスクと Gazebo の link attacher プラグインを組み合わせ、「棚の下に潜り込んで結合し (アタッチ)、目的地で置く (デタッチ)」動作を再現します。標準の TeleportDispenser / TeleportIngestor では避けきれなかった事象と、その回避のために書いた小さな ROS 2 ノードが中心です。

完成すると、こんな動きになります:

2. 今回のシミュレーションで使わせていただいたリポジトリ

本記事のシミュレーション環境は、以下の OSS リポジトリの成果物を組み合わせて構築しています。

用途 取り込み元 本記事での扱い
ロボットの見た目 (本体メッシュ STL) CyberAgentAILab/kachaka_ros2_dev_kit (Apache-2.0) body.stl を取得し、TinyRobot の visual に差し替え
3 段棚のモデル GAI-313/kachaka_shelf_description (MIT License) STL を入力として Blender で結合・加工し利用
棚の剛体結合プラグイン IFRA-Cranfield/IFRA_LinkAttacher (Apache-2.0) world プラグインとして利用 (ATTACHLINK / DETACHLINK)
ベース RMF 環境 open-rmf/rmf_demos (Apache-2.0) デモのワークスペース構成を利用し、新しい demo / map (evtest_kachaka) を追加

3. 実行環境

  • Ubuntu 22.04
  • ROS 2 Humble
  • Open-RMF (humble)
  • Gazebo Classic 11
  • Python 3.10
  • Blender 4.5 LTS
  • traffic-editor

ワークスペースは ~/rmf_ws を前提に記述します。検証済みの構成ですが、それ以外の組み合わせでも動く可能性はあります。

4. 全体アーキテクチャ

image.png

ロボットは「中身は TinyRobot のまま、見た目だけ Kachaka」。棚は 既存のSTL から作成したモデル。取り付け / 切り離しは、kachaka_shelf_attacher.py という小さなノードを用意して、

  1. RMF タスクから飛んでくる attach / detach の perform_action を受け取り、
  2. Gazebo 側の link attacher プラグインの ATTACHLINK / DETACHLINK サービスを呼んで剛体結合し、
  3. 棚の位置を SetEntityState で物理的に補正する

という流れで動かしています。理由は Step 3 で述べます。

5. 手順

5.1 Step 0. 前提（rmf_ws が動くこと）

本記事は、私が以前に構築した evtest というシミュレーション環境（エレベータ付き2階建ての建物に複数の tinyRobot を配置した Open-RMF 環境）が動作していること を前提に進めます。evtest.launch.xml や evtest.building.yaml、tinyRobot_config.yaml などはこの環境で作成済みのものを使います。

evtest 環境そのものの作り方（traffic-editor での建物・エレベータ・参照点の作成、launch / config / rviz ファイルの用意、ビルドまで）は、以下の記事で解説しています。

まず、構築済みの環境が起動できることを確認しておきます。

cd ~/rmf_ws
source /opt/ros/humble/setup.bash
source ~/rmf_ws/install/setup.bash
ros2 launch rmf_demos_gz_classic evtest.launch.xml

ここまで問題なく起動できる状態を出発点にします。

5.2 Step 1. tinyRobot の見た目を Kachaka に差し替える（STL 版）

方針はこれだけです。

  • RMF で動く “中身” は TinyRobot (slotcar) をそのまま使う
  • 見た目 (visual) だけを Kachaka の STL に置換する
  • evtest_kachaka の world が model://TinyRobotKachaka を参照するように building.yaml を用意し、rmf_demos_maps を再生成する

Step 1-1. TinyRobotKachaka モデルを作る（rmf_demos_assets）

TinyRobot を複製

cd ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_assets/models
cp -r TinyRobot TinyRobotKachaka

Kachaka の本体メッシュ（STL）を配置

本体メッシュは下記から取得しました。

STLにしました （DAE は壊れていると gzclient が落ちることがあるので避けます）。取得した STL を kachaka_body.stl という名前で配置します。

cp /path/to/kachaka_body.stl \
  ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_assets/models/TinyRobotKachaka/meshes/kachaka_body.stl

TinyRobotKachaka/model.sdf を編集（“visual だけ” 変える）

編集方針は slotcar / collision / joint は一切触らない（挙動を壊さない）こと。触るのは次の点だけです。

  1. <model name='TinyRobot'> を TinyRobotKachaka に変更
  2. 本体 visual の uri を tinyRobot_body から kachaka_body.stl に変更
  3. （任意）車輪・キャスターの visual は二重表示防止でコメントアウト

A) モデル名

<model name='TinyRobotKachaka'>

B) 本体 visual の uri を STL に

元の箇所（tinyRobot_body.dae）をこう置換します。

<visual name='base_footprint_fixed_joint_lump__base_link_visual'>
  <pose frame=''>0 0 0 0 0 0</pose>
  <geometry>
    <mesh>
      <scale>1 1 1</scale>
      <uri>model://TinyRobotKachaka/meshes/kachaka_body.stl</uri>
    </mesh>
  </geometry>
</visual>

見た目の位置や大きさが合わない場合は、ここだけ調整します。

  • 大きさ：<scale>
  • 浮く/沈む：<pose> の z
  • 向き：<pose> の yaw（最後の値）

C) 車輪/キャスターの visual を消す（任意）

Kachaka メッシュに車輪が含まれる場合が多いので、二重表示が邪魔なら visual だけコメントアウトします（collision は残す）。

  • base_footprint_fixed_joint_lump__base_link_visual_1/2（キャスター）
  • left_wheel_visual
  • right_wheel_visual

Step 1-2. rmf_demos_assets をビルドして install に反映

cd ~/rmf_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select rmf_demos_assets
source ~/rmf_ws/install/setup.bash

反映確認（install 側に model.sdf があること）：

find ~/rmf_ws/install/rmf_demos_assets -path "*TinyRobotKachaka*/model.sdf" -print

Step 1-3. evtest_kachaka.building.yaml を用意（rmf_demos_maps）

maps/evtest をコピーして maps/evtest_kachaka を作る

cd ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_maps/maps
cp -r evtest evtest_kachaka

building.yaml を evtest_kachaka.building.yaml にする

cd ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_maps/maps/evtest_kachaka
cp evtest.building.yaml evtest_kachaka.building.yaml

spawn_robot_type を TinyRobotKachaka に変更

evtest_kachaka.building.yaml の vertices 内にある、各ロボットの spawn 定義を変更します。例（evtest だとこういう行）：

spawn_robot_name: [1, tinyRobot_1]
spawn_robot_type: [1, TinyRobot]

これを TinyRobotKachaka にします：

- spawn_robot_type: [1, TinyRobot]
+ spawn_robot_type: [1, TinyRobotKachaka]

確認コマンド（2 箇所出るのが正常）：

grep -n "spawn_robot_type" evtest_kachaka.building.yaml | head -n 20

Step 1-4. rmf_demos_maps をビルドして world を再生成

cd ~/rmf_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select rmf_demos_maps
source ~/rmf_ws/install/setup.bash

生成確認：

ls -lh ~/rmf_ws/install/rmf_demos_maps/share/rmf_demos_maps/maps/evtest_kachaka/

world が TinyRobotKachaka を参照していることを確認：

grep -n "model://TinyRobotKachaka" \
  ~/rmf_ws/install/rmf_demos_maps/share/rmf_demos_maps/maps/evtest_kachaka/evtest_kachaka.world | head

ここが出れば OK です。

Step 1-5. 起動（map_name を evtest_kachaka にする）

rmf_demos_gz_classic の launch / map_name を evtest_kachaka にして起動します。

cd ~/rmf_ws
source ~/rmf_ws/install/setup.bash
ros2 launch rmf_demos_gz_classic evtest_kachaka.launch.xml

Step 1-6. 見た目が変わったか確認

起動中に、実際に読まれている world を確認します。

WORLD=$(ps -ef | grep gzserver | grep -v grep | sed -n 's/.* \(\/.*\.world\).*/\1/p' | head -n 1)
echo "[WORLD] $WORLD"

期待値は .../maps/evtest_kachaka/evtest_kachaka.world です。

Gazebo 上のモデル名一覧は、環境によって gz model -l が使えないことがあるので、ROS トピックで見ます。

ros2 topic echo -n 1 /gazebo/model_states | head -n 80

ここに tinyRobot_1, tinyRobot_2 が出ていればスポーン成功です。見た目は Gazebo GUI 上で Kachaka になっているはずです。

Step 1-7. RMF タスクで動作確認（例：patrol）

見た目を変えても中身は slotcar なので、いつも通りタスクが動きます。

ros2 run rmf_demos_tasks dispatch_patrol -p tinyRobot_1_charger tinyRobot_2_charger -n 1 --use_sim_time

5.3 Step 2. Kachaka 3 段棚（キャスター付き）をTraffic Editor で配置する

ここでは、入手した STL を入力として 3 段棚のモデルを、Gazeboモデル化して Traffic Editor に出せるところまでを行います。

補足: 入力に使う棚は GAI-313/kachaka_shelf_description（MIT License、Copyright (c) 2026 GAI）のモデルです。MIT License のもとで自由に利用・改変できます（著作権表示とライセンス文の併記が条件）。本記事は当時の STL を入力とする手順で記述しています。現在は同リポジトリが URDF 構成（description.launch の shelf_type で段数選択）に更新されているため、最新の使い方はリポジトリの README を参照してください。

Step 2-1. 入力 STL（kachaka_shelf_description）を用意

GAI-313/kachaka_shelf_description から取得した棚の STL を入力に使います。本記事の手順は、当時のリポジトリにあった stl/ 配下の以下 3 ファイルを前提にしています。

  • kachaka_shelf_base.stl（土台＋支柱）
  • kachaka_shelf_plate.stl（中段の棚板）
  • kachaka_shelf.stl（最上段の棚板）

例：~/rmf_ws/_assets/kachaka_shelf_description/stl/ に置く

mkdir -p ~/rmf_ws/_assets/kachaka_shelf_description/stl
# 取得した STL をここに配置してから
ls -lh ~/rmf_ws/_assets/kachaka_shelf_description/stl/kachaka_shelf*.stl

Step 2-2. Blenderで編集して「3 段＋キャスター」を調整する

以下の点に注意して3段の棚＋キャスターを調整しました。

  • 中段が低い/高い
  • 全高を 70cm に固定

加工後のSTLは、Step 2-3 で参照するため ~/rmf_ws/_assets/kachaka_shelf_3tier.stl という名前でエクスポートしておきます。

ls -lh ~/rmf_ws/_assets/kachaka_shelf_3tier.stl

Step 2-3. Gazebo モデルとして rmf_demos_assets に登録

Traffic Editor は “モデル名” を building.yaml に書くだけなので、Gazebo モデル（model.config / model.sdf）が必要です。

モデルディレクトリ作成＆STL配置

MODEL=~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_assets/models/kachaka_shelf_3tier
mkdir -p $MODEL/meshes
cp ~/rmf_ws/_assets/kachaka_shelf_3tier.stl $MODEL/meshes/shelf_3tier.stl

model.config

cat > $MODEL/model.config <<'EOF'
<?xml version="1.0"?>
<model>
  <name>kachaka_shelf_3tier</name>
  <version>1.0</version>
  <sdf version="1.6">model.sdf</sdf>
  <description>Kachaka shelf 3-tier (blender combined)</description>
</model>
EOF

model.sdf（まずは配置用：静的モデル）

Traffic Editor での配置・見た目確認の段階では、<static>true</static> の軽量モデルで十分です。collision はざっくり box で構いません。

cat > $MODEL/model.sdf <<'EOF'
<?xml version="1.0"?>
<sdf version="1.6">
  <model name="kachaka_shelf_3tier">
    <static>true</static>
    <link name="base_link">
      <visual name="visual">
        <geometry>
          <mesh>
            <uri>model://kachaka_shelf_3tier/meshes/shelf_3tier.stl</uri>
            <scale>1 1 1</scale>
          </mesh>
        </geometry>
      </visual>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.55 0.55 0.95</size>
          </box>
        </geometry>
      </collision>
    </link>
  </model>
</sdf>
EOF

このモデルは「配置・見た目確認」用の最小構成です。実際に着脱 (アタッチ・デタッチ) させる段では、当たり判定の高さや脚の摩擦、運用上の static 指定などを見直します。詳しくは Step 3 で説明します。まずはこの静的モデルで Traffic Editor 配置まで通すのがおすすめです。

Step 2-4. rmf_demos_assets を反映（ビルド）

cd ~/rmf_ws
source /opt/ros/humble/setup.bash

# 一度壊れた build が残って symlink エラーが出ることがあるので、
# 安全に作り直すなら build/log を消してからやるのが確実
rm -rf build log

colcon build --symlink-install
source ~/rmf_ws/install/setup.bash

モデルが install 側に入ったことを確認：

ls -lh ~/rmf_ws/install/rmf_demos_assets/share/rmf_demos_assets/models/kachaka_shelf_3tier/

Step 2-5. Traffic Editor にモデルを出す（サムネ＋model_list）

Traffic Editor は、

  • ~/rmf_ws/te_thumbnails/model_list.yaml を読み
  • ~/rmf_ws/te_thumbnails/images/cropped/<model>.png を探す

という挙動でした。

te_thumbnails の最低構成を作る

mkdir -p ~/rmf_ws/te_thumbnails/images/cropped

サムネを置く

サムネ画像は kachaka_shelf_3tier.png という名前で用意します（手元の png をコピーするだけ）。

cp /tmp/kachaka_shelf_3tier.png ~/rmf_ws/te_thumbnails/images/cropped/kachaka_shelf_3tier.png
ls -lh ~/rmf_ws/te_thumbnails/images/cropped/kachaka_shelf_3tier.png

model_list.yaml にモデル名を追記

# なければ作る（最小の形式：YAMLのlist）
touch ~/rmf_ws/te_thumbnails/model_list.yaml

# 追記（重複しないように）
python3 - <<'PY'
import yaml, os
path = os.path.expanduser("~/rmf_ws/te_thumbnails/model_list.yaml")
name = "kachaka_shelf_3tier"

try:
    data = yaml.safe_load(open(path, "r", encoding="utf-8"))
except Exception:
    data = None

if data is None:
    data = []

if isinstance(data, list):
    if name not in data:
        data.append(name)
        data.sort()
else:
    # 形式が違うなら、強制的にlistへ（この用途ではlistで十分）
    data = [name]

with open(path, "w", encoding="utf-8") as f:
    yaml.safe_dump(data, f, sort_keys=False, allow_unicode=True)

print("[OK] updated:", path)
PY

確認：

grep -n "kachaka_shelf_3tier" ~/rmf_ws/te_thumbnails/model_list.yaml

Traffic Editor の設定（Thumbnail Path）

Traffic Editor を起動して、

  • Edit → Preferences…
  • Thumbnail Path を ~/rmf_ws/te_thumbnails に設定

Step 2-6. Traffic Editor で配置

  1. traffic-editor を起動
  2. evtest_kachaka.building.yaml を開く
    • ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos_maps/maps/evtest_kachaka/evtest_kachaka.building.yaml
  3. Add Model で kachaka_shelf_3tier を選んで配置
  4. 保存

Step 2-7 world に反映（Gazebo で出す）

Traffic Editor で保存できたら、rmf_demos_maps をビルドして world を再生成します。

cd ~/rmf_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select rmf_demos_maps
source ~/rmf_ws/install/setup.bash

world に入ったか確認：

grep -n "kachaka_shelf_3tier" \
  ~/rmf_ws/install/rmf_demos_maps/share/rmf_demos_maps/maps/evtest_kachaka/evtest_kachaka.world | head

起動して配置を確認します。この段階では着脱ノードはまだ不要なので、汎用の simulation.launch.xml に map_name を渡して起動します（着脱ノードを組み込んだ専用 evtest_kachaka.launch.xml は Step 3 で使います。

ros2 launch rmf_demos_gz_classic simulation.launch.xml map_name:=evtest_kachaka

5.4 Step 3. Open-RMF ＋ Gazebo プラグインで取り付け / 切り離しを再現する

ここからは、Step 2 で作った棚を「下に潜り込んで結合し → 離れて置く」という動作にします。Open-RMF のタスク (perform_action) と Gazebo の link attacher プラグインを組み合わせ、間を取り持つ小さな ROS 2 ノードを置くのがポイントです。

棚のSDF からは static を外し、運用は building.yaml の static フラグで切り替える
Step 2-3 では配置・見た目確認のため <static>true</static> を入れていましたが、静的モデルは link attacher で結合しないため、着脱させる本ステップで SDF から <static> タグを削除します。kachaka_shelf_3tier の SDF はこの1つだけで、静的/動的の切り替えは building.yaml の配置インスタンス側で行います（今回は static: false）。

着脱用へ修正したmodel.sdf
<model name="kachaka_shelf_3tier">
  <!-- static は外す（静的モデルは link attacher で結合しない） -->
  <link name="base_link">
    <visual name="visual"> ... </visual>
    <collision name="collision">
      <geometry>
        <box>
          <!-- 全高 70cm に合わせる（Step 2-2 の設計値と一致させる） -->
          <size>0.55 0.55 0.70</size>
        </box>
      </geometry>
      <!-- 棚パッドを無摩擦に。デタッチ後のスライド挙動の前提 -->
      <surface>
        <friction>
          <ode>
            <mu>0</mu>
            <mu2>0</mu2>
          </ode>
        </friction>
      </surface>
    </collision>
  </link>
</model>

Step 3-0. 事前準備: link attacher プラグインの導入

棚の剛体結合には、Gazebo Classic 用の link attacher プラグインを使います。本記事で使ったのは純正の gazebo_ros_link_attacher ではなく、IFRA-Cranfield 版の IFRA_LinkAttacher（Apache-2.0） です。プラグインのファイル名が libgazebo_link_attacher.so（ros が付かない）で純正と紛らわしいので注意してください。

cd ~/rmf_ws/src
git clone https://github.com/IFRA-Cranfield/IFRA_LinkAttacher.git
cd ~/rmf_ws
colcon build --symlink-install
source ~/rmf_ws/install/setup.bash

起動後、サービスが生えていれば導入成功です。

ros2 service list | grep -E "ATTACHLINK|DETACHLINK"
# /ATTACHLINK
# /DETACHLINK

このプラグインを world にどう読み込ませているか
world ファイルを手で編集するのではなく、rmf_demos_maps のビルド時にプラグイン宣言を .world へ自動挿入しています（evtest_kachaka ワールドのみ対象）。別の world で使いたい場合は、生成済み .world の <world name="world"> 直後に <plugin name="gazebo_link_attacher" filename="libgazebo_link_attacher.so"/> を追記すれば同じことができます。

Step 3-1. launch ファイルとフリート設定

新しい launch rmf_demos_gz_classic/launch/evtest_kachaka.launch.xml（Step 1 で使ったもの）に、自作ノード kachaka_shelf_attacher.py の起動を追加します。要点だけ抜粋：

<arg name="robot_name" default="tinyRobot_2"/>
<node pkg="rmf_demos" exec="kachaka_shelf_attacher.py"
      name="kachaka_shelf_attacher" output="screen">
  <param name="robot_name"      value="$(var robot_name)"/>
  <param name="shelf_name"      value="kachaka_shelf_dispensable"/>
  <param name="dispenser_name"  value="kachaka_shelf_pickup_dispenser"/>
  <param name="ingestor_name"   value="kachaka_shelf_dropoff_ingestor"/>
  <param name="fleet_name"      value="tinyRobot"/>
</node>

このノードに渡しているのは「どのロボットがアタッチ対象か」「どの棚モデルか」だけ。挙動の微調整は Python 側のデフォルト値で持ちます。

Step 3-2. 落とし穴: dispenser / ingestor は building.yaml に「無い」

ここが一番ハマりやすいポイントです。上の launch で渡している dispenser_name / ingestor_name、すなわち kachaka_shelf_pickup_dispenser と kachaka_shelf_dropoff_ingestor は、building.yaml をいくら grep しても出てきません。

これらは、ビルド時にスクリプトが .world ファイルへ直接注入する TeleportDispenser / TeleportIngestor モデルだからです。building.yaml には存在せず、生成後の world にだけ現れます。「定義はどこ？」と building.yaml を探しても見つからないので、最初は確実に詰まります。

なお、注入する ingestor の配置座標はスクリプト内にハードコードしています。この座標は私の環境の配置に合わせた値なので、別の環境ではマップに合わせて変更が必要です。

Step 3-3. なぜ自作ノードが必要だったのか

最初は標準の TeleportDispenser / TeleportIngestor だけで実装しようとしましたが、以下 2 つの理由で動きませんでした。

(a) 棚が空中 1m 以上の高さで生成される
PlaceOnEntity は対象 entity の collision バウンディングボックス上端に物を置く仕様のため、collision の高さや <pose> の取り方によっては棚がカチャカの上空に「テレポート」してしまい、潜り込んだ感じがだせません。

(b) デタッチ後に棚がずっとスライドする

棚パッドの摩擦を mu=0（摩擦ゼロ）にしているため、カチャカの動きで一度でも棚に速度が乗ると、TeleportIngestor の SetWorldPose は位置のみ設定して twist (速度) を残し、棚が床を永遠に滑り続けます。

そこで標準プラグインをバイパスし、

  • Gazebo の /get_entity_state, /set_entity_state で棚の位置・速度を直接制御
  • link attacher プラグインの /ATTACHLINK, /DETACHLINK で剛体結合
  • 標準の DispenserRequest / IngestorRequest のフローも互換のために併存

する小さな ROS 2 ノードを書きました。

Step 3-4. RMF のタスクから自作ノードが呼ばれるまで

RMF のタスク JSON に書いた attach / detach が、どうやって自作ノードのサービスに届くのか。配線はこうなっています。フリートアダプタ側で add_performable_action('attach', ...) を宣言してそのカテゴリを受理可能にし、attach のアクションが来たら /kachaka_shelf_attacher/attach（std_srvs/Trigger）を呼ぶ、という流れです。

キモは、ノードが success=True を返した時点でアダプタが execution.finished() を呼び、その phase を完了扱いにすること（タイムアウトは 30 秒）。逆に言えば、ノード側のコールバックが棚の取り込み・配置・ATTACHLINK まで終えて True を返すまで、RMF はそのアクションを待ち続けます。

JSON(perform_action: attach)
  → Fleet Adapter が attach を受理 (add_performable_action)
  → /kachaka_shelf_attacher/attach を call
  → ノードの _attach_cb (get_entity_state → set_entity_state → ATTACHLINK)
  → success=True
  → execution.finished() で phase 完了

detach も同じ経路で /kachaka_shelf_attacher/detach → _detach_cb に届きます。

Step 3-5. ノードの全体像

rmf_demos/scripts/kachaka_shelf_attacher.py の構成：

class KachakaShelfAttacher(Node):
    def __init__(self):
        super().__init__('kachaka_shelf_attacher')
        # ----- パラメータ宣言 -----
        self.declare_parameter('robot_name', 'tinyRobot_1')
        self.declare_parameter('shelf_name', 'kachaka_shelf_3tier')
        ...
        # 棚の向きオフセット (SDF が 90° ずれているため)
        self.declare_parameter('shelf_yaw_offset', math.pi / 2)
        # 棚の横方向オフセット (モデル原点がジオメトリ中心と一致しない)
        self.declare_parameter('attach_lateral_offset', 0.15)
        # デタッチ時の後方オフセット距離
        self.declare_parameter('detach_offset_distance', 0.5)
        # デタッチ後の棚ピン留め間隔
        self.declare_parameter('detach_pin_interval_sec', 0.3)

        # ----- サービス -----
        self.create_service(Trigger, '~/attach', self._attach_cb, ...)
        self.create_service(Trigger, '~/detach', self._detach_cb, ...)

本記事では「なぜこういう実装になっているか」のキーになる 3 点を深掘りします。

Step 3-6. つまずきポイント (1): DETACHLINK と get_entity_state の競合

最初の実装では「DETACHLINK → 短く sleep → ロボットのポーズ取得 → 棚を後方にピン留め」という順番でしたが、ロボットのポーズ取得がタイムアウトする事象に遭遇しました。

detach: sent DETACHLINK (fire-and-forget)
Service call timed out
get_entity_state failed for [tinyRobot_2]
detach done (could not pin shelf — robot pose unknown)

原因は DETACHLINK と get_entity_state が Gazebo Classic の同じ物理スレッドで直列化されていることです。DETACHLINK が物理エンジンの剛体破棄を処理している間、entity-state クエリは応答できません。

対策: ロボットのポーズを DETACHLINK の前に取得する ように順序を入れ替えました。

def _detach_cb(self, _req, res):
    # ロボットのポーズは DETACHLINK 送信前に取得しておく
    robot_pose = self._get_entity_pose(self._param('robot_name'))

    # 続いて DETACHLINK (fire-and-forget)
    self._detach_cli.call_async(detach_req)
    time.sleep(self._fparam('post_detach_settle_sec'))

    # 事前取得した robot_pose を使って棚の置き場所を計算
    ...

この一手だけでデタッチ成功率が安定しました。

Step 3-7. つまずきポイント (2): デタッチ後の棚が滑る

mu=0 の棚パッドのため、デタッチ直後のカチャカの離脱動作で棚に少しでも力が入ると、棚が床を永遠に滑り続けます。

対策: デタッチ位置を「ロボットの後方 30 〜 50 cm」とし、かつデタッチ後にバックグラウンドタイマーで一定間隔ごとに SetEntityState を再発行して棚を固定します。SetEntityState のデフォルト twist は 0 なので、棚を完全に静止状態に張り付けることができます。

def _start_pin(self, x, y, z, yaw):
    self._pin_target = (x, y, z, yaw)
    period = self._fparam('detach_pin_interval_sec')   # 既定 0.3 s
    self._pin_timer = self.create_timer(
        period, self._pin_tick, callback_group=self._pin_cb_group)

def _pin_tick(self):
    if self._pin_target is None:
        return
    x, y, z, yaw = self._pin_target
    self._set_shelf_pose(x, y, z, yaw)

ピン留めは「次の attach 呼び出しが来た瞬間」に解除します。次の配達タスクが始まるまで棚は完全に静止し続け、カチャカが近くを通っても押し動かされません。

Step 3-8. つまずきポイント (3): 棚の向きと位置のずれ

実際に動かすと、

  • 棚の SDF 上の「正面」がカチャカの正面と 90° ずれている（モデル仕様）
  • 棚の原点がジオメトリ中心と一致しておらず、アタッチすると見た目が左右にずれる

ということが分かりました。これは SDF 自体を直すのが本筋ですが、トライ&エラーで詰めたい段階だったため、パラメータで実行時調整できる ようにしました。

  • shelf_yaw_offset (rad, 既定 π/2)
  • attach_lateral_offset (m, ロボット座標系の +Y = 左, 既定 0.15)

これらを Python 側のデフォルト値か launch 側 <param> で微調整して、見た目が一番自然な値に落とし込みます。

Step 3-9. タスク JSON

配達タスクは JSON で定義します。rmf_demos/scripts/kachaka_deliver.json:

{
  "phases": [
    {
      "activity": {
        "category": "sequence",
        "description": {
          "activities": [
            { "category": "go_to_place",    "description": "kachaka_shelf_pickup" },
            { "category": "perform_action", "description": {
                "category": "attach",
                "expected_finish_location": "kachaka_shelf_pickup",
                "unix_millis_action_duration_estimate": 10000,
                "description": {},
                "use_tool_sink": false
              }
            },
            { "category": "go_to_place",    "description": "coke_pickup_2" },
            { "category": "perform_action", "description": {
                "category": "detach",
                "expected_finish_location": "coke_pickup_2",
                "unix_millis_action_duration_estimate": 10000,
                "description": {},
                "use_tool_sink": false
              }
            },
            { "category": "go_to_place",    "description": "point_1_L2" }
          ]
        }
      }
    }
  ]
}

末尾の go_to_place: point_1_L2 がポイントです。これを書かない場合、デタッチ完了直後にカチャカは「最寄りの is_charger: true の vertex」、つまりチャージャに自動で戻ろうとします。チャージャがデタッチした棚の真横にあると、棚に近寄ってしまい見た目が悪いので、明示的に 離れた方向の vertex を経由 させて棚との距離を稼ぐ書き方にしています。

Step 3-10. 実行

起動（ターミナル 1）

ログを後で参照できるよう tee で保存します。

mkdir -p ~/rmf_logs
source ~/rmf_ws/install/setup.bash
ros2 launch rmf_demos_gz_classic evtest_kachaka.launch.xml \
  2>&1 | tee ~/rmf_logs/sim_$(date +%Y%m%d_%H%M%S).log

タスク投入（ターミナル 2）

source ~/rmf_ws/install/setup.bash
ros2 run rmf_demos_tasks dispatch_json \
  -f ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos/scripts/kachaka_deliver.json \
  --use_sim_time 2>&1 | tee ~/rmf_logs/dispatch_$(date +%Y%m%d_%H%M%S).log

期待される動作

シミュレータ側ログに以下が順番に現れれば成功です：

attach: True — ATTACHED: {tinyRobot_2, base_footprint} -- {kachaka_shelf_dispensable, base_link}
detach: sent DETACHLINK (fire-and-forget)
shelf pin engaged @ (x, y) — held until next attach

Gazebo 上は、

  1. tinyRobot_2 が kachaka_shelf_pickup へ移動
  2. 棚の下に潜り込み、棚がロボットに追従する
  3. coke_pickup_2 まで搬送
  4. デタッチで棚がロボットの後方 50 cm に静置
  5. ロボットだけが point_1_L2 方向へ走り出す
  6. 棚はその場で静止し続ける

という流れになります。

6. トラブルシューティング（全体）

症状 原因 対策
見た目が Kachaka に変わらない world が model://TinyRobot を参照したまま world を再生成し、grep "model://TinyRobotKachaka" が出るか確認
Traffic Editor に棚が出てこない サムネ / model_list の場所が違う Thumbnail Path/images/cropped/<model>.png に置く。model_list.yaml にモデル名を追記
colcon が「symlink 作れない：Is a directory」で落ちる 壊れた build が残存 rm -rf build log → colcon build --symlink-install
/ATTACHLINK /DETACHLINK が見つからない link attacher プラグインが未ビルド or world に未挿入 IFRA_LinkAttacher を clone してビルド。`ros2 service list
dispenser_name / ingestor_name の定義が building.yaml に見当たらない これらは world へのビルド時注入で、building.yaml には存在しない 生成後の .world を確認。grep 対象を building.yaml ではなく world にする
棚がカチャカの 1m 上空に出現する TeleportDispenser の PlaceOnEntity がバウンディング Z 上端に置いた 自作ノードが SetEntityState で床面に置くので、自作ノード経由で attach する
デタッチ後に棚が滑り続ける 棚パッドの mu=0 + 残留 twist デタッチ後にピン留めタイマーを継続
get_entity_state failed for [tinyRobot_X] のあと棚が固定できない DETACHLINK と get_entity_state が物理スレッドで競合 ロボットポーズを DETACHLINK 前に取得
棚がカチャカに対して 90° ずれて見える SDF の正面軸の取り方 shelf_yaw_offset を ±π/2 で調整
棚の中心がカチャカからずれる モデル原点とジオメトリ中心のオフセット attach_lateral_offset を微調整

7. まとめ

本記事では、

  • TinyRobot の中身 (slotcar) はそのままに、見た目だけを Kachaka の STL に差し替え
  • STL を Blender で結合・加工して 3 段棚 (キャスター付き) を自作し、Gazebo モデル化して Traffic Editor で配置
  • Open-RMF のタスクと link attacher プラグインを組み合わせて、棚の取り付け / 切り離し (潜り込み搬送) を再現

という 3 本立てをステップバイステップでまとめました。見た目の差し替えは world の参照先がすべてなので、grep "model://TinyRobotKachaka" の一発チェックが効きます。アタッチ系は link attacher プラグインの素朴な使い方では躓きやすく、特に DETACHLINK の物理スレッド占有、デタッチ後の棚スライド、そして dispenser / ingestor が building.yaml に無い (world へのビルド時注入) という点は、他のロボットへの応用にも効くと考えられます。

8. ライセンス表記

本記事は以下のオープンソース成果物を利用しています。各ライセンスの条件に従い、著作権表示・ライセンス・改変点を以下に示します。

1. ロボット本体メッシュ（kachaka_ros2_dev_kit）

  • リポジトリ: CyberAgentAILab/kachaka_ros2_dev_kit
  • ライセンス: Apache License 2.0（LICENSE）
  • 著作権表示: Copyright 2025 CyberAgent AI Lab
  • 改変点: 本体メッシュ (body.stl) を、RMF の TinyRobot モデルの visual として組み込みました（visual の差し替えのみ。slotcar / collision / joint は変更していません）。

2. ベース RMF 環境（rmf_demos）

  • リポジトリ: open-rmf/rmf_demos
  • ライセンス: Apache License 2.0（LICENSE）
  • 著作権表示: Copyright 2021 Open Source Robotics Foundation, Inc.
  • 改変点: デモ用に新しい map / demo (evtest_kachaka) を追加し、補助ノード等を加えて利用しました。

3. link attacher プラグイン（IFRA_LinkAttacher）

  • リポジトリ: IFRA-Cranfield/IFRA_LinkAttacher
  • ライセンス: Apache License 2.0
  • 改変点: 棚の剛体結合 (ATTACHLINK / DETACHLINK) のため、改変せず world プラグインとして利用しました。

4. 3 段棚モデル（kachaka_shelf_description）

  • リポジトリ: GAI-313/kachaka_shelf_description
  • ライセンス: MIT License
  • 改変点: 提供されている STL を入力として Blender で結合・加工し、3 段棚のモデルとして利用しました。

MIT License の条件に従い、著作権表示とライセンス文の全文を以下に掲載します。

MIT License

Copyright (c) 2026 GAI

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

棚モデルについて（謝辞）
本記事の 3 段棚は、GAI-313/kachaka_shelf_description のモデルを入力として Blender で結合・加工したものです。利用にあたり作者の GAI-313 さんに確認したところ、快く許諾いただき、リポジトリに MIT License も明示していただきました。この場を借りて御礼申し上げます。MIT License のもとで利用・改変・再配布・動画掲載が可能ですが、著作権表示 (Copyright (c) 2026 GAI) とライセンス文の併記が条件です。本記事末尾に記載しています。

なお、本記事執筆後にリポジトリが更新され、現在は棚が URDF モデルとして提供される構成（description.launch の shelf_type で段数を選択）に変わっています。本記事の手順は当時の STL を入力とする流れで記述していますので、最新のリポジトリ構成とは異なる場合があります。最新の使い方はリポジトリの README を参照してください。

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?