1. はじめに
前回の記事では、Open-RMF のデモ環境 rmf_demos（Gazebo Classic）上で、カチャカ型ロボット（TinyRobotKachaka）が棚の下に潜り込んでアタッチ／デタッチするところまでをやってみました。
ひととおり棚を運べるようになったので次は、「棚を載せたままエレベータで別の階に運んで、向こうの階に置いてある棚と入れ替えて戻ってこられないかなあ」と考えるようになりまして。これに取り組んでみたというのがこの記事の内容になります。
今回実装したシナリオは、以下の5ステップです。
- 下の階で棚A をアタッチする
- エレベータで上の階へ運ぶ
- 上の階で棚A を切り離す
- 上の階に最初から置いてある**別の棚B（赤）**の下に潜ってアタッチする
- 棚B をエレベータで下の階へ運び、元の場所で切り離して終了
「ロボットに荷物を載せたままエレベータに乗せる」という、RMF の標準機能には無い動きなので、いくつか独自の工夫とハマりどころがありました。本記事ではそれをステップバイステップでまとめます。
完成すると、こんな動きになります:
2. 実行環境
本記事は、前回の記事で構築した環境をそのまま使います。
- Ubuntu 22.04
- ROS 2 Humble
- Open-RMF (humble)
- Gazebo Classic 11
ワークスペースは ~/rmf_ws（colcon build --symlink-install）を前提に記述します。
3. 全体構成（変更するファイル）
今回の入れ替えデモは、地図・棚モデル・制御ノード・タスクをそれぞれ手直しして実現しますが、キーとなるのは棚の運び方です。方針は次の3点に集約されます。
- 剛体ジョイントをやめ、キネマティック運搬にする（毎フレーム、棚をロボットの現在姿勢に貼り付け直す）。これでエレベータを通り抜けられます。
- ロボット直下の棚を自動で選ぶので、下の階では棚A、上の階では棚Bが同じ仕組みで自然に選ばれます。
- 置く高さをロボットの高さに追従させ、標高の違う上階でも棚が床に正しく乗ります。
流れをざっくり書くと、こうなります。
アタッチ → 直下の棚を選ぶ → 毎フレーム棚をロボットに追従（運搬） →
デタッチ → ロボットの後方に棚を置く → その場に固定（次の attach まで保持）
これらを実現するために変更するファイルは、以下のとおりです。各ファイルの中身はこれから各 Step で順に説明するので、ここは「どこを触るのか」の情報としてざっと確認いただけるだけでOKです。
| ファイル | 役割 | 変更内容 |
|---|---|---|
rmf_demos_maps/maps/evtest_kachaka/evtest_kachaka.building.yaml |
建物・地図 | 上の階に棚B、ドロップ地点 point_2_L2、棚の位置調整 |
rmf_demos_maps/scripts/inject_link_attacher.py |
world生成後の注入 |
gazebo_ros_state プラグインを注入（重要） |
rmf_demos_assets/models/kachaka_shelf_3tier/model.sdf |
棚モデル | 浮いた当たり判定を削除 |
rmf_demos_assets/models/kachaka_shelf_3tier_red/ |
棚モデル（赤） | 上の階の棚を見分けるための赤い変種を新規作成 |
rmf_demos/rmf_demos/scripts/kachaka_shelf_attacher.py |
制御ノード | 複数棚対応＋キネマティック運搬＋階対応 |
rmf_demos/rmf_demos/launch/evtest_kachaka.launch.xml |
起動 | 棚名リストを渡す |
rmf_demos/rmf_demos/scripts/kachaka_elevator_swap.json |
タスク | 往復＋入れ替えの手順 |
ポイント:
--symlink-installのため、Python スクリプト・launch・タスクjson は
編集後再ビルド不要（再 launch だけ）。一方 building.yaml と モデル(SDF) の追加は
ビルド時に world が再生成されるのでcolcon buildが必要です。
4. 手順
4.1 Step 0. 前提（前回の環境が動いていること）
本記事は、前回の記事で作ったevtest_kachaka の環境（カチャカが棚の下に潜り込んでアタッチ／デタッチするところまで）が動作していることを前提に進めます。launch・building.yaml・制御ノードなどはこの環境で作成済みのものを土台にして、ここに「エレベータ越しの入れ替え」を足していく流れです。
まずは前回までの構成がそのまま起動できる状態を出発点にします。
4.2 Step 1. 地図に「上の階」「エレベータ」「2つ目の棚」を用意する
evtest_kachaka.building.yaml には既に下の階・上の階の2フロアと、それを繋ぐlift_1（lowest=下の階 / highest=上の階, 標高差5m）が定義されている前提です。
Step 1-1. 上の階に棚B を追加
上の階の models: に、もう一つの棚を追加します。座標はドロップ地点 point_0_L2に合わせます（ロボットがその真下に潜って拾うため）。
# levels: 上の階: models:
- {dispensable: false, model_name: kachaka_shelf_3tier_red,
name: kachaka_shelf_dispensable_2, static: false,
x: 778.86, y: 255.67, yaw: 1.5708, z: 0}
-
model_nameは後で作る赤い変種を指定（見分けるため）。 -
name（エンティティ名）はkachaka_shelf_dispensable_2。制御ノードはこの名前で棚を扱うので、モデルを赤に変えても動作は変わりません。 - building.yaml 上の
zはそのレベル相対です。world 生成時に上の階の標高(5m)が自動で加算され、最終的にz=5.0で配置されます。
Step 1-2. 切り離し地点 point_2_L2 を追加（頂点＋レーン）
「棚A を置く場所」と「棚B が待つ場所」は物理的に分ける必要があります（ロボットは棚B の真下に潜るため、棚A を載せたまま同じ場所には行けない）。そこで上の階の point_0_L2 の隣に point_2_L2 を追加し、棚A はここで切り離すことにします。
traffic-editor で頂点を打つか、building.yaml の vertices: に直接追記します。
# levels: 上の階: vertices:
- [778.089, 297.538, 0, point_2_L2, {is_holding_point: [4, true]}]
重要: 頂点を足しただけでは到達できません。lanes: にレーンも追加します（頂点インデックスはファイル内の並び順。ここでは 19=分岐点, 21=point_0_L2, 23=point_2_L2 の例）。
# levels: 上の階: lanes:
- [19, 23, {bidirectional: [4, true], graph_idx: [2, 0], ...}] # 分岐 ↔ point_2_L2
- [23, 21, {bidirectional: [4, true], graph_idx: [2, 0], ...}] # point_2_L2 ↔ point_0_L2
これで リフト → 分岐 → point_2_L2 → point_0_L2 と繋がります。
Step 1-3. 棚を少し動かして「押し」を回避
ロボットが拾いに近づくとき、足元の棚に体が当たって棚をズラしてしまうことがあります。棚の2D位置を少しずらすと改善します（本デモでは両棚を北に約0.7m）。
4.3 Step 2. エレベータ制御の前提となる Gazebo サービスを world に注入する
ここは最初に踏んだ大きな落とし穴でした。
制御ノードはロボットの姿勢取得・棚の配置に Gazebo の/get_entity_state /set_entity_state サービスを使います。これらは**gazebo_ros_state ワールドプラグイン**が提供します。
ところが evtest_kachaka の world は colcon build のたびに building.yaml から丸ごと再生成されるため、生成された .world に手で書き足したプラグインは毎回消えてしまいます。実際、地図を再ビルドした瞬間に attach が/gazebo/get_entity_state not available で全失敗しました。
対策は、world 生成後に走る POST_BUILD スクリプトrmf_demos_maps/scripts/inject_link_attacher.py にプラグイン注入を組み込むことです（ここなら再ビルドのたびに必ず復活する）。
# inject_link_attacher.py — gazebo_ros_state を注入
state_plugin_tag = (
'\n <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">'
'\n <ros><namespace>/</namespace></ros>'
'\n <update_rate>10.0</update_rate>'
'\n </plugin>'
)
if 'gazebo_ros_state' not in content:
content = content.replace(
'<world name="world">',
'<world name="world">' + state_plugin_tag, 1)
このスクリプトは元々
gazebo_link_attacher（/ATTACHLINK/DETACHLINK）や
TeleportDispenser/Ingestor も注入しています。gazebo_ros_stateをそこに加える形です。
Step 2-補足. inject_link_attacher.py は何をしているのか
上で追加した gazebo_ros_state の注入は、実はこのスクリプトが行う4つの注入処理のうちの1つです。読者が実際にこのファイルを編集することになるので、ここでスクリプト全体を整理しておきます。このスクリプトは、生成された .world に対して次の4つを（それぞれ「まだ無ければ」）注入します。
-
gazebo_link_attacherプラグイン —/ATTACHLINK/DETACHLINK
（剛体ジョイントで link 同士を連結）を提供。 -
gazebo_ros_stateプラグイン —/get_entity_state/set_entity_stateを提供。
本記事で追加したのはこれで、kachaka_shelf_attacher.pyがロボット姿勢の取得と
棚の配置・固定に使います。 - TeleportDispenser モデル — 棚のピック地点の隣に配置。標準の dispenserワークフロー用。棚の
<pose>を正規表現で拾い、その真横に置きます。 - TeleportIngestor モデル — ドロップ地点に配置。同じく標準ワークフロー用。
各ブロックとも先頭で if '...' not in content: と存在チェックしてから注入するので、**何度実行しても二重に入らない（冪等）**のがポイントです。colcon build のたびにworld が作り直され、そのたびにこの POST_BUILD スクリプトが走っても、既にあるものはスキップされます。
重要: このデモにとって本質的なのは 2 の
gazebo_ros_stateだけです。
本記事の運搬方式は剛体ジョイントも dispenser も使わない（Step 5 でキネマティック
運搬に切り替えた）ため、1・3・4 は前回環境から引き継いでいるだけで、入れ替え
デモでは実際には呼ばれません。とはいえ残しておいても害はなく、前回までの
dispenser ベースのデモも動くのでそのままにしています。入れ替えデモだけを最小構成で
動かしたい場合は、1・3・4 の注入は省いても構いません。
スクリプト全文は以下のとおりです。
#!/usr/bin/env python3
"""Inject gazebo_link_attacher plugin and TeleportDispenser into a generated .world file."""
import re
import sys
world_path = sys.argv[1]
content = open(world_path).read()
# 1. link attacher ワールドプラグイン（/ATTACHLINK, /DETACHLINK）
plugin_tag = '\n <plugin name="gazebo_link_attacher" filename="libgazebo_link_attacher.so"/>'
if 'gazebo_link_attacher' not in content:
content = content.replace(
'<world name="world">',
'<world name="world">' + plugin_tag,
1
)
print(f'[inject_link_attacher] patched link attacher plugin in {world_path}')
else:
print(f'[inject_link_attacher] link attacher already present, skipping')
# 2. gazebo_ros_state ワールドプラグイン（/get_entity_state, /set_entity_state）。
# kachaka_shelf_attacher.py がロボット姿勢の取得と棚の配置・固定に使う。
# world は rmf_demos_maps を colcon build するたびに再生成されるので、
# 生成後の world に手で書くのではなく必ずここで注入する（手編集は再ビルドで消える）。
state_plugin_tag = (
'\n <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">'
'\n <ros>'
'\n <namespace>/</namespace>'
'\n </ros>'
'\n <update_rate>10.0</update_rate>'
'\n </plugin>'
)
if 'gazebo_ros_state' not in content:
content = content.replace(
'<world name="world">',
'<world name="world">' + state_plugin_tag,
1
)
print(f'[inject_link_attacher] patched gazebo_ros_state plugin in {world_path}')
else:
print(f'[inject_link_attacher] gazebo_ros_state already present, skipping')
# 3. TeleportDispenser を kachaka_shelf_dispensable の隣に注入
if 'kachaka_shelf_dispensable' in content and 'kachaka_shelf_pickup_dispenser' not in content:
# 棚の include ブロックを探して pose を取り出す
m = re.search(
r'(<include>\s*<name>kachaka_shelf_dispensable</name>.*?<pose>([\d\.\-e]+ [\d\.\-e]+) [\d\.\-e]+ [^\<]*</pose>.*?</include>)',
content, re.DOTALL
)
if m:
shelf_xy = m.group(2) # "x y"
dispenser_block = (
'\n <include>'
f'\n <name>kachaka_shelf_pickup_dispenser</name>'
f'\n <uri>model://TeleportDispenser</uri>'
f'\n <pose>{shelf_xy} 0.0 0 0 0.0</pose>'
'\n </include>'
)
content = content.replace(m.group(1), m.group(1) + dispenser_block, 1)
print(f'[inject_link_attacher] injected TeleportDispenser at ({shelf_xy}) in {world_path}')
else:
print('[inject_link_attacher] WARNING: could not locate shelf pose, skipping TeleportDispenser injection')
else:
print(f'[inject_link_attacher] TeleportDispenser already present or no shelf found, skipping')
# 4. ドロップ地点に TeleportIngestor を注入。
# 地点は nav graph 側の waypoint で world には現れないため XY はハードコード。
DROPOFF_NAME = 'kachaka_shelf_dropoff_ingestor'
DROPOFF_XY = '22.054915851060734 -2.2016351220606003'
if 'kachaka_shelf_dispensable' in content and DROPOFF_NAME not in content:
ingestor_block = (
'\n <include>'
f'\n <name>{DROPOFF_NAME}</name>'
f'\n <uri>model://TeleportIngestor</uri>'
f'\n <pose>{DROPOFF_XY} 0.0 0 0 0.0</pose>'
'\n <static>True</static>'
'\n </include>'
)
# </world> の直前に差し込む
content = content.replace('</world>', ingestor_block + '\n </world>', 1)
print(f'[inject_link_attacher] injected TeleportIngestor at ({DROPOFF_XY})')
else:
print(f'[inject_link_attacher] TeleportIngestor already present or no shelf found, skipping')
open(world_path, 'w').write(content)
補足として、TeleportIngestor の座標 DROPOFF_XY はハードコードです（この地点はnav graph 側の waypoint で world ファイルには現れないため）。地図を作り変えて地点がずれた場合は、ここも合わせて更新する必要があります。一方 TeleportDispenser の方は棚の <pose> を正規表現で拾って隣に置くので、棚を動かせば自動で追従します。
教訓: 生成される .world は手で編集しない。必要なプラグインは生成スクリプトで注入する。再ビルド後は grep gazebo_ros_state <world> とros2 service list | grep entity_state で確認します。
4.4 Step 3. 棚モデルの当たり判定を見直す
棚モデル kachaka_shelf_3tier/model.sdf には、もともとz=2.0 に浮いた大きな当たり判定の箱がありました。これは「カチャカが下を潜れるように」「TeleportDispenser の bbox 計算用」に置かれたものです。
しかし今回の制御方式（後述）では dispenser を完全にバイパスしており、この浮き箱は不要です。さらに、この箱がエレベータのドア枠やかご壁に干渉して出庫を妨げる一因にもなり得ます。そこで削除し、床に接地するための
小さな足pad（4個, 摩擦0）と見た目（visual）だけ残します。
<!-- model.sdf: 浮いた collision_body は削除。足padのみ残す -->
<collision name="pad_fl">
<pose>0.23 0.23 0.01 0 0 0</pose>
<geometry><box><size>0.04 0.04 0.02</size></box></geometry>
<surface><friction><ode><mu>0.0</mu><mu2>0.0</mu2></ode></friction></surface>
</collision>
<!-- pad_fr / pad_rl / pad_rr も同様 -->
<visual name="visual">
<geometry>
<mesh><uri>model://kachaka_shelf_3tier/meshes/shelf_3tier.stl</uri></mesh>
</geometry>
</visual>
4.5 Step 4. 上の階の棚を「赤」にして見分ける
2つの棚は同じモデルなので、上の階の棚だけ色を変えるには色付きの変種モデルを作って割り当てます（Gazebo Classic では <include> 単位の色上書きが難しいため）。
rmf_demos_assets/models/kachaka_shelf_3tier_red/ を新規作成し、メッシュは元モデルを再利用、<visual> に赤マテリアルを足します。
<!-- kachaka_shelf_3tier_red/model.sdf （要点）-->
<model name="kachaka_shelf_3tier_red">
<link name="base_link">
<!-- 足pad 4個は kachaka_shelf_3tier と同一 -->
<visual name="visual">
<geometry>
<mesh><uri>model://kachaka_shelf_3tier/meshes/shelf_3tier.stl</uri></mesh>
</geometry>
<material>
<ambient>0.8 0.0 0.0 1</ambient>
<diffuse>0.8 0.0 0.0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
</material>
</visual>
</link>
</model>
model.config も忘れずに（<name>kachaka_shelf_3tier_red</name>）。色を黒にしたい場合は ambient/diffuse を 0 0 0 1 に変えるだけです。
補足: 新しいモデルディレクトリを足したら
colcon build --packages-select rmf_demos_assetsが必要です
（install にシンボリックリンクが作られる）。
4.6 Step 5. 制御ノードを「複数棚＋キネマティック運搬」に作り替える
ここが本記事の核心です。kachaka_shelf_attacher.py を改修します。
Step 5-1. 複数棚に対応し、ロボット直下の棚を自動選択
attach サービスは「ロボットが今いる場所の真下にある、まだ運んでいない棚」を自動で選びます。これにより、下の階では棚A、上の階では棚B が同じサービスで自然に選ばれます。
def _select_shelf_under_robot(self, robot_pose):
"""ロボットに最も近い未運搬の棚を attach_max_radius(≈2m) 以内で選ぶ"""
rx, ry = robot_pose.position.x, robot_pose.position.y
max_r = self._fparam('attach_max_radius')
best_name, best_d2 = None, None
for name in self._shelf_names(): # shelf_names パラメータ（配列）
if name == self._carried_shelf:
continue
pose = self._get_entity_pose(name)
if pose is None:
continue
d2 = (pose.position.x - rx)**2 + (pose.position.y - ry)**2
if d2 <= max_r*max_r and (best_d2 is None or d2 < best_d2):
best_name, best_d2 = name, d2
return best_name
Step 5-2. つまずきポイント: 剛体ジョイントだとエレベータから降りられない
ここが今回いちばんの山場でした。最初は /ATTACHLINK（剛体の固定ジョイント）で棚をロボットに連結していました。平らな床では問題ありませんが、エレベータに乗るとロボットがかご内で固まって降りられなくなりました。
原因は、lift プラグインはロボット（slotcar）をキネマティックに（位置を直接書き換えて）動かすのに対し、剛体ジョイントがその制御と衝突するためです（ODEJoint::SetDamping ... out of bounds というエラーも出ます）。棚なしでは普通に降車できることから、棚の連結方法が原因と切り分けられました。
そこで運搬方式を変更しました:
剛体ジョイントで固定するのをやめ、タイマーで毎フレーム、棚をロボットの現在姿勢に SetEntityState で"貼り付け直す"。つまり棚を物理連結せず、毎フレーム位置だけ上書きして追従させる方式です（本記事ではこれを「キネマティック運搬＝パペット方式」と呼びます）。
こうするとロボットには棚からの拘束力が一切かからず、棚なしと完全に同じ挙動になる（＝エレベータを通れる）一方、棚は見た目どおり付いてきます。
def _update_tick(self):
# 運搬中の棚をロボットに貼り付け（キネマティック運搬）
carried = self._carried_shelf
if carried is not None:
robot_pose = self._get_entity_pose(self._param('robot_name'), timeout=1.0)
if robot_pose is not None:
x, y, z, yaw = self._carried_shelf_pose(robot_pose)
self._set_shelf_pose(carried, x, y, z, yaw, timeout=1.0)
# 切り離した棚はその場に保持（足padは摩擦0なので放置すると滑る）
for name, (x, y, z, yaw) in list(self._pin_targets.items()):
self._set_shelf_pose(name, x, y, z, yaw, timeout=1.0)
attach は「棚を選ぶ → 運搬中に設定 → タイマー開始」だけ。剛体連結はしません。
def _attach_cb(self, _req, res):
robot_pose = self._get_entity_pose(self._param('robot_name'))
shelf_name = self._select_shelf_under_robot(robot_pose)
...
self._stop_pin(shelf_name) # 待機保持を解除
x, y, z, yaw = self._carried_shelf_pose(robot_pose)
self._set_shelf_pose(shelf_name, x, y, z, yaw) # まず1回置く
self._carried_shelf = shelf_name # 以後はタイマーが追従
self._ensure_update_timer()
res.success = True
return res
Step 5-3. 置く高さはロボットのz座標に追従させる（多階対応）
上の階は標高5m。棚の高さを 0 固定にすると、上階で棚が床より5m下に出ます。配置は必ずロボット自身のz座標を基準にします。
def _carried_shelf_pose(self, robot_pose):
yaw = self._yaw_from_quat(robot_pose.orientation)
shelf_yaw = yaw + self._fparam('shelf_yaw_offset') # 棚は90°ずれている
lat = self._fparam('attach_lateral_offset') # 中心オフセット≈0.15m
x = robot_pose.position.x - lat * math.sin(yaw)
y = robot_pose.position.y + lat * math.cos(yaw)
z = robot_pose.position.z + self._fparam('shelf_floor_z') # ← ロボットzに追従
return x, y, z, shelf_yaw
Step 5-4. 切り離しは「後方に置いて、その場に保持」
detach は運搬を止め、ロボットの少し後ろに棚を置き、_pin_targets に登録してその場に固定します（足padが摩擦0なので、固定しないとロボットが離れる際に滑ってしまう）。棚ごとに保持できるので、上の階の棚A と下の階の棚B を同時に保持できます。
4.7 Step 6. 起動ファイルに棚名リストを渡す
rmf_demos/rmf_demos/launch/evtest_kachaka.launch.xml の制御ノードに、
扱う棚の名前を配列で渡します。
<node pkg="rmf_demos" exec="kachaka_shelf_attacher.py" name="kachaka_shelf_attacher" output="screen">
<param name="robot_name" value="$(var robot_name)"/> <!-- 例: tinyRobot_2 -->
<param name="shelf_names" value="[kachaka_shelf_dispensable, kachaka_shelf_dispensable_2]"/>
<param name="fleet_name" value="tinyRobot"/>
</node>
attach/detachは RMF のperform_actionから fleet_adapter 経由で
/kachaka_shelf_attacher/attach/detach（std_srvs/Trigger）として呼ばれます。
どの棚かはノード側が自動選択するので、RMF 側の配線は変更不要です。
4.8 Step 7. 入れ替えタスクを書く
rmf_demos/rmf_demos/scripts/kachaka_elevator_swap.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": "point_2_L2" },
{ "category": "perform_action", "description": {
"category": "detach", "expected_finish_location": "point_2_L2",
"unix_millis_action_duration_estimate": 10000, "description": {}, "use_tool_sink": false } },
{ "category": "go_to_place", "description": "point_0_L2" },
{ "category": "perform_action", "description": {
"category": "attach", "expected_finish_location": "point_0_L2",
"unix_millis_action_duration_estimate": 10000, "description": {}, "use_tool_sink": false } },
{ "category": "go_to_place", "description": "kachaka_shelf_pickup" },
{ "category": "perform_action", "description": {
"category": "detach", "expected_finish_location": "kachaka_shelf_pickup",
"unix_millis_action_duration_estimate": 10000, "description": {}, "use_tool_sink": false } }
] } } }]
}
go_to_place で上の階の地点を指定すると、RMF が自動で lift_1 を呼び、ロボットを階移動させます（フロア間のレーンは building 生成時に自動で作られます）。
4.9 Step 8. ビルドと実行
building.yaml・モデルを変更したので、地図とアセットを再ビルドします（Python/launch/json だけの変更なら再ビルド不要）。
cd ~/rmf_ws
colcon build --packages-select rmf_demos_assets rmf_demos_maps
シミュレーションを起動:
cd ~/rmf_ws && source install/setup.bash
ros2 launch rmf_demos_gz_classic evtest_kachaka.launch.xml \
| tee ~/rmf_logs/sim_$(date +%Y%m%d_%H%M%S).log
別ターミナルでタスクを投入:
cd ~/rmf_ws && source install/setup.bash
ros2 run rmf_demos_tasks dispatch_json \
-f src/demonstrations/rmf_demos/rmf_demos/scripts/kachaka_elevator_swap.json \
--use_sim_time \
| tee ~/rmf_logs/dispatch_$(date +%Y%m%d_%H%M%S).log
期待される動き:
- 下の階
kachaka_shelf_pickupで棚A をアタッチ - lift で上の階へ（棚を載せたまま降車できる）
-
point_2_L2で棚A を切り離し -
point_0_L2で赤い棚B をアタッチ - lift で下の階へ
-
kachaka_shelf_pickupで棚B を切り離して終了
5. トラブルシューティング（全体）
| 症状 | 原因 | 対策 |
|---|---|---|
再ビルド後 attach が全失敗 (get_entity_state not available) |
world 再生成で gazebo_ros_state プラグインが消える |
生成スクリプトで毎回注入する |
| 上階で棚が床下5mに出る | 配置zを0固定にしていた | ロボットのzに追従させる |
| 棚を載せるとエレベータから降りられない | 剛体ジョイントが lift のキネマティック制御と衝突 | キネマティック運搬（毎フレーム貼り付け）に変更 |
| 棚がドア枠に引っかかる | 浮いた当たり判定の箱 | 不要なので削除（足padのみ） |
| 拾う時にロボットが棚を押す | 軽い棚＋摩擦0で滑る | 棚位置を微調整／必要なら常時その場固定 |
| 2棚の見分けがつかない | 同一モデル | 赤い変種モデルを作り上の階用に割り当て |
切り分けで効いたのは「まず棚なしでエレベータ往復を試す」ことでした。
棚なしで降車できると分かったことで、原因を「棚の連結方法」に絞り込めました。
6. まとめ
本記事では、
- 「ロボットに荷物を載せたままエレベータ移動」を、制御ノードで棚をキネマティックにパペット制御することで実現しました。
- 物理ジョイントに頼らないことで、slotcar / lift のキネマティック制御と喧嘩せず、安定して階移動できました。
- 多階対応では「配置高さのロボットのz座標への追従」が必須でした。
- 生成される world は手編集せず、生成スクリプトでプラグインを注入しました。
同じように「ロボットで物を運ぶ」デモを RMF + Gazebo Classic で作りたい方の参考になれば幸いです。
7. ライセンス表記
本記事のシミュレーション環境は、前回の記事から引き続き複数のオープンソース成果物を組み合わせて構築しています。本体メッシュ（kachaka_ros2_dev_kit, Apache-2.0 /Copyright 2025 CyberAgent AI Lab）や link attacher プラグイン（IFRA_LinkAttacher,Apache-2.0）など、前回から引き続き利用しているものの詳しい出典・著作権表示・改変点は前回の記事にまとめています。ここでは、本記事で新たに変更・利用した範囲を中心に記載します。
1. ベース RMF 環境（rmf_demos）
- リポジトリ: open-rmf/rmf_demos
- ライセンス: Apache License 2.0
- 著作権表示:
Copyright 2021 Open Source Robotics Foundation, Inc. - 改変点: 新しい map / demo（
evtest_kachaka）に対し、入れ替え用のタスク・制御ノード
（複数棚対応＋キネマティック運搬＋階対応）を追加して利用しました。
2. Gazebo の状態取得／設定プラグイン（gazebo_ros_state）
- 提供元:
gazebo_ros_pkgs（Open Robotics） - ライセンス: Apache License 2.0
- 利用点:
libgazebo_ros_state.so（/get_entity_state//set_entity_state）を、
改変せず world プラグインとして利用しました（生成スクリプトで.worldに注入）。
ROS 2 / Gazebo Classic の標準ツールをそのまま使う形です。
3. 3 段棚モデル（kachaka_shelf_description）
- リポジトリ: GAI-313/kachaka_shelf_description
- ライセンス: MIT License
- 改変点: 提供されている STL を入力に Blender で結合・加工した
kachaka_shelf_3tierの
メッシュを、本記事では色違い（赤）の変種モデルkachaka_shelf_3tier_redでも再利用
しています（メッシュは共通で、<material>に赤を追加しただけ）。
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.