Skip to content

ROS 2 と接続する

fibril_sim_ros は、fibril_sim_runtime が所有する simulation execution と ROS 2 graph の境界を作る adapter を提供します。 SimulationRuntime は SimulationHost と externally visible SimulationServer の lifetime を ROS executor から独立して所有します。 ROS 側は completed frame の配信、ingress enqueue、/clock、ROS graph 固有の service/topic 変換を担当します。

/clock

SessionClockPublisher は Instance の SessionTime を rosgraph_msgs/msg/Clock として publish します。 EpisodeTime ではなく SessionTime を使うため、episode reset 後も ROS time は後退しません。

1
2
3
4
fibril::sim::SessionClockPublisher clock_publisher(node, instance);

instance.step();
clock_publisher.publish();

simulation time を利用する ROS node では通常どおり use_sim_time を有効にします。

generic fibril_simulation node の live ROS execution は runtime.mode: realtime_paced を要求します。 node は fibril_sim_ament の standard Plan factory で SimulationPlan を構築し、その Plan から SimulationRuntime を生成します。 SimulationRuntime が SimulationHost と SimulationServer を所有し、ROS dispatcher が lifecycle boundary を開始した後は execution-owner thread が wall-clock pacing を行います。 ROS 側の dispatcher timer は completed SimulationHostFrame の drain、/clock publish、ROS bridge dispatch だけを行い、physics step を request しません。 したがって dispatcher period を長くして複数 frame をまとめて配送しても、physics grid の進行時刻は ROS timer cadence に依存しません。

DeviceOutput を ROS topic に出す

DeviceOutputPublisher<RosMessageT, DeviceSampleT> は、delivery 済み DeviceOutput を指定した ROS message へ変換して publish します。 converter は Fibril sample と timing metadata を受け取るため、message header の timestamp などを application 側で決められます。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
fibril::sim::DeviceOutputPublisher<MyRosMsg, MyDeviceSample> publisher(
  node,
  "/sensor",
  "front_sensor",
  "my/sample/v1",
  [](const MyDeviceSample & sample, const fibril::sim::DeviceOutput & output) {
    MyRosMsg message;
    // sample と output の timing metadata を ROS message へ変換する。
    return message;
  });

instance.step();
publisher.publish(instance.drain_device_outputs());

ROS publisher が active MuJoCo state を直接読む必要はありません。 publish 対象は Instance が生成した immutable output です。

register_standard_ros_bridges() は fibril_sim/imu、fibril_sim/ahrs、fibril_sim/planar_lidar、fibril_sim/body_odometry の標準bridgeを登録します。 fibril_sim/ahrs は AhrsSample のorientation、angular velocity、linear accelerationを sensor_msgs/msg/Imu へ写像します。

fibril_sim/planar_lidar bridge は既定ではDeviceのnominal acquisition timeを LaserScan.header.stamp に使用します。 driverがheader timestampを粗いclock格子へ丸めるdeploymentを再現する場合は、bridge configに header_stamp_quantization_s を指定できます。

1
2
3
4
5
6
7
- id: lidar
  type: fibril_sim/planar_lidar
  config:
    device: lidar
    topic: scan
    frame_id: laser
    header_stamp_quantization_s: 0.01

このquantizationはROS message headerだけに適用されます。 Deviceのacquisition period、rolling beam timing、transport delivery timeは変更しません。

ROS topic を ExternalInput に入れる

create_external_input_subscription は ROS callback の受信値を ExternalInputPayload に変換して Instance へ enqueue します。 callback は active MuJoCo state を読み書きせず、simulation-visible な可視化は ExternalInputCommit phase で行われます。

1
2
3
4
5
6
7
8
9
auto subscription = fibril::sim::create_external_input_subscription<MyCommandMsg>(
  node,
  instance,
  "/command",
  "operator_command",
  "my/command/v1",
  [](const MyCommandMsg & message) -> std::any {
    return MyCommandPayload{message.value};
  });

この ingress-only rule は thread safety のためだけではありません。 同じ external input ordering を trace と replay で再構成するための simulation contract でもあります。

hardware-compatible simulated driver は、ROS topic を simulation 内部 transport として使いません。 driver component は fibril_sim_runtime::SimulationConnection から typed ExternalInputContract<T> と DeviceOutputContract<T> を利用し、production と同じ ROS API を外側へ提供します。 この connection は iceoryx2-backed same-host IPC で、compatible driver と simulation host は同一 process / 別 processのどちらでも同じ contract を使います。 MCU や production driver の CAN/UART を再現する場合は、Fibril Simulation IPC の先に SocketCAN/PTY などの virtual hardware boundary を置きます。

rosbag2 へ記録する

Rosbag2Recorder は typed ROS message を rosbag2 へ書き込みます。 RunProvenance を渡すと、Fibril の model hash、Manifest hash、revision、MuJoCo version などを rosbag2 metadata の custom data に埋め込めます。

1
2
3
fibril::sim::Rosbag2Recorder recorder("run_bag", plan.provenance());
recorder.write("/sensor", message, instance.session_time());
recorder.close();

provenance を持たない通常の rosbag2 も有効な入力として扱われます。 read_rosbag2_run_provenance() は metadata が存在しない場合に値を捏造せず std::nullopt を返します。

rosbag2 を ExternalInput として replay する

enqueue_rosbag2_external_inputs は指定 topic の recorded receive timestamp を最初の sample からの相対時刻へ正規化し、Instance の ingress mailbox に enqueue します。 active MuJoCo state へ直接 message を適用する API ではありません。

1
2
3
4
5
6
7
8
9
fibril::sim::enqueue_rosbag2_external_inputs<MyCommandMsg>(
  "run_bag",
  "/command",
  instance,
  "operator_command",
  "my/command/v1",
  [](const MyCommandMsg & message) -> std::any {
    return MyCommandPayload{message.value};
  });

semantic trace の replay と rosbag2 replay は用途が異なります。 Fibril Simulation runtime が実際に commit した input ordering まで再現したい場合は MCAP semantic trace の replay を使います。

Live simulation の Fibril Simulation trace を保存する

generic fibril_simulation node は optional trace_path parameter を持ちます。 non-empty path を指定すると Plan の trace policy を有効にし、completed simulation boundary の Fibril Simulation trace を MCAP へ保存します。

1
2
3
4
5
ros2 run fibril_sim_ros fibril_simulation_node \
  --ros-args \
  -p manifest_path:=/path/to/simulation.yaml \
  -p ros_config_path:=/path/to/ros.yaml \
  -p trace_path:=/tmp/failing-run.mcap

実際の deployment が component / launch wrapper を使う場合は、その node parameter として同じ trace_path を渡します。

trace enabled の場合も mutable state ownership は変わりません。 SimulationRuntime 配下の SimulationHost worker thread が SimulationInstance を step した後、trace frame を immutable SimulationHostFrame へ move し、ROS frontend 側が MCAP に persist します。 ROS callback や viewer thread が active mjData を直接読む必要はありません。