2
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(べべべふ)

ROS 2 + Gazebo で「人混みの中で働くロボット」を動かす — OpenRMF と HuNavSim を1つのワールドに統合する

2
Posted at

1. はじめに

前回の記事(ROS 2 + Gazebo で「人混みの中を走るロボット」を動かす — HuNavSim を一から構築してみた)では、歩行者シミュレータ HuNavSim をUbuntu環境に構築し、PMB2 ロボットの周りで人が反応する様子を動かしました。

今回はその続きで、ロボット側を OpenRMFに差し替えます。つまり、

  • ロボットは OpenRMF で動かす … ナビグラフ上を走り、エレベーターを呼び、タスクを実行する
  • 人は HuNavSim で動かす … HuNavSim で歩き、ロボットに反応する

この2つを ひとつの Gazebo Classicの中で同時に走らせるのが今回のゴールです。最終的に、ロボットがエレベーターで2つの階を往復して2つの棚を上下階（L3⇔L4）で入れ替えるタスクを実行し、その走行中に歩行者が反応する、という状態まで持っていきました。

▲ エレベーター前の廊下。棚を積んだ tinyRobot が近づくと、歩行者が立ち止まって振り向くという挙動がみられた

HuNavSim も OpenRMF も筆者が開発したものではありません。この記事は、両者を統合するために実際にどのソースをどう書き換えたかということを記載しています。

2. 実行環境

項目 内容
OS Ubuntu 22.04.5 LTS
ROS ROS 2 Humble
シミュレータ Gazebo Classic 11.10.2
GPU NVIDIA RTX 2070（無くても動きますが描画に影響します）

なお本記事では、下の階を L3（床の高さ z=0）、上の階を L4（床の高さ z=5）と呼びます。マップを作ったときの階層名をそのまま使っているだけで、建物の3階・4階という意味ではありません。

3. この統合が成立する理由

前提として押さえておきたいのは、本記事でのOpenRMF も HuNavSim も同じ Gazebo Classic 11 を使っていることです。

  • OpenRMF は rmf_demos_gz_classic 経由で gzserver にワールドを渡し、slotcar プラグインでロボットを走らせる
  • HuNavSim は hunav_gazebo_wrapper の libHuNavPlugin.so をワールドに埋め込み、<actor> を動かす

どちらも「1つの .world ファイル」に対して働くので、そのワールドファイルを合成してしまえば同居できる——のではないかと思いました。ロボットを spawn するのは RMF、人を動かすのは HuNavPlugin、という役割分担になります。

image.png

4. どこに手を入れたか

統合のために触ったのは 3つのリポジトリです。

リポジトリ 役割 主な変更
hunav_gazebo_wrapper(コピーしてきたもの) 歩行者側 C++ プラグイン2ファイルを修正 + シナリオ・BT 追加
rmf_demos(コピーしてきたもの) ロボット側 棚着脱ノードを新規追加 + フリートアダプタ拡張 + 自作マップ
統合グルー(新規作成) 合成 launch・ワールド合成スクリプト

ソース修正の実体は、これらのファイルです(歩行者側 fork の1コミット分)。

ファイル 内容
src/HuNavPlugin.cpp 凍結の修正(pause ラッパー削除) + 多層階対応(床 z オフセット)
src/WorldGenerator.cpp get_parameters の空応答にリトライガード
launch/simulation.launch.py GPU オフロード無効化・PMB2 パラメータ整合(前回記事の対応)
scenarios/agents_evtest.yaml 歩行者の定義
behavior_trees/agents_evtest__agent_{1..5}_bt.xml エージェント別 Behavior Tree

ロボット側 fork の主な変更は次の通りです。

ファイル 内容
rmf_demos/scripts/kachaka_shelf_attacher.py 棚の着脱・キネマティック搬送ノード
rmf_demos_fleet_adapter/.../fleet_adapter.py attach / detach を RMF のアクションとして登録
rmf_demos_fleet_adapter/.../fleet_manager.py 初期化前の null 参照で落ちるのを防ぐガード
rmf_demos_gz_classic/launch/simulation.launch.xml GAZEBO_PLUGIN_PATH に ros2_linkattacher を追加
rmf_demos/launch/evtest_kachaka.launch.xml 棚着脱ノードの起動とパラメータ（新規）
rmf_demos/scripts/kachaka_elevator_swap.json 棚入れ替えタスクの定義 （新規）
rmf_demos_maps/maps/evtest_kachaka/ 自作マップ(L3 / L4 + エレベーター) （新規）

以下、この変更を上から順に説明していきます。

5. ワールドを合成する

HuNavSim には hunav_gazebo_world_generator というワールドジェネレータがあります。ベースとなる .world に <actor> と libHuNavPlugin.so を注入して、新しいワールドを吐き出すノードです。ここに RMF が生成したワールドを合成していきます。

# 1) 歩行者定義を読み込むローダを起動
ros2 run hunav_agent_manager hunav_loader --ros-args \
  --params-file ~/hunav_ws/install/hunav_gazebo_wrapper/share/hunav_gazebo_wrapper/scenarios/agents_evtest.yaml &
sleep 4

# 2) RMF のワールドに actor と HuNavPlugin を注入
ros2 run hunav_gazebo_wrapper hunav_gazebo_world_generator --ros-args \
  -p base_world:=$HOME/rmf_hunav/evtest_kachaka_base.world \
  -p robot_name:=tinyRobot_2 \
  -p use_gazebo_obs:=false \
  -p update_rate:=100.0 \
  -p global_frame_to_publish:=map \
  -p use_navgoal_to_start:=false \
  -p ignore_models:="ground_plane"

出力された generatedWorld.world が、RMF の建物・ロボット・エレベーター + HuNav の歩行者4人 + HuNavPlugin を全部含んだ1枚のワールドになります。確認は <actor> の数を数えるのが手軽です。

grep -cE '<actor' generatedWorld.world   # => 4

agents_evtest.yaml や BT を編集したら、毎回この再生成が必要です。筆者は gen_merged_world.sh にまとめました。

ハマったところ①: use_gazebo_obs:=true だと歩行者が動けない

HuNavSim には「Gazebo 上のモデルを障害物として避ける」機能(use_gazebo_obs)があります。ところがこの障害物判定はモデルのバウンディングボックス基準です。

RMF のマップは建物全体がひとつの巨大なモデルとして読み込まれるため、そのバウンディングボックスは建物全域(おおよそ x:[2, 29], y:[-16, -0.6] m)を覆います。結果、歩行者はどこにいても「巨大な障害物の中」にいる扱いになり、斥力で押し出されてまともに歩けません。

対策: use_gazebo_obs:=false にして Gazebo 障害物判定を切り、代わりに歩行者のゴールを RMF のナビグラフのレーン上に置く。レーンはロボットが走る通路なので、壁がないことが保証されています。

このため agents_evtest.yaml のゴール座標は、maps/evtest_kachaka/nav_graphs/0.yaml の頂点・辺から拾っています。歩行者はゴール間を直線で歩くので、1本のレーン上に2点を置けば、その通路を往復するという作り方です。

# agents_evtest.yaml (抜粋) — ゴールはすべてナビグラフのレーン上
global_goals:
  1:  {x: 13.30, y: -5.09}   # レーンA(水平 y=-5.09)の左端
  6:  {x: 17.54, y: -5.09}   # レーンA の右端 -> agent1 がここを往復
  8:  {x:  5.22, y: -2.40}   # レーンC(水平 y=-2.40)
  9:  {x: 11.06, y: -2.40}
  10: {x: 13.0,  y: -4.0}    # L4 のリフトロビー側レーン
  11: {x: 18.5,  y: -4.0}
  12: {x: 14.0,  y: -5.5}    # L4 の1本南の帯
  13: {x: 18.5,  y: -5.5}

6. 起動順序を作る

合成ワールドができても、素直に起動するとシミュレーションが立ち上がりません。ここで2つハマりました。

ハマったところ②: 起動直後にデッドロックして /clock が進まない

HuNavPlugin は Gazebo のワールドロード中に /get_agents サービスを呼び、応答が来るまでブロックします。ところが /get_agents を提供するノードより先に Gazebo を起動すると、

  1. HuNavPlugin がワールドロードを止める
  2. /clock が発行されない
  3. use_sim_time: true の HuNav 系ノードが時刻待ちで止まる
  4. /get_agents がいつまでも立ち上がらない

という完全な循環待ちになります。gzserver は生きているのに何も進みません。

対策: launch で HuNav 系ノードを先に起動し、Gazebo を TimerAction で遅延させる。

ハマったところ③: /get_agents を提供しているのは hunav_agent_manager ではない

名前から hunav_agent_manager だと思い込んでいましたが、/get_agents を提供しているのは hunav_gazebo_world_generator です。

つまりこのノードは「ワールドを生成したら終了」ではなく、シミュレーション中ずっと動かし続ける必要がある。generate-and-exit にすると、Gazebo 起動時に HuNavPlugin が初期エージェント情報を取りに来た瞬間に応答できず、ハマったところ②のデッドロックに落ちます。

この2つを踏まえた起動順序が以下です。

# evtest_kachaka_hunav.launch.py (抜粋)
#   t=0  hunav_loader + hunav_agent_manager + RMF コア一式
#   t=2  hunav_gazebo_world_generator(loader のパラメータが必要 / /get_agents を提供)
#   t=8  gzserver + gzclient(このときサービスは全部揃っている)
delayed_worldgen = TimerAction(period=2.0, actions=[hunav_worldgen])
delayed_gazebo   = TimerAction(period=8.0, actions=[gzserver, gzclient])

return LaunchDescription([
    rmf, hunav_loader, hunav_manager, delayed_worldgen, delayed_gazebo])

hunav_gazebo_world_generator は Node として起動し、終了させません(ハマったところ③)。

このノードは5章で手動で叩いたものと同じで、起動のたびにワールドを生成し直したうえでそのまま常駐します。つまり launch を使う場合、5章のコマンドを毎回別に実行する必要はありません。gen_merged_world.sh のほうは、Gazebo を上げる前に <actor> の数だけ確認したいときに使っています。

7. エージェント別の Behavior Tree を用意する

ハマったところ④: BT ファイルが無いとマネージャが SIGABRT する

HuNavSim v2.0 は、エージェントごとに <yaml_base_name>__agent_<id>_bt.xml という専用の Behavior Tree ファイルを要求します。無いと hunav_agent_manager が BT::LogicError を投げて即死します。

=> [hunav_agent_manager]: Loading BT file: .../agents_evtest__agent_5_bt.xml
=> [hunav_agent_manager] process has died [SIGABRT]

対策: 既存の性格別 BT(cafe シナリオの Regular / Surprised / Threatening)をコピーし、中の SetGoal ノードの goal_id を新しいエージェントのゴール番号に書き換えるだけで動きます。

yaml_base_name: agents_evtest に対して、behavior_trees/agents_evtest__agent_1_bt.xml 〜 agent_5_bt.xml の5本を用意しました(各245〜254行)。うち agent_3 の分は 10.4 のとおり後で使わなくなったため、現在の構成では読み込まれません。

8. Gazebo のパスを通す

ハマったところ⑤: プラグインとメッシュのパスが足りず gzclient が落ちる

2つのシミュレータのリソースを1つの Gazebo に読ませるので、環境変数が両方分の合併になります。

変数 通すもの
GAZEBO_PLUGIN_PATH RMF のプラグイン2種 + ros2_linkattacher + HuNav の lib
GAZEBO_MODEL_PATH RMF のマップ models + rmf_demos_assets + HuNav の models(actor メッシュ)
GAZEBO_RESOURCE_PATH rmf_demos_assets + /usr/share/gazebo-11(シェーダ) + HuNav の models

特に /usr/share/gazebo-11 が抜けると、前回記事と同じく gzclient が Unable to find shader lib 系のアサートで落ちます。source /usr/share/gazebo/setup.sh を忘れないこと。

RMF 側の launch にも1行足しています。棚の着脱に使う ros2_linkattacher のプラグインを Gazebo が読めるようにするためです。

  <!-- rmf_demos_gz_classic/launch/simulation.launch.xml -->
- <let name="plugin_path" value="...rmf_robot_sim_gz_classic_plugins:...rmf_building_sim_gz_classic_plugins:/usr/share/gazebo-$(var gazebo_version)" />
+ <let name="plugin_path" value="...rmf_robot_sim_gz_classic_plugins:...rmf_building_sim_gz_classic_plugins:$(find-pkg-prefix ros2_linkattacher)/lib:/usr/share/gazebo-$(var gazebo_version)" />

9. 最大の難関: タスクを投げた瞬間にワールドが凍る

ここが一番苦労しました。エレベーターや棚の入れ替えタスクを dispatch した瞬間に、統合シミュレーションだけが凍る。gzserver のプロセスは生きているのに /clock が止まり、全部が固まります。しかも RMF 単体では起きないので、統合が原因なのは確実でした。

9.1 gdb で追う

凍った gzserver に gdb をアタッチして thread apply all bt を撮ると、全スレッドがアイドルで、物理スレッドが World::Step の pause ループの中にいました。

試しに手で unpause を叩くと、

ros2 service call /unpause_physics std_srvs/srv/Empty
# => 一瞬 /clock が進んで、すぐまた止まる

つまりワールドが「一時停止」状態に落ち込んで抜け出せなくなっている。誰かが pause フラグを立てたまま戻していない、ということです。

9.2 犯人は HuNavPlugin の pause ラッパー

HuNavPlugin.cpp の UpdateGazeboPedestrians() を読むと、歩行者1体ごと・毎更新ごとにこう書かれていました。

// src/HuNavPlugin.cpp UpdateGazeboPedestrians() 変更前
bool is_paused = world->IsPaused();
world->SetPaused(true);
model->SetWorldPose(actorPose);   // 歩行者をキネマティックに移動
world->SetPaused(is_paused);

これは グローバルな Gazebo の pause フラグを read-modify-write しています。しかも呼び出し元はワールド更新スレッドです。

一方、RMF 側で棚を運ぶ kachaka_shelf_attacher.py は、棚を「ロボットに接着したまま運ぶ」ために /set_entity_state を 0.1 秒周期で叩き続けます。こちらは gazebo_ros のエグゼキュータスレッドから同じワールド状態に触ります。

この2つの read-modify-write が interleave すると、unpause を1回取りこぼした時点でワールドが停止したまま固着します。棚搬送が始まる perform_action の瞬間に凍るのは、まさにこのタイミングで両者が同時にワールドを触るからでした。

ハマったところ⑥: pause フラグの競合でワールドが固着する

対策: pause ラッパーを丸ごと削除する。歩行者は SetWorldPose でキネマティックに動かしているだけなので、そもそも物理を止める必要がありません。

  // src/HuNavPlugin.cpp UpdateGazeboPedestrians()
- bool is_paused = world->IsPaused();
- world->SetPaused(true);
-   model->SetWorldPose(actorPose);  //, true, true); // false, false);
+   model->SetWorldPose(actorPose);
   if (useCollision)
   {
     gazebo::physics::ModelPtr body_model = world->ModelByName(a.name + "_body");
     actorPose.Rot() = ignition::math::Quaterniond(0,0, yaw);
-     body_model->SetWorldPose(actorPose);  //, true, true); // false, false);
+     body_model->SetWorldPose(actorPose);
   }
- world->SetPaused(is_paused);

ビルドし直すと、swap 中も /clock が止まらず、ロボットがエレベーターで L3 ↔ L4 を何度も往復できるようになりました。

10. 上の階に人を立たせる

HuNavSim は 床が z=0 の平面ひとつという前提で書かれています。今回のマップはエレベーターで 上下階を行き来する多層構造なので、そのままでは上の階に人を置けません。HuNavPlugin.cpp に3箇所手を入れました。

10.1 エージェントごとの床の高さを覚える

まず、spawn 時の z(= その階の床の高さ)を記憶するメンバを足します。

// src/HuNavPlugin.cpp class HuNavPluginPrivate に追加
/// エージェントごとの床の高さ(その階のワールド z)。InitializeAgents で spawn
/// pose から取得し、毎更新で skin の身長に足す。これで L4(z=5)の人が
/// L3(z=0)に落とされずに自分の階に立つ。
std::map<int, double> agentFloorZ;

// src/HuNavPlugin.cpp InitializeAgents() に追加
ignition::math::Pose3d actorPose = actor->WorldPose();
agentFloorZ[agent.id] = actorPose.Pos().Z();   // <- YAML の init_pose.z が入る

YAML の init_pose.z にはその階の床の高さを書きます(L3 なら 0、L4 なら 5)。人の身長ではありません。

10.2 マネージャに送る z は 0 に固定する

ここが効きました。HuNavSim の社会力モデルは 2D で、ゴール座標も z=0 で持っています。エージェントの実 z(L4 なら 5.0)をそのまま送ると、ゴールへの引力が真下(z=0 方向)を向いてしまい、水平方向の力がほとんど残りません。結果、上階の人はほぼ動かなくなります。

// src/HuNavPlugin.cpp GetPedestrians() に追加
pedestrians[i].position.position.x = xf;
pedestrians[i].position.position.y = yf;
// マネージャには平坦な z を報告する。SFM とゴール探索は 2D で、ゴールは z=0。
// 実 z(L4 なら 5.0)を送るとゴール引力が下向きになり水平方向に動かなくなる。
// ロボット側も z=0 で報告されているので、上階の人が x,y でロボットに反応できる。
// 見た目の床の高さは UpdateGazeboPedestrians の agentFloorZ で戻す。
pedestrians[i].position.position.z = 0.0;

つまり 「計算は全員 z=0 の平面で、描画だけ階ごとに持ち上げる」 という割り切りです。

10.3 描画時に床の高さを足し戻す

// src/HuNavPlugin.cpp UpdateGazeboPedestrians() に追加
// この直前で skin ごとに actor の絶対 z をセットしているが、そこは「床が z=0」前提。
// InitializeAgents で覚えた床の高さを足して、上階の人を自分の階に描く。
{
  auto fz = agentFloorZ.find(a.id);
  if (fz != agentFloorZ.end())
    actorPose.Pos().Z(actorPose.Pos().Z() + fz->second);
}

この3点と、7章の専用 BT で、L4 に人が立って歩くようになりました。

ハマったところ⑦: 上階の人が特定の場所で「固まる」

上記を直しても、L4 のある地点に置いた人が少し歩いて動かなくなることがありました。原因は完全には特定できていませんが、経験則として 既知の「歩ける帯」にゴールを置くと安定します。今回は L4 のエレベーター前の廊下帯(y ≈ -4.0 と y = -5.5)に配置しました。

ハマったところ⑧: 検知距離を広げたら「いない階のロボット」に反応した

agent5 をエレベーターから少し離した際、「離れても驚けるように」と BT の IsRobotVisible の検知距離を 1.4 → 3.0 に広げました。するとロボットが来ていないのに立ち止まるようになりました。

原因は 10.2 の割り切りの裏返しです。マネージャの世界では全員 z=0 なので、階が違ってもロボットを x, y だけで見ます。1つ下の L3 でエレベーター付近(x ≈ 15.8)を通過するロボットに、L4 の agent5 が反応していたのです。

対策: 検知距離を 1.4 に戻す。多層構造では「検知距離を欲張らない」のがコツです。

10.4 最終的なエージェント構成

エージェント 階 behavior skin max_vel 経路
agent1 L3 Regular 0(elegant_man) 1.0 水平レーン y=-5.09 を往復
agent2 L3 Surprised 1(casual_man) 1.2 水平レーン y=-2.40 を往復
agent3 ― ― ― ― 当初は L3 に配置。ロボットの走行レーン上で立ち止まってしまいタスクが進まないため削除
agent4 L4 Regular 3(regular_man) 1.0 リフトロビー側 y=-4.0 を往復
agent5 L4 Surprised 1(casual_man) 1.2 1本南の帯 y=-5.5 を往復 
# agents_evtest.yaml (抜粋) — L4 で立ち止まって驚く人
agent5:
  id: 5
  skin: 1
  max_vel: 1.2
  cyclic_goals: true
  init_pose:
    x: 17.0
    y: -5.5
    z: 5.0          # <- L4 の床の高さ(身長ではない)
    h: 0.0
  behavior:
    type: Surprised
    duration: 30.0
    once: true
    dist: 1.4       # <- 広げると階をまたいで誤検知する(ハマったところ⑧)
  goals:
    - 12
    - 13

L4 の agent4 は Regular にしています。理由は後述の限界にある通り、RMF のロボットは人を避けないためです。Surprised の人はその場で固まるので、ロボットの走行レーン上に立たせると突き抜けられてしまいます。そこで、ロボットがリフトを出て右に曲がる交差点に対し、agent4 は左の枝を歩かせて、通過するロボットに反応させつつ轢かれないようにしました。

11. RMF のタスクから棚を着脱する

ここは HuNavSim とは独立した、RMF 側の作り込みです。「棚を持ち上げて運ぶ」ロボットを RMF のタスクで動かすために、2つのものを足しました。

11.1 棚着脱ノード kachaka_shelf_attacher.py(新規 439行)

ros2_linkattacher の /ATTACHLINK / /DETACHLINK でロボットと棚を剛接合し、さらに /set_entity_state で棚の姿勢を 0.1 秒ごとに上書きしてキネマティックに運ぶノードです。パラメータで挙動を調整できるようにしました。

パラメータ 既定値 意味
attach_max_radius 2.0 この半径内の未装着の棚だけを「ロボットの下の棚」とみなす
detach_offset_distance 0.5 切り離すとき、ロボットの何 m 後ろに置くか
shelf_floor_z 0.0 床に置くときの z オフセット。階ごとに変えてはいけない
shelf_yaw_offset π/2 棚 SDF の正面軸がロボットの進行方向と 90° ずれている分の補正
attach_lateral_offset 0.15 棚の原点が幾何中心でないための横方向補正
shelf_update_interval_sec 0.1 搬送・保持ループの周期

shelf_floor_z を 0 に固定しているのがポイントです。ロボットの base_footprint の z はすでにその階の高さを追従している(L3≈0, L4≈5)ので、ここに床の高さを足すと上階で棚が 5 m 下に落ちます。

11.2 フリートアダプタに attach / detach アクションを登録(+56行)

RMF のロボットは速度やゴールで直接動かすものではなく、タスクを投げて動かします。棚の着脱をタスクの一部として実行させるため、perform_action のカテゴリを2つ追加しました。

# rmf_demos_fleet_adapter/fleet_adapter.py
fleet_handle = adapter.add_easy_fleet(fleet_config)
fleet_handle.more().add_performable_action('attach', rmf_adapter.consider_all())
fleet_handle.more().add_performable_action('detach', rmf_adapter.consider_all())

# 受け取ったアクションを棚着脱ノードの Trigger サービスに中継する
self._attach_cli = node.create_client(Trigger, '/kachaka_shelf_attacher/attach')
self._detach_cli = node.create_client(Trigger, '/kachaka_shelf_attacher/detach')

def execute_action(self, category: str, description: dict, execution):
    match category:
        case 'attach':
            self.attempt_cmd_until_success(
                cmd=self._call_trigger_service, args=(self._attach_cli, execution))
        case 'detach':
            self.attempt_cmd_until_success(
                cmd=self._call_trigger_service, args=(self._detach_cli, execution))

これで、タスク JSON の中に perform_action として棚の着脱を書けるようになります。

{"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": {}}}

なお fleet_manager.py にも小さなガードを足しています。状態がまだ来ていないロボットに navigate 要求が飛ぶと None 参照で落ちるためです。

- if robot_name not in self.robots or len(dest.destination) < 1:
+ if robot_name not in self.robots or dest.destination is None or len(dest.destination) < 1:
      return response
  robot = self.robots[robot_name]
+ if robot.state is None:
+     response['msg'] = 'Robot state not yet initialized'
+     return response

12. 棚の入れ替え(swap)タスク

仕上げは、エレベーターを使って L3 の棚を L4 へ、L4 の棚を L3 へ入れ替えるタスクです。kachaka_elevator_swap.json は go_to_place と perform_action を交互に、計8ステップ並べたシーケンスです。

go_to_place kachaka_shelf_pickup (L3) -> attach   … L3 の棚を積む
go_to_place point_2_L2          (L4) -> detach   … L4 に降ろす
go_to_place point_0_L2          (L4) -> attach   … L4 の棚を積む
go_to_place kachaka_shelf_pickup (L3) -> detach   … L3 に降ろす

補足: L4 のウェイポイント名が _L2 で終わっているのは、マップ作成時の名残です。実際には L4 上にあります。

ros2 run rmf_demos_tasks dispatch_json -c compose \
  -f ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos/scripts/kachaka_elevator_swap.json \
  -F tinyRobot -R tinyRobot_2 --use_sim_time

-R tinyRobot_2 は必須です。省略すると RMF が tinyRobot_1 に自動割り当てしてしまい、tinyRobot_2 に反応するよう設定した歩行者が何も反応しません。

ところが最初、入れ替わらず「同じ棚を持って帰ってくる」　という不具合がありました。

ハマったところ⑨: swap が「棚交換」にならない

kachaka_shelf_attacher.py の _select_shelf_under_robot() は、attach_max_radius 以内でロボットに最も近い未装着の棚を選びます。既定値は 2.0 m です。

swap の途中、ロボットは運んできた棚を detach_offset_distance(0.5 m)だけ後ろに置きます。次の attach 地点に移動しても、「たった今置いた棚」が半径 2.0 m の中に残っている。そしてそれがもう一方の棚より近ければ、同じ棚を掴み直してしまうわけです。

対策: 距離の大小関係を作って、半径で切り分ける。

  • もう一方の棚を、attach 地点から 0.90 m の位置に寄せる
  • 直前に置いた棚は attach 地点から 1.18 m 離れる配置にする
  • attach_max_radius を 2.0 → 1.1 に絞る

1.1 m は 0.90 m を含み 1.18 m を除外するので、必ず「もう一方の棚」が選ばれます。

<!-- rmf_demos/launch/evtest_kachaka.launch.xml -->
<param name="attach_max_radius" value="1.1"/>

検証は棚の z 座標をポーリングして行いました。

=> L3棚 z: 0 → 5   (L3 から L4 へ上昇)
=> L4棚 z: 5 → 0   (L4 から L3 へ下降)

この間 /clock は止まっておらず、ハマったところ⑥の凍結対策が効いていることも同時に確認できました。

13. agent5 を追加したら segfault した話

最後に、地味ですが厄介だったバグを1つ。agent5 を追加した瞬間、ワールドジェネレータが segfaultしました。まだ agent1〜agent5 の5体を定義していた頃の話で、10.4 のとおり agent3 を抜いたのはこの後です。

=> [hunav_gazebo_world_generator]: Agent parameters for agent5: 0 parameters found
=> [ros2run]: Segmentation fault

「agent5 のパラメータが 0 個」と言って落ちます。ところが調べると、

  • ros2 param get /hunav_loader agent5.id は正常に 5 を返す
  • しかし22個のパラメータを一括取得すると 0 個が返る
  • 同じ要求を繰り返すと、成功と失敗が交互に出る

という不安定な挙動でした。パラメータ総数(5人 × 22個 ≈ 110個)が増えたことで、パラメータサービスの応答が間欠的に空になる、FastDDS + Humble の同期の問題と思われます。

ハマったところ⑩: get_parameters の一括取得が間欠的に空を返す

WorldGenerator.cpp は返ってきた配列を aparams[0] で無条件に参照していたので、空応答でそのまま落ちていました。

対策: 期待した個数が返るまでリトライするガードを入れる。再要求すると応答が復帰します。エージェントのパラメータと、ゴールのパラメータの2箇所に必要でした。

// src/WorldGenerator.cpp readAgentParams()
auto aparams = parameters_client->get_parameters({agent_params});
for (int aretry = 0; aparams.size() != agent_params.size() && aretry < 30; ++aretry)
{
  RCLCPP_WARN(this->get_logger(),
              "get_parameters for %s returned %li/%li values, retrying (%d)...",
              an.c_str(), aparams.size(), agent_params.size(), aretry);
  rclcpp::sleep_for(std::chrono::milliseconds(100));
  aparams = parameters_client->get_parameters({agent_params});
}

14. 起動手順まとめ

ここまでの対応を全部入れた上での起動手順です。

# 0) 古いプロセスを全部掃除(前回記事のハマったところ④。統合ではノードが多いので特に重要)
pkill -9 -f 'gzserver|gzclient|hunav|rmf_|fleet|rviz2|world_generator|adapter|building_map|visualizer|traffic|supervisor|dispenser|ingestor|ros2 launch'
sleep 3

# 1) 環境を作る(両ワークスペース + Gazebo のパス)
source /opt/ros/humble/setup.bash
source ~/rmf_ws/install/setup.bash
source ~/hunav_ws/install/setup.bash
source /usr/share/gazebo/setup.sh

# 2) 歩行者定義や BT を変えたらワールドを再生成
bash ~/rmf_hunav/gen_merged_world.sh tinyRobot_2    # => actors: 4

# 3) 統合シミュレーションを起動(HuNav 系 -> Gazebo の順に自動で遅延起動)
ros2 launch ~/rmf_hunav/evtest_kachaka_hunav.launch.py

ログに次が出れば、HuNavPlugin がロボットを認識して歩行者制御を始めた合図です。

[hunav_plugin]: Robot tinyRobot_2 detected!!! initializing params...
[hunav_agent_manager]: Received 4 agents

そのうえでタスクを投げます。

ros2 run rmf_demos_tasks dispatch_json -c compose \
  -f ~/rmf_ws/src/demonstrations/rmf_demos/rmf_demos/scripts/kachaka_elevator_swap.json \
  -F tinyRobot -R tinyRobot_2 --use_sim_time

15. トラブルシューティングまとめ

症状 原因 対処
起動直後に /clock が来ず全部固まる HuNavPlugin が /get_agents 待ちでワールドロードをブロック HuNav 系を先に起動し Gazebo を遅延(ハマったところ②)
/get_agents がいつまでも現れない ワールドジェネレータを終了させている ジェネレータを常駐させる(ハマったところ③)
hunav_agent_manager が SIGABRT エージェント別 BT ファイルが無い agents_<base>__agent_<id>_bt.xml を用意(ハマったところ④)
gzclient が起動直後に落ちる GAZEBO_RESOURCE_PATH にシェーダのパスが無い source /usr/share/gazebo/setup.sh(ハマったところ⑤)
歩行者がまともに歩かない・押し出される use_gazebo_obs:=true + RMF の巨大な単一建物モデル use_gazebo_obs:=false + ゴールをナビグラフのレーン上に置く(ハマったところ①)
タスク実行の瞬間に凍る HuNavPlugin の pause ラッパーと RMF の棚搬送が競合 SetPaused ラッパーを削除(ハマったところ⑥)
上階の人がほとんど動かない 実 z を送るとゴール引力が下向きになる マネージャへ送る z を 0 に固定、描画時に床 z を足し戻す
上階の人がロボット不在で立ち止まる 単一平面 SFM が階をまたいで x,y で見る BT の検知距離を欲張らない(ハマったところ⑧)
swap で同じ棚を持ち帰る 直前に置いた棚が検知半径内にある attach_max_radius を 1.1 に絞る(ハマったところ⑨)
人数を増やすとワールドジェネレータが segfault get_parameters が間欠的に空応答 リトライガードを入れる(ハマったところ⑩)
歩行者が誰も反応しない タスクが tinyRobot_1 に割り当てられた dispatch に -R tinyRobot_2 を付ける

16. 現状の限界

この統合には以下の限界があります。

  • ロボットは人を避けません。 HuNavSim の人はロボットを避けますが、RMF のロボットはナビグラフ上を走るだけで、HuNavSim の歩行者を障害物として認識しません。特に Surprised の人はその場で固まるので、走行レーン上に立たれるとすり抜けられます。
  • HuNavSim が反応するロボットは1体だけです(robot_name で指定した1体)。複数ロボットへの反応は HuNavSim 側の課題です。
  • HuNavSimのモデルが 2D仕様なので、階をまたいでロボットの x, y に反応します(ハマったところ⑧)。「計算は z=0 の平面、描画だけ階ごと」という割り切りの副作用です。
  • Gazebo 障害物判定が使えません(ハマったところ①)。歩行者の経路はナビグラフのレーンに手動で乗せる必要があり、自由な配置はできません。

改善するなら、RMF 側のフリートアダプタに /people トピックを購読させて歩行者回避を入れる、あるいは HuNavPlugin にレベル(階)の概念を持たせて検知を階内に閉じる、といった方向になると思います。

17. まとめ

エレベーターを使って棚を入れ替えるという、RMF らしいタスクを実行し、その様子に対して性格の異なる歩行者が反応する——というシミュレーションが一通り動くようになりました。
次のステップとして、ロボット側にも歩行者回避を実装し、「人を避けながらタスクをこなす」ところまで持っていきたいと考えています。あわせて歩行者側も、複数のロボットに反応できるようにしつつ、階の異なるロボットには反応しない、という挙動を実現したいと考えています。

18. 参考リンク / ライセンス

参考リンク

ライセンス / クレジット

本記事は下記のオープンソースを利用しています。ライセンスは各リポジトリの LICENSE ファイルに従います。

ソースを改変して利用したもの

  • HuNavSim（hunav_sim / hunav_gazebo_wrapper）— MIT License, robotics-upo
  • rmf_demos — Apache-2.0 License, Open-RMF

そのまま利用したもの

  • IFRA_LinkAttacher — Apache-2.0 License, IFRA Group, Cranfield University
  • lightsfm — BSD 3-Clause License, Service Robotics Lab（HuNavSim の依存）
  • people_msgs — BSD License, wg-perception（HuNavSim の依存）

引用

  • N. Pérez-Higueras, R. Otero, F. Caballero and L. Merino, "HuNavSim: A ROS 2 Human Navigation Simulator for Benchmarking Human-Aware Robot Navigation," IEEE Robotics and Automation Letters, vol. 8, no. 11, pp. 7130-7137, Nov. 2023, doi: 10.1109/LRA.2023.3316072.
  • IFRA-Cranfield (2023) Gazebo-ROS2 Link Attacher. URL: https://github.com/IFRA-Cranfield/IFRA_LinkAttacher
2
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
2
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?