diff --git a/docs/simulation/isaac_sim/overhead_camera.md b/docs/simulation/isaac_sim/overhead_camera.md
index 86fb91e1b..a50a98301 100644
--- a/docs/simulation/isaac_sim/overhead_camera.md
+++ b/docs/simulation/isaac_sim/overhead_camera.md
@@ -70,25 +70,38 @@ The three constants (`OVERHEAD_ALTITUDE_M`, `OVERHEAD_COVERAGE_M`, `OVERHEAD_PX_
| `OVERHEAD_COVERAGE_M` | `200.0` | Side length of the captured square (m). |
| `OVERHEAD_PX_PER_METER` | `4.0` | Texture density. Increase for sharper text/markings; capped at `max_resolution=2048`. |
-The camera is positioned at world origin `(0, 0)`. If your scene's points of interest are off-origin, shift the camera's `prim_path` xform after `add_orthographic_camera` returns:
+### Re-centering or transforming the camera
+
+By default the camera sits over world origin `(0, 0)`. For an off-origin area of interest, pass `center_x_m` / `center_y_m` to both helpers — they take care of the camera xform and the spec topics the GCS reads:
```python
-from pxr import Gf, UsdGeom
+CENTER_X_M, CENTER_Y_M = 50.0, -25.0
-cam_path = add_orthographic_camera(stage, prim_path="/World/MapCamera", ...)
+cam_path = add_orthographic_camera(
+ stage, prim_path="/World/MapCamera",
+ altitude_m=OVERHEAD_ALTITUDE_M,
+ coverage_m=OVERHEAD_COVERAGE_M,
+ scene_scale_factor=scene_scale_factor,
+ center_x_m=CENTER_X_M,
+ center_y_m=CENTER_Y_M,
+)
-# Re-center the camera over (CENTER_X_M, CENTER_Y_M) instead of world origin.
-CENTER_X_M, CENTER_Y_M = 50.0, -25.0
-xform = UsdGeom.Xformable(stage.GetPrimAtPath(cam_path))
-xform.ClearXformOpOrder()
-xform.AddTranslateOp().Set(Gf.Vec3d(
- CENTER_X_M * scene_scale_factor,
- CENTER_Y_M * scene_scale_factor,
- OVERHEAD_ALTITUDE_M * scene_scale_factor,
-))
+add_overhead_camera_publisher(
+ parent_graph_path="/World/MapCameraGraph",
+ camera_prim_path=cam_path,
+ topic="/sim/overhead/image",
+ spec_topic="/sim/overhead/spec",
+ center_x_topic="/sim/overhead/center_x",
+ center_y_topic="/sim/overhead/center_y",
+ frame_id="map",
+ coverage_m=OVERHEAD_COVERAGE_M,
+ center_x_m=CENTER_X_M,
+ center_y_m=CENTER_Y_M,
+ pixels_per_meter=OVERHEAD_PX_PER_METER,
+ domain_id=0,
+)
```
-
## GCS side
The GCS rendering is handled by `_build_sim_ground_marker` in `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py`. It:
@@ -113,8 +126,6 @@ The default downsample (0.8 cells/m, cap 384) is conservative. To raise the rend
```
-`1024` over a 200 m scene gives ~5 cells/m. If the 3D panel slows down, drop back toward `768`. Any change here also requires bumping `OVERHEAD_PX_PER_METER` on the sim side (otherwise you're sampling a low-resolution source more densely).
-
To change other rendering behavior (alpha, lighting), edit `_build_sim_ground_marker` directly. To force a re-render, restart the GCS visualizer.
## See also
diff --git a/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md
index a05072d5e..744436bc8 100644
--- a/docs/simulation/isaac_sim/spawning_drones.md
+++ b/docs/simulation/isaac_sim/spawning_drones.md
@@ -55,6 +55,98 @@ set_gps_origins(DRONE_CONFIGS, world_origin=(40.4433, -79.9436, 280.0)) # Pitts
The anchor only affects the geographic location reported via GPS; nothing in the scene moves. Pick something close to where you want the drones to "be" — Foxglove's Map panel will center on it, and any GPS-referenced inputs to your stack will be relative to it.
+## Scene prep helpers — `scene_prep.py`
+
+`simulation/isaac-sim/utils/scene_prep.py` is the small toolbox of stage preparation helpers `example_multi_drone_scene_import.py` uses inside its post-load callback (after the stage is loaded, before drones spawn). The full file has more — what's documented here is what you'll reach for in 95% of scenes.
+
+```python
+from utils.scene_prep import (
+ get_stage_meters_per_unit, scale_stage_prim, add_colliders,
+ add_dome_light, save_scene_as_contained_usd,
+ add_orthographic_camera, add_overhead_camera_publisher,
+)
+
+mpu, scene_scale_factor = get_stage_meters_per_unit(stage)
+```
+
+### Scaling — `scale_stage_prim`
+
+USD scenes are authored at all sorts of stage units. To apply a uniform scale to the imported stage root once, before drones spawn:
+
+```python
+STAGE_SCALE = 0.01 # cm → m
+scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+```
+
+### Colliders — `add_colliders`
+
+Recursively applies `UsdPhysics.CollisionAPI` to every `UsdGeom.Mesh` under the given prim. Imported environment USDs are usually visual-only; without this, drones fall through buildings.
+
+```python
+stage_prim = stage.GetPrimAtPath("/World/stage")
+add_colliders(stage_prim)
+```
+
+Skips prims that already have the API applied. Run it on the stage root after `scale_stage_prim` returns.
+
+### Lighting — `add_dome_light`
+
+Incase the scene is missing any lights, this adds a dome light that can act like an overhead 'sun'.
+
+```python
+add_dome_light(
+ stage,
+ prim_path="/World/DomeLight",
+ intensity=3500.0,
+ exposure=-5.0, # negative = darker; tune per scene
+)
+```
+
+### Overhead camera — `add_orthographic_camera` + `add_overhead_camera_publisher`
+
+Used as a pair: one drops an orthographic camera over the scene, the other wires an OmniGraph to publish its frame plus three Float32 spec topics (`coverage_m`, `center_x_m`, `center_y_m`) the GCS uses to texture a ground plane in Foxglove's 3D panel.
+
+```python
+cam_path = add_orthographic_camera(
+ stage,
+ prim_path="/World/MapCamera",
+ altitude_m=165.0,
+ coverage_m=225.0,
+ scene_scale_factor=scene_scale_factor,
+ center_x_m=0.0, # set if your area of interest isn't at world origin
+ center_y_m=0.0,
+)
+add_overhead_camera_publisher(
+ parent_graph_path="/World/MapCameraGraph",
+ camera_prim_path=cam_path,
+ topic="/sim/overhead/image",
+ spec_topic="/sim/overhead/spec",
+ center_x_topic="/sim/overhead/center_x",
+ center_y_topic="/sim/overhead/center_y",
+ frame_id="map",
+ coverage_m=225.0,
+ center_x_m=0.0,
+ center_y_m=0.0,
+ pixels_per_meter=10.0,
+ domain_id=0,
+)
+```
+
+Full setup, GCS-side rendering, and tuning knobs are on the **[Overhead Camera](overhead_camera.md)** page.
+
+### Saving a self-contained copy — `save_scene_as_contained_usd`
+
+For scenes you'd like to keep working with offline (no Nucleus connection), or for sharing a scene with collaborators, collect the root USD plus every referenced asset (textures, MDLs, sublayers) into a local directory:
+
+```python
+save_scene_as_contained_usd(
+ source_usd_url=ENV_URL,
+ output_dir="/tmp/collected_scene",
+)
+```
+
+The collected folder contains a standalone root USD with relative references — load it directly via `omniverse://localhost/...` or a local file path. The collected scene will include modifications for scale, colliders, etc applied before saving.
+
## Common issues
| Symptom | Likely cause | Fix |
diff --git a/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
index 144b04107..2dd231662 100644
--- a/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
@@ -27,7 +27,7 @@
from pegasus.simulator.logic.interface.pegasus_interface import PegasusInterface
from pegasus.simulator.ogn.api.spawn_multirotor import spawn_px4_multirotor_node
from pegasus.simulator.ogn.api.spawn_zed_camera import add_zed_stereo_camera_subgraph
-from pegasus.simulator.ogn.api.spawn_ouster_lidar import add_ouster_lidar_subgraph
+from pegasus.simulator.ogn.api.spawn_rtx_lidar import add_rtx_lidar_subgraph
# gps_utils lives in the same directory as this script
_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
@@ -39,7 +39,7 @@
import scene_prep
from scene_prep import (
scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit,
- reference_root_prims_under_world,
+ reference_root_prims_under_world, dedupe_physics_scenes,
add_orthographic_camera, add_overhead_camera_publisher,
)
@@ -49,9 +49,7 @@
#env/stage path and scale
ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/scenes/urban/allegheny_county_fire_academy/fire_academy.scene.usd"
-#f"omniverse://{NUCLEUS_SERVER}/Library/Assets/FireAcademyFaro/fire_academy_faro.usd"
-#f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/RayFronts-Planner/FireAcademy.scene.usd"
-#f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.usd"
+
STAGE_SCALE = 0.01
DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
@@ -74,21 +72,27 @@
# spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd:
# {"domain_id": 1, "x_m": 20.0, "y_m": -7.0, ...}
# {"domain_id": 2, "x_m": 17.0, "y_m": 1.5, ...}
+
SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.03
DRONE_CONFIGS = [
- {"domain_id": 1, "x_m": 27.0, "y_m": 7.6, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
- {"domain_id": 2, "x_m": 23.0, "y_m": 9.8, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
- {"domain_id": 3, "x_m": 27.0, "y_m": 12.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
- {"domain_id": 4, "x_m": 23.0, "y_m": 14.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0}
-]
-
-# Top-down "map" camera over (0, 0). Captures one aerial of the static scene
-# that the GCS visualizer turns into a textured ground in Foxglove's 3D panel.
-OVERHEAD_ALTITUDE_M = 150.0
-OVERHEAD_COVERAGE_M = 200.0 # per-map knob: world meters per side.
-OVERHEAD_PX_PER_METER = 12.0 # Source-image density. Bump for sharper texture.
+ {"domain_id": 1, "x_m": 32.0, "y_m": 12.6, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
+ {"domain_id": 2, "x_m": 28.0, "y_m": 14.8, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0},
+ {"domain_id": 3, "x_m": 32.0, "y_m": 19.8, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, -0.937, 0.35], "lidar_min_range": 4.0}
+ ]
+
+# Top-down "map" camera. Captures one aerial of the static scene that the
+# GCS visualizer turns into a textured ground in Foxglove's 3D panel. The
+# camera centers on (OVERHEAD_CENTER_X_M, OVERHEAD_CENTER_Y_M) in world
+# meters — leave both 0.0 for the legacy origin-centered behavior.
+OVERHEAD_ALTITUDE_M = 165.0
+OVERHEAD_COVERAGE_M = 225 # per-map knob: world meters per side.
+OVERHEAD_CENTER_X_M = 0.0 #-152 # world-X of camera center / texture center.
+OVERHEAD_CENTER_Y_M = 0.0 #-80 # world-Y of camera center / texture center.
+OVERHEAD_PX_PER_METER = 10.0 # Source-image density. Bump for sharper texture.
OVERHEAD_TOPIC = "/sim/overhead/image"
OVERHEAD_SPEC_TOPIC = "/sim/overhead/spec"
+OVERHEAD_CENTER_X_TOPIC = "/sim/overhead/center_x"
+OVERHEAD_CENTER_Y_TOPIC = "/sim/overhead/center_y"
OVERHEAD_FRAME_ID = "map"
OVERHEAD_DOMAIN_ID = 0
# ---------------------------------------------------------
@@ -165,8 +169,13 @@ def __init__(self):
if not wait_for_stage(stage):
carb.log_warn("Stage load timed out — continuing anyway.")
+ dedupe_physics_scenes(stage)
+
# ----- Scene preparation -----
- # Bring in sky/sun/environment prims that sit outside /World in the source USD
+ # Bring in sky/sun/environment prims that sit at root level in the
+ # source USD next to the defaultPrim that pg.load_environment already
+ # loaded into /World/stage. reference_root_prims_under_world skips
+ # the defaultPrim, so this can't duplicate geometry.
reference_root_prims_under_world(stage, ENV_URL)
stage_prim = stage.GetPrimAtPath("/World/stage")
@@ -193,14 +202,20 @@ def __init__(self):
altitude_m=OVERHEAD_ALTITUDE_M,
coverage_m=OVERHEAD_COVERAGE_M,
scene_scale_factor=s,
+ center_x_m=OVERHEAD_CENTER_X_M,
+ center_y_m=OVERHEAD_CENTER_Y_M,
)
add_overhead_camera_publisher(
parent_graph_path="/World/MapCameraGraph",
camera_prim_path=cam_path,
topic=OVERHEAD_TOPIC,
spec_topic=OVERHEAD_SPEC_TOPIC,
+ center_x_topic=OVERHEAD_CENTER_X_TOPIC,
+ center_y_topic=OVERHEAD_CENTER_Y_TOPIC,
frame_id=OVERHEAD_FRAME_ID,
coverage_m=OVERHEAD_COVERAGE_M,
+ center_x_m=OVERHEAD_CENTER_X_M,
+ center_y_m=OVERHEAD_CENTER_Y_M,
pixels_per_meter=OVERHEAD_PX_PER_METER,
domain_id=OVERHEAD_DOMAIN_ID,
)
@@ -230,14 +245,15 @@ def __init__(self):
camera_rotation_offset=[0.0, 0.0, 0.0],
)
- add_ouster_lidar_subgraph(
+ add_rtx_lidar_subgraph(
parent_graph_handle=graph_handle,
drone_prim=f"/World/drone{i}/base_link",
robot_name=f"robot_{i}",
- lidar_name="OS1_REV6_128_10hz___512_resolution",
+ lidar_config="ouster_os1",
+ lidar_topic_name="point_cloud_raw",
lidar_offset=[0.0, 0.0, 0.025],
lidar_rotation_offset=[0.0, 0.0, 0.0],
- lidar_min_range=cfg["lidar_min_range"],
+ min_range=cfg["lidar_min_range"],
)
self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py
index a6dc93cf3..dcd390960 100644
--- a/simulation/isaac-sim/utils/scene_prep.py
+++ b/simulation/isaac-sim/utils/scene_prep.py
@@ -85,6 +85,38 @@ def _precision(attr_name, default=UsdGeom.XformOp.PrecisionDouble):
return prim
+# ---------------------------------------------------------------------------
+# Physics scene dedupe
+# ---------------------------------------------------------------------------
+
+def dedupe_physics_scenes(stage) -> str | None:
+ """Keep the first UsdPhysics.Scene found in the stage; delete the rest.
+
+ Isaac's World autocreates a PhysicsScene on init, and Kit-saved USDs
+ often bake one in too. PhysX can only step a single scene coherently,
+ so duplicates trigger "Physics scenes stepping is not the same" and
+ desynced sensors. Returns the kept prim path (or None if no scene).
+ """
+ scenes = [p for p in stage.Traverse() if p.IsA(UsdPhysics.Scene)]
+ if not scenes:
+ print("[scene_prep] No PhysicsScene found in stage")
+ return None
+ keep, *extras = scenes
+ for extra in extras:
+ path = extra.GetPath()
+ # Prims that come from a referenced sublayer (e.g. PhysicsScene baked
+ # into the loaded USD) can't be deleted with RemovePrim from the live
+ # root layer — there's no spec there to remove. SetActive(False)
+ # writes an "active = false" override on the root layer, which makes
+ # USD ignore the prim entirely without touching the source asset.
+ if not extra.SetActive(False):
+ print(f"[scene_prep] WARN: failed to deactivate PhysicsScene at {path}")
+ else:
+ print(f"[scene_prep] Deactivated duplicate PhysicsScene: {path}")
+ print(f"[scene_prep] Kept PhysicsScene: {keep.GetPath()}")
+ return str(keep.GetPath())
+
+
# ---------------------------------------------------------------------------
# Collision
# ---------------------------------------------------------------------------
@@ -135,8 +167,11 @@ def add_orthographic_camera(stage,
prim_path: str = "/World/MapCamera",
altitude_m: float = 80.0,
coverage_m: float = 80.0,
- scene_scale_factor: float = 1.0):
- """Create a static orthographic camera straight over (0, 0) looking down.
+ scene_scale_factor: float = 1.0,
+ center_x_m: float = 0.0,
+ center_y_m: float = 0.0):
+ """Create a static orthographic camera straight over a configurable XY
+ point looking down.
Args:
stage: Active USD stage.
@@ -146,18 +181,21 @@ def add_orthographic_camera(stage,
scene_scale_factor: 1 / meters_per_unit. Pass the value returned by
``get_stage_meters_per_unit`` so metric inputs
land in the right stage-space units.
+ center_x_m: World-X of the camera center (metric). Default 0.
+ center_y_m: World-Y of the camera center (metric). Default 0.
Returns:
The string prim path (handy for callers that pass it to the OG helper).
"""
cam = UsdGeom.Camera.Define(stage, Sdf.Path(prim_path))
- # Place the camera over (0, 0). USD cameras look along -Z by default,
- # which is already straight down — no rotation needed.
+ # USD cameras look along -Z by default — already straight down, no rotation.
xform = UsdGeom.Xformable(cam.GetPrim())
xform.ClearXformOpOrder()
xform.AddTranslateOp().Set(
- Gf.Vec3d(0.0, 0.0, float(altitude_m) * float(scene_scale_factor)))
+ Gf.Vec3d(float(center_x_m) * float(scene_scale_factor),
+ float(center_y_m) * float(scene_scale_factor),
+ float(altitude_m) * float(scene_scale_factor)))
# Orthographic projection. USD aperture is in tenths of a stage unit, so
# for a 1 m stage `coverage_m * 10` puts a coverage_m × coverage_m square
@@ -170,7 +208,8 @@ def add_orthographic_camera(stage,
Gf.Vec2f(0.1, max(2.0, float(altitude_m) * 2.0) * float(scene_scale_factor)))
print(f"[scene_prep] Orthographic map camera at '{prim_path}' "
- f"(alt={altitude_m} m, coverage={coverage_m} m)")
+ f"(alt={altitude_m} m, coverage={coverage_m} m, "
+ f"center=({center_x_m}, {center_y_m}) m)")
return prim_path
@@ -178,14 +217,19 @@ def add_overhead_camera_publisher(parent_graph_path: str,
camera_prim_path: str,
topic: str = "/sim/overhead/image",
spec_topic: str = "/sim/overhead/spec",
+ center_x_topic: str = "/sim/overhead/center_x",
+ center_y_topic: str = "/sim/overhead/center_y",
frame_id: str = "map",
coverage_m: float = 80.0,
+ center_x_m: float = 0.0,
+ center_y_m: float = 0.0,
pixels_per_meter: float = 4.0,
max_resolution: int = 2048,
domain_id: int = 0):
"""Wire an orthographic camera to a raw ``sensor_msgs/Image`` topic, plus
- a spec ``std_msgs/Float32`` topic carrying ``coverage_m`` so consumers
- can size the ground texture without manual configuration.
+ three spec ``std_msgs/Float32`` topics carrying ``coverage_m``,
+ ``center_x_m``, ``center_y_m`` so consumers can size **and place** the
+ ground texture without manual configuration.
The image resolution is auto-derived from ``coverage_m × pixels_per_meter``
and capped at ``max_resolution`` so a typo can't blow up bandwidth. Sim
@@ -215,11 +259,18 @@ def add_overhead_camera_publisher(parent_graph_path: str,
"rgb": f"{g}/MapCameraRGBHelper",
"frame": f"{g}/MapCameraFrameId",
"topic": f"{g}/MapCameraTopic",
- # Spec branch: publishes coverage_m once per tick on a separate topic
- # so the GCS visualizer auto-discovers FOV.
+ # Spec branches: publish coverage_m / center_x_m / center_y_m once per
+ # tick on separate Float32 topics so the GCS visualizer auto-discovers
+ # both FOV and world placement.
"spec_value": f"{g}/MapCameraSpecValue",
"spec_topic": f"{g}/MapCameraSpecTopic",
"spec_pub": f"{g}/MapCameraSpecPublisher",
+ "cx_value": f"{g}/MapCameraCenterXValue",
+ "cx_topic": f"{g}/MapCameraCenterXTopic",
+ "cx_pub": f"{g}/MapCameraCenterXPublisher",
+ "cy_value": f"{g}/MapCameraCenterYValue",
+ "cy_topic": f"{g}/MapCameraCenterYTopic",
+ "cy_pub": f"{g}/MapCameraCenterYPublisher",
}
controller.edit(
@@ -235,6 +286,12 @@ def add_overhead_camera_publisher(parent_graph_path: str,
(nodes["spec_value"], "omni.graph.nodes.ConstantFloat"),
(nodes["spec_topic"], "omni.graph.nodes.ConstantString"),
(nodes["spec_pub"], "isaacsim.ros2.bridge.ROS2Publisher"),
+ (nodes["cx_value"], "omni.graph.nodes.ConstantFloat"),
+ (nodes["cx_topic"], "omni.graph.nodes.ConstantString"),
+ (nodes["cx_pub"], "isaacsim.ros2.bridge.ROS2Publisher"),
+ (nodes["cy_value"], "omni.graph.nodes.ConstantFloat"),
+ (nodes["cy_topic"], "omni.graph.nodes.ConstantString"),
+ (nodes["cy_pub"], "isaacsim.ros2.bridge.ROS2Publisher"),
],
og.Controller.Keys.CONNECT: [
# Image branch
@@ -245,42 +302,66 @@ def add_overhead_camera_publisher(parent_graph_path: str,
(f"{nodes['context']}.outputs:context", f"{nodes['rgb']}.inputs:context"),
(f"{nodes['frame']}.inputs:value", f"{nodes['rgb']}.inputs:frameId"),
(f"{nodes['topic']}.inputs:value", f"{nodes['rgb']}.inputs:topicName"),
- # Spec branch
+ # Spec (coverage) branch
(f"{nodes['playback']}.outputs:tick", f"{nodes['spec_pub']}.inputs:execIn"),
(f"{nodes['context']}.outputs:context", f"{nodes['spec_pub']}.inputs:context"),
(f"{nodes['spec_topic']}.inputs:value", f"{nodes['spec_pub']}.inputs:topicName"),
+ # Center-X branch
+ (f"{nodes['playback']}.outputs:tick", f"{nodes['cx_pub']}.inputs:execIn"),
+ (f"{nodes['context']}.outputs:context", f"{nodes['cx_pub']}.inputs:context"),
+ (f"{nodes['cx_topic']}.inputs:value", f"{nodes['cx_pub']}.inputs:topicName"),
+ # Center-Y branch
+ (f"{nodes['playback']}.outputs:tick", f"{nodes['cy_pub']}.inputs:execIn"),
+ (f"{nodes['context']}.outputs:context", f"{nodes['cy_pub']}.inputs:context"),
+ (f"{nodes['cy_topic']}.inputs:value", f"{nodes['cy_pub']}.inputs:topicName"),
],
og.Controller.Keys.SET_VALUES: [
- (f"{nodes['context']}.inputs:domain_id", int(domain_id)),
- (f"{nodes['create_rp']}.inputs:cameraPrim", camera_prim_path),
- (f"{nodes['create_rp']}.inputs:width", res),
- (f"{nodes['create_rp']}.inputs:height", res),
- (f"{nodes['rgb']}.inputs:type", "rgb"),
- (f"{nodes['frame']}.inputs:value", str(frame_id)),
- (f"{nodes['topic']}.inputs:value", str(topic)),
- (f"{nodes['spec_value']}.inputs:value", float(coverage_m)),
- (f"{nodes['spec_topic']}.inputs:value", str(spec_topic)),
- (f"{nodes['spec_pub']}.inputs:messageName", "Float32"),
- (f"{nodes['spec_pub']}.inputs:messagePackage", "std_msgs"),
- (f"{nodes['spec_pub']}.inputs:messageSubfolder", "msg"),
+ (("inputs:domain_id", nodes["context"]), int(domain_id)),
+ (("inputs:cameraPrim", nodes["create_rp"]), camera_prim_path),
+ (("inputs:width", nodes["create_rp"]), res),
+ (("inputs:height", nodes["create_rp"]), res),
+ (("inputs:type", nodes["rgb"]), "rgb"),
+ (("inputs:value", nodes["frame"]), str(frame_id)),
+ (("inputs:value", nodes["topic"]), str(topic)),
+ (("inputs:value", nodes["spec_value"]), float(coverage_m)),
+ (("inputs:value", nodes["spec_topic"]), str(spec_topic)),
+ (("inputs:messageName", nodes["spec_pub"]), "Float32"),
+ (("inputs:messagePackage", nodes["spec_pub"]), "std_msgs"),
+ (("inputs:messageSubfolder", nodes["spec_pub"]), "msg"),
+ (("inputs:value", nodes["cx_value"]), float(center_x_m)),
+ (("inputs:value", nodes["cx_topic"]), str(center_x_topic)),
+ (("inputs:messageName", nodes["cx_pub"]), "Float32"),
+ (("inputs:messagePackage", nodes["cx_pub"]), "std_msgs"),
+ (("inputs:messageSubfolder", nodes["cx_pub"]), "msg"),
+ (("inputs:value", nodes["cy_value"]), float(center_y_m)),
+ (("inputs:value", nodes["cy_topic"]), str(center_y_topic)),
+ (("inputs:messageName", nodes["cy_pub"]), "Float32"),
+ (("inputs:messagePackage", nodes["cy_pub"]), "std_msgs"),
+ (("inputs:messageSubfolder", nodes["cy_pub"]), "msg"),
],
},
)
# The ROS2Publisher's value input is dynamically typed — created on the
- # node after the message type is set. Connect ConstantFloat → publisher.
+ # node after the message type is set. Connect ConstantFloat → publisher
+ # for each of the three spec topics.
controller.edit(
graph_id=g,
edit_commands={
og.Controller.Keys.CONNECT: [
(f"{nodes['spec_value']}.inputs:value",
f"{nodes['spec_pub']}.inputs:data"),
+ (f"{nodes['cx_value']}.inputs:value",
+ f"{nodes['cx_pub']}.inputs:data"),
+ (f"{nodes['cy_value']}.inputs:value",
+ f"{nodes['cy_pub']}.inputs:data"),
],
},
)
print(f"[scene_prep] Overhead camera publisher wired: "
f"{topic} ({res}x{res} raw Image), {spec_topic} ({coverage_m} m), "
+ f"{center_x_topic}={center_x_m} m, {center_y_topic}={center_y_m} m, "
f"domain_id={domain_id}")
@@ -289,12 +370,16 @@ def add_overhead_camera_publisher(parent_graph_path: str,
# ---------------------------------------------------------------------------
def reference_root_prims_under_world(stage, source_usd_url: str) -> list:
- """Reference non-/World root prims from *source_usd_url* under /World.
+ """Reference sibling root prims from *source_usd_url* under /World/.
- When a USD is loaded via pg.load_environment (reference scoped to /World),
- root-level prims like /Sky, /Sun, /Environment are excluded. This function
- opens the source layer, finds those prims, and adds them as individual
- references under /World — without touching the geometry already loaded.
+ pg.load_environment references the source USD's defaultPrim into
+ /World/stage — anything outside that defaultPrim (sky, sun, environment
+ sitting at root level) gets dropped. This function pulls those siblings
+ in as individual references.
+
+ Skips the defaultPrim itself, since re-referencing it would create a
+ second independent copy of the same geometry next to /World/stage.
+ Also skips a literal /World prim if one exists in the source.
Args:
stage: Active USD stage.
@@ -308,18 +393,25 @@ def reference_root_prims_under_world(stage, source_usd_url: str) -> list:
print(f"[scene_prep] reference_root_prims_under_world: could not open {source_usd_url}", flush=True)
return []
- non_world = [spec.name for spec in source_layer.rootPrims if spec.name != 'World']
- if not non_world:
- print("[scene_prep] reference_root_prims_under_world: no non-World root prims found", flush=True)
+ default_prim = source_layer.defaultPrim # name only, e.g. "Stage"
+ skip = {'World'}
+ if default_prim:
+ skip.add(default_prim)
+
+ siblings = [spec.name for spec in source_layer.rootPrims if spec.name not in skip]
+ if not siblings:
+ print(f"[scene_prep] reference_root_prims_under_world: no sibling root prims to pull in "
+ f"(defaultPrim={default_prim!r}, all roots={[s.name for s in source_layer.rootPrims]})",
+ flush=True)
return []
- for name in non_world:
+ for name in siblings:
dest_path = f"/World/{name}"
dest_prim = stage.DefinePrim(dest_path)
dest_prim.GetReferences().AddReference(source_usd_url, f"/{name}")
print(f"[scene_prep] Referenced /{name} at {dest_path}", flush=True)
- return non_world
+ return siblings
def move_root_prims_to_world_live(stage) -> list:
@@ -428,7 +520,7 @@ def save_scene_as_contained_usd(source_usd_url: str, output_dir: str) -> bool:
usd_path=source_usd_url,
collect_dir=output_dir,
usd_only=False, # include textures, MDLs, etc.
- flat_collection=True, # flatten asset references into one directory
+ flat_collection=True, # preserve source folder hierarchy
skip_existing=False,
)