From 4e2b0112682301486b1a37650982ac8b4177c7f2 Mon Sep 17 00:00:00 2001 From: krrishj18 Date: Fri, 15 May 2026 14:52:27 -0400 Subject: [PATCH 1/3] fixes to scene_prep_utils.py --- .../example_multi_drone_scene_import.py | 78 ++++++--- simulation/isaac-sim/utils/scene_prep.py | 162 ++++++++++++++---- 2 files changed, 182 insertions(+), 58 deletions(-) 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..e70272a8d 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, ) @@ -48,10 +48,17 @@ NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu" #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" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/scenes/urban/allegheny_county_fire_academy/fire_academy.scene.usd" +ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/AbandonedFactory/AbandonedFactory.stage.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/ConstructionSite/ConstructionSite.stage.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/MilitaryBase_t_x1100_y200_z0_o_x0_y0_z90.scene.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/copy-rayfronts-planner/AbandonedCity.scene.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/downtown_edited_v3_818.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/copy-rayfronts-planner/environments_start_pos/SnowyVillage_t_x-152_y-80_z-2_o_x0_y0_z_90.scene.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/edit_v1_shipyard.usd" +#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/ModernCityDowntown.stage.usd" + STAGE_SCALE = 0.01 DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" @@ -60,7 +67,7 @@ ADD_DOME_LIGHT = False DOME_LIGHT_PATH = "/World/DomeLight" DOME_LIGHT_INTENSITY = 3500.0 -DOME_LIGHT_EXPOSURE = -3.0 +DOME_LIGHT_EXPOSURE = -5.0 # GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates. # Matches the Lisbon default in px4_config.yaml — change here to relocate the sim world. @@ -74,21 +81,34 @@ # 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 + +SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.3#0.03 +# DRONE_CONFIGS = [ +# {"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} +# ] + + 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": 7.0, "y_m": 0.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0], "lidar_min_range": 4.0}, + {"domain_id": 2, "x_m": 0.0, "y_m": 0.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0], "lidar_min_range": 4.0}, + {"domain_id": 3, "x_m": -7.0, "y_m": 0.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0], "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 +185,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 +218,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 +261,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, ) From 6f3c8f6bb4c5b96c39c2caedc3d14d7e8456ee41 Mon Sep 17 00:00:00 2001 From: krrishj18 Date: Mon, 18 May 2026 13:34:34 -0400 Subject: [PATCH 2/3] edited docs --- docs/simulation/isaac_sim/overhead_camera.md | 41 +++++---- docs/simulation/isaac_sim/spawning_drones.md | 92 ++++++++++++++++++++ 2 files changed, 118 insertions(+), 15 deletions(-) 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 | From ed095fc0e46241fc54b76e9b21dbf86020e61a41 Mon Sep 17 00:00:00 2001 From: krrishj18 Date: Mon, 18 May 2026 13:36:29 -0400 Subject: [PATCH 3/3] clean launch script --- .../example_multi_drone_scene_import.py | 28 ++++--------------- 1 file changed, 6 insertions(+), 22 deletions(-) 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 e70272a8d..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 @@ -48,16 +48,7 @@ NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu" #env/stage path and scale -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/scenes/urban/allegheny_county_fire_academy/fire_academy.scene.usd" -ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/AbandonedFactory/AbandonedFactory.stage.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/ConstructionSite/ConstructionSite.stage.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/MilitaryBase_t_x1100_y200_z0_o_x0_y0_z90.scene.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/copy-rayfronts-planner/AbandonedCity.scene.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/downtown_edited_v3_818.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/copy-rayfronts-planner/environments_start_pos/SnowyVillage_t_x-152_y-80_z-2_o_x0_y0_z_90.scene.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/edit_v1_shipyard.usd" -#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/Dmytro/ModernCityDowntown.stage.usd" +ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Projects/AirStack/scenes/urban/allegheny_county_fire_academy/fire_academy.scene.usd" STAGE_SCALE = 0.01 @@ -67,7 +58,7 @@ ADD_DOME_LIGHT = False DOME_LIGHT_PATH = "/World/DomeLight" DOME_LIGHT_INTENSITY = 3500.0 -DOME_LIGHT_EXPOSURE = -5.0 +DOME_LIGHT_EXPOSURE = -3.0 # GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates. # Matches the Lisbon default in px4_config.yaml — change here to relocate the sim world. @@ -82,18 +73,11 @@ # {"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.3#0.03 -# DRONE_CONFIGS = [ -# {"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} -# ] - - +SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.03 DRONE_CONFIGS = [ - {"domain_id": 1, "x_m": 7.0, "y_m": 0.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0], "lidar_min_range": 4.0}, - {"domain_id": 2, "x_m": 0.0, "y_m": 0.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0], "lidar_min_range": 4.0}, - {"domain_id": 3, "x_m": -7.0, "y_m": 0.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0], "lidar_min_range": 4.0}, + {"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