diff --git a/robot/ros_ws/manual_exploration.sh b/robot/ros_ws/manual_exploration.sh index 8804c070a..28ad57e20 100755 --- a/robot/ros_ws/manual_exploration.sh +++ b/robot/ros_ws/manual_exploration.sh @@ -2,10 +2,10 @@ set -euo pipefail # Pair 2 (run in parallel) -ros2 service call /robot_1/behavior/global_plan_toggle \ +ros2 service call /${ROBOT_NAME}/behavior/global_plan_toggle \ std_srvs/srv/Trigger "{}" & p1=$! -ros2 service call /robot_1/trajectory_controller/set_trajectory_mode \ +ros2 service call /${ROBOT_NAME}/trajectory_controller/set_trajectory_mode \ airstack_msgs/srv/TrajectoryMode "{mode: 3}" & p2=$! wait $p1 $p2 diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml index f38ff4656..efc46f87f 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle" + srv_global_plan_toggle_topic: "~/global_plan_toggle" way_point_planners: - name: "random_walk" config: diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml index d642eb556..4b3bbb0b7 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/config/exploration_config.yaml @@ -14,7 +14,7 @@ sub_vdb_topic: "vdb_grid" sub_robot_tf_topic: "/tf" sub_target_path_topic: "/perspective_goals" - srv_exploration_toggle_topic: "/robot_1/behavior/global_plan_toggle" + srv_exploration_toggle_topic: "~/global_plan_toggle" publish_visualizations: false # should trajectory visualizations be published num_paths_to_generate: 5 # how many paths to string together diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml index 459ba14c0..90fd427ad 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/exploration_launch.xml @@ -6,6 +6,7 @@ + @@ -17,5 +18,6 @@ + --> \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml index 8d5b60582..ae5fed486 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/exploration/launch/robot_launch_gazebo/gz_global_launch.xml @@ -11,6 +11,7 @@ + --> diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml index 118d6751d..6b0f6071d 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml @@ -6,7 +6,7 @@ pub_trajectory_viz_topic: "~/traj_viz" sub_map_topic: "vdb_map_visualization" sub_robot_tf_topic: "/tf" - srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle" + srv_random_walk_toggle_topic: "~/global_plan_toggle" publish_visualizations: false # should trajectory visualizations be published num_paths_to_generate: 5 # how many random walk paths to string together diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml index 5a18b0888..5b28e16ca 100644 --- a/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml +++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml @@ -11,6 +11,7 @@ + diff --git a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz index 983346f1e..ef3f06bc2 100644 --- a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz +++ b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz @@ -1,6 +1,6 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 70 Name: Displays Property Tree Widget: Expanded: @@ -15,8 +15,9 @@ Panels: - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1 - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1 - /Global1 + - /Global1/Global Plan1 Splitter Ratio: 0.590062141418457 - Tree Height: 551 + Tree Height: 341 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -37,7 +38,7 @@ Panels: SyncSource: Lidar - Class: rviz_behavior_tree_panel::BehaviorTreePanel Name: BehaviorTreePanel - topic: /robot_1/behavior/behavior_tree_graphviz + topic: /robot_2/behavior/behavior_tree_graphviz zoom_factor: 0.1919851303100586 Visualization Manager: Class: "" @@ -119,44 +120,43 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - map: - base_link: - base_link_body_body_link: - OS1_REV6_128_10hz___512_resolution: - lidar: - {} - base_link_ZED_X: - camera_left: - macvo_ned: + world: + map: + base_link: + base_link_body_body_link: + OS1_REV6_128_10hz___512_resolution: + lidar: + {} + base_link_ZED_X: + camera_left: + macvo_ned: + {} + camera_right: + {} + imu: {} - camera_right: + rotor0: {} - imu: + rotor1: {} - rotor0: - {} - rotor1: - {} - rotor2: - {} - rotor3: + rotor2: + {} + rotor3: + {} + base_link_frd: {} - base_link_frd: + base_link_stabilized: + {} + look_ahead_point: + {} + look_ahead_point_stabilized: + {} + map_ned: + {} + tracking_point: + {} + tracking_point_stabilized: {} - base_link_stabilized: - {} - look_ahead_point: - {} - look_ahead_point_stabilized: - {} - map_ned: - {} - tracking_point: - {} - tracking_point_stabilized: - {} - world: - {} Update Interval: 0 Value: true - Alpha: 0.5 @@ -273,8 +273,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 0.017708778381347656 - Min Value: -0.07378101348876953 + Max Value: -0.03934618458151817 + Min Value: -0.0849701464176178 Value: true Axis: Z Channel Name: intensity @@ -492,8 +492,7 @@ Visualization Manager: Enabled: true Name: Trimmed Global Plan for DROAN Namespaces: - global_plan: true - global_plan_extra_text: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -640,12 +639,7 @@ Visualization Manager: Enabled: true Name: Traj Debug Namespaces: - collision_points: true - collision_trajectories: true - free_points: true - free_trajectories: true - unseen_points: true - unseen_trajectories: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -657,7 +651,7 @@ Visualization Manager: Enabled: true Name: Graph Vis Namespaces: - default: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -683,8 +677,7 @@ Visualization Manager: Enabled: true Name: Global Plan Vis Namespaces: - global_plan: true - global_plan_extra_text: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -696,7 +689,7 @@ Visualization Manager: Enabled: true Name: Rewind Info Namespaces: - rewind_info: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -713,7 +706,6 @@ Visualization Manager: Name: Traj Vis Namespaces: traj_controller: true - traj_controller_extra_text: true Topic: Depth: 5 Durability Policy: Volatile @@ -782,6 +774,20 @@ Visualization Manager: Value: true Enabled: true Name: Global + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/sensors/front_stereo/left/image_rect + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -828,7 +834,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 16.645448684692383 + Distance: 42.59531784057617 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -843,26 +849,28 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7353981733322144 + Pitch: 0.2153983861207962 Target Frame: base_link Value: Orbit (rviz) - Yaw: 0.13039594888687134 + Yaw: 4.4285736083984375 Saved: ~ Window Geometry: BehaviorTreePanel: collapsed: false Displays: - collapsed: true + collapsed: false Front Right Depth: collapsed: false Front Right RGB: collapsed: true Height: 1376 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true + Image: + collapsed: false MACVO Disparity: collapsed: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -870,7 +878,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 1245 - X: 3235 - Y: 27 + collapsed: false + Width: 1745 + X: 1263 + Y: 60 diff --git a/scenes/two_drone_RetroNeighborhood.usd b/scenes/two_drone_RetroNeighborhood.usd new file mode 100644 index 000000000..18bfb3358 Binary files /dev/null and b/scenes/two_drone_RetroNeighborhood.usd differ diff --git a/scenes/two_drone_fire_new.usd b/scenes/two_drone_fire_new.usd new file mode 100644 index 000000000..dd57a8d79 Binary files /dev/null and b/scenes/two_drone_fire_new.usd differ diff --git a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py new file mode 100644 index 000000000..16906d0b1 --- /dev/null +++ b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python3 + +import carb +from isaacsim import SimulationApp + +# Start Isaac Sim's simulation environment (Must start this before importing omni modules) +simulation_app = SimulationApp({"headless": False}) + +import omni.kit.app +import omni.timeline +import omni.usd +import omni.client +import asyncio +import time + +from omni.isaac.core.world import World +from omni.isaac.core.objects import GroundPlane +from pxr import Gf, UsdGeom, UsdLux, Sdf, UsdPhysics + +# Pegasus imports +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 + + +# --------------------- CONFIGURATION --------------------- +NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu" + +#env/stage path and scale +ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.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 = "/root/Documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd" + +# Lighting +ADD_DOME_LIGHT = True +DOME_LIGHT_PATH = "/World/DomeLight" +DOME_LIGHT_INTENSITY = 3500.0 +DOME_LIGHT_EXPOSURE = -3.0 + +#Drone offset +SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15 + +DRONE1_XY_M = (-3.0, 3.5) +DRONE2_XY_M = (3.0, 3.0) +# --------------------------------------------------------- + + +ext_manager = omni.kit.app.get_app().get_extension_manager() +for ext in [ + "omni.physx.forcefields", + "omni.graph.core", # Core runtime for OmniGraph engine + "omni.graph.action", # Action Graph framework + "omni.graph.action_nodes", # Built-in Action Graph node library + "omni.graph.ui", # UI scaffolding for graph tools + "omni.graph.visualization.nodes", # Visualization helper nodes + "omni.graph.scriptnode", # Python script node support + "omni.graph.window.action", # Action Graph editor window + "omni.graph.window.generic", # Generic graph UI tools + "omni.graph.ui_nodes", # UI node building helpers + "airlab.pegasus", # Airlab extension Pegasus core extension + "pegasus.simulator", +]: + if not ext_manager.is_extension_enabled(ext): + # Try immediate enable if available (more robust across Kit versions), fall back otherwise + try: + ext_manager.set_extension_enabled_immediate(ext, True) + except Exception: + ext_manager.set_extension_enabled(ext, True) + + +def nucleus_stat(url: str) -> bool: + result, info = omni.client.stat(url) + return result == omni.client.Result.OK + + +def add_dome_light(stage): + if stage.GetPrimAtPath(DOME_LIGHT_PATH).IsValid(): + dome = UsdLux.DomeLight.Get(stage, DOME_LIGHT_PATH) + else: + dome = UsdLux.DomeLight.Define(stage, Sdf.Path(DOME_LIGHT_PATH)) + + dome.CreateIntensityAttr(DOME_LIGHT_INTENSITY) + dome.CreateExposureAttr(DOME_LIGHT_EXPOSURE) + + +def get_stage_scale(stage): + mpu = UsdGeom.GetStageMetersPerUnit(stage) + if mpu is None or mpu <= 0: + mpu = 1.0 + s = 1.0 / mpu + return mpu, s + + +def add_collision_to_prim(prim): + if prim.IsA(UsdGeom.Mesh): + if not prim.HasAPI(UsdPhysics.CollisionAPI): + UsdPhysics.CollisionAPI.Apply(prim) + print(f"Added collision to: {prim.GetPath()}") + + # Recursively process children + for child in prim.GetChildren(): + add_collision_to_prim(child) + + +class PegasusApp: + + def __init__(self): + omni.client.initialize() + nucleus_stat(f"omniverse://{NUCLEUS_SERVER}") + nucleus_stat(ENV_URL) + + # Timeline for controlling play/stop + self.timeline = omni.timeline.get_timeline_interface() + + # Start Pegasus interface + world + self.pg = PegasusInterface() + self.pg._world = World(**self.pg._world_settings) + self.world = self.pg.world + + # Load environment + self.pg.load_environment(ENV_URL) + + stage = omni.usd.get_context().get_stage() + if stage is None: + raise RuntimeError("Stage failed to load") + + # Wait for the stage to fully load + for _ in range(100): # Wait up to ~10 seconds + omni.kit.app.get_app().update() + world_prim = stage.GetPrimAtPath("/World") + if world_prim.IsValid(): + children = list(world_prim.GetChildren()) + # Check if we have more than just PhysicsScene + non_physics_children = [c for c in children if c.GetName() != "PhysicsScene"] + if len(non_physics_children) > 0: + break + time.sleep(0.1) + + world_prim = stage.GetPrimAtPath("/World") + + # Scale the /World/stage prim + stage_prim = stage.GetPrimAtPath("/World/stage") + if stage_prim.IsValid(): + xformable = UsdGeom.Xformable(stage_prim) + xformable.ClearXformOpOrder() + + translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble) + translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0)) + + scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble) + scale_op.Set(Gf.Vec3d(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE)) + + add_collision_to_prim(stage_prim) + print("Finished adding collisions.") + + # Let the app process the changes + for _ in range(10): + omni.kit.app.get_app().update() + + # Optionally save the stage + # stage.GetRootLayer().Export("/path/to/save/scene.usd") + + else: + print("Warning: /World/stage not found, environment not scaled") + + # Lighting + if ADD_DOME_LIGHT: + add_dome_light(stage) + + # Units + mpu, s = get_stage_scale(stage) + + drone1_z_m = SPAWN_HEIGHT_ABOVE_FLOOR_M + drone2_z_m = SPAWN_HEIGHT_ABOVE_FLOOR_M + + drone1_pos = [DRONE1_XY_M[0] * s, DRONE1_XY_M[1] * s, drone1_z_m * s] + drone2_pos = [DRONE2_XY_M[0] * s, DRONE2_XY_M[1] * s, drone2_z_m * s] + + #################################################################################################### + # Spawn vehicle 1 + #################################################################################################### + graph_handle1 = spawn_px4_multirotor_node( + pegasus_node_name="PX4Multirotor_1", + drone_prim="/World/drone1/base_link", + robot_name="robot_1", + vehicle_id=1, # defines MAVLink port offset + domain_id=1, # defines ROS2 domain ID + usd_file=DRONE_USD, + init_pos=drone1_pos, + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + # Add a ZED stereo camera (with an associated subgraph) to the drone + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle1, + drone_prim="/World/drone1/base_link", + robot_name="robot_1", + camera_name="ZEDCamera", + camera_offset=[0.1, 0.0, 0.0], # X, Y, Z offset from drone base_link + camera_rotation_offset=[0.0, 0.0, 0.0], # Rotation in degrees (roll, pitch, yaw) + ) + + # Add an Ouster lidar (with an associated subgraph) to the drone + add_ouster_lidar_subgraph( + parent_graph_handle=graph_handle1, + drone_prim="/World/drone1/base_link", + robot_name="robot_1", + lidar_name="OS1_REV6_128_10hz___512_resolution", + lidar_offset=[0.0, 0.0, 0.025], # X, Y, Z offset from drone base_link + lidar_rotation_offset=[0.0, 0.0, 0.0], # Rotation in degrees (roll, pitch, yaw) + lidar_min_range = 0.75, # Minimum detection range (m) to avoid propeller hits + ) + + #################################################################################################### + # Spawn vehicle 2 + #################################################################################################### + graph_handle2 = spawn_px4_multirotor_node( + pegasus_node_name="PX4Multirotor_2", + drone_prim="/World/drone2/base_link", + robot_name="robot_2", + vehicle_id=2, # defines MAVLink port offset. Define as 2 for second vehicle + domain_id=2, # defines ROS2 domain ID. Define as 2 for second vehicle + usd_file=DRONE_USD, + init_pos=drone2_pos, + init_orient=[0.0, 0.0, 0.0, 1.0], + ) + + # Add a ZED stereo camera (with an associated subgraph) to the drone + add_zed_stereo_camera_subgraph( + parent_graph_handle=graph_handle2, + drone_prim="/World/drone2/base_link", + robot_name="robot_2", + camera_name="ZEDCamera", + camera_offset=[0.1, 0.0, 0.0], # X, Y, Z offset from drone base_link + camera_rotation_offset=[0.0, 0.0, 0.0], # Rotation in degrees (roll, pitch, yaw) + ) + + # Add an Ouster lidar (with an associated subgraph) to the drone + add_ouster_lidar_subgraph( + parent_graph_handle=graph_handle2, + drone_prim="/World/drone2/base_link", + robot_name="robot_2", + lidar_name="OS1_REV6_128_10hz___512_resolution", + lidar_offset=[0.0, 0.0, 0.025], # X, Y, Z offset from drone base_link + lidar_rotation_offset=[0.0, 0.0, 0.0], + lidar_min_range = 0.75 + ) + + # Reset so physics/articulations are ready + self.world.reset() + + self.stop_sim = False + + def run(self): + # Start sim timeline + self.timeline.play() + + # Main loop + while simulation_app.is_running() and not self.stop_sim: + try: + self.world.step(render=True) + except Exception as e: + carb.log_error(f"Error during simulation step: {e}") + break + + # Cleanup + carb.log_warn("PegasusApp Simulation App is closing.") + self.timeline.stop() + simulation_app.close() + + +def main(): + pg_app = PegasusApp() + pg_app.run() + + +if __name__ == "__main__": + main() \ No newline at end of file