From a06bb25e14fde2c8a5ea7d4a2fc6b14b17aee7df Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Fri, 13 Feb 2026 14:12:43 -0500
Subject: [PATCH 01/55] Fixed multi-drone global plan

---
 common/ros_packages/desktop_bringup/rviz/robot.rviz             | 1 -
 robot/ros_ws/src/global/global_bringup/launch/global.launch.xml | 1 +
 .../global/planners/random_walk/config/random_walk_config.yaml  | 2 ++
 3 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz
index 183df82e..d3c3c312 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz
@@ -688,7 +688,6 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
-                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 6b153253..490570ff 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -14,6 +14,7 @@
         <remap from="~/exploration_task" to="/$(env ROBOT_NAME)/tasks/exploration" />
         <remap from="navigate_task" to="/$(env ROBOT_NAME)/tasks/navigate" />
         <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node>
     
 </launch>
diff --git a/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml b/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml
index ac5da3ad..25101979 100644
--- a/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml
+++ b/robot/ros_ws/src/global/planners/random_walk/config/random_walk_config.yaml
@@ -6,6 +6,8 @@
     pub_trajectory_viz_topic: "~/traj_viz"
     sub_map_topic: "vdb_map_visualization"
     sub_odometry_topic: "odometry"
+    sub_robot_tf_topic: "/tf"
+    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

From 3b993b7d881a9f83b2988639e923e9b2986fe875 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Tue, 17 Feb 2026 17:07:13 -0500
Subject: [PATCH 02/55] added sep files for fire and retro

---
 .../desktop_bringup/CMakeLists.txt            |   5 +
 .../desktop_bringup/robot_1_odom_bridge.py    |  28 ++
 .../robot_2_relative_position.py              |   0
 .../robot_descriptions/CMakeLists.txt         |   5 +
 .../src/robot_2_relative_position.py          |  61 ++++
 .../src/robot_2_relative_position.py          |  79 +++++
 scenes/fire_academy_faro_with_sky.scene.usd   | Bin 0 -> 24731 bytes
 scenes/two_drone_RetroNeighborhood.usd        | Bin 175503 -> 0 bytes
 scenes/two_drone_fire_new.usd                 | Bin 785787 -> 0 bytes
 .../two_drone_RetroNeighbourhood.py           | 284 ++++++++++++++++++
 .../launch_scripts/two_drone_fire_academy.py  | 284 ++++++++++++++++++
 .../launch_scripts/two_drone_scene_import.py  |   8 +-
 12 files changed, 750 insertions(+), 4 deletions(-)
 create mode 100755 common/ros_packages/desktop_bringup/robot_1_odom_bridge.py
 create mode 100755 common/ros_packages/desktop_bringup/robot_2_relative_position.py
 create mode 100644 gcs/gcs_bringup/src/robot_2_relative_position.py
 create mode 100755 gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py
 create mode 100644 scenes/fire_academy_faro_with_sky.scene.usd
 delete mode 100644 scenes/two_drone_RetroNeighborhood.usd
 delete mode 100644 scenes/two_drone_fire_new.usd
 create mode 100644 simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
 create mode 100644 simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py

diff --git a/common/ros_packages/desktop_bringup/CMakeLists.txt b/common/ros_packages/desktop_bringup/CMakeLists.txt
index 159bf518..4af7f60f 100644
--- a/common/ros_packages/desktop_bringup/CMakeLists.txt
+++ b/common/ros_packages/desktop_bringup/CMakeLists.txt
@@ -19,4 +19,9 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
+install(PROGRAMS
+  robot_2_relative_position.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()
diff --git a/common/ros_packages/desktop_bringup/robot_1_odom_bridge.py b/common/ros_packages/desktop_bringup/robot_1_odom_bridge.py
new file mode 100755
index 00000000..0c9b228f
--- /dev/null
+++ b/common/ros_packages/desktop_bringup/robot_1_odom_bridge.py
@@ -0,0 +1,28 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+
+class OdomBridgeNode(Node):
+    def __init__(self):
+        super().__init__('robot_1_odom_bridge')
+        self.pub = self.create_publisher(Odometry, '/robot_1/odom_bridge', 10)
+        self.sub = self.create_subscription(
+            Odometry,
+            '/robot_1/odometry_conversion/odometry',
+            self.odom_callback,
+            10
+        )
+
+    def odom_callback(self, msg):
+        self.pub.publish(msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomBridgeNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/common/ros_packages/desktop_bringup/robot_2_relative_position.py b/common/ros_packages/desktop_bringup/robot_2_relative_position.py
new file mode 100755
index 00000000..e69de29b
diff --git a/common/ros_packages/robot_descriptions/CMakeLists.txt b/common/ros_packages/robot_descriptions/CMakeLists.txt
index e06c17c9..6d26a041 100644
--- a/common/ros_packages/robot_descriptions/CMakeLists.txt
+++ b/common/ros_packages/robot_descriptions/CMakeLists.txt
@@ -24,4 +24,9 @@ endif()
 install(DIRECTORY iris DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
+install(PROGRAMS
+  src/robot_2_relative_position.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()
diff --git a/gcs/gcs_bringup/src/robot_2_relative_position.py b/gcs/gcs_bringup/src/robot_2_relative_position.py
new file mode 100644
index 00000000..064cf31f
--- /dev/null
+++ b/gcs/gcs_bringup/src/robot_2_relative_position.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+
+class RelativePositionNode(Node):
+    def __init__(self, robot_1_init):
+        super().__init__('robot_2_relative_position')
+        self.robot_1_init = robot_1_init  # (x, y, z)
+        self.robot_1_pos = None
+        self.robot_2_pos = None
+        self.rel_pub = self.create_publisher(Odometry, '/robot_2/relative_position', 10)
+        self.create_subscription(
+            Odometry,
+            '/robot_1/odometry_conversion/odometry',
+            self.robot_1_callback,
+            10
+        )
+        self.create_subscription(
+            Odometry,
+            '/robot_2/odometry_conversion/odometry',
+            self.robot_2_callback,
+            10
+        )
+
+    def robot_1_callback(self, msg):
+        self.robot_1_pos = (
+            msg.pose.pose.position.x,
+            msg.pose.pose.position.y,
+            msg.pose.pose.position.z
+        )
+
+    def robot_2_callback(self, msg):
+        self.robot_2_pos = (
+            msg.pose.pose.position.x,
+            msg.pose.pose.position.y,
+            msg.pose.pose.position.z
+        )
+        self.publish_relative(msg)
+
+    def publish_relative(self, msg):
+        if self.robot_1_pos is None or self.robot_2_pos is None:
+            return
+        rel = [self.robot_2_pos[i] - self.robot_1_pos[i] for i in range(3)]
+        rel_msg = Odometry()
+        rel_msg.header = msg.header
+        rel_msg.pose.pose.position.x = rel[0]
+        rel_msg.pose.pose.position.y = rel[1]
+        rel_msg.pose.pose.position.z = rel[2]
+        self.rel_pub.publish(rel_msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+    robot_1_init = [35.0, -19, 0.0015]
+    node = RelativePositionNode(robot_1_init)
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
diff --git a/gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py b/gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py
new file mode 100755
index 00000000..687b9533
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+
+
+class Robot2RelToRobot1Start(Node):
+    """
+    Publishes robot_2 position expressed in a frame whose origin is robot_1's
+    INITIAL world position.
+
+    Inputs:
+      /robot_2/odometry_conversion/odometry  (robot2 LOCAL odom; starts near 0,0,0)
+
+    Known constants (world):
+      robot_1_init, robot_2_init
+
+    Output:
+      /robot_2/relative_position (Odometry) where:
+        p_rel = (robot_2_init - robot_1_init) + p2_local
+    """
+
+    def __init__(self, robot_1_init, robot_2_init):
+        super().__init__('robot_2_relative_to_robot_1_start')
+
+        self.robot_1_init = tuple(robot_1_init)  # (x,y,z) in world
+        self.robot_2_init = tuple(robot_2_init)  # (x,y,z) in world
+
+        # Constant offset from robot1 start to robot2 start in world
+        self.offset = (
+            self.robot_2_init[0] - self.robot_1_init[0],
+            self.robot_2_init[1] - self.robot_1_init[1],
+            self.robot_2_init[2] - self.robot_1_init[2],
+        )
+
+        self.rel_pub = self.create_publisher(Odometry, '/robot_2/relative_position', 10)
+
+        self.create_subscription(
+            Odometry,
+            '/robot_2/odometry_conversion/odometry',
+            self.robot_2_callback,
+            10
+        )
+
+    def robot_2_callback(self, msg: Odometry):
+        p2 = msg.pose.pose.position  # robot2 LOCAL (0,0,0 at its start)
+
+        rel_msg = Odometry()
+        rel_msg.header = msg.header
+
+        # Make the frame semantics explicit (optional but helpful)
+        rel_msg.header.frame_id = 'robot_1_start'
+        rel_msg.child_frame_id = 'robot_2_relative'
+
+        rel_msg.pose.pose.position.x = self.offset[0] + p2.x
+        rel_msg.pose.pose.position.y = self.offset[1] + p2.y
+        rel_msg.pose.pose.position.z = self.offset[2] + p2.z
+
+        # If you only care about position, set identity orientation
+        rel_msg.pose.pose.orientation.w = 1.0
+
+        self.rel_pub.publish(rel_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    robot_1_init = (-3.0, 3.5, 0.15)
+    robot_2_init = ( 3.0, 3.0, 0.15)
+
+    node = Robot2RelToRobot1Start(robot_1_init, robot_2_init)
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/scenes/fire_academy_faro_with_sky.scene.usd b/scenes/fire_academy_faro_with_sky.scene.usd
new file mode 100644
index 0000000000000000000000000000000000000000..622e7a360040a1320b8e5675bd7f2f475e36ddea
GIT binary patch
diff --git a/scenes/two_drone_RetroNeighborhood.usd b/scenes/two_drone_RetroNeighborhood.usd
deleted file mode 100644
index 18bfb335885b9c147afcb192015a8e6015f7a450..0000000000000000000000000000000000000000
GIT binary patch
diff --git a/scenes/two_drone_fire_new.usd b/scenes/two_drone_fire_new.usd
deleted file mode 100644
index dd57a8d7981929afe69f6501ea1d7a90872311a2..0000000000000000000000000000000000000000
GIT binary patch
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py b/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
new file mode 100644
index 00000000..2b83b76c
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.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 = (35.0, -19)
+DRONE2_XY_M = (30, -19)
+# ---------------------------------------------------------
+
+
+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
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
new file mode 100644
index 00000000..30f7e508
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.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/Assets/Fire_Academy_Digital_Twin/fire_academy.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 = 2.0
+
+
+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 = (20.0, -7)
+DRONE2_XY_M = (17, 1.5)
+# ---------------------------------------------------------
+
+
+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 = 1.0, # 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 = 1.0 
+        )
+
+        # 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
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
index 70ff96f4..e5c911e2 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
@@ -28,11 +28,11 @@
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
 
 #env/stage path and scale
-ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.usd"
+ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.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
+STAGE_SCALE = 2.0
 
 
 DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
@@ -46,8 +46,8 @@
 #Drone offset
 SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
 
-DRONE1_XY_M = (-3.0, 3.5)
-DRONE2_XY_M = (3.0, 3.0)
+DRONE1_XY_M = (20.0, -7)
+DRONE2_XY_M = (17, 1.5)
 # ---------------------------------------------------------
 
 

From b68bd9b1faddc109e62046171f632cb06d513180 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Wed, 18 Feb 2026 16:27:54 -0500
Subject: [PATCH 03/55] added robot2 relative pos; diff rviz files; bridge for
 rayfronts topics

---
 .../desktop_bringup/params/domain_bridge.yaml |   27 +
 .../desktop_bringup/rviz/robot_1.rviz         | 1045 +++++++++++++++++
 .../desktop_bringup/rviz/robot_2.rviz         | 1045 +++++++++++++++++
 .../src/robot_2_relative_position.py          |    2 +-
 .../src/robot_2_relative_position.py          |   79 --
 robot/ros_ws/src/input_prompt.py              |   64 +
 .../config/takeoff_landing_planner.yaml       |    2 +-
 .../two_drone_RetroNeighbourhood.py           |    2 +-
 .../launch_scripts/two_drone_fire_academy.py  |    8 +-
 9 files changed, 2188 insertions(+), 86 deletions(-)
 create mode 100644 common/ros_packages/desktop_bringup/rviz/robot_1.rviz
 create mode 100644 common/ros_packages/desktop_bringup/rviz/robot_2.rviz
 delete mode 100755 gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py
 create mode 100644 robot/ros_ws/src/input_prompt.py

diff --git a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
index c07a5acb..6f6ce4c2 100644
--- a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
+++ b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
@@ -18,6 +18,33 @@ topics:
     from_domain: 1
     to_domain: 0
     type: nav_msgs/msg/Odometry
+  
+
+  #rayfronts topics to bridge
+  /robot_1/odometry_conversion/odometry:
+    from_domain: 1
+    to_domain: 2
+    type: nav_msgs/msg/Odometry
+  
+  /robot_2/odometry_conversion/odometry:
+    from_domain: 2
+    to_domain: 1
+    type: nav_msgs/msg/Odometry
+  
+  /robot_2/relative_position:
+    from_domain: 2
+    to_domain: 1
+    type: nav_msgs/msg/Odometry
+  
+  /robot_1/filtered_rays:
+    from_domain: 1
+    to_domain: 2
+    type: visualization_msgs/msg/Marker
+  
+  /robot_2/filtered_rays:
+    from_domain: 2
+    to_domain: 1
+    type: visualization_msgs/msg/Marker
 
   /robot_2/vdb_mapping/vdb_map_visualization:
     from_domain: 2
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
new file mode 100644
index 00000000..3a224bad
--- /dev/null
+++ b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
@@ -0,0 +1,1045 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 70
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Frames1
+        - /RobotModel1
+        - /Sensors1
+        - /Sensors1/Lidar1
+        - /Perception1
+        - /Local1
+        - /Local1/DROAN1
+        - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
+        - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
+        - /Global1
+        - /Global1/Global Plan1
+        - /PointCloud21
+        - /PointCloud22
+        - /PointCloud23
+        - /MarkerArray1
+        - /MarkerArray1/Topic1
+        - /MarkerArray2
+        - /MarkerArray2/Topic1
+      Splitter Ratio: 0.590062141418457
+    Tree Height: 224
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_behavior_tree_panel::BehaviorTreePanel
+    Name: BehaviorTreePanel
+    topic: /robot_1/behavior/behavior_tree_graphviz
+    zoom_factor: 0.1919851303100586
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        OS1_REV6_128_10hz___512_resolution:
+          Value: false
+        base_link:
+          Value: true
+        base_link_ZED_X:
+          Value: false
+        base_link_body_body_link:
+          Value: false
+        base_link_frd:
+          Value: true
+        base_link_stabilized:
+          Value: false
+        camera_left:
+          Value: true
+        camera_right:
+          Value: true
+        imu:
+          Value: false
+        lidar:
+          Value: false
+        look_ahead_point:
+          Value: false
+        look_ahead_point_stabilized:
+          Value: false
+        macvo_ned:
+          Value: false
+        map:
+          Value: true
+        map_ned:
+          Value: false
+        odom:
+          Value: true
+        odom_ned:
+          Value: true
+        rotor0:
+          Value: true
+        rotor1:
+          Value: true
+        rotor2:
+          Value: true
+        rotor3:
+          Value: true
+        tracking_point:
+          Value: false
+        tracking_point_stabilized:
+          Value: false
+        world:
+          Value: false
+      Marker Scale: 0.30000001192092896
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        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:
+                    {}
+                rotor0:
+                  {}
+                rotor1:
+                  {}
+                rotor2:
+                  {}
+                rotor3:
+                  {}
+              base_link_frd:
+                {}
+            base_link_stabilized:
+              {}
+            look_ahead_point:
+              {}
+            look_ahead_point_stabilized:
+              {}
+            map_ned:
+              {}
+            tracking_point:
+              {}
+            tracking_point_stabilized:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.5
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        OS1_REV6_128_10hz___512_resolution:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link_ZED_X:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link_body_body_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rotor0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rotor1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rotor2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rotor3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Front Right RGB
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/sensors/front_stereo/right/image_rect
+          Value: true
+        - Class: rviz_default_plugins/Image
+          Enabled: false
+          Max Value: 100
+          Median window: 5
+          Min Value: 0
+          Name: Front Right Depth
+          Normalize Range: false
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: sensors/front_stereo/right/depth_ground_truth
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 4.254574298858643
+            Min Value: -0.06456756591796875
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 170; 170; 255
+          Color Transformer: AxisColor
+          Decay Time: 1
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Lidar
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: sensors/lidar/point_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Angle Tolerance: 0
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: false
+          Keep: 1
+          Name: Odometry
+          Position Tolerance: 0
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Axes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: odometry_conversion/odometry
+          Value: false
+        - Class: rviz_default_plugins/Image
+          Enabled: false
+          Max Value: 100
+          Median window: 5
+          Min Value: 0
+          Name: Front Right Depth
+          Normalize Range: false
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/sensors/front_stereo/right/depth_ground_truth
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 18.48733139038086
+            Min Value: -5.648834705352783
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 170; 170; 255
+          Color Transformer: RGB8
+          Decay Time: 1
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Disparity
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/sensors/front_stereo/pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+      Enabled: true
+      Name: Sensors
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: MACVO Disparity
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/perception/macvo/disparity
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud
+          Color: 255; 255; 255
+          Color Transformer: RGBF32
+          Decay Time: 5
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: MACVO PointCloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/perception/macvo/point_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 100
+          Name: MACVO Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/macvo/odometry
+          Value: true
+      Enabled: false
+      Name: Perception
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Trimmed Global Plan for DROAN
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: droan/local_planner_global_plan_vis
+              Value: true
+            - Class: rviz_common/Group
+              Displays:
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: false
+                  Name: ExpansionPoly
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: droan/expansion_poly
+                  Value: false
+                - Class: rviz_default_plugins/Marker
+                  Enabled: false
+                  Name: Disparity Frustum
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_1/droan/frustum
+                  Value: false
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: false
+                  Name: Disparity Map Collision Checking
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_1/droan/disparity_map_debug
+                  Value: false
+                - Alpha: 1
+                  Autocompute Intensity Bounds: true
+                  Autocompute Value Bounds:
+                    Max Value: 10
+                    Min Value: -10
+                    Value: true
+                  Axis: Z
+                  Channel Name: intensity
+                  Class: rviz_default_plugins/PointCloud2
+                  Color: 255; 255; 255
+                  Color Transformer: Intensity
+                  Decay Time: 0
+                  Enabled: false
+                  Invert Rainbow: false
+                  Max Color: 255; 255; 255
+                  Max Intensity: 220
+                  Min Color: 0; 0; 0
+                  Min Intensity: 120
+                  Name: Expansion Cloud
+                  Position Transformer: XYZ
+                  Selectable: true
+                  Size (Pixels): 3
+                  Size (m): 0.009999999776482582
+                  Style: Flat Squares
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: droan/expansion_cloud
+                  Use Fixed Frame: true
+                  Use rainbow: true
+                  Value: false
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: false
+                  Name: Traj Library
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: droan/trajectory_library_vis
+                  Value: false
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: true
+                  Name: Disparity Graph Poses
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_1/droan/disparity_graph
+                  Value: true
+              Enabled: false
+              Name: Droan CPU
+            - Class: rviz_common/Group
+              Displays:
+                - Alpha: 1
+                  Autocompute Intensity Bounds: true
+                  Autocompute Value Bounds:
+                    Max Value: 10
+                    Min Value: -10
+                    Value: true
+                  Axis: Z
+                  Channel Name: intensity
+                  Class: rviz_default_plugins/PointCloud2
+                  Color: 255; 255; 255
+                  Color Transformer: Intensity
+                  Decay Time: 0
+                  Enabled: true
+                  Invert Rainbow: false
+                  Max Color: 255; 255; 255
+                  Max Intensity: 45
+                  Min Color: 0; 0; 0
+                  Min Intensity: 0
+                  Name: Foreground Background Cloud
+                  Position Transformer: XYZ
+                  Selectable: true
+                  Size (Pixels): 3
+                  Size (m): 0.009999999776482582
+                  Style: Points
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_1/droan/fg_bg_cloud
+                  Use Fixed Frame: true
+                  Use rainbow: false
+                  Value: true
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: true
+                  Name: Traj Debug
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_1/droan/traj_debug
+                  Value: true
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: true
+                  Name: Graph Vis
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_1/droan/graph_vis
+                  Value: true
+              Enabled: false
+              Name: Droan GPU
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Virtual Obstacles
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: droan/virtual_obstacles
+              Value: true
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Global Plan Vis
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_1/droan/local_planner_global_plan_vis
+              Value: true
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Rewind Info
+              Namespaces:
+                rewind_info: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_1/droan/rewind_info
+              Value: true
+          Enabled: true
+          Name: DROAN
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Traj Vis
+              Namespaces:
+                traj_controller: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: trajectory_controller/trajectory_vis
+              Value: true
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: false
+              Name: Traj Debug
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: trajectory_controller/trajectory_controller_debug_markers
+              Value: false
+          Enabled: true
+          Name: Trajectory Controller
+      Enabled: true
+      Name: Local
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Marker
+          Enabled: false
+          Name: VDB Mapping Marker
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: vdb_mapping/vdb_map_visualization
+          Value: false
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 0; 255; 255
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Billboards
+          Line Width: 0.30000001192092896
+          Name: Global Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/global_plan
+          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_1/sensors/front_stereo/left/image_rect
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 0
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/frontier_viewpoints
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      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_1/sensors/front_stereo/left/image_rect
+      Value: false
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      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_1/sensors/front_stereo/left/image_rect
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/voxel_rgb
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.20000000298023224
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/frontiers
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/semantic_ray_frontiers
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/filtered_rays
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 75.61663818359375
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 7.733954906463623
+        Y: 3.460301399230957
+        Z: -13.681711196899414
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.0847973823547363
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 5.58674430847168
+    Saved: ~
+Window Geometry:
+  BehaviorTreePanel:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Front Right Depth:
+    collapsed: false
+  Front Right RGB:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  MACVO Disparity:
+    collapsed: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2273
+  X: 1072
+  Y: 27
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
new file mode 100644
index 00000000..73af31cb
--- /dev/null
+++ b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
@@ -0,0 +1,1045 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 70
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Frames1
+        - /RobotModel1
+        - /Sensors1
+        - /Sensors1/Lidar1
+        - /Perception1
+        - /Local1
+        - /Local1/DROAN1
+        - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
+        - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
+        - /Global1
+        - /Global1/Global Plan1
+        - /PointCloud21
+        - /PointCloud22
+        - /PointCloud23
+        - /MarkerArray1
+        - /MarkerArray1/Topic1
+        - /MarkerArray2
+        - /MarkerArray2/Topic1
+      Splitter Ratio: 0.590062141418457
+    Tree Height: 224
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_behavior_tree_panel::BehaviorTreePanel
+    Name: BehaviorTreePanel
+    topic: /robot_2/behavior/behavior_tree_graphviz
+    zoom_factor: 0.1919851303100586
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        OS1_REV6_128_10hz___512_resolution:
+          Value: false
+        base_link:
+          Value: true
+        base_link_ZED_X:
+          Value: false
+        base_link_body_body_link:
+          Value: false
+        base_link_frd:
+          Value: true
+        base_link_stabilized:
+          Value: false
+        camera_left:
+          Value: true
+        camera_right:
+          Value: true
+        imu:
+          Value: false
+        lidar:
+          Value: false
+        look_ahead_point:
+          Value: false
+        look_ahead_point_stabilized:
+          Value: false
+        macvo_ned:
+          Value: false
+        map:
+          Value: true
+        map_ned:
+          Value: false
+        odom:
+          Value: true
+        odom_ned:
+          Value: true
+        rotor0:
+          Value: true
+        rotor1:
+          Value: true
+        rotor2:
+          Value: true
+        rotor3:
+          Value: true
+        tracking_point:
+          Value: false
+        tracking_point_stabilized:
+          Value: false
+        world:
+          Value: false
+      Marker Scale: 0.30000001192092896
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        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:
+                    {}
+                rotor0:
+                  {}
+                rotor1:
+                  {}
+                rotor2:
+                  {}
+                rotor3:
+                  {}
+              base_link_frd:
+                {}
+            base_link_stabilized:
+              {}
+            look_ahead_point:
+              {}
+            look_ahead_point_stabilized:
+              {}
+            map_ned:
+              {}
+            tracking_point:
+              {}
+            tracking_point_stabilized:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.5
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        OS1_REV6_128_10hz___512_resolution:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link_ZED_X:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link_body_body_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        lidar:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        rotor0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rotor1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rotor2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rotor3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Front Right RGB
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/sensors/front_stereo/right/image_rect
+          Value: true
+        - Class: rviz_default_plugins/Image
+          Enabled: false
+          Max Value: 100
+          Median window: 5
+          Min Value: 0
+          Name: Front Right Depth
+          Normalize Range: false
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: sensors/front_stereo/right/depth_ground_truth
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 4.254574298858643
+            Min Value: -0.06456756591796875
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 170; 170; 255
+          Color Transformer: AxisColor
+          Decay Time: 1
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Lidar
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 5
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: sensors/lidar/point_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Angle Tolerance: 0
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: false
+          Keep: 1
+          Name: Odometry
+          Position Tolerance: 0
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Axes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: odometry_conversion/odometry
+          Value: false
+        - Class: rviz_default_plugins/Image
+          Enabled: false
+          Max Value: 100
+          Median window: 5
+          Min Value: 0
+          Name: Front Right Depth
+          Normalize Range: false
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/sensors/front_stereo/right/depth_ground_truth
+          Value: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 18.48733139038086
+            Min Value: -5.648834705352783
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 170; 170; 255
+          Color Transformer: RGB8
+          Decay Time: 1
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Disparity
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Points
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/sensors/front_stereo/pointcloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+      Enabled: true
+      Name: Sensors
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: MACVO Disparity
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/perception/macvo/disparity
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud
+          Color: 255; 255; 255
+          Color Transformer: RGBF32
+          Decay Time: 5
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: MACVO PointCloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/perception/macvo/point_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 100
+          Name: MACVO Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 1
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Arrow
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/macvo/odometry
+          Value: true
+      Enabled: false
+      Name: Perception
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Trimmed Global Plan for DROAN
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: droan/local_planner_global_plan_vis
+              Value: true
+            - Class: rviz_common/Group
+              Displays:
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: false
+                  Name: ExpansionPoly
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: droan/expansion_poly
+                  Value: false
+                - Class: rviz_default_plugins/Marker
+                  Enabled: false
+                  Name: Disparity Frustum
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_2/droan/frustum
+                  Value: false
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: false
+                  Name: Disparity Map Collision Checking
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_2/droan/disparity_map_debug
+                  Value: false
+                - Alpha: 1
+                  Autocompute Intensity Bounds: true
+                  Autocompute Value Bounds:
+                    Max Value: 10
+                    Min Value: -10
+                    Value: true
+                  Axis: Z
+                  Channel Name: intensity
+                  Class: rviz_default_plugins/PointCloud2
+                  Color: 255; 255; 255
+                  Color Transformer: Intensity
+                  Decay Time: 0
+                  Enabled: false
+                  Invert Rainbow: false
+                  Max Color: 255; 255; 255
+                  Max Intensity: 220
+                  Min Color: 0; 0; 0
+                  Min Intensity: 120
+                  Name: Expansion Cloud
+                  Position Transformer: XYZ
+                  Selectable: true
+                  Size (Pixels): 3
+                  Size (m): 0.009999999776482582
+                  Style: Flat Squares
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: droan/expansion_cloud
+                  Use Fixed Frame: true
+                  Use rainbow: true
+                  Value: false
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: false
+                  Name: Traj Library
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: droan/trajectory_library_vis
+                  Value: false
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: true
+                  Name: Disparity Graph Poses
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_2/droan/disparity_graph
+                  Value: true
+              Enabled: false
+              Name: Droan CPU
+            - Class: rviz_common/Group
+              Displays:
+                - Alpha: 1
+                  Autocompute Intensity Bounds: true
+                  Autocompute Value Bounds:
+                    Max Value: 10
+                    Min Value: -10
+                    Value: true
+                  Axis: Z
+                  Channel Name: intensity
+                  Class: rviz_default_plugins/PointCloud2
+                  Color: 255; 255; 255
+                  Color Transformer: Intensity
+                  Decay Time: 0
+                  Enabled: true
+                  Invert Rainbow: false
+                  Max Color: 255; 255; 255
+                  Max Intensity: 45
+                  Min Color: 0; 0; 0
+                  Min Intensity: 0
+                  Name: Foreground Background Cloud
+                  Position Transformer: XYZ
+                  Selectable: true
+                  Size (Pixels): 3
+                  Size (m): 0.009999999776482582
+                  Style: Points
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    Filter size: 10
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_2/droan/fg_bg_cloud
+                  Use Fixed Frame: true
+                  Use rainbow: false
+                  Value: true
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: true
+                  Name: Traj Debug
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_2/droan/traj_debug
+                  Value: true
+                - Class: rviz_default_plugins/MarkerArray
+                  Enabled: true
+                  Name: Graph Vis
+                  Namespaces:
+                    {}
+                  Topic:
+                    Depth: 5
+                    Durability Policy: Volatile
+                    History Policy: Keep Last
+                    Reliability Policy: Reliable
+                    Value: /robot_2/droan/graph_vis
+                  Value: true
+              Enabled: false
+              Name: Droan GPU
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Virtual Obstacles
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: droan/virtual_obstacles
+              Value: true
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Global Plan Vis
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_2/droan/local_planner_global_plan_vis
+              Value: true
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Rewind Info
+              Namespaces:
+                rewind_info: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: /robot_2/droan/rewind_info
+              Value: true
+          Enabled: true
+          Name: DROAN
+        - Class: rviz_common/Group
+          Displays:
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: true
+              Name: Traj Vis
+              Namespaces:
+                traj_controller: true
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: trajectory_controller/trajectory_vis
+              Value: true
+            - Class: rviz_default_plugins/MarkerArray
+              Enabled: false
+              Name: Traj Debug
+              Namespaces:
+                {}
+              Topic:
+                Depth: 5
+                Durability Policy: Volatile
+                History Policy: Keep Last
+                Reliability Policy: Reliable
+                Value: trajectory_controller/trajectory_controller_debug_markers
+              Value: false
+          Enabled: true
+          Name: Trajectory Controller
+      Enabled: true
+      Name: Local
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Marker
+          Enabled: false
+          Name: VDB Mapping Marker
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: vdb_mapping/vdb_map_visualization
+          Value: false
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 0; 255; 255
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Billboards
+          Line Width: 0.30000001192092896
+          Name: Global Plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_2/global_plan
+          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
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 0
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/frontier_viewpoints
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      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: false
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      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: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/rayfronts/voxel_rgb
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.20000000298023224
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/rayfronts/frontiers
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/rayfronts/semantic_ray_frontiers
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/filtered_rays
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 75.61663818359375
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 7.733954906463623
+        Y: 3.460301399230957
+        Z: -13.681711196899414
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.0847973823547363
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 5.58674430847168
+    Saved: ~
+Window Geometry:
+  BehaviorTreePanel:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Front Right Depth:
+    collapsed: false
+  Front Right RGB:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  MACVO Disparity:
+    collapsed: false
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2273
+  X: 1072
+  Y: 27
diff --git a/gcs/gcs_bringup/src/robot_2_relative_position.py b/gcs/gcs_bringup/src/robot_2_relative_position.py
index 064cf31f..f85ad2ed 100644
--- a/gcs/gcs_bringup/src/robot_2_relative_position.py
+++ b/gcs/gcs_bringup/src/robot_2_relative_position.py
@@ -51,7 +51,7 @@ def publish_relative(self, msg):
 
 def main(args=None):
     rclpy.init(args=args)
-    robot_1_init = [35.0, -19, 0.0015]
+    robot_1_init = [20.0, -7, 0.0015]
     node = RelativePositionNode(robot_1_init)
     rclpy.spin(node)
     node.destroy_node()
diff --git a/gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py b/gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py
deleted file mode 100755
index 687b9533..00000000
--- a/gcs/ros_ws/src/gcs_bringup/src/robot_2_relative_position.py
+++ /dev/null
@@ -1,79 +0,0 @@
-#!/usr/bin/env python3
-import rclpy
-from rclpy.node import Node
-from nav_msgs.msg import Odometry
-
-
-class Robot2RelToRobot1Start(Node):
-    """
-    Publishes robot_2 position expressed in a frame whose origin is robot_1's
-    INITIAL world position.
-
-    Inputs:
-      /robot_2/odometry_conversion/odometry  (robot2 LOCAL odom; starts near 0,0,0)
-
-    Known constants (world):
-      robot_1_init, robot_2_init
-
-    Output:
-      /robot_2/relative_position (Odometry) where:
-        p_rel = (robot_2_init - robot_1_init) + p2_local
-    """
-
-    def __init__(self, robot_1_init, robot_2_init):
-        super().__init__('robot_2_relative_to_robot_1_start')
-
-        self.robot_1_init = tuple(robot_1_init)  # (x,y,z) in world
-        self.robot_2_init = tuple(robot_2_init)  # (x,y,z) in world
-
-        # Constant offset from robot1 start to robot2 start in world
-        self.offset = (
-            self.robot_2_init[0] - self.robot_1_init[0],
-            self.robot_2_init[1] - self.robot_1_init[1],
-            self.robot_2_init[2] - self.robot_1_init[2],
-        )
-
-        self.rel_pub = self.create_publisher(Odometry, '/robot_2/relative_position', 10)
-
-        self.create_subscription(
-            Odometry,
-            '/robot_2/odometry_conversion/odometry',
-            self.robot_2_callback,
-            10
-        )
-
-    def robot_2_callback(self, msg: Odometry):
-        p2 = msg.pose.pose.position  # robot2 LOCAL (0,0,0 at its start)
-
-        rel_msg = Odometry()
-        rel_msg.header = msg.header
-
-        # Make the frame semantics explicit (optional but helpful)
-        rel_msg.header.frame_id = 'robot_1_start'
-        rel_msg.child_frame_id = 'robot_2_relative'
-
-        rel_msg.pose.pose.position.x = self.offset[0] + p2.x
-        rel_msg.pose.pose.position.y = self.offset[1] + p2.y
-        rel_msg.pose.pose.position.z = self.offset[2] + p2.z
-
-        # If you only care about position, set identity orientation
-        rel_msg.pose.pose.orientation.w = 1.0
-
-        self.rel_pub.publish(rel_msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    robot_1_init = (-3.0, 3.5, 0.15)
-    robot_2_init = ( 3.0, 3.0, 0.15)
-
-    node = Robot2RelToRobot1Start(robot_1_init, robot_2_init)
-    rclpy.spin(node)
-
-    node.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()
diff --git a/robot/ros_ws/src/input_prompt.py b/robot/ros_ws/src/input_prompt.py
new file mode 100644
index 00000000..8bcc56cb
--- /dev/null
+++ b/robot/ros_ws/src/input_prompt.py
@@ -0,0 +1,64 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import threading
+import time
+
+class InputPublisher(Node):
+    def __init__(self):
+        super().__init__('input_publisher')
+        self.publisher_ = self.create_publisher(String, '/input_prompt', 10)
+        self.current_input = ""
+        self.running = True
+        self.publish_thread = threading.Thread(target=self.publish_loop)
+        self.publish_thread.start()
+
+    def publish_loop(self):
+        msg = String()
+        while self.running:
+            msg.data = self.current_input
+            self.publisher_.publish(msg)
+            time.sleep(0.2)  # 5Hz
+
+    def set_input(self, new_input):
+        self.current_input = new_input
+
+    def clear_input(self):
+        self.current_input = ""
+
+    def stop(self):
+        self.running = False
+        self.publish_thread.join()
+
+def main():
+    rclpy.init()
+    node = InputPublisher()
+
+    try:
+        while True:
+            print("\n=== Input Publisher ===")
+            print(f"Current input: '{node.current_input}'")
+            print("1. Set new input")
+            print("2. Clear input")
+            print("3. Exit")
+            choice = input("Choose option: ").strip()
+            if choice == '1':
+                new_input = input("Enter new input: ")
+                node.set_input(new_input)
+            elif choice == '2':
+                node.clear_input()
+                print("Input cleared.")
+            elif choice == '3':
+                break
+            else:
+                print("Invalid choice.")
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.stop()
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml
index 1a7fc8ed..557efa3c 100644
--- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    takeoff_height: 8.0
+    takeoff_height: 5.0
     high_takeoff_height: 8.0
     takeoff_velocity: 1.0
     landing_velocity: 0.3
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py b/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
index 2b83b76c..34073e52 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
@@ -214,7 +214,7 @@ def __init__(self):
             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
+            lidar_min_range = 1.5, # Minimum detection range (m) to avoid propeller hits
         )
 
         ####################################################################################################
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
index 30f7e508..a218956b 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
@@ -193,7 +193,7 @@ def __init__(self):
             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],
+            init_orient=[0.0, 0.0, 1.0, 0.0],
         )
 
         # Add a ZED stereo camera (with an associated subgraph) to the drone
@@ -214,7 +214,7 @@ def __init__(self):
             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 = 1.0, # Minimum detection range (m) to avoid propeller hits
+            lidar_min_range = 1.5, # Minimum detection range (m) to avoid propeller hits
         )
 
         ####################################################################################################
@@ -228,7 +228,7 @@ def __init__(self):
             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],
+            init_orient=[0.0, 0.0, 1.0, 0.0],
         )
 
         # Add a ZED stereo camera (with an associated subgraph) to the drone
@@ -249,7 +249,7 @@ def __init__(self):
             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 = 1.0 
+            lidar_min_range = 0.75 
         )
 
         # Reset so physics/articulations are ready

From 5d32861dfc5e7856dff8e2b88f2e40c66db85cd0 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Wed, 18 Feb 2026 22:45:58 -0500
Subject: [PATCH 04/55] added sharing of semantic rays

---
 .../desktop_bringup/CMakeLists.txt            |   2 +
 .../ros_packages/desktop_bringup/package.xml  |   2 +
 .../desktop_bringup/params/domain_bridge.yaml |  14 ++-
 .../robot_1_semantic_ray_transposer.py        |  90 +++++++++++++
 .../robot_2_filtered_ray_transposer.py        |  91 ++++++++++++++
 .../desktop_bringup/rviz/robot_1.rviz         | 118 ++++++++++++++----
 .../desktop_bringup/rviz/robot_2.rviz         |  65 +++++++---
 7 files changed, 338 insertions(+), 44 deletions(-)
 create mode 100644 common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
 create mode 100644 common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py

diff --git a/common/ros_packages/desktop_bringup/CMakeLists.txt b/common/ros_packages/desktop_bringup/CMakeLists.txt
index 4af7f60f..6fc2bfb3 100644
--- a/common/ros_packages/desktop_bringup/CMakeLists.txt
+++ b/common/ros_packages/desktop_bringup/CMakeLists.txt
@@ -21,6 +21,8 @@ install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
 install(PROGRAMS
   robot_2_relative_position.py
+  robot_1_semantic_ray_transposer.py
+  robot_2_filtered_ray_transposer.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/common/ros_packages/desktop_bringup/package.xml b/common/ros_packages/desktop_bringup/package.xml
index c5555107..8476f6ae 100644
--- a/common/ros_packages/desktop_bringup/package.xml
+++ b/common/ros_packages/desktop_bringup/package.xml
@@ -13,6 +13,8 @@
   <depend>tf2_ros</depend>
   <depend>xacro</depend>
   <depend>urdf</depend>
+  <depend>rclpy</depend>
+  <depend>visualization_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
index 6f6ce4c2..417c6488 100644
--- a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
+++ b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
@@ -39,12 +39,22 @@ topics:
   /robot_1/filtered_rays:
     from_domain: 1
     to_domain: 2
-    type: visualization_msgs/msg/Marker
+    type: visualization_msgs/msg/MarkerArray
   
   /robot_2/filtered_rays:
     from_domain: 2
     to_domain: 1
-    type: visualization_msgs/msg/Marker
+    type: visualization_msgs/msg/MarkerArray
+  
+  /robot_2/rayfronts/semantic_ray_frontiers:
+    from_domain: 2
+    to_domain: 1
+    type: visualization_msgs/msg/MarkerArray
+  
+  /robot_1/rayfronts/semantic_ray_frontiers:
+    from_domain: 1
+    to_domain: 2
+    type: visualization_msgs/msg/MarkerArray
 
   /robot_2/vdb_mapping/vdb_map_visualization:
     from_domain: 2
diff --git a/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py b/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
new file mode 100644
index 00000000..5da3a1d1
--- /dev/null
+++ b/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+import copy
+
+import rclpy
+from rclpy.node import Node
+from visualization_msgs.msg import MarkerArray
+
+
+class SemanticRayTransposer(Node):
+    def __init__(self):
+        super().__init__('robot_1_semantic_ray_transposer')
+
+        # Taken from simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
+        robot_1_init = (20.0, -7.0, 0.15)
+        robot_2_init = (17.0, 1.5, 0.15)
+
+        dx = robot_2_init[0] - robot_1_init[0]
+        dy = robot_2_init[1] - robot_1_init[1]
+        dz = robot_2_init[2] - robot_1_init[2]
+
+        self.declare_parameter('translation_x', dx)
+        self.declare_parameter('translation_y', dy)
+        self.declare_parameter('translation_z', dz)
+        self.declare_parameter('color_r', 0.0)
+        self.declare_parameter('color_g', 0.6)
+        self.declare_parameter('color_b', 1.0)
+        self.declare_parameter('color_a', 1.0)
+
+        self.translation_x = float(self.get_parameter('translation_x').value)
+        self.translation_y = float(self.get_parameter('translation_y').value)
+        self.translation_z = float(self.get_parameter('translation_z').value)
+        self.color_r = float(self.get_parameter('color_r').value)
+        self.color_g = float(self.get_parameter('color_g').value)
+        self.color_b = float(self.get_parameter('color_b').value)
+        self.color_a = float(self.get_parameter('color_a').value)
+
+        self.subscription = self.create_subscription(
+            MarkerArray,
+            '/robot_1/filtered_rays',
+            self.callback,
+            10,
+        )
+        self.publisher = self.create_publisher(
+            MarkerArray,
+            '/robot_1/filtered_rays/transposed',
+            10,
+        )
+
+        self.get_logger().info(
+            f'Publishing transposed rays with translation '
+            f'({self.translation_x:.3f}, {self.translation_y:.3f}, {self.translation_z:.3f})'
+        )
+
+    def callback(self, msg: MarkerArray):
+        out = copy.deepcopy(msg)
+
+        for marker in out.markers:
+            marker.pose.position.x -= self.translation_x/2
+            marker.pose.position.y -= self.translation_y/2
+            marker.pose.position.z -= self.translation_z/2
+            marker.color.r = self.color_r
+            marker.color.g = self.color_g
+            marker.color.b = self.color_b
+            marker.color.a = self.color_a
+
+            for point in marker.points:
+                point.x -= self.translation_x/2
+                point.y -= self.translation_y/2
+                point.z -= self.translation_z/2
+
+            # If marker uses per-point colors, force those to bright blue too.
+            for color in marker.colors:
+                color.r = self.color_r
+                color.g = self.color_g
+                color.b = self.color_b
+                color.a = self.color_a
+
+        self.publisher.publish(out)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SemanticRayTransposer()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py b/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
new file mode 100644
index 00000000..918982d0
--- /dev/null
+++ b/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+import copy
+
+import rclpy
+from rclpy.node import Node
+from visualization_msgs.msg import MarkerArray
+
+
+class Robot2FilteredRayTransposer(Node):
+    def __init__(self):
+        super().__init__('robot_2_filtered_ray_transposer')
+
+        # Taken from simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
+        robot_1_init = (20.0, -7.0, 0.15)
+        robot_2_init = (17.0, 1.5, 0.15)
+
+        dx = robot_2_init[0] - robot_1_init[0]
+        dy = robot_2_init[1] - robot_1_init[1]
+        dz = robot_2_init[2] - robot_1_init[2]
+
+        self.declare_parameter('translation_x', dx)
+        self.declare_parameter('translation_y', dy)
+        self.declare_parameter('translation_z', dz)
+        self.declare_parameter('color_r', 0.0)
+        self.declare_parameter('color_g', 0.6)
+        self.declare_parameter('color_b', 1.0)
+        self.declare_parameter('color_a', 1.0)
+
+        self.translation_x = float(self.get_parameter('translation_x').value)
+        self.translation_y = float(self.get_parameter('translation_y').value)
+        self.translation_z = float(self.get_parameter('translation_z').value)
+        self.color_r = float(self.get_parameter('color_r').value)
+        self.color_g = float(self.get_parameter('color_g').value)
+        self.color_b = float(self.get_parameter('color_b').value)
+        self.color_a = float(self.get_parameter('color_a').value)
+
+        self.subscription = self.create_subscription(
+            MarkerArray,
+            '/robot_2/filtered_rays',
+            self.callback,
+            10,
+        )
+        self.publisher = self.create_publisher(
+            MarkerArray,
+            '/robot_2/filtered_rays/transposed',
+            10,
+        )
+
+        self.get_logger().info(
+            f'Publishing robot_2 transposed rays with translation '
+            f'({self.translation_x:.3f}, {self.translation_y:.3f}, {self.translation_z:.3f})'
+        )
+
+    def callback(self, msg: MarkerArray):
+        out = copy.deepcopy(msg)
+
+        for marker in out.markers:
+            # Opposite direction from robot_1_semantic_ray_transposer.py:
+            # apply +translation instead of -translation.
+            marker.pose.position.x += self.translation_x/2
+            marker.pose.position.y += self.translation_y/2
+            marker.pose.position.z += self.translation_z/2
+            marker.color.r = self.color_r
+            marker.color.g = self.color_g
+            marker.color.b = self.color_b
+            marker.color.a = self.color_a
+
+            for point in marker.points:
+                point.x += self.translation_x/2
+                point.y += self.translation_y/2
+                point.z += self.translation_z/2
+
+            for color in marker.colors:
+                color.r = self.color_r
+                color.g = self.color_g
+                color.b = self.color_b
+                color.a = self.color_a
+
+        self.publisher.publish(out)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = Robot2FilteredRayTransposer()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
index 3a224bad..561af051 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
@@ -23,8 +23,12 @@ Panels:
         - /MarkerArray1/Topic1
         - /MarkerArray2
         - /MarkerArray2/Topic1
+        - /Odometry1
+        - /MarkerArray3
+        - /MarkerArray4
+        - /MarkerArray4/Topic1
       Splitter Ratio: 0.590062141418457
-    Tree Height: 224
+    Tree Height: 220
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -42,7 +46,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: PointCloud2
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
     topic: /robot_1/behavior/behavior_tree_graphviz
@@ -250,7 +254,7 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Image
-          Enabled: true
+          Enabled: false
           Max Value: 1
           Median window: 5
           Min Value: 0
@@ -262,7 +266,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /robot_1/sensors/front_stereo/right/image_rect
-          Value: true
+          Value: false
         - Class: rviz_default_plugins/Image
           Enabled: false
           Max Value: 100
@@ -499,7 +503,8 @@ 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
@@ -684,7 +689,8 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                {}
+                global_plan: true
+                global_plan_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -713,6 +719,7 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
+                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -869,7 +876,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -890,7 +897,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/voxel_rgb
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -903,7 +910,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -924,9 +931,9 @@ Visualization Manager:
         Value: /robot_1/rayfronts/frontiers
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
+      Enabled: false
       Name: MarkerArray
       Namespaces:
         {}
@@ -936,12 +943,12 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /robot_1/rayfronts/semantic_ray_frontiers
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/MarkerArray
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        {}
+        arrows: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -949,6 +956,69 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /robot_1/filtered_rays
       Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 100
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Axes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/relative_position
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: false
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/rayfronts/semantic_ray_frontiers
+      Value: false
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        arrows: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_2/filtered_rays/transposed
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -995,25 +1065,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 75.61663818359375
+      Distance: 49.394229888916016
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 7.733954906463623
-        Y: 3.460301399230957
-        Z: -13.681711196899414
+        X: -19.787370681762695
+        Y: 5.465390682220459
+        Z: -1.4304490089416504
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0847973823547363
+      Pitch: 1.2752116918563843
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 5.58674430847168
+      Yaw: 6.239780426025391
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
@@ -1024,14 +1094,14 @@ Window Geometry:
     collapsed: false
   Front Right RGB:
     collapsed: false
-  Height: 1376
+  Height: 1037
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1040,6 +1110,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2273
-  X: 1072
-  Y: 27
+  Width: 1930
+  X: 70
+  Y: 169
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
index 73af31cb..74ff8a44 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
@@ -23,8 +23,12 @@ Panels:
         - /MarkerArray1/Topic1
         - /MarkerArray2
         - /MarkerArray2/Topic1
+        - /MarkerArray3
+        - /MarkerArray3/Topic1
+        - /MarkerArray3/Namespaces1
+        - /MarkerArray4
       Splitter Ratio: 0.590062141418457
-    Tree Height: 224
+    Tree Height: 305
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -42,7 +46,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: PointCloud2
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
     topic: /robot_2/behavior/behavior_tree_graphviz
@@ -250,7 +254,7 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Image
-          Enabled: true
+          Enabled: false
           Max Value: 1
           Median window: 5
           Min Value: 0
@@ -262,7 +266,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /robot_2/sensors/front_stereo/right/image_rect
-          Value: true
+          Value: false
         - Class: rviz_default_plugins/Image
           Enabled: false
           Max Value: 100
@@ -713,6 +717,7 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
+                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -926,7 +931,7 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
+      Enabled: false
       Name: MarkerArray
       Namespaces:
         {}
@@ -936,9 +941,9 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /robot_2/rayfronts/semantic_ray_frontiers
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
+      Enabled: false
       Name: MarkerArray
       Namespaces:
         {}
@@ -948,6 +953,30 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /robot_2/filtered_rays
+      Value: false
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        arrows: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/filtered_rays/transposed
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: MarkerArray
+      Namespaces:
+        arrows: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/filtered_rays
       Value: true
   Enabled: true
   Global Options:
@@ -995,25 +1024,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 75.61663818359375
+      Distance: 58.59126281738281
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 7.733954906463623
-        Y: 3.460301399230957
-        Z: -13.681711196899414
+        X: -8.417328834533691
+        Y: 3.635751724243164
+        Z: -0.462860107421875
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0847973823547363
+      Pitch: 1.3097960948944092
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 5.58674430847168
+      Yaw: 0.09855291247367859
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
@@ -1024,14 +1053,14 @@ Window Geometry:
     collapsed: false
   Front Right RGB:
     collapsed: false
-  Height: 1376
+  Height: 1122
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1040,6 +1069,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2273
-  X: 1072
-  Y: 27
+  Width: 1931
+  X: 1509
+  Y: 96

From e68e2695426b86ce5951c0e7dcd07d4fb375917c Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Wed, 18 Feb 2026 23:27:39 -0500
Subject: [PATCH 05/55] changed rviz for both drones

---
 .../desktop_bringup/rviz/robot_1.rviz         | 101 +++++++++---------
 .../desktop_bringup/rviz/robot_2.rviz         |  38 +++----
 2 files changed, 70 insertions(+), 69 deletions(-)

diff --git a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
index 561af051..603036ae 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
@@ -131,43 +131,44 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        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:
-                    {}
-                rotor0:
-                  {}
-                rotor1:
+        map:
+          base_link:
+            base_link_body_body_link:
+              OS1_REV6_128_10hz___512_resolution:
+                lidar:
                   {}
-                rotor2:
+              base_link_ZED_X:
+                camera_left:
+                  macvo_ned:
+                    {}
+                camera_right:
                   {}
-                rotor3:
+                imu:
                   {}
-              base_link_frd:
+              rotor0:
                 {}
-            base_link_stabilized:
-              {}
-            look_ahead_point:
-              {}
-            look_ahead_point_stabilized:
-              {}
-            map_ned:
-              {}
-            tracking_point:
-              {}
-            tracking_point_stabilized:
+              rotor1:
+                {}
+              rotor2:
+                {}
+              rotor3:
+                {}
+            base_link_frd:
               {}
+          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
@@ -503,8 +504,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
@@ -689,8 +689,7 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                global_plan: true
-                global_plan_extra_text: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -876,7 +875,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -897,7 +896,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/voxel_rgb
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -910,7 +909,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -931,7 +930,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/frontiers
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Class: rviz_default_plugins/MarkerArray
       Enabled: false
       Name: MarkerArray
@@ -948,7 +947,7 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        arrows: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -1072,36 +1071,36 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -19.787370681762695
-        Y: 5.465390682220459
-        Z: -1.4304490089416504
+        X: -16.599695205688477
+        Y: 7.076057434082031
+        Z: 1.1929526329040527
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.2752116918563843
+      Pitch: 0.41521188616752625
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 6.239780426025391
+      Yaw: 6.209778785705566
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
     collapsed: false
   Displays:
-    collapsed: false
+    collapsed: true
   Front Right Depth:
     collapsed: false
   Front Right RGB:
     collapsed: false
   Height: 1037
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: false
   Image:
     collapsed: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1109,7 +1108,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
+    collapsed: true
   Width: 1930
   X: 70
-  Y: 169
+  Y: 58
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
index 74ff8a44..68dfffaa 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
@@ -503,7 +503,8 @@ 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
@@ -688,7 +689,8 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                {}
+                global_plan: true
+                global_plan_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -943,22 +945,22 @@ Visualization Manager:
         Value: /robot_2/rayfronts/semantic_ray_frontiers
       Value: false
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: false
+      Enabled: true
       Name: MarkerArray
       Namespaces:
-        {}
+        arrows: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /robot_2/filtered_rays
-      Value: false
+      Value: true
     - Class: rviz_default_plugins/MarkerArray
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        arrows: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -970,7 +972,7 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        arrows: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -1024,43 +1026,43 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 58.59126281738281
+      Distance: 35.13690948486328
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -8.417328834533691
-        Y: 3.635751724243164
-        Z: -0.462860107421875
+        X: -9.02755069732666
+        Y: 1.401604175567627
+        Z: 0.6234289407730103
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.3097960948944092
+      Pitch: 0.4097968637943268
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 0.09855291247367859
+      Yaw: 6.221737861633301
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
     collapsed: false
   Displays:
-    collapsed: false
+    collapsed: true
   Front Right Depth:
     collapsed: false
   Front Right RGB:
     collapsed: false
   Height: 1122
-  Hide Left Dock: false
+  Hide Left Dock: true
   Hide Right Dock: false
   Image:
     collapsed: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1068,7 +1070,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
+    collapsed: true
   Width: 1931
   X: 1509
-  Y: 96
+  Y: 65

From f5a5c9ef3498bcc090a163f72a5e3433a388cfdc Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Mon, 23 Feb 2026 22:56:44 -0500
Subject: [PATCH 06/55] added target sharing

---
 .../desktop_bringup/params/domain_bridge.yaml | 11 +++
 .../robot_1_semantic_ray_transposer.py        | 24 +++--
 .../robot_2_filtered_ray_transposer.py        | 23 +++--
 .../desktop_bringup/rviz/robot_1.rviz         | 96 +++++++++----------
 .../desktop_bringup/rviz/robot_2.rviz         | 33 +++----
 .../launch_scripts/two_drone_fire_academy.py  | 49 +++++++---
 6 files changed, 138 insertions(+), 98 deletions(-)

diff --git a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
index 417c6488..491728e2 100644
--- a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
+++ b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
@@ -21,6 +21,17 @@ topics:
   
 
   #rayfronts topics to bridge
+
+  /robot_1/current_target:
+    from_domain: 1
+    to_domain: 2
+    type: std_msgs/msg/String
+  
+  /robot_2/current_target:
+    from_domain: 2
+    to_domain: 1
+    type: std_msgs/msg/String
+
   /robot_1/odometry_conversion/odometry:
     from_domain: 1
     to_domain: 2
diff --git a/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py b/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
index 5da3a1d1..3c4e1b73 100644
--- a/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
+++ b/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
@@ -11,8 +11,16 @@ def __init__(self):
         super().__init__('robot_1_semantic_ray_transposer')
 
         # Taken from simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
-        robot_1_init = (20.0, -7.0, 0.15)
-        robot_2_init = (17.0, 1.5, 0.15)
+        #old fire academy init:
+        # robot_1_init = (20.0, -7.0, 0.15)
+        # robot_2_init = (17.0, 1.5, 0.15)
+        #RetroNeighborhood init:
+        # robot_1_init = (35.0, -19.0, 0.15)
+        # robot_2_init = (30.0, -19.0, 0.15)
+
+        robot_1_init = (25.0, 7.6, 0.15)
+        robot_2_init = (23.4, 9.8, 0.15)
+
 
         dx = robot_2_init[0] - robot_1_init[0]
         dy = robot_2_init[1] - robot_1_init[1]
@@ -55,18 +63,18 @@ def callback(self, msg: MarkerArray):
         out = copy.deepcopy(msg)
 
         for marker in out.markers:
-            marker.pose.position.x -= self.translation_x/2
-            marker.pose.position.y -= self.translation_y/2
-            marker.pose.position.z -= self.translation_z/2
+            marker.pose.position.x -= self.translation_x
+            marker.pose.position.y -= self.translation_y
+            marker.pose.position.z -= self.translation_z
             marker.color.r = self.color_r
             marker.color.g = self.color_g
             marker.color.b = self.color_b
             marker.color.a = self.color_a
 
             for point in marker.points:
-                point.x -= self.translation_x/2
-                point.y -= self.translation_y/2
-                point.z -= self.translation_z/2
+                point.x -= self.translation_x
+                point.y -= self.translation_y
+                point.z -= self.translation_z
 
             # If marker uses per-point colors, force those to bright blue too.
             for color in marker.colors:
diff --git a/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py b/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
index 918982d0..3544346e 100644
--- a/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
+++ b/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
@@ -11,8 +11,15 @@ def __init__(self):
         super().__init__('robot_2_filtered_ray_transposer')
 
         # Taken from simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
-        robot_1_init = (20.0, -7.0, 0.15)
-        robot_2_init = (17.0, 1.5, 0.15)
+        #old fire academy init:
+        # robot_1_init = (20.0, -7.0, 0.15)
+        # robot_2_init = (17.0, 1.5, 0.15)
+        #RetroNeighborhood init:
+        # robot_1_init = (35.0, -19.0, 0.15)
+        # robot_2_init = (30.0, -19.0, 0.15)
+
+        robot_1_init = (25.0, 7.6, 0.15)
+        robot_2_init = (23.4, 9.8, 0.15)
 
         dx = robot_2_init[0] - robot_1_init[0]
         dy = robot_2_init[1] - robot_1_init[1]
@@ -57,18 +64,18 @@ def callback(self, msg: MarkerArray):
         for marker in out.markers:
             # Opposite direction from robot_1_semantic_ray_transposer.py:
             # apply +translation instead of -translation.
-            marker.pose.position.x += self.translation_x/2
-            marker.pose.position.y += self.translation_y/2
-            marker.pose.position.z += self.translation_z/2
+            marker.pose.position.x += self.translation_x
+            marker.pose.position.y += self.translation_y
+            marker.pose.position.z += self.translation_z
             marker.color.r = self.color_r
             marker.color.g = self.color_g
             marker.color.b = self.color_b
             marker.color.a = self.color_a
 
             for point in marker.points:
-                point.x += self.translation_x/2
-                point.y += self.translation_y/2
-                point.z += self.translation_z/2
+                point.x += self.translation_x
+                point.y += self.translation_y
+                point.z += self.translation_z
 
             for color in marker.colors:
                 color.r = self.color_r
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
index 603036ae..955e9351 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
@@ -28,7 +28,7 @@ Panels:
         - /MarkerArray4
         - /MarkerArray4/Topic1
       Splitter Ratio: 0.590062141418457
-    Tree Height: 220
+    Tree Height: 373
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -46,7 +46,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: ""
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
     topic: /robot_1/behavior/behavior_tree_graphviz
@@ -131,44 +131,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:
                     {}
-                camera_right:
+                  imu:
+                    {}
+                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
@@ -255,7 +254,7 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Image
-          Enabled: false
+          Enabled: true
           Max Value: 1
           Median window: 5
           Min Value: 0
@@ -267,7 +266,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /robot_1/sensors/front_stereo/right/image_rect
-          Value: false
+          Value: true
         - Class: rviz_default_plugins/Image
           Enabled: false
           Max Value: 100
@@ -718,7 +717,6 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
-                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -1010,7 +1008,7 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        arrows: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -1064,25 +1062,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 49.394229888916016
+      Distance: 30.03053092956543
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -16.599695205688477
-        Y: 7.076057434082031
-        Z: 1.1929526329040527
+        X: -5.642820358276367
+        Y: -0.14288032054901123
+        Z: -2.2876358032226562
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.41521188616752625
+      Pitch: 0.5275139212608337
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 6.209778785705566
+      Yaw: 6.182468891143799
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
@@ -1093,14 +1091,14 @@ Window Geometry:
     collapsed: false
   Front Right RGB:
     collapsed: false
-  Height: 1037
+  Height: 911
   Hide Left Dock: true
   Hide Right Dock: false
   Image:
     collapsed: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1110,5 +1108,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1930
-  X: 70
-  Y: 58
+  X: 1510
+  Y: 631
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
index 68dfffaa..1b953c8e 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
@@ -28,7 +28,7 @@ Panels:
         - /MarkerArray3/Namespaces1
         - /MarkerArray4
       Splitter Ratio: 0.590062141418457
-    Tree Height: 305
+    Tree Height: 373
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -46,7 +46,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: ""
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
     topic: /robot_2/behavior/behavior_tree_graphviz
@@ -254,7 +254,7 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Image
-          Enabled: false
+          Enabled: true
           Max Value: 1
           Median window: 5
           Min Value: 0
@@ -266,7 +266,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /robot_2/sensors/front_stereo/right/image_rect
-          Value: false
+          Value: true
         - Class: rviz_default_plugins/Image
           Enabled: false
           Max Value: 100
@@ -503,8 +503,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
@@ -689,8 +688,7 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                global_plan: true
-                global_plan_extra_text: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -719,7 +717,6 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
-                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -948,7 +945,7 @@ Visualization Manager:
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        arrows: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -1026,25 +1023,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 35.13690948486328
+      Distance: 26.823678970336914
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -9.02755069732666
-        Y: 1.401604175567627
-        Z: 0.6234289407730103
+        X: -8.390859603881836
+        Y: 3.887568235397339
+        Z: 0.3204301595687866
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4097968637943268
+      Pitch: 0.23979705572128296
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 6.221737861633301
+      Yaw: 6.241737365722656
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
@@ -1062,7 +1059,7 @@ Window Geometry:
     collapsed: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000030400000258fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a005600690065007700730000000078000000a0000000a000fffffffb000000100044006900730070006c006100790073000000011e000001b2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c90000002800fffffffb00000028004d004100430056004f00200049006d00610067006500200046006500610074007500720065007300000002ba000000ca0000000000000000fb0000002200460072006f006e00740020005200690067006800740020004400650070007400680000000259000001290000002800fffffffb0000000a0049006d006100670065000000029c000001320000002800fffffffb0000000a0049006d0061006700650000000217000001b70000002800fffffffb0000000a0049006d00610067006500000003610000006d000000000000000000000001000001f600000356fc0200000008fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001e00460072006f006e007400200052006900670068007400200052004700420000000078000003560000002800fffffffc000001ac000002220000000000fffffffa000000010100000002fb0000002200460072006f006e00740020005200690067006800740020004400650070007400680000000000ffffffff0000009e00fffffffb0000002200460072006f006e007400200052006900670068007400200044006500700074006801000007c4000001f60000000000000000fb0000002200460072006f006e0074002000520069006700680074002000440065007000740068010000014a000001cc0000000000000000fb0000001e004d004100430056004f002000440069007300700061007200690074007900000002ca000001040000002800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000078b00000037fc0100000002fb0000000800540069006d006501000000000000078b0000025300fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078b00000171fc0100000002fb00000022004200650068006100760069006f0072005400720065006500500061006e0065006c01000000000000078b000001fa00fffffffb0000000800540069006d006501000000000000045000000000000000000000078b0000025800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -1073,4 +1070,4 @@ Window Geometry:
     collapsed: true
   Width: 1931
   X: 1509
-  Y: 65
+  Y: 27
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
index a218956b..d800d480 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
@@ -28,11 +28,11 @@
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
 
 #env/stage path and scale
-ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.usd"
+ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/FireAcademyFaro/fire_academy_fixed_textures.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 = 2.0
+STAGE_SCALE = 0.01
 
 
 DRONE_USD = "/root/Documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
@@ -46,8 +46,12 @@
 #Drone offset
 SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
 
-DRONE1_XY_M = (20.0, -7)
-DRONE2_XY_M = (17, 1.5)
+# spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd
+# DRONE1_XY_M = (20.0, -7.0)
+# DRONE2_XY_M = (17, 1.5)
+
+DRONE1_XY_M = (25.0, 7.6)
+DRONE2_XY_M = (23.4, 9.8)
 # ---------------------------------------------------------
 
 
@@ -148,13 +152,28 @@ def __init__(self):
         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))
+
+            translate_attr = stage_prim.GetAttribute("xformOp:translate")
+            if translate_attr.IsValid():
+                if str(translate_attr.GetTypeName()) == "float3":
+                    translate_attr.Set(Gf.Vec3f(0.0, 0.0, 0.0))
+                else:
+                    translate_attr.Set(Gf.Vec3d(0.0, 0.0, 0.0))
+            else:
+                # If missing, create a new translate op.
+                translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
+                translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
+
+            scale_attr = stage_prim.GetAttribute("xformOp:scale")
+            if scale_attr.IsValid():
+                if str(scale_attr.GetTypeName()) == "double3":
+                    scale_attr.Set(Gf.Vec3d(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
+                else:
+                    scale_attr.Set(Gf.Vec3f(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
+            else:
+                # If missing, create a new scale op.
+                scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionFloat)
+                scale_op.Set(Gf.Vec3f(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
             
             add_collision_to_prim(stage_prim)
             print("Finished adding collisions.")
@@ -193,7 +212,7 @@ def __init__(self):
             domain_id=1,  # defines ROS2 domain ID
             usd_file=DRONE_USD,
             init_pos=drone1_pos,
-            init_orient=[0.0, 0.0, 1.0, 0.0],
+            init_orient=[0.0, 0.0, -0.937, 0.35], #[0,0,1,0]
         )
 
         # Add a ZED stereo camera (with an associated subgraph) to the drone
@@ -228,7 +247,7 @@ def __init__(self):
             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, 1.0, 0.0],
+            init_orient=[0.0, 0.0, -0.937, 0.35], #[0,0,1,0]
         )
 
         # Add a ZED stereo camera (with an associated subgraph) to the drone
@@ -249,7 +268,7 @@ def __init__(self):
             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 
+            lidar_min_range = 1.5 
         )
 
         # Reset so physics/articulations are ready
@@ -281,4 +300,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    main()

From efb2e55db496d0a2b1d2d39bb720c5488759e05e Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Mon, 23 Feb 2026 23:34:39 -0500
Subject: [PATCH 07/55] changed drone start pos

---
 .../isaac-sim/launch_scripts/two_drone_fire_academy.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
index d800d480..33c1082a 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
@@ -44,14 +44,14 @@
 DOME_LIGHT_EXPOSURE = -3.0
 
 #Drone offset
-SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
+SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.07
 
 # spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd
 # DRONE1_XY_M = (20.0, -7.0)
 # DRONE2_XY_M = (17, 1.5)
 
-DRONE1_XY_M = (25.0, 7.6)
-DRONE2_XY_M = (23.4, 9.8)
+DRONE1_XY_M = (27.0, 7.6)
+DRONE2_XY_M = (23, 9.8)
 # ---------------------------------------------------------
 
 
@@ -233,7 +233,7 @@ def __init__(self):
             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 = 1.5, # Minimum detection range (m) to avoid propeller hits
+            lidar_min_range = 4.0, # Minimum detection range (m) to avoid propeller hits
         )
 
         ####################################################################################################
@@ -268,7 +268,7 @@ def __init__(self):
             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 = 1.5 
+            lidar_min_range = 3.5 
         )
 
         # Reset so physics/articulations are ready

From 435837c47e401e17f38ef7902336dd4a935add41 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sun, 15 Mar 2026 23:05:11 -0400
Subject: [PATCH 08/55] gossip layer w/o relay

---
 .env                                          |   6 +-
 .../launch/robot_state_publisher.launch.py    |   6 +-
 .../autonomy_bringup/launch/robot.launch.xml  |  18 +-
 .../config/gossip_dds_router.yaml             |  18 ++
 .../coordination_bringup/__init__.py          |   0
 .../coordination_bringup/gossip_node.py       | 177 ++++++++++
 .../coordination_bringup/peer_profile.py      | 184 +++++++++++
 .../launch/gossip.launch.xml                  |  21 ++
 .../coordination_bringup/package.xml          |  23 ++
 .../resource/coordination_bringup             |   0
 .../coordination_bringup/scripts/gossip_node  |   3 +
 .../scripts/peer_registry_monitor.py          | 152 +++++++++
 .../coordination_bringup/setup.py             |  29 ++
 .../coordination_msgs/CMakeLists.txt          |  15 +
 .../coordination_msgs/msg/PeerProfile.msg     |  22 ++
 .../msg/PeerProfilePayload.msg                |   6 +
 .../coordination_msgs/package.xml             |  22 ++
 .../three_drone_scene_import.py               | 304 ++++++++++++++++++
 18 files changed, 998 insertions(+), 8 deletions(-)
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/config/gossip_dds_router.yaml
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/__init__.py
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/package.xml
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/resource/coordination_bringup
 create mode 100755 robot/ros_ws/src/coordination/coordination_bringup/scripts/gossip_node
 create mode 100755 robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/setup.py
 create mode 100644 robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
 create mode 100644 robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
 create mode 100644 robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
 create mode 100644 robot/ros_ws/src/coordination/coordination_msgs/package.xml
 create mode 100644 simulation/isaac-sim/launch_scripts/three_drone_scene_import.py

diff --git a/.env b/.env
index 07eb9c74..3ca9b121 100644
--- a/.env
+++ b/.env
@@ -26,15 +26,15 @@ COMPOSE_PROFILES="desktop"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="1"  # Number of robot containers to launch.
+NUM_ROBOTS="3"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
 ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_pegasus.scene.usd"
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
-ISAAC_SIM_USE_STANDALONE="false" # "true" or "false"
+ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="example_one_px4_pegasus_launch_script.py"
+ISAAC_SIM_SCRIPT_NAME="three_drone_scene_import.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py b/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py
index 474614cb..1737a603 100644
--- a/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py
+++ b/common/ros_packages/robot_descriptions/launch/robot_state_publisher.launch.py
@@ -49,9 +49,9 @@ def launch_setup(context, *args, **kwargs):
                 relative_path
             ])
         else:
-            # Use relative path within robot_bringup package
+            # Use relative path within autonomy_bringup package
             urdf_file = PathJoinSubstitution([
-                FindPackageShare('robot_bringup'),
+                FindPackageShare('autonomy_bringup'),
                 'urdf',
                 urdf_file_path
             ])
@@ -95,7 +95,7 @@ def generate_launch_description():
     urdf_file_path_arg = DeclareLaunchArgument(
         'urdf_file_path',
         default_value='robot.urdf.xacro',
-        description='Path to the URDF/xacro file. Can be relative to robot_bringup/urdf/ or an absolute path'
+        description='Path to the URDF/xacro file. Can be relative to autonomy_bringup/urdf/ or an absolute path'
     )
     
     publish_frequency_arg = DeclareLaunchArgument(
diff --git a/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml b/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml
index 802a07a1..e3e21061 100644
--- a/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml
+++ b/robot/ros_ws/src/autonomy_bringup/launch/robot.launch.xml
@@ -10,6 +10,8 @@ lite, desktop_split sim)
   <arg name="role" default="$(env AUTONOMY_ROLE full)" />
   <arg name="urdf_file" default="$(env URDF_FILE)" />
   <arg name="gcs_domain" default="0" />
+  <arg name="gossip_domain" default="99" />
+  <arg name="gossip_publish_rate" default="1.0" />
 
   <push_ros_namespace namespace="$(env ROBOT_NAME)" />
 
@@ -35,12 +37,18 @@ lite, desktop_split sim)
     <include
       file="$(find-pkg-share autonomy_bringup)/onboard_all/launch/onboard_autonomy_all.launch.xml" />
 
-    <!-- Domain Bridge -->
+    <!-- Domain Bridge (GCS) -->
     <include file="$(find-pkg-share autonomy_bringup)/launch/interpolate_dds_router.launch.py">
       <arg name="config_file"
         value="$(find-pkg-share autonomy_bringup)/onboard_all/config/dds_router.yaml" />
       <arg name="args" value="gcs_domain:=$(var gcs_domain)" />
     </include>
+
+    <!-- Gossip coordination layer -->
+    <include file="$(find-pkg-share coordination_bringup)/launch/gossip.launch.xml">
+      <arg name="gossip_domain"  value="$(var gossip_domain)" />
+      <arg name="publish_rate"   value="$(var gossip_publish_rate)" />
+    </include>
   </group>
 
   <!-- onboard: lite modules only (local planning, perception, etc.); global is handled offboard -->
@@ -48,13 +56,19 @@ lite, desktop_split sim)
     <include
       file="$(find-pkg-share autonomy_bringup)/onboard_local_offboard_global/launch/onboard_autonomy_local.launch.xml" />
 
-    <!-- Domain Bridge -->
+    <!-- Domain Bridge (GCS) -->
     <include file="$(find-pkg-share autonomy_bringup)/launch/interpolate_dds_router.launch.py">
       <arg name="config_file"
         value="$(find-pkg-share autonomy_bringup)/onboard_local_offboard_global/config/dds_router.yaml" />
       <arg name="args" value="gcs_domain:=$(var gcs_domain)" />
     </include>
 
+    <!-- Gossip coordination layer -->
+    <include file="$(find-pkg-share coordination_bringup)/launch/gossip.launch.xml">
+      <arg name="gossip_domain"  value="$(var gossip_domain)" />
+      <arg name="publish_rate"   value="$(var gossip_publish_rate)" />
+    </include>
+
   </group>
 
   <!-- offboard: global planning only; runs on GCS paired with onboard robots -->
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_dds_router.yaml b/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_dds_router.yaml
new file mode 100644
index 00000000..4da27561
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_dds_router.yaml
@@ -0,0 +1,18 @@
+# Gossip DDS Router configuration
+# Bridges /gossip/peers between the robot's own ROS_DOMAIN_ID and the
+# shared gossip domain so all robots can see each other's PeerProfile
+# broadcasts without any per-robot enumeration.
+#
+# Every robot runs this identical config.  Adding a new robot only
+# requires incrementing NUM_ROBOTS – no changes here needed.
+
+participants:
+  - name: "robot"
+    kind: "local"
+    domain: $(env ROS_DOMAIN_ID)
+  - name: "gossip_bus"
+    kind: "local"
+    domain: $(var gossip_domain)
+
+allowlist:
+  - name: "rt/gossip/peers"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/__init__.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
new file mode 100644
index 00000000..1c8c258d
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -0,0 +1,177 @@
+"""
+gossip_node.py
+==============
+ROS 2 node that implements the gossip-protocol coordination layer.
+
+Each robot:
+  - Subscribes to its own odometry and global plan
+  - Publishes its own PeerProfile to /gossip/peers (shared gossip domain via DDS Router)
+  - Subscribes to /gossip/peers to receive all peer profiles
+  - Maintains a registry of known peers (latest-wins, no expiry)
+  - Republishes the full registry as a latched topic for planners to consume
+
+Publish rate:
+  - Fixed 1 Hz wall-clock timer (publish_rate parameter) — always fires regardless of
+    sim time, so heartbeating works even when the robot is stationary.
+  - A new waypoint also triggers an immediate extra publish.
+"""
+
+import os
+
+import rclpy
+from rclpy.node import Node
+from rclpy.clock import ROSClock
+from rclpy.qos import (
+    QoSDurabilityPolicy,
+    QoSHistoryPolicy,
+    QoSProfile,
+    QoSReliabilityPolicy,
+)
+
+from nav_msgs.msg import Odometry, Path
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+
+from coordination_bringup.peer_profile import PeerProfile
+
+
+# QoS for gossip topic: best-effort, keep last 1 (low-overhead, high-freq)
+GOSSIP_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+# QoS for the registry topic: reliable + transient-local so late subscribers
+# immediately receive the latest snapshot
+REGISTRY_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.RELIABLE,
+    durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
+
+class GossipNode(Node):
+
+    def __init__(self):
+        super().__init__("gossip_node")
+
+        # ── Parameters ──────────────────────────────────────────────────
+        self.declare_parameter("robot_name", os.environ.get("ROBOT_NAME", "unknown_robot"))
+        self.declare_parameter("publish_rate", 1.0)   # Hz — wall-clock timer
+
+        self._robot_name = self.get_parameter("robot_name").get_parameter_value().string_value
+        publish_rate = self.get_parameter("publish_rate").get_parameter_value().double_value
+
+        # ── State ────────────────────────────────────────────────────────
+        self._profile = PeerProfile(robot_name=self._robot_name)
+
+        # Registry: robot_name → latest PeerProfile ROS msg
+        self._registry: dict[str, PeerProfileMsg] = {}
+
+        # ── Subscriptions ────────────────────────────────────────────────
+        self._odom_sub = self.create_subscription(
+            Odometry,
+            f"/{self._robot_name}/odometry_conversion/odometry",
+            self._on_odom,
+            GOSSIP_QOS,
+        )
+        self._path_sub = self.create_subscription(
+            Path,
+            f"/{self._robot_name}/random_walk/global_plan",
+            self._on_global_plan,
+            GOSSIP_QOS,
+        )
+        self._peer_sub = self.create_subscription(
+            PeerProfileMsg,
+            "/gossip/peers",
+            self._on_peer_msg,
+            GOSSIP_QOS,
+        )
+
+        # ── Publishers ───────────────────────────────────────────────────
+        self._gossip_pub = self.create_publisher(
+            PeerProfileMsg,
+            "/gossip/peers",
+            GOSSIP_QOS,
+        )
+        # Registry snapshot – latched so planners that start late still get it
+        self._registry_pub = self.create_publisher(
+            PeerProfileMsg,
+            f"/{self._robot_name}/coordination/peer_registry",
+            REGISTRY_QOS,
+        )
+
+        # ── Publish timer (wall clock — unaffected by sim time) ──────────
+        # Use a wall-clock timer so heartbeating works even when sim is paused
+        # or use_sim_time is set.
+        period = 1.0 / max(publish_rate, 0.01)
+        self._publish_timer = self.create_timer(
+            period,
+            self._publish_tick,
+            clock=ROSClock(),
+        )
+
+        self.get_logger().info(
+            f"GossipNode started for '{self._robot_name}' "
+            f"(publish_rate={publish_rate:.1f} Hz wall-clock)"
+        )
+
+    # ------------------------------------------------------------------ #
+    # Subscription callbacks                                               #
+    # ------------------------------------------------------------------ #
+
+    def _on_odom(self, msg: Odometry) -> None:
+        self._profile.set_pose_from_odom(msg)
+
+    def _on_global_plan(self, msg: Path) -> None:
+        self._profile.set_waypoint_from_path(msg)
+
+    def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
+        if msg.robot_name == self._robot_name:
+            return  # discard own messages echoed back from the gossip domain
+        self._update_registry(msg)
+
+    # ------------------------------------------------------------------ #
+    # Publish logic                                                        #
+    # ------------------------------------------------------------------ #
+
+    def _publish_tick(self) -> None:
+        """Wall-clock timer callback — publish own profile at fixed rate."""
+        self._publish_own()
+
+    def _publish_own(self) -> None:
+        self._gossip_pub.publish(self._profile.to_ros_msg())
+
+    # ------------------------------------------------------------------ #
+    # Registry management                                                  #
+    # ------------------------------------------------------------------ #
+
+    def _update_registry(self, msg: PeerProfileMsg) -> None:
+        """Update registry with latest-wins semantics, then republish snapshot."""
+        existing = self._registry.get(msg.robot_name)
+        if existing is not None:
+            # Only update if the incoming message is newer
+            new_stamp = msg.pose.header.stamp.sec + msg.pose.header.stamp.nanosec * 1e-9
+            old_stamp = existing.pose.header.stamp.sec + existing.pose.header.stamp.nanosec * 1e-9
+            if new_stamp <= old_stamp:
+                return
+
+        self._registry[msg.robot_name] = msg
+        self._registry_pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = GossipNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
new file mode 100644
index 00000000..a7c4bbe3
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
@@ -0,0 +1,184 @@
+"""
+peer_profile.py
+===============
+Base class for gossip-protocol peer state.  Subclass this to attach
+task-specific data as typed ROS payloads.
+
+Usage
+-----
+# Basic – just use the base class
+profile = PeerProfile("robot_1")
+profile.set_pose_from_odom(odom_msg)
+profile.set_waypoint_from_path(path_msg)
+ros_msg = profile.to_ros_msg()
+
+# Extended – subclass and add payloads
+class SearchProfile(PeerProfile):
+    def attach_frontier(self, frontier_msg):
+        self.add_payload(frontier_msg)
+
+profile = SearchProfile("robot_1")
+profile.attach_frontier(my_frontier)
+ros_msg = profile.to_ros_msg()
+
+# Receiving side
+profile = PeerProfile.from_ros_msg(ros_msg)
+grid = profile.get_payload("nav_msgs/msg/OccupancyGrid")
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+from enum import IntEnum
+from typing import Any, Dict, List, Optional
+
+from rclpy.serialization import deserialize_message, serialize_message
+import rosidl_runtime_py.utilities as rosidl_utils
+
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Odometry, Path
+
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+from coordination_msgs.msg import PeerProfilePayload as PeerProfilePayloadMsg
+
+
+class Source(IntEnum):
+    DIRECT = 0
+    RELAYED = 1
+
+
+@dataclass
+class PeerProfile:
+    """
+    Base peer state broadcast over the gossip bus.
+
+    Subclass and override ``extra_payloads_to_attach()`` to automatically
+    include task-specific ROS messages every time ``to_ros_msg()`` is called,
+    or call ``add_payload()`` / ``clear_payloads()`` manually.
+    """
+
+    robot_name: str
+    pose: PoseStamped = field(default_factory=PoseStamped)
+    waypoint: PoseStamped = field(default_factory=PoseStamped)
+    source: Source = Source.DIRECT
+    relay_hops: int = 0
+
+    # Internal payload store: list of dicts with keys "type" and "data"
+    _payloads: List[Dict[str, Any]] = field(default_factory=list, repr=False)
+
+    # ------------------------------------------------------------------ #
+    # Pose / waypoint helpers                                              #
+    # ------------------------------------------------------------------ #
+
+    def set_pose_from_odom(self, odom: Odometry) -> None:
+        """Extract pose (position + orientation) from an Odometry message."""
+        stamped = PoseStamped()
+        stamped.header = odom.header
+        stamped.pose = odom.pose.pose
+        self.pose = stamped
+
+    def set_waypoint_from_path(self, path: Optional[Path]) -> None:
+        """
+        Extract the goal (last pose) from a nav_msgs/Path.
+        Passing None or an empty path leaves waypoint as all-zeros,
+        which signals 'no plan available' to receivers.
+        """
+        if path is not None and len(path.poses) > 0:
+            self.waypoint = path.poses[-1]
+        else:
+            self.waypoint = PoseStamped()
+
+    def has_waypoint(self) -> bool:
+        """Return True if a valid waypoint has been set (non-zero stamp)."""
+        s = self.waypoint.header.stamp
+        return s.sec != 0 or s.nanosec != 0
+
+    # ------------------------------------------------------------------ #
+    # Payload management                                                   #
+    # ------------------------------------------------------------------ #
+
+    def add_payload(self, msg: Any) -> None:
+        """Serialize and attach any ROS message as an additional payload."""
+        type_str = _ros_type_string(msg)
+        self._payloads.append({
+            "type": type_str,
+            "data": serialize_message(msg),
+        })
+
+    def clear_payloads(self) -> None:
+        self._payloads.clear()
+
+    def get_payload(self, payload_type: str) -> Optional[Any]:
+        """
+        Deserialize and return the first payload matching *payload_type*.
+        *payload_type* is the fully-qualified ROS type string,
+        e.g. ``"nav_msgs/msg/OccupancyGrid"``.
+        Returns None if not found.
+        """
+        for p in self._payloads:
+            if p["type"] == payload_type:
+                msg_class = rosidl_utils.get_message(payload_type)
+                return deserialize_message(p["data"], msg_class)
+        return None
+
+    def get_all_payloads(self) -> List[Any]:
+        """Deserialize and return all payloads in order."""
+        result = []
+        for p in self._payloads:
+            msg_class = rosidl_utils.get_message(p["type"])
+            result.append(deserialize_message(p["data"], msg_class))
+        return result
+
+    def payload_types(self) -> List[str]:
+        """Return the type strings of all attached payloads."""
+        return [p["type"] for p in self._payloads]
+
+    # ------------------------------------------------------------------ #
+    # Serialisation                                                        #
+    # ------------------------------------------------------------------ #
+
+    def to_ros_msg(self) -> PeerProfileMsg:
+        msg = PeerProfileMsg()
+        msg.robot_name = self.robot_name
+        msg.pose = self.pose
+        msg.waypoint = self.waypoint
+        msg.source = int(self.source)
+        msg.relay_hops = self.relay_hops
+        msg.payloads = [
+            PeerProfilePayloadMsg(payload_type=p["type"], payload_data=list(p["data"]))
+            for p in self._payloads
+        ]
+        return msg
+
+    @classmethod
+    def from_ros_msg(cls, msg: PeerProfileMsg) -> "PeerProfile":
+        profile = cls(robot_name=msg.robot_name)
+        profile.pose = msg.pose
+        profile.waypoint = msg.waypoint
+        profile.source = Source(msg.source)
+        profile.relay_hops = msg.relay_hops
+        profile._payloads = [
+            {"type": p.payload_type, "data": bytes(p.payload_data)}
+            for p in msg.payloads
+        ]
+        return profile
+
+
+# ------------------------------------------------------------------ #
+# Internal helpers                                                     #
+# ------------------------------------------------------------------ #
+
+def _ros_type_string(msg: Any) -> str:
+    """Return the fully-qualified ROS type string for a message instance.
+
+    E.g. nav_msgs.msg.OccupancyGrid → "nav_msgs/msg/OccupancyGrid"
+    """
+    module = type(msg).__module__    # e.g. "nav_msgs.msg._occupancy_grid"
+    name = type(msg).__name__        # e.g. "OccupancyGrid"
+    # Convert "nav_msgs.msg._occupancy_grid" → "nav_msgs/msg"
+    parts = module.split(".")
+    if len(parts) >= 2:
+        pkg = parts[0]
+        sub = parts[1]
+        return f"{pkg}/{sub}/{name}"
+    return f"{module}/{name}"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml b/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
new file mode 100644
index 00000000..f405c4be
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
@@ -0,0 +1,21 @@
+<!-- Gossip coordination layer bringup.
+     Starts the gossip DDS Router bridge and the gossip_node.
+     Included by robot.launch.xml for both 'full' and 'onboard' roles.
+-->
+<launch>
+  <arg name="gossip_domain"  default="99" />
+  <arg name="publish_rate"   default="1.0" />
+
+  <!-- Bridge robot domain ↔ gossip domain for /gossip/peers -->
+  <include file="$(find-pkg-share autonomy_bringup)/launch/interpolate_dds_router.launch.py">
+    <arg name="config_file"
+      value="$(find-pkg-share coordination_bringup)/config/gossip_dds_router.yaml" />
+    <arg name="args" value="gossip_domain:=$(var gossip_domain)" />
+  </include>
+
+  <!-- Gossip node -->
+  <node pkg="coordination_bringup" exec="gossip_node" name="gossip_node" output="screen">
+    <param name="robot_name"   value="$(env ROBOT_NAME)" />
+    <param name="publish_rate" value="$(var publish_rate)" />
+  </node>
+</launch>
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/package.xml b/robot/ros_ws/src/coordination/coordination_bringup/package.xml
new file mode 100644
index 00000000..46666053
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>coordination_bringup</name>
+  <version>0.0.0</version>
+  <description>Gossip-protocol multi-agent coordination layer for AirStack</description>
+  <maintainer email="airlab@andrew.cmu.edu">AirLab</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <depend>rclpy</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>coordination_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/resource/coordination_bringup b/robot/ros_ws/src/coordination/coordination_bringup/resource/coordination_bringup
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/gossip_node b/robot/ros_ws/src/coordination/coordination_bringup/scripts/gossip_node
new file mode 100755
index 00000000..8f7d7d46
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/scripts/gossip_node
@@ -0,0 +1,3 @@
+#!/usr/bin/env python3
+from coordination_bringup.gossip_node import main
+main()
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
new file mode 100755
index 00000000..a0fa038e
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+"""
+peer_registry_monitor.py
+========================
+CLI diagnostic tool – subscribe to /gossip/peers on the gossip domain
+and pretty-print the live peer registry.
+
+Run on any robot or from a machine joined to domain 99:
+
+    ROS_DOMAIN_ID=99 python3 peer_registry_monitor.py
+
+Or on a specific robot's domain to see what *that* robot receives:
+
+    ROS_DOMAIN_ID=1 python3 peer_registry_monitor.py
+
+Options
+-------
+  --robot   Only show entries for this robot name (partial match)
+  --rate    Refresh rate in Hz (default: 2)
+"""
+
+import argparse
+import os
+import sys
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
+
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+
+GOSSIP_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+RESET = "\033[0m"
+BOLD = "\033[1m"
+CYAN = "\033[36m"
+YELLOW = "\033[33m"
+GREEN = "\033[32m"
+DIM = "\033[2m"
+
+
+def _fmt_pose(pose_stamped) -> str:
+    p = pose_stamped.pose.position
+    o = pose_stamped.pose.orientation
+    return f"pos=({p.x:7.2f}, {p.y:7.2f}, {p.z:7.2f})  orient=({o.x:.3f}, {o.y:.3f}, {o.z:.3f}, {o.w:.3f})"
+
+
+def _fmt_waypoint(pose_stamped) -> str:
+    s = pose_stamped.header.stamp
+    if s.sec == 0 and s.nanosec == 0:
+        return f"{DIM}(no plan yet){RESET}"
+    p = pose_stamped.pose.position
+    o = pose_stamped.pose.orientation
+    return f"pos=({p.x:7.2f}, {p.y:7.2f}, {p.z:7.2f})  orient=({o.x:.3f}, {o.y:.3f}, {o.z:.3f}, {o.w:.3f})"
+
+
+def _fmt_stamp(pose_stamped) -> str:
+    s = pose_stamped.header.stamp
+    if s.sec == 0 and s.nanosec == 0:
+        return "n/a"
+    t = s.sec + s.nanosec * 1e-9
+    return time.strftime("%H:%M:%S", time.localtime(t)) + f".{s.nanosec // 1_000_000:03d}"
+
+
+def _clear():
+    sys.stdout.write("\033[2J\033[H")
+    sys.stdout.flush()
+
+
+class RegistryMonitor(Node):
+
+    def __init__(self, filter_name: str = ""):
+        super().__init__("peer_registry_monitor")
+        self._registry: dict[str, PeerProfileMsg] = {}
+        self._filter = filter_name.lower()
+
+        self._sub = self.create_subscription(
+            PeerProfileMsg,
+            "/gossip/peers",
+            self._on_msg,
+            GOSSIP_QOS,
+        )
+
+    def _on_msg(self, msg: PeerProfileMsg) -> None:
+        existing = self._registry.get(msg.robot_name)
+        if existing is not None:
+            new_t = msg.pose.header.stamp.sec + msg.pose.header.stamp.nanosec * 1e-9
+            old_t = existing.pose.header.stamp.sec + existing.pose.header.stamp.nanosec * 1e-9
+            if new_t <= old_t:
+                return
+        self._registry[msg.robot_name] = msg
+
+    def print_registry(self) -> None:
+        _clear()
+        domain = os.environ.get("ROS_DOMAIN_ID", "?")
+        now = time.strftime("%H:%M:%S")
+        print(f"{BOLD}Peer Registry  {DIM}[domain={domain}  {now}]{RESET}")
+        print("─" * 80)
+
+        entries = sorted(self._registry.values(), key=lambda m: m.robot_name)
+        if self._filter:
+            entries = [e for e in entries if self._filter in e.robot_name.lower()]
+
+        if not entries:
+            print(f"  {DIM}(no peers seen yet){RESET}")
+        else:
+            for msg in entries:
+                src = "direct" if msg.source == 0 else f"relayed({msg.relay_hops}h)"
+                payload_summary = (
+                    f"{len(msg.payloads)} payload(s): "
+                    + ", ".join(p.payload_type for p in msg.payloads)
+                    if msg.payloads
+                    else "no payloads"
+                )
+                stamp_str = _fmt_stamp(msg.pose)
+                print(f"  {CYAN}{BOLD}{msg.robot_name}{RESET}  {DIM}[{src}  stamp={stamp_str}]{RESET}")
+                print(f"    {GREEN}pose    {RESET} {_fmt_pose(msg.pose)}")
+                print(f"    {YELLOW}waypoint{RESET} {_fmt_waypoint(msg.waypoint)}")
+                print(f"    {DIM}payloads{RESET} {payload_summary}")
+                print()
+
+        print(f"{DIM}Listening on /gossip/peers — Ctrl+C to quit{RESET}")
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Live peer registry monitor")
+    parser.add_argument("--robot", default="", help="Filter by robot name (partial)")
+    parser.add_argument("--rate", type=float, default=2.0, help="Refresh rate Hz (default 2)")
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = RegistryMonitor(filter_name=args.robot)
+    interval = 1.0 / max(args.rate, 0.1)
+
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=interval)
+            node.print_registry()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/setup.py b/robot/ros_ws/src/coordination/coordination_bringup/setup.py
new file mode 100644
index 00000000..aa8c7c93
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/setup.py
@@ -0,0 +1,29 @@
+from setuptools import find_packages, setup
+
+package_name = 'coordination_bringup'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/gossip.launch.xml']),
+        ('share/' + package_name + '/config', ['config/gossip_dds_router.yaml']),
+        ('lib/' + package_name, ['scripts/gossip_node', 'scripts/peer_registry_monitor.py']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='AirLab',
+    maintainer_email='airlab@andrew.cmu.edu',
+    description='Gossip-protocol multi-agent coordination layer for AirStack',
+    license='BSD-3-Clause',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'gossip_node = coordination_bringup.gossip_node:main',
+        ],
+    },
+)
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt b/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
new file mode 100644
index 00000000..b5f015f6
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(coordination_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/PeerProfilePayload.msg"
+  "msg/PeerProfile.msg"
+  DEPENDENCIES geometry_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
new file mode 100644
index 00000000..c8f78240
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
@@ -0,0 +1,22 @@
+# Gossip-protocol peer state broadcast.
+# Each robot publishes one of these on /gossip/peers at its own rate.
+
+# Identity
+string robot_name
+
+# Current pose (position + orientation) extracted from odometry_conversion/odometry
+# pose.header.stamp carries the odometry timestamp
+geometry_msgs/PoseStamped pose
+
+# Current navigation goal – last pose in the global planner's published path.
+# All-zero header stamp signals that no plan is available yet (null waypoint).
+geometry_msgs/PoseStamped waypoint
+
+# Gossip metadata
+uint8 SOURCE_DIRECT  = 0
+uint8 SOURCE_RELAYED = 1
+uint8 source      # how this message reached us (unused in Phase 1, reserved)
+uint8 relay_hops  # number of relay hops (unused in Phase 1, reserved)
+
+# Arbitrary typed payloads – any number, any ROS message type
+PeerProfilePayload[] payloads
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
new file mode 100644
index 00000000..4f0b909b
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
@@ -0,0 +1,6 @@
+# A single typed payload carried inside a PeerProfile.
+# payload_type holds the fully-qualified ROS message type string,
+# e.g. "nav_msgs/msg/OccupancyGrid". payload_data holds the
+# serialized bytes produced by rclpy.serialization.serialize_message().
+string  payload_type
+uint8[] payload_data
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/package.xml b/robot/ros_ws/src/coordination/coordination_msgs/package.xml
new file mode 100644
index 00000000..64d1236f
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_msgs/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>coordination_msgs</name>
+  <version>0.0.0</version>
+  <description>Custom message definitions for multi-agent gossip coordination layer</description>
+  <maintainer email="airlab@andrew.cmu.edu">AirLab</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
new file mode 100644
index 00000000..38ba88bd
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -0,0 +1,304 @@
+#!/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"
+STAGE_SCALE = 0.01
+
+DRONE_USD = "~/.local/share/ov/data/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)
+DRONE3_XY_M = ( 0.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:
+            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()
+
+        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_pos = [DRONE1_XY_M[0] * s, DRONE1_XY_M[1] * s, SPAWN_HEIGHT_ABOVE_FLOOR_M * s]
+        drone2_pos = [DRONE2_XY_M[0] * s, DRONE2_XY_M[1] * s, SPAWN_HEIGHT_ABOVE_FLOOR_M * s]
+        drone3_pos = [DRONE3_XY_M[0] * s, DRONE3_XY_M[1] * s, SPAWN_HEIGHT_ABOVE_FLOOR_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,
+            domain_id=1,
+            usd_file=DRONE_USD,
+            init_pos=drone1_pos,
+            init_orient=[0.0, 0.0, 0.0, 1.0],
+        )
+
+        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.2, 0.0, -0.05],
+            camera_rotation_offset=[0.0, 0.0, 0.0],
+        )
+
+        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],
+            lidar_rotation_offset=[0.0, 0.0, 0.0],
+            lidar_min_range=0.75,
+        )
+
+        ####################################################################################################
+        # 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,
+            domain_id=2,
+            usd_file=DRONE_USD,
+            init_pos=drone2_pos,
+            init_orient=[0.0, 0.0, 0.0, 1.0],
+        )
+
+        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.2, 0.0, -0.05],
+            camera_rotation_offset=[0.0, 0.0, 0.0],
+        )
+
+        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],
+            lidar_rotation_offset=[0.0, 0.0, 0.0],
+            lidar_min_range=0.75,
+        )
+
+        ####################################################################################################
+        # Spawn vehicle 3
+        ####################################################################################################
+        graph_handle3 = spawn_px4_multirotor_node(
+            pegasus_node_name="PX4Multirotor_3",
+            drone_prim="/World/drone3/base_link",
+            robot_name="robot_3",
+            vehicle_id=3,
+            domain_id=3,
+            usd_file=DRONE_USD,
+            init_pos=drone3_pos,
+            init_orient=[0.0, 0.0, 0.0, 1.0],
+        )
+
+        add_zed_stereo_camera_subgraph(
+            parent_graph_handle=graph_handle3,
+            drone_prim="/World/drone3/base_link",
+            robot_name="robot_3",
+            camera_name="ZEDCamera",
+            camera_offset=[0.2, 0.0, -0.05],
+            camera_rotation_offset=[0.0, 0.0, 0.0],
+        )
+
+        add_ouster_lidar_subgraph(
+            parent_graph_handle=graph_handle3,
+            drone_prim="/World/drone3/base_link",
+            robot_name="robot_3",
+            lidar_name="OS1_REV6_128_10hz___512_resolution",
+            lidar_offset=[0.0, 0.0, 0.025],
+            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()

From 45dd5e8211296a09035c7c20b73cbf77dddcf07b Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 16 Mar 2026 00:17:17 -0400
Subject: [PATCH 09/55] added global coords under
 /{ROBOT_NAME}/interface/mavros/global_position/raw/fix(not my topic, it was
 already publishing to that)

---
 .../isaac-sim/launch_scripts/gps_utils.py     |  88 +++++++++
 .../three_drone_scene_import.py               | 170 +++++++-----------
 2 files changed, 150 insertions(+), 108 deletions(-)
 create mode 100644 simulation/isaac-sim/launch_scripts/gps_utils.py

diff --git a/simulation/isaac-sim/launch_scripts/gps_utils.py b/simulation/isaac-sim/launch_scripts/gps_utils.py
new file mode 100644
index 00000000..4fdac957
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/gps_utils.py
@@ -0,0 +1,88 @@
+"""
+GPS origin utilities for AirStack multi-drone simulation.
+
+Computes per-robot GPS origins from Isaac Sim world-frame spawn positions and
+sets them as process environment variables so the OmniGraph PX4 node picks them
+up when building each vehicle's PX4MavlinkBackendConfig.
+
+Usage in a scene script:
+    from utils.gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
+    DRONE_CONFIGS = [
+        {"domain_id": 1, "x_m": -3.0, "y_m":  3.5, "z_m": 0.15, "orient": [0, 0, 0, 1]},
+        {"domain_id": 2, "x_m":  3.0, "y_m":  3.0, "z_m": 0.15, "orient": [0, 0, 0, 1]},
+    ]
+    set_gps_origins(DRONE_CONFIGS)
+"""
+
+import math
+import os
+
+# GPS coordinates that correspond to world origin (0, 0, 0) in the simulation.
+# Zurich default matches the existing px4_config.yaml hardcoded values.
+DEFAULT_WORLD_ORIGIN = (47.3667, 8.5500, 408.0)  # (lat_deg, lon_deg, alt_m WGS-84)
+
+
+def compute_gps_origin(x_m, y_m, z_m, world_origin=DEFAULT_WORLD_ORIGIN):
+    """
+    Convert a world-frame spawn position (meters) to a GPS geo_origin.
+
+    Axis convention assumed (Isaac Sim stage, Z-up ENU):
+        +X = East  →  affects longitude
+        +Y = North →  affects latitude
+        +Z = Up    →  affects altitude
+
+    NOTE: If drones appear shifted in the wrong compass direction after testing,
+    swap the x_m and y_m assignments in delta_lat / delta_lon below.
+
+    Args:
+        x_m: spawn X position in world-frame meters
+        y_m: spawn Y position in world-frame meters
+        z_m: spawn Z position in world-frame meters (height above world origin)
+        world_origin: (lat_deg, lon_deg, alt_m) that world (0,0,0) maps to
+
+    Returns:
+        (lat_deg, lon_deg, alt_m) — the GPS home position for this robot
+    """
+    base_lat, base_lon, base_alt = world_origin
+    delta_lat = y_m / 111320.0
+    delta_lon = x_m / (111320.0 * math.cos(math.radians(base_lat)))
+    return (base_lat + delta_lat, base_lon + delta_lon, base_alt + z_m)
+
+
+def set_gps_origins(drone_configs, world_origin=DEFAULT_WORLD_ORIGIN):
+    """
+    Compute GPS origins for all drones and set them as process environment variables.
+
+    Called once at the start of a scene script before spawning vehicles.
+    Sets PX4_HOME_LAT_<domain_id>, PX4_HOME_LON_<domain_id>, PX4_HOME_ALT_<domain_id>
+    in the current process so the OmniGraph PX4 node reads them when building
+    PX4MavlinkBackendConfig, which passes them to PX4LaunchTool as PX4_HOME_LAT/LON/ALT
+    on the PX4 subprocess environment.
+
+    Args:
+        drone_configs: list of dicts, one per drone:
+            [
+                {
+                    "domain_id": int,      # matches vehicle_id passed to spawn_px4_multirotor_node
+                    "x_m":       float,    # world-frame spawn East  (meters)
+                    "y_m":       float,    # world-frame spawn North (meters)
+                    "z_m":       float,    # world-frame spawn Up    (meters)
+                    "orient":    list,     # quaternion [x, y, z, w]
+                },
+                ...
+            ]
+        world_origin: (lat_deg, lon_deg, alt_m) for world (0, 0, 0).
+    """
+    print(f"[gps_utils] World anchor: lat={world_origin[0]}, lon={world_origin[1]}, alt={world_origin[2]}")
+
+    for cfg in drone_configs:
+        domain_id = cfg["domain_id"]
+        lat, lon, alt = compute_gps_origin(cfg["x_m"], cfg["y_m"], cfg["z_m"], world_origin)
+
+        os.environ[f"PX4_HOME_LAT_{domain_id}"] = str(lat)
+        os.environ[f"PX4_HOME_LON_{domain_id}"] = str(lon)
+        os.environ[f"PX4_HOME_ALT_{domain_id}"] = str(alt)
+
+        print(f"[gps_utils]   domain_id={domain_id}: "
+              f"lat={lat:.7f}, lon={lon:.7f}, alt={alt:.2f}")
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
index 38ba88bd..ed5840ec 100644
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -1,5 +1,8 @@
 #!/usr/bin/env python3
 
+import sys
+import os
+
 import carb
 from isaacsim import SimulationApp
 
@@ -23,11 +26,17 @@
 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
 
+# gps_utils lives in the same directory as this script
+_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
+if _LAUNCH_SCRIPTS_DIR not in sys.path:
+    sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
+from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
 
 # --------------------- CONFIGURATION ---------------------
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
 
-#env/stage path and scale
+# env/stage path and scale
 ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.usd"
 STAGE_SCALE = 0.01
 
@@ -39,12 +48,21 @@
 DOME_LIGHT_INTENSITY = 3500.0
 DOME_LIGHT_EXPOSURE = -3.0
 
-# Drone offset
-SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
+# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates.
+# Matches the Zürich default in px4_config.yaml — change here to relocate the sim world.
+WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
 
-DRONE1_XY_M = (-3.0,  3.5)
-DRONE2_XY_M = ( 3.0,  3.0)
-DRONE3_XY_M = ( 0.0, -3.0)
+# Drone spawn configs.
+# x_m = East offset from world origin (meters)
+# y_m = North offset from world origin (meters)
+# z_m = Up offset / height above floor (meters)
+# orient = initial quaternion [x, y, z, w]
+SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
+DRONE_CONFIGS = [
+    {"domain_id": 1, "x_m": -3.0, "y_m":  3.5, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0]},
+    {"domain_id": 2, "x_m":  3.0, "y_m":  3.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0]},
+    {"domain_id": 3, "x_m":  0.0, "y_m": -3.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0]},
+]
 # ---------------------------------------------------------
 
 
@@ -107,6 +125,10 @@ def add_collision_to_prim(prim):
 class PegasusApp:
 
     def __init__(self):
+        # Write GPS origins immediately so robot containers can read them
+        # before this container finishes its heavy USD loading.
+        set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
+
         omni.client.initialize()
         nucleus_stat(f"omniverse://{NUCLEUS_SERVER}")
         nucleus_stat(ENV_URL)
@@ -169,108 +191,40 @@ def __init__(self):
         # Units
         mpu, s = get_stage_scale(stage)
 
-        drone1_pos = [DRONE1_XY_M[0] * s, DRONE1_XY_M[1] * s, SPAWN_HEIGHT_ABOVE_FLOOR_M * s]
-        drone2_pos = [DRONE2_XY_M[0] * s, DRONE2_XY_M[1] * s, SPAWN_HEIGHT_ABOVE_FLOOR_M * s]
-        drone3_pos = [DRONE3_XY_M[0] * s, DRONE3_XY_M[1] * s, SPAWN_HEIGHT_ABOVE_FLOOR_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,
-            domain_id=1,
-            usd_file=DRONE_USD,
-            init_pos=drone1_pos,
-            init_orient=[0.0, 0.0, 0.0, 1.0],
-        )
-
-        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.2, 0.0, -0.05],
-            camera_rotation_offset=[0.0, 0.0, 0.0],
-        )
-
-        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],
-            lidar_rotation_offset=[0.0, 0.0, 0.0],
-            lidar_min_range=0.75,
-        )
-
-        ####################################################################################################
-        # 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,
-            domain_id=2,
-            usd_file=DRONE_USD,
-            init_pos=drone2_pos,
-            init_orient=[0.0, 0.0, 0.0, 1.0],
-        )
-
-        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.2, 0.0, -0.05],
-            camera_rotation_offset=[0.0, 0.0, 0.0],
-        )
-
-        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],
-            lidar_rotation_offset=[0.0, 0.0, 0.0],
-            lidar_min_range=0.75,
-        )
-
-        ####################################################################################################
-        # Spawn vehicle 3
-        ####################################################################################################
-        graph_handle3 = spawn_px4_multirotor_node(
-            pegasus_node_name="PX4Multirotor_3",
-            drone_prim="/World/drone3/base_link",
-            robot_name="robot_3",
-            vehicle_id=3,
-            domain_id=3,
-            usd_file=DRONE_USD,
-            init_pos=drone3_pos,
-            init_orient=[0.0, 0.0, 0.0, 1.0],
-        )
-
-        add_zed_stereo_camera_subgraph(
-            parent_graph_handle=graph_handle3,
-            drone_prim="/World/drone3/base_link",
-            robot_name="robot_3",
-            camera_name="ZEDCamera",
-            camera_offset=[0.2, 0.0, -0.05],
-            camera_rotation_offset=[0.0, 0.0, 0.0],
-        )
-
-        add_ouster_lidar_subgraph(
-            parent_graph_handle=graph_handle3,
-            drone_prim="/World/drone3/base_link",
-            robot_name="robot_3",
-            lidar_name="OS1_REV6_128_10hz___512_resolution",
-            lidar_offset=[0.0, 0.0, 0.025],
-            lidar_rotation_offset=[0.0, 0.0, 0.0],
-            lidar_min_range=0.75,
-        )
+        # Spawn all drones
+        for cfg in DRONE_CONFIGS:
+            i = cfg["domain_id"]
+            pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
+
+            graph_handle = spawn_px4_multirotor_node(
+                pegasus_node_name=f"PX4Multirotor_{i}",
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                vehicle_id=i,
+                domain_id=i,
+                usd_file=DRONE_USD,
+                init_pos=pos,
+                init_orient=cfg["orient"],
+            )
+
+            add_zed_stereo_camera_subgraph(
+                parent_graph_handle=graph_handle,
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                camera_name="ZEDCamera",
+                camera_offset=[0.2, 0.0, -0.05],
+                camera_rotation_offset=[0.0, 0.0, 0.0],
+            )
+
+            add_ouster_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_offset=[0.0, 0.0, 0.025],
+                lidar_rotation_offset=[0.0, 0.0, 0.0],
+                lidar_min_range=0.75,
+            )
 
         # Reset so physics/articulations are ready
         self.world.reset()

From 8729411a3f6ef0eb4f07d92fcd7bedd1edb5f7cf Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 20 Mar 2026 17:51:56 -0400
Subject: [PATCH 10/55] gossip, threedrone,peerprofile

---
 .../coordination_bringup/gossip_node.py       | 31 +++++++++++++------
 .../coordination_bringup/peer_profile.py      | 30 +++++++++++-------
 .../coordination_bringup/package.xml          |  1 +
 .../scripts/peer_registry_monitor.py          | 29 +++++++++++------
 .../coordination_msgs/CMakeLists.txt          |  3 +-
 .../coordination_msgs/msg/PeerProfile.msg     |  9 ++++--
 .../coordination_msgs/package.xml             |  1 +
 .../perception/macvo_ros2/macvo_ros2/macvo    |  2 +-
 .../three_drone_scene_import.py               |  2 +-
 9 files changed, 70 insertions(+), 38 deletions(-)

diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
index 1c8c258d..d979e099 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -4,7 +4,7 @@
 ROS 2 node that implements the gossip-protocol coordination layer.
 
 Each robot:
-  - Subscribes to its own odometry and global plan
+  - Subscribes to its own GPS fix (NavSatFix) and compass heading, and global plan
   - Publishes its own PeerProfile to /gossip/peers (shared gossip domain via DDS Router)
   - Subscribes to /gossip/peers to receive all peer profiles
   - Maintains a registry of known peers (latest-wins, no expiry)
@@ -28,7 +28,9 @@
     QoSReliabilityPolicy,
 )
 
-from nav_msgs.msg import Odometry, Path
+from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix
+from std_msgs.msg import Float64
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 
 from coordination_bringup.peer_profile import PeerProfile
@@ -70,10 +72,16 @@ def __init__(self):
         self._registry: dict[str, PeerProfileMsg] = {}
 
         # ── Subscriptions ────────────────────────────────────────────────
-        self._odom_sub = self.create_subscription(
-            Odometry,
-            f"/{self._robot_name}/odometry_conversion/odometry",
-            self._on_odom,
+        self._navsat_sub = self.create_subscription(
+            NavSatFix,
+            f"/{self._robot_name}/interface/mavros/global_position/raw/fix",
+            self._on_navsat,
+            GOSSIP_QOS,
+        )
+        self._compass_sub = self.create_subscription(
+            Float64,
+            f"/{self._robot_name}/interface/mavros/global_position/compass_hdg",
+            self._on_compass,
             GOSSIP_QOS,
         )
         self._path_sub = self.create_subscription(
@@ -121,8 +129,11 @@ def __init__(self):
     # Subscription callbacks                                               #
     # ------------------------------------------------------------------ #
 
-    def _on_odom(self, msg: Odometry) -> None:
-        self._profile.set_pose_from_odom(msg)
+    def _on_navsat(self, msg: NavSatFix) -> None:
+        self._profile.set_gps_from_navsat(msg)
+
+    def _on_compass(self, msg: Float64) -> None:
+        self._profile.set_heading(msg.data)
 
     def _on_global_plan(self, msg: Path) -> None:
         self._profile.set_waypoint_from_path(msg)
@@ -152,8 +163,8 @@ def _update_registry(self, msg: PeerProfileMsg) -> None:
         existing = self._registry.get(msg.robot_name)
         if existing is not None:
             # Only update if the incoming message is newer
-            new_stamp = msg.pose.header.stamp.sec + msg.pose.header.stamp.nanosec * 1e-9
-            old_stamp = existing.pose.header.stamp.sec + existing.pose.header.stamp.nanosec * 1e-9
+            new_stamp = msg.gps_fix.header.stamp.sec + msg.gps_fix.header.stamp.nanosec * 1e-9
+            old_stamp = existing.gps_fix.header.stamp.sec + existing.gps_fix.header.stamp.nanosec * 1e-9
             if new_stamp <= old_stamp:
                 return
 
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
index a7c4bbe3..b589f37c 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
@@ -8,7 +8,8 @@
 -----
 # Basic – just use the base class
 profile = PeerProfile("robot_1")
-profile.set_pose_from_odom(odom_msg)
+profile.set_gps_from_navsat(navsat_msg)
+profile.set_heading(compass_hdg_msg.data)
 profile.set_waypoint_from_path(path_msg)
 ros_msg = profile.to_ros_msg()
 
@@ -36,7 +37,8 @@ def attach_frontier(self, frontier_msg):
 import rosidl_runtime_py.utilities as rosidl_utils
 
 from geometry_msgs.msg import PoseStamped
-from nav_msgs.msg import Odometry, Path
+from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix
 
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 from coordination_msgs.msg import PeerProfilePayload as PeerProfilePayloadMsg
@@ -58,7 +60,8 @@ class PeerProfile:
     """
 
     robot_name: str
-    pose: PoseStamped = field(default_factory=PoseStamped)
+    gps_fix: NavSatFix = field(default_factory=NavSatFix)
+    heading: float = 0.0  # degrees clockwise from North (0-360)
     waypoint: PoseStamped = field(default_factory=PoseStamped)
     source: Source = Source.DIRECT
     relay_hops: int = 0
@@ -67,15 +70,16 @@ class PeerProfile:
     _payloads: List[Dict[str, Any]] = field(default_factory=list, repr=False)
 
     # ------------------------------------------------------------------ #
-    # Pose / waypoint helpers                                              #
+    # GPS / waypoint helpers                                               #
     # ------------------------------------------------------------------ #
 
-    def set_pose_from_odom(self, odom: Odometry) -> None:
-        """Extract pose (position + orientation) from an Odometry message."""
-        stamped = PoseStamped()
-        stamped.header = odom.header
-        stamped.pose = odom.pose.pose
-        self.pose = stamped
+    def set_gps_from_navsat(self, msg: NavSatFix) -> None:
+        """Store GPS fix from a sensor_msgs/NavSatFix message."""
+        self.gps_fix = msg
+
+    def set_heading(self, degrees: float) -> None:
+        """Set heading in degrees clockwise from North (0-360)."""
+        self.heading = float(degrees)
 
     def set_waypoint_from_path(self, path: Optional[Path]) -> None:
         """
@@ -140,7 +144,8 @@ def payload_types(self) -> List[str]:
     def to_ros_msg(self) -> PeerProfileMsg:
         msg = PeerProfileMsg()
         msg.robot_name = self.robot_name
-        msg.pose = self.pose
+        msg.gps_fix = self.gps_fix
+        msg.heading = self.heading
         msg.waypoint = self.waypoint
         msg.source = int(self.source)
         msg.relay_hops = self.relay_hops
@@ -153,7 +158,8 @@ def to_ros_msg(self) -> PeerProfileMsg:
     @classmethod
     def from_ros_msg(cls, msg: PeerProfileMsg) -> "PeerProfile":
         profile = cls(robot_name=msg.robot_name)
-        profile.pose = msg.pose
+        profile.gps_fix = msg.gps_fix
+        profile.heading = msg.heading
         profile.waypoint = msg.waypoint
         profile.source = Source(msg.source)
         profile.relay_hops = msg.relay_hops
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/package.xml b/robot/ros_ws/src/coordination/coordination_bringup/package.xml
index 46666053..f9482e5a 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/package.xml
+++ b/robot/ros_ws/src/coordination/coordination_bringup/package.xml
@@ -10,6 +10,7 @@
   <depend>rclpy</depend>
   <depend>nav_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>coordination_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
index a0fa038e..8af1b3ee 100755
--- a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
@@ -44,10 +44,19 @@
 DIM = "\033[2m"
 
 
-def _fmt_pose(pose_stamped) -> str:
-    p = pose_stamped.pose.position
-    o = pose_stamped.pose.orientation
-    return f"pos=({p.x:7.2f}, {p.y:7.2f}, {p.z:7.2f})  orient=({o.x:.3f}, {o.y:.3f}, {o.z:.3f}, {o.w:.3f})"
+def _fmt_gps(gps_fix, heading: float) -> str:
+    from sensor_msgs.msg import NavSatStatus
+    status = gps_fix.status.status
+    status_str = {
+        NavSatStatus.STATUS_NO_FIX: "NO_FIX",
+        NavSatStatus.STATUS_FIX: "FIX",
+        NavSatStatus.STATUS_SBAS_FIX: "SBAS",
+        NavSatStatus.STATUS_GBAS_FIX: "GBAS",
+    }.get(status, f"?{status}")
+    return (
+        f"lat={gps_fix.latitude:11.7f}  lon={gps_fix.longitude:11.7f}  "
+        f"alt={gps_fix.altitude:7.2f}m  hdg={heading:6.1f}°  [{status_str}]"
+    )
 
 
 def _fmt_waypoint(pose_stamped) -> str:
@@ -59,8 +68,8 @@ def _fmt_waypoint(pose_stamped) -> str:
     return f"pos=({p.x:7.2f}, {p.y:7.2f}, {p.z:7.2f})  orient=({o.x:.3f}, {o.y:.3f}, {o.z:.3f}, {o.w:.3f})"
 
 
-def _fmt_stamp(pose_stamped) -> str:
-    s = pose_stamped.header.stamp
+def _fmt_stamp(gps_fix) -> str:
+    s = gps_fix.header.stamp
     if s.sec == 0 and s.nanosec == 0:
         return "n/a"
     t = s.sec + s.nanosec * 1e-9
@@ -89,8 +98,8 @@ def __init__(self, filter_name: str = ""):
     def _on_msg(self, msg: PeerProfileMsg) -> None:
         existing = self._registry.get(msg.robot_name)
         if existing is not None:
-            new_t = msg.pose.header.stamp.sec + msg.pose.header.stamp.nanosec * 1e-9
-            old_t = existing.pose.header.stamp.sec + existing.pose.header.stamp.nanosec * 1e-9
+            new_t = msg.gps_fix.header.stamp.sec + msg.gps_fix.header.stamp.nanosec * 1e-9
+            old_t = existing.gps_fix.header.stamp.sec + existing.gps_fix.header.stamp.nanosec * 1e-9
             if new_t <= old_t:
                 return
         self._registry[msg.robot_name] = msg
@@ -117,9 +126,9 @@ def print_registry(self) -> None:
                     if msg.payloads
                     else "no payloads"
                 )
-                stamp_str = _fmt_stamp(msg.pose)
+                stamp_str = _fmt_stamp(msg.gps_fix)
                 print(f"  {CYAN}{BOLD}{msg.robot_name}{RESET}  {DIM}[{src}  stamp={stamp_str}]{RESET}")
-                print(f"    {GREEN}pose    {RESET} {_fmt_pose(msg.pose)}")
+                print(f"    {GREEN}gps     {RESET} {_fmt_gps(msg.gps_fix, msg.heading)}")
                 print(f"    {YELLOW}waypoint{RESET} {_fmt_waypoint(msg.waypoint)}")
                 print(f"    {DIM}payloads{RESET} {payload_summary}")
                 print()
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt b/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
index b5f015f6..ffb9e823 100644
--- a/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
+++ b/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
@@ -4,11 +4,12 @@ project(coordination_msgs)
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/PeerProfilePayload.msg"
   "msg/PeerProfile.msg"
-  DEPENDENCIES geometry_msgs
+  DEPENDENCIES geometry_msgs sensor_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
index c8f78240..f85b537f 100644
--- a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
+++ b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
@@ -4,9 +4,12 @@
 # Identity
 string robot_name
 
-# Current pose (position + orientation) extracted from odometry_conversion/odometry
-# pose.header.stamp carries the odometry timestamp
-geometry_msgs/PoseStamped pose
+# Current GPS position from interface/mavros/global_position/raw/fix
+# gps_fix.header.stamp carries the NavSatFix timestamp
+sensor_msgs/NavSatFix gps_fix
+
+# Heading in degrees clockwise from North (0-360), from compass_hdg
+float64 heading
 
 # Current navigation goal – last pose in the global planner's published path.
 # All-zero header stamp signals that no plan is available yet (null waypoint).
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/package.xml b/robot/ros_ws/src/coordination/coordination_msgs/package.xml
index 64d1236f..27108aa0 100644
--- a/robot/ros_ws/src/coordination/coordination_msgs/package.xml
+++ b/robot/ros_ws/src/coordination/coordination_msgs/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <member_of_group>rosidl_interface_packages</member_of_group>
 
diff --git a/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo b/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
index 92a191dc..8683b532 160000
--- a/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
+++ b/robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
@@ -1 +1 @@
-Subproject commit 92a191dc601ec71c8d1c1a19b55b3a900103595c
+Subproject commit 8683b532d6f6e8981f6b1a646fb5f52f02c1ea61
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
index ed5840ec..ef084ab3 100644
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -223,7 +223,7 @@ def __init__(self):
                 lidar_name="OS1_REV6_128_10hz___512_resolution",
                 lidar_offset=[0.0, 0.0, 0.025],
                 lidar_rotation_offset=[0.0, 0.0, 0.0],
-                lidar_min_range=0.75,
+                lidar_min_range=2.0,
             )
 
         # Reset so physics/articulations are ready

From 9d66ab82242e5e38dfcd0a7be715278ed06e0e9b Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sat, 21 Mar 2026 19:08:01 -0400
Subject: [PATCH 11/55] multi drone vis in foxglove, odom doesn't work in
 foxglove yet

---
 .../desktop_bringup/launch/gcs.launch.xml     |   3 +
 .../msgs/airstack_msgs/msg/BeliefMapData.msg  |  10 +
 .../gcs_visualizer/gcs_visualizer/__init__.py |   0
 .../gcs_visualizer/robot_marker_node.py       | 223 ++++++++++++++++++
 .../launch/gcs_visualizer.launch.xml          |   7 +
 gcs/ros_ws/src/gcs_visualizer/package.xml     |  26 ++
 .../gcs_visualizer/resource/gcs_visualizer    |   0
 gcs/ros_ws/src/gcs_visualizer/setup.cfg       |   4 +
 gcs/ros_ws/src/gcs_visualizer/setup.py        |  28 +++
 .../onboard_all/config/dds_router.yaml        |   2 +-
 .../isaac-sim/launch_scripts/gps_utils.py     |   4 +-
 11 files changed, 304 insertions(+), 3 deletions(-)
 create mode 100644 common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/__init__.py
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/package.xml
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/resource/gcs_visualizer
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/setup.cfg
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/setup.py

diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index ac3dd2f2..ee09563f 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -30,6 +30,9 @@
 
    
 
+  <!-- GCS Visualizer: drone mesh markers in global ENU frame -->
+  <include file="$(find-pkg-share gcs_visualizer)/launch/gcs_visualizer.launch.xml" />
+
   <!-- ===================== -->
 
 </launch>
\ No newline at end of file
diff --git a/common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg b/common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg
new file mode 100644
index 00000000..22f5162d
--- /dev/null
+++ b/common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg
@@ -0,0 +1,10 @@
+# serialized BeliefMap data
+# assumes same origin, same grid resolution
+
+int16 x_start # cell number relative to the origin. max int16 is 32767, so this is a reasonable assumption
+int16 y_start
+int32 x_end # if we break things up in the future, could assume always square and remove this
+int32 y_end # if we break things up in the future, could assume always square and remove this
+
+uint16[] map_values # between 0 and 1, 65535 values, precision of 0.000015
+
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/__init__.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
new file mode 100644
index 00000000..4435d5fa
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
@@ -0,0 +1,223 @@
+import math
+import re
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
+from sensor_msgs.msg import NavSatFix
+from nav_msgs.msg import Odometry
+from visualization_msgs.msg import Marker, MarkerArray
+from builtin_interfaces.msg import Duration
+from geometry_msgs.msg import TransformStamped
+from tf2_ros import StaticTransformBroadcaster, TransformBroadcaster
+
+# Lisbon — matches Pegasus configs.yaml and gps_utils.py DEFAULT_WORLD_ORIGIN
+ORIGIN_LAT = 38.736832
+ORIGIN_LON = -9.137977
+ORIGIN_ALT = 90.0
+
+SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    durability=DurabilityPolicy.VOLATILE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+GPS_SUFFIX = '/interface/mavros/global_position/global'
+ODOM_SUFFIX = '/odometry_conversion/odometry'
+
+ROBOT_COLORS = [
+    (1.0, 0.2, 0.2),  # red
+    (0.2, 1.0, 0.2),  # green
+    (0.2, 0.2, 1.0),  # blue
+]
+
+# OBJ -> ROS axis correction quaternion (belly -Z, nose +X)
+AXIS_CORRECTION = (-0.5, -0.5, 0.5, 0.5)  # x, y, z, w
+
+
+def gps_to_enu(lat, lon, alt, alt_ground):
+    """Convert GPS lat/lon/alt to ENU metres relative to Lisbon origin. z is relative to first fix."""
+    x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
+    y = (lat - ORIGIN_LAT) * 111320.0
+    z = alt - alt_ground
+    return x, y, z
+
+
+def multiply_quaternions(q1, q2):
+    """Hamilton product q1 * q2 (apply q2 first, then q1). Both are (x,y,z,w) tuples."""
+    x1, y1, z1, w1 = q1
+    x2, y2, z2, w2 = q2
+    return (
+        w1*x2 + x1*w2 + y1*z2 - z1*y2,
+        w1*y2 - x1*z2 + y1*w2 + z1*x2,
+        w1*z2 + x1*y2 - y1*x2 + z1*w2,
+        w1*w2 - x1*x2 - y1*y2 - z1*z2,
+    )
+
+
+class RobotMarkerNode(Node):
+    def __init__(self):
+        super().__init__('robot_marker_node')
+
+        self.declare_parameter('robot_name_prefix', 'robot')
+        self._prefix = self.get_parameter('robot_name_prefix').value
+        self._gps_pattern = re.compile(
+            rf'^/({re.escape(self._prefix)}_\w+){re.escape(GPS_SUFFIX)}$'
+        )
+        self._odom_pattern = re.compile(
+            rf'^/({re.escape(self._prefix)}_\w+){re.escape(ODOM_SUFFIX)}$'
+        )
+
+        self._gps_positions = {}    # robot_name -> (x, y, z) ENU metres
+        self._orientations = {}     # robot_name -> (x, y, z, w) from odometry
+        self._subscribed_gps = set()
+        self._subscribed_odom = set()
+        self._alt_ground = None     # altitude of first fix, used as z=0 reference
+
+        self._pub = self.create_publisher(MarkerArray, '/gcs/robot_markers', 10)
+
+        # Broadcast map as root TF frame so Foxglove 3D panel has a display frame
+        self._static_tf = StaticTransformBroadcaster(self)
+        self._dynamic_tf = TransformBroadcaster(self)
+        map_tf = TransformStamped()
+        map_tf.header.stamp = self.get_clock().now().to_msg()
+        map_tf.header.frame_id = 'map'
+        map_tf.child_frame_id = 'enu_origin'
+        map_tf.transform.rotation.w = 1.0
+        self._static_tf.sendTransform(map_tf)
+
+        self.create_timer(5.0, self._discover_robots)
+        self.create_timer(0.1, self._publish_markers)
+        self._discover_robots()
+
+    def _discover_robots(self):
+        for topic, type_list in self.get_topic_names_and_types():
+            if topic not in self._subscribed_gps:
+                m = self._gps_pattern.match(topic)
+                if m and 'sensor_msgs/msg/NavSatFix' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        NavSatFix, topic,
+                        lambda msg, n=name: self._gps_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_gps.add(topic)
+                    self.get_logger().info(f'Subscribed to GPS: {topic}')
+
+            if topic not in self._subscribed_odom:
+                m = self._odom_pattern.match(topic)
+                if m and 'nav_msgs/msg/Odometry' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        Odometry, topic,
+                        lambda msg, n=name: self._odom_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_odom.add(topic)
+                    self.get_logger().info(f'Subscribed to odom: {topic}')
+
+    def _gps_callback(self, msg: NavSatFix, robot_name: str):
+        if msg.status.status < 0:
+            return
+        if self._alt_ground is None:
+            self._alt_ground = msg.altitude
+        self._gps_positions[robot_name] = gps_to_enu(msg.latitude, msg.longitude, msg.altitude, self._alt_ground)
+
+    def _odom_callback(self, msg: Odometry, robot_name: str):
+        o = msg.pose.pose.orientation
+        self._orientations[robot_name] = (o.x, o.y, o.z, o.w)
+
+    def _publish_markers(self):
+        if not self._gps_positions:
+            return
+
+        array = MarkerArray()
+        now = self.get_clock().now().to_msg()
+        lifetime = Duration(sec=1, nanosec=0)
+
+        for i, robot_name in enumerate(sorted(self._gps_positions.keys())):
+            color = ROBOT_COLORS[i % len(ROBOT_COLORS)]
+            x, y, z = self._gps_positions[robot_name]
+            orientation = self._orientations.get(robot_name)  # (x,y,z,w) or None
+
+            # --- Mesh marker ---
+            mesh = Marker()
+            mesh.header.frame_id = 'map'
+            mesh.header.stamp = now
+            mesh.ns = 'robot_meshes'
+            mesh.id = i * 3
+            mesh.type = Marker.MESH_RESOURCE
+            mesh.action = Marker.ADD
+            mesh.mesh_resource = 'package://robot_descriptions/iris/meshes/base_link_body_body.obj'
+            mesh.pose.position.x = x
+            mesh.pose.position.y = y
+            mesh.pose.position.z = z
+            if orientation:
+                qx, qy, qz, qw = multiply_quaternions(orientation, AXIS_CORRECTION)
+            else:
+                qx, qy, qz, qw = AXIS_CORRECTION
+            mesh.pose.orientation.x = qx
+            mesh.pose.orientation.y = qy
+            mesh.pose.orientation.z = qz
+            mesh.pose.orientation.w = qw
+            mesh.scale.x = mesh.scale.y = mesh.scale.z = 1.0
+            mesh.color.r = color[0]
+            mesh.color.g = color[1]
+            mesh.color.b = color[2]
+            mesh.color.a = 0.9
+            mesh.lifetime = lifetime
+            array.markers.append(mesh)
+
+            # --- Label marker ---
+            label = Marker()
+            label.header.frame_id = 'map'
+            label.header.stamp = now
+            label.ns = 'robot_labels'
+            label.id = i * 3 + 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position.x = x
+            label.pose.position.y = y
+            label.pose.position.z = z + 1.0
+            label.pose.orientation.w = 1.0
+            label.scale.z = 0.5
+            label.color.r = label.color.g = label.color.b = label.color.a = 1.0
+            label.text = robot_name
+            label.lifetime = lifetime
+            array.markers.append(label)
+
+            # --- TF: map -> robot_N/robot_pose ---
+            tf = TransformStamped()
+            tf.header.stamp = now
+            tf.header.frame_id = 'map'
+            tf.child_frame_id = f'{robot_name}/robot_pose'
+            tf.transform.translation.x = x
+            tf.transform.translation.y = y
+            tf.transform.translation.z = z
+            if orientation:
+                tf.transform.rotation.x = orientation[0]
+                tf.transform.rotation.y = orientation[1]
+                tf.transform.rotation.z = orientation[2]
+                tf.transform.rotation.w = orientation[3]
+            else:
+                tf.transform.rotation.w = 1.0
+            self._dynamic_tf.sendTransform(tf)
+
+        self._pub.publish(array)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RobotMarkerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
new file mode 100644
index 00000000..9930c773
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
@@ -0,0 +1,7 @@
+<launch>
+  <arg name="robot_name_prefix" default="robot" />
+
+  <node pkg="gcs_visualizer" exec="robot_marker_node" name="robot_marker_node" output="screen">
+    <param name="robot_name_prefix" value="$(var robot_name_prefix)" />
+  </node>
+</launch>
diff --git a/gcs/ros_ws/src/gcs_visualizer/package.xml b/gcs/ros_ws/src/gcs_visualizer/package.xml
new file mode 100644
index 00000000..7872ebbc
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gcs_visualizer</name>
+  <version>0.0.1</version>
+  <description>GCS visualization node: publishes drone mesh markers in a global ENU frame for Foxglove</description>
+  <maintainer email="airlab@cmu.edu">AirLab CMU</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/gcs/ros_ws/src/gcs_visualizer/resource/gcs_visualizer b/gcs/ros_ws/src/gcs_visualizer/resource/gcs_visualizer
new file mode 100644
index 00000000..e69de29b
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.cfg b/gcs/ros_ws/src/gcs_visualizer/setup.cfg
new file mode 100644
index 00000000..44287497
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gcs_visualizer
+[install]
+install_scripts=$base/lib/gcs_visualizer
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.py b/gcs/ros_ws/src/gcs_visualizer/setup.py
new file mode 100644
index 00000000..8b189726
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.py
@@ -0,0 +1,28 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = 'gcs_visualizer'
+
+setup(
+    name=package_name,
+    version='0.0.1',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.xml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='AirLab CMU',
+    maintainer_email='airlab@cmu.edu',
+    description='GCS visualization node for drone mesh markers in Foxglove',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'robot_marker_node = gcs_visualizer.robot_marker_node:main',
+        ],
+    },
+)
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index 426d8ea3..bc01a4ef 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -23,7 +23,7 @@ participants:
 allowlist:
   # state information
   - name: "rt/$(env ROBOT_NAME)/odometry_conversion/odometry"
-  - name: "rt/$(env ROBOT_NAME)/interface/mavros/global_position/raw/fix"
+  - name: "rt/$(env ROBOT_NAME)/interface/mavros/global_position/global"
 
   # behavior tree
   - name: "rt/$(env ROBOT_NAME)/behavior/behavior_tree_commands"
diff --git a/simulation/isaac-sim/launch_scripts/gps_utils.py b/simulation/isaac-sim/launch_scripts/gps_utils.py
index 4fdac957..d17e8516 100644
--- a/simulation/isaac-sim/launch_scripts/gps_utils.py
+++ b/simulation/isaac-sim/launch_scripts/gps_utils.py
@@ -19,8 +19,8 @@
 import os
 
 # GPS coordinates that correspond to world origin (0, 0, 0) in the simulation.
-# Zurich default matches the existing px4_config.yaml hardcoded values.
-DEFAULT_WORLD_ORIGIN = (47.3667, 8.5500, 408.0)  # (lat_deg, lon_deg, alt_m WGS-84)
+# Lisbon default matches the Pegasus configs.yaml default.
+DEFAULT_WORLD_ORIGIN = (38.736832, -9.137977, 90.0)  # (lat_deg, lon_deg, alt_m WGS-84) — Lisbon, matches Pegasus configs.yaml
 
 
 def compute_gps_origin(x_m, y_m, z_m, world_origin=DEFAULT_WORLD_ORIGIN):

From cabf9ae0b77c126e59c7568a2a1c4b78ee9b074d Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sat, 21 Mar 2026 19:18:51 -0400
Subject: [PATCH 12/55] multi drone vis in foxglove works with odom

---
 .../gcs_visualizer/robot_marker_node.py       | 78 +++++++++++++------
 1 file changed, 54 insertions(+), 24 deletions(-)

diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
index 4435d5fa..2c77e258 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
@@ -8,8 +8,8 @@
 from nav_msgs.msg import Odometry
 from visualization_msgs.msg import Marker, MarkerArray
 from builtin_interfaces.msg import Duration
-from geometry_msgs.msg import TransformStamped
-from tf2_ros import StaticTransformBroadcaster, TransformBroadcaster
+from geometry_msgs.msg import TransformStamped, Point
+from tf2_ros import StaticTransformBroadcaster
 
 # Lisbon — matches Pegasus configs.yaml and gps_utils.py DEFAULT_WORLD_ORIGIN
 ORIGIN_LAT = 38.736832
@@ -56,6 +56,22 @@ def multiply_quaternions(q1, q2):
     )
 
 
+def rotate_vector(v, q):
+    """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
+    vx, vy, vz = v
+    qx, qy, qz, qw = q
+    # v' = q * pure_quat(v) * q_conj
+    # Using optimised formula: v' = v + 2*qw*(q×v) + 2*(q×(q×v))
+    cx = qy * vz - qz * vy
+    cy = qz * vx - qx * vz
+    cz = qx * vy - qy * vx
+    return (
+        vx + 2.0 * (qw * cx + qy * cz - qz * cy),
+        vy + 2.0 * (qw * cy + qz * cx - qx * cz),
+        vz + 2.0 * (qw * cz + qx * cy - qy * cx),
+    )
+
+
 class RobotMarkerNode(Node):
     def __init__(self):
         super().__init__('robot_marker_node')
@@ -79,7 +95,6 @@ def __init__(self):
 
         # Broadcast map as root TF frame so Foxglove 3D panel has a display frame
         self._static_tf = StaticTransformBroadcaster(self)
-        self._dynamic_tf = TransformBroadcaster(self)
         map_tf = TransformStamped()
         map_tf.header.stamp = self.get_clock().now().to_msg()
         map_tf.header.frame_id = 'map'
@@ -162,10 +177,10 @@ def _publish_markers(self):
             mesh.pose.orientation.z = qz
             mesh.pose.orientation.w = qw
             mesh.scale.x = mesh.scale.y = mesh.scale.z = 1.0
-            mesh.color.r = color[0]
-            mesh.color.g = color[1]
-            mesh.color.b = color[2]
-            mesh.color.a = 0.9
+            mesh.color.r = 0.0
+            mesh.color.g = 0.0
+            mesh.color.b = 0.0
+            mesh.color.a = 1.0
             mesh.lifetime = lifetime
             array.markers.append(mesh)
 
@@ -181,28 +196,43 @@ def _publish_markers(self):
             label.pose.position.y = y
             label.pose.position.z = z + 1.0
             label.pose.orientation.w = 1.0
-            label.scale.z = 0.5
+            label.scale.z = 0.2
             label.color.r = label.color.g = label.color.b = label.color.a = 1.0
             label.text = robot_name
             label.lifetime = lifetime
             array.markers.append(label)
 
-            # --- TF: map -> robot_N/robot_pose ---
-            tf = TransformStamped()
-            tf.header.stamp = now
-            tf.header.frame_id = 'map'
-            tf.child_frame_id = f'{robot_name}/robot_pose'
-            tf.transform.translation.x = x
-            tf.transform.translation.y = y
-            tf.transform.translation.z = z
-            if orientation:
-                tf.transform.rotation.x = orientation[0]
-                tf.transform.rotation.y = orientation[1]
-                tf.transform.rotation.z = orientation[2]
-                tf.transform.rotation.w = orientation[3]
-            else:
-                tf.transform.rotation.w = 1.0
-            self._dynamic_tf.sendTransform(tf)
+            # --- Axes markers (X=red, Y=green, Z=blue) ---
+            axes = [
+                ((1.0, 0.0, 0.0), (1.0, 0.0, 0.0)),  # X axis, red
+                ((0.0, 1.0, 0.0), (0.0, 1.0, 0.0)),  # Y axis, green
+                ((0.0, 0.0, 1.0), (0.0, 0.0, 1.0)),  # Z axis, blue
+            ]
+            q = orientation if orientation else (0.0, 0.0, 0.0, 1.0)
+            axis_len = 0.6
+            for j, (unit_vec, axis_color) in enumerate(axes):
+                tip = rotate_vector(unit_vec, q)
+                arrow = Marker()
+                arrow.header.frame_id = 'map'
+                arrow.header.stamp = now
+                arrow.ns = f'{robot_name}_axes'
+                arrow.id = i * 3 + 2 + j  # unique id per axis per robot (offset past mesh+label)
+                arrow.type = Marker.ARROW
+                arrow.action = Marker.ADD
+                start = Point(x=x, y=y, z=z)
+                end = Point(x=x + tip[0] * axis_len,
+                            y=y + tip[1] * axis_len,
+                            z=z + tip[2] * axis_len)
+                arrow.points = [start, end]
+                arrow.scale.x = 0.04  # shaft diameter
+                arrow.scale.y = 0.08  # head diameter
+                arrow.scale.z = 0.0   # head length (0 = auto)
+                arrow.color.r = axis_color[0]
+                arrow.color.g = axis_color[1]
+                arrow.color.b = axis_color[2]
+                arrow.color.a = 1.0
+                arrow.lifetime = lifetime
+                array.markers.append(arrow)
 
         self._pub.publish(array)
 

From ffd1ff6ebbadd35906e4871fe3a57f8315c6a024 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sat, 21 Mar 2026 20:24:06 -0400
Subject: [PATCH 13/55] global plan added

---
 .agents/skills/visualize-in-foxglove/SKILL.md | 174 ++++++++++++++++++
 .env                                          |   1 +
 AGENTS.md                                     |   1 +
 .../gcs_visualizer/robot_marker_node.py       | 133 ++++++++++---
 robot/docker/docker-compose.yaml              |   1 +
 .../onboard_all/config/dds_router.yaml        |   2 +
 .../global_bringup/config/vdb_params.yaml     |   2 -
 .../global_bringup/launch/global.launch.xml   |   2 +-
 .../three_drone_scene_import.py               |   4 +-
 9 files changed, 287 insertions(+), 33 deletions(-)
 create mode 100644 .agents/skills/visualize-in-foxglove/SKILL.md

diff --git a/.agents/skills/visualize-in-foxglove/SKILL.md b/.agents/skills/visualize-in-foxglove/SKILL.md
new file mode 100644
index 00000000..232e5f4f
--- /dev/null
+++ b/.agents/skills/visualize-in-foxglove/SKILL.md
@@ -0,0 +1,174 @@
+---
+name: visualize-in-foxglove
+description: Add visualization of a ROS 2 topic to Foxglove/GCS. Use when you want a new topic (path, markers, odometry, etc.) to appear in the Foxglove dashboard on the GCS. Covers DDS router bridging, robot_marker_node integration, and coordinate frame translation.
+license: Apache-2.0
+metadata:
+  author: AirLab CMU
+  repository: AirStack
+---
+
+# Skill: Visualize a Topic in Foxglove / GCS
+
+## When to Use
+
+You want a topic published by a robot container to be visible in the Foxglove dashboard
+running in the GCS container.
+
+## Architecture Overview
+
+```
+Robot container (domain: ROS_DOMAIN_ID)
+  └─ publishes topics
+
+DDS Router (onboard_all)
+  └─ bridges allowlisted topics to GCS domain
+
+GCS container (domain: 0)
+  ├─ Foxglove bridge → streams to browser
+  └─ robot_marker_node → transforms & republishes as /gcs/robot_markers MarkerArray
+```
+
+**Key insight:** A topic must appear in the DDS router allowlist AND be subscribed to
+in the GCS before it will appear in Foxglove. Missing either step = nothing shows up.
+
+---
+
+## Step 1 — Bridge the Topic in DDS Router
+
+**File:** `robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml`
+
+Add an entry to the `allowlist` for every topic you want on the GCS:
+
+```yaml
+allowlist:
+  - name: "rt/$(env ROBOT_NAME)/your/topic/here"
+```
+
+**Rules:**
+- All ROS 2 topics must be prefixed with `rt/` (ROS Topic).
+- Services use `rq/` (request) and `rr/` (reply).
+- The router runs per-robot (one instance per robot container), so `$(env ROBOT_NAME)`
+  expands to `robot_1`, `robot_2`, etc. automatically.
+- Topics are bidirectional by default.
+- Without this entry the topic simply does not cross domain boundaries — the GCS node
+  will never see it regardless of how it subscribes.
+
+After editing this file, **restart the robot containers** for the change to take effect.
+
+---
+
+## Step 2 — Subscribe and Visualize on the GCS
+
+There are two paths depending on what you want to display:
+
+### Path A — Display the raw topic directly in Foxglove
+
+If the topic message type is natively supported by Foxglove (e.g. `nav_msgs/Path`,
+`sensor_msgs/PointCloud2`, `visualization_msgs/MarkerArray`), just bridge it and add
+a panel in Foxglove pointing at the topic. No extra GCS code needed.
+
+**Limitation:** The topic arrives in the robot's local odom frame (`map` frame origin =
+drone boot position). If you need it georeferenced (aligned with GPS/ENU), you must
+translate it — see Path B.
+
+### Path B — Translate and republish via robot_marker_node
+
+**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py`
+
+This node auto-discovers robot topics, applies a GPS boot offset to convert from the
+robot's local odom frame to ENU (map frame), and republishes everything as a single
+`/gcs/robot_markers` MarkerArray.
+
+**Coordinate frame context:**
+- Robot odometry uses a local `map` frame whose origin is the drone's position at boot.
+- GPS coordinates are converted to ENU relative to `ORIGIN_LAT/LON/ALT` (Lisbon by default).
+- `_gps_boot[robot_name]` = ENU position of the odom origin = offset to add to all
+  odom-frame coordinates.
+- Trajectory and global plan markers are in odom frame → add boot offset to `points`.
+- Do NOT also offset `pose.position` for LINE_STRIP/ARROW markers — their points are
+  already in the header frame; double-offsetting the pose causes wrong positions.
+
+**To add a new topic type, follow this pattern (shown for `nav_msgs/Path`):**
+
+1. **Add suffix constant and regex pattern:**
+```python
+PLAN_SUFFIX = '/global_plan'
+self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+```
+
+2. **Add state dicts and subscribed set:**
+```python
+self._global_plans    = {}   # robot_name -> latest nav_msgs/Path
+self._subscribed_plan = set()
+```
+
+3. **Discover and subscribe in `_discover_robots`:**
+```python
+if topic not in self._subscribed_plan:
+    m = self._plan_pattern.match(topic)
+    if m and 'nav_msgs/msg/Path' in type_list:
+        name = m.group(1)
+        self.create_subscription(
+            Path, topic,
+            lambda msg, n=name: self._plan_callback(msg, n),
+            10,   # use default RELIABLE QoS for planning topics
+                  # use SENSOR_QOS for high-rate sensor streams
+        )
+        self._subscribed_plan.add(topic)
+```
+
+4. **Add callback:**
+```python
+def _plan_callback(self, msg: Path, robot_name: str):
+    self._global_plans[robot_name] = msg
+```
+
+5. **Render in `_publish_markers` (skip silently if not yet received):**
+```python
+plan = self._global_plans.get(robot_name)
+if plan is not None and boot is not None:
+    line = Marker()
+    line.header.frame_id = 'map'
+    line.ns = f'{robot_name}_global_plan'
+    line.type = Marker.LINE_STRIP
+    ...
+    for pose_stamped in plan.poses:
+        p = pose_stamped.pose.position
+        line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
+    array.markers.append(line)
+```
+
+**QoS guidance:**
+- High-rate sensor/visualization streams (odom, trajectory_vis): use `SENSOR_QOS` (BEST_EFFORT)
+- Infrequently-published planning topics (global_plan): use `10` (default RELIABLE)
+
+---
+
+## Step 3 — Verify
+
+```bash
+# Check topic is crossing the domain bridge
+docker exec airstack-robot-desktop-1 bash -c "ros2 topic echo /robot_1/your_topic --once"
+
+# Check GCS is receiving it
+docker exec airstack-gcs-1 bash -c "ros2 topic echo /robot_1/your_topic --once"
+
+# Check GCS node subscribed (look for log line)
+docker logs airstack-gcs-1 2>&1 | grep "Subscribed to"
+
+# Check the combined marker output
+docker exec airstack-gcs-1 bash -c "ros2 topic echo /gcs/robot_markers --once"
+```
+
+---
+
+## Common Pitfalls
+
+| Symptom | Cause | Fix |
+|---------|-------|-----|
+| Topic visible on robot, not on GCS | Not in dds_router allowlist | Add `rt/$(env ROBOT_NAME)/topic` to allowlist |
+| Topic on GCS but not in Foxglove | Not subscribed in robot_marker_node or Foxglove panel missing | Add subscription or add panel |
+| Marker appears at wrong position | Missing boot GPS offset | Apply `bx, by, bz` from `_gps_boot` to all points |
+| Marker double-offset | Added boot to both `pose.position` AND `points` | Only offset `points` for LINE_STRIP/ARROW markers |
+| Planning topic missed after late publish | Using BEST_EFFORT QoS | Use `10` (RELIABLE) for planning topics |
+| New robot not discovered | Topic appeared before discovery timer fired | Discovery runs every 5s; wait or trigger manually |
diff --git a/.env b/.env
index 3ca9b121..e2947f19 100644
--- a/.env
+++ b/.env
@@ -45,6 +45,7 @@ MS_AIRSIM_BINARY_PATH="/ms-airsim-env/AbandonedPark/LinuxNoEditor/AbandonedPark.
 
 # ================= ROBOT =====================
 ROBOT_NAME_MAP_CONFIG_FILE="default_robot_name_map.yaml"  # Determines how to set ROBOT_NAME and ROS_DOMAIN_ID. See robot/docker/robot_name_map/
+VDB_SOURCE="lidar"  # "lidar" (Ouster, sensors/ouster/point_cloud) or "stereo" (stereo_image_proc/point_cloud)
 
 URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"
 
diff --git a/AGENTS.md b/AGENTS.md
index c3852bfa..11d6c245 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -83,6 +83,7 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc
 | [add-task-executor](.agents/skills/add-task-executor) | Implementing a task executor as a ROS 2 action server |
 | [integrate-module-into-layer](.agents/skills/integrate-module-into-layer) | Adding module to layer bringup |
 | [write-isaac-sim-scene](.agents/skills/write-isaac-sim-scene) | Creating custom simulation scenes |
+| [visualize-in-foxglove](.agents/skills/visualize-in-foxglove) | Adding topic visualization to Foxglove/GCS |
 | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules |
 | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs |
 | [test-in-simulation](.agents/skills/test-in-simulation) | End-to-end simulation testing |
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
index 2c77e258..07d158bb 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
@@ -1,3 +1,4 @@
+import copy
 import math
 import re
 
@@ -5,7 +6,7 @@
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
 from sensor_msgs.msg import NavSatFix
-from nav_msgs.msg import Odometry
+from nav_msgs.msg import Odometry, Path
 from visualization_msgs.msg import Marker, MarkerArray
 from builtin_interfaces.msg import Duration
 from geometry_msgs.msg import TransformStamped, Point
@@ -23,8 +24,10 @@
     depth=10,
 )
 
-GPS_SUFFIX = '/interface/mavros/global_position/global'
+GPS_SUFFIX  = '/interface/mavros/global_position/global'
 ODOM_SUFFIX = '/odometry_conversion/odometry'
+TRAJ_SUFFIX = '/trajectory_controller/trajectory_vis'
+PLAN_SUFFIX = '/global_plan'
 
 ROBOT_COLORS = [
     (1.0, 0.2, 0.2),  # red
@@ -60,8 +63,6 @@ def rotate_vector(v, q):
     """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
     vx, vy, vz = v
     qx, qy, qz, qw = q
-    # v' = q * pure_quat(v) * q_conj
-    # Using optimised formula: v' = v + 2*qw*(q×v) + 2*(q×(q×v))
     cx = qy * vz - qz * vy
     cy = qz * vx - qx * vz
     cz = qx * vy - qy * vx
@@ -78,22 +79,24 @@ def __init__(self):
 
         self.declare_parameter('robot_name_prefix', 'robot')
         self._prefix = self.get_parameter('robot_name_prefix').value
-        self._gps_pattern = re.compile(
-            rf'^/({re.escape(self._prefix)}_\w+){re.escape(GPS_SUFFIX)}$'
-        )
-        self._odom_pattern = re.compile(
-            rf'^/({re.escape(self._prefix)}_\w+){re.escape(ODOM_SUFFIX)}$'
-        )
-
-        self._gps_positions = {}    # robot_name -> (x, y, z) ENU metres
-        self._orientations = {}     # robot_name -> (x, y, z, w) from odometry
-        self._subscribed_gps = set()
-        self._subscribed_odom = set()
-        self._alt_ground = None     # altitude of first fix, used as z=0 reference
+        self._gps_pattern  = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(GPS_SUFFIX)}$')
+        self._odom_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(ODOM_SUFFIX)}$')
+        self._traj_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(TRAJ_SUFFIX)}$')
+        self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+
+        self._gps_positions    = {}   # robot_name -> (x, y, z) ENU metres current position
+        self._gps_boot         = {}   # robot_name -> (x, y, z) ENU metres at first fix (odom origin)
+        self._orientations     = {}   # robot_name -> (x, y, z, w) from odometry
+        self._trajectories     = {}   # robot_name -> latest MarkerArray
+        self._global_plans     = {}   # robot_name -> latest nav_msgs/Path
+        self._subscribed_gps   = set()
+        self._subscribed_odom  = set()
+        self._subscribed_traj  = set()
+        self._subscribed_plan  = set()
+        self._alt_ground       = None
 
         self._pub = self.create_publisher(MarkerArray, '/gcs/robot_markers', 10)
 
-        # Broadcast map as root TF frame so Foxglove 3D panel has a display frame
         self._static_tf = StaticTransformBroadcaster(self)
         map_tf = TransformStamped()
         map_tf.header.stamp = self.get_clock().now().to_msg()
@@ -132,17 +135,50 @@ def _discover_robots(self):
                     self._subscribed_odom.add(topic)
                     self.get_logger().info(f'Subscribed to odom: {topic}')
 
+            if topic not in self._subscribed_traj:
+                m = self._traj_pattern.match(topic)
+                if m and 'visualization_msgs/msg/MarkerArray' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        MarkerArray, topic,
+                        lambda msg, n=name: self._traj_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_traj.add(topic)
+                    self.get_logger().info(f'Subscribed to trajectory_vis: {topic}')
+
+            if topic not in self._subscribed_plan:
+                m = self._plan_pattern.match(topic)
+                if m and 'nav_msgs/msg/Path' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        Path, topic,
+                        lambda msg, n=name: self._plan_callback(msg, n),
+                        10,
+                    )
+                    self._subscribed_plan.add(topic)
+                    self.get_logger().info(f'Subscribed to global_plan: {topic}')
+
     def _gps_callback(self, msg: NavSatFix, robot_name: str):
         if msg.status.status < 0:
             return
         if self._alt_ground is None:
             self._alt_ground = msg.altitude
-        self._gps_positions[robot_name] = gps_to_enu(msg.latitude, msg.longitude, msg.altitude, self._alt_ground)
+        pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude, self._alt_ground)
+        self._gps_positions[robot_name] = pos
+        if robot_name not in self._gps_boot:
+            self._gps_boot[robot_name] = pos
 
     def _odom_callback(self, msg: Odometry, robot_name: str):
         o = msg.pose.pose.orientation
         self._orientations[robot_name] = (o.x, o.y, o.z, o.w)
 
+    def _traj_callback(self, msg: MarkerArray, robot_name: str):
+        self._trajectories[robot_name] = msg
+
+    def _plan_callback(self, msg: Path, robot_name: str):
+        self._global_plans[robot_name] = msg
+
     def _publish_markers(self):
         if not self._gps_positions:
             return
@@ -152,7 +188,6 @@ def _publish_markers(self):
         lifetime = Duration(sec=1, nanosec=0)
 
         for i, robot_name in enumerate(sorted(self._gps_positions.keys())):
-            color = ROBOT_COLORS[i % len(ROBOT_COLORS)]
             x, y, z = self._gps_positions[robot_name]
             orientation = self._orientations.get(robot_name)  # (x,y,z,w) or None
 
@@ -161,7 +196,7 @@ def _publish_markers(self):
             mesh.header.frame_id = 'map'
             mesh.header.stamp = now
             mesh.ns = 'robot_meshes'
-            mesh.id = i * 3
+            mesh.id = i * 10
             mesh.type = Marker.MESH_RESOURCE
             mesh.action = Marker.ADD
             mesh.mesh_resource = 'package://robot_descriptions/iris/meshes/base_link_body_body.obj'
@@ -189,7 +224,7 @@ def _publish_markers(self):
             label.header.frame_id = 'map'
             label.header.stamp = now
             label.ns = 'robot_labels'
-            label.id = i * 3 + 1
+            label.id = i * 10 + 1
             label.type = Marker.TEXT_VIEW_FACING
             label.action = Marker.ADD
             label.pose.position.x = x
@@ -204,9 +239,9 @@ def _publish_markers(self):
 
             # --- Axes markers (X=red, Y=green, Z=blue) ---
             axes = [
-                ((1.0, 0.0, 0.0), (1.0, 0.0, 0.0)),  # X axis, red
-                ((0.0, 1.0, 0.0), (0.0, 1.0, 0.0)),  # Y axis, green
-                ((0.0, 0.0, 1.0), (0.0, 0.0, 1.0)),  # Z axis, blue
+                ((1.0, 0.0, 0.0), (1.0, 0.0, 0.0)),
+                ((0.0, 1.0, 0.0), (0.0, 1.0, 0.0)),
+                ((0.0, 0.0, 1.0), (0.0, 0.0, 1.0)),
             ]
             q = orientation if orientation else (0.0, 0.0, 0.0, 1.0)
             axis_len = 0.6
@@ -216,7 +251,7 @@ def _publish_markers(self):
                 arrow.header.frame_id = 'map'
                 arrow.header.stamp = now
                 arrow.ns = f'{robot_name}_axes'
-                arrow.id = i * 3 + 2 + j  # unique id per axis per robot (offset past mesh+label)
+                arrow.id = i * 10 + 2 + j
                 arrow.type = Marker.ARROW
                 arrow.action = Marker.ADD
                 start = Point(x=x, y=y, z=z)
@@ -224,9 +259,9 @@ def _publish_markers(self):
                             y=y + tip[1] * axis_len,
                             z=z + tip[2] * axis_len)
                 arrow.points = [start, end]
-                arrow.scale.x = 0.04  # shaft diameter
-                arrow.scale.y = 0.08  # head diameter
-                arrow.scale.z = 0.0   # head length (0 = auto)
+                arrow.scale.x = 0.04
+                arrow.scale.y = 0.08
+                arrow.scale.z = 0.0
                 arrow.color.r = axis_color[0]
                 arrow.color.g = axis_color[1]
                 arrow.color.b = axis_color[2]
@@ -234,6 +269,48 @@ def _publish_markers(self):
                 arrow.lifetime = lifetime
                 array.markers.append(arrow)
 
+            # --- Trajectory markers (offset by boot GPS position = odom origin, skip if not ready) ---
+            traj = self._trajectories.get(robot_name)
+            boot = self._gps_boot.get(robot_name)
+            if traj is not None and boot is not None:
+                bx, by, bz = boot
+                for k, orig in enumerate(traj.markers):
+                    m = copy.deepcopy(orig)
+                    m.header.frame_id = 'map'
+                    m.header.stamp = now
+                    m.ns = f'{robot_name}_traj'
+                    m.id = i * 10000 + k
+                    m.lifetime = lifetime
+                    for pt in m.points:
+                        pt.x += bx
+                        pt.y += by
+                        pt.z += bz
+                    array.markers.append(m)
+
+            # --- Global plan (offset from odom origin to ENU, same as trajectory) ---
+            plan = self._global_plans.get(robot_name)
+            if plan is not None and boot is not None:
+                bx, by, bz = boot
+                color = ROBOT_COLORS[i % len(ROBOT_COLORS)]
+                line = Marker()
+                line.header.frame_id = 'map'
+                line.header.stamp = now
+                line.ns = f'{robot_name}_global_plan'
+                line.id = i * 10000 + 9999
+                line.type = Marker.LINE_STRIP
+                line.action = Marker.ADD
+                line.pose.orientation.w = 1.0
+                line.scale.x = 0.1
+                line.color.r = color[0]
+                line.color.g = color[1]
+                line.color.b = color[2]
+                line.color.a = 0.8
+                line.lifetime = Duration(sec=2, nanosec=0)
+                for pose_stamped in plan.poses:
+                    p = pose_stamped.pose.position
+                    line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
+                array.markers.append(line)
+
         self._pub.publish(array)
 
 
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index abfef1cb..de5de536 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -24,6 +24,7 @@ services:
       - LAUNCH_PACKAGE=desktop_bringup  # desktop_bringup adds RViz; real robots use autonomy_bringup
       - AUTONOMY_ROLE=full
       - SIM_IP=${SIM_IP:-172.31.0.200}
+      - VDB_SOURCE=${VDB_SOURCE:-lidar}  # "lidar" or "stereo", set in .env
       # FCU_URL and TGT_SYSTEM not set, dynamically calculated in interface.launch.py
     # 'command' uses variables so that it can be shared across robot-desktop and robot-l4t, with different launch packages and roles.
     command: >
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index bc01a4ef..73a70ed4 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -24,6 +24,8 @@ allowlist:
   # state information
   - name: "rt/$(env ROBOT_NAME)/odometry_conversion/odometry"
   - name: "rt/$(env ROBOT_NAME)/interface/mavros/global_position/global"
+  - name: "rt/$(env ROBOT_NAME)/trajectory_controller/trajectory_vis"
+  - name: "rt/$(env ROBOT_NAME)/global_plan"
 
   # behavior tree
   - name: "rt/$(env ROBOT_NAME)/behavior/behavior_tree_commands"
diff --git a/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml b/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml
index 41275589..5071d182 100644
--- a/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml
+++ b/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml
@@ -46,5 +46,3 @@
         x: 10.0
         y: 10.0
         z: 10.0
-
-
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 490570ff..45d5973d 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -16,5 +16,5 @@
         <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
         <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node>
-    
+
 </launch>
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
index ef084ab3..988cecd9 100644
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -218,12 +218,12 @@ def __init__(self):
 
             add_ouster_lidar_subgraph(
                 parent_graph_handle=graph_handle,
-                drone_prim=f"/World/drone{i}/base_link",
+                drone_prim=f"/World/drone{i}/abase_link",
                 robot_name=f"robot_{i}",
                 lidar_name="OS1_REV6_128_10hz___512_resolution",
                 lidar_offset=[0.0, 0.0, 0.025],
                 lidar_rotation_offset=[0.0, 0.0, 0.0],
-                lidar_min_range=2.0,
+                lidar_min_range=3.0,
             )
 
         # Reset so physics/articulations are ready

From 9fdf0455f3933d612e6e27eadf709d92900d700a Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sun, 22 Mar 2026 16:17:02 -0400
Subject: [PATCH 14/55] added image, vdb markers(not transformed yet)

---
 .../desktop_bringup/launch/gcs.launch.xml        |  1 +
 ...arker_node.py => foxglove_visualizer_node.py} | 16 ++++++++++------
 .../launch/gcs_visualizer.launch.xml             |  2 +-
 gcs/ros_ws/src/gcs_visualizer/setup.py           |  2 +-
 .../onboard_all/config/dds_router.yaml           | 13 +++++++++++++
 .../global/global_bringup/config/vdb_params.yaml |  2 ++
 .../global_bringup/launch/global.launch.xml      |  2 +-
 7 files changed, 29 insertions(+), 9 deletions(-)
 rename gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/{robot_marker_node.py => foxglove_visualizer_node.py} (97%)

diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index ee09563f..9e286ad9 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -24,6 +24,7 @@
   <node pkg="foxglove_bridge" exec="foxglove_bridge" output="screen" respawn="true" respawn_delay="1">
     <param name="port" value="8765" />
     <param name="use_sim_time" value="$(var sim)" />
+    <param name="send_buffer_limit" value="10000000" />
   </node>
   ?>
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
similarity index 97%
rename from gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
rename to gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 07d158bb..8df71833 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/robot_marker_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -73,9 +73,9 @@ def rotate_vector(v, q):
     )
 
 
-class RobotMarkerNode(Node):
+class FoxgloveVisualizerNode(Node):
     def __init__(self):
-        super().__init__('robot_marker_node')
+        super().__init__('foxglove_visualizer_node')
 
         self.declare_parameter('robot_name_prefix', 'robot')
         self._prefix = self.get_parameter('robot_name_prefix').value
@@ -212,9 +212,9 @@ def _publish_markers(self):
             mesh.pose.orientation.z = qz
             mesh.pose.orientation.w = qw
             mesh.scale.x = mesh.scale.y = mesh.scale.z = 1.0
-            mesh.color.r = 0.0
-            mesh.color.g = 0.0
-            mesh.color.b = 0.0
+            mesh.color.r = 0.6
+            mesh.color.g = 0.6
+            mesh.color.b = 0.6
             mesh.color.a = 1.0
             mesh.lifetime = lifetime
             array.markers.append(mesh)
@@ -285,6 +285,10 @@ def _publish_markers(self):
                         pt.x += bx
                         pt.y += by
                         pt.z += bz
+                    if m.color.a > 0:
+                        m.color.r = 0.8
+                        m.color.g = 0.5
+                        m.color.b = 0.0
                     array.markers.append(m)
 
             # --- Global plan (offset from odom origin to ENU, same as trajectory) ---
@@ -316,7 +320,7 @@ def _publish_markers(self):
 
 def main(args=None):
     rclpy.init(args=args)
-    node = RobotMarkerNode()
+    node = FoxgloveVisualizerNode()
     try:
         rclpy.spin(node)
     except KeyboardInterrupt:
diff --git a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
index 9930c773..4957e391 100644
--- a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
+++ b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
@@ -1,7 +1,7 @@
 <launch>
   <arg name="robot_name_prefix" default="robot" />
 
-  <node pkg="gcs_visualizer" exec="robot_marker_node" name="robot_marker_node" output="screen">
+  <node pkg="gcs_visualizer" exec="foxglove_visualizer_node" name="foxglove_visualizer_node" output="screen">
     <param name="robot_name_prefix" value="$(var robot_name_prefix)" />
   </node>
 </launch>
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.py b/gcs/ros_ws/src/gcs_visualizer/setup.py
index 8b189726..55b055f6 100644
--- a/gcs/ros_ws/src/gcs_visualizer/setup.py
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.py
@@ -22,7 +22,7 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'robot_marker_node = gcs_visualizer.robot_marker_node:main',
+            'foxglove_visualizer_node = gcs_visualizer.foxglove_visualizer_node:main',
         ],
     },
 )
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index 73a70ed4..bd17a09c 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -21,6 +21,19 @@ participants:
     domain: $(var gcs_domain)
 
 allowlist:
+  # TF (needed for Foxglove to transform point clouds between frames)
+  - name: "rt/tf"
+  - name: "rt/tf_static"
+
+  # lidar and world model
+  - name: "rt/$(env ROBOT_NAME)/sensors/ouster/point_cloud"
+  - name: "rt/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization"
+
+  # camera streams
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/left/image_rect"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/image_rect"
+  - name: "rt/$(env ROBOT_NAME)/perception/stereo_image_proc/point_cloud"
+
   # state information
   - name: "rt/$(env ROBOT_NAME)/odometry_conversion/odometry"
   - name: "rt/$(env ROBOT_NAME)/interface/mavros/global_position/global"
diff --git a/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml b/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml
index 5071d182..41275589 100644
--- a/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml
+++ b/robot/ros_ws/src/global/global_bringup/config/vdb_params.yaml
@@ -46,3 +46,5 @@
         x: 10.0
         y: 10.0
         z: 10.0
+
+
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 45d5973d..490570ff 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -16,5 +16,5 @@
         <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
         <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
     </node>
-
+    
 </launch>

From 22322124d3afe85fde396ac30e85eff6b97f5cb1 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sun, 22 Mar 2026 16:32:17 -0400
Subject: [PATCH 15/55] fixed state estimation flickering and vdb transform

---
 .../foxglove_visualizer_node.py               | 35 +++++++++++++++++++
 .../onboard_all/config/dds_router.yaml        |  4 ---
 2 files changed, 35 insertions(+), 4 deletions(-)

diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 8df71833..3e33d859 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -28,6 +28,7 @@
 ODOM_SUFFIX = '/odometry_conversion/odometry'
 TRAJ_SUFFIX = '/trajectory_controller/trajectory_vis'
 PLAN_SUFFIX = '/global_plan'
+VDB_SUFFIX  = '/vdb_mapping/vdb_map_visualization'
 
 ROBOT_COLORS = [
     (1.0, 0.2, 0.2),  # red
@@ -83,16 +84,19 @@ def __init__(self):
         self._odom_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(ODOM_SUFFIX)}$')
         self._traj_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(TRAJ_SUFFIX)}$')
         self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+        self._vdb_pattern  = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(VDB_SUFFIX)}$')
 
         self._gps_positions    = {}   # robot_name -> (x, y, z) ENU metres current position
         self._gps_boot         = {}   # robot_name -> (x, y, z) ENU metres at first fix (odom origin)
         self._orientations     = {}   # robot_name -> (x, y, z, w) from odometry
         self._trajectories     = {}   # robot_name -> latest MarkerArray
         self._global_plans     = {}   # robot_name -> latest nav_msgs/Path
+        self._vdb_markers      = {}   # robot_name -> latest VDB Marker
         self._subscribed_gps   = set()
         self._subscribed_odom  = set()
         self._subscribed_traj  = set()
         self._subscribed_plan  = set()
+        self._subscribed_vdb   = set()
         self._alt_ground       = None
 
         self._pub = self.create_publisher(MarkerArray, '/gcs/robot_markers', 10)
@@ -159,6 +163,18 @@ def _discover_robots(self):
                     self._subscribed_plan.add(topic)
                     self.get_logger().info(f'Subscribed to global_plan: {topic}')
 
+            if topic not in self._subscribed_vdb:
+                m = self._vdb_pattern.match(topic)
+                if m and 'visualization_msgs/msg/Marker' in type_list:
+                    name = m.group(1)
+                    self.create_subscription(
+                        Marker, topic,
+                        lambda msg, n=name: self._vdb_callback(msg, n),
+                        SENSOR_QOS,
+                    )
+                    self._subscribed_vdb.add(topic)
+                    self.get_logger().info(f'Subscribed to vdb_map_visualization: {topic}')
+
     def _gps_callback(self, msg: NavSatFix, robot_name: str):
         if msg.status.status < 0:
             return
@@ -179,6 +195,9 @@ def _traj_callback(self, msg: MarkerArray, robot_name: str):
     def _plan_callback(self, msg: Path, robot_name: str):
         self._global_plans[robot_name] = msg
 
+    def _vdb_callback(self, msg: Marker, robot_name: str):
+        self._vdb_markers[robot_name] = msg
+
     def _publish_markers(self):
         if not self._gps_positions:
             return
@@ -315,6 +334,22 @@ def _publish_markers(self):
                     line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
                 array.markers.append(line)
 
+            # --- VDB map (offset from odom origin to ENU) ---
+            vdb = self._vdb_markers.get(robot_name)
+            if vdb is not None and boot is not None:
+                bx, by, bz = boot
+                m = copy.deepcopy(vdb)
+                m.header.frame_id = 'map'
+                m.header.stamp = now
+                m.ns = f'{robot_name}_vdb'
+                m.id = i * 10000 + 9998
+                m.lifetime = Duration(sec=2, nanosec=0)
+                for pt in m.points:
+                    pt.x += bx
+                    pt.y += by
+                    pt.z += bz
+                array.markers.append(m)
+
         self._pub.publish(array)
 
 
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index bd17a09c..a04a886e 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -21,10 +21,6 @@ participants:
     domain: $(var gcs_domain)
 
 allowlist:
-  # TF (needed for Foxglove to transform point clouds between frames)
-  - name: "rt/tf"
-  - name: "rt/tf_static"
-
   # lidar and world model
   - name: "rt/$(env ROBOT_NAME)/sensors/ouster/point_cloud"
   - name: "rt/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization"

From 66f044a9bceeec79218e6228579ac6ea0904836d Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sun, 22 Mar 2026 18:07:11 -0400
Subject: [PATCH 16/55] added custom foxglove buttons for commands

---
 gcs/docker/gcs-base-docker-compose.yaml       |   3 +
 gcs/foxglove_extensions/install.sh            |  15 ++
 gcs/foxglove_extensions/robot-commands.foxe   | Bin 0 -> 2010 bytes
 .../robot-commands/dist/extension.js          | 167 ++++++++++++++++++
 .../robot-commands/package.json               |   9 +
 .../robot-commands/robot-commands.foxe        | Bin 0 -> 2006 bytes
 .../robot-commands/robot-commands.foxe.bak    | Bin 0 -> 3355 bytes
 7 files changed, 194 insertions(+)
 create mode 100644 gcs/foxglove_extensions/install.sh
 create mode 100644 gcs/foxglove_extensions/robot-commands.foxe
 create mode 100644 gcs/foxglove_extensions/robot-commands/dist/extension.js
 create mode 100644 gcs/foxglove_extensions/robot-commands/package.json
 create mode 100644 gcs/foxglove_extensions/robot-commands/robot-commands.foxe
 create mode 100644 gcs/foxglove_extensions/robot-commands/robot-commands.foxe.bak

diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index c1ec4b3f..26b7830f 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -10,6 +10,7 @@ services:
     command: >
       bash -c "
       ssh service restart;
+      python3 /root/AirStack/gcs/foxglove_extensions/install.sh;
       tmux new -d -s bringup;
       if [ $$AUTOLAUNCH = 'true' ]; then
         tmux send-keys -t bringup:0.0 'bws &&  sws; ros2 launch desktop_bringup gcs.launch.xml' ENTER;
@@ -50,6 +51,8 @@ services:
       - ../../.devcontainer/gcs/launch.json:/root/AirStack/.vscode/launch.json:rw
       - ../../.devcontainer/gcs/tasks.json:/root/AirStack/.vscode/tasks.json:rw
       - ./Foxglove:/root/.config/Foxglove:rw
+      # foxglove extensions
+      - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
       - ../../common/ros_packages:/root/AirStack/gcs/ros_ws/src/common:rw
       - ../../common/fastdds.xml:/root/AirStack/gcs/ros_ws/src/fastdds.xml
diff --git a/gcs/foxglove_extensions/install.sh b/gcs/foxglove_extensions/install.sh
new file mode 100644
index 00000000..fdeb6edb
--- /dev/null
+++ b/gcs/foxglove_extensions/install.sh
@@ -0,0 +1,15 @@
+#!/usr/bin/env python3
+import json, os, shutil
+
+src = '/root/AirStack/gcs/foxglove_extensions'
+dst = '/root/.foxglove-studio/extensions'
+os.makedirs(dst, exist_ok=True)
+
+for ext in os.listdir(src):
+    pkg_path = os.path.join(src, ext, 'package.json')
+    if not os.path.exists(pkg_path):
+        continue
+    pkg = json.load(open(pkg_path))
+    name = '{}.{}-{}'.format(pkg['publisher'], pkg['name'], pkg['version'])
+    shutil.copytree(os.path.join(src, ext), os.path.join(dst, name), dirs_exist_ok=True)
+    print('Installed Foxglove extension:', name)
diff --git a/gcs/foxglove_extensions/robot-commands.foxe b/gcs/foxglove_extensions/robot-commands.foxe
new file mode 100644
index 0000000000000000000000000000000000000000..f14869ba22ccea2efe5bcc24fb94a4f55c0ee6d8
GIT binary patch
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
new file mode 100644
index 00000000..1dfa6208
--- /dev/null
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -0,0 +1,167 @@
+(()=>{
+"use strict";
+
+const BT_COMMANDS_DATATYPES = new Map([
+  [
+    "behavior_tree_msgs/msg/BehaviorTreeCommands",
+    {
+      definitions: [
+        {
+          name: "commands",
+          type: "behavior_tree_msgs/msg/BehaviorTreeCommand",
+          isArray: true,
+          isComplex: true,
+        },
+      ],
+    },
+  ],
+  [
+    "behavior_tree_msgs/msg/BehaviorTreeCommand",
+    {
+      definitions: [
+        { name: "condition_name", type: "string", isArray: false, isComplex: false },
+        { name: "status", type: "int8", isArray: false, isComplex: false },
+      ],
+    },
+  ],
+]);
+
+const PRESET_COMMANDS = [
+  { label: "Arm & Takeoff",        value: "Auto Takeoff Commanded" },
+  { label: "Global Plan",          value: "Global Plan Commanded" },
+  { label: "Fixed Trajectory",     value: "Fixed Trajectory Commanded" },
+  { label: "Autonomously Explore", value: "Autonomously Explore Commanded" },
+  { label: "Pause",                value: "Pause Commanded" },
+  { label: "Rewind",               value: "Rewind Commanded" },
+  { label: "Land",                 value: "Land Commanded" },
+  { label: "Disarm",               value: "Disarm Commanded" },
+  { label: "Custom",               value: "__custom__" },
+];
+
+const DEFAULT_STATE = {
+  robot: "robot_1",
+  command: "Auto Takeoff Commanded",
+  custom_command: "",
+  button_label: "Arm & Takeoff",
+  button_color: "#10B981",
+};
+
+function getConditionName(state) {
+  return state.command === "__custom__" ? state.custom_command : state.command;
+}
+
+function activate(extensionContext) {
+  extensionContext.registerPanel({
+    name: "Robot Command Button",
+    initPanel: (panelContext) => {
+      let state = Object.assign({}, DEFAULT_STATE, panelContext.initialState ?? {});
+      let advertisedTopic = null;
+
+      const root = panelContext.panelElement;
+      root.style.cssText =
+        "display:flex;align-items:center;justify-content:center;height:100%;padding:12px;box-sizing:border-box;";
+
+      const btn = document.createElement("button");
+      btn.style.cssText =
+        "width:100%;height:100%;min-height:48px;border:none;border-radius:6px;" +
+        "font-size:16px;font-weight:bold;color:white;cursor:pointer;transition:opacity 0.1s;";
+      btn.addEventListener("mousedown", () => (btn.style.opacity = "0.75"));
+      btn.addEventListener("mouseup", () => (btn.style.opacity = "1"));
+      btn.addEventListener("mouseleave", () => (btn.style.opacity = "1"));
+      btn.addEventListener("click", handleClick);
+      root.appendChild(btn);
+
+      function render() {
+        btn.textContent = state.button_label || "Button";
+        btn.style.backgroundColor = state.button_color || "#10B981";
+        panelContext.setDefaultPanelTitle(state.button_label || "Robot Command Button");
+      }
+
+      function handleClick() {
+        const conditionName = getConditionName(state);
+        if (!conditionName) return;
+        const topic = `/${state.robot}/behavior/behavior_tree_commands`;
+
+        if (advertisedTopic !== topic) {
+          if (advertisedTopic != null) {
+            panelContext.unadvertise(advertisedTopic);
+          }
+          panelContext.advertise(topic, "behavior_tree_msgs/msg/BehaviorTreeCommands", {
+            datatypes: BT_COMMANDS_DATATYPES,
+          });
+          advertisedTopic = topic;
+        }
+
+        panelContext.publish(topic, {
+          commands: [{ condition_name: conditionName, status: 2 }],
+        });
+      }
+
+      function updateSettings() {
+        const isCustom = state.command === "__custom__";
+        const fields = {
+          button_label: {
+            label: "Label",
+            input: "string",
+            value: state.button_label,
+          },
+          button_color: {
+            label: "Color",
+            input: "rgb",
+            value: state.button_color,
+          },
+          robot: {
+            label: "Robot Name",
+            input: "string",
+            value: state.robot,
+            placeholder: "robot_1",
+          },
+          command: {
+            label: "Command",
+            input: "select",
+            value: state.command,
+            options: PRESET_COMMANDS,
+          },
+        };
+
+        if (isCustom) {
+          fields.custom_command = {
+            label: "Custom Command Name",
+            input: "string",
+            value: state.custom_command,
+            placeholder: "e.g. My Custom Commanded",
+          };
+        }
+
+        panelContext.updatePanelSettingsEditor({
+          actionHandler: (action) => {
+            if (action.action !== "update") return;
+            const key = action.payload.path[1];
+            state[key] = action.payload.value;
+            panelContext.saveState(state);
+            render();
+            updateSettings();
+          },
+          nodes: {
+            general: {
+              label: "Button",
+              fields,
+            },
+          },
+        });
+      }
+
+      render();
+      updateSettings();
+
+      return () => {
+        if (advertisedTopic != null) {
+          panelContext.unadvertise(advertisedTopic);
+        }
+      };
+    },
+  });
+}
+
+module.exports = { activate };
+})();
diff --git a/gcs/foxglove_extensions/robot-commands/package.json b/gcs/foxglove_extensions/robot-commands/package.json
new file mode 100644
index 00000000..ea84a6c2
--- /dev/null
+++ b/gcs/foxglove_extensions/robot-commands/package.json
@@ -0,0 +1,9 @@
+{
+  "name": "robot-commands",
+  "displayName": "Robot Command Button",
+  "description": "Configurable button that publishes a behavior_tree_msgs/BehaviorTreeCommands condition to a robot.",
+  "publisher": "AirLab CMU",
+  "version": "1.0.0",
+  "license": "Apache-2.0",
+  "main": "./dist/extension.js"
+}
diff --git a/gcs/foxglove_extensions/robot-commands/robot-commands.foxe b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe
new file mode 100644
index 0000000000000000000000000000000000000000..858ec6a58156576fdedee391a1b73a78588a4382
GIT binary patch
diff --git a/gcs/foxglove_extensions/robot-commands/robot-commands.foxe.bak b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe.bak
new file mode 100644
index 0000000000000000000000000000000000000000..fc818d4640f396891ae11c051591cb8d08011fd4
GIT binary patch

From 7e344f10e58af392f47202379a1be1b940ce15b3 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Sun, 22 Mar 2026 22:21:53 -0400
Subject: [PATCH 17/55] added modular payloads to peerprofile, foxglove reads
 the payloads and vizualizes it,currently works for rayfronts

---
 .agents/skills/attach-gossip-payload/SKILL.md | 105 +++++++++
 AGENTS.md                                     |   1 +
 gcs/docker/gcs-base-docker-compose.yaml       |   1 +
 .../robot-commands/dist/extension.js          |  17 +-
 .../foxglove_visualizer_node.py               | 116 ++++------
 .../gcs_visualizer/gcs_utils.py               | 135 +++++++++++
 .../gcs_visualizer/payload_visualizer_node.py | 215 ++++++++++++++++++
 .../launch/gcs_visualizer.launch.xml          |   5 +
 gcs/ros_ws/src/gcs_visualizer/setup.py        |   1 +
 .../onboard_all/config/dds_router.yaml        |   3 +
 .../config/gossip_payloads.yaml               |  42 ++++
 .../coordination_bringup/gossip_node.py       |  87 ++++++-
 .../coordination_bringup/peer_profile.py      |  31 ++-
 .../launch/gossip.launch.xml                  |   5 +-
 .../coordination_bringup/setup.py             |   2 +-
 .../msg/PeerProfilePayload.msg                |   3 +
 16 files changed, 688 insertions(+), 81 deletions(-)
 create mode 100644 .agents/skills/attach-gossip-payload/SKILL.md
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml

diff --git a/.agents/skills/attach-gossip-payload/SKILL.md b/.agents/skills/attach-gossip-payload/SKILL.md
new file mode 100644
index 00000000..0cbaf873
--- /dev/null
+++ b/.agents/skills/attach-gossip-payload/SKILL.md
@@ -0,0 +1,105 @@
+# Skill: Attach Custom Payload to PeerProfile (Gossip Protocol)
+
+## When to use
+When you want to broadcast any ROS message to all peer robots via the gossip
+protocol — for example, a frontier map, sensor summary, or task status.
+
+## Background
+
+Each robot runs a `gossip_node` that periodically broadcasts a `PeerProfile`
+to all other robots on the gossip domain (default domain 99). The profile
+carries structured fields (GPS, heading, waypoint) plus an open-ended
+`payloads` array of serialized ROS messages.
+
+**Key files:**
+| File | Purpose |
+|------|---------|
+| `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml` | Lists topics to attach as payloads — **edit this to add payloads** |
+| `coordination_bringup/coordination_bringup/gossip_node.py` | Reads config, subscribes, attaches payloads on each publish tick |
+| `coordination_bringup/coordination_bringup/peer_profile.py` | `PeerProfile` helper class with `add_payload` / `get_payload` API |
+| `coordination_msgs/msg/PeerProfile.msg` | Wire format — `payloads` is `PeerProfilePayload[]` |
+| `coordination_msgs/msg/PeerProfilePayload.msg` | `string payload_type` + `uint8[] payload_data` |
+
+## How to add a payload (config-driven — no code changes)
+
+### Step 1 — Edit `gossip_payloads.yaml`
+
+```yaml
+payload_topics:
+  # existing entries ...
+
+  # Your new payload:
+  - topic: "/{robot_name}/your/topic"
+    type: "your_msgs/msg/YourType"
+```
+
+- `{robot_name}` is automatically substituted at runtime (e.g. → `/robot_1/your/topic`)
+- If the topic hasn't published yet, the payload is silently skipped — no crash
+- `type` must be the fully-qualified ROS 2 type string
+
+### Step 2 — Rebuild and restart gossip_node
+
+```bash
+bws --packages-select coordination_bringup
+ros2 launch coordination_bringup gossip.launch.xml
+```
+
+### Step 3 — Verify
+
+Check that the payload is being attached:
+```bash
+ros2 topic echo /gossip/peers --field payloads
+# should show entries with your payload_type string
+```
+
+Or use the registry monitor:
+```bash
+ros2 run coordination_bringup peer_registry_monitor
+# shows payload_types per peer
+```
+
+## How to read a payload on the receiving side
+
+```python
+from coordination_bringup.peer_profile import PeerProfile
+
+def on_peer_msg(self, msg):
+    profile = PeerProfile.from_ros_msg(msg)
+
+    # Get a specific payload by type string
+    rays = profile.get_payload("visualization_msgs/msg/MarkerArray")
+    if rays is not None:
+        # use rays as visualization_msgs/msg/MarkerArray
+        pass
+
+    # List all payload types present
+    print(profile.payload_types())
+
+    # Get all payloads deserialized
+    for payload in profile.get_all_payloads():
+        print(type(payload))
+```
+
+## Current payloads (Rayfronts example)
+
+| Topic | Type | Purpose |
+|-------|------|---------|
+| `/{robot_name}/filtered_rays` | `visualization_msgs/msg/MarkerArray` | Semantic filtered rays — peers avoid re-exploring these areas |
+| `/{robot_name}/frontier_viewpoints` | `sensor_msgs/msg/PointCloud2` | Candidate exploration targets |
+
+## Architecture notes
+
+- `gossip_node` runs on the **robot's domain** (e.g. domain 1 for `robot_1`)
+  and can subscribe directly to any topic on that domain, including Rayfronts
+- The gossip DDS Router bridges `/gossip/peers` to the shared gossip domain
+  (default 99) — the payload bytes travel inside the PeerProfile message,
+  so payload topics do **not** need their own DDS router entries
+- Payloads are re-serialized every publish tick from the latest cached message;
+  stale data is never cleared between ticks (latest-wins per topic)
+- Payload size matters for gossip bandwidth — avoid attaching large point clouds
+  at high rates; 1 Hz (the default gossip rate) is usually fine
+
+## QoS note
+
+Payload subscriptions use `GOSSIP_QOS` (BEST_EFFORT, KEEP_LAST 1). If your
+source topic uses RELIABLE QoS you may need to adjust — see `gossip_node.py`.
diff --git a/AGENTS.md b/AGENTS.md
index 11d6c245..0ec02b29 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -84,6 +84,7 @@ For detailed step-by-step instructions, refer to the **`.agents/skills/`** direc
 | [integrate-module-into-layer](.agents/skills/integrate-module-into-layer) | Adding module to layer bringup |
 | [write-isaac-sim-scene](.agents/skills/write-isaac-sim-scene) | Creating custom simulation scenes |
 | [visualize-in-foxglove](.agents/skills/visualize-in-foxglove) | Adding topic visualization to Foxglove/GCS |
+| [attach-gossip-payload](.agents/skills/attach-gossip-payload) | Broadcasting custom ROS messages to peers via PeerProfile gossip payloads |
 | [debug-module](.agents/skills/debug-module) | Autonomous debugging of ROS 2 modules |
 | [update-documentation](.agents/skills/update-documentation) | Documenting new modules and updating mkdocs |
 | [test-in-simulation](.agents/skills/test-in-simulation) | End-to-end simulation testing |
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 26b7830f..26485565 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -55,6 +55,7 @@ services:
       - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
       - ../../common/ros_packages:/root/AirStack/gcs/ros_ws/src/common:rw
+      - ../../robot/ros_ws/src/coordination:/root/AirStack/gcs/ros_ws/src/coordination:ro
       - ../../common/fastdds.xml:/root/AirStack/gcs/ros_ws/src/fastdds.xml
       - ../ros_ws:/root/AirStack/gcs/ros_ws:rw
       # bags
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index 1dfa6208..20e47441 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -92,9 +92,20 @@ function activate(extensionContext) {
           advertisedTopic = topic;
         }
 
-        panelContext.publish(topic, {
-          commands: [{ condition_name: conditionName, status: 2 }],
-        });
+        // Send 2 (SUCCESS) for the selected command, 0 (IDLE) for all others.
+        // Matches rqt behaviour exactly.
+        const commands = [];
+        for (const preset of PRESET_COMMANDS) {
+          if (preset.value === "__custom__") continue;
+          commands.push({
+            condition_name: preset.value,
+            status: preset.value === conditionName ? 2 : 0,
+          });
+        }
+        if (state.command === "__custom__" && conditionName) {
+          commands.push({ condition_name: conditionName, status: 2 });
+        }
+        panelContext.publish(topic, { commands });
       }
 
       function updateSettings() {
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 3e33d859..a634fafc 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -1,5 +1,23 @@
+"""
+foxglove_visualizer_node.py
+===========================
+Standard GCS visualization node. Handles data common to every project:
+  - Robot mesh markers (with orientation)
+  - Robot name labels
+  - Body-frame axes (X/Y/Z arrows)
+  - Local trajectory (trajectory_controller/trajectory_vis)
+  - Global plan (global_plan)
+  - VDB occupancy map (vdb_mapping/vdb_map_visualization)
+
+All markers are published to /gcs/robot_markers as a MarkerArray in the
+global ENU 'map' frame.
+
+Project-specific data (e.g. Rayfronts) should go in a separate node that
+also publishes to its own MarkerArray or PointCloud2 topic. See
+payload_visualizer_node.py as an example.
+"""
+
 import copy
-import math
 import re
 
 import rclpy
@@ -12,10 +30,9 @@
 from geometry_msgs.msg import TransformStamped, Point
 from tf2_ros import StaticTransformBroadcaster
 
-# Lisbon — matches Pegasus configs.yaml and gps_utils.py DEFAULT_WORLD_ORIGIN
-ORIGIN_LAT = 38.736832
-ORIGIN_LON = -9.137977
-ORIGIN_ALT = 90.0
+from gcs_visualizer.gcs_utils import (
+    gps_to_enu, multiply_quaternions, rotate_vector, transform_marker_array,
+)
 
 SENSOR_QOS = QoSProfile(
     reliability=ReliabilityPolicy.BEST_EFFORT,
@@ -40,40 +57,6 @@
 AXIS_CORRECTION = (-0.5, -0.5, 0.5, 0.5)  # x, y, z, w
 
 
-def gps_to_enu(lat, lon, alt, alt_ground):
-    """Convert GPS lat/lon/alt to ENU metres relative to Lisbon origin. z is relative to first fix."""
-    x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
-    y = (lat - ORIGIN_LAT) * 111320.0
-    z = alt - alt_ground
-    return x, y, z
-
-
-def multiply_quaternions(q1, q2):
-    """Hamilton product q1 * q2 (apply q2 first, then q1). Both are (x,y,z,w) tuples."""
-    x1, y1, z1, w1 = q1
-    x2, y2, z2, w2 = q2
-    return (
-        w1*x2 + x1*w2 + y1*z2 - z1*y2,
-        w1*y2 - x1*z2 + y1*w2 + z1*x2,
-        w1*z2 + x1*y2 - y1*x2 + z1*w2,
-        w1*w2 - x1*x2 - y1*y2 - z1*z2,
-    )
-
-
-def rotate_vector(v, q):
-    """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
-    vx, vy, vz = v
-    qx, qy, qz, qw = q
-    cx = qy * vz - qz * vy
-    cy = qz * vx - qx * vz
-    cz = qx * vy - qy * vx
-    return (
-        vx + 2.0 * (qw * cx + qy * cz - qz * cy),
-        vy + 2.0 * (qw * cy + qz * cx - qx * cz),
-        vz + 2.0 * (qw * cz + qx * cy - qy * cx),
-    )
-
-
 class FoxgloveVisualizerNode(Node):
     def __init__(self):
         super().__init__('foxglove_visualizer_node')
@@ -86,18 +69,18 @@ def __init__(self):
         self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
         self._vdb_pattern  = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(VDB_SUFFIX)}$')
 
-        self._gps_positions    = {}   # robot_name -> (x, y, z) ENU metres current position
-        self._gps_boot         = {}   # robot_name -> (x, y, z) ENU metres at first fix (odom origin)
-        self._orientations     = {}   # robot_name -> (x, y, z, w) from odometry
-        self._trajectories     = {}   # robot_name -> latest MarkerArray
-        self._global_plans     = {}   # robot_name -> latest nav_msgs/Path
-        self._vdb_markers      = {}   # robot_name -> latest VDB Marker
-        self._subscribed_gps   = set()
-        self._subscribed_odom  = set()
-        self._subscribed_traj  = set()
-        self._subscribed_plan  = set()
-        self._subscribed_vdb   = set()
-        self._alt_ground       = None
+        self._gps_positions  = {}   # robot_name -> (x, y, z) ENU metres current position
+        self._gps_boot       = {}   # robot_name -> (x, y, z) ENU metres at first fix (odom origin)
+        self._orientations   = {}   # robot_name -> (x, y, z, w) from odometry
+        self._trajectories   = {}   # robot_name -> latest MarkerArray
+        self._global_plans   = {}   # robot_name -> latest nav_msgs/Path
+        self._vdb_markers    = {}   # robot_name -> latest VDB Marker
+        self._subscribed_gps  = set()
+        self._subscribed_odom = set()
+        self._subscribed_traj = set()
+        self._subscribed_plan = set()
+        self._subscribed_vdb  = set()
+        self._alt_ground      = None
 
         self._pub = self.create_publisher(MarkerArray, '/gcs/robot_markers', 10)
 
@@ -288,29 +271,29 @@ def _publish_markers(self):
                 arrow.lifetime = lifetime
                 array.markers.append(arrow)
 
-            # --- Trajectory markers (offset by boot GPS position = odom origin, skip if not ready) ---
-            traj = self._trajectories.get(robot_name)
             boot = self._gps_boot.get(robot_name)
+
+            # --- Trajectory markers (offset by boot GPS position = odom origin) ---
+            traj = self._trajectories.get(robot_name)
             if traj is not None and boot is not None:
                 bx, by, bz = boot
-                for k, orig in enumerate(traj.markers):
-                    m = copy.deepcopy(orig)
-                    m.header.frame_id = 'map'
-                    m.header.stamp = now
-                    m.ns = f'{robot_name}_traj'
-                    m.id = i * 10000 + k
-                    m.lifetime = lifetime
-                    for pt in m.points:
-                        pt.x += bx
-                        pt.y += by
-                        pt.z += bz
+                for m in transform_marker_array(
+                        traj, bx, by, bz,
+                        ns=f'{robot_name}_traj',
+                        id_base=i * 10000,
+                        stamp=now, lifetime=lifetime):
                     if m.color.a > 0:
                         m.color.r = 0.8
                         m.color.g = 0.5
                         m.color.b = 0.0
+                    # skip markers that require points but have none
+                    needs_points = m.type in (Marker.LINE_STRIP, Marker.LINE_LIST,
+                                              Marker.POINTS, Marker.ARROW)
+                    if needs_points and len(m.points) == 0:
+                        continue
                     array.markers.append(m)
 
-            # --- Global plan (offset from odom origin to ENU, same as trajectory) ---
+            # --- Global plan (offset from odom origin to ENU) ---
             plan = self._global_plans.get(robot_name)
             if plan is not None and boot is not None:
                 bx, by, bz = boot
@@ -332,7 +315,8 @@ def _publish_markers(self):
                 for pose_stamped in plan.poses:
                     p = pose_stamped.pose.position
                     line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
-                array.markers.append(line)
+                if len(line.points) >= 2:
+                    array.markers.append(line)
 
             # --- VDB map (offset from odom origin to ENU) ---
             vdb = self._vdb_markers.get(robot_name)
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
new file mode 100644
index 00000000..27500576
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
@@ -0,0 +1,135 @@
+"""
+gcs_utils.py
+============
+Shared utilities for GCS visualizer nodes.
+
+Coordinate system
+-----------------
+All visualizers transform data from each robot's local odometry frame into
+a common ENU (East-North-Up) frame anchored at the first GPS fix seen on the
+ground (``alt_ground``). This ENU frame is published as the ``map`` frame in
+Foxglove.
+
+The key concept is the **boot offset**: the ENU coordinates of a robot's first
+GPS fix, which equals that robot's odometry frame origin. Any data expressed in
+the robot's local odometry frame must be translated by this boot offset to
+display correctly alongside other robots in the global map frame.
+"""
+
+import math
+import copy
+import struct
+
+# ── Origin ────────────────────────────────────────────────────────────────────
+# Matches Pegasus configs.yaml and gps_utils.py DEFAULT_WORLD_ORIGIN (Lisbon).
+# Update this if your simulation/field site uses a different origin.
+ORIGIN_LAT = 38.736832
+ORIGIN_LON = -9.137977
+ORIGIN_ALT = 90.0
+
+
+# ── GPS / coordinate helpers ──────────────────────────────────────────────────
+
+def gps_to_enu(lat, lon, alt, alt_ground):
+    """Convert GPS lat/lon/alt to ENU metres relative to the world origin.
+
+    z is relative to ``alt_ground`` (the altitude of the first GPS fix seen).
+    """
+    x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
+    y = (lat - ORIGIN_LAT) * 111320.0
+    z = alt - alt_ground
+    return x, y, z
+
+
+def multiply_quaternions(q1, q2):
+    """Hamilton product q1 * q2 (apply q2 first, then q1). Both are (x,y,z,w)."""
+    x1, y1, z1, w1 = q1
+    x2, y2, z2, w2 = q2
+    return (
+        w1*x2 + x1*w2 + y1*z2 - z1*y2,
+        w1*y2 - x1*z2 + y1*w2 + z1*x2,
+        w1*z2 + x1*y2 - y1*x2 + z1*w2,
+        w1*w2 - x1*x2 - y1*y2 - z1*z2,
+    )
+
+
+def rotate_vector(v, q):
+    """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
+    vx, vy, vz = v
+    qx, qy, qz, qw = q
+    cx = qy * vz - qz * vy
+    cy = qz * vx - qx * vz
+    cz = qx * vy - qy * vx
+    return (
+        vx + 2.0 * (qw * cx + qy * cz - qz * cy),
+        vy + 2.0 * (qw * cy + qz * cx - qx * cz),
+        vz + 2.0 * (qw * cz + qx * cy - qy * cx),
+    )
+
+
+# ── Marker / PointCloud2 transform helpers ────────────────────────────────────
+
+def transform_marker_array(marker_array, bx, by, bz, ns, id_base, stamp, lifetime):
+    """Deep-copy a MarkerArray and translate all point coordinates by (bx, by, bz).
+
+    Sets frame_id='map', the given namespace, sequential IDs starting at
+    ``id_base``, and the provided stamp and lifetime on every marker.
+
+    Only translates ``points[]`` — does NOT touch ``pose.position``, which is
+    intentional: LINE_STRIP and ARROW markers encode geometry in ``points``.
+    Returns a list of Marker messages.
+    """
+    from geometry_msgs.msg import Point
+
+    result = []
+    for k, orig in enumerate(marker_array.markers):
+        m = copy.deepcopy(orig)
+        m.header.frame_id = 'map'
+        m.header.stamp = stamp
+        m.ns = ns
+        m.id = id_base + k
+        m.lifetime = lifetime
+        for pt in m.points:
+            pt.x += bx
+            pt.y += by
+            pt.z += bz
+        result.append(m)
+    return result
+
+
+def transform_point_cloud2(cloud, bx, by, bz):
+    """Return a copy of a PointCloud2 with all x,y,z values offset by (bx, by, bz).
+
+    Assumes the cloud has float32 x, y, z fields (standard ROS convention).
+    Reads field offsets from the message so non-standard field orderings work.
+    Sets frame_id='map' on the returned message.
+    """
+    # Find byte offsets for x, y, z fields
+    field_offsets = {}
+    for f in cloud.fields:
+        if f.name in ('x', 'y', 'z'):
+            field_offsets[f.name] = f.offset
+
+    if not all(k in field_offsets for k in ('x', 'y', 'z')):
+        return cloud  # can't transform without x,y,z fields
+
+    ox = field_offsets['x']
+    oy = field_offsets['y']
+    oz = field_offsets['z']
+
+    new_cloud = copy.copy(cloud)
+    new_cloud.header = copy.copy(cloud.header)
+    new_cloud.header.frame_id = 'map'
+    data = bytearray(cloud.data)
+    n_points = cloud.width * cloud.height
+    ps = cloud.point_step
+    for i in range(n_points):
+        base = i * ps
+        x, = struct.unpack_from('<f', data, base + ox)
+        y, = struct.unpack_from('<f', data, base + oy)
+        z, = struct.unpack_from('<f', data, base + oz)
+        struct.pack_into('<f', data, base + ox, x + bx)
+        struct.pack_into('<f', data, base + oy, y + by)
+        struct.pack_into('<f', data, base + oz, z + bz)
+    new_cloud.data = bytes(data)
+    return new_cloud
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
new file mode 100644
index 00000000..561a7e53
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -0,0 +1,215 @@
+"""
+payload_visualizer_node.py
+==========================
+Project-specific GCS visualization node. Reads PeerProfile payloads from
+/gossip/peers, applies coordinate frame translation, and publishes for Foxglove.
+
+Payloads are defined in gossip_payloads.yaml on the robot side — no DDS router
+changes are needed when adding new payload types.
+
+How to adapt for your project
+------------------------------
+1. On the robot: add entries to gossip_payloads.yaml (topic + ROS type).
+2. Here: add a handler method and register it in PAYLOAD_HANDLERS.
+
+Handler signature:
+    def _handle_<name>(self, robot_name, msg, boot, i, now, array):
+        # msg   — deserialized ROS message
+        # boot  — (bx, by, bz) ENU translation from robot odom frame to global map
+        # i     — stable robot index (use for marker IDs to avoid collisions)
+        # now   — current ROS timestamp
+        # array — MarkerArray to append Marker messages to
+        # For non-Marker outputs (e.g. PointCloud2), publish a separate topic directly.
+
+Coordinate frame note
+---------------------
+Payload data is in each robot's local odometry frame (origin = robot's first
+GPS fix in ENU). Apply the boot offset to translate into the global 'map' frame.
+
+Use transform_marker_array() for MarkerArray payloads.
+Use transform_point_cloud2() for PointCloud2 payloads.
+Both helpers are in gcs_utils.py.
+"""
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
+
+import struct
+
+from sensor_msgs.msg import NavSatFix
+from visualization_msgs.msg import MarkerArray, Marker
+from geometry_msgs.msg import Point
+from std_msgs.msg import ColorRGBA
+from builtin_interfaces.msg import Duration
+
+from coordination_msgs.msg import PeerProfile as PeerProfileMsg
+from coordination_bringup.peer_profile import PeerProfile
+
+from gcs_visualizer.gcs_utils import gps_to_enu, transform_marker_array
+
+GOSSIP_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    durability=DurabilityPolicy.VOLATILE,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+GPS_SUFFIX = '/interface/mavros/global_position/global'
+
+
+class PayloadVisualizerNode(Node):
+    def __init__(self):
+        super().__init__('payload_visualizer_node')
+
+        self._gps_boot      = {}   # robot_name -> (bx, by, bz) ENU at first GPS fix
+        self._alt_ground    = None
+        self._payload_cache = {}   # (robot_name, type_str) -> last deserialized payload msg
+
+        self._markers_pub = self.create_publisher(
+            MarkerArray, '/gcs/payload_markers', 10)
+
+        # Subscribe to /gossip/peers — PeerProfiles arrive here from all robots
+        self.create_subscription(
+            PeerProfileMsg, '/gossip/peers',
+            self._on_peer_profile, GOSSIP_QOS)
+
+        self.get_logger().info('PayloadVisualizerNode started')
+
+    # ── Colour palette for per-robot point clouds ─────────────────────────────
+
+    _ROBOT_COLORS = [
+        (0.0, 1.0, 1.0, 1.0),   # cyan
+        (1.0, 0.5, 0.0, 1.0),   # orange
+        (0.5, 1.0, 0.0, 1.0),   # lime
+        (1.0, 0.0, 0.5, 1.0),   # pink
+        (0.5, 0.0, 1.0, 1.0),   # purple
+    ]
+
+    # ── Boot offset tracking ───────────────────────────────────────────────
+
+    def _update_boot_from_gps(self, robot_name: str, gps_fix) -> None:
+        """Track the first GPS fix seen per robot as the boot (odom origin) offset."""
+        if gps_fix.status.status < 0:
+            return
+        if self._alt_ground is None:
+            self._alt_ground = gps_fix.altitude
+        if robot_name not in self._gps_boot:
+            pos = gps_to_enu(
+                gps_fix.latitude, gps_fix.longitude,
+                gps_fix.altitude, self._alt_ground)
+            self._gps_boot[robot_name] = pos
+
+    # ── Peer profile callback ─────────────────────────────────────────────
+
+    def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
+        robot_name = msg.robot_name
+        self._update_boot_from_gps(robot_name, msg.gps_fix)
+
+        boot = self._gps_boot.get(robot_name)
+        if boot is None:
+            return  # no GPS fix yet, skip
+
+        profile = PeerProfile.from_ros_msg(msg)
+        now = self.get_clock().now().to_msg()
+        array = MarkerArray()
+
+        robot_index = self._robot_index(robot_name)
+
+        for type_str, handler in self.PAYLOAD_HANDLERS.items():
+            payload = profile.get_payload(type_str)
+            if payload is not None:
+                self._payload_cache[(robot_name, type_str)] = payload
+            cached = self._payload_cache.get((robot_name, type_str))
+            if cached is not None:
+                handler(self, robot_name, cached, boot, robot_index, now, array)
+
+        if array.markers:
+            self._markers_pub.publish(array)
+
+    def _robot_index(self, robot_name: str) -> int:
+        """Stable integer index for a robot name (alphabetical order)."""
+        known = sorted(self._gps_boot.keys())
+        return known.index(robot_name) if robot_name in known else 0
+
+    # ── Payload handlers ──────────────────────────────────────────────────
+    # Each handler receives:
+    #   robot_name  – e.g. "robot_1"
+    #   msg         – deserialized ROS message
+    #   boot        – (bx, by, bz) ENU translation for this robot's odom frame
+    #   i           – stable robot index (for marker IDs)
+    #   now         – current ROS timestamp
+    #   array       – MarkerArray to append Marker messages to
+    #
+    # For non-marker outputs (e.g. PointCloud2), publish directly from here.
+
+    def _handle_filtered_rays(self, robot_name, msg, boot, i, now, array):
+        """Translate filtered_rays MarkerArray from odom frame to ENU map frame."""
+        bx, by, bz = boot
+        lifetime = Duration(sec=2, nanosec=0)
+        markers = transform_marker_array(
+            msg, bx, by, bz,
+            ns=f'{robot_name}_filtered_rays',
+            id_base=i * 100000,
+            stamp=now, lifetime=lifetime)
+        array.markers.extend(markers)
+
+    def _handle_frontier_viewpoints(self, robot_name, msg, boot, i, now, array):
+        """Convert frontier_viewpoints PointCloud2 to a POINTS Marker in the map frame."""
+        bx, by, bz = boot
+        r, g, b, a = self._ROBOT_COLORS[i % len(self._ROBOT_COLORS)]
+
+        # Extract xyz from the PointCloud2 and apply boot offset
+        field_offsets = {f.name: f.offset for f in msg.fields if f.name in ('x', 'y', 'z')}
+        if not all(k in field_offsets for k in ('x', 'y', 'z')):
+            return
+        ox, oy, oz = field_offsets['x'], field_offsets['y'], field_offsets['z']
+        ps = msg.point_step
+        n_points = msg.width * msg.height
+        data = bytes(msg.data)
+
+        m = Marker()
+        m.header.frame_id = 'map'
+        m.header.stamp = now
+        m.ns = f'{robot_name}_frontier_viewpoints'
+        m.id = i * 100000 + 90000
+        m.type = Marker.SPHERE_LIST
+        m.action = Marker.ADD
+        m.scale.x = 1.0
+        m.scale.y = 1.0
+        m.scale.z = 1.0
+        m.lifetime = Duration(sec=2, nanosec=0)
+        m.color = ColorRGBA(r=r, g=g, b=b, a=a)
+
+        for idx in range(n_points):
+            base = idx * ps
+            x, = struct.unpack_from('<f', data, base + ox)
+            y, = struct.unpack_from('<f', data, base + oy)
+            z, = struct.unpack_from('<f', data, base + oz)
+            m.points.append(Point(x=x + bx, y=y + by, z=z + bz))
+
+        array.markers.append(m)
+
+    # ── Handler registry ──────────────────────────────────────────────────
+    # Maps ROS type string -> handler method.
+    # Add or remove entries here to change what gets visualized.
+    PAYLOAD_HANDLERS = {
+        'visualization_msgs/msg/MarkerArray': _handle_filtered_rays,
+        'sensor_msgs/msg/PointCloud2':        _handle_frontier_viewpoints,
+    }
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PayloadVisualizerNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
index 4957e391..d8f6478d 100644
--- a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
+++ b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
@@ -1,7 +1,12 @@
 <launch>
   <arg name="robot_name_prefix" default="robot" />
 
+  <!-- Standard visualization: mesh, labels, axes, trajectory, global plan, VDB -->
   <node pkg="gcs_visualizer" exec="foxglove_visualizer_node" name="foxglove_visualizer_node" output="screen">
     <param name="robot_name_prefix" value="$(var robot_name_prefix)" />
   </node>
+
+  <!-- Payload visualization: project-specific data from PeerProfile gossip payloads -->
+  <!-- Remove or replace this node for projects that don't use Rayfronts -->
+  <node pkg="gcs_visualizer" exec="payload_visualizer_node" name="payload_visualizer_node" output="screen" />
 </launch>
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.py b/gcs/ros_ws/src/gcs_visualizer/setup.py
index 55b055f6..5d89b2bb 100644
--- a/gcs/ros_ws/src/gcs_visualizer/setup.py
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.py
@@ -23,6 +23,7 @@
     entry_points={
         'console_scripts': [
             'foxglove_visualizer_node = gcs_visualizer.foxglove_visualizer_node:main',
+            'payload_visualizer_node = gcs_visualizer.payload_visualizer_node:main',
         ],
     },
 )
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index a04a886e..776be121 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -65,3 +65,6 @@ allowlist:
 
   # control commands
   - name: "rt/$(env ROBOT_NAME)/fixed_trajectory_generator/fixed_trajectory_command"
+
+  # gossip peer profiles (carries all robot payloads — no need to bridge individual topics)
+  - name: "rt/gossip/peers"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml b/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml
new file mode 100644
index 00000000..32134c30
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml
@@ -0,0 +1,42 @@
+# ─────────────────────────────────────────────────────────────────────────────
+# Gossip Payload Topics
+# ─────────────────────────────────────────────────────────────────────────────
+# List ROS topics here to have them automatically included as typed payloads
+# in this robot's PeerProfile and distributed to all peers via gossip.
+#
+# Each entry requires:
+#   topic: topic name — use {robot_name} as a placeholder for the robot namespace
+#   type:  fully-qualified ROS message type (package/msg/Type)
+#
+# If a topic has never published, the payload is silently skipped — the node
+# will not crash or block waiting for it.
+#
+# ── How to add your own payload ──────────────────────────────────────────────
+# 1. Add an entry below with the topic + type.
+# 2. On the receiving end, call peer_profile.get_payload("<type>") to get the
+#    deserialized message, or peer_profile.payload_types() to list what's there.
+#
+# Example (receiving side in Python):
+#   from coordination_bringup.peer_profile import PeerProfile
+#   profile = PeerProfile.from_ros_msg(peer_msg)
+#   rays = profile.get_payload("visualization_msgs/msg/MarkerArray")
+#   if rays is not None:
+#       # use rays as a visualization_msgs/msg/MarkerArray
+# ─────────────────────────────────────────────────────────────────────────────
+
+payload_topics:
+
+  # ── Rayfronts ──────────────────────────────────────────────────────────────
+  # Filtered semantic rays — peer robots consume this as filtered_rays/transposed
+  # to avoid revisiting already-explored areas.
+  - topic: "/{robot_name}/filtered_rays"
+    type: "visualization_msgs/msg/MarkerArray"
+
+  # Frontier viewpoints — candidate exploration targets computed by Rayfronts.
+  # Peers can use these to select non-overlapping goals.
+  - topic: "/{robot_name}/frontier_viewpoints"
+    type: "sensor_msgs/msg/PointCloud2"
+
+  # ── Add more payloads below ────────────────────────────────────────────────
+  # - topic: "/{robot_name}/your/topic"
+  #   type: "your_msgs/msg/YourType"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
index d979e099..d0951a0d 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -17,6 +17,7 @@
 """
 
 import os
+import yaml
 
 import rclpy
 from rclpy.node import Node
@@ -27,6 +28,7 @@
     QoSProfile,
     QoSReliabilityPolicy,
 )
+import rosidl_runtime_py.utilities as rosidl_utils
 
 from nav_msgs.msg import Path
 from sensor_msgs.msg import NavSatFix
@@ -38,9 +40,16 @@
 
 # QoS for gossip topic: best-effort, keep last 1 (low-overhead, high-freq)
 GOSSIP_QOS = QoSProfile(
+    reliability=QoSReliabilityPolicy.RELIABLE,
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=10,
+)
+
+# MAVROS and other sensor topics publish as BEST_EFFORT
+SENSOR_QOS = QoSProfile(
     reliability=QoSReliabilityPolicy.BEST_EFFORT,
     history=QoSHistoryPolicy.KEEP_LAST,
-    depth=1,
+    depth=10,
 )
 
 # QoS for the registry topic: reliable + transient-local so late subscribers
@@ -61,9 +70,11 @@ def __init__(self):
         # ── Parameters ──────────────────────────────────────────────────
         self.declare_parameter("robot_name", os.environ.get("ROBOT_NAME", "unknown_robot"))
         self.declare_parameter("publish_rate", 1.0)   # Hz — wall-clock timer
+        self.declare_parameter("payload_topics_config", "")  # path to gossip_payloads.yaml
 
         self._robot_name = self.get_parameter("robot_name").get_parameter_value().string_value
         publish_rate = self.get_parameter("publish_rate").get_parameter_value().double_value
+        payload_config_path = self.get_parameter("payload_topics_config").get_parameter_value().string_value
 
         # ── State ────────────────────────────────────────────────────────
         self._profile = PeerProfile(robot_name=self._robot_name)
@@ -71,24 +82,32 @@ def __init__(self):
         # Registry: robot_name → latest PeerProfile ROS msg
         self._registry: dict[str, PeerProfileMsg] = {}
 
+        # Payload cache: topic → latest message (None until first message arrives)
+        self._payload_cache: dict[str, object] = {}
+        self._payload_subs: list = []
+
+        # ── Payload topic subscriptions (config-driven) ──────────────────
+        if payload_config_path:
+            self._setup_payload_subscriptions(payload_config_path)
+
         # ── Subscriptions ────────────────────────────────────────────────
         self._navsat_sub = self.create_subscription(
             NavSatFix,
             f"/{self._robot_name}/interface/mavros/global_position/raw/fix",
             self._on_navsat,
-            GOSSIP_QOS,
+            SENSOR_QOS,
         )
         self._compass_sub = self.create_subscription(
             Float64,
             f"/{self._robot_name}/interface/mavros/global_position/compass_hdg",
             self._on_compass,
-            GOSSIP_QOS,
+            SENSOR_QOS,
         )
         self._path_sub = self.create_subscription(
             Path,
             f"/{self._robot_name}/random_walk/global_plan",
             self._on_global_plan,
-            GOSSIP_QOS,
+            SENSOR_QOS,
         )
         self._peer_sub = self.create_subscription(
             PeerProfileMsg,
@@ -122,9 +141,59 @@ def __init__(self):
 
         self.get_logger().info(
             f"GossipNode started for '{self._robot_name}' "
-            f"(publish_rate={publish_rate:.1f} Hz wall-clock)"
+            f"(publish_rate={publish_rate:.1f} Hz wall-clock, "
+            f"{len(self._payload_cache)} payload topic(s))"
         )
 
+    # ------------------------------------------------------------------ #
+    # Config-driven payload subscriptions                                  #
+    # ------------------------------------------------------------------ #
+
+    def _setup_payload_subscriptions(self, config_path: str) -> None:
+        """
+        Parse gossip_payloads.yaml and create a subscription for each entry.
+
+        Each subscription caches the latest message in self._payload_cache.
+        Messages are only added to the profile at publish time, so a topic
+        that hasn't published yet simply contributes no payload.
+        """
+        try:
+            with open(config_path, "r") as f:
+                cfg = yaml.safe_load(f)
+        except Exception as e:
+            self.get_logger().warn(f"Could not load payload config '{config_path}': {e}")
+            return
+
+        for entry in cfg.get("payload_topics", []):
+            topic_template = entry.get("topic", "")
+            type_str = entry.get("type", "")
+            if not topic_template or not type_str:
+                continue
+
+            topic = topic_template.replace("{robot_name}", self._robot_name)
+
+            try:
+                msg_class = rosidl_utils.get_message(type_str)
+            except Exception as e:
+                self.get_logger().warn(f"Unknown payload type '{type_str}': {e}")
+                continue
+
+            # Cache stores (msg, stamp) tuples. stamp is extracted from the
+            # message header if present, otherwise the current ROS clock time.
+            self._payload_cache[topic] = None  # None = not received yet
+
+            def _make_callback(t):
+                def cb(msg):
+                    stamp = getattr(getattr(msg, 'header', None), 'stamp', None)
+                    if stamp is None:
+                        stamp = self.get_clock().now().to_msg()
+                    self._payload_cache[t] = (msg, stamp)
+                return cb
+
+            sub = self.create_subscription(msg_class, topic, _make_callback(topic), SENSOR_QOS)
+            self._payload_subs.append(sub)
+            self.get_logger().info(f"Payload subscription: {topic} ({type_str})")
+
     # ------------------------------------------------------------------ #
     # Subscription callbacks                                               #
     # ------------------------------------------------------------------ #
@@ -152,6 +221,14 @@ def _publish_tick(self) -> None:
         self._publish_own()
 
     def _publish_own(self) -> None:
+        # Rebuild payloads from cache on every tick. The cache retains the last
+        # received message per topic indefinitely, so peers always get the most
+        # recent known data even if the source publishes slower than gossip rate.
+        self._profile.clear_payloads()
+        for topic, entry in self._payload_cache.items():
+            if entry is not None:
+                msg, stamp = entry
+                self._profile.add_payload(msg, stamp=stamp)
         self._gossip_pub.publish(self._profile.to_ros_msg())
 
     # ------------------------------------------------------------------ #
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
index b589f37c..9e854b8f 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
@@ -101,12 +101,19 @@ def has_waypoint(self) -> bool:
     # Payload management                                                   #
     # ------------------------------------------------------------------ #
 
-    def add_payload(self, msg: Any) -> None:
-        """Serialize and attach any ROS message as an additional payload."""
+    def add_payload(self, msg: Any, stamp=None) -> None:
+        """Serialize and attach any ROS message as an additional payload.
+
+        ``stamp`` is a builtin_interfaces/Time (or None to use zero time).
+        Pass the header stamp of the source message so receivers can judge
+        how fresh the data is independently of the gossip message timestamp.
+        """
+        from builtin_interfaces.msg import Time
         type_str = _ros_type_string(msg)
         self._payloads.append({
             "type": type_str,
             "data": serialize_message(msg),
+            "stamp": stamp if stamp is not None else Time(),
         })
 
     def clear_payloads(self) -> None:
@@ -125,6 +132,18 @@ def get_payload(self, payload_type: str) -> Optional[Any]:
                 return deserialize_message(p["data"], msg_class)
         return None
 
+    def get_payload_with_stamp(self, payload_type: str):
+        """
+        Like get_payload() but returns (msg, stamp) tuple so the caller can
+        check when the source data was last produced.
+        Returns (None, None) if not found.
+        """
+        for p in self._payloads:
+            if p["type"] == payload_type:
+                msg_class = rosidl_utils.get_message(payload_type)
+                return deserialize_message(p["data"], msg_class), p.get("stamp")
+        return None, None
+
     def get_all_payloads(self) -> List[Any]:
         """Deserialize and return all payloads in order."""
         result = []
@@ -150,7 +169,11 @@ def to_ros_msg(self) -> PeerProfileMsg:
         msg.source = int(self.source)
         msg.relay_hops = self.relay_hops
         msg.payloads = [
-            PeerProfilePayloadMsg(payload_type=p["type"], payload_data=list(p["data"]))
+            PeerProfilePayloadMsg(
+                stamp=p.get("stamp") or PeerProfilePayloadMsg().stamp,
+                payload_type=p["type"],
+                payload_data=list(p["data"]),
+            )
             for p in self._payloads
         ]
         return msg
@@ -164,7 +187,7 @@ def from_ros_msg(cls, msg: PeerProfileMsg) -> "PeerProfile":
         profile.source = Source(msg.source)
         profile.relay_hops = msg.relay_hops
         profile._payloads = [
-            {"type": p.payload_type, "data": bytes(p.payload_data)}
+            {"type": p.payload_type, "data": bytes(p.payload_data), "stamp": p.stamp}
             for p in msg.payloads
         ]
         return profile
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml b/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
index f405c4be..c293be26 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
+++ b/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
@@ -15,7 +15,8 @@
 
   <!-- Gossip node -->
   <node pkg="coordination_bringup" exec="gossip_node" name="gossip_node" output="screen">
-    <param name="robot_name"   value="$(env ROBOT_NAME)" />
-    <param name="publish_rate" value="$(var publish_rate)" />
+    <param name="robot_name"            value="$(env ROBOT_NAME)" />
+    <param name="publish_rate"          value="$(var publish_rate)" />
+    <param name="payload_topics_config" value="$(find-pkg-share coordination_bringup)/config/gossip_payloads.yaml" />
   </node>
 </launch>
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/setup.py b/robot/ros_ws/src/coordination/coordination_bringup/setup.py
index aa8c7c93..381b0fe0 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/setup.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/setup.py
@@ -11,7 +11,7 @@
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/launch', ['launch/gossip.launch.xml']),
-        ('share/' + package_name + '/config', ['config/gossip_dds_router.yaml']),
+        ('share/' + package_name + '/config', ['config/gossip_dds_router.yaml', 'config/gossip_payloads.yaml']),
         ('lib/' + package_name, ['scripts/gossip_node', 'scripts/peer_registry_monitor.py']),
     ],
     install_requires=['setuptools'],
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
index 4f0b909b..02ba1f31 100644
--- a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
+++ b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
@@ -2,5 +2,8 @@
 # payload_type holds the fully-qualified ROS message type string,
 # e.g. "nav_msgs/msg/OccupancyGrid". payload_data holds the
 # serialized bytes produced by rclpy.serialization.serialize_message().
+# stamp is the time the source topic was last received — receivers can use
+# this to detect stale payloads independently of the gossip message timestamp.
+builtin_interfaces/Time stamp
 string  payload_type
 uint8[] payload_data

From ae062e5191be8668cddadaf782b15a7aa15bf359 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Wed, 25 Mar 2026 17:20:18 -0400
Subject: [PATCH 18/55] fixing the rotation of payload

---
 .agents/skills/attach-gossip-payload/SKILL.md |  97 +++++-
 .../gcs_visualizer/gcs_utils.py               |  61 ++++
 .../gcs_visualizer/payload_visualizer_node.py | 158 ++++-----
 .../onboard_all/config/dds_router.yaml        |   1 -
 .../config/gossip_payloads.yaml               |   3 +
 .../launch_scripts/two_drone_fire.py          | 303 ++++++++++++++++++
 6 files changed, 526 insertions(+), 97 deletions(-)
 create mode 100644 simulation/isaac-sim/launch_scripts/two_drone_fire.py

diff --git a/.agents/skills/attach-gossip-payload/SKILL.md b/.agents/skills/attach-gossip-payload/SKILL.md
index 0cbaf873..351f8a9f 100644
--- a/.agents/skills/attach-gossip-payload/SKILL.md
+++ b/.agents/skills/attach-gossip-payload/SKILL.md
@@ -80,12 +80,99 @@ def on_peer_msg(self, msg):
         print(type(payload))
 ```
 
-## Current payloads (Rayfronts example)
+## Step 4 — Add GCS visualization
 
-| Topic | Type | Purpose |
-|-------|------|---------|
-| `/{robot_name}/filtered_rays` | `visualization_msgs/msg/MarkerArray` | Semantic filtered rays — peers avoid re-exploring these areas |
-| `/{robot_name}/frontier_viewpoints` | `sensor_msgs/msg/PointCloud2` | Candidate exploration targets |
+After adding a payload to `gossip_payloads.yaml`, add a handler so it appears in
+Foxglove. Each payload is published to its own topic:
+`/gcs/payload/{robot_name}/{payload_name}`
+
+This means Foxglove exposes full visualization controls (point size, color mapping,
+marker type, etc.) for each payload independently.
+
+**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py`
+
+### 4a — Read the payload type from `gossip_payloads.yaml`
+
+Open `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml`
+and note the `type:` field for your new entry. This determines how to deserialize it.
+
+If your type is **unique** (not already in `PAYLOAD_HANDLERS`), go to step 4b.
+If your type **already exists** (e.g. a second `sensor_msgs/msg/PointCloud2`), go to step 4c.
+
+### 4b — Unique type: add to `PAYLOAD_HANDLERS`
+
+Add a handler and register it:
+
+```python
+PAYLOAD_HANDLERS = {
+    'visualization_msgs/msg/MarkerArray': _handle_filtered_rays,
+    'sensor_msgs/msg/PointCloud2':        _handle_frontier_viewpoints,
+    'your_msgs/msg/YourType':             _handle_your_payload,   # ← add
+}
+```
+
+Handler signature — all handlers must match exactly:
+```python
+def _handle_your_payload(self, robot_name, msg, boot, i, now):
+    # msg  — deserialized ROS message
+    # boot — (bx, by, bz) ENU offset; add to all positions to go odom→map frame
+    # i    — stable robot index (use for marker IDs: i * 100000 + unique_offset)
+    # now  — current ROS timestamp (builtin_interfaces/Time)
+    bx, by, bz = boot
+    # transform and publish to the payload's dedicated topic:
+    self._pub_for(f'/gcs/payload/{robot_name}/your_name', YourMsgType).publish(out)
+```
+
+### 4c — Duplicate type: dispatch by index in `_on_peer_profile`
+
+`PAYLOAD_HANDLERS` is a dict and cannot hold duplicate keys. After the
+`PAYLOAD_HANDLERS` loop in `_on_peer_profile`, access payloads by their position
+among same-type entries — order matches `gossip_payloads.yaml`:
+
+```python
+# Example: 2nd sensor_msgs/msg/PointCloud2 (index 1)
+pc2_list = [p for p in profile._payloads if p["type"] == "sensor_msgs/msg/PointCloud2"]
+if len(pc2_list) >= 2:
+    msg = deserialize_message(bytes(pc2_list[1]["data"]), PointCloud2)
+    self._payload_cache[(robot_name, 'my_cache_key')] = msg
+cached = self._payload_cache.get((robot_name, 'my_cache_key'))
+if cached is not None:
+    self._handle_my_payload(robot_name, cached, boot, robot_index, now)
+```
+
+### 4d — Available transform helpers (`gcs_utils.py`)
+
+Check `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py` before writing
+transform logic. Add a new helper there if none fits.
+
+| Helper | Use for |
+|--------|---------|
+| `transform_marker_array(ma, bx, by, bz, ns, id_base, stamp, lifetime)` | `MarkerArray` → translated `MarkerArray` |
+| `transform_point_cloud2(cloud, bx, by, bz)` | `PointCloud2` → translated `PointCloud2` (preserves all fields including `rgb`) |
+
+Publish raw `PointCloud2` when you want Foxglove's viz controls (point size, color
+mapping, etc.). Publish `MarkerArray` when the shape/color is fixed by the publisher.
+
+### 4e — Rebuild GCS
+
+```bash
+docker exec airstack-gcs-1 bash -c "bws --packages-select gcs_visualizer && sws"
+```
+
+Verify topics exist:
+```bash
+docker exec airstack-gcs-1 bash -c "ros2 topic list | grep /gcs/payload"
+```
+
+---
+
+## Current payloads
+
+| Topic in `gossip_payloads.yaml` | Type | GCS topic | Foxglove controls |
+|--------------------------------|------|-----------|-------------------|
+| `/{robot_name}/filtered_rays` | `visualization_msgs/msg/MarkerArray` | `/gcs/payload/{robot}/filtered_rays` | Fixed (MarkerArray) |
+| `/{robot_name}/frontier_viewpoints` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/frontier_viewpoints` | Full (raw PointCloud2) |
+| `/{robot_name}/rayfronts/voxel_rgb` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/rgb_voxels` | Full (raw PointCloud2 with RGB) |
 
 ## Architecture notes
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
index 27500576..5321c0cb 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
@@ -97,6 +97,67 @@ def transform_marker_array(marker_array, bx, by, bz, ns, id_base, stamp, lifetim
     return result
 
 
+def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetime,
+                                fallback_color=None, scale=0.2):
+    """Convert a PointCloud2 to a CUBE_LIST Marker translated into the map frame.
+
+    Extracts x, y, z fields and applies the boot offset (bx, by, bz).
+    If the cloud has an ``rgb`` field (PCL-packed uint32: 0x00RRGGBB stored as
+    float32 bytes), per-point colors are written to ``colors[]``.
+    Otherwise ``fallback_color`` (r, g, b, a) is used as a uniform color.
+
+    pose.orientation.w is set to 1.0 — required for CUBE_LIST so the per-point
+    positions are not distorted by an invalid default quaternion.
+
+    Returns a Marker, or None if the cloud has no x/y/z fields.
+    """
+    from visualization_msgs.msg import Marker
+    from geometry_msgs.msg import Point as GPoint
+    from std_msgs.msg import ColorRGBA
+
+    field_map = {f.name: f.offset for f in cloud.fields}
+    if not all(k in field_map for k in ('x', 'y', 'z')):
+        return None
+
+    ox, oy, oz = field_map['x'], field_map['y'], field_map['z']
+    o_rgb = field_map.get('rgb')
+    ps = cloud.point_step
+    n_points = cloud.width * cloud.height
+    data = bytes(cloud.data)
+
+    m = Marker()
+    m.header.frame_id = 'map'
+    m.header.stamp = stamp
+    m.ns = ns
+    m.id = marker_id
+    m.pose.orientation.w = 1.0   # identity — must be set or CUBE_LIST distorts positions
+    m.type = Marker.CUBE_LIST
+    m.action = Marker.ADD
+    m.scale.x = scale
+    m.scale.y = scale
+    m.scale.z = scale
+    m.lifetime = lifetime
+
+    for idx in range(n_points):
+        base = idx * ps
+        x, = struct.unpack_from('<f', data, base + ox)
+        y, = struct.unpack_from('<f', data, base + oy)
+        z, = struct.unpack_from('<f', data, base + oz)
+        m.points.append(GPoint(x=x + bx, y=y + by, z=z + bz))
+        if o_rgb is not None:
+            packed, = struct.unpack_from('<I', data, base + o_rgb)
+            r = ((packed >> 16) & 0xFF) / 255.0
+            g = ((packed >> 8)  & 0xFF) / 255.0
+            b = (packed         & 0xFF) / 255.0
+            m.colors.append(ColorRGBA(r=r, g=g, b=b, a=1.0))
+
+    if not m.colors:
+        fc = fallback_color or (1.0, 1.0, 1.0, 1.0)
+        m.color = ColorRGBA(r=fc[0], g=fc[1], b=fc[2], a=fc[3])
+
+    return m
+
+
 def transform_point_cloud2(cloud, bx, by, bz):
     """Return a copy of a PointCloud2 with all x,y,z values offset by (bx, by, bz).
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
index 561a7e53..68911ee8 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -2,51 +2,54 @@
 payload_visualizer_node.py
 ==========================
 Project-specific GCS visualization node. Reads PeerProfile payloads from
-/gossip/peers, applies coordinate frame translation, and publishes for Foxglove.
+/gossip/peers, applies coordinate frame translation, and publishes each payload
+to its own topic: /gcs/payload/{robot_name}/{payload_name}
+
+Publishing per-topic (rather than a single MarkerArray) lets Foxglove expose
+its full visualization controls (point size, color mapping, etc.) for each payload.
 
 Payloads are defined in gossip_payloads.yaml on the robot side — no DDS router
 changes are needed when adding new payload types.
 
-How to adapt for your project
-------------------------------
-1. On the robot: add entries to gossip_payloads.yaml (topic + ROS type).
-2. Here: add a handler method and register it in PAYLOAD_HANDLERS.
+How to add a new payload for visualization
+------------------------------------------
+1. Read gossip_payloads.yaml and find the `type:` field of the new entry.
+2. Add a handler method here (see signature below).
+3. If the type is unique (not already in PAYLOAD_HANDLERS), add it to the dict.
+   If the type already exists (e.g. a second PointCloud2), dispatch by index in
+   _on_peer_profile after the PAYLOAD_HANDLERS loop (order matches yaml order).
 
 Handler signature:
-    def _handle_<name>(self, robot_name, msg, boot, i, now, array):
-        # msg   — deserialized ROS message
-        # boot  — (bx, by, bz) ENU translation from robot odom frame to global map
-        # i     — stable robot index (use for marker IDs to avoid collisions)
-        # now   — current ROS timestamp
-        # array — MarkerArray to append Marker messages to
-        # For non-Marker outputs (e.g. PointCloud2), publish a separate topic directly.
+    def _handle_<name>(self, robot_name, msg, boot, i, now):
+        # msg  — deserialized ROS message
+        # boot — (bx, by, bz) ENU offset: add to all positions to go odom→map frame
+        # i    — stable robot index (for marker IDs: i * 100000 + unique_offset)
+        # now  — current ROS timestamp
+        # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
 
 Coordinate frame note
 ---------------------
-Payload data is in each robot's local odometry frame (origin = robot's first
-GPS fix in ENU). Apply the boot offset to translate into the global 'map' frame.
+Payload data is in each robot's local odometry frame. Apply the boot offset to
+translate into the global 'map' frame before publishing.
 
 Use transform_marker_array() for MarkerArray payloads.
-Use transform_point_cloud2() for PointCloud2 payloads.
-Both helpers are in gcs_utils.py.
+Use transform_point_cloud2() for PointCloud2 payloads (preserves all fields + rgb).
+Both are in gcs_utils.py.
 """
 
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
 
-import struct
-
-from sensor_msgs.msg import NavSatFix
-from visualization_msgs.msg import MarkerArray, Marker
-from geometry_msgs.msg import Point
-from std_msgs.msg import ColorRGBA
+from rclpy.serialization import deserialize_message
+from sensor_msgs.msg import PointCloud2
+from visualization_msgs.msg import MarkerArray
 from builtin_interfaces.msg import Duration
 
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 from coordination_bringup.peer_profile import PeerProfile
 
-from gcs_visualizer.gcs_utils import gps_to_enu, transform_marker_array
+from gcs_visualizer.gcs_utils import gps_to_enu, transform_marker_array, transform_point_cloud2
 
 GOSSIP_QOS = QoSProfile(
     reliability=ReliabilityPolicy.RELIABLE,
@@ -55,19 +58,14 @@ def _handle_<name>(self, robot_name, msg, boot, i, now, array):
     depth=10,
 )
 
-GPS_SUFFIX = '/interface/mavros/global_position/global'
-
-
 class PayloadVisualizerNode(Node):
     def __init__(self):
         super().__init__('payload_visualizer_node')
 
         self._gps_boot      = {}   # robot_name -> (bx, by, bz) ENU at first GPS fix
         self._alt_ground    = None
-        self._payload_cache = {}   # (robot_name, type_str) -> last deserialized payload msg
-
-        self._markers_pub = self.create_publisher(
-            MarkerArray, '/gcs/payload_markers', 10)
+        self._payload_cache = {}   # (robot_name, cache_key) -> last deserialized payload msg
+        self._pubs          = {}   # topic -> Publisher (created lazily)
 
         # Subscribe to /gossip/peers — PeerProfiles arrive here from all robots
         self.create_subscription(
@@ -76,15 +74,12 @@ def __init__(self):
 
         self.get_logger().info('PayloadVisualizerNode started')
 
-    # ── Colour palette for per-robot point clouds ─────────────────────────────
+    # ── Lazy publisher helper ──────────────────────────────────────────────
 
-    _ROBOT_COLORS = [
-        (0.0, 1.0, 1.0, 1.0),   # cyan
-        (1.0, 0.5, 0.0, 1.0),   # orange
-        (0.5, 1.0, 0.0, 1.0),   # lime
-        (1.0, 0.0, 0.5, 1.0),   # pink
-        (0.5, 0.0, 1.0, 1.0),   # purple
-    ]
+    def _pub_for(self, topic, msg_type):
+        if topic not in self._pubs:
+            self._pubs[topic] = self.create_publisher(msg_type, topic, 10)
+        return self._pubs[topic]
 
     # ── Boot offset tracking ───────────────────────────────────────────────
 
@@ -112,8 +107,6 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
 
         profile = PeerProfile.from_ros_msg(msg)
         now = self.get_clock().now().to_msg()
-        array = MarkerArray()
-
         robot_index = self._robot_index(robot_name)
 
         for type_str, handler in self.PAYLOAD_HANDLERS.items():
@@ -122,10 +115,16 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
                 self._payload_cache[(robot_name, type_str)] = payload
             cached = self._payload_cache.get((robot_name, type_str))
             if cached is not None:
-                handler(self, robot_name, cached, boot, robot_index, now, array)
+                handler(self, robot_name, cached, boot, robot_index, now)
 
-        if array.markers:
-            self._markers_pub.publish(array)
+        # voxel_rgb is the 2nd PointCloud2 payload (frontier_viewpoints is 1st)
+        pc2_list = [p for p in profile._payloads if p["type"] == "sensor_msgs/msg/PointCloud2"]
+        if len(pc2_list) >= 2:
+            voxel_rgb = deserialize_message(bytes(pc2_list[1]["data"]), PointCloud2)
+            self._payload_cache[(robot_name, 'rgb_voxels')] = voxel_rgb
+        cached_voxels = self._payload_cache.get((robot_name, 'rgb_voxels'))
+        if cached_voxels is not None:
+            self._handle_rgb_voxels(robot_name, cached_voxels, boot, robot_index, now)
 
     def _robot_index(self, robot_name: str) -> int:
         """Stable integer index for a robot name (alphabetical order)."""
@@ -134,61 +133,38 @@ def _robot_index(self, robot_name: str) -> int:
 
     # ── Payload handlers ──────────────────────────────────────────────────
     # Each handler receives:
-    #   robot_name  – e.g. "robot_1"
-    #   msg         – deserialized ROS message
-    #   boot        – (bx, by, bz) ENU translation for this robot's odom frame
-    #   i           – stable robot index (for marker IDs)
-    #   now         – current ROS timestamp
-    #   array       – MarkerArray to append Marker messages to
-    #
-    # For non-marker outputs (e.g. PointCloud2), publish directly from here.
-
-    def _handle_filtered_rays(self, robot_name, msg, boot, i, now, array):
-        """Translate filtered_rays MarkerArray from odom frame to ENU map frame."""
+    #   robot_name – e.g. "robot_1"
+    #   msg        – deserialized ROS message
+    #   boot       – (bx, by, bz) ENU translation for this robot's odom frame
+    #   i          – stable robot index (for marker IDs)
+    #   now        – current ROS timestamp
+    # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
+
+    def _handle_filtered_rays(self, robot_name, msg, boot, i, now):
+        """Publish filtered_rays MarkerArray translated into the map frame."""
         bx, by, bz = boot
-        lifetime = Duration(sec=2, nanosec=0)
-        markers = transform_marker_array(
+        out = MarkerArray()
+        out.markers.extend(transform_marker_array(
             msg, bx, by, bz,
             ns=f'{robot_name}_filtered_rays',
             id_base=i * 100000,
-            stamp=now, lifetime=lifetime)
-        array.markers.extend(markers)
+            stamp=now,
+            lifetime=Duration(sec=2, nanosec=0)))
+        self._pub_for(f'/gcs/payload/{robot_name}/filtered_rays', MarkerArray).publish(out)
 
-    def _handle_frontier_viewpoints(self, robot_name, msg, boot, i, now, array):
-        """Convert frontier_viewpoints PointCloud2 to a POINTS Marker in the map frame."""
+    def _handle_frontier_viewpoints(self, robot_name, msg, boot, i, now):
+        """Publish frontier_viewpoints PointCloud2 translated into the map frame."""
         bx, by, bz = boot
-        r, g, b, a = self._ROBOT_COLORS[i % len(self._ROBOT_COLORS)]
+        transformed = transform_point_cloud2(msg, bx, by, bz)
+        transformed.header.stamp = now
+        self._pub_for(f'/gcs/payload/{robot_name}/frontier_viewpoints', PointCloud2).publish(transformed)
 
-        # Extract xyz from the PointCloud2 and apply boot offset
-        field_offsets = {f.name: f.offset for f in msg.fields if f.name in ('x', 'y', 'z')}
-        if not all(k in field_offsets for k in ('x', 'y', 'z')):
-            return
-        ox, oy, oz = field_offsets['x'], field_offsets['y'], field_offsets['z']
-        ps = msg.point_step
-        n_points = msg.width * msg.height
-        data = bytes(msg.data)
-
-        m = Marker()
-        m.header.frame_id = 'map'
-        m.header.stamp = now
-        m.ns = f'{robot_name}_frontier_viewpoints'
-        m.id = i * 100000 + 90000
-        m.type = Marker.SPHERE_LIST
-        m.action = Marker.ADD
-        m.scale.x = 1.0
-        m.scale.y = 1.0
-        m.scale.z = 1.0
-        m.lifetime = Duration(sec=2, nanosec=0)
-        m.color = ColorRGBA(r=r, g=g, b=b, a=a)
-
-        for idx in range(n_points):
-            base = idx * ps
-            x, = struct.unpack_from('<f', data, base + ox)
-            y, = struct.unpack_from('<f', data, base + oy)
-            z, = struct.unpack_from('<f', data, base + oz)
-            m.points.append(Point(x=x + bx, y=y + by, z=z + bz))
-
-        array.markers.append(m)
+    def _handle_rgb_voxels(self, robot_name, msg, boot, i, now):
+        """Publish voxel_rgb PointCloud2 translated into the map frame."""
+        bx, by, bz = boot
+        transformed = transform_point_cloud2(msg, bx, by, bz)
+        transformed.header.stamp = now
+        self._pub_for(f'/gcs/payload/{robot_name}/rgb_voxels', PointCloud2).publish(transformed)
 
     # ── Handler registry ──────────────────────────────────────────────────
     # Maps ROS type string -> handler method.
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index 776be121..5b2cea84 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -19,7 +19,6 @@ participants:
   - name: "gcs"
     kind: "local"
     domain: $(var gcs_domain)
-
 allowlist:
   # lidar and world model
   - name: "rt/$(env ROBOT_NAME)/sensors/ouster/point_cloud"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml b/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml
index 32134c30..ac7dc162 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml
+++ b/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml
@@ -36,6 +36,9 @@ payload_topics:
   # Peers can use these to select non-overlapping goals.
   - topic: "/{robot_name}/frontier_viewpoints"
     type: "sensor_msgs/msg/PointCloud2"
+  
+  - topic : "/{robot_name}/rayfronts/voxel_rgb"
+    type: "sensor_msgs/msg/PointCloud2"
 
   # ── Add more payloads below ────────────────────────────────────────────────
   # - topic: "/{robot_name}/your/topic"
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
new file mode 100644
index 00000000..f8fe702c
--- /dev/null
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -0,0 +1,303 @@
+#!/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/Assets/ACFA/FireAcademyFaro/fire_academy_fixed_textures.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.07
+
+# spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd
+# DRONE1_XY_M = (20.0, -7.0)
+# DRONE2_XY_M = (17, 1.5)
+
+DRONE1_XY_M = (27.0, 7.6)
+DRONE2_XY_M = (23, 9.8)
+# ---------------------------------------------------------
+
+
+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)
+
+            translate_attr = stage_prim.GetAttribute("xformOp:translate")
+            if translate_attr.IsValid():
+                if str(translate_attr.GetTypeName()) == "float3":
+                    translate_attr.Set(Gf.Vec3f(0.0, 0.0, 0.0))
+                else:
+                    translate_attr.Set(Gf.Vec3d(0.0, 0.0, 0.0))
+            else:
+                # If missing, create a new translate op.
+                translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
+                translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
+
+            scale_attr = stage_prim.GetAttribute("xformOp:scale")
+            if scale_attr.IsValid():
+                if str(scale_attr.GetTypeName()) == "double3":
+                    scale_attr.Set(Gf.Vec3d(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
+                else:
+                    scale_attr.Set(Gf.Vec3f(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
+            else:
+                # If missing, create a new scale op.
+                scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionFloat)
+                scale_op.Set(Gf.Vec3f(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.937, 0.35], #[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 = 4.0, # 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.937, 0.35], #[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 = 3.5 
+        )
+
+        # 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

From ce32a224e39c029714055a0ebf52dab86061b88b Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Wed, 25 Mar 2026 17:50:42 -0400
Subject: [PATCH 19/55] syncing devices

---
 .env                                          |   4 +-
 .../foxglove_visualizer_node.py               |  11 +-
 .../gcs_visualizer/gcs_utils.py               |  94 +------
 .../gcs_visualizer/payload_visualizer_node.py | 131 +++++----
 .../coordination_bringup/frame_utils.py       | 122 ++++++++
 .../coordination_bringup/gossip_node.py       | 134 +++++++--
 .../scripts/peer_registry_monitor.py          |  65 ++++-
 .../three_drone_scene_import.py               |  88 ++----
 .../launch_scripts/two_drone_fire.py          | 265 +++++++-----------
 .../launch_scripts/two_drone_scene_import.py  | 246 +++++++---------
 simulation/isaac-sim/utils/scene_prep.py      |  24 +-
 11 files changed, 618 insertions(+), 566 deletions(-)
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py

diff --git a/.env b/.env
index e2947f19..c58dd6b7 100644
--- a/.env
+++ b/.env
@@ -26,7 +26,7 @@ COMPOSE_PROFILES="desktop"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="3"  # Number of robot containers to launch.
+NUM_ROBOTS="2"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
@@ -34,7 +34,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="three_drone_scene_import.py"
+ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index a634fafc..5ab9106b 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -277,11 +277,12 @@ def _publish_markers(self):
             traj = self._trajectories.get(robot_name)
             if traj is not None and boot is not None:
                 bx, by, bz = boot
-                for m in transform_marker_array(
-                        traj, bx, by, bz,
-                        ns=f'{robot_name}_traj',
-                        id_base=i * 10000,
-                        stamp=now, lifetime=lifetime):
+                transformed = transform_marker_array(traj, bx, by, bz)
+                for k, m in enumerate(transformed.markers):
+                    m.ns = f'{robot_name}_traj'
+                    m.id = i * 10000 + k
+                    m.header.stamp = now
+                    m.lifetime = lifetime
                     if m.color.a > 0:
                         m.color.r = 0.8
                         m.color.g = 0.5
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
index 5321c0cb..760e4ab2 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
@@ -20,9 +20,17 @@
 import copy
 import struct
 
+# Shared frame utilities live in coordination_bringup so both robot and GCS
+# packages can import them.  Re-export the ones callers expect from here.
+from coordination_bringup.frame_utils import (
+    gps_to_enu as _gps_to_enu_abs,
+    heading_to_quat,
+    rotate_vector,
+    transform_marker_array,
+    transform_point_cloud2 as _transform_pc2,
+)
+
 # ── Origin ────────────────────────────────────────────────────────────────────
-# Matches Pegasus configs.yaml and gps_utils.py DEFAULT_WORLD_ORIGIN (Lisbon).
-# Update this if your simulation/field site uses a different origin.
 ORIGIN_LAT = 38.736832
 ORIGIN_LON = -9.137977
 ORIGIN_ALT = 90.0
@@ -53,50 +61,6 @@ def multiply_quaternions(q1, q2):
     )
 
 
-def rotate_vector(v, q):
-    """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
-    vx, vy, vz = v
-    qx, qy, qz, qw = q
-    cx = qy * vz - qz * vy
-    cy = qz * vx - qx * vz
-    cz = qx * vy - qy * vx
-    return (
-        vx + 2.0 * (qw * cx + qy * cz - qz * cy),
-        vy + 2.0 * (qw * cy + qz * cx - qx * cz),
-        vz + 2.0 * (qw * cz + qx * cy - qy * cx),
-    )
-
-
-# ── Marker / PointCloud2 transform helpers ────────────────────────────────────
-
-def transform_marker_array(marker_array, bx, by, bz, ns, id_base, stamp, lifetime):
-    """Deep-copy a MarkerArray and translate all point coordinates by (bx, by, bz).
-
-    Sets frame_id='map', the given namespace, sequential IDs starting at
-    ``id_base``, and the provided stamp and lifetime on every marker.
-
-    Only translates ``points[]`` — does NOT touch ``pose.position``, which is
-    intentional: LINE_STRIP and ARROW markers encode geometry in ``points``.
-    Returns a list of Marker messages.
-    """
-    from geometry_msgs.msg import Point
-
-    result = []
-    for k, orig in enumerate(marker_array.markers):
-        m = copy.deepcopy(orig)
-        m.header.frame_id = 'map'
-        m.header.stamp = stamp
-        m.ns = ns
-        m.id = id_base + k
-        m.lifetime = lifetime
-        for pt in m.points:
-            pt.x += bx
-            pt.y += by
-            pt.z += bz
-        result.append(m)
-    return result
-
-
 def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetime,
                                 fallback_color=None, scale=0.2):
     """Convert a PointCloud2 to a CUBE_LIST Marker translated into the map frame.
@@ -158,39 +122,5 @@ def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetim
     return m
 
 
-def transform_point_cloud2(cloud, bx, by, bz):
-    """Return a copy of a PointCloud2 with all x,y,z values offset by (bx, by, bz).
-
-    Assumes the cloud has float32 x, y, z fields (standard ROS convention).
-    Reads field offsets from the message so non-standard field orderings work.
-    Sets frame_id='map' on the returned message.
-    """
-    # Find byte offsets for x, y, z fields
-    field_offsets = {}
-    for f in cloud.fields:
-        if f.name in ('x', 'y', 'z'):
-            field_offsets[f.name] = f.offset
-
-    if not all(k in field_offsets for k in ('x', 'y', 'z')):
-        return cloud  # can't transform without x,y,z fields
-
-    ox = field_offsets['x']
-    oy = field_offsets['y']
-    oz = field_offsets['z']
-
-    new_cloud = copy.copy(cloud)
-    new_cloud.header = copy.copy(cloud.header)
-    new_cloud.header.frame_id = 'map'
-    data = bytearray(cloud.data)
-    n_points = cloud.width * cloud.height
-    ps = cloud.point_step
-    for i in range(n_points):
-        base = i * ps
-        x, = struct.unpack_from('<f', data, base + ox)
-        y, = struct.unpack_from('<f', data, base + oy)
-        z, = struct.unpack_from('<f', data, base + oz)
-        struct.pack_into('<f', data, base + ox, x + bx)
-        struct.pack_into('<f', data, base + oy, y + by)
-        struct.pack_into('<f', data, base + oz, z + bz)
-    new_cloud.data = bytes(data)
-    return new_cloud
+# transform_point_cloud2 is re-exported from coordination_bringup.frame_utils
+transform_point_cloud2 = _transform_pc2
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
index 68911ee8..318d1dde 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -2,8 +2,8 @@
 payload_visualizer_node.py
 ==========================
 Project-specific GCS visualization node. Reads PeerProfile payloads from
-/gossip/peers, applies coordinate frame translation, and publishes each payload
-to its own topic: /gcs/payload/{robot_name}/{payload_name}
+/gossip/peers and publishes each payload to its own topic:
+/gcs/payload/{robot_name}/{payload_name}
 
 Publishing per-topic (rather than a single MarkerArray) lets Foxglove expose
 its full visualization controls (point size, color mapping, etc.) for each payload.
@@ -11,6 +11,12 @@
 Payloads are defined in gossip_payloads.yaml on the robot side — no DDS router
 changes are needed when adding new payload types.
 
+Coordinate frame note
+---------------------
+Payloads arrive already transformed into the global ENU 'map' frame by
+gossip_node on each robot.  No coordinate transform is needed here — handlers
+just set frame_id='map' and republish.
+
 How to add a new payload for visualization
 ------------------------------------------
 1. Read gossip_payloads.yaml and find the `type:` field of the new entry.
@@ -20,21 +26,11 @@
    _on_peer_profile after the PAYLOAD_HANDLERS loop (order matches yaml order).
 
 Handler signature:
-    def _handle_<name>(self, robot_name, msg, boot, i, now):
-        # msg  — deserialized ROS message
-        # boot — (bx, by, bz) ENU offset: add to all positions to go odom→map frame
+    def _handle_<name>(self, robot_name, msg, i, now):
+        # msg  — deserialized ROS message (already in global ENU / 'map' frame)
         # i    — stable robot index (for marker IDs: i * 100000 + unique_offset)
         # now  — current ROS timestamp
         # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
-
-Coordinate frame note
----------------------
-Payload data is in each robot's local odometry frame. Apply the boot offset to
-translate into the global 'map' frame before publishing.
-
-Use transform_marker_array() for MarkerArray payloads.
-Use transform_point_cloud2() for PointCloud2 payloads (preserves all fields + rgb).
-Both are in gcs_utils.py.
 """
 
 import rclpy
@@ -48,8 +44,12 @@ def _handle_<name>(self, robot_name, msg, boot, i, now):
 
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 from coordination_bringup.peer_profile import PeerProfile
+from gcs_visualizer.gcs_utils import transform_marker_array, transform_point_cloud2
+
+# ENU origin altitude used by gossip_node when transforming payloads at source.
+# Must match frame_utils.DEFAULT_ORIGIN_ALT.
+_GOSSIP_ORIGIN_ALT = 90.0
 
-from gcs_visualizer.gcs_utils import gps_to_enu, transform_marker_array, transform_point_cloud2
 
 GOSSIP_QOS = QoSProfile(
     reliability=ReliabilityPolicy.RELIABLE,
@@ -62,8 +62,9 @@ class PayloadVisualizerNode(Node):
     def __init__(self):
         super().__init__('payload_visualizer_node')
 
-        self._gps_boot      = {}   # robot_name -> (bx, by, bz) ENU at first GPS fix
-        self._alt_ground    = None
+        self._gps_boot      = {}   # robot_name -> True once a valid GPS fix has been seen
+        self._last_stamp    = {}   # robot_name -> float: ROS stamp of last accepted message
+        self._alt_ground    = None # altitude (m) of first GPS fix — display z datum
         self._payload_cache = {}   # (robot_name, cache_key) -> last deserialized payload msg
         self._pubs          = {}   # topic -> Publisher (created lazily)
 
@@ -81,28 +82,50 @@ def _pub_for(self, topic, msg_type):
             self._pubs[topic] = self.create_publisher(msg_type, topic, 10)
         return self._pubs[topic]
 
-    # ── Boot offset tracking ───────────────────────────────────────────────
+    # ── GPS boot tracking (for robot index, not transforms) ───────────────
 
-    def _update_boot_from_gps(self, robot_name: str, gps_fix) -> None:
-        """Track the first GPS fix seen per robot as the boot (odom origin) offset."""
+    def _update_boot(self, robot_name: str, gps_fix) -> None:
+        """Record that a valid GPS fix has been seen for this robot.
+
+        Also tracks alt_ground (first GPS altitude seen across all robots) so
+        payload z values can be shifted to match the foxglove_visualizer datum.
+        """
         if gps_fix.status.status < 0:
             return
         if self._alt_ground is None:
             self._alt_ground = gps_fix.altitude
         if robot_name not in self._gps_boot:
-            pos = gps_to_enu(
-                gps_fix.latitude, gps_fix.longitude,
-                gps_fix.altitude, self._alt_ground)
-            self._gps_boot[robot_name] = pos
+            self._gps_boot[robot_name] = True
+
+    def _display_z_offset(self) -> float:
+        """Z shift to apply to payload positions before display.
+
+        gossip_node transforms payloads with a fixed ENU origin_alt of 90 m.
+        foxglove_visualizer uses alt_ground (first GPS fix altitude) as its datum.
+        If those two differ, payloads appear above or below the drone.
+        This correction aligns them without touching the peer-profile data.
+        """
+        if self._alt_ground is None:
+            return 0.0
+        return _GOSSIP_ORIGIN_ALT - self._alt_ground
 
     # ── Peer profile callback ─────────────────────────────────────────────
 
     def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
         robot_name = msg.robot_name
-        self._update_boot_from_gps(robot_name, msg.gps_fix)
 
-        boot = self._gps_boot.get(robot_name)
-        if boot is None:
+        # Per-robot ordering: accept only if this message is newer than the last
+        # accepted one for THIS robot.  robot_1 and robot_2 are tracked independently
+        # so neither can block the other.
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+        if new_t < self._last_stamp.get(robot_name, 0.0):
+            return
+        self._last_stamp[robot_name] = new_t
+
+        self._update_boot(robot_name, msg.gps_fix)
+
+        if robot_name not in self._gps_boot:
             return  # no GPS fix yet, skip
 
         profile = PeerProfile.from_ros_msg(msg)
@@ -115,7 +138,7 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
                 self._payload_cache[(robot_name, type_str)] = payload
             cached = self._payload_cache.get((robot_name, type_str))
             if cached is not None:
-                handler(self, robot_name, cached, boot, robot_index, now)
+                handler(self, robot_name, cached, robot_index, now)
 
         # voxel_rgb is the 2nd PointCloud2 payload (frontier_viewpoints is 1st)
         pc2_list = [p for p in profile._payloads if p["type"] == "sensor_msgs/msg/PointCloud2"]
@@ -124,7 +147,7 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
             self._payload_cache[(robot_name, 'rgb_voxels')] = voxel_rgb
         cached_voxels = self._payload_cache.get((robot_name, 'rgb_voxels'))
         if cached_voxels is not None:
-            self._handle_rgb_voxels(robot_name, cached_voxels, boot, robot_index, now)
+            self._handle_rgb_voxels(robot_name, cached_voxels, robot_index, now)
 
     def _robot_index(self, robot_name: str) -> int:
         """Stable integer index for a robot name (alphabetical order)."""
@@ -134,37 +157,33 @@ def _robot_index(self, robot_name: str) -> int:
     # ── Payload handlers ──────────────────────────────────────────────────
     # Each handler receives:
     #   robot_name – e.g. "robot_1"
-    #   msg        – deserialized ROS message
-    #   boot       – (bx, by, bz) ENU translation for this robot's odom frame
+    #   msg        – deserialized ROS message (already in global ENU / 'map' frame)
     #   i          – stable robot index (for marker IDs)
     #   now        – current ROS timestamp
     # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
 
-    def _handle_filtered_rays(self, robot_name, msg, boot, i, now):
-        """Publish filtered_rays MarkerArray translated into the map frame."""
-        bx, by, bz = boot
-        out = MarkerArray()
-        out.markers.extend(transform_marker_array(
-            msg, bx, by, bz,
-            ns=f'{robot_name}_filtered_rays',
-            id_base=i * 100000,
-            stamp=now,
-            lifetime=Duration(sec=2, nanosec=0)))
-        self._pub_for(f'/gcs/payload/{robot_name}/filtered_rays', MarkerArray).publish(out)
-
-    def _handle_frontier_viewpoints(self, robot_name, msg, boot, i, now):
-        """Publish frontier_viewpoints PointCloud2 translated into the map frame."""
-        bx, by, bz = boot
-        transformed = transform_point_cloud2(msg, bx, by, bz)
-        transformed.header.stamp = now
-        self._pub_for(f'/gcs/payload/{robot_name}/frontier_viewpoints', PointCloud2).publish(transformed)
-
-    def _handle_rgb_voxels(self, robot_name, msg, boot, i, now):
-        """Publish voxel_rgb PointCloud2 translated into the map frame."""
-        bx, by, bz = boot
-        transformed = transform_point_cloud2(msg, bx, by, bz)
-        transformed.header.stamp = now
-        self._pub_for(f'/gcs/payload/{robot_name}/rgb_voxels', PointCloud2).publish(transformed)
+    def _handle_filtered_rays(self, robot_name, msg, i, now):
+        """Republish filtered_rays MarkerArray shifted to the GCS display datum."""
+        bz = self._display_z_offset()
+        out_ma = transform_marker_array(msg, 0.0, 0.0, bz)
+        for k, m in enumerate(out_ma.markers):
+            m.header.stamp = now
+            m.ns = f'{robot_name}_filtered_rays'
+            m.id = i * 100000 + k
+            m.lifetime = Duration(sec=2, nanosec=0)
+        self._pub_for(f'/gcs/payload/{robot_name}/filtered_rays', MarkerArray).publish(out_ma)
+
+    def _handle_frontier_viewpoints(self, robot_name, msg, i, now):
+        """Republish frontier_viewpoints PointCloud2 shifted to the GCS display datum."""
+        out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
+        out.header.stamp = now
+        self._pub_for(f'/gcs/payload/{robot_name}/frontier_viewpoints', PointCloud2).publish(out)
+
+    def _handle_rgb_voxels(self, robot_name, msg, i, now):
+        """Republish voxel_rgb PointCloud2 shifted to the GCS display datum."""
+        out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
+        out.header.stamp = now
+        self._pub_for(f'/gcs/payload/{robot_name}/rgb_voxels', PointCloud2).publish(out)
 
     # ── Handler registry ──────────────────────────────────────────────────
     # Maps ROS type string -> handler method.
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
new file mode 100644
index 00000000..19fd75d6
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
@@ -0,0 +1,122 @@
+"""
+frame_utils.py
+==============
+Coordinate frame utilities shared between gossip_node (robot) and
+gcs_visualizer (GCS).
+
+Robots transform their payloads into global ENU before attaching them to
+PeerProfile, so every consumer receives world-frame data directly.
+
+ENU world origin (what GPS (0,0,0) maps to in metres):
+  Must match gcs_utils.py ORIGIN_* and the simulation's GPS home position.
+  Override via the 'enu_origin_lat/lon/alt' ROS parameters in gossip_node,
+  or leave at the Lisbon default used everywhere else in AirStack.
+"""
+
+import copy
+import math
+import struct
+
+# Default world origin — Lisbon (matches gcs_utils.py and gps_utils.py)
+DEFAULT_ORIGIN_LAT = 38.736832
+DEFAULT_ORIGIN_LON = -9.137977
+DEFAULT_ORIGIN_ALT = 90.0
+
+
+def gps_to_enu(lat, lon, alt,
+               origin_lat=DEFAULT_ORIGIN_LAT,
+               origin_lon=DEFAULT_ORIGIN_LON,
+               origin_alt=DEFAULT_ORIGIN_ALT):
+    """Convert GPS lat/lon/alt to ENU metres relative to the world origin."""
+    x = (lon - origin_lon) * 111320.0 * math.cos(math.radians(origin_lat))
+    y = (lat - origin_lat) * 111320.0
+    z = alt - origin_alt
+    return x, y, z
+
+
+def heading_to_quat(heading_deg):
+    """Compass heading (degrees CW from North) → ENU yaw quaternion (x,y,z,w).
+
+    ENU: yaw=0 → East (+x), yaw=90° → North (+y).
+    heading=0 (North) → yaw=90° → q=(0,0,sin45,cos45)
+    heading=90 (East)  → yaw=0°  → q=(0,0,0,1)
+    """
+    yaw_enu = math.radians(90.0 - heading_deg)
+    return (0.0, 0.0, math.sin(yaw_enu / 2.0), math.cos(yaw_enu / 2.0))
+
+
+def rotate_vector(v, q):
+    """Rotate vector v=(vx,vy,vz) by quaternion q=(x,y,z,w). Returns (x,y,z)."""
+    vx, vy, vz = v
+    qx, qy, qz, qw = q
+    cx = qy * vz - qz * vy
+    cy = qz * vx - qx * vz
+    cz = qx * vy - qy * vx
+    return (
+        vx + 2.0 * (qw * cx + qy * cz - qz * cy),
+        vy + 2.0 * (qw * cy + qz * cx - qx * cz),
+        vz + 2.0 * (qw * cz + qx * cy - qy * cx),
+    )
+
+
+def transform_marker_array(marker_array, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
+    """Deep-copy a MarkerArray and transform all points: p_map = R(q)*p + (bx,by,bz).
+
+    Transforms every point in points[] (LINE_LIST, LINE_STRIP, CUBE_LIST,
+    SPHERE_LIST, POINTS, ARROW-with-2-points, TRIANGLE_LIST).
+
+    Note: pose.position is intentionally NOT translated here.  In the ROS
+    marker convention, points[] are expressed in the marker's local frame
+    (defined by pose).  These markers typically use an identity pose so that
+    points[] are already in the parent frame; translating pose.position as
+    well would double-apply the offset.  If you have single-geometry markers
+    (SPHERE, CUBE, TEXT, MESH_RESOURCE) whose position lives in pose.position
+    rather than points[], translate pose.position at the call site instead.
+
+    Sets frame_id='map'.  Returns a new MarkerArray (does not modify in-place).
+    """
+    from visualization_msgs.msg import MarkerArray as MA
+    out = MA()
+    for orig in marker_array.markers:
+        m = copy.deepcopy(orig)
+        m.header.frame_id = 'map'
+        for pt in m.points:
+            rx, ry, rz = rotate_vector((pt.x, pt.y, pt.z), q)
+            pt.x = rx + bx
+            pt.y = ry + by
+            pt.z = rz + bz
+        out.markers.append(m)
+    return out
+
+
+def transform_point_cloud2(cloud, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
+    """Return a copy of PointCloud2 with all xyz transformed: p_map = R(q)*p + (bx,by,bz).
+
+    Sets frame_id='map'.  Reads field offsets from the message header so
+    non-standard field orderings work.
+    """
+    field_offsets = {f.name: f.offset for f in cloud.fields if f.name in ('x', 'y', 'z')}
+    if not all(k in field_offsets for k in ('x', 'y', 'z')):
+        return cloud
+
+    ox, oy, oz = field_offsets['x'], field_offsets['y'], field_offsets['z']
+    ps = cloud.point_step
+    n_points = cloud.width * cloud.height
+    if ps == 0 or len(cloud.data) < n_points * ps:
+        return cloud  # malformed cloud — skip rather than raise
+
+    new_cloud = copy.copy(cloud)
+    new_cloud.header = copy.copy(cloud.header)
+    new_cloud.header.frame_id = 'map'
+    data = bytearray(cloud.data)
+    for i in range(n_points):
+        base = i * ps
+        x, = struct.unpack_from('<f', data, base + ox)
+        y, = struct.unpack_from('<f', data, base + oy)
+        z, = struct.unpack_from('<f', data, base + oz)
+        rx, ry, rz = rotate_vector((x, y, z), q)
+        struct.pack_into('<f', data, base + ox, rx + bx)
+        struct.pack_into('<f', data, base + oy, ry + by)
+        struct.pack_into('<f', data, base + oz, rz + bz)
+    new_cloud.data = bytes(data)
+    return new_cloud
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
index d0951a0d..d727addb 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -17,6 +17,7 @@
 """
 
 import os
+import threading
 import yaml
 
 import rclpy
@@ -36,6 +37,12 @@
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 
 from coordination_bringup.peer_profile import PeerProfile
+from coordination_bringup.frame_utils import (
+    gps_to_enu, heading_to_quat, transform_marker_array, transform_point_cloud2,
+)
+
+from visualization_msgs.msg import MarkerArray
+from sensor_msgs.msg import PointCloud2
 
 
 # QoS for gossip topic: best-effort, keep last 1 (low-overhead, high-freq)
@@ -79,10 +86,23 @@ def __init__(self):
         # ── State ────────────────────────────────────────────────────────
         self._profile = PeerProfile(robot_name=self._robot_name)
 
-        # Registry: robot_name → latest PeerProfile ROS msg
+        # Boot pose: set on first valid GPS fix + first heading reading.
+        # Used to transform payload data from local odom frame → global ENU
+        # before attaching to PeerProfile, so every receiver gets world-frame data.
+        self._boot_pos: tuple | None = None   # (bx, by, bz) ENU metres
+        self._boot_quat: tuple | None = None  # (qx, qy, qz, qw) rotation
+
+        # Registry: robot_name → latest accepted PeerProfile (monotonic by stamp)
         self._registry: dict[str, PeerProfileMsg] = {}
+        self._registry_lock = threading.Lock()
+
+        # Per-robot inbox: buffers incoming peer messages so every robot gets
+        # processed fairly on each drain tick, regardless of DDS arrival order.
+        # Stores only the latest message per robot (by gossip stamp).
+        self._peer_inbox: dict[str, PeerProfileMsg] = {}
+        self._peer_inbox_lock = threading.Lock()
 
-        # Payload cache: topic → latest message (None until first message arrives)
+        # Payload cache: topic → (msg, stamp) or None until first message arrives
         self._payload_cache: dict[str, object] = {}
         self._payload_subs: list = []
 
@@ -105,7 +125,7 @@ def __init__(self):
         )
         self._path_sub = self.create_subscription(
             Path,
-            f"/{self._robot_name}/random_walk/global_plan",
+            f"/{self._robot_name}/global_plan",
             self._on_global_plan,
             SENSOR_QOS,
         )
@@ -139,6 +159,15 @@ def __init__(self):
             clock=ROSClock(),
         )
 
+        # ── Peer inbox drain timer (5 Hz wall clock) ──────────────────────
+        # Drains the per-robot inbox at a fixed rate so all robots are
+        # processed fairly — robot N never blocks robot M from being updated.
+        self._drain_timer = self.create_timer(
+            0.2,
+            self._drain_peer_inbox,
+            clock=ROSClock(),
+        )
+
         self.get_logger().info(
             f"GossipNode started for '{self._robot_name}' "
             f"(publish_rate={publish_rate:.1f} Hz wall-clock, "
@@ -199,10 +228,25 @@ def cb(msg):
     # ------------------------------------------------------------------ #
 
     def _on_navsat(self, msg: NavSatFix) -> None:
+        # Only store valid fixes — ignore NO_FIX so GPS never zeros out.
+        if msg.status.status < 0:
+            return
         self._profile.set_gps_from_navsat(msg)
+        # Record boot position on first valid GPS fix
+        if self._boot_pos is None:
+            self._boot_pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude)
+            # Only set boot_quat now if heading has already been received
+            # (profile.heading != 0.0 means a compass reading arrived first).
+            # If heading hasn't arrived yet, _on_compass will set boot_quat
+            # once the first reading comes in.
+            if self._boot_quat is None and self._profile.heading != 0.0:
+                self._boot_quat = heading_to_quat(self._profile.heading)
 
     def _on_compass(self, msg: Float64) -> None:
         self._profile.set_heading(msg.data)
+        # Set boot quat on first heading reading once we have GPS
+        if self._boot_pos is not None and self._boot_quat is None:
+            self._boot_quat = heading_to_quat(msg.data)
 
     def _on_global_plan(self, msg: Path) -> None:
         self._profile.set_waypoint_from_path(msg)
@@ -210,7 +254,26 @@ def _on_global_plan(self, msg: Path) -> None:
     def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
         if msg.robot_name == self._robot_name:
             return  # discard own messages echoed back from the gossip domain
-        self._update_registry(msg)
+        # Stage in per-robot inbox: keep only the newest message per robot so
+        # all robots are drained together on the next tick (fair processing).
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+        with self._peer_inbox_lock:
+            existing = self._peer_inbox.get(msg.robot_name)
+            if existing is not None:
+                old_t = (existing.gps_fix.header.stamp.sec
+                         + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                if new_t < old_t:
+                    return  # already buffered a newer message this tick
+            self._peer_inbox[msg.robot_name] = msg
+
+    def _drain_peer_inbox(self) -> None:
+        """Process all buffered peer messages at 5 Hz, one per robot per tick."""
+        with self._peer_inbox_lock:
+            inbox = dict(self._peer_inbox)
+            self._peer_inbox.clear()
+        for msg in inbox.values():
+            self._update_registry(msg)
 
     # ------------------------------------------------------------------ #
     # Publish logic                                                        #
@@ -221,31 +284,60 @@ def _publish_tick(self) -> None:
         self._publish_own()
 
     def _publish_own(self) -> None:
-        # Rebuild payloads from cache on every tick. The cache retains the last
-        # received message per topic indefinitely, so peers always get the most
-        # recent known data even if the source publishes slower than gossip rate.
+        # Rebuild payloads from cache on every tick.
+        # If the boot pose is known, transform each payload into global ENU so
+        # every receiver (peers and GCS) gets world-frame data directly.
+        # If boot pose is not yet known, skip payloads this tick — we cannot
+        # produce a meaningful global-frame transform without GPS.
         self._profile.clear_payloads()
-        for topic, entry in self._payload_cache.items():
-            if entry is not None:
-                msg, stamp = entry
-                self._profile.add_payload(msg, stamp=stamp)
+        if self._boot_pos is not None and self._boot_quat is not None:
+            bx, by, bz = self._boot_pos
+            q = self._boot_quat
+            for topic, entry in self._payload_cache.items():
+                if entry is not None:
+                    msg, stamp = entry
+                    transformed = self._transform_to_global(msg, bx, by, bz, q)
+                    self._profile.add_payload(transformed, stamp=stamp)
+
+        # Stamp the outgoing profile with the current ROS clock time.
+        # Receivers use this stamp — not the MAVROS GPS stamp — to enforce
+        # monotonic ordering and discard out-of-order messages.
+        self._profile.gps_fix.header.stamp = self.get_clock().now().to_msg()
+
         self._gossip_pub.publish(self._profile.to_ros_msg())
 
+    def _transform_to_global(self, msg, bx, by, bz, q):
+        """Transform a payload message from local odom frame to global ENU."""
+        if isinstance(msg, MarkerArray):
+            return transform_marker_array(msg, bx, by, bz, q)
+        if isinstance(msg, PointCloud2):
+            return transform_point_cloud2(msg, bx, by, bz, q)
+        return msg  # unknown type — pass through untransformed
+
     # ------------------------------------------------------------------ #
     # Registry management                                                  #
     # ------------------------------------------------------------------ #
 
     def _update_registry(self, msg: PeerProfileMsg) -> None:
-        """Update registry with latest-wins semantics, then republish snapshot."""
-        existing = self._registry.get(msg.robot_name)
-        if existing is not None:
-            # Only update if the incoming message is newer
-            new_stamp = msg.gps_fix.header.stamp.sec + msg.gps_fix.header.stamp.nanosec * 1e-9
-            old_stamp = existing.gps_fix.header.stamp.sec + existing.gps_fix.header.stamp.nanosec * 1e-9
-            if new_stamp <= old_stamp:
-                return
-
-        self._registry[msg.robot_name] = msg
+        """Update registry, then republish snapshot.
+
+        Accept only if the incoming message is newer (or equal) to what we
+        already have.  gossip_node stamps gps_fix.header with the ROS clock
+        at publish time, so this gives strict monotonic ordering — stale or
+        out-of-order messages are silently dropped.
+        """
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+
+        with self._registry_lock:
+            existing = self._registry.get(msg.robot_name)
+            if existing is not None:
+                old_t = (existing.gps_fix.header.stamp.sec
+                         + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                if new_t < old_t:
+                    return  # out-of-order — discard
+            self._registry[msg.robot_name] = msg
+
         self._registry_pub.publish(msg)
 
 
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
index 8af1b3ee..1b3ccbac 100755
--- a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
@@ -22,6 +22,7 @@
 import argparse
 import os
 import sys
+import threading
 import time
 
 import rclpy
@@ -31,7 +32,7 @@
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 
 GOSSIP_QOS = QoSProfile(
-    reliability=QoSReliabilityPolicy.BEST_EFFORT,
+    reliability=QoSReliabilityPolicy.RELIABLE,
     history=QoSHistoryPolicy.KEEP_LAST,
     depth=10,
 )
@@ -86,8 +87,15 @@ class RegistryMonitor(Node):
     def __init__(self, filter_name: str = ""):
         super().__init__("peer_registry_monitor")
         self._registry: dict[str, PeerProfileMsg] = {}
+        self._recv_times: dict[str, float] = {}  # robot_name -> wall time of last received msg
+        self._registry_lock = threading.Lock()
         self._filter = filter_name.lower()
 
+        # Per-robot inbox: buffer incoming messages so all robots are processed
+        # together on each drain, regardless of DDS arrival burst order.
+        self._inbox: dict[str, PeerProfileMsg] = {}
+        self._inbox_lock = threading.Lock()
+
         self._sub = self.create_subscription(
             PeerProfileMsg,
             "/gossip/peers",
@@ -96,28 +104,56 @@ def __init__(self, filter_name: str = ""):
         )
 
     def _on_msg(self, msg: PeerProfileMsg) -> None:
-        existing = self._registry.get(msg.robot_name)
-        if existing is not None:
-            new_t = msg.gps_fix.header.stamp.sec + msg.gps_fix.header.stamp.nanosec * 1e-9
-            old_t = existing.gps_fix.header.stamp.sec + existing.gps_fix.header.stamp.nanosec * 1e-9
-            if new_t <= old_t:
-                return
-        self._registry[msg.robot_name] = msg
+        """Buffer incoming message; always track receipt time for liveness display."""
+        new_t = (msg.gps_fix.header.stamp.sec
+                 + msg.gps_fix.header.stamp.nanosec * 1e-9)
+        with self._inbox_lock:
+            existing = self._inbox.get(msg.robot_name)
+            if existing is not None:
+                old_t = (existing.gps_fix.header.stamp.sec
+                         + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                if new_t < old_t:
+                    # Already buffered a newer message — only update recv time
+                    self._recv_times[msg.robot_name] = time.time()
+                    return
+            self._inbox[msg.robot_name] = msg
+            self._recv_times[msg.robot_name] = time.time()
+
+    def _drain_inbox(self) -> None:
+        """Apply registry ordering check for all buffered robots at once."""
+        with self._inbox_lock:
+            inbox = dict(self._inbox)
+            self._inbox.clear()
+        for robot_name, msg in inbox.items():
+            new_t = (msg.gps_fix.header.stamp.sec
+                     + msg.gps_fix.header.stamp.nanosec * 1e-9)
+            with self._registry_lock:
+                existing = self._registry.get(robot_name)
+                if existing is not None:
+                    old_t = (existing.gps_fix.header.stamp.sec
+                             + existing.gps_fix.header.stamp.nanosec * 1e-9)
+                    if new_t < old_t:
+                        continue  # out-of-order — keep existing
+                self._registry[robot_name] = msg
 
     def print_registry(self) -> None:
+        self._drain_inbox()
         _clear()
         domain = os.environ.get("ROS_DOMAIN_ID", "?")
-        now = time.strftime("%H:%M:%S")
-        print(f"{BOLD}Peer Registry  {DIM}[domain={domain}  {now}]{RESET}")
+        now_str = time.strftime("%H:%M:%S")
+        print(f"{BOLD}Peer Registry  {DIM}[domain={domain}  {now_str}]{RESET}")
         print("─" * 80)
 
-        entries = sorted(self._registry.values(), key=lambda m: m.robot_name)
+        with self._registry_lock:
+            entries = sorted(self._registry.values(), key=lambda m: m.robot_name)
+            recv_times = dict(self._recv_times)
         if self._filter:
             entries = [e for e in entries if self._filter in e.robot_name.lower()]
 
         if not entries:
             print(f"  {DIM}(no peers seen yet){RESET}")
         else:
+            now = time.time()
             for msg in entries:
                 src = "direct" if msg.source == 0 else f"relayed({msg.relay_hops}h)"
                 payload_summary = (
@@ -126,8 +162,11 @@ def print_registry(self) -> None:
                     if msg.payloads
                     else "no payloads"
                 )
-                stamp_str = _fmt_stamp(msg.gps_fix)
-                print(f"  {CYAN}{BOLD}{msg.robot_name}{RESET}  {DIM}[{src}  stamp={stamp_str}]{RESET}")
+                recv_t = recv_times.get(msg.robot_name)
+                age = f"{now - recv_t:.1f}s ago" if recv_t is not None else "?"
+                recv_wall = time.strftime("%H:%M:%S", time.localtime(recv_t)) if recv_t else "?"
+                stamp_str = f"{recv_wall}  ({age})"
+                print(f"  {CYAN}{BOLD}{msg.robot_name}{RESET}  {DIM}[{src}  last_recv={stamp_str}]{RESET}")
                 print(f"    {GREEN}gps     {RESET} {_fmt_gps(msg.gps_fix, msg.heading)}")
                 print(f"    {YELLOW}waypoint{RESET} {_fmt_waypoint(msg.waypoint)}")
                 print(f"    {DIM}payloads{RESET} {payload_summary}")
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
index 988cecd9..0c0d0f56 100644
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -9,16 +9,14 @@
 # Start Isaac Sim's simulation environment (Must start this before importing omni modules)
 simulation_app = SimulationApp({"headless": False})
 
+import time
+
 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
@@ -32,6 +30,10 @@
     sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
 from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
 
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
+import scene_prep
+from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
+
 
 # --------------------- CONFIGURATION ---------------------
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
@@ -93,33 +95,17 @@ def nucleus_stat(url: str) -> bool:
     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)
+def wait_for_stage(stage, timeout_s: float = 10.0):
+    """Pump the Kit app loop until /World has content (scene fully loaded)."""
+    for _ in range(int(timeout_s / 0.1)):
+        omni.kit.app.get_app().update()
+        world_prim = stage.GetPrimAtPath("/World")
+        if world_prim.IsValid():
+            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
+            if non_physics:
+                return True
+        time.sleep(0.1)
+    return False
 
 
 class PegasusApp:
@@ -148,48 +134,24 @@ def __init__(self):
         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)
+        if not wait_for_stage(stage):
+            carb.log_warn("Stage load timed out — continuing anyway.")
 
-        world_prim = stage.GetPrimAtPath("/World")
-
-        # Scale the /World/stage prim
+        # ----- Scene preparation -----
         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
+            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+            add_colliders(stage_prim)
             for _ in range(10):
                 omni.kit.app.get_app().update()
-
         else:
-            print("Warning: /World/stage not found, environment not scaled")
+            carb.log_warn("/World/stage not found — skipping scale and collision.")
 
-        # Lighting
         if ADD_DOME_LIGHT:
-            add_dome_light(stage)
+            add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
 
         # Units
-        mpu, s = get_stage_scale(stage)
+        mpu, s = get_stage_meters_per_unit(stage)
 
         # Spawn all drones
         for cfg in DRONE_CONFIGS:
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index f8fe702c..f6947f9f 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -6,16 +6,16 @@
 # Start Isaac Sim's simulation environment (Must start this before importing omni modules)
 simulation_app = SimulationApp({"headless": False})
 
+import os
+import sys
+import time
+
 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
@@ -23,6 +23,16 @@
 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
 
+# gps_utils lives in the same directory as this script
+_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
+if _LAUNCH_SCRIPTS_DIR not in sys.path:
+    sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
+from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
+import scene_prep
+from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
+
 
 # --------------------- CONFIGURATION ---------------------
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
@@ -30,12 +40,11 @@
 #env/stage path and scale
 ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/ACFA/FireAcademyFaro/fire_academy_fixed_textures.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}/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"
+DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
 
 # Lighting
 ADD_DOME_LIGHT = True
@@ -43,15 +52,23 @@
 DOME_LIGHT_INTENSITY = 3500.0
 DOME_LIGHT_EXPOSURE = -3.0
 
-#Drone offset
+# 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.
+WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
+
+# Drone spawn configs.
+# x_m = East offset from world origin (meters)
+# y_m = North offset from world origin (meters)
+# z_m = Up offset / height above floor (meters)
+# orient = initial quaternion [x, y, z, w]
+# 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.07
-
-# spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd
-# DRONE1_XY_M = (20.0, -7.0)
-# DRONE2_XY_M = (17, 1.5)
-
-DRONE1_XY_M = (27.0, 7.6)
-DRONE2_XY_M = (23, 9.8)
+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": 3.5},
+]
 # ---------------------------------------------------------
 
 
@@ -83,38 +100,26 @@ def nucleus_stat(url: str) -> bool:
     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)
+def wait_for_stage(stage, timeout_s: float = 10.0):
+    """Pump the Kit app loop until /World has content (scene fully loaded)."""
+    for _ in range(int(timeout_s / 0.1)):
+        omni.kit.app.get_app().update()
+        world_prim = stage.GetPrimAtPath("/World")
+        if world_prim.IsValid():
+            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
+            if non_physics:
+                return True
+        time.sleep(0.1)
+    return False
 
 
 class PegasusApp:
 
     def __init__(self):
+        # Write GPS origins immediately so robot containers can read them
+        # before this container finishes its heavy USD loading.
+        set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
+
         omni.client.initialize()
         nucleus_stat(f"omniverse://{NUCLEUS_SERVER}")
         nucleus_stat(ENV_URL)
@@ -134,142 +139,64 @@ def __init__(self):
         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")
+        if not wait_for_stage(stage):
+            carb.log_warn("Stage load timed out — continuing anyway.")
 
-        # Scale the /World/stage prim
+        # ----- Scene preparation -----
         stage_prim = stage.GetPrimAtPath("/World/stage")
         if stage_prim.IsValid():
-            xformable = UsdGeom.Xformable(stage_prim)
-
-            translate_attr = stage_prim.GetAttribute("xformOp:translate")
-            if translate_attr.IsValid():
-                if str(translate_attr.GetTypeName()) == "float3":
-                    translate_attr.Set(Gf.Vec3f(0.0, 0.0, 0.0))
-                else:
-                    translate_attr.Set(Gf.Vec3d(0.0, 0.0, 0.0))
-            else:
-                # If missing, create a new translate op.
-                translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
-                translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
-
-            scale_attr = stage_prim.GetAttribute("xformOp:scale")
-            if scale_attr.IsValid():
-                if str(scale_attr.GetTypeName()) == "double3":
-                    scale_attr.Set(Gf.Vec3d(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
-                else:
-                    scale_attr.Set(Gf.Vec3f(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
-            else:
-                # If missing, create a new scale op.
-                scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionFloat)
-                scale_op.Set(Gf.Vec3f(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
-            
-            add_collision_to_prim(stage_prim)
-            print("Finished adding collisions.")
-            
-            # Let the app process the changes
+            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+            add_colliders(stage_prim)
             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")
+            carb.log_warn("/World/stage not found — skipping scale and collision.")
 
-        # Lighting
         if ADD_DOME_LIGHT:
-            add_dome_light(stage)
+            add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
 
         # 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.937, 0.35], #[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 = 4.0, # 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.937, 0.35], #[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 = 3.5 
-        )
+        mpu, s = get_stage_meters_per_unit(stage)
+
+        # Spawn all drones
+        for cfg in DRONE_CONFIGS:
+            i = cfg["domain_id"]
+            pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
+
+            graph_handle = spawn_px4_multirotor_node(
+                pegasus_node_name=f"PX4Multirotor_{i}",
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                vehicle_id=i,
+                domain_id=i,
+                usd_file=DRONE_USD,
+                init_pos=pos,
+                init_orient=cfg["orient"],
+            )
+
+            add_zed_stereo_camera_subgraph(
+                parent_graph_handle=graph_handle,
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                camera_name="ZEDCamera",
+                camera_offset=[0.1, 0.0, 0.0],
+                camera_rotation_offset=[0.0, 0.0, 0.0],
+            )
+
+            add_ouster_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_offset=[0.0, 0.0, 0.025],
+                lidar_rotation_offset=[0.0, 0.0, 0.0],
+                lidar_min_range=cfg["lidar_min_range"],
+            )
+
+        # Give physics time to register the spawned drone prims before reset.
+        # Complex scenes (e.g. fire academy) need more ticks than simpler ones.
+        for _ in range(30):
+            omni.kit.app.get_app().update()
 
         # Reset so physics/articulations are ready
         self.world.reset()
@@ -300,4 +227,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    main()
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
index e5c911e2..2c8b3a94 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
@@ -6,16 +6,16 @@
 # Start Isaac Sim's simulation environment (Must start this before importing omni modules)
 simulation_app = SimulationApp({"headless": False})
 
+import os
+import sys
+import time
+
 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
@@ -23,17 +23,27 @@
 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
 
+# gps_utils lives in the same directory as this script
+_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
+if _LAUNCH_SCRIPTS_DIR not in sys.path:
+    sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
+from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
+sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
+import scene_prep
+from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
+
 
 # --------------------- CONFIGURATION ---------------------
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
 
 #env/stage path and scale
-ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.usd"
+#ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Stages/RetroNeighborhood/RetroNeighborhood.stage.usd"
+ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/ACFA/FireAcademyFaro/fire_academy_fixed_textures.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}/Projects/AirStack/RayFronts-Planner/FireAcademy.scene.usd"
 #f"omniverse://{NUCLEUS_SERVER}/Library/Assets/Fire_Academy_Digital_Twin/fire_academy.usd"
-STAGE_SCALE = 2.0
-
+STAGE_SCALE = 0.01
 
 DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
 
@@ -43,11 +53,20 @@
 DOME_LIGHT_INTENSITY = 3500.0
 DOME_LIGHT_EXPOSURE = -3.0
 
-#Drone offset
-SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
+# 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.
+WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
 
-DRONE1_XY_M = (20.0, -7)
-DRONE2_XY_M = (17, 1.5)
+# Drone spawn configs.
+# x_m = East offset from world origin (meters)
+# y_m = North offset from world origin (meters)
+# z_m = Up offset / height above floor (meters)
+# orient = initial quaternion [x, y, z, w]
+SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
+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.0, 1.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.0, 1.0]},
+]
 # ---------------------------------------------------------
 
 
@@ -79,38 +98,26 @@ def nucleus_stat(url: str) -> bool:
     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)
+def wait_for_stage(stage, timeout_s: float = 10.0):
+    """Pump the Kit app loop until /World has content (scene fully loaded)."""
+    for _ in range(int(timeout_s / 0.1)):
+        omni.kit.app.get_app().update()
+        world_prim = stage.GetPrimAtPath("/World")
+        if world_prim.IsValid():
+            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
+            if non_physics:
+                return True
+        time.sleep(0.1)
+    return False
 
 
 class PegasusApp:
 
     def __init__(self):
+        # Write GPS origins immediately so robot containers can read them
+        # before this container finishes its heavy USD loading.
+        set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
+
         omni.client.initialize()
         nucleus_stat(f"omniverse://{NUCLEUS_SERVER}")
         nucleus_stat(ENV_URL)
@@ -130,127 +137,64 @@ def __init__(self):
         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")
+        if not wait_for_stage(stage):
+            carb.log_warn("Stage load timed out — continuing anyway.")
 
-        # Scale the /World/stage prim
+        # ----- Scene preparation -----
         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
+            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
+            add_colliders(stage_prim)
             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")
+            carb.log_warn("/World/stage not found — skipping scale and collision.")
 
-        # Lighting
         if ADD_DOME_LIGHT:
-            add_dome_light(stage)
+            add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
 
         # 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.2, 0.0, -0.05],  # 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.2, 0.0, -0.05],  # 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 
-        )
+        mpu, s = get_stage_meters_per_unit(stage)
+
+        # Spawn all drones
+        for cfg in DRONE_CONFIGS:
+            i = cfg["domain_id"]
+            pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
+
+            graph_handle = spawn_px4_multirotor_node(
+                pegasus_node_name=f"PX4Multirotor_{i}",
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                vehicle_id=i,
+                domain_id=i,
+                usd_file=DRONE_USD,
+                init_pos=pos,
+                init_orient=cfg["orient"],
+            )
+
+            add_zed_stereo_camera_subgraph(
+                parent_graph_handle=graph_handle,
+                drone_prim=f"/World/drone{i}/base_link",
+                robot_name=f"robot_{i}",
+                camera_name="ZEDCamera",
+                camera_offset=[0.2, 0.0, -0.05],
+                camera_rotation_offset=[0.0, 0.0, 0.0],
+            )
+
+            add_ouster_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_offset=[0.0, 0.0, 0.025],
+                lidar_rotation_offset=[0.0, 0.0, 0.0],
+                lidar_min_range=0.75,
+            )
+
+        # Give physics time to register the spawned drone prims before reset.
+        # Complex scenes (e.g. fire academy) need more ticks than simpler ones.
+        for _ in range(30):
+            omni.kit.app.get_app().update()
 
         # Reset so physics/articulations are ready
         self.world.reset()
@@ -281,4 +225,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    main()
diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py
index ac03416b..95c49a83 100644
--- a/simulation/isaac-sim/utils/scene_prep.py
+++ b/simulation/isaac-sim/utils/scene_prep.py
@@ -56,11 +56,27 @@ def scale_stage_prim(stage, prim_path: str, scale_factor: float):
     xformable = UsdGeom.Xformable(prim)
     xformable.ClearXformOpOrder()
 
-    translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
-    translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
+    # Match the precision of any existing xform attrs to avoid type-mismatch exceptions.
+    # ClearXformOpOrder removes attrs from the op order but leaves the attributes on the prim,
+    # so AddXformOp will fail if the requested precision doesn't match the baked-in type.
+    def _precision(attr_name, default=UsdGeom.XformOp.PrecisionDouble):
+        attr = prim.GetAttribute(attr_name)
+        if attr.IsValid() and str(attr.GetTypeName()) == "float3":
+            return UsdGeom.XformOp.PrecisionFloat
+        return default
+
+    translate_prec = _precision("xformOp:translate")
+    scale_prec = _precision("xformOp:scale")
+
+    if translate_prec == UsdGeom.XformOp.PrecisionFloat:
+        xformable.AddTranslateOp(translate_prec).Set(Gf.Vec3f(0.0, 0.0, 0.0))
+    else:
+        xformable.AddTranslateOp(translate_prec).Set(Gf.Vec3d(0.0, 0.0, 0.0))
 
-    scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionDouble)
-    scale_op.Set(Gf.Vec3d(scale_factor, scale_factor, scale_factor))
+    if scale_prec == UsdGeom.XformOp.PrecisionFloat:
+        xformable.AddScaleOp(scale_prec).Set(Gf.Vec3f(scale_factor, scale_factor, scale_factor))
+    else:
+        xformable.AddScaleOp(scale_prec).Set(Gf.Vec3d(scale_factor, scale_factor, scale_factor))
 
     print(f"[scene_prep] Scaled '{prim_path}' by {scale_factor}")
     return prim

From 61befb94312f6d84f4f6232c4dc78bcfa88bd91c Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj18@users.noreply.github.com>
Date: Wed, 25 Mar 2026 23:04:29 -0400
Subject: [PATCH 20/55] fixed gossip + translate

---
 .../desktop_bringup/launch/gcs.launch.xml     |  3 ++
 .../gcs_visualizer/payload_visualizer_node.py | 20 ++++++++++-
 .../onboard_all/config/dds_router.yaml        |  4 +--
 .../onboard_all/config/domain_bridge.yaml     |  7 +++-
 .../config/gcs_gossip_dds_router.yaml         | 17 +++++++++
 .../coordination_bringup/gossip_node.py       | 36 ++++++++++++++++---
 .../launch/gcs_gossip_bridge.launch.py        | 29 +++++++++++++++
 .../scripts/peer_registry_monitor.py          |  2 +-
 .../coordination_bringup/setup.py             |  4 +--
 9 files changed, 111 insertions(+), 11 deletions(-)
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
 create mode 100644 robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py

diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index 9e286ad9..f912be0c 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -31,6 +31,9 @@
 
    
 
+  <!-- Gossip bridge: connects GCS domain (0) to gossip bus (domain 99) -->
+  <include file="$(find-pkg-share coordination_bringup)/launch/gcs_gossip_bridge.launch.py" />
+
   <!-- GCS Visualizer: drone mesh markers in global ENU frame -->
   <include file="$(find-pkg-share gcs_visualizer)/launch/gcs_visualizer.launch.xml" />
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
index 318d1dde..193ffdf7 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -33,6 +33,8 @@ def _handle_<name>(self, robot_name, msg, i, now):
         # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
 """
 
+from collections import OrderedDict
+
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
@@ -51,8 +53,12 @@ def _handle_<name>(self, robot_name, msg, i, now):
 _GOSSIP_ORIGIN_ALT = 90.0
 
 
+# Seen-set size: same reasoning as gossip_node — covers well beyond the window
+# in which DDS Router duplicates arrive.
+_GOSSIP_SEEN_SIZE = 50
+
 GOSSIP_QOS = QoSProfile(
-    reliability=ReliabilityPolicy.RELIABLE,
+    reliability=ReliabilityPolicy.BEST_EFFORT,
     durability=DurabilityPolicy.VOLATILE,
     history=HistoryPolicy.KEEP_LAST,
     depth=10,
@@ -67,6 +73,7 @@ def __init__(self):
         self._alt_ground    = None # altitude (m) of first GPS fix — display z datum
         self._payload_cache = {}   # (robot_name, cache_key) -> last deserialized payload msg
         self._pubs          = {}   # topic -> Publisher (created lazily)
+        self._seen: OrderedDict = OrderedDict()  # seen-set for duplicate suppression
 
         # Subscribe to /gossip/peers — PeerProfiles arrive here from all robots
         self.create_subscription(
@@ -114,6 +121,17 @@ def _display_z_offset(self) -> float:
     def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
         robot_name = msg.robot_name
 
+        # Seen-set deduplication: drop exact duplicates before any other work.
+        # Key is (robot_name, sec, nanosec) — robot_name prevents cross-robot collisions.
+        msg_id = (robot_name,
+                  msg.gps_fix.header.stamp.sec,
+                  msg.gps_fix.header.stamp.nanosec)
+        if msg_id in self._seen:
+            return
+        self._seen[msg_id] = None
+        if len(self._seen) > _GOSSIP_SEEN_SIZE:
+            self._seen.popitem(last=False)
+
         # Per-robot ordering: accept only if this message is newer than the last
         # accepted one for THIS robot.  robot_1 and robot_2 are tracked independently
         # so neither can block the other.
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index 5b2cea84..a6eda979 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -65,5 +65,5 @@ allowlist:
   # control commands
   - name: "rt/$(env ROBOT_NAME)/fixed_trajectory_generator/fixed_trajectory_command"
 
-  # gossip peer profiles (carries all robot payloads — no need to bridge individual topics)
-  - name: "rt/gossip/peers"
+  # gossip peer profiles are bridged by the dedicated gossip_dds_router (domain 99),
+  # NOT here — having it in both routers causes message flooding/amplification.
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml
index 326e2225..423554be 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml
@@ -41,4 +41,9 @@ topics:
   bag_record/set_recording_status:
     type: std_msgs/msg/Bool
     from_domain: $(var gcs_domain)
-    to_domain: $(env ROS_DOMAIN_ID)
\ No newline at end of file
+    to_domain: $(env ROS_DOMAIN_ID)
+
+  /robot_1/current_target:
+    type: std_msgs/msg/String
+    from_domain: 1
+    to_domain: 2
\ No newline at end of file
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml b/robot/ros_ws/src/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
new file mode 100644
index 00000000..0b9b80a0
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
@@ -0,0 +1,17 @@
+# GCS-side gossip DDS Router configuration
+# Bridges /gossip/peers between the GCS domain (0) and the shared gossip
+# domain (99) so the GCS can receive PeerProfile broadcasts from all robots.
+#
+# This runs in the GCS container.  Each robot runs its own gossip_dds_router
+# (robot domain ↔ domain 99).  This is the GCS counterpart.
+
+participants:
+  - name: "gcs"
+    kind: "local"
+    domain: 0    # GCS ROS_DOMAIN_ID
+  - name: "gossip_bus"
+    kind: "local"
+    domain: 99   # shared gossip domain
+
+allowlist:
+  - name: "rt/gossip/peers"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
index d727addb..d5ae6c9b 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -19,6 +19,7 @@
 import os
 import threading
 import yaml
+from collections import OrderedDict
 
 import rclpy
 from rclpy.node import Node
@@ -45,9 +46,16 @@
 from sensor_msgs.msg import PointCloud2
 
 
-# QoS for gossip topic: best-effort, keep last 1 (low-overhead, high-freq)
+# Seen-set size: deduplicates the last N unique (robot_name, stamp) pairs.
+# At 1 Hz per robot, 50 entries covers ~25 s for 2 robots or ~5 s for 10 robots —
+# well beyond the millisecond window in which DDS Router duplicates arrive.
+_GOSSIP_SEEN_SIZE = 50
+
+# QoS for gossip topic: BEST_EFFORT — at 1 Hz a dropped message recovers on the
+# next tick, so reliability overhead (heartbeats, ACKs across domain bridges) is
+# unnecessary and was the main source of DDS-layer amplification.
 GOSSIP_QOS = QoSProfile(
-    reliability=QoSReliabilityPolicy.RELIABLE,
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
     history=QoSHistoryPolicy.KEEP_LAST,
     depth=10,
 )
@@ -102,6 +110,10 @@ def __init__(self):
         self._peer_inbox: dict[str, PeerProfileMsg] = {}
         self._peer_inbox_lock = threading.Lock()
 
+        # Seen-set: deduplicates incoming peer messages by (robot_name, sec, nanosec).
+        # Including robot_name in the key ensures robot_1 and robot_2 never share IDs.
+        self._seen: OrderedDict = OrderedDict()
+
         # Payload cache: topic → (msg, stamp) or None until first message arrives
         self._payload_cache: dict[str, object] = {}
         self._payload_subs: list = []
@@ -254,6 +266,19 @@ def _on_global_plan(self, msg: Path) -> None:
     def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
         if msg.robot_name == self._robot_name:
             return  # discard own messages echoed back from the gossip domain
+
+        # Seen-set deduplication: discard if this exact (robot, stamp) was already
+        # processed.  Key includes robot_name so robot_1 and robot_2 can never share
+        # an ID even when their sequence counters coincide.
+        msg_id = (msg.robot_name,
+                  msg.gps_fix.header.stamp.sec,
+                  msg.gps_fix.header.stamp.nanosec)
+        if msg_id in self._seen:
+            return
+        self._seen[msg_id] = None
+        if len(self._seen) > _GOSSIP_SEEN_SIZE:
+            self._seen.popitem(last=False)  # evict oldest entry
+
         # Stage in per-robot inbox: keep only the newest message per robot so
         # all robots are drained together on the next tick (fair processing).
         new_t = (msg.gps_fix.header.stamp.sec
@@ -290,9 +315,12 @@ def _publish_own(self) -> None:
         # If boot pose is not yet known, skip payloads this tick — we cannot
         # produce a meaningful global-frame transform without GPS.
         self._profile.clear_payloads()
-        if self._boot_pos is not None and self._boot_quat is not None:
+        if self._boot_pos is not None:
             bx, by, bz = self._boot_pos
-            q = self._boot_quat
+            # PX4/MAVROS odom frame is ENU-aligned — x=East, y=North, z=Up
+            # regardless of drone heading.  Only translation is needed; applying
+            # heading rotation would incorrectly rotate the payload data.
+            q = (0.0, 0.0, 0.0, 1.0)
             for topic, entry in self._payload_cache.items():
                 if entry is not None:
                     msg, stamp = entry
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py b/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
new file mode 100644
index 00000000..03bec2dd
--- /dev/null
+++ b/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
@@ -0,0 +1,29 @@
+"""
+gcs_gossip_bridge.launch.py
+============================
+Launches the GCS-side DDS Router that bridges /gossip/peers between the GCS
+domain (0) and the shared gossip bus domain (99).
+
+Include this from gcs.launch.xml so the GCS can receive PeerProfile messages
+from all robots without going through each robot's onboard DDS router.
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('coordination_bringup'),
+        'config', 'gcs_gossip_dds_router.yaml',
+    )
+    return LaunchDescription([
+        ExecuteProcess(
+            cmd=['ddsrouter', '-c', config],
+            output='screen',
+            name='gcs_gossip_dds_router',
+        ),
+    ])
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
index 1b3ccbac..6a140d3e 100755
--- a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
@@ -32,7 +32,7 @@
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 
 GOSSIP_QOS = QoSProfile(
-    reliability=QoSReliabilityPolicy.RELIABLE,
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,
     history=QoSHistoryPolicy.KEEP_LAST,
     depth=10,
 )
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/setup.py b/robot/ros_ws/src/coordination/coordination_bringup/setup.py
index 381b0fe0..e670cf8d 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/setup.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/setup.py
@@ -10,8 +10,8 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name + '/launch', ['launch/gossip.launch.xml']),
-        ('share/' + package_name + '/config', ['config/gossip_dds_router.yaml', 'config/gossip_payloads.yaml']),
+        ('share/' + package_name + '/launch', ['launch/gossip.launch.xml', 'launch/gcs_gossip_bridge.launch.py']),
+        ('share/' + package_name + '/config', ['config/gossip_dds_router.yaml', 'config/gossip_payloads.yaml', 'config/gcs_gossip_dds_router.yaml']),
         ('lib/' + package_name, ['scripts/gossip_node', 'scripts/peer_registry_monitor.py']),
     ],
     install_requires=['setuptools'],

From ed4abfe5e31e049200e1690ee891d5b1e5435c14 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 27 Mar 2026 14:35:43 -0400
Subject: [PATCH 21/55] added skill for foxglove/coordination

---
 .agents/skills/attach-gossip-payload/SKILL.md |  34 ++++
 .env                                          |   6 +-
 .../foxglove_visualizer_node.py               |  46 ++---
 .../gcs_visualizer/gcs_utils.py               |  41 +----
 .../gcs_visualizer/payload_visualizer_node.py | 102 +++--------
 robot/ros_ws/src/coordination/README.md       |  76 +++++++++
 .../coordination_bringup/frame_utils.py       |  38 +----
 .../coordination_bringup/gossip_node.py       | 159 ++++--------------
 .../coordination_bringup/peer_profile.py      |  96 ++---------
 .../launch/gcs_gossip_bridge.launch.py        |  10 +-
 .../scripts/peer_registry_monitor.py          |  27 +--
 11 files changed, 216 insertions(+), 419 deletions(-)
 create mode 100644 robot/ros_ws/src/coordination/README.md

diff --git a/.agents/skills/attach-gossip-payload/SKILL.md b/.agents/skills/attach-gossip-payload/SKILL.md
index 351f8a9f..9709995f 100644
--- a/.agents/skills/attach-gossip-payload/SKILL.md
+++ b/.agents/skills/attach-gossip-payload/SKILL.md
@@ -186,6 +186,40 @@ docker exec airstack-gcs-1 bash -c "ros2 topic list | grep /gcs/payload"
 - Payload size matters for gossip bandwidth — avoid attaching large point clouds
   at high rates; 1 Hz (the default gossip rate) is usually fine
 
+## Message deduplication
+
+Every `PeerProfile` message is identified by the triple:
+
+```
+(robot_name, gps_fix.header.stamp.sec, gps_fix.header.stamp.nanosec)
+```
+
+The stamp is set **at publish time** by the originating robot. Each receiver
+maintains a seen-set (size 50, FIFO eviction) and drops any message whose ID
+has already been processed.
+
+**Expected behaviour:** every drone will forward/receive a message at least
+once — this is intentional. The seen-set prevents infinite re-processing but
+does not prevent the initial fan-out that comes from all robots being on the
+same shared DDS domain.
+
+**Relay fields (reserved for future use):**
+- `uint8 source` — `SOURCE_DIRECT (0)` or `SOURCE_RELAYED (1)`
+- `uint8 relay_hops` — number of hops the message has traversed
+
+These fields exist in the wire format and Python API but relay logic is not
+yet implemented. The seen-set deduplication is already wired to handle it
+correctly when relay is activated.
+
+## Registry behaviour
+
+- Each robot keeps a **per-robot inbox** (latest message per peer, drained at
+  5 Hz) and a **global registry** (latest-wins, monotonic per robot timestamp)
+- Registry entries are **never evicted** — a crashed robot stays visible
+  indefinitely until the node is restarted
+- The registry is published to `/{robot_name}/coordination/peer_registry` with
+  RELIABLE + TRANSIENT_LOCAL QoS so late-joining nodes get the full snapshot
+
 ## QoS note
 
 Payload subscriptions use `GOSSIP_QOS` (BEST_EFFORT, KEEP_LAST 1). If your
diff --git a/.env b/.env
index c58dd6b7..ba32b8aa 100644
--- a/.env
+++ b/.env
@@ -26,7 +26,7 @@ COMPOSE_PROFILES="desktop"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="2"  # Number of robot containers to launch.
+NUM_ROBOTS="3"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
@@ -34,7 +34,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py"
+ISAAC_SIM_SCRIPT_NAME="three_drone_scene_import.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
@@ -45,7 +45,7 @@ MS_AIRSIM_BINARY_PATH="/ms-airsim-env/AbandonedPark/LinuxNoEditor/AbandonedPark.
 
 # ================= ROBOT =====================
 ROBOT_NAME_MAP_CONFIG_FILE="default_robot_name_map.yaml"  # Determines how to set ROBOT_NAME and ROS_DOMAIN_ID. See robot/docker/robot_name_map/
-VDB_SOURCE="lidar"  # "lidar" (Ouster, sensors/ouster/point_cloud) or "stereo" (stereo_image_proc/point_cloud)
+
 
 URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 5ab9106b..38173345 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -1,20 +1,9 @@
 """
-foxglove_visualizer_node.py
-===========================
-Standard GCS visualization node. Handles data common to every project:
-  - Robot mesh markers (with orientation)
-  - Robot name labels
-  - Body-frame axes (X/Y/Z arrows)
-  - Local trajectory (trajectory_controller/trajectory_vis)
-  - Global plan (global_plan)
-  - VDB occupancy map (vdb_mapping/vdb_map_visualization)
-
-All markers are published to /gcs/robot_markers as a MarkerArray in the
-global ENU 'map' frame.
-
-Project-specific data (e.g. Rayfronts) should go in a separate node that
-also publishes to its own MarkerArray or PointCloud2 topic. See
-payload_visualizer_node.py as an example.
+foxglove_visualizer_node.py — Standard GCS visualization node.
+
+Handles data common to every project: robot mesh markers, name labels,
+body-frame axes, local trajectory, global plan, and VDB occupancy map.
+All markers are published to /gcs/robot_markers in the global ENU 'map' frame.
 """
 
 import copy
@@ -53,8 +42,8 @@
     (0.2, 0.2, 1.0),  # blue
 ]
 
-# OBJ -> ROS axis correction quaternion (belly -Z, nose +X)
-AXIS_CORRECTION = (-0.5, -0.5, 0.5, 0.5)  # x, y, z, w
+# OBJ mesh axis correction quaternion (belly -Z, nose +X)
+AXIS_CORRECTION = (-0.5, -0.5, 0.5, 0.5)
 
 
 class FoxgloveVisualizerNode(Node):
@@ -69,12 +58,12 @@ def __init__(self):
         self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
         self._vdb_pattern  = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(VDB_SUFFIX)}$')
 
-        self._gps_positions  = {}   # robot_name -> (x, y, z) ENU metres current position
-        self._gps_boot       = {}   # robot_name -> (x, y, z) ENU metres at first fix (odom origin)
-        self._orientations   = {}   # robot_name -> (x, y, z, w) from odometry
-        self._trajectories   = {}   # robot_name -> latest MarkerArray
-        self._global_plans   = {}   # robot_name -> latest nav_msgs/Path
-        self._vdb_markers    = {}   # robot_name -> latest VDB Marker
+        self._gps_positions  = {}
+        self._gps_boot       = {}
+        self._orientations   = {}
+        self._trajectories   = {}
+        self._global_plans   = {}
+        self._vdb_markers    = {}
         self._subscribed_gps  = set()
         self._subscribed_odom = set()
         self._subscribed_traj = set()
@@ -191,7 +180,7 @@ def _publish_markers(self):
 
         for i, robot_name in enumerate(sorted(self._gps_positions.keys())):
             x, y, z = self._gps_positions[robot_name]
-            orientation = self._orientations.get(robot_name)  # (x,y,z,w) or None
+            orientation = self._orientations.get(robot_name)
 
             # --- Mesh marker ---
             mesh = Marker()
@@ -273,7 +262,7 @@ def _publish_markers(self):
 
             boot = self._gps_boot.get(robot_name)
 
-            # --- Trajectory markers (offset by boot GPS position = odom origin) ---
+            # --- Trajectory (offset by boot GPS = odom origin) ---
             traj = self._trajectories.get(robot_name)
             if traj is not None and boot is not None:
                 bx, by, bz = boot
@@ -287,14 +276,13 @@ def _publish_markers(self):
                         m.color.r = 0.8
                         m.color.g = 0.5
                         m.color.b = 0.0
-                    # skip markers that require points but have none
                     needs_points = m.type in (Marker.LINE_STRIP, Marker.LINE_LIST,
                                               Marker.POINTS, Marker.ARROW)
                     if needs_points and len(m.points) == 0:
                         continue
                     array.markers.append(m)
 
-            # --- Global plan (offset from odom origin to ENU) ---
+            # --- Global plan ---
             plan = self._global_plans.get(robot_name)
             if plan is not None and boot is not None:
                 bx, by, bz = boot
@@ -319,7 +307,7 @@ def _publish_markers(self):
                 if len(line.points) >= 2:
                     array.markers.append(line)
 
-            # --- VDB map (offset from odom origin to ENU) ---
+            # --- VDB map ---
             vdb = self._vdb_markers.get(robot_name)
             if vdb is not None and boot is not None:
                 bx, by, bz = boot
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
index 760e4ab2..650d6e1d 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/gcs_utils.py
@@ -1,27 +1,9 @@
-"""
-gcs_utils.py
-============
-Shared utilities for GCS visualizer nodes.
-
-Coordinate system
------------------
-All visualizers transform data from each robot's local odometry frame into
-a common ENU (East-North-Up) frame anchored at the first GPS fix seen on the
-ground (``alt_ground``). This ENU frame is published as the ``map`` frame in
-Foxglove.
-
-The key concept is the **boot offset**: the ENU coordinates of a robot's first
-GPS fix, which equals that robot's odometry frame origin. Any data expressed in
-the robot's local odometry frame must be translated by this boot offset to
-display correctly alongside other robots in the global map frame.
-"""
+"""Shared utilities for GCS visualizer nodes."""
 
 import math
 import copy
 import struct
 
-# Shared frame utilities live in coordination_bringup so both robot and GCS
-# packages can import them.  Re-export the ones callers expect from here.
 from coordination_bringup.frame_utils import (
     gps_to_enu as _gps_to_enu_abs,
     heading_to_quat,
@@ -30,19 +12,13 @@
     transform_point_cloud2 as _transform_pc2,
 )
 
-# ── Origin ────────────────────────────────────────────────────────────────────
 ORIGIN_LAT = 38.736832
 ORIGIN_LON = -9.137977
 ORIGIN_ALT = 90.0
 
 
-# ── GPS / coordinate helpers ──────────────────────────────────────────────────
-
 def gps_to_enu(lat, lon, alt, alt_ground):
-    """Convert GPS lat/lon/alt to ENU metres relative to the world origin.
-
-    z is relative to ``alt_ground`` (the altitude of the first GPS fix seen).
-    """
+    """Convert GPS lat/lon/alt to ENU metres. z is relative to alt_ground."""
     x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
     y = (lat - ORIGIN_LAT) * 111320.0
     z = alt - alt_ground
@@ -65,13 +41,9 @@ def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetim
                                 fallback_color=None, scale=0.2):
     """Convert a PointCloud2 to a CUBE_LIST Marker translated into the map frame.
 
-    Extracts x, y, z fields and applies the boot offset (bx, by, bz).
-    If the cloud has an ``rgb`` field (PCL-packed uint32: 0x00RRGGBB stored as
-    float32 bytes), per-point colors are written to ``colors[]``.
-    Otherwise ``fallback_color`` (r, g, b, a) is used as a uniform color.
-
-    pose.orientation.w is set to 1.0 — required for CUBE_LIST so the per-point
-    positions are not distorted by an invalid default quaternion.
+    Extracts xyz and applies boot offset (bx, by, bz). If the cloud has an 'rgb'
+    field (PCL-packed uint32 0x00RRGGBB stored as float32 bytes), per-point colors
+    are written to colors[]; otherwise fallback_color (r,g,b,a) is used.
 
     Returns a Marker, or None if the cloud has no x/y/z fields.
     """
@@ -94,7 +66,7 @@ def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetim
     m.header.stamp = stamp
     m.ns = ns
     m.id = marker_id
-    m.pose.orientation.w = 1.0   # identity — must be set or CUBE_LIST distorts positions
+    m.pose.orientation.w = 1.0  # must be set or CUBE_LIST distorts positions
     m.type = Marker.CUBE_LIST
     m.action = Marker.ADD
     m.scale.x = scale
@@ -122,5 +94,4 @@ def point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetim
     return m
 
 
-# transform_point_cloud2 is re-exported from coordination_bringup.frame_utils
 transform_point_cloud2 = _transform_pc2
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
index 193ffdf7..72f55516 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -1,36 +1,20 @@
 """
-payload_visualizer_node.py
-==========================
-Project-specific GCS visualization node. Reads PeerProfile payloads from
-/gossip/peers and publishes each payload to its own topic:
-/gcs/payload/{robot_name}/{payload_name}
-
-Publishing per-topic (rather than a single MarkerArray) lets Foxglove expose
-its full visualization controls (point size, color mapping, etc.) for each payload.
-
-Payloads are defined in gossip_payloads.yaml on the robot side — no DDS router
-changes are needed when adding new payload types.
-
-Coordinate frame note
----------------------
-Payloads arrive already transformed into the global ENU 'map' frame by
-gossip_node on each robot.  No coordinate transform is needed here — handlers
-just set frame_id='map' and republish.
-
-How to add a new payload for visualization
-------------------------------------------
-1. Read gossip_payloads.yaml and find the `type:` field of the new entry.
-2. Add a handler method here (see signature below).
-3. If the type is unique (not already in PAYLOAD_HANDLERS), add it to the dict.
-   If the type already exists (e.g. a second PointCloud2), dispatch by index in
-   _on_peer_profile after the PAYLOAD_HANDLERS loop (order matches yaml order).
+payload_visualizer_node.py — GCS visualization for gossip payloads.
+
+Reads PeerProfile payloads from /gossip/peers and republishes each to its own
+topic (/gcs/payload/{robot_name}/{payload_name}) so Foxglove exposes full
+visualization controls per payload independently.
+
+Payloads arrive already transformed into global ENU by gossip_node — handlers
+only need to apply the display z-offset and republish.
+
+To add a new payload type: add a handler method and register it in PAYLOAD_HANDLERS.
+For duplicate types (e.g. a second PointCloud2), dispatch by index after the
+PAYLOAD_HANDLERS loop in _on_peer_profile (order matches gossip_payloads.yaml).
 
 Handler signature:
     def _handle_<name>(self, robot_name, msg, i, now):
-        # msg  — deserialized ROS message (already in global ENU / 'map' frame)
-        # i    — stable robot index (for marker IDs: i * 100000 + unique_offset)
-        # now  — current ROS timestamp
-        # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
+        # i — stable robot index (for marker IDs: i * 100000 + offset)
 """
 
 from collections import OrderedDict
@@ -48,13 +32,10 @@ def _handle_<name>(self, robot_name, msg, i, now):
 from coordination_bringup.peer_profile import PeerProfile
 from gcs_visualizer.gcs_utils import transform_marker_array, transform_point_cloud2
 
-# ENU origin altitude used by gossip_node when transforming payloads at source.
-# Must match frame_utils.DEFAULT_ORIGIN_ALT.
+# Must match frame_utils.DEFAULT_ORIGIN_ALT — used to compute the z-offset between
+# gossip_node's fixed ENU origin and the GCS display datum (first GPS altitude seen).
 _GOSSIP_ORIGIN_ALT = 90.0
 
-
-# Seen-set size: same reasoning as gossip_node — covers well beyond the window
-# in which DDS Router duplicates arrive.
 _GOSSIP_SEEN_SIZE = 50
 
 GOSSIP_QOS = QoSProfile(
@@ -68,35 +49,25 @@ class PayloadVisualizerNode(Node):
     def __init__(self):
         super().__init__('payload_visualizer_node')
 
-        self._gps_boot      = {}   # robot_name -> True once a valid GPS fix has been seen
-        self._last_stamp    = {}   # robot_name -> float: ROS stamp of last accepted message
-        self._alt_ground    = None # altitude (m) of first GPS fix — display z datum
-        self._payload_cache = {}   # (robot_name, cache_key) -> last deserialized payload msg
-        self._pubs          = {}   # topic -> Publisher (created lazily)
-        self._seen: OrderedDict = OrderedDict()  # seen-set for duplicate suppression
+        self._gps_boot      = {}
+        self._last_stamp    = {}
+        self._alt_ground    = None
+        self._payload_cache = {}
+        self._pubs          = {}
+        self._seen: OrderedDict = OrderedDict()
 
-        # Subscribe to /gossip/peers — PeerProfiles arrive here from all robots
         self.create_subscription(
             PeerProfileMsg, '/gossip/peers',
             self._on_peer_profile, GOSSIP_QOS)
 
         self.get_logger().info('PayloadVisualizerNode started')
 
-    # ── Lazy publisher helper ──────────────────────────────────────────────
-
     def _pub_for(self, topic, msg_type):
         if topic not in self._pubs:
             self._pubs[topic] = self.create_publisher(msg_type, topic, 10)
         return self._pubs[topic]
 
-    # ── GPS boot tracking (for robot index, not transforms) ───────────────
-
     def _update_boot(self, robot_name: str, gps_fix) -> None:
-        """Record that a valid GPS fix has been seen for this robot.
-
-        Also tracks alt_ground (first GPS altitude seen across all robots) so
-        payload z values can be shifted to match the foxglove_visualizer datum.
-        """
         if gps_fix.status.status < 0:
             return
         if self._alt_ground is None:
@@ -105,24 +76,14 @@ def _update_boot(self, robot_name: str, gps_fix) -> None:
             self._gps_boot[robot_name] = True
 
     def _display_z_offset(self) -> float:
-        """Z shift to apply to payload positions before display.
-
-        gossip_node transforms payloads with a fixed ENU origin_alt of 90 m.
-        foxglove_visualizer uses alt_ground (first GPS fix altitude) as its datum.
-        If those two differ, payloads appear above or below the drone.
-        This correction aligns them without touching the peer-profile data.
-        """
+        """Correct for the altitude datum difference between gossip_node and foxglove_visualizer."""
         if self._alt_ground is None:
             return 0.0
         return _GOSSIP_ORIGIN_ALT - self._alt_ground
 
-    # ── Peer profile callback ─────────────────────────────────────────────
-
     def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
         robot_name = msg.robot_name
 
-        # Seen-set deduplication: drop exact duplicates before any other work.
-        # Key is (robot_name, sec, nanosec) — robot_name prevents cross-robot collisions.
         msg_id = (robot_name,
                   msg.gps_fix.header.stamp.sec,
                   msg.gps_fix.header.stamp.nanosec)
@@ -132,9 +93,6 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
         if len(self._seen) > _GOSSIP_SEEN_SIZE:
             self._seen.popitem(last=False)
 
-        # Per-robot ordering: accept only if this message is newer than the last
-        # accepted one for THIS robot.  robot_1 and robot_2 are tracked independently
-        # so neither can block the other.
         new_t = (msg.gps_fix.header.stamp.sec
                  + msg.gps_fix.header.stamp.nanosec * 1e-9)
         if new_t < self._last_stamp.get(robot_name, 0.0):
@@ -144,7 +102,7 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
         self._update_boot(robot_name, msg.gps_fix)
 
         if robot_name not in self._gps_boot:
-            return  # no GPS fix yet, skip
+            return
 
         profile = PeerProfile.from_ros_msg(msg)
         now = self.get_clock().now().to_msg()
@@ -172,16 +130,7 @@ def _robot_index(self, robot_name: str) -> int:
         known = sorted(self._gps_boot.keys())
         return known.index(robot_name) if robot_name in known else 0
 
-    # ── Payload handlers ──────────────────────────────────────────────────
-    # Each handler receives:
-    #   robot_name – e.g. "robot_1"
-    #   msg        – deserialized ROS message (already in global ENU / 'map' frame)
-    #   i          – stable robot index (for marker IDs)
-    #   now        – current ROS timestamp
-    # Publish to self._pub_for('/gcs/payload/{robot_name}/{name}', MsgType)
-
     def _handle_filtered_rays(self, robot_name, msg, i, now):
-        """Republish filtered_rays MarkerArray shifted to the GCS display datum."""
         bz = self._display_z_offset()
         out_ma = transform_marker_array(msg, 0.0, 0.0, bz)
         for k, m in enumerate(out_ma.markers):
@@ -192,20 +141,15 @@ def _handle_filtered_rays(self, robot_name, msg, i, now):
         self._pub_for(f'/gcs/payload/{robot_name}/filtered_rays', MarkerArray).publish(out_ma)
 
     def _handle_frontier_viewpoints(self, robot_name, msg, i, now):
-        """Republish frontier_viewpoints PointCloud2 shifted to the GCS display datum."""
         out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
         out.header.stamp = now
         self._pub_for(f'/gcs/payload/{robot_name}/frontier_viewpoints', PointCloud2).publish(out)
 
     def _handle_rgb_voxels(self, robot_name, msg, i, now):
-        """Republish voxel_rgb PointCloud2 shifted to the GCS display datum."""
         out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
         out.header.stamp = now
         self._pub_for(f'/gcs/payload/{robot_name}/rgb_voxels', PointCloud2).publish(out)
 
-    # ── Handler registry ──────────────────────────────────────────────────
-    # Maps ROS type string -> handler method.
-    # Add or remove entries here to change what gets visualized.
     PAYLOAD_HANDLERS = {
         'visualization_msgs/msg/MarkerArray': _handle_filtered_rays,
         'sensor_msgs/msg/PointCloud2':        _handle_frontier_viewpoints,
diff --git a/robot/ros_ws/src/coordination/README.md b/robot/ros_ws/src/coordination/README.md
new file mode 100644
index 00000000..d1cb8bee
--- /dev/null
+++ b/robot/ros_ws/src/coordination/README.md
@@ -0,0 +1,76 @@
+# Coordination
+
+Multi-robot coordination layer for AirStack. Implements a gossip protocol over a shared DDS domain so drones can share state and payloads without a central broker.
+
+## Architecture
+
+```
+Robot (domain N)                      Shared gossip domain (99)
+┌─────────────────────┐               ┌────────────────────────┐
+│  gossip_node        │               │                        │
+│  ├─ PeerProfile     │──DDS Router──▶│   /gossip/peers        │◀──▶ GCS
+│  │  ├─ GPS/heading  │               │                        │
+│  │  ├─ waypoint     │               └────────────────────────┘
+│  │  └─ payloads[]   │
+│  └─ peer_registry   │◀── drains inbox, publishes snapshot
+└─────────────────────┘
+```
+
+Every robot publishes its own `PeerProfile` at 1 Hz (wall-clock) and receives profiles from all peers via the shared domain.
+
+## Packages
+
+### `coordination_msgs`
+Wire-format message definitions:
+- `PeerProfile.msg` — robot identity, GPS, heading, waypoint, and a typed payload array
+- `PeerProfilePayload.msg` — a single serialized ROS message (`payload_type` string + `payload_data` bytes)
+
+### `coordination_bringup`
+Runtime nodes and configuration:
+- **`gossip_node.py`** — publishes own profile, receives peer profiles, maintains registry
+- **`peer_profile.py`** — Python helper class for serializing/deserializing `PeerProfile` and its payloads
+- **`frame_utils.py`** — GPS ↔ ENU coordinate conversion helpers
+- **`config/gossip_payloads.yaml`** — declares which local topics to attach as payloads (config-driven, no code changes)
+
+## Message Deduplication
+
+Each message is identified by `(robot_name, stamp.sec, stamp.nanosec)`, where the stamp is set at publish time by the originating robot. Receivers maintain a seen-set (50 entries, FIFO eviction) and drop already-processed IDs.
+
+Every drone receives a message at least once — this is expected. The seen-set prevents re-processing, not initial fan-out.
+
+**Relay fields** (`source`, `relay_hops`) exist in the wire format for future multi-hop use but relay logic is not yet active.
+
+## Adding a Payload
+
+Edit `coordination_bringup/config/gossip_payloads.yaml`:
+
+```yaml
+payload_topics:
+  - topic: "/{robot_name}/your/topic"
+    type: "your_msgs/msg/YourType"
+```
+
+`{robot_name}` is substituted at runtime. Topics that haven't published yet are silently skipped.
+
+See [`.agents/skills/attach-gossip-payload`](../../../.agents/skills/attach-gossip-payload/SKILL.md) for the full workflow including GCS visualization.
+
+## Topics
+
+| Topic | Direction | QoS | Purpose |
+|-------|-----------|-----|---------|
+| `/gossip/peers` | pub/sub | BEST_EFFORT | Shared profile bus across all robots and GCS |
+| `/{robot_name}/coordination/peer_registry` | pub | RELIABLE, TRANSIENT_LOCAL | Snapshot of all known peers (latest-wins) |
+
+## Key Parameters (`gossip_node`)
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `robot_name` | — | Robot identifier, also used as topic namespace |
+| `publish_rate` | `1.0` | Hz, wall-clock (fires even when sim time is paused) |
+| `gossip_domain` | `99` | Shared DDS domain for the gossip bus |
+
+## Future Plans
+
+- **Payload version hashing** — hash `payload_data` bytes and skip re-sending unchanged payloads (e.g. static voxel maps). Reduces gossip bandwidth by up to 90% for slow-changing payloads like PointCloud2 maps.
+
+- **OLSR Multipoint Relay (MPR)** — when relay forwarding (`SOURCE_RELAYED`, `relay_hops`) is activated, use OLSR MPR selection to elect the minimal set of relay nodes that cover all 2-hop neighbors. Prevents O(n²) message explosion from naive flooding in partial-mesh / long-range deployments.
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
index 19fd75d6..eac7f627 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
@@ -1,17 +1,4 @@
-"""
-frame_utils.py
-==============
-Coordinate frame utilities shared between gossip_node (robot) and
-gcs_visualizer (GCS).
-
-Robots transform their payloads into global ENU before attaching them to
-PeerProfile, so every consumer receives world-frame data directly.
-
-ENU world origin (what GPS (0,0,0) maps to in metres):
-  Must match gcs_utils.py ORIGIN_* and the simulation's GPS home position.
-  Override via the 'enu_origin_lat/lon/alt' ROS parameters in gossip_node,
-  or leave at the Lisbon default used everywhere else in AirStack.
-"""
+"""Coordinate frame utilities shared between gossip_node (robot) and gcs_visualizer (GCS)."""
 
 import copy
 import math
@@ -37,9 +24,7 @@ def gps_to_enu(lat, lon, alt,
 def heading_to_quat(heading_deg):
     """Compass heading (degrees CW from North) → ENU yaw quaternion (x,y,z,w).
 
-    ENU: yaw=0 → East (+x), yaw=90° → North (+y).
-    heading=0 (North) → yaw=90° → q=(0,0,sin45,cos45)
-    heading=90 (East)  → yaw=0°  → q=(0,0,0,1)
+    ENU: yaw=0 → East (+x). heading=0 (North) → yaw=90° → q=(0,0,sin45,cos45).
     """
     yaw_enu = math.radians(90.0 - heading_deg)
     return (0.0, 0.0, math.sin(yaw_enu / 2.0), math.cos(yaw_enu / 2.0))
@@ -62,18 +47,12 @@ def rotate_vector(v, q):
 def transform_marker_array(marker_array, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
     """Deep-copy a MarkerArray and transform all points: p_map = R(q)*p + (bx,by,bz).
 
-    Transforms every point in points[] (LINE_LIST, LINE_STRIP, CUBE_LIST,
-    SPHERE_LIST, POINTS, ARROW-with-2-points, TRIANGLE_LIST).
-
-    Note: pose.position is intentionally NOT translated here.  In the ROS
-    marker convention, points[] are expressed in the marker's local frame
-    (defined by pose).  These markers typically use an identity pose so that
-    points[] are already in the parent frame; translating pose.position as
-    well would double-apply the offset.  If you have single-geometry markers
-    (SPHERE, CUBE, TEXT, MESH_RESOURCE) whose position lives in pose.position
-    rather than points[], translate pose.position at the call site instead.
+    Transforms points[] only — not pose.position. These markers use an identity pose
+    so points[] are already in the parent frame; translating pose.position would
+    double-apply the offset. For single-geometry markers (SPHERE, CUBE, MESH) whose
+    position lives in pose.position, translate at the call site instead.
 
-    Sets frame_id='map'.  Returns a new MarkerArray (does not modify in-place).
+    Sets frame_id='map'. Returns a new MarkerArray.
     """
     from visualization_msgs.msg import MarkerArray as MA
     out = MA()
@@ -92,8 +71,7 @@ def transform_marker_array(marker_array, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
 def transform_point_cloud2(cloud, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
     """Return a copy of PointCloud2 with all xyz transformed: p_map = R(q)*p + (bx,by,bz).
 
-    Sets frame_id='map'.  Reads field offsets from the message header so
-    non-standard field orderings work.
+    Sets frame_id='map'. Reads field offsets from the message header.
     """
     field_offsets = {f.name: f.offset for f in cloud.fields if f.name in ('x', 'y', 'z')}
     if not all(k in field_offsets for k in ('x', 'y', 'z')):
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
index d5ae6c9b..3a903ef4 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -1,19 +1,8 @@
 """
-gossip_node.py
-==============
-ROS 2 node that implements the gossip-protocol coordination layer.
-
-Each robot:
-  - Subscribes to its own GPS fix (NavSatFix) and compass heading, and global plan
-  - Publishes its own PeerProfile to /gossip/peers (shared gossip domain via DDS Router)
-  - Subscribes to /gossip/peers to receive all peer profiles
-  - Maintains a registry of known peers (latest-wins, no expiry)
-  - Republishes the full registry as a latched topic for planners to consume
-
-Publish rate:
-  - Fixed 1 Hz wall-clock timer (publish_rate parameter) — always fires regardless of
-    sim time, so heartbeating works even when the robot is stationary.
-  - A new waypoint also triggers an immediate extra publish.
+gossip_node.py — ROS 2 gossip-protocol coordination layer.
+
+Each robot publishes its PeerProfile to /gossip/peers at 1 Hz (wall-clock,
+unaffected by sim time). A new waypoint triggers an immediate extra publish.
 """
 
 import os
@@ -46,29 +35,25 @@
 from sensor_msgs.msg import PointCloud2
 
 
-# Seen-set size: deduplicates the last N unique (robot_name, stamp) pairs.
-# At 1 Hz per robot, 50 entries covers ~25 s for 2 robots or ~5 s for 10 robots —
-# well beyond the millisecond window in which DDS Router duplicates arrive.
+# Deduplication seen-set size. At 1 Hz, 50 entries covers ~5 s for 10 robots —
+# well beyond the window in which DDS Router duplicates arrive.
 _GOSSIP_SEEN_SIZE = 50
 
-# QoS for gossip topic: BEST_EFFORT — at 1 Hz a dropped message recovers on the
-# next tick, so reliability overhead (heartbeats, ACKs across domain bridges) is
-# unnecessary and was the main source of DDS-layer amplification.
+# BEST_EFFORT: at 1 Hz a dropped message recovers on the next tick; reliability
+# overhead across domain bridges was the main source of DDS-layer amplification.
 GOSSIP_QOS = QoSProfile(
     reliability=QoSReliabilityPolicy.BEST_EFFORT,
     history=QoSHistoryPolicy.KEEP_LAST,
     depth=10,
 )
 
-# MAVROS and other sensor topics publish as BEST_EFFORT
 SENSOR_QOS = QoSProfile(
     reliability=QoSReliabilityPolicy.BEST_EFFORT,
     history=QoSHistoryPolicy.KEEP_LAST,
     depth=10,
 )
 
-# QoS for the registry topic: reliable + transient-local so late subscribers
-# immediately receive the latest snapshot
+# RELIABLE + TRANSIENT_LOCAL so late-joining planners get the full snapshot.
 REGISTRY_QOS = QoSProfile(
     reliability=QoSReliabilityPolicy.RELIABLE,
     durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
@@ -82,47 +67,36 @@ class GossipNode(Node):
     def __init__(self):
         super().__init__("gossip_node")
 
-        # ── Parameters ──────────────────────────────────────────────────
         self.declare_parameter("robot_name", os.environ.get("ROBOT_NAME", "unknown_robot"))
-        self.declare_parameter("publish_rate", 1.0)   # Hz — wall-clock timer
-        self.declare_parameter("payload_topics_config", "")  # path to gossip_payloads.yaml
+        self.declare_parameter("publish_rate", 1.0)
+        self.declare_parameter("payload_topics_config", "")
 
         self._robot_name = self.get_parameter("robot_name").get_parameter_value().string_value
         publish_rate = self.get_parameter("publish_rate").get_parameter_value().double_value
         payload_config_path = self.get_parameter("payload_topics_config").get_parameter_value().string_value
 
-        # ── State ────────────────────────────────────────────────────────
         self._profile = PeerProfile(robot_name=self._robot_name)
 
-        # Boot pose: set on first valid GPS fix + first heading reading.
-        # Used to transform payload data from local odom frame → global ENU
-        # before attaching to PeerProfile, so every receiver gets world-frame data.
+        # Boot pose: set on first valid GPS fix + first heading.
+        # Used to transform payload data from local odom → global ENU before broadcast.
         self._boot_pos: tuple | None = None   # (bx, by, bz) ENU metres
-        self._boot_quat: tuple | None = None  # (qx, qy, qz, qw) rotation
+        self._boot_quat: tuple | None = None  # (qx, qy, qz, qw)
 
-        # Registry: robot_name → latest accepted PeerProfile (monotonic by stamp)
         self._registry: dict[str, PeerProfileMsg] = {}
         self._registry_lock = threading.Lock()
 
-        # Per-robot inbox: buffers incoming peer messages so every robot gets
-        # processed fairly on each drain tick, regardless of DDS arrival order.
-        # Stores only the latest message per robot (by gossip stamp).
+        # Per-robot inbox: holds only the latest message per robot so all robots
+        # are drained together on each tick regardless of DDS arrival order.
         self._peer_inbox: dict[str, PeerProfileMsg] = {}
         self._peer_inbox_lock = threading.Lock()
 
-        # Seen-set: deduplicates incoming peer messages by (robot_name, sec, nanosec).
-        # Including robot_name in the key ensures robot_1 and robot_2 never share IDs.
         self._seen: OrderedDict = OrderedDict()
-
-        # Payload cache: topic → (msg, stamp) or None until first message arrives
         self._payload_cache: dict[str, object] = {}
         self._payload_subs: list = []
 
-        # ── Payload topic subscriptions (config-driven) ──────────────────
         if payload_config_path:
             self._setup_payload_subscriptions(payload_config_path)
 
-        # ── Subscriptions ────────────────────────────────────────────────
         self._navsat_sub = self.create_subscription(
             NavSatFix,
             f"/{self._robot_name}/interface/mavros/global_position/raw/fix",
@@ -148,37 +122,16 @@ def __init__(self):
             GOSSIP_QOS,
         )
 
-        # ── Publishers ───────────────────────────────────────────────────
-        self._gossip_pub = self.create_publisher(
-            PeerProfileMsg,
-            "/gossip/peers",
-            GOSSIP_QOS,
-        )
-        # Registry snapshot – latched so planners that start late still get it
+        self._gossip_pub = self.create_publisher(PeerProfileMsg, "/gossip/peers", GOSSIP_QOS)
         self._registry_pub = self.create_publisher(
             PeerProfileMsg,
             f"/{self._robot_name}/coordination/peer_registry",
             REGISTRY_QOS,
         )
 
-        # ── Publish timer (wall clock — unaffected by sim time) ──────────
-        # Use a wall-clock timer so heartbeating works even when sim is paused
-        # or use_sim_time is set.
         period = 1.0 / max(publish_rate, 0.01)
-        self._publish_timer = self.create_timer(
-            period,
-            self._publish_tick,
-            clock=ROSClock(),
-        )
-
-        # ── Peer inbox drain timer (5 Hz wall clock) ──────────────────────
-        # Drains the per-robot inbox at a fixed rate so all robots are
-        # processed fairly — robot N never blocks robot M from being updated.
-        self._drain_timer = self.create_timer(
-            0.2,
-            self._drain_peer_inbox,
-            clock=ROSClock(),
-        )
+        self._publish_timer = self.create_timer(period, self._publish_tick, clock=ROSClock())
+        self._drain_timer = self.create_timer(0.2, self._drain_peer_inbox, clock=ROSClock())
 
         self.get_logger().info(
             f"GossipNode started for '{self._robot_name}' "
@@ -186,17 +139,10 @@ def __init__(self):
             f"{len(self._payload_cache)} payload topic(s))"
         )
 
-    # ------------------------------------------------------------------ #
-    # Config-driven payload subscriptions                                  #
-    # ------------------------------------------------------------------ #
-
     def _setup_payload_subscriptions(self, config_path: str) -> None:
-        """
-        Parse gossip_payloads.yaml and create a subscription for each entry.
+        """Parse gossip_payloads.yaml and subscribe to each listed topic.
 
-        Each subscription caches the latest message in self._payload_cache.
-        Messages are only added to the profile at publish time, so a topic
-        that hasn't published yet simply contributes no payload.
+        Topics that haven't published yet simply contribute no payload on that tick.
         """
         try:
             with open(config_path, "r") as f:
@@ -219,9 +165,7 @@ def _setup_payload_subscriptions(self, config_path: str) -> None:
                 self.get_logger().warn(f"Unknown payload type '{type_str}': {e}")
                 continue
 
-            # Cache stores (msg, stamp) tuples. stamp is extracted from the
-            # message header if present, otherwise the current ROS clock time.
-            self._payload_cache[topic] = None  # None = not received yet
+            self._payload_cache[topic] = None
 
             def _make_callback(t):
                 def cb(msg):
@@ -235,28 +179,17 @@ def cb(msg):
             self._payload_subs.append(sub)
             self.get_logger().info(f"Payload subscription: {topic} ({type_str})")
 
-    # ------------------------------------------------------------------ #
-    # Subscription callbacks                                               #
-    # ------------------------------------------------------------------ #
-
     def _on_navsat(self, msg: NavSatFix) -> None:
-        # Only store valid fixes — ignore NO_FIX so GPS never zeros out.
-        if msg.status.status < 0:
+        if msg.status.status < 0:  # ignore NO_FIX so GPS never zeros out
             return
         self._profile.set_gps_from_navsat(msg)
-        # Record boot position on first valid GPS fix
         if self._boot_pos is None:
             self._boot_pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude)
-            # Only set boot_quat now if heading has already been received
-            # (profile.heading != 0.0 means a compass reading arrived first).
-            # If heading hasn't arrived yet, _on_compass will set boot_quat
-            # once the first reading comes in.
             if self._boot_quat is None and self._profile.heading != 0.0:
                 self._boot_quat = heading_to_quat(self._profile.heading)
 
     def _on_compass(self, msg: Float64) -> None:
         self._profile.set_heading(msg.data)
-        # Set boot quat on first heading reading once we have GPS
         if self._boot_pos is not None and self._boot_quat is None:
             self._boot_quat = heading_to_quat(msg.data)
 
@@ -267,9 +200,6 @@ def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
         if msg.robot_name == self._robot_name:
             return  # discard own messages echoed back from the gossip domain
 
-        # Seen-set deduplication: discard if this exact (robot, stamp) was already
-        # processed.  Key includes robot_name so robot_1 and robot_2 can never share
-        # an ID even when their sequence counters coincide.
         msg_id = (msg.robot_name,
                   msg.gps_fix.header.stamp.sec,
                   msg.gps_fix.header.stamp.nanosec)
@@ -277,10 +207,8 @@ def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
             return
         self._seen[msg_id] = None
         if len(self._seen) > _GOSSIP_SEEN_SIZE:
-            self._seen.popitem(last=False)  # evict oldest entry
+            self._seen.popitem(last=False)
 
-        # Stage in per-robot inbox: keep only the newest message per robot so
-        # all robots are drained together on the next tick (fair processing).
         new_t = (msg.gps_fix.header.stamp.sec
                  + msg.gps_fix.header.stamp.nanosec * 1e-9)
         with self._peer_inbox_lock:
@@ -289,37 +217,25 @@ def _on_peer_msg(self, msg: PeerProfileMsg) -> None:
                 old_t = (existing.gps_fix.header.stamp.sec
                          + existing.gps_fix.header.stamp.nanosec * 1e-9)
                 if new_t < old_t:
-                    return  # already buffered a newer message this tick
+                    return
             self._peer_inbox[msg.robot_name] = msg
 
     def _drain_peer_inbox(self) -> None:
-        """Process all buffered peer messages at 5 Hz, one per robot per tick."""
         with self._peer_inbox_lock:
             inbox = dict(self._peer_inbox)
             self._peer_inbox.clear()
         for msg in inbox.values():
             self._update_registry(msg)
 
-    # ------------------------------------------------------------------ #
-    # Publish logic                                                        #
-    # ------------------------------------------------------------------ #
-
     def _publish_tick(self) -> None:
-        """Wall-clock timer callback — publish own profile at fixed rate."""
         self._publish_own()
 
     def _publish_own(self) -> None:
-        # Rebuild payloads from cache on every tick.
-        # If the boot pose is known, transform each payload into global ENU so
-        # every receiver (peers and GCS) gets world-frame data directly.
-        # If boot pose is not yet known, skip payloads this tick — we cannot
-        # produce a meaningful global-frame transform without GPS.
         self._profile.clear_payloads()
         if self._boot_pos is not None:
             bx, by, bz = self._boot_pos
-            # PX4/MAVROS odom frame is ENU-aligned — x=East, y=North, z=Up
-            # regardless of drone heading.  Only translation is needed; applying
-            # heading rotation would incorrectly rotate the payload data.
+            # PX4/MAVROS odom frame is ENU-aligned regardless of drone heading —
+            # only translation is needed; rotation would incorrectly rotate payloads.
             q = (0.0, 0.0, 0.0, 1.0)
             for topic, entry in self._payload_cache.items():
                 if entry is not None:
@@ -327,33 +243,20 @@ def _publish_own(self) -> None:
                     transformed = self._transform_to_global(msg, bx, by, bz, q)
                     self._profile.add_payload(transformed, stamp=stamp)
 
-        # Stamp the outgoing profile with the current ROS clock time.
-        # Receivers use this stamp — not the MAVROS GPS stamp — to enforce
-        # monotonic ordering and discard out-of-order messages.
+        # Stamp with current ROS clock (not MAVROS GPS stamp) so receivers can
+        # enforce monotonic ordering across ticks.
         self._profile.gps_fix.header.stamp = self.get_clock().now().to_msg()
-
         self._gossip_pub.publish(self._profile.to_ros_msg())
 
     def _transform_to_global(self, msg, bx, by, bz, q):
-        """Transform a payload message from local odom frame to global ENU."""
         if isinstance(msg, MarkerArray):
             return transform_marker_array(msg, bx, by, bz, q)
         if isinstance(msg, PointCloud2):
             return transform_point_cloud2(msg, bx, by, bz, q)
         return msg  # unknown type — pass through untransformed
 
-    # ------------------------------------------------------------------ #
-    # Registry management                                                  #
-    # ------------------------------------------------------------------ #
-
     def _update_registry(self, msg: PeerProfileMsg) -> None:
-        """Update registry, then republish snapshot.
-
-        Accept only if the incoming message is newer (or equal) to what we
-        already have.  gossip_node stamps gps_fix.header with the ROS clock
-        at publish time, so this gives strict monotonic ordering — stale or
-        out-of-order messages are silently dropped.
-        """
+        """Accept msg only if newer than what we have; republish updated snapshot."""
         new_t = (msg.gps_fix.header.stamp.sec
                  + msg.gps_fix.header.stamp.nanosec * 1e-9)
 
@@ -363,7 +266,7 @@ def _update_registry(self, msg: PeerProfileMsg) -> None:
                 old_t = (existing.gps_fix.header.stamp.sec
                          + existing.gps_fix.header.stamp.nanosec * 1e-9)
                 if new_t < old_t:
-                    return  # out-of-order — discard
+                    return
             self._registry[msg.robot_name] = msg
 
         self._registry_pub.publish(msg)
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
index 9e854b8f..71832601 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
@@ -1,32 +1,3 @@
-"""
-peer_profile.py
-===============
-Base class for gossip-protocol peer state.  Subclass this to attach
-task-specific data as typed ROS payloads.
-
-Usage
------
-# Basic – just use the base class
-profile = PeerProfile("robot_1")
-profile.set_gps_from_navsat(navsat_msg)
-profile.set_heading(compass_hdg_msg.data)
-profile.set_waypoint_from_path(path_msg)
-ros_msg = profile.to_ros_msg()
-
-# Extended – subclass and add payloads
-class SearchProfile(PeerProfile):
-    def attach_frontier(self, frontier_msg):
-        self.add_payload(frontier_msg)
-
-profile = SearchProfile("robot_1")
-profile.attach_frontier(my_frontier)
-ros_msg = profile.to_ros_msg()
-
-# Receiving side
-profile = PeerProfile.from_ros_msg(ros_msg)
-grid = profile.get_payload("nav_msgs/msg/OccupancyGrid")
-"""
-
 from __future__ import annotations
 
 from dataclasses import dataclass, field
@@ -51,13 +22,7 @@ class Source(IntEnum):
 
 @dataclass
 class PeerProfile:
-    """
-    Base peer state broadcast over the gossip bus.
-
-    Subclass and override ``extra_payloads_to_attach()`` to automatically
-    include task-specific ROS messages every time ``to_ros_msg()`` is called,
-    or call ``add_payload()`` / ``clear_payloads()`` manually.
-    """
+    """Base peer state broadcast over the gossip bus."""
 
     robot_name: str
     gps_fix: NavSatFix = field(default_factory=NavSatFix)
@@ -66,47 +31,30 @@ class PeerProfile:
     source: Source = Source.DIRECT
     relay_hops: int = 0
 
-    # Internal payload store: list of dicts with keys "type" and "data"
     _payloads: List[Dict[str, Any]] = field(default_factory=list, repr=False)
 
-    # ------------------------------------------------------------------ #
-    # GPS / waypoint helpers                                               #
-    # ------------------------------------------------------------------ #
-
     def set_gps_from_navsat(self, msg: NavSatFix) -> None:
-        """Store GPS fix from a sensor_msgs/NavSatFix message."""
         self.gps_fix = msg
 
     def set_heading(self, degrees: float) -> None:
-        """Set heading in degrees clockwise from North (0-360)."""
         self.heading = float(degrees)
 
     def set_waypoint_from_path(self, path: Optional[Path]) -> None:
-        """
-        Extract the goal (last pose) from a nav_msgs/Path.
-        Passing None or an empty path leaves waypoint as all-zeros,
-        which signals 'no plan available' to receivers.
-        """
+        """Extract goal (last pose) from a Path. None or empty path sets waypoint to all-zeros (no plan)."""
         if path is not None and len(path.poses) > 0:
             self.waypoint = path.poses[-1]
         else:
             self.waypoint = PoseStamped()
 
     def has_waypoint(self) -> bool:
-        """Return True if a valid waypoint has been set (non-zero stamp)."""
         s = self.waypoint.header.stamp
         return s.sec != 0 or s.nanosec != 0
 
-    # ------------------------------------------------------------------ #
-    # Payload management                                                   #
-    # ------------------------------------------------------------------ #
-
     def add_payload(self, msg: Any, stamp=None) -> None:
-        """Serialize and attach any ROS message as an additional payload.
+        """Serialize and attach a ROS message as a payload.
 
-        ``stamp`` is a builtin_interfaces/Time (or None to use zero time).
-        Pass the header stamp of the source message so receivers can judge
-        how fresh the data is independently of the gossip message timestamp.
+        Pass the source message's header stamp so receivers can judge data freshness
+        independently of the gossip message timestamp.
         """
         from builtin_interfaces.msg import Time
         type_str = _ros_type_string(msg)
@@ -120,12 +68,7 @@ def clear_payloads(self) -> None:
         self._payloads.clear()
 
     def get_payload(self, payload_type: str) -> Optional[Any]:
-        """
-        Deserialize and return the first payload matching *payload_type*.
-        *payload_type* is the fully-qualified ROS type string,
-        e.g. ``"nav_msgs/msg/OccupancyGrid"``.
-        Returns None if not found.
-        """
+        """Return the first payload matching payload_type (e.g. 'nav_msgs/msg/OccupancyGrid'), or None."""
         for p in self._payloads:
             if p["type"] == payload_type:
                 msg_class = rosidl_utils.get_message(payload_type)
@@ -133,11 +76,7 @@ def get_payload(self, payload_type: str) -> Optional[Any]:
         return None
 
     def get_payload_with_stamp(self, payload_type: str):
-        """
-        Like get_payload() but returns (msg, stamp) tuple so the caller can
-        check when the source data was last produced.
-        Returns (None, None) if not found.
-        """
+        """Like get_payload() but returns (msg, stamp). Returns (None, None) if not found."""
         for p in self._payloads:
             if p["type"] == payload_type:
                 msg_class = rosidl_utils.get_message(payload_type)
@@ -145,7 +84,6 @@ def get_payload_with_stamp(self, payload_type: str):
         return None, None
 
     def get_all_payloads(self) -> List[Any]:
-        """Deserialize and return all payloads in order."""
         result = []
         for p in self._payloads:
             msg_class = rosidl_utils.get_message(p["type"])
@@ -153,13 +91,8 @@ def get_all_payloads(self) -> List[Any]:
         return result
 
     def payload_types(self) -> List[str]:
-        """Return the type strings of all attached payloads."""
         return [p["type"] for p in self._payloads]
 
-    # ------------------------------------------------------------------ #
-    # Serialisation                                                        #
-    # ------------------------------------------------------------------ #
-
     def to_ros_msg(self) -> PeerProfileMsg:
         msg = PeerProfileMsg()
         msg.robot_name = self.robot_name
@@ -193,21 +126,14 @@ def from_ros_msg(cls, msg: PeerProfileMsg) -> "PeerProfile":
         return profile
 
 
-# ------------------------------------------------------------------ #
-# Internal helpers                                                     #
-# ------------------------------------------------------------------ #
-
 def _ros_type_string(msg: Any) -> str:
     """Return the fully-qualified ROS type string for a message instance.
-
-    E.g. nav_msgs.msg.OccupancyGrid → "nav_msgs/msg/OccupancyGrid"
+    E.g. nav_msgs.msg.OccupancyGrid → 'nav_msgs/msg/OccupancyGrid'
     """
-    module = type(msg).__module__    # e.g. "nav_msgs.msg._occupancy_grid"
-    name = type(msg).__name__        # e.g. "OccupancyGrid"
+    module = type(msg).__module__
+    name = type(msg).__name__
     # Convert "nav_msgs.msg._occupancy_grid" → "nav_msgs/msg"
     parts = module.split(".")
     if len(parts) >= 2:
-        pkg = parts[0]
-        sub = parts[1]
-        return f"{pkg}/{sub}/{name}"
+        return f"{parts[0]}/{parts[1]}/{name}"
     return f"{module}/{name}"
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py b/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
index 03bec2dd..091a13f4 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
@@ -1,12 +1,4 @@
-"""
-gcs_gossip_bridge.launch.py
-============================
-Launches the GCS-side DDS Router that bridges /gossip/peers between the GCS
-domain (0) and the shared gossip bus domain (99).
-
-Include this from gcs.launch.xml so the GCS can receive PeerProfile messages
-from all robots without going through each robot's onboard DDS router.
-"""
+"""Launches the GCS-side DDS Router bridging /gossip/peers between GCS domain (0) and gossip domain (99)."""
 
 import os
 
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
index 6a140d3e..41a2cfd3 100755
--- a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
@@ -1,20 +1,14 @@
 #!/usr/bin/env python3
 """
-peer_registry_monitor.py
-========================
-CLI diagnostic tool – subscribe to /gossip/peers on the gossip domain
-and pretty-print the live peer registry.
+peer_registry_monitor.py — CLI diagnostic tool for the gossip peer registry.
 
 Run on any robot or from a machine joined to domain 99:
-
     ROS_DOMAIN_ID=99 python3 peer_registry_monitor.py
 
-Or on a specific robot's domain to see what *that* robot receives:
-
+Or on a specific robot's domain to see what that robot receives:
     ROS_DOMAIN_ID=1 python3 peer_registry_monitor.py
 
-Options
--------
+Options:
   --robot   Only show entries for this robot name (partial match)
   --rate    Refresh rate in Hz (default: 2)
 """
@@ -87,24 +81,17 @@ class RegistryMonitor(Node):
     def __init__(self, filter_name: str = ""):
         super().__init__("peer_registry_monitor")
         self._registry: dict[str, PeerProfileMsg] = {}
-        self._recv_times: dict[str, float] = {}  # robot_name -> wall time of last received msg
+        self._recv_times: dict[str, float] = {}
         self._registry_lock = threading.Lock()
         self._filter = filter_name.lower()
-
-        # Per-robot inbox: buffer incoming messages so all robots are processed
-        # together on each drain, regardless of DDS arrival burst order.
         self._inbox: dict[str, PeerProfileMsg] = {}
         self._inbox_lock = threading.Lock()
 
         self._sub = self.create_subscription(
-            PeerProfileMsg,
-            "/gossip/peers",
-            self._on_msg,
-            GOSSIP_QOS,
+            PeerProfileMsg, "/gossip/peers", self._on_msg, GOSSIP_QOS,
         )
 
     def _on_msg(self, msg: PeerProfileMsg) -> None:
-        """Buffer incoming message; always track receipt time for liveness display."""
         new_t = (msg.gps_fix.header.stamp.sec
                  + msg.gps_fix.header.stamp.nanosec * 1e-9)
         with self._inbox_lock:
@@ -113,14 +100,12 @@ def _on_msg(self, msg: PeerProfileMsg) -> None:
                 old_t = (existing.gps_fix.header.stamp.sec
                          + existing.gps_fix.header.stamp.nanosec * 1e-9)
                 if new_t < old_t:
-                    # Already buffered a newer message — only update recv time
                     self._recv_times[msg.robot_name] = time.time()
                     return
             self._inbox[msg.robot_name] = msg
             self._recv_times[msg.robot_name] = time.time()
 
     def _drain_inbox(self) -> None:
-        """Apply registry ordering check for all buffered robots at once."""
         with self._inbox_lock:
             inbox = dict(self._inbox)
             self._inbox.clear()
@@ -133,7 +118,7 @@ def _drain_inbox(self) -> None:
                     old_t = (existing.gps_fix.header.stamp.sec
                              + existing.gps_fix.header.stamp.nanosec * 1e-9)
                     if new_t < old_t:
-                        continue  # out-of-order — keep existing
+                        continue
                 self._registry[robot_name] = msg
 
     def print_registry(self) -> None:

From ddee8f454315a106b09792a4d253791583ae874b Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 27 Mar 2026 14:41:38 -0400
Subject: [PATCH 22/55] removed VDB ENV

---
 robot/docker/docker-compose.yaml | 1 -
 1 file changed, 1 deletion(-)

diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index de5de536..abfef1cb 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -24,7 +24,6 @@ services:
       - LAUNCH_PACKAGE=desktop_bringup  # desktop_bringup adds RViz; real robots use autonomy_bringup
       - AUTONOMY_ROLE=full
       - SIM_IP=${SIM_IP:-172.31.0.200}
-      - VDB_SOURCE=${VDB_SOURCE:-lidar}  # "lidar" or "stereo", set in .env
       # FCU_URL and TGT_SYSTEM not set, dynamically calculated in interface.launch.py
     # 'command' uses variables so that it can be shared across robot-desktop and robot-l4t, with different launch packages and roles.
     command: >

From 088259fdece535fc6291f1b9b0e9dec04310791c Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 27 Mar 2026 14:49:35 -0400
Subject: [PATCH 23/55] rebase with main

---
 simulation/isaac-sim/extensions/PegasusSimulator | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/simulation/isaac-sim/extensions/PegasusSimulator b/simulation/isaac-sim/extensions/PegasusSimulator
index e1bc1211..1223a301 160000
--- a/simulation/isaac-sim/extensions/PegasusSimulator
+++ b/simulation/isaac-sim/extensions/PegasusSimulator
@@ -1 +1 @@
-Subproject commit e1bc1211c04e538077a3cbfd9710ea26a846aec4
+Subproject commit 1223a301f60d16ebb387a8537503aec8c45239d8

From 6c9fefa301181a06352a12841a38e2da0640d30d Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 27 Mar 2026 16:13:33 -0400
Subject: [PATCH 24/55] updated docs

---
 .agents/skills/attach-gossip-payload/SKILL.md |  64 ++++++----
 .env                                          |   2 +-
 docs/robot/autonomy/coordination/index.md     |  69 ++++++++++
 docs/robot/autonomy/coordination/payloads.md  | 119 ++++++++++++++++++
 .../foxglove_visualizer_node.py               |   6 -
 .../gcs_visualizer/payload_visualizer_node.py |  45 ++++---
 mkdocs.yml                                    |   3 +
 .../coordination_bringup/frame_utils.py       |  11 +-
 .../coordination_bringup/gossip_node.py       |  11 +-
 .../coordination_bringup/peer_profile.py      |  25 ++--
 .../msg/PeerProfilePayload.msg                |   1 +
 11 files changed, 275 insertions(+), 81 deletions(-)
 create mode 100644 docs/robot/autonomy/coordination/index.md
 create mode 100644 docs/robot/autonomy/coordination/payloads.md

diff --git a/.agents/skills/attach-gossip-payload/SKILL.md b/.agents/skills/attach-gossip-payload/SKILL.md
index 9709995f..a38843e3 100644
--- a/.agents/skills/attach-gossip-payload/SKILL.md
+++ b/.agents/skills/attach-gossip-payload/SKILL.md
@@ -97,47 +97,61 @@ Open `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.
 and note the `type:` field for your new entry. This determines how to deserialize it.
 
 If your type is **unique** (not already in `PAYLOAD_HANDLERS`), go to step 4b.
-If your type **already exists** (e.g. a second `sensor_msgs/msg/PointCloud2`), go to step 4c.
+### 4b — Add handler and register in `PAYLOAD_HANDLERS`
 
-### 4b — Unique type: add to `PAYLOAD_HANDLERS`
+`PAYLOAD_HANDLERS` is keyed by **payload name** (the last segment of the topic path
+in `gossip_payloads.yaml`). This means multiple payloads of the same ROS type work
+without any special casing.
 
 Add a handler and register it:
 
 ```python
 PAYLOAD_HANDLERS = {
-    'visualization_msgs/msg/MarkerArray': _handle_filtered_rays,
-    'sensor_msgs/msg/PointCloud2':        _handle_frontier_viewpoints,
-    'your_msgs/msg/YourType':             _handle_your_payload,   # ← add
+    'filtered_rays':       ('visualization_msgs/msg/MarkerArray', _handle_filtered_rays),
+    'frontier_viewpoints': ('sensor_msgs/msg/PointCloud2',        _handle_frontier_viewpoints),
+    'rgb_voxels':          ('sensor_msgs/msg/PointCloud2',        _handle_rgb_voxels),
+    'your_name':           ('your_msgs/msg/YourType',             _handle_your_payload),  # ← add
 }
 ```
 
+The key `'your_name'` must match the last path segment of the topic in `gossip_payloads.yaml`.
+For example, `/{robot_name}/rayfronts/voxel_rgb` → key is `voxel_rgb`.
+
 Handler signature — all handlers must match exactly:
 ```python
-def _handle_your_payload(self, robot_name, msg, boot, i, now):
-    # msg  — deserialized ROS message
-    # boot — (bx, by, bz) ENU offset; add to all positions to go odom→map frame
+def _handle_your_payload(self, robot_name, msg, i, now):
+    # msg  — deserialized ROS message (already in global ENU / 'map' frame)
     # i    — stable robot index (use for marker IDs: i * 100000 + unique_offset)
     # now  — current ROS timestamp (builtin_interfaces/Time)
-    bx, by, bz = boot
     # transform and publish to the payload's dedicated topic:
     self._pub_for(f'/gcs/payload/{robot_name}/your_name', YourMsgType).publish(out)
 ```
 
-### 4c — Duplicate type: dispatch by index in `_on_peer_profile`
+### 4c — Visualization options
+
+For `PointCloud2` payloads, choose one approach:
 
-`PAYLOAD_HANDLERS` is a dict and cannot hold duplicate keys. After the
-`PAYLOAD_HANDLERS` loop in `_on_peer_profile`, access payloads by their position
-among same-type entries — order matches `gossip_payloads.yaml`:
+**Default:** Publish as raw `PointCloud2` — Foxglove GUI controls point size, shape, and color. No extra code needed.
+
+**Preconfigured shape/size:** Convert to a `CUBE_LIST` `MarkerArray` in the handler (see `voxel_rgb` for a real example). Use this when you want a fixed visual style regardless of user layout settings:
 
 ```python
-# Example: 2nd sensor_msgs/msg/PointCloud2 (index 1)
-pc2_list = [p for p in profile._payloads if p["type"] == "sensor_msgs/msg/PointCloud2"]
-if len(pc2_list) >= 2:
-    msg = deserialize_message(bytes(pc2_list[1]["data"]), PointCloud2)
-    self._payload_cache[(robot_name, 'my_cache_key')] = msg
-cached = self._payload_cache.get((robot_name, 'my_cache_key'))
-if cached is not None:
-    self._handle_my_payload(robot_name, cached, boot, robot_index, now)
+from gcs_visualizer.gcs_utils import point_cloud2_to_cube_marker
+
+def _handle_your_payload(self, robot_name, msg, i, now):
+    marker = point_cloud2_to_cube_marker(
+        msg, 0.0, 0.0, self._display_z_offset(),
+        ns=f'{robot_name}_your_name',
+        marker_id=i * 100000,
+        stamp=now,
+        lifetime=Duration(sec=2, nanosec=0),
+        fallback_color=None,  # uses per-point rgb field; set to (r, g, b, a) for a solid color
+        scale=0.5,   # cube size in metres
+    )
+    if marker is not None:
+        out = MarkerArray()
+        out.markers.append(marker)
+        self._pub_for(f'/gcs/payload/{robot_name}/your_name', MarkerArray).publish(out)
 ```
 
 ### 4d — Available transform helpers (`gcs_utils.py`)
@@ -147,11 +161,9 @@ transform logic. Add a new helper there if none fits.
 
 | Helper | Use for |
 |--------|---------|
-| `transform_marker_array(ma, bx, by, bz, ns, id_base, stamp, lifetime)` | `MarkerArray` → translated `MarkerArray` |
+| `transform_marker_array(ma, bx, by, bz)` | `MarkerArray` → translated `MarkerArray` |
 | `transform_point_cloud2(cloud, bx, by, bz)` | `PointCloud2` → translated `PointCloud2` (preserves all fields including `rgb`) |
-
-Publish raw `PointCloud2` when you want Foxglove's viz controls (point size, color
-mapping, etc.). Publish `MarkerArray` when the shape/color is fixed by the publisher.
+| `point_cloud2_to_cube_marker(cloud, bx, by, bz, ns, marker_id, stamp, lifetime, scale)` | `PointCloud2` → `CUBE_LIST` Marker with fixed voxel size and per-point RGB |
 
 ### 4e — Rebuild GCS
 
@@ -172,7 +184,7 @@ docker exec airstack-gcs-1 bash -c "ros2 topic list | grep /gcs/payload"
 |--------------------------------|------|-----------|-------------------|
 | `/{robot_name}/filtered_rays` | `visualization_msgs/msg/MarkerArray` | `/gcs/payload/{robot}/filtered_rays` | Fixed (MarkerArray) |
 | `/{robot_name}/frontier_viewpoints` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/frontier_viewpoints` | Full (raw PointCloud2) |
-| `/{robot_name}/rayfronts/voxel_rgb` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/rgb_voxels` | Full (raw PointCloud2 with RGB) |
+| `/{robot_name}/rayfronts/voxel_rgb` | `sensor_msgs/msg/PointCloud2` | `/gcs/payload/{robot}/voxel_rgb` | Fixed (CUBE_LIST MarkerArray, 0.5 m) |
 
 ## Architecture notes
 
diff --git a/.env b/.env
index ba32b8aa..39ca95d6 100644
--- a/.env
+++ b/.env
@@ -34,7 +34,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="three_drone_scene_import.py"
+ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/docs/robot/autonomy/coordination/index.md b/docs/robot/autonomy/coordination/index.md
new file mode 100644
index 00000000..aab36cdc
--- /dev/null
+++ b/docs/robot/autonomy/coordination/index.md
@@ -0,0 +1,69 @@
+# Coordination
+
+The coordination layer lets drones share state with each other and the GCS without a central broker. Each robot periodically broadcasts a `PeerProfile` — its GPS position, heading, current waypoint, and any custom data payloads — over a shared DDS domain. Every robot and the GCS receives every other robot's profile directly.
+
+## Architecture
+
+```
+Robot 1 (domain 1)          Shared gossip domain (99)          GCS (domain 0)
+┌──────────────────┐         ┌─────────────────────┐         ┌──────────────┐
+│  gossip_node     │──────▶  │   /gossip/peers      │ ──────▶│  GCS         │
+│  publishes own   │         │                      │        │  visualizer  │
+│  PeerProfile     │ ◀──────  │  (all robots + GCS   │        └──────────────┘
+│                  │         │   subscribe here)    │
+│  local registry  │         └─────────────────────┘
+│  (read-only)     │
+└──────────────────┘
+         ▲
+    DDS Router
+    bridges domain 1
+    ↔ domain 99
+```
+
+Each robot builds a local registry of all known peers from incoming messages. The registry never leaves the robot — only each drone's own profile is transmitted.
+
+!!! note "Partial connectivity"
+    Today the system assumes all-to-all visibility via the shared domain. If a drone is out of radio range of another, it will simply not appear in that drone's registry. Relay forwarding is planned but not yet implemented.
+
+## PeerProfile
+
+Every message on `/gossip/peers` is a `PeerProfile` containing:
+
+| Field | Type | Description |
+|---|---|---|
+| `robot_name` | string | Unique robot identifier |
+| `gps_fix` | NavSatFix | Current GPS position (also used as message ID for dedup) |
+| `heading` | float64 | Compass heading, degrees CW from North |
+| `waypoint` | PoseStamped | Current navigation goal (all-zeros = no plan) |
+| `payloads` | PeerProfilePayload[] | Arbitrary serialized ROS messages |
+| `source` | uint8 | `0` = direct, `1` = relayed (reserved) |
+| `relay_hops` | uint8 | Hop count (reserved) |
+
+## Launch
+
+Coordination is included in the main autonomy bringup automatically. To launch standalone:
+
+```bash
+ros2 launch coordination_bringup gossip.launch.xml
+```
+
+Key parameters:
+
+| Parameter | Default | Description |
+|---|---|---|
+| `robot_name` | `$ROBOT_NAME` | Robot identifier and topic namespace |
+| `publish_rate` | `1.0` | Publish rate in Hz (wall-clock) |
+| `gossip_domain` | `99` | Shared DDS domain |
+
+## Monitoring
+
+```bash
+# Live peer registry in the terminal
+ROS_DOMAIN_ID=99 ros2 run coordination_bringup peer_registry_monitor
+
+# Filter to one robot
+ROS_DOMAIN_ID=99 ros2 run coordination_bringup peer_registry_monitor --robot robot_1
+
+# Inspect raw messages
+ros2 topic echo /gossip/peers
+```
diff --git a/docs/robot/autonomy/coordination/payloads.md b/docs/robot/autonomy/coordination/payloads.md
new file mode 100644
index 00000000..da9cb678
--- /dev/null
+++ b/docs/robot/autonomy/coordination/payloads.md
@@ -0,0 +1,119 @@
+# Payloads & Foxglove Visualization
+
+Payloads let you attach any ROS message to the `PeerProfile` so it gets broadcast to all peers and the GCS alongside GPS/heading. Common uses: sharing maps, frontier viewpoints, semantic rays, or any per-robot data you want visible fleet-wide.
+
+Payloads are **config-driven** — no changes to `gossip_node.py` are needed.
+
+## How payloads work
+
+1. `gossip_node` subscribes to each topic listed in `gossip_payloads.yaml`
+2. On each 1 Hz publish tick, the latest message from each topic is serialized and attached to the `PeerProfile`
+3. Before attaching, the payload is transformed from the robot's local odom frame → global ENU using the robot's boot GPS position
+4. Peers and GCS receive the payload already in world frame — no transform needed on the receiving side
+
+## Step 1 — Add to gossip_payloads.yaml
+
+**File:** `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml`
+
+```yaml
+payload_topics:
+  - topic: "/{robot_name}/your/topic"
+    type: "your_msgs/msg/YourType"
+```
+
+- `{robot_name}` is substituted at runtime (e.g. → `/robot_1/your/topic`)
+- Topics that haven't published yet are silently skipped
+- Only `MarkerArray` and `PointCloud2` are automatically transformed to world frame; other types pass through as-is
+
+Rebuild after editing:
+
+```bash
+bws --packages-select coordination_bringup
+```
+
+Verify the payload is being attached:
+
+```bash
+ros2 topic echo /gossip/peers --field payloads
+# or
+ros2 run coordination_bringup peer_registry_monitor
+```
+
+## Step 2 — Visualize in Foxglove
+
+Payloads don't appear in Foxglove automatically — you need a handler in `payload_visualizer_node.py` that republishes the payload to its own topic. There are two ways to do this:
+
+### Option A — Use the Claude skill (recommended)
+
+The `attach-gossip-payload` skill handles both the yaml edit and the GCS handler in one go. In Claude Code:
+
+```
+Follow the attach-gossip-payload skill to add /{robot_name}/your/topic
+of type your_msgs/msg/YourType and visualize it in Foxglove
+```
+
+See the full skill at .agents/skills/attach-gossip-payload
+
+### Option B — Manual
+
+**File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py`
+
+**1. Add a handler method:**
+
+```python
+def _handle_your_payload(self, robot_name, msg, i, now):
+    # msg is already in global ENU / 'map' frame
+    # Apply display z-offset to align with the GCS datum
+    out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
+    out.header.stamp = now
+    self._pub_for(f'/gcs/payload/{robot_name}/your_name', PointCloud2).publish(out)
+```
+
+**2. Register it in `PAYLOAD_HANDLERS`:**
+
+```python
+PAYLOAD_HANDLERS = {
+    'filtered_rays':       ('visualization_msgs/msg/MarkerArray', _handle_filtered_rays),
+    'frontier_viewpoints': ('sensor_msgs/msg/PointCloud2',        _handle_frontier_viewpoints),
+    'rgb_voxels':          ('sensor_msgs/msg/PointCloud2',        _handle_rgb_voxels),
+    'your_name':           ('your_msgs/msg/YourType',             _handle_your_payload),  # ← add
+}
+```
+
+**3. Rebuild GCS:**
+
+```bash
+docker exec airstack-gcs-1 bash -c "bws --packages-select gcs_visualizer && sws"
+```
+or restart Airstack
+
+Foxglove will now show `/gcs/payload/{robot_name}/your_name` as a subscribable topic with full visualization controls.
+
+## Visualization options
+
+For `PointCloud2` payloads, you have two options:
+
+**Default — Foxglove GUI:** Publish as raw `PointCloud2`. Foxglove's panel settings control point size, shape, and color per-user. No code changes needed.
+
+**Preconfigured — fixed shape/size/color in code:** Convert to a `MarkerArray` in the handler. An example to set`voxel_rgb` to render as 0.5 m cubes with per-point RGB colors:
+
+```python
+def _handle_rgb_voxels(self, robot_name, msg, i, now):
+    marker = point_cloud2_to_cube_marker(
+        msg, 0.0, 0.0, self._display_z_offset(),
+        ns=f'{robot_name}_voxel_rgb',
+        marker_id=i * 100000,
+        stamp=now,
+        lifetime=Duration(sec=2, nanosec=0),
+        fallback_color=None,  # uses per-point rgb field; set to (r, g, b, a) for a solid color
+        scale=0.5,   # cube size in metres
+    )
+    if marker is not None:
+        out = MarkerArray()
+        out.markers.append(marker)
+        self._pub_for(f'/gcs/payload/{robot_name}/voxel_rgb', MarkerArray).publish(out)
+```
+
+## Bandwidth note
+
+Payloads are re-serialized and sent in full every tick. A 500 KB PointCloud2 at 1 Hz is ~4 Mbps per robot — keep an eye on this for large maps. Reduce the gossip `publish_rate` parameter or only attach payloads when needed for bandwidth-constrained deployments.
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 38173345..361ba685 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -182,7 +182,6 @@ def _publish_markers(self):
             x, y, z = self._gps_positions[robot_name]
             orientation = self._orientations.get(robot_name)
 
-            # --- Mesh marker ---
             mesh = Marker()
             mesh.header.frame_id = 'map'
             mesh.header.stamp = now
@@ -210,7 +209,6 @@ def _publish_markers(self):
             mesh.lifetime = lifetime
             array.markers.append(mesh)
 
-            # --- Label marker ---
             label = Marker()
             label.header.frame_id = 'map'
             label.header.stamp = now
@@ -228,7 +226,6 @@ def _publish_markers(self):
             label.lifetime = lifetime
             array.markers.append(label)
 
-            # --- Axes markers (X=red, Y=green, Z=blue) ---
             axes = [
                 ((1.0, 0.0, 0.0), (1.0, 0.0, 0.0)),
                 ((0.0, 1.0, 0.0), (0.0, 1.0, 0.0)),
@@ -262,7 +259,6 @@ def _publish_markers(self):
 
             boot = self._gps_boot.get(robot_name)
 
-            # --- Trajectory (offset by boot GPS = odom origin) ---
             traj = self._trajectories.get(robot_name)
             if traj is not None and boot is not None:
                 bx, by, bz = boot
@@ -282,7 +278,6 @@ def _publish_markers(self):
                         continue
                     array.markers.append(m)
 
-            # --- Global plan ---
             plan = self._global_plans.get(robot_name)
             if plan is not None and boot is not None:
                 bx, by, bz = boot
@@ -307,7 +302,6 @@ def _publish_markers(self):
                 if len(line.points) >= 2:
                     array.markers.append(line)
 
-            # --- VDB map ---
             vdb = self._vdb_markers.get(robot_name)
             if vdb is not None and boot is not None:
                 bx, by, bz = boot
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
index 72f55516..0932ead5 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py
@@ -8,9 +8,8 @@
 Payloads arrive already transformed into global ENU by gossip_node — handlers
 only need to apply the display z-offset and republish.
 
-To add a new payload type: add a handler method and register it in PAYLOAD_HANDLERS.
-For duplicate types (e.g. a second PointCloud2), dispatch by index after the
-PAYLOAD_HANDLERS loop in _on_peer_profile (order matches gossip_payloads.yaml).
+To add a new payload: add a handler method and register it in PAYLOAD_HANDLERS
+keyed by the payload name (last segment of the topic in gossip_payloads.yaml).
 
 Handler signature:
     def _handle_<name>(self, robot_name, msg, i, now):
@@ -23,14 +22,13 @@ def _handle_<name>(self, robot_name, msg, i, now):
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
 
-from rclpy.serialization import deserialize_message
 from sensor_msgs.msg import PointCloud2
 from visualization_msgs.msg import MarkerArray
 from builtin_interfaces.msg import Duration
 
 from coordination_msgs.msg import PeerProfile as PeerProfileMsg
 from coordination_bringup.peer_profile import PeerProfile
-from gcs_visualizer.gcs_utils import transform_marker_array, transform_point_cloud2
+from gcs_visualizer.gcs_utils import transform_marker_array, transform_point_cloud2, point_cloud2_to_cube_marker
 
 # Must match frame_utils.DEFAULT_ORIGIN_ALT — used to compute the z-offset between
 # gossip_node's fixed ENU origin and the GCS display datum (first GPS altitude seen).
@@ -108,23 +106,14 @@ def _on_peer_profile(self, msg: PeerProfileMsg) -> None:
         now = self.get_clock().now().to_msg()
         robot_index = self._robot_index(robot_name)
 
-        for type_str, handler in self.PAYLOAD_HANDLERS.items():
-            payload = profile.get_payload(type_str)
+        for name, (type_str, handler) in self.PAYLOAD_HANDLERS.items():
+            payload = profile.get_payload_by_name(name)
             if payload is not None:
-                self._payload_cache[(robot_name, type_str)] = payload
-            cached = self._payload_cache.get((robot_name, type_str))
+                self._payload_cache[(robot_name, name)] = payload
+            cached = self._payload_cache.get((robot_name, name))
             if cached is not None:
                 handler(self, robot_name, cached, robot_index, now)
 
-        # voxel_rgb is the 2nd PointCloud2 payload (frontier_viewpoints is 1st)
-        pc2_list = [p for p in profile._payloads if p["type"] == "sensor_msgs/msg/PointCloud2"]
-        if len(pc2_list) >= 2:
-            voxel_rgb = deserialize_message(bytes(pc2_list[1]["data"]), PointCloud2)
-            self._payload_cache[(robot_name, 'rgb_voxels')] = voxel_rgb
-        cached_voxels = self._payload_cache.get((robot_name, 'rgb_voxels'))
-        if cached_voxels is not None:
-            self._handle_rgb_voxels(robot_name, cached_voxels, robot_index, now)
-
     def _robot_index(self, robot_name: str) -> int:
         """Stable integer index for a robot name (alphabetical order)."""
         known = sorted(self._gps_boot.keys())
@@ -146,13 +135,23 @@ def _handle_frontier_viewpoints(self, robot_name, msg, i, now):
         self._pub_for(f'/gcs/payload/{robot_name}/frontier_viewpoints', PointCloud2).publish(out)
 
     def _handle_rgb_voxels(self, robot_name, msg, i, now):
-        out = transform_point_cloud2(msg, 0.0, 0.0, self._display_z_offset())
-        out.header.stamp = now
-        self._pub_for(f'/gcs/payload/{robot_name}/rgb_voxels', PointCloud2).publish(out)
+        marker = point_cloud2_to_cube_marker(
+            msg, 0.0, 0.0, self._display_z_offset(),
+            ns=f'{robot_name}_voxel_rgb',
+            marker_id=i * 100000,
+            stamp=now,
+            lifetime=Duration(sec=2, nanosec=0),
+            scale=0.5,
+        )
+        if marker is not None:
+            out = MarkerArray()
+            out.markers.append(marker)
+            self._pub_for(f'/gcs/payload/{robot_name}/voxel_rgb', MarkerArray).publish(out)
 
     PAYLOAD_HANDLERS = {
-        'visualization_msgs/msg/MarkerArray': _handle_filtered_rays,
-        'sensor_msgs/msg/PointCloud2':        _handle_frontier_viewpoints,
+        'filtered_rays':       ('visualization_msgs/msg/MarkerArray', _handle_filtered_rays),
+        'frontier_viewpoints': ('sensor_msgs/msg/PointCloud2',        _handle_frontier_viewpoints),
+        'voxel_rgb':           ('sensor_msgs/msg/PointCloud2',        _handle_rgb_voxels),
     }
 
 
diff --git a/mkdocs.yml b/mkdocs.yml
index b6ebaea6..cf0c56c9 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -146,6 +146,9 @@ nav:
               - Behavior Tree: docs/robot/autonomy/behavior/behavior_tree.md
               - Behavior Executive: docs/robot/autonomy/behavior/behavior_executive.md
           - Integration Guide: docs/robot/autonomy/integration_checklist.md
+          - Coordination:
+              - Overview: docs/robot/autonomy/coordination/index.md
+              - Payloads & Foxglove: docs/robot/autonomy/coordination/payloads.md
       - Autonomy Modes: docs/robot/autonomy_modes.md
       - DDS Router: docs/robot/autonomy/dds_router.md
       - Static Transforms:
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
index eac7f627..12a72490 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
@@ -45,14 +45,11 @@ def rotate_vector(v, q):
 
 
 def transform_marker_array(marker_array, bx, by, bz, q=(0.0, 0.0, 0.0, 1.0)):
-    """Deep-copy a MarkerArray and transform all points: p_map = R(q)*p + (bx,by,bz).
+    """Deep-copy a MarkerArray and transform all points[]: p_map = R(q)*p + (bx,by,bz).
 
-    Transforms points[] only — not pose.position. These markers use an identity pose
-    so points[] are already in the parent frame; translating pose.position would
-    double-apply the offset. For single-geometry markers (SPHERE, CUBE, MESH) whose
-    position lives in pose.position, translate at the call site instead.
-
-    Sets frame_id='map'. Returns a new MarkerArray.
+    Transforms points[] only, not pose.position — LINE_STRIP/POINTS markers store
+    geometry in points[] with an identity pose, so translating pose.position would
+    double-apply the offset. Sets frame_id='map'. Returns a new MarkerArray.
     """
     from visualization_msgs.msg import MarkerArray as MA
     out = MA()
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
index 3a903ef4..e842109a 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
@@ -35,12 +35,8 @@
 from sensor_msgs.msg import PointCloud2
 
 
-# Deduplication seen-set size. At 1 Hz, 50 entries covers ~5 s for 10 robots —
-# well beyond the window in which DDS Router duplicates arrive.
 _GOSSIP_SEEN_SIZE = 50
 
-# BEST_EFFORT: at 1 Hz a dropped message recovers on the next tick; reliability
-# overhead across domain bridges was the main source of DDS-layer amplification.
 GOSSIP_QOS = QoSProfile(
     reliability=QoSReliabilityPolicy.BEST_EFFORT,
     history=QoSHistoryPolicy.KEEP_LAST,
@@ -85,13 +81,12 @@ def __init__(self):
         self._registry: dict[str, PeerProfileMsg] = {}
         self._registry_lock = threading.Lock()
 
-        # Per-robot inbox: holds only the latest message per robot so all robots
-        # are drained together on each tick regardless of DDS arrival order.
         self._peer_inbox: dict[str, PeerProfileMsg] = {}
         self._peer_inbox_lock = threading.Lock()
 
         self._seen: OrderedDict = OrderedDict()
         self._payload_cache: dict[str, object] = {}
+        self._payload_names: dict[str, str] = {}  # topic → short name (last path segment)
         self._payload_subs: list = []
 
         if payload_config_path:
@@ -158,6 +153,8 @@ def _setup_payload_subscriptions(self, config_path: str) -> None:
                 continue
 
             topic = topic_template.replace("{robot_name}", self._robot_name)
+            # Use the last path segment as a short human-readable name (e.g. 'filtered_rays')
+            self._payload_names[topic] = topic_template.rstrip("/").split("/")[-1]
 
             try:
                 msg_class = rosidl_utils.get_message(type_str)
@@ -241,7 +238,7 @@ def _publish_own(self) -> None:
                 if entry is not None:
                     msg, stamp = entry
                     transformed = self._transform_to_global(msg, bx, by, bz, q)
-                    self._profile.add_payload(transformed, stamp=stamp)
+                    self._profile.add_payload(transformed, stamp=stamp, name=self._payload_names.get(topic, ""))
 
         # Stamp with current ROS clock (not MAVROS GPS stamp) so receivers can
         # enforce monotonic ordering across ticks.
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
index 71832601..5f9cd11c 100644
--- a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
+++ b/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
@@ -50,15 +50,12 @@ def has_waypoint(self) -> bool:
         s = self.waypoint.header.stamp
         return s.sec != 0 or s.nanosec != 0
 
-    def add_payload(self, msg: Any, stamp=None) -> None:
-        """Serialize and attach a ROS message as a payload.
-
-        Pass the source message's header stamp so receivers can judge data freshness
-        independently of the gossip message timestamp.
-        """
+    def add_payload(self, msg: Any, stamp=None, name: str = "") -> None:
+        """Serialize and attach a ROS message as a payload."""
         from builtin_interfaces.msg import Time
         type_str = _ros_type_string(msg)
         self._payloads.append({
+            "name": name,
             "type": type_str,
             "data": serialize_message(msg),
             "stamp": stamp if stamp is not None else Time(),
@@ -75,6 +72,14 @@ def get_payload(self, payload_type: str) -> Optional[Any]:
                 return deserialize_message(p["data"], msg_class)
         return None
 
+    def get_payload_by_name(self, name: str) -> Optional[Any]:
+        """Return the payload with the given name, or None."""
+        for p in self._payloads:
+            if p.get("name") == name:
+                msg_class = rosidl_utils.get_message(p["type"])
+                return deserialize_message(p["data"], msg_class)
+        return None
+
     def get_payload_with_stamp(self, payload_type: str):
         """Like get_payload() but returns (msg, stamp). Returns (None, None) if not found."""
         for p in self._payloads:
@@ -104,6 +109,7 @@ def to_ros_msg(self) -> PeerProfileMsg:
         msg.payloads = [
             PeerProfilePayloadMsg(
                 stamp=p.get("stamp") or PeerProfilePayloadMsg().stamp,
+                payload_name=p.get("name", ""),
                 payload_type=p["type"],
                 payload_data=list(p["data"]),
             )
@@ -120,19 +126,16 @@ def from_ros_msg(cls, msg: PeerProfileMsg) -> "PeerProfile":
         profile.source = Source(msg.source)
         profile.relay_hops = msg.relay_hops
         profile._payloads = [
-            {"type": p.payload_type, "data": bytes(p.payload_data), "stamp": p.stamp}
+            {"name": p.payload_name, "type": p.payload_type, "data": bytes(p.payload_data), "stamp": p.stamp}
             for p in msg.payloads
         ]
         return profile
 
 
 def _ros_type_string(msg: Any) -> str:
-    """Return the fully-qualified ROS type string for a message instance.
-    E.g. nav_msgs.msg.OccupancyGrid → 'nav_msgs/msg/OccupancyGrid'
-    """
+    """Return the fully-qualified ROS type string, e.g. 'nav_msgs/msg/OccupancyGrid'."""
     module = type(msg).__module__
     name = type(msg).__name__
-    # Convert "nav_msgs.msg._occupancy_grid" → "nav_msgs/msg"
     parts = module.split(".")
     if len(parts) >= 2:
         return f"{parts[0]}/{parts[1]}/{name}"
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
index 02ba1f31..870b541f 100644
--- a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
+++ b/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
@@ -5,5 +5,6 @@
 # stamp is the time the source topic was last received — receivers can use
 # this to detect stale payloads independently of the gossip message timestamp.
 builtin_interfaces/Time stamp
+string  payload_name
 string  payload_type
 uint8[] payload_data

From 27355fac5efbcd8d08588070986deb51fd7eb763 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 3 Apr 2026 15:19:54 -0400
Subject: [PATCH 25/55] fixed launch files so they have play start on sim.
 scene_prep utils: added non-world prims to save in flattened manner

---
 .env                                          |   2 +-
 .gitignore                                    |   7 +
 .../barebones_pegasus_launch.py               |   7 +-
 .../three_drone_scene_import.py               |   7 +-
 .../launch_scripts/two_drone_fire.py          |   9 +-
 .../launch_scripts/two_drone_scene_import.py  |   7 +-
 simulation/isaac-sim/utils/scene_prep.py      | 145 +++++++++++++++++-
 7 files changed, 173 insertions(+), 11 deletions(-)

diff --git a/.env b/.env
index 39ca95d6..4dc9b4df 100644
--- a/.env
+++ b/.env
@@ -34,7 +34,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py"
+ISAAC_SIM_SCRIPT_NAME="prepare_scene.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/.gitignore b/.gitignore
index cdb047ca..cabc5025 100644
--- a/.gitignore
+++ b/.gitignore
@@ -75,3 +75,10 @@ simulation/isaac-sim/launch_scripts/prepare_scene.py
 
 # Generated Microsoft AirSim (legacy) config
 simulation/ms-airsim/config/settings.json
+
+# scenes and raven-specific launch scripts
+scenes/
+simulation/isaac-sim/launch_scripts/AbandonedFactory_Launch.py
+simulation/isaac-sim/launch_scripts/ConstructionSite_Launch.py
+simulation/isaac-sim/launch_scripts/FireAcademy_Launch.py
+simulation/isaac-sim/launch_scripts/RetroNeighborhood_Launch.py
diff --git a/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py b/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py
index 5f9d9945..aed76e0e 100755
--- a/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py
+++ b/simulation/isaac-sim/launch_scripts/barebones_pegasus_launch.py
@@ -81,11 +81,14 @@ def __init__(self):
         # Reset so physics/articulations are ready
         self.world.reset()
 
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
         self.stop_sim = False
 
     def run(self):
-        # Start sim timeline
-        self.timeline.play()
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
 
         # Main loop
         while simulation_app.is_running() and not self.stop_sim:
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
index 0c0d0f56..5394faaa 100644
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -191,11 +191,14 @@ def __init__(self):
         # Reset so physics/articulations are ready
         self.world.reset()
 
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
         self.stop_sim = False
 
     def run(self):
-        # Start sim timeline
-        self.timeline.play()
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
 
         # Main loop
         while simulation_app.is_running() and not self.stop_sim:
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index f6947f9f..7b6333ab 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -38,7 +38,7 @@
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
 
 #env/stage path and scale
-ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/ACFA/FireAcademyFaro/fire_academy_fixed_textures.usd"
+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"
@@ -201,11 +201,14 @@ def __init__(self):
         # Reset so physics/articulations are ready
         self.world.reset()
 
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
         self.stop_sim = False
 
     def run(self):
-        # Start sim timeline
-        self.timeline.play()
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
 
         # Main loop
         while simulation_app.is_running() and not self.stop_sim:
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
index 2c8b3a94..28f8ca8a 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
@@ -199,11 +199,14 @@ def __init__(self):
         # Reset so physics/articulations are ready
         self.world.reset()
 
+        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
         self.stop_sim = False
 
     def run(self):
-        # Start sim timeline
-        self.timeline.play()
+        if self.play_on_start:
+            self.timeline.play()
+        else:
+            self.timeline.stop()
 
         # Main loop
         while simulation_app.is_running() and not self.stop_sim:
diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py
index 95c49a83..026a2953 100644
--- a/simulation/isaac-sim/utils/scene_prep.py
+++ b/simulation/isaac-sim/utils/scene_prep.py
@@ -10,6 +10,8 @@
 """
 
 import asyncio
+import os
+import re
 import omni.kit.app
 import omni.usd
 from pxr import Gf, UsdGeom, UsdPhysics, UsdLux, Sdf
@@ -122,6 +124,51 @@ def add_dome_light(stage, prim_path: str = "/World/DomeLight", intensity: float
     print(f"[scene_prep] Dome light set at '{prim_path}' (intensity={intensity}, exposure={exposure})")
 
 
+# ---------------------------------------------------------------------------
+# Consolidate root prims under /World
+# ---------------------------------------------------------------------------
+
+def move_root_prims_to_world(usd_path: str) -> list:
+    """Move any non-/World root prims (e.g. /Environment) under /World.
+
+    After export_as_stage_async, sibling root prims like /Environment are
+    excluded when pg.load_environment references the file via defaultPrim=/World.
+    This function opens the flat exported USD layer directly and relocates
+    those prims under /World so they are included in the reference.
+
+    Args:
+        usd_path: Path to the flat exported USD file to patch in-place.
+
+    Returns:
+        List of prim names that were moved.
+    """
+    layer = Sdf.Layer.Find(usd_path) or Sdf.Layer.FindOrOpen(usd_path)
+    if layer is None:
+        print(f"[scene_prep] move_root_prims_to_world: could not open {usd_path}", flush=True)
+        return []
+
+    all_root = [spec.name for spec in layer.rootPrims]
+    print(f"[scene_prep] move_root_prims_to_world: root prims in exported USD: {all_root}", flush=True)
+    print(f"[scene_prep] move_root_prims_to_world: sublayers: {layer.subLayerPaths}", flush=True)
+
+    to_move = [name for name in all_root if name != 'World']
+    if not to_move:
+        print(f"[scene_prep] move_root_prims_to_world: nothing to move", flush=True)
+        return []
+
+    edit = Sdf.BatchNamespaceEdit()
+    for name in to_move:
+        edit.Add(Sdf.Path(f"/{name}"), Sdf.Path(f"/World/{name}"))
+
+    if not layer.Apply(edit):
+        print(f"[scene_prep] move_root_prims_to_world: namespace edit failed for {to_move}", flush=True)
+        return []
+
+    layer.Save()
+    print(f"[scene_prep] Moved root prims under /World: {to_move}", flush=True)
+    return to_move
+
+
 # ---------------------------------------------------------------------------
 # Save as self-contained USD collection
 # ---------------------------------------------------------------------------
@@ -153,7 +200,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=False, # preserve source folder hierarchy
+        flat_collection=True, # preserve source folder hierarchy
         skip_existing=False,
     )
 
@@ -180,3 +227,99 @@ def on_finish():
 
     collector.destroy()
     return result[0]
+
+
+# ---------------------------------------------------------------------------
+# Fix missing MDL textures
+# ---------------------------------------------------------------------------
+
+def _resolve_nucleus_url(base_url: str, relative: str) -> str:
+    """Resolve a relative path against a Nucleus base directory URL."""
+    parts = base_url.rstrip('/').split('/')
+    for segment in relative.replace('\\', '/').split('/'):
+        if segment == '..':
+            parts.pop()
+        elif segment and segment != '.':
+            parts.append(segment)
+    return '/'.join(parts)
+
+
+def fix_missing_mdl_textures(output_dir: str, nucleus_env_url: str) -> int:
+    """Download textures referenced in MDL files that the Collector missed.
+
+    The Collector rewrites texture paths inside MDL files to relative local
+    paths but does not always copy the actual texture files. This function
+    downloads the original Nucleus MDL to find the real texture URLs, then
+    downloads any missing textures to the expected local paths.
+
+    Args:
+        output_dir:       Local directory written by the Collector.
+        nucleus_env_url:  Original omniverse:// URL of the source scene.
+
+    Returns:
+        Number of textures downloaded.
+    """
+    import omni.client
+    import tempfile
+
+    nucleus_base = nucleus_env_url.rsplit('/', 1)[0]
+    nucleus_materials_dir = f"{nucleus_base}/Materials"
+
+    texture_pattern = re.compile(
+        r'["\']([^"\']*\.(?:png|jpg|jpeg|exr|hdr|dds|tga|bmp))["\']',
+        re.IGNORECASE,
+    )
+    downloaded = 0
+
+    for root, dirs, files in os.walk(output_dir):
+        for fname in files:
+            if not fname.endswith('.mdl'):
+                continue
+
+            local_mdl = os.path.join(root, fname)
+
+            # Download the original Nucleus MDL to a temp file
+            nucleus_mdl_url = f"{nucleus_materials_dir}/{fname}"
+            tmp_mdl = os.path.join(tempfile.gettempdir(), f"orig_{fname}")
+            copy_result = omni.client.copy(
+                nucleus_mdl_url, tmp_mdl, omni.client.CopyBehavior.OVERWRITE
+            )
+            if copy_result != omni.client.Result.OK:
+                print(f"[scene_prep] Could not fetch original MDL from Nucleus: {nucleus_mdl_url}")
+                continue
+
+            with open(tmp_mdl, 'r', errors='replace') as f:
+                orig_content = f.read()
+            os.remove(tmp_mdl)
+
+            with open(local_mdl, 'r', errors='replace') as f:
+                local_content = f.read()
+
+            orig_refs = [m.group(1) for m in texture_pattern.finditer(orig_content)]
+            local_refs = [m.group(1) for m in texture_pattern.finditer(local_content)]
+
+            for orig_ref, local_ref in zip(orig_refs, local_refs):
+                # Resolve local expected path
+                local_abs = os.path.normpath(os.path.join(os.path.dirname(local_mdl), local_ref))
+
+                if os.path.exists(local_abs):
+                    continue
+
+                # Resolve absolute Nucleus URL for the texture
+                if orig_ref.startswith('omniverse:'):
+                    nucleus_tex_url = orig_ref
+                else:
+                    nucleus_tex_url = _resolve_nucleus_url(nucleus_materials_dir, orig_ref)
+
+                os.makedirs(os.path.dirname(local_abs), exist_ok=True)
+                result = omni.client.copy(
+                    nucleus_tex_url, local_abs, omni.client.CopyBehavior.OVERWRITE
+                )
+                if result == omni.client.Result.OK:
+                    downloaded += 1
+                    print(f"[scene_prep] Downloaded: {os.path.basename(local_abs)}")
+                else:
+                    print(f"[scene_prep] Failed ({result}): {nucleus_tex_url}")
+
+    print(f"[scene_prep] fix_missing_mdl_textures: {downloaded} texture(s) downloaded.")
+    return downloaded

From 946441e88b569cbb7399c3d60ee1c9815c48c9d7 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 7 Apr 2026 18:53:43 -0400
Subject: [PATCH 26/55] created raven_nav package

---
 .env                                          |   2 +-
 .../robot_descriptions/CMakeLists.txt         |   4 -
 robot/docker/Dockerfile.robot                 |   1 +
 .../planners/raven_nav/config/raven_nav.yaml  |   9 +
 .../raven_nav/launch/raven_nav.launch.xml     |   8 +
 .../src/global/planners/raven_nav/package.xml |  26 ++
 .../planners/raven_nav/raven_nav/__init__.py  |   0
 .../raven_nav/raven_nav/behavior_manager.py   |  39 +++
 .../raven_nav/raven_nav/behaviors/__init__.py |   0
 .../raven_nav/behaviors/frontier_behavior.py  | 133 +++++++++
 .../raven_nav/behaviors/ray_behavior.py       | 259 ++++++++++++++++++
 .../raven_nav/behaviors/voxel_behavior.py     |  15 +
 .../raven_nav/raven_nav/raven_nav_node.py     | 219 +++++++++++++++
 .../planners/raven_nav/resource/raven_nav     |   0
 .../src/global/planners/raven_nav/setup.cfg   |   4 +
 .../src/global/planners/raven_nav/setup.py    |  27 ++
 .../isaac-sim/extensions/PegasusSimulator     |   2 +-
 .../launch_scripts/two_drone_fire.py          |   9 +-
 simulation/isaac-sim/utils/scene_prep.py      |  68 +++++
 19 files changed, 816 insertions(+), 9 deletions(-)
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/package.xml
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/__init__.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/__init__.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/resource/raven_nav
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/setup.cfg
 create mode 100644 robot/ros_ws/src/global/planners/raven_nav/setup.py

diff --git a/.env b/.env
index 4dc9b4df..39ca95d6 100644
--- a/.env
+++ b/.env
@@ -34,7 +34,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="prepare_scene.py"
+ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/common/ros_packages/robot_descriptions/CMakeLists.txt b/common/ros_packages/robot_descriptions/CMakeLists.txt
index 6d26a041..616ef402 100644
--- a/common/ros_packages/robot_descriptions/CMakeLists.txt
+++ b/common/ros_packages/robot_descriptions/CMakeLists.txt
@@ -24,9 +24,5 @@ endif()
 install(DIRECTORY iris DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
-install(PROGRAMS
-  src/robot_2_relative_position.py
-  DESTINATION lib/${PROJECT_NAME}
-)
 
 ament_package()
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index 87ec185e..992442ce 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -134,6 +134,7 @@ RUN pip3 install --break-system-packages --ignore-installed \
   six \
   toml \
   scipy \
+  scikit-learn \
   pypose \
   rich \
   tqdm \
diff --git a/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml b/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
new file mode 100644
index 00000000..8795e894
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
@@ -0,0 +1,9 @@
+raven_nav:
+  ros__parameters:
+    timer_period: 0.5       # seconds between behavior loop iterations
+    score_threshold: 0.95   # minimum softmax sim score to consider a ray
+    # Must match the query order in RayFronts' queries.txt
+    query_labels:
+      - 'red building'
+      - 'water tower'
+      - 'radio tower'
diff --git a/robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml b/robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml
new file mode 100644
index 00000000..81b3152f
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+    <node pkg="raven_nav" exec="raven_nav_node" name="raven_nav" output="screen">
+        <param from="$(find-pkg-share raven_nav)/config/raven_nav.yaml"
+               allow_substs="true" />
+        <remap from="/$(env ROBOT_NAME)/odometry"
+               to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+    </node>
+</launch>
diff --git a/robot/ros_ws/src/global/planners/raven_nav/package.xml b/robot/ros_ws/src/global/planners/raven_nav/package.xml
new file mode 100644
index 00000000..be054d48
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>raven_nav</name>
+  <version>0.0.0</version>
+  <description>Navigation planner that consumes RayFronts mapper outputs to produce waypoints</description>
+  <maintainer email="todo@todo.todo">todo</maintainer>
+  <license>TODO</license>
+
+  <depend>rclpy</depend>
+  <depend>python3-sklearn</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/__init__.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
new file mode 100644
index 00000000..38ec3606
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
@@ -0,0 +1,39 @@
+from raven_nav.behaviors.frontier_behavior import FrontierBehavior
+from raven_nav.behaviors.ray_behavior import RayBehavior
+
+
+class BehaviorManager:
+    def __init__(self, get_clock, publisher_dict, score_threshold=0.95):
+        self.behavior_mode = 'Frontier-based'
+        self.get_clock = get_clock
+        self.frontier_behavior = FrontierBehavior(self.get_clock)
+        current_target_publisher = publisher_dict.get('current_target')
+        self.ray_behavior = RayBehavior(self.get_clock, current_target_publisher,
+                                        score_threshold=score_threshold)
+        self.behaviors = [self.ray_behavior, self.frontier_behavior]
+
+    def mode_select(self, ray_origins, ray_dirs, ray_scores,
+                    query_labels, target_objects):
+        for behavior in self.behaviors:
+            if behavior.name == 'Ray-based':
+                if behavior.condition_check(ray_origins, ray_dirs, ray_scores,
+                                            query_labels, target_objects):
+                    self.behavior_mode = behavior.name
+                    return
+            else:
+                if behavior.condition_check():
+                    self.behavior_mode = behavior.name
+                    return
+
+    def behavior_execute(self, behavior_mode, frontiers, cur_pose_np,
+                         waypoint_locked, target_waypoint, target_waypoint2,
+                         publisher_dict):
+        if behavior_mode == 'Frontier-based':
+            return self.frontier_behavior.execute(
+                frontiers, cur_pose_np, waypoint_locked,
+                target_waypoint, target_waypoint2, publisher_dict)
+        elif behavior_mode == 'Ray-based':
+            return self.ray_behavior.execute(
+                cur_pose_np, waypoint_locked,
+                target_waypoint, target_waypoint2, publisher_dict)
+        return waypoint_locked, target_waypoint, target_waypoint2
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/__init__.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
new file mode 100644
index 00000000..08691d8d
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
@@ -0,0 +1,133 @@
+import torch
+import numpy as np
+from sklearn.cluster import DBSCAN
+from std_msgs.msg import Header
+from sensor_msgs.msg import PointField
+from sensor_msgs_py import point_cloud2
+from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped
+
+
+class FrontierBehavior:
+    def __init__(self, get_clock):
+        self.get_clock = get_clock
+        self.name = 'Frontier-based'
+
+    def condition_check(self):
+        return True
+
+    def execute(self, frontiers_raw, cur_pose_np, waypoint_locked, target_waypoint,
+                target_waypoint2, publisher_dict):
+        viewpoint_publisher = publisher_dict['viewpoint']
+        path_publisher = publisher_dict['path']
+
+        if frontiers_raw is None or len(frontiers_raw) == 0:
+            return waypoint_locked, target_waypoint, target_waypoint2
+
+        # msg_serv/frontiers are published in RDF frame (no transform applied by RayFronts)
+        # Convert RDF → FLU: world_x=rdf_z, world_y=-rdf_x, world_z=-rdf_y
+        frontiers = np.stack([
+            frontiers_raw[:, 2],
+            -frontiers_raw[:, 0],
+            -frontiers_raw[:, 1],
+        ], axis=1)
+
+        frontiers = frontiers[frontiers[:, 2] > 1.5]
+        if frontiers.shape[0] == 0:
+            return waypoint_locked, target_waypoint, target_waypoint2
+
+        clustering = DBSCAN(eps=2.7, min_samples=3).fit(frontiers)
+        labels = clustering.labels_
+        unique_labels = [l for l in set(labels) if l != -1]
+        viewpoints = []
+        for l in unique_labels:
+            cluster_pts = frontiers[labels == l]
+            centroid = cluster_pts.mean(axis=0)
+            if centroid[2] > 4.0 and centroid[2] < 10.0:
+                viewpoints.append(centroid)
+
+        if len(viewpoints) == 0:
+            return waypoint_locked, target_waypoint, target_waypoint2
+
+        viewpoints = np.stack(viewpoints)
+        cent_msg = self._create_pointcloud2_msg(viewpoints)
+        viewpoint_publisher.publish(cent_msg)
+
+        robot_pos = cur_pose_np
+        distances = np.linalg.norm(viewpoints - robot_pos, axis=1)
+
+        if target_waypoint is not None:
+            cur_motion_vec = target_waypoint - robot_pos
+            cur_motion_vec = cur_motion_vec / (np.linalg.norm(cur_motion_vec) + 1e-6)
+            candidate_vecs = viewpoints - robot_pos
+            norms = np.linalg.norm(candidate_vecs, axis=1, keepdims=True)
+            candidate_vecs = candidate_vecs / (norms + 1e-6)
+            cos_sim = candidate_vecs @ cur_motion_vec
+            momentum_weight = 5.0
+            scores = distances + momentum_weight * (1.0 - cos_sim)
+        else:
+            scores = distances
+
+        top_n = 5
+        num_candidates = min(top_n, viewpoints.shape[0])
+        if num_candidates == 0:
+            return waypoint_locked, target_waypoint, target_waypoint2
+
+        top_indices = np.argsort(scores)[:num_candidates]
+        best_idx = top_indices[np.random.randint(0, num_candidates)]
+        best_cent = viewpoints[best_idx]
+
+        path = Path()
+        path.header.stamp = self.get_clock().now().to_msg()
+        path.header.frame_id = 'map'
+
+        if not waypoint_locked:
+            target_waypoint = best_cent
+            direction = target_waypoint - cur_pose_np
+            dir_norm = np.linalg.norm(direction)
+            if dir_norm > 1e-6:
+                direction = direction / dir_norm
+                target_waypoint2 = target_waypoint + 2.0 * direction
+            else:
+                target_waypoint2 = target_waypoint.copy()
+            waypoint_locked = True
+
+        target_pose = PoseStamped()
+        target_pose.header.stamp = self.get_clock().now().to_msg()
+        target_pose.header.frame_id = 'map'
+        target_pose.pose.position.x = float(target_waypoint[0])
+        target_pose.pose.position.y = float(target_waypoint[1])
+        target_pose.pose.position.z = float(target_waypoint[2])
+        target_pose.pose.orientation.w = 1.0
+        path.poses.append(target_pose)
+
+        target_pose2 = PoseStamped()
+        target_pose2.header.stamp = self.get_clock().now().to_msg()
+        target_pose2.header.frame_id = 'map'
+        target_pose2.pose.position.x = float(target_waypoint2[0])
+        target_pose2.pose.position.y = float(target_waypoint2[1])
+        target_pose2.pose.position.z = float(target_waypoint2[2])
+        target_pose2.pose.orientation.w = 1.0
+        path.poses.append(target_pose2)
+
+        path_publisher.publish(path)
+
+        if np.linalg.norm(cur_pose_np - target_waypoint) < 5.0:
+            waypoint_locked = False
+
+        return waypoint_locked, target_waypoint, target_waypoint2
+
+    def _create_pointcloud2_msg(self, xyz):
+        if isinstance(xyz, np.ndarray):
+            pass
+        else:
+            xyz = np.array(xyz, dtype=np.float32)
+        header = Header()
+        header.stamp = self.get_clock().now().to_msg()
+        header.frame_id = 'map'
+        fields = [PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
+                  PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
+                  PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1)]
+        points = [[float(xyz[i, 0]), float(xyz[i, 1]), float(xyz[i, 2])]
+                  for i in range(xyz.shape[0])]
+        return point_cloud2.create_cloud(header, fields, points)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
new file mode 100644
index 00000000..90815a4b
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
@@ -0,0 +1,259 @@
+import numpy as np
+from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped, Point
+from visualization_msgs.msg import Marker, MarkerArray
+from std_msgs.msg import String
+
+
+import logging
+_log = logging.getLogger(__name__)
+
+
+class RayBehavior:
+    def __init__(self, get_clock, current_target_publisher=None, score_threshold=0.95):
+        self.get_clock = get_clock
+        self.name = 'Ray-based'
+        self.score_threshold = score_threshold
+        self.prev_filtered_marker_ids = 0
+        self.current_target = None
+        self.current_target_pub = current_target_publisher
+
+        # Set by condition_check, consumed by execute
+        self._filtered_indices = None   # indices into ray arrays that pass threshold
+        self._ray_origins = None        # (N, 3) FLU — full ray set
+        self._ray_dirs = None           # (N, 3) FLU
+        self._per_ray_label = None      # list[str] — best label per filtered ray
+
+        # Active target: the target the drone is committed to pursuing.
+        # Set on first acquisition; stays fixed until explicitly reset.
+        self._active_target = None
+
+    def condition_check(self, ray_origins, ray_dirs, ray_scores,
+                        query_labels, target_objects):
+        """Returns True if any ray scores above threshold for a target object."""
+        self.current_target = None
+        self._filtered_indices = None
+
+        if (ray_origins is None or ray_scores is None or
+                len(ray_origins) == 0 or len(target_objects) == 0):
+            return False
+
+        # Find column indices in ray_scores for each target object
+        label_indices = [query_labels.index(t) for t in target_objects
+                         if t in query_labels]
+        if not label_indices:
+            return False
+
+        relevant_scores = ray_scores[:, label_indices]          # (N, num_targets)
+        mask = (relevant_scores > self.score_threshold).any(axis=1)
+        indices = np.where(mask)[0]
+
+        if indices.size == 0:
+            return False
+
+        # Assign each passing ray its best-scoring target label
+        per_ray_label = []
+        targets_found = []
+        for idx in indices:
+            scores_i = relevant_scores[idx]
+            best_col = int(np.argmax(scores_i))
+            label = target_objects[best_col]
+            per_ray_label.append(label)
+            targets_found.append(label)
+
+        from collections import Counter
+        self.current_target = Counter(targets_found).most_common(1)[0][0]
+        if self.current_target_pub is not None:
+            self.current_target_pub.publish(String(data=self.current_target))
+
+        self._filtered_indices = indices
+        self._ray_origins = ray_origins
+        self._ray_dirs = ray_dirs
+        self._per_ray_label = per_ray_label
+        return True
+
+    def execute(self, cur_pose_np, waypoint_locked, target_waypoint1,
+                target_waypoint2, publisher_dict):
+        path_publisher = publisher_dict['path']
+
+        origins = self._ray_origins[self._filtered_indices]   # (K, 3) FLU
+        dirs = self._ray_dirs[self._filtered_indices]          # (K, 3) FLU
+        labels = self._per_ray_label
+
+        # --- Filter rays pointing behind the robot in XY ---
+        xy_dirs = dirs[:, :2]
+        norms = np.linalg.norm(xy_dirs, axis=1, keepdims=True)
+        xy_dirs_normed = xy_dirs / (norms + 1e-6)
+
+        ray_target_xy = origins[:, :2] + xy_dirs_normed
+        to_target = ray_target_xy - cur_pose_np[:2]
+        dot = np.einsum('ij,ij->i', xy_dirs_normed, to_target)
+        valid = dot > 0
+
+        _log.warning(
+            f'ray execute: score_filtered={len(origins)} '
+            f'direction_valid={int(valid.sum())} '
+            f'robot_xy=({cur_pose_np[0]:.1f},{cur_pose_np[1]:.1f})')
+
+        if not valid.any():
+            # All rays point behind the robot. Two cases:
+            # 1. Same target + same direction as current travel → already heading
+            #    correctly (e.g. drone passed the ray origin), keep going.
+            # 2. Same target + different direction, OR different target → stay
+            #    committed to active target, keep current waypoint.
+            behind_for_active = (self._active_target is None or
+                                 self.current_target == self._active_target)
+
+            same_direction = False
+            if target_waypoint1 is not None:
+                travel_xy = np.array([target_waypoint1[0], target_waypoint1[1]]) - cur_pose_np[:2]
+                travel_norm = np.linalg.norm(travel_xy)
+                if travel_norm > 1e-6:
+                    travel_dir = travel_xy / travel_norm
+                    avg_behind = xy_dirs_normed.mean(axis=0)
+                    avg_behind = avg_behind / (np.linalg.norm(avg_behind) + 1e-6)
+                    same_direction = float(np.dot(avg_behind, travel_dir)) > np.cos(np.deg2rad(45))
+
+            if behind_for_active and same_direction:
+                _log.warning(
+                    f'ray execute: all rays behind, consistent with active target '
+                    f'({self._active_target}) and direction — keeping waypoint')
+            else:
+                _log.warning(
+                    f'ray execute: all rays behind — staying committed to active '
+                    f'target ({self._active_target}, current={self.current_target})')
+            return waypoint_locked, target_waypoint1, target_waypoint2
+
+        origins_v = origins[valid]
+        dirs_v = dirs[valid]
+        xy_dirs_v = xy_dirs_normed[valid]
+        labels_v = [labels[i] for i in range(len(labels)) if valid[i]]
+
+        # --- Greedy spatial clustering per label (45 deg XY threshold) ---
+        angle_cos_thresh = np.cos(np.deg2rad(45))
+        groups = []
+        for i in range(len(origins_v)):
+            xy_dir = xy_dirs_v[i]
+            label = labels_v[i]
+            assigned = False
+            for group in groups:
+                if group['label'] != label:
+                    continue
+                if np.dot(xy_dir, group['centroid']) >= angle_cos_thresh:
+                    group['indices'].append(i)
+                    group['rays'].append(xy_dir)
+                    c = np.mean(group['rays'], axis=0)
+                    group['centroid'] = c / (np.linalg.norm(c) + 1e-6)
+                    assigned = True
+                    break
+            if not assigned:
+                groups.append({
+                    'centroid': xy_dir.copy(),
+                    'rays': [xy_dir],
+                    'indices': [i],
+                    'label': label,
+                })
+
+        if not groups:
+            _log.warning('ray execute: no groups formed — no waypoint')
+            return waypoint_locked, target_waypoint1, target_waypoint2
+
+        _log.warning(f'ray execute: {len(groups)} groups, publishing waypoint')
+
+        # --- Score groups: lower is better ---
+        # score = dist(avg_origin, robot) - 5 * group_size
+        k = 5.0
+        group_avgs = []
+        for group in groups:
+            idxs = group['indices']
+            avg_origin = origins_v[idxs].mean(axis=0)
+            avg_dir = dirs_v[idxs].mean(axis=0)
+            avg_dir = avg_dir / (np.linalg.norm(avg_dir) + 1e-6)
+            density = len(idxs)
+            score = np.linalg.norm(avg_origin - cur_pose_np) - k * density
+            group_avgs.append((avg_origin, avg_dir, density, group['label'], score))
+
+        group_avgs.sort(key=lambda g: g[4])
+        best = group_avgs[0]
+        best_origin, best_dir = best[0], best[1]
+        direction = best_dir / (np.linalg.norm(best_dir) + 1e-6)
+
+        target_waypoint1 = best_origin + direction * 6.0
+        target_waypoint2 = best_origin + direction * 12.0
+
+        path = Path()
+        path.header.stamp = self.get_clock().now().to_msg()
+        path.header.frame_id = 'map'
+        for wp in (target_waypoint1, target_waypoint2):
+            ps = PoseStamped()
+            ps.header.stamp = self.get_clock().now().to_msg()
+            ps.header.frame_id = 'map'
+            ps.pose.position.x = float(wp[0])
+            ps.pose.position.y = float(wp[1])
+            ps.pose.position.z = float(wp[2])
+            ps.pose.orientation.w = 1.0
+            path.poses.append(ps)
+        path_publisher.publish(path)
+
+        published_target = best[3]
+        if self._active_target is None:
+            self._active_target = published_target
+            _log.warning(f'ray execute: active target set to {published_target}')
+
+        self._visualize_filtered_rays(groups, origins_v, dirs_v, publisher_dict)
+
+        return waypoint_locked, target_waypoint1, target_waypoint2
+
+    def _visualize_filtered_rays(self, groups, origins, dirs, publisher_dict):
+        pub = publisher_dict['filtered_rays']
+        self._clear_filtered_rays(pub)
+        colors = [
+            (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0),
+            (1.0, 1.0, 0.0), (0.0, 1.0, 1.0), (1.0, 0.0, 1.0),
+            (0.5, 0.5, 0.5), (1.0, 0.5, 0.0), (0.5, 0.0, 1.0),
+            (0.0, 0.5, 0.5),
+        ]
+        arrow_length = 2.0
+        marker_array = MarkerArray()
+        j = 0
+        for i, group in enumerate(groups):
+            rr, gg, bb = colors[i % len(colors)]
+            for idx in group['indices']:
+                p0 = origins[idx]
+                d = dirs[idx]
+                p1 = p0 + arrow_length * (d / (np.linalg.norm(d) + 1e-6))
+                arrow = Marker()
+                arrow.header.frame_id = 'map'
+                arrow.header.stamp = self.get_clock().now().to_msg()
+                arrow.ns = 'arrows'
+                arrow.id = j
+                arrow.type = Marker.ARROW
+                arrow.action = Marker.ADD
+                arrow.points = [
+                    Point(x=float(p0[0]), y=float(p0[1]), z=float(p0[2])),
+                    Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2])),
+                ]
+                arrow.scale.x = 0.6
+                arrow.scale.y = 1.2
+                arrow.scale.z = 0.75
+                arrow.color.r = rr
+                arrow.color.g = gg
+                arrow.color.b = bb
+                arrow.color.a = 0.5
+                marker_array.markers.append(arrow)
+                j += 1
+        self.prev_filtered_marker_ids = j
+        pub.publish(marker_array)
+
+    def _clear_filtered_rays(self, pub):
+        if self.prev_filtered_marker_ids > 0:
+            clear_array = MarkerArray()
+            for i in range(self.prev_filtered_marker_ids):
+                m = Marker()
+                m.header.frame_id = 'map'
+                m.header.stamp = self.get_clock().now().to_msg()
+                m.ns = 'arrows'
+                m.id = i
+                m.action = Marker.DELETE
+                clear_array.markers.append(m)
+            pub.publish(clear_array)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
new file mode 100644
index 00000000..9b090754
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
@@ -0,0 +1,15 @@
+# Voxel-based behavior - placeholder for future implementation.
+# Enable once voxel behavior strategy is decided.
+
+# class VoxelBehavior:
+#     def __init__(self, get_clock):
+#         self.get_clock = get_clock
+#         self.name = 'Voxel-based'
+#
+#     def condition_check(self, voxels, voxel_scores, target_labels, target_objects):
+#         return False
+#
+#     def execute(self, voxels, voxel_scores, target_labels, target_objects,
+#                 cur_pose_np, waypoint_locked, target_waypoint, target_waypoint2,
+#                 publisher_dict):
+#         return waypoint_locked, target_waypoint, target_waypoint2
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
new file mode 100644
index 00000000..1e14e2ee
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
@@ -0,0 +1,219 @@
+import os
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry, Path
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs_py import point_cloud2
+from std_msgs.msg import String
+from visualization_msgs.msg import MarkerArray
+
+from raven_nav.behavior_manager import BehaviorManager
+
+
+class RavenNavNode(Node):
+    def __init__(self):
+        super().__init__('raven_nav')
+
+        robot_name = os.getenv('ROBOT_NAME', 'robot')
+        self._prefix = f'/{robot_name}'
+
+        # Ray data parsed from rays_sim/all (all in FLU world frame after conversion)
+        self._ray_origins = None    # (N, 3) FLU
+        self._ray_dirs = None       # (N, 3) FLU
+        self._ray_scores = None     # (N, M) softmax scores
+
+        self._frontiers = None      # (N, 3) FLU — published already in FLU
+        self._cur_pose = None       # (3,)
+
+        self._waypoint_locked = False
+        self._target_waypoint = None
+        self._target_waypoint2 = None
+        self._behavior_mode = 'Frontier-based'
+
+        # Parameters
+        self._score_threshold = self.declare_parameter('score_threshold', 0.95).value
+        query_labels_param = self.declare_parameter(
+            'query_labels', ['red building', 'water tower', 'radio tower']).value
+        self._query_labels = list(query_labels_param)
+        self._target_objects = self._query_labels[:]
+
+        timer_period = self.declare_parameter('timer_period', 0.5).value
+
+        self._path_pub = self.create_publisher(
+            Path, f'{self._prefix}/global_plan', 10)
+        self._filtered_rays_pub = self.create_publisher(
+            MarkerArray, f'{self._prefix}/filtered_rays', 10)
+        self._viewpoint_pub = self.create_publisher(
+            PointCloud2, f'{self._prefix}/frontier_viewpoints', 10)
+        self._current_target_pub = self.create_publisher(
+            String, f'{self._prefix}/current_target', 10)
+
+        self._publisher_dict = {
+            'path': self._path_pub,
+            'filtered_rays': self._filtered_rays_pub,
+            'viewpoint': self._viewpoint_pub,
+            'current_target': self._current_target_pub,
+        }
+
+        self._behavior_manager = BehaviorManager(
+            get_clock=self.get_clock,
+            publisher_dict=self._publisher_dict,
+            score_threshold=self._score_threshold,
+        )
+
+        self.create_subscription(
+            PointCloud2,
+            f'{self._prefix}/rayfronts/msg_serv/rays_sim/all',
+            self._rays_cb, 10)
+        self.create_subscription(
+            PointCloud2,
+            f'{self._prefix}/rayfronts/msg_serv/frontiers',
+            self._frontiers_cb, 10)
+        self.create_subscription(
+            Odometry,
+            f'{self._prefix}/odometry',
+            self._odometry_cb, 10)
+        self.create_subscription(
+            String,
+            '/input_prompt',
+            self._input_prompt_cb, 10)
+
+        self.create_timer(timer_period, self._timer_cb)
+
+        self.get_logger().info(
+            f'raven_nav started | robot={robot_name} | '
+            f'timer={timer_period:.2f}s | '
+            f'query_labels={self._query_labels} | '
+            f'score_threshold={self._score_threshold}')
+
+    def _rays_cb(self, msg: PointCloud2):
+        """Parse rays_sim/all PointCloud2.
+
+        Fields: x, y, z (RDF origin), theta, phi (degrees), sim_0, sim_1, ...
+        Converts origin + direction to world FLU frame.
+        """
+        field_names = [f.name for f in msg.fields]
+        pts = list(point_cloud2.read_points(msg, field_names=field_names,
+                                            skip_nans=True))
+        if not pts:
+            self._ray_origins = None
+            self._ray_dirs = None
+            self._ray_scores = None
+            return
+
+        pts_arr = np.array([list(p) for p in pts], dtype=np.float32)
+
+        x_idx = field_names.index('x')
+        y_idx = field_names.index('y')
+        z_idx = field_names.index('z')
+        theta_idx = field_names.index('theta')
+        phi_idx = field_names.index('phi')
+
+        rdf_origins = pts_arr[:, [x_idx, y_idx, z_idx]]
+        theta = np.deg2rad(pts_arr[:, theta_idx])
+        phi = np.deg2rad(pts_arr[:, phi_idx])
+
+        # Spherical → Cartesian in RDF frame
+        # Matches geometry3d.spherical_to_cartesian / cartesian_to_spherical:
+        #   theta = azimuthal in xy-plane, phi = polar from +z (forward)
+        # x=cos(theta)*sin(phi), y=sin(theta)*sin(phi), z=cos(phi)
+        dx_rdf = np.cos(theta) * np.sin(phi)
+        dy_rdf = np.sin(theta) * np.sin(phi)
+        dz_rdf = np.cos(phi)
+        rdf_dirs = np.stack([dx_rdf, dy_rdf, dz_rdf], axis=1)
+
+        # RDF → FLU: world_x=rdf_z, world_y=-rdf_x, world_z=-rdf_y
+        self._ray_origins = np.stack([
+            rdf_origins[:, 2], -rdf_origins[:, 0], -rdf_origins[:, 1]], axis=1)
+        self._ray_dirs = np.stack([
+            rdf_dirs[:, 2], -rdf_dirs[:, 0], -rdf_dirs[:, 1]], axis=1)
+
+        # Collect sim_* columns in order
+        sim_cols = [i for i, n in enumerate(field_names) if n.startswith('sim_')]
+        sim_cols.sort(key=lambda i: int(field_names[i].split('_')[1]))
+        if sim_cols:
+            self._ray_scores = pts_arr[:, sim_cols]  # (N, M)
+        else:
+            self._ray_scores = None
+
+    def _frontiers_cb(self, msg: PointCloud2):
+        """Parse msg_serv/frontiers — already in FLU world frame."""
+        pts = list(point_cloud2.read_points(msg, field_names=('x', 'y', 'z'),
+                                            skip_nans=True))
+        if pts:
+            self._frontiers = np.array([[p[0], p[1], p[2]] for p in pts],
+                                       dtype=np.float32)
+        else:
+            self._frontiers = None
+
+    def _odometry_cb(self, msg: Odometry):
+        p = msg.pose.pose.position
+        self._cur_pose = np.array([p.x, p.y, p.z], dtype=np.float64)
+
+    def _input_prompt_cb(self, msg: String):
+        targets = [t.strip().lower() for t in msg.data.split(',') if t.strip()]
+        if targets:
+            self._target_objects = targets
+            self.get_logger().info(f'target objects updated: {self._target_objects}')
+
+    def _timer_cb(self):
+        if self._cur_pose is None:
+            self.get_logger().warn('waiting for odometry...', throttle_duration_sec=5.0)
+            return
+
+        prev_mode = self._behavior_mode
+        self._behavior_manager.mode_select(
+            ray_origins=self._ray_origins,
+            ray_dirs=self._ray_dirs,
+            ray_scores=self._ray_scores,
+            query_labels=self._query_labels,
+            target_objects=self._target_objects,
+        )
+        self._behavior_mode = self._behavior_manager.behavior_mode
+
+        if self._behavior_mode != prev_mode:
+            self.get_logger().info(f'behavior mode: {prev_mode} -> {self._behavior_mode}')
+            self._waypoint_locked = False
+            self._target_waypoint = None
+            self._target_waypoint2 = None
+
+        self._waypoint_locked, self._target_waypoint, self._target_waypoint2 = \
+            self._behavior_manager.behavior_execute(
+                behavior_mode=self._behavior_mode,
+                frontiers=self._frontiers,
+                cur_pose_np=self._cur_pose,
+                waypoint_locked=self._waypoint_locked,
+                target_waypoint=self._target_waypoint,
+                target_waypoint2=self._target_waypoint2,
+                publisher_dict=self._publisher_dict,
+            )
+
+        # Throttled status summary
+        n_frontiers = len(self._frontiers) if self._frontiers is not None else 0
+        n_rays = len(self._ray_origins) if self._ray_origins is not None else 0
+        ray_beh = self._behavior_manager.ray_behavior
+        n_filtered = len(ray_beh._filtered_indices) if ray_beh._filtered_indices is not None else 0
+        publishing = self._target_waypoint is not None
+        wp = (f'({self._target_waypoint[0]:.1f}, {self._target_waypoint[1]:.1f}, '
+              f'{self._target_waypoint[2]:.1f})') if publishing else 'none'
+        self.get_logger().info(
+            f'[{self._behavior_mode}] '
+            f'frontiers={n_frontiers} | rays={n_rays} filtered={n_filtered} | '
+            f'target={self._behavior_manager.ray_behavior.current_target} | '
+            f'publishing={publishing} wp={wp}',
+            throttle_duration_sec=2.0,
+        )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RavenNavNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
diff --git a/robot/ros_ws/src/global/planners/raven_nav/resource/raven_nav b/robot/ros_ws/src/global/planners/raven_nav/resource/raven_nav
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/global/planners/raven_nav/setup.cfg b/robot/ros_ws/src/global/planners/raven_nav/setup.cfg
new file mode 100644
index 00000000..1b9b84ea
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/raven_nav
+[install]
+install_scripts=$base/lib/raven_nav
diff --git a/robot/ros_ws/src/global/planners/raven_nav/setup.py b/robot/ros_ws/src/global/planners/raven_nav/setup.py
new file mode 100644
index 00000000..ce503284
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/raven_nav/setup.py
@@ -0,0 +1,27 @@
+from setuptools import find_packages, setup
+
+package_name = 'raven_nav'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/raven_nav.launch.xml']),
+        ('share/' + package_name + '/config', ['config/raven_nav.yaml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='todo',
+    maintainer_email='todo@todo.todo',
+    description='Navigation planner consuming RayFronts mapper outputs',
+    license='TODO',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'raven_nav_node = raven_nav.raven_nav_node:main',
+        ],
+    },
+)
diff --git a/simulation/isaac-sim/extensions/PegasusSimulator b/simulation/isaac-sim/extensions/PegasusSimulator
index 1223a301..f40897f1 160000
--- a/simulation/isaac-sim/extensions/PegasusSimulator
+++ b/simulation/isaac-sim/extensions/PegasusSimulator
@@ -1 +1 @@
-Subproject commit 1223a301f60d16ebb387a8537503aec8c45239d8
+Subproject commit f40897f1640bfafa45a9220731cc58c7dfbde33d
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 7b6333ab..2f90c1ac 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -31,7 +31,7 @@
 
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 import scene_prep
-from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
+from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit, reference_root_prims_under_world
 
 
 # --------------------- CONFIGURATION ---------------------
@@ -47,7 +47,7 @@
 DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
 
 # Lighting
-ADD_DOME_LIGHT = True
+ADD_DOME_LIGHT = False
 DOME_LIGHT_PATH = "/World/DomeLight"
 DOME_LIGHT_INTENSITY = 3500.0
 DOME_LIGHT_EXPOSURE = -3.0
@@ -132,7 +132,7 @@ def __init__(self):
         self.pg._world = World(**self.pg._world_settings)
         self.world = self.pg.world
 
-        # Load environment
+        # Load environment (places geometry at /World/stage)
         self.pg.load_environment(ENV_URL)
 
         stage = omni.usd.get_context().get_stage()
@@ -143,6 +143,9 @@ def __init__(self):
             carb.log_warn("Stage load timed out — continuing anyway.")
 
         # ----- Scene preparation -----
+        # Bring in sky/sun/environment prims that sit outside /World in the source USD
+        reference_root_prims_under_world(stage, ENV_URL)
+
         stage_prim = stage.GetPrimAtPath("/World/stage")
         if stage_prim.IsValid():
             scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py
index 026a2953..09cd7479 100644
--- a/simulation/isaac-sim/utils/scene_prep.py
+++ b/simulation/isaac-sim/utils/scene_prep.py
@@ -128,6 +128,74 @@ def add_dome_light(stage, prim_path: str = "/World/DomeLight", intensity: float
 # Consolidate root prims under /World
 # ---------------------------------------------------------------------------
 
+def reference_root_prims_under_world(stage, source_usd_url: str) -> list:
+    """Reference non-/World 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.
+
+    Args:
+        stage:          Active USD stage.
+        source_usd_url: omniverse:// or local path of the source USD.
+
+    Returns:
+        List of prim names that were referenced.
+    """
+    source_layer = Sdf.Layer.FindOrOpen(source_usd_url)
+    if source_layer is None:
+        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)
+        return []
+
+    for name in non_world:
+        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
+
+
+def move_root_prims_to_world_live(stage) -> list:
+    """Move any non-/World root prims (e.g. /Environment, /Sun, /Sky) under /World
+    on the currently active live stage.
+
+    Useful when loading a USD from Nucleus whose sky/sun/environment prims sit at
+    the root rather than under /World, causing them to be invisible to the sim.
+
+    Args:
+        stage: Active USD stage (from omni.usd.get_context().get_stage()).
+
+    Returns:
+        List of prim names that were moved.
+    """
+    root_layer = stage.GetRootLayer()
+    all_root = [spec.name for spec in root_layer.rootPrims]
+    print(f"[scene_prep] move_root_prims_to_world_live: root prims = {all_root}", flush=True)
+
+    to_move = [name for name in all_root if name != 'World']
+    if not to_move:
+        print("[scene_prep] move_root_prims_to_world_live: nothing to move", flush=True)
+        return []
+
+    edit = Sdf.BatchNamespaceEdit()
+    for name in to_move:
+        edit.Add(Sdf.Path(f"/{name}"), Sdf.Path(f"/World/{name}"))
+
+    if not root_layer.Apply(edit):
+        print(f"[scene_prep] move_root_prims_to_world_live: namespace edit failed for {to_move}", flush=True)
+        return []
+
+    print(f"[scene_prep] Moved root prims under /World: {to_move}", flush=True)
+    return to_move
+
+
 def move_root_prims_to_world(usd_path: str) -> list:
     """Move any non-/World root prims (e.g. /Environment) under /World.
 

From 3329d8805c157c98a9fbc36a8509732baad1c46d Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 20:35:52 -0400
Subject: [PATCH 27/55] moved coordination to common

---
 common/fastdds.xml                               |  9 +++++++++
 .../ros_packages}/coordination/README.md         |  0
 .../config/gcs_gossip_dds_router.yaml            |  0
 .../config/gossip_dds_router.yaml                |  0
 .../config/gossip_payloads.yaml                  |  0
 .../coordination_bringup/__init__.py             |  0
 .../coordination_bringup/frame_utils.py          |  0
 .../coordination_bringup/gossip_node.py          |  0
 .../coordination_bringup/peer_profile.py         |  0
 .../launch/gcs_gossip_bridge.launch.py           |  0
 .../launch/gossip.launch.xml                     |  0
 .../coordination_bringup/package.xml             |  0
 .../resource/coordination_bringup                |  0
 .../coordination_bringup/scripts/gossip_node     |  0
 .../scripts/peer_registry_monitor.py             |  0
 .../coordination/coordination_bringup/setup.py   |  0
 .../coordination_msgs/CMakeLists.txt             |  0
 .../coordination_msgs/msg/PeerProfile.msg        |  0
 .../coordination_msgs/msg/PeerProfilePayload.msg |  0
 .../coordination/coordination_msgs/package.xml   |  0
 gcs/docker/gcs-base-docker-compose.yaml          |  1 -
 simulation/isaac-sim/docker/docker-compose.yaml  | 16 ++++++++--------
 .../launch_scripts/three_drone_scene_import.py   |  6 ++++++
 .../isaac-sim/launch_scripts/two_drone_fire.py   |  6 ++++++
 24 files changed, 29 insertions(+), 9 deletions(-)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/README.md (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/config/gossip_dds_router.yaml (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/config/gossip_payloads.yaml (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/coordination_bringup/__init__.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/coordination_bringup/frame_utils.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/coordination_bringup/gossip_node.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/coordination_bringup/peer_profile.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/launch/gossip.launch.xml (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/package.xml (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/resource/coordination_bringup (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/scripts/gossip_node (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/scripts/peer_registry_monitor.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_bringup/setup.py (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_msgs/CMakeLists.txt (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_msgs/msg/PeerProfile.msg (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_msgs/msg/PeerProfilePayload.msg (100%)
 rename {robot/ros_ws/src => common/ros_packages}/coordination/coordination_msgs/package.xml (100%)

diff --git a/common/fastdds.xml b/common/fastdds.xml
index d9d106bb..58de2126 100644
--- a/common/fastdds.xml
+++ b/common/fastdds.xml
@@ -18,6 +18,15 @@
             <!-- https://answers.ros.org/question/370595/ros2-foxy-nodes-cant-communicate-through-docker-container-border/ -->
             <!-- https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#isaac-sim-app-install-ros:~:text=Run%20export%20FASTRTPS_DEFAULT_PROFILES_FILE%3D~/.ros/fastdds.xml%20in%20the%20terminals%20that%20will%20use%20ROS%202%20functions -->
             <useBuiltinTransports>false</useBuiltinTransports>
+            <builtin>
+                <initialPeersList>
+                    <locator>
+                        <udpv4>
+                            <address>192.168.233.25</address>
+                        </udpv4>
+                    </locator>
+                </initialPeersList>
+            </builtin>
         </rtps>
     </participant>
 </profiles>
diff --git a/robot/ros_ws/src/coordination/README.md b/common/ros_packages/coordination/README.md
similarity index 100%
rename from robot/ros_ws/src/coordination/README.md
rename to common/ros_packages/coordination/README.md
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml b/common/ros_packages/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
rename to common/ros_packages/coordination/coordination_bringup/config/gcs_gossip_dds_router.yaml
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_dds_router.yaml b/common/ros_packages/coordination/coordination_bringup/config/gossip_dds_router.yaml
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/config/gossip_dds_router.yaml
rename to common/ros_packages/coordination/coordination_bringup/config/gossip_dds_router.yaml
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml b/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml
rename to common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/__init__.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/__init__.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/__init__.py
rename to common/ros_packages/coordination/coordination_bringup/coordination_bringup/__init__.py
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/frame_utils.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/frame_utils.py
rename to common/ros_packages/coordination/coordination_bringup/coordination_bringup/frame_utils.py
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/gossip_node.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/gossip_node.py
rename to common/ros_packages/coordination/coordination_bringup/coordination_bringup/gossip_node.py
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py b/common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_profile.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/coordination_bringup/peer_profile.py
rename to common/ros_packages/coordination/coordination_bringup/coordination_bringup/peer_profile.py
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py b/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
rename to common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml b/common/ros_packages/coordination/coordination_bringup/launch/gossip.launch.xml
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/launch/gossip.launch.xml
rename to common/ros_packages/coordination/coordination_bringup/launch/gossip.launch.xml
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/package.xml b/common/ros_packages/coordination/coordination_bringup/package.xml
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/package.xml
rename to common/ros_packages/coordination/coordination_bringup/package.xml
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/resource/coordination_bringup b/common/ros_packages/coordination/coordination_bringup/resource/coordination_bringup
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/resource/coordination_bringup
rename to common/ros_packages/coordination/coordination_bringup/resource/coordination_bringup
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/gossip_node b/common/ros_packages/coordination/coordination_bringup/scripts/gossip_node
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/scripts/gossip_node
rename to common/ros_packages/coordination/coordination_bringup/scripts/gossip_node
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py b/common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/scripts/peer_registry_monitor.py
rename to common/ros_packages/coordination/coordination_bringup/scripts/peer_registry_monitor.py
diff --git a/robot/ros_ws/src/coordination/coordination_bringup/setup.py b/common/ros_packages/coordination/coordination_bringup/setup.py
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_bringup/setup.py
rename to common/ros_packages/coordination/coordination_bringup/setup.py
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt b/common/ros_packages/coordination/coordination_msgs/CMakeLists.txt
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_msgs/CMakeLists.txt
rename to common/ros_packages/coordination/coordination_msgs/CMakeLists.txt
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfile.msg
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfile.msg
rename to common/ros_packages/coordination/coordination_msgs/msg/PeerProfile.msg
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg b/common/ros_packages/coordination/coordination_msgs/msg/PeerProfilePayload.msg
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_msgs/msg/PeerProfilePayload.msg
rename to common/ros_packages/coordination/coordination_msgs/msg/PeerProfilePayload.msg
diff --git a/robot/ros_ws/src/coordination/coordination_msgs/package.xml b/common/ros_packages/coordination/coordination_msgs/package.xml
similarity index 100%
rename from robot/ros_ws/src/coordination/coordination_msgs/package.xml
rename to common/ros_packages/coordination/coordination_msgs/package.xml
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 26485565..26b7830f 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -55,7 +55,6 @@ services:
       - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
       - ../../common/ros_packages:/root/AirStack/gcs/ros_ws/src/common:rw
-      - ../../robot/ros_ws/src/coordination:/root/AirStack/gcs/ros_ws/src/coordination:ro
       - ../../common/fastdds.xml:/root/AirStack/gcs/ros_ws/src/fastdds.xml
       - ../ros_ws:/root/AirStack/gcs/ros_ws:rw
       # bags
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index 60e17925..39734fcc 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -86,11 +86,11 @@ services:
     networks: !reset null
   # ===================================================================================================================
   # for running over ethernet for hitl to stream to the NVIDIA jetson device
-  # isaac-sim-hitl:
-  #   extends:
-  #     service: isaac-sim
-  #   profiles: !override
-  #     - hitl
-  #   container_name: isaac-sim
-  #   network_mode: host
-  #   networks: !reset null
+  isaac-sim-hitl:
+    extends:
+      service: isaac-sim
+    profiles: !override
+      - hitl
+    container_name: isaac-sim
+    network_mode: host
+    networks: !reset null
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
index 5394faaa..6a38e848 100644
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
@@ -9,6 +9,12 @@
 # Start Isaac Sim's simulation environment (Must start this before importing omni modules)
 simulation_app = SimulationApp({"headless": False})
 
+# Set local Nucleus as asset root before importing Pegasus (which resolves it at import time)
+carb.settings.get_settings().set(
+    "/persistent/isaac/asset_root/default",
+    "omniverse://airlab-nucleus.andrew.cmu.edu/NVIDIA/Assets/Isaac/5.1"
+)
+
 import time
 
 import omni.kit.app
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 2f90c1ac..1af07ed8 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -6,6 +6,12 @@
 # Start Isaac Sim's simulation environment (Must start this before importing omni modules)
 simulation_app = SimulationApp({"headless": False})
 
+# Set local Nucleus as asset root before importing Pegasus (which resolves it at import time)
+carb.settings.get_settings().set(
+    "/persistent/isaac/asset_root/default",
+    "omniverse://airlab-nucleus.andrew.cmu.edu/NVIDIA/Assets/Isaac/5.1"
+)
+
 import os
 import sys
 import time

From 79a584da65b999c82852a12e24c8888100007542 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 22:09:45 -0400
Subject: [PATCH 28/55] fixed gcs<->robot dds

---
 .env                                          | 29 +++++------
 common/fastdds.xml                            |  9 ----
 .../desktop_bringup/launch/gcs.launch.xml     |  5 +-
 .../desktop_bringup/params/domain_bridge.yaml | 48 -------------------
 docker-compose.yaml                           |  1 -
 gcs/docker/gcs-base-docker-compose.yaml       |  5 +-
 robot/docker/.bashrc                          |  4 +-
 robot/docker/docker-compose.yaml              |  9 ++--
 8 files changed, 24 insertions(+), 86 deletions(-)

diff --git a/.env b/.env
index 20f568c8..47e9bad0 100644
--- a/.env
+++ b/.env
@@ -2,32 +2,34 @@
 # These variables do NOT get set within the robot container itself. For setting container environment variables, use env_file: attribute in docker-compose.yaml.
 # Standard Usage: airstack --env-file .env up
 
-# Warning: even though this file is organized into sections, all variables get propagated to all sub-level 
+# Warning: even though this file is organized into sections, all variables get propagated to all sub-level
 #  docker-compose files, so be careful about naming conflicts.
 
 # =============== PROJECT ====================
 # THESE VARIABLES ARE USED TO TAG THE DOCKER IMAGES.
 # The name of the project. This is used as the repository name for the docker images, and also as part of the default image tag.
 PROJECT_NAME="airstack"
-# If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made 
+# If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made
 # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version.
-VERSION="0.18.0-alpha.3"
+# auto-generated from git commit hash. `airstack version` will output this tag.
+# auto-generated from git commit hash
+DOCKER_IMAGE_TAG="0.17.0"
 # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image
-DOCKER_IMAGE_BUILD_MODE="dev"  
+DOCKER_IMAGE_BUILD_MODE="dev"
 # Where to push and pull images from. Can replace with your docker hub username if using docker hub.
 PROJECT_DOCKER_REGISTRY="airlab-docker.andrew.cmu.edu/airstack"
-# This is the default profile that docker compose will use if you just run `docker compose up` without specifying a profile. 
+# This is the default profile that docker compose will use if you just run `docker compose up` without specifying a profile.
 # It can be overridden with the --profile flag, or by setting the COMPOSE_PROFILES environment variable to a different value
 #  before running `docker compose up`.
-COMPOSE_PROFILES="desktop,isaac-sim"
+COMPOSE_PROFILES="desktop, isaac-sim"
 # ============================================
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="3"  # Number of robot containers to launch.
+NUM_ROBOTS="2"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
-# ============== ISAAC SIM =====================
+# ============== SIMULATION =====================
 ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_pegasus.scene.usd"
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
@@ -36,17 +38,12 @@ ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
-# ================= MS-AIRSIM =====================
-MS_AIRSIM_ENV_DIR="../environments"
-MS_AIRSIM_BINARY_PATH="/ms-airsim-env/AbandonedPark/LinuxNoEditor/AbandonedPark.sh"
-# =================================================
-
 # ================= ROBOT =====================
 ROBOT_NAME_MAP_CONFIG_FILE="default_robot_name_map.yaml"  # Determines how to set ROBOT_NAME and ROS_DOMAIN_ID. See robot/docker/robot_name_map/
 
+URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"  #
 
-URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"
-
-# offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication. 
+# offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication.
 OFFBOARD_BASE_PORT=14540
 ONBOARD_BASE_PORT=14580
+# ===============================================
diff --git a/common/fastdds.xml b/common/fastdds.xml
index 58de2126..d9d106bb 100644
--- a/common/fastdds.xml
+++ b/common/fastdds.xml
@@ -18,15 +18,6 @@
             <!-- https://answers.ros.org/question/370595/ros2-foxy-nodes-cant-communicate-through-docker-container-border/ -->
             <!-- https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#isaac-sim-app-install-ros:~:text=Run%20export%20FASTRTPS_DEFAULT_PROFILES_FILE%3D~/.ros/fastdds.xml%20in%20the%20terminals%20that%20will%20use%20ROS%202%20functions -->
             <useBuiltinTransports>false</useBuiltinTransports>
-            <builtin>
-                <initialPeersList>
-                    <locator>
-                        <udpv4>
-                            <address>192.168.233.25</address>
-                        </udpv4>
-                    </locator>
-                </initialPeersList>
-            </builtin>
         </rtps>
     </participant>
 </profiles>
diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index f912be0c..c52af4a4 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -18,15 +18,16 @@
     args="--perspective-file $(find-pkg-share rqt_gcs)/config/gcs.perspective --ros-args --log-level INFO"
     output="screen" respawn="true" respawn_delay="1" />
 
-  <?ignore
+  <!-- launch foxglove-studio -no-standbox -->
+
   <executable cmd="bash -c 'DISPLAY=$DISPLAY XAUTHORITY=$XAUTHORITY foxglove-studio --no-sandbox'" output="screen"/>
 
   <node pkg="foxglove_bridge" exec="foxglove_bridge" output="screen" respawn="true" respawn_delay="1">
     <param name="port" value="8765" />
+    <!-- <param name="address" value="0.0.0.0" /> -->
     <param name="use_sim_time" value="$(var sim)" />
     <param name="send_buffer_limit" value="10000000" />
   </node>
-  ?>
 
 
    
diff --git a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
index 491728e2..c07a5acb 100644
--- a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
+++ b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
@@ -18,54 +18,6 @@ topics:
     from_domain: 1
     to_domain: 0
     type: nav_msgs/msg/Odometry
-  
-
-  #rayfronts topics to bridge
-
-  /robot_1/current_target:
-    from_domain: 1
-    to_domain: 2
-    type: std_msgs/msg/String
-  
-  /robot_2/current_target:
-    from_domain: 2
-    to_domain: 1
-    type: std_msgs/msg/String
-
-  /robot_1/odometry_conversion/odometry:
-    from_domain: 1
-    to_domain: 2
-    type: nav_msgs/msg/Odometry
-  
-  /robot_2/odometry_conversion/odometry:
-    from_domain: 2
-    to_domain: 1
-    type: nav_msgs/msg/Odometry
-  
-  /robot_2/relative_position:
-    from_domain: 2
-    to_domain: 1
-    type: nav_msgs/msg/Odometry
-  
-  /robot_1/filtered_rays:
-    from_domain: 1
-    to_domain: 2
-    type: visualization_msgs/msg/MarkerArray
-  
-  /robot_2/filtered_rays:
-    from_domain: 2
-    to_domain: 1
-    type: visualization_msgs/msg/MarkerArray
-  
-  /robot_2/rayfronts/semantic_ray_frontiers:
-    from_domain: 2
-    to_domain: 1
-    type: visualization_msgs/msg/MarkerArray
-  
-  /robot_1/rayfronts/semantic_ray_frontiers:
-    from_domain: 1
-    to_domain: 2
-    type: visualization_msgs/msg/MarkerArray
 
   /robot_2/vdb_mapping/vdb_map_visualization:
     from_domain: 2
diff --git a/docker-compose.yaml b/docker-compose.yaml
index 9511e696..e2e26c03 100644
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -6,7 +6,6 @@
 include:
   - simulation/isaac-sim/docker/docker-compose.yaml
   - simulation/simple-sim/docker/docker-compose.yaml
-  - simulation/ms-airsim/docker/docker-compose.yaml
   - robot/docker/docker-compose.yaml
   - gcs/docker/docker-compose.yaml
   - docs/docker/docker-compose.yaml
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 26b7830f..f9b8fd38 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -6,7 +6,7 @@ services:
       context: ../
       dockerfile: docker/Dockerfile.gcs
       tags:
-        - ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_gcs
+        - ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_gcs
     command: >
       bash -c "
       ssh service restart;
@@ -33,7 +33,7 @@ services:
       - NVIDIA_DRIVER_CAPABILITIES=all
       # Record bags
       - RECORD_BAGS=${RECORD_BAGS}
-    image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_gcs
+    image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_gcs
     # Needed to display graphical applications
     # ipc: host
     privileged: true
@@ -55,6 +55,7 @@ services:
       - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
       - ../../common/ros_packages:/root/AirStack/gcs/ros_ws/src/common:rw
+      - ../../robot/ros_ws/src/coordination:/root/AirStack/gcs/ros_ws/src/coordination:ro
       - ../../common/fastdds.xml:/root/AirStack/gcs/ros_ws/src/fastdds.xml
       - ../ros_ws:/root/AirStack/gcs/ros_ws:rw
       # bags
diff --git a/robot/docker/.bashrc b/robot/docker/.bashrc
index d2c3c877..e4f21b84 100755
--- a/robot/docker/.bashrc
+++ b/robot/docker/.bashrc
@@ -17,10 +17,8 @@ export PYTHONWARNINGS
 # Convenience functions for ROS2 workspace
 
 function bws(){
-    # Since multiple robot containers may mount a shared volume, we need to use flock to prevent race conditions.
     echo "Running \`colcon build $@\` in $ROS2_WS_DIR"
-    COLCON_LOG_PATH="$ROS2_WS_DIR"/log flock "$ROS2_WS_DIR/.build.lock" \
-        colcon build --symlink-install --base-paths "$ROS2_WS_DIR"/ --build-base "$ROS2_WS_DIR"/build/ --install-base "$ROS2_WS_DIR"/install/ "$@"
+    COLCON_LOG_PATH="$ROS2_WS_DIR"/log colcon build --symlink-install --base-paths "$ROS2_WS_DIR"/ --build-base "$ROS2_WS_DIR"/build/ --install-base "$ROS2_WS_DIR"/install/ "$@"
 }
 function sws(){
     if [ -f "$ROS2_WS_DIR/install/local_setup.bash" ]; then
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index abfef1cb..f06693e1 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -9,7 +9,7 @@ services:
     extends:
       file: ./robot-base-docker-compose.yaml
       service: robot_base
-    image: &desktop_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_robot-x86-64_${DOCKER_IMAGE_BUILD_MODE}
+    image: &desktop_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_robot-x86-64_${DOCKER_IMAGE_BUILD_MODE}
     build:
       dockerfile: ./Dockerfile.robot
       args:
@@ -23,7 +23,6 @@ services:
       - NVIDIA_DRIVER_CAPABILITIES=all
       - LAUNCH_PACKAGE=desktop_bringup  # desktop_bringup adds RViz; real robots use autonomy_bringup
       - AUTONOMY_ROLE=full
-      - SIM_IP=${SIM_IP:-172.31.0.200}
       # FCU_URL and TGT_SYSTEM not set, dynamically calculated in interface.launch.py
     # 'command' uses variables so that it can be shared across robot-desktop and robot-l4t, with different launch packages and roles.
     command: >
@@ -105,7 +104,7 @@ services:
     extends:  # NOTE THIS EXTENDS THE BASE, NOT ROBOT-DESKTOP
       file: ./robot-base-docker-compose.yaml
       service: robot_base
-    image: &voxl_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_robot-voxl_${DOCKER_IMAGE_BUILD_MODE}
+    image: &voxl_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_robot-voxl_${DOCKER_IMAGE_BUILD_MODE}
     build:
       dockerfile: ./Dockerfile.robot
       args:
@@ -140,7 +139,7 @@ services:
     extends:
       file: ./robot-base-docker-compose.yaml
       service: robot_base
-    image: &l4t_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_robot-l4t_${DOCKER_IMAGE_BUILD_MODE}
+    image: &l4t_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_robot-l4t_${DOCKER_IMAGE_BUILD_MODE}
     build:
       dockerfile: ./Dockerfile.robot
       args:
@@ -195,7 +194,7 @@ services:
   zed-l4t:
     profiles:
       - l4t
-    image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_zed-l4t-36-4-0_${DOCKER_IMAGE_BUILD_MODE}
+    image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_zed-l4t-36-4-0_${DOCKER_IMAGE_BUILD_MODE}
     build: 
       context: ./
       dockerfile: zed/Dockerfile.zed-l4t

From b1f2b06648065a3761052dee47553611df5a2a3d Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 22:21:38 -0400
Subject: [PATCH 29/55] added hitl functionality

---
 .env                                          |  11 ++
 .../desktop_bringup/launch/gcs.launch.xml     |   2 +-
 docs/gcs/docker/index.md                      |  30 ++++-
 docs/real_world/HITL/index.md                 | 123 +++++++++++++++---
 docs/simulation/index.md                      |  26 ++++
 gcs/docker/.bashrc                            |  49 +++++++
 gcs/docker/docker-compose.yaml                |  31 +++++
 robot/docker/.bashrc                          |  49 +++++++
 robot/docker/docker-compose.yaml              |  20 +++
 simulation/isaac-sim/docker/.bashrc           |  48 +++++++
 .../isaac-sim/docker/docker-compose.yaml      |   6 +
 11 files changed, 376 insertions(+), 19 deletions(-)

diff --git a/.env b/.env
index 47e9bad0..891b6a52 100644
--- a/.env
+++ b/.env
@@ -46,4 +46,15 @@ URDF_FILE="robot_descriptions/iris/urdf/iris_with_sensors.pegasus.robot.urdf"  #
 # offboard API streaming out. this is so that ports don't conflict for multi-agent FCU communication.
 OFFBOARD_BASE_PORT=14540
 ONBOARD_BASE_PORT=14580
+
+# ============== HITL DDS DISCOVERY ==============
+# Discovery mode for hitl profiles: "server" (default, scalable) or "static-peer" (fallback).
+HITL_DISCOVERY_MODE="server"
+DISCOVERY_SERVER_IP="192.168.233.25"
+DISCOVERY_SERVER_PORT="11811"
+DISCOVERY_SERVER_ID="0"
+# Optional comma-separated backup discovery server IPs (same port).
+DISCOVERY_SERVER_BACKUP_IPS=""
+# Used only when HITL_DISCOVERY_MODE=static-peer.
+FASTDDS_STATIC_PEER_IP="192.168.233.25"
 # ===============================================
diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index c52af4a4..3bec5211 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -24,7 +24,7 @@
 
   <node pkg="foxglove_bridge" exec="foxglove_bridge" output="screen" respawn="true" respawn_delay="1">
     <param name="port" value="8765" />
-    <!-- <param name="address" value="0.0.0.0" /> -->
+    <param name="address" value="0.0.0.0" />
     <param name="use_sim_time" value="$(var sim)" />
     <param name="send_buffer_limit" value="10000000" />
   </node>
diff --git a/docs/gcs/docker/index.md b/docs/gcs/docker/index.md
index 4d94181f..ae6db45d 100644
--- a/docs/gcs/docker/index.md
+++ b/docs/gcs/docker/index.md
@@ -50,12 +50,20 @@ The GCS container is launched with:
 airstack up gcs
 ```
 
+For HITL (host networking + scalable DDS discovery), use:
+
+```bash
+airstack up gcs-hitl
+```
+
+`gcs-hitl` automatically starts `dds-discovery-hitl` via `depends_on`, so the discovery server always runs on the GCS host in HITL mode.
+
 **What happens:**
 
 1. Docker Compose starts the GCS container from `gcs/docker/docker-compose.yaml`
 2. The `command:` attribute launches GCS ROS 2 nodes
 3. RQT GUI opens automatically (if `DISPLAY` configured)
-4. Foxglove Studio starts on port 8765
+4. Foxglove Studio starts and `foxglove_bridge` serves on port 8765
 
 **Container command:**
 
@@ -106,6 +114,14 @@ The GCS container connects to the `airstack_network` bridge network, allowing co
 - **ROS_DOMAIN_ID:** 0 (GCS shares domain 0)
 - **Ports:** 8765 (Foxglove), 22 (SSH)
 
+### HITL Host-Network Mode
+
+When using `gcs-hitl`, the container runs in `network_mode: host`:
+
+- Foxglove bridge binds to `0.0.0.0:8765`
+- DDS discovery uses `ROS_DISCOVERY_SERVER` (from `DISCOVERY_SERVER_IP` / `DISCOVERY_SERVER_PORT`)
+- No `airstack_network` bridge isolation; traffic is on the host LAN interfaces
+
 ## Accessing the GCS
 
 ### Via X11 Forwarding
@@ -143,6 +159,12 @@ Access Foxglove Studio in your web browser:
 http://localhost:8765
 ```
 
+For remote visualization from another machine on the same network:
+
+```
+ws://<gcs_host_ip>:8765
+```
+
 Pre-configured layouts are available in `gcs/docker/Foxglove/layouts/`.
 
 ## Foxglove Configuration
@@ -250,6 +272,12 @@ docker compose build --no-cache gcs
 - Check container networking: `docker inspect airstack-gcs-1`
 - Access via container IP if localhost fails
 
+**HITL DDS discovery issues:**
+
+- Verify `ROS_DISCOVERY_SERVER` is set in the GCS container
+- Verify `DISCOVERY_SERVER_IP:DISCOVERY_SERVER_PORT` is reachable from all robot hosts
+- If discovery works but no data flows, verify DDS Router allowlist and QoS compatibility
+
 **ROS 2 communication issues:**
 
 - Verify `ROS_DOMAIN_ID=0` in GCS container
diff --git a/docs/real_world/HITL/index.md b/docs/real_world/HITL/index.md
index bf6057c7..f7f373d9 100644
--- a/docs/real_world/HITL/index.md
+++ b/docs/real_world/HITL/index.md
@@ -1,25 +1,114 @@
 # Hardware-In-The-Loop Simulation
-We configure a multi-machine HITL simulation, where a powerful desktop computer runs Isaac Simulator and rendering, and one/multiple jetson compute boards run robot-specific programs (planning, mapping, etc.).
-## Requirement
-A desktop computer configured according to [here](/docs/getting_started). One/multiple ORIN AGX/NX configured according to [here](/docs/real_world/installation/).
 
-## Communication
-All machines should connect to the same network. In our test, all machines are connected to the same router with ethernet cables. Ensure that all machines are able to `ping` others' IP addresses.
+AirStack supports multi-machine HITL where simulation and GCS run on one host, and one or more robot compute hosts run autonomy containers. HITL services use `network_mode: host` so DDS traffic is exchanged directly on your LAN.
 
-### Run
-On the desktop computer, under your Airstack folder, run
-```
-docker compose up isaac-sim-hitl
+## Requirements
+
+- One simulation/GCS host configured per [Getting Started](/docs/getting_started)
+- One or more robot hosts (Jetson or desktop) configured per [Real-World Installation](/docs/real_world/installation/)
+- All hosts on the same routed network with mutual `ping` reachability
+
+## Discovery Model (Default)
+
+HITL defaults to Fast DDS Discovery Server mode for scalability:
+
+- Discovery server endpoint defaults to:
+  - `DISCOVERY_SERVER_IP=192.168.233.25`
+  - `DISCOVERY_SERVER_PORT=11811`
+- Discovery clients:
+  - `isaac-sim-hitl`
+  - `gcs-hitl`
+  - `robot-desktop-hitl`
+- Optional fallback mode:
+  - `HITL_DISCOVERY_MODE=static-peer`
+  - `FASTDDS_STATIC_PEER_IP=<peer_ip>`
+
+## Services
+
+### Simulation/GCS Host
+
+Run:
+
+```bash
+airstack up isaac-sim-hitl gcs-hitl
 ```
-You should see the isaac simulator being launched.
-On the Jetson computer, run
+
+This starts:
+
+- Isaac Sim (`isaac-sim-hitl`)
+- GCS (`gcs-hitl`)
+- Fast DDS discovery server (`dds-discovery-hitl`, auto-started as dependency of `gcs-hitl`)
+
+### Robot Host
+
+For a desktop robot host:
+
+```bash
+airstack up robot-desktop-hitl
 ```
-docker compose up robot_l4t
+
+For a Jetson host:
+
+```bash
+airstack up robot-l4t
 ```
-Once the scene is played in the Isaac simulator, the rviz GUI on the Jetson should start displaying sensor data, which means the connection is successful. 
 
-Screen record of desktop computer:
-<iframe src="https://drive.google.com/file/d/1sNkEattgDyBAI9xFPVQsXn8sRi6gYQSG/view?usp=sharing" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
+## Two-Computer Validation Checklist
+
+1. Verify discovery variables on both hosts:
+   ```bash
+   echo "$DISCOVERY_SERVER_IP:$DISCOVERY_SERVER_PORT"
+   ```
+2. Verify DDS discovery server visibility from robot host:
+   ```bash
+   ping "$DISCOVERY_SERVER_IP"
+   ```
+3. Verify ROS graph visibility:
+   ```bash
+   ros2 topic list
+   ```
+4. Verify payload flow (not just discovery):
+   ```bash
+   ros2 topic hz /robot_1/odometry
+   ros2 topic echo /robot_1/odometry --once
+   ```
+5. Verify Foxglove:
+   - On GCS host: `ws://localhost:8765`
+   - On remote client (optional): `ws://<gcs_host_ip>:8765`
+
+## Multi-Robot / Multi-Computer Scaling
+
+For `N` robot hosts:
+
+- `gcs-hitl` starts and owns the discovery server on the sim/GCS host.
+- Start one robot container stack per robot host (`robot-desktop-hitl` or `robot-l4t`).
+- Ensure each robot has unique `ROBOT_NAME` and `ROS_DOMAIN_ID` mapping.
+
+Recommended:
+
+- Use discovery server mode for `N >= 3`.
+- Configure backup discovery servers using `DISCOVERY_SERVER_BACKUP_IPS` (comma-separated IP list).
+
+## Troubleshooting
+
+### Topics discovered but no data
+
+This is usually not discovery failure. Check:
+
+1. DDS Router allowlist includes the topic
+2. ROS domain IDs are correct on both sides
+3. QoS compatibility between publisher/subscriber
+4. Firewall allows DDS UDP traffic
+5. Topic namespaced correctly (`/robot_X/...`)
+
+### Foxglove shows no topics
+
+- Confirm `foxglove_bridge` is running in GCS launch
+- Confirm GCS is receiving ROS topics in domain 0
+- Use `ws://localhost:8765` on GCS host first, then test remote URL
+
+### Discovery server unreachable
 
-Screen record of Jetson computer:
-<iframe src="https://drive.google.com/file/d/19S8Yceq8t2FPubN8Mly003Up1AbLzCA8/view?usp=sharing" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
\ No newline at end of file
+- Verify `DISCOVERY_SERVER_IP` matches the host running `gcs-hitl` (which auto-starts `dds-discovery-hitl`)
+- Verify no port conflict on `11811`
+- Verify host firewall/network ACLs
\ No newline at end of file
diff --git a/docs/simulation/index.md b/docs/simulation/index.md
index 6927aaa4..ddde7f33 100644
--- a/docs/simulation/index.md
+++ b/docs/simulation/index.md
@@ -133,6 +133,26 @@ A lightweight 2D/3D simulator for basic testing and development when full Isaac
 
 **Learn more:** [Docker Workflow](../development/beginner/airstack-cli/docker_usage.md#robot)
 
+### Multi-Computer HITL Simulation
+
+For cross-machine HITL, run simulation + GCS on one host and robot containers on other hosts:
+
+```bash
+# Sim/GCS host
+airstack up isaac-sim-hitl gcs-hitl
+
+# Robot host
+airstack up robot-desktop-hitl
+```
+
+Discovery defaults:
+
+- `HITL_DISCOVERY_MODE=server`
+- `DISCOVERY_SERVER_IP=192.168.233.25`
+- `DISCOVERY_SERVER_PORT=11811`
+
+For larger fleets, set `DISCOVERY_SERVER_BACKUP_IPS` with backup server IPs.
+
 ### Custom Scenes
 
 Create custom Isaac Sim scenes with:
@@ -184,6 +204,12 @@ PLAY_SIM_ON_START=false airstack up isaac-sim
 - Check `ROS_DOMAIN_ID` settings in containers
 - See [Docker Workflow](../development/beginner/airstack-cli/docker_usage.md)
 
+**HITL discovery but no payload data:**
+
+- Verify `ROS_DISCOVERY_SERVER` in all HITL containers
+- Verify DDS Router allowlist includes required topics
+- Verify QoS compatibility (`ros2 topic info -v <topic>`)
+
 **Performance issues:**
 
 - Reduce scene complexity
diff --git a/gcs/docker/.bashrc b/gcs/docker/.bashrc
index ba851a34..03ffeb85 100644
--- a/gcs/docker/.bashrc
+++ b/gcs/docker/.bashrc
@@ -10,6 +10,55 @@ ROS2_WS_DIR="$HOME/AirStack/gcs/ros_ws"
 # needed for communication with Isaac Sim ROS2  # https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#enabling-the-ros-bridge-extension
 export FASTRTPS_DEFAULT_PROFILES_FILE="$ROS2_WS_DIR/src/fastdds.xml"
 
+# HITL DDS mode:
+# - server (default): use Fast DDS Discovery Server via ROS_DISCOVERY_SERVER.
+# - static-peer: generate a temporary Fast DDS profile with one initial peer.
+if [ "${HITL_DISCOVERY_MODE:-}" = "static-peer" ]; then
+    if [ -n "${FASTDDS_STATIC_PEER_IP:-}" ]; then
+        cat > /tmp/fastdds_static_peer.xml <<EOF
+<?xml version="1.0" encoding="UTF-8" ?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
+    <transport_descriptors>
+        <transport_descriptor>
+            <transport_id>UdpTransport</transport_id>
+            <type>UDPv4</type>
+        </transport_descriptor>
+    </transport_descriptors>
+    <participant profile_name="udp_transport_profile" is_default_profile="true">
+        <rtps>
+            <userTransports>
+                <transport_id>UdpTransport</transport_id>
+            </userTransports>
+            <useBuiltinTransports>false</useBuiltinTransports>
+            <builtin>
+                <initialPeersList>
+                    <locator>
+                        <udpv4>
+                            <address>${FASTDDS_STATIC_PEER_IP}</address>
+                        </udpv4>
+                    </locator>
+                </initialPeersList>
+            </builtin>
+        </rtps>
+    </participant>
+</profiles>
+EOF
+        export FASTRTPS_DEFAULT_PROFILES_FILE="/tmp/fastdds_static_peer.xml"
+    fi
+    unset ROS_DISCOVERY_SERVER
+elif [ -n "${DISCOVERY_SERVER_IP:-}" ]; then
+    discovery_server_list="${DISCOVERY_SERVER_IP}:${DISCOVERY_SERVER_PORT:-11811}"
+    if [ -n "${DISCOVERY_SERVER_BACKUP_IPS:-}" ]; then
+        IFS=',' read -r -a discovery_backup_ips <<< "${DISCOVERY_SERVER_BACKUP_IPS}"
+        for backup_ip in "${discovery_backup_ips[@]}"; do
+            if [ -n "${backup_ip}" ]; then
+                discovery_server_list="${discovery_server_list};${backup_ip}:${DISCOVERY_SERVER_PORT:-11811}"
+            fi
+        done
+    fi
+    export ROS_DISCOVERY_SERVER="${discovery_server_list}"
+fi
+
 # fix ROS2 jazzy setuptools deprecation warning https://robotics.stackexchange.com/questions/24230/setuptoolsdeprecationwarning-in-ros2-jazzy/24349#24349
 PYTHONWARNINGS="ignore:easy_install command is deprecated,ignore:setup.py install is deprecated"
 export PYTHONWARNINGS
diff --git a/gcs/docker/docker-compose.yaml b/gcs/docker/docker-compose.yaml
index b73f9bc8..2ce5165c 100644
--- a/gcs/docker/docker-compose.yaml
+++ b/gcs/docker/docker-compose.yaml
@@ -27,3 +27,34 @@ services:
       - $HOME/bags:/bags
       # TODO: Update rviz config location if needed
       - ../plot:/plot
+
+  # ===================================================================================================================
+  # hitl alias for host-network GCS with discovery-server client defaults.
+  gcs-hitl:
+    profiles: !override
+      - hitl
+    extends:
+      file: ./docker-compose.yaml
+      service: gcs-real
+    depends_on:
+      - dds-discovery-hitl
+    environment:
+      - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
+      - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
+      - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
+      - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
+      - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
+
+  # ===================================================================================================================
+  # Fast DDS discovery server for scalable multi-host HITL discovery.
+  dds-discovery-hitl:
+    profiles:
+      - hitl
+    image: eprosima/fastdds:latest
+    network_mode: host
+    command: >
+      fastdds discovery
+      -i ${DISCOVERY_SERVER_ID:-0}
+      -l 0.0.0.0
+      -p ${DISCOVERY_SERVER_PORT:-11811}
+    restart: unless-stopped
diff --git a/robot/docker/.bashrc b/robot/docker/.bashrc
index e4f21b84..14d29b2a 100755
--- a/robot/docker/.bashrc
+++ b/robot/docker/.bashrc
@@ -10,6 +10,55 @@ ROS2_WS_DIR="$HOME/AirStack/robot/ros_ws"
 # needed for communication with Isaac Sim ROS2  # https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#enabling-the-ros-bridge-extension
 export FASTRTPS_DEFAULT_PROFILES_FILE="$ROS2_WS_DIR/src/fastdds.xml"
 
+# HITL DDS mode:
+# - server (default): use Fast DDS Discovery Server via ROS_DISCOVERY_SERVER.
+# - static-peer: generate a temporary Fast DDS profile with one initial peer.
+if [ "${HITL_DISCOVERY_MODE:-}" = "static-peer" ]; then
+    if [ -n "${FASTDDS_STATIC_PEER_IP:-}" ]; then
+        cat > /tmp/fastdds_static_peer.xml <<EOF
+<?xml version="1.0" encoding="UTF-8" ?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
+    <transport_descriptors>
+        <transport_descriptor>
+            <transport_id>UdpTransport</transport_id>
+            <type>UDPv4</type>
+        </transport_descriptor>
+    </transport_descriptors>
+    <participant profile_name="udp_transport_profile" is_default_profile="true">
+        <rtps>
+            <userTransports>
+                <transport_id>UdpTransport</transport_id>
+            </userTransports>
+            <useBuiltinTransports>false</useBuiltinTransports>
+            <builtin>
+                <initialPeersList>
+                    <locator>
+                        <udpv4>
+                            <address>${FASTDDS_STATIC_PEER_IP}</address>
+                        </udpv4>
+                    </locator>
+                </initialPeersList>
+            </builtin>
+        </rtps>
+    </participant>
+</profiles>
+EOF
+        export FASTRTPS_DEFAULT_PROFILES_FILE="/tmp/fastdds_static_peer.xml"
+    fi
+    unset ROS_DISCOVERY_SERVER
+elif [ -n "${DISCOVERY_SERVER_IP:-}" ]; then
+    discovery_server_list="${DISCOVERY_SERVER_IP}:${DISCOVERY_SERVER_PORT:-11811}"
+    if [ -n "${DISCOVERY_SERVER_BACKUP_IPS:-}" ]; then
+        IFS=',' read -r -a discovery_backup_ips <<< "${DISCOVERY_SERVER_BACKUP_IPS}"
+        for backup_ip in "${discovery_backup_ips[@]}"; do
+            if [ -n "${backup_ip}" ]; then
+                discovery_server_list="${discovery_server_list};${backup_ip}:${DISCOVERY_SERVER_PORT:-11811}"
+            fi
+        done
+    fi
+    export ROS_DISCOVERY_SERVER="${discovery_server_list}"
+fi
+
 # fix ROS2 jazzy setuptools deprecation warning https://robotics.stackexchange.com/questions/24230/setuptoolsdeprecationwarning-in-ros2-humble/24349#24349
 PYTHONWARNINGS="ignore:easy_install command is deprecated,ignore:setup.py install is deprecated"
 export PYTHONWARNINGS
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index f06693e1..0e46235a 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -51,6 +51,26 @@ services:
               count: 1
               capabilities: [gpu]
 
+  # ===================================================================================================================
+  # hitl desktop: run full robot stack on a desktop host network for multi-machine HITL.
+  robot-desktop-hitl:
+    profiles: !override
+      - hitl
+    extends:
+      file: ./docker-compose.yaml
+      service: robot-desktop
+    network_mode: host
+    networks: !reset null
+    ports: !reset {}
+    deploy: !reset {}
+    environment:
+      - AUTONOMY_ROLE=full
+      - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
+      - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
+      - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
+      - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
+      - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
+
   # ===================================================================================================================
   # desktop_split: simulates onboard computer on desktop for debugging the split configuration.
   # Same image as robot-desktop; role=onboard means only lite autonomy modules launch.
diff --git a/simulation/isaac-sim/docker/.bashrc b/simulation/isaac-sim/docker/.bashrc
index 256a5221..76904177 100644
--- a/simulation/isaac-sim/docker/.bashrc
+++ b/simulation/isaac-sim/docker/.bashrc
@@ -127,6 +127,54 @@ source /isaac-sim/jazzy_ws/install/setup.bash  # isaacsim ros2 package
 # needed for communication with Isaac Sim ROS2  # https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#enabling-the-ros-bridge-extension
 export FASTRTPS_DEFAULT_PROFILES_FILE="/isaac-sim/.ros/fastdds.xml"
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+# HITL DDS mode:
+# - server (default): use Fast DDS Discovery Server via ROS_DISCOVERY_SERVER.
+# - static-peer: generate a temporary Fast DDS profile with one initial peer.
+if [ "${HITL_DISCOVERY_MODE:-}" = "static-peer" ]; then
+    if [ -n "${FASTDDS_STATIC_PEER_IP:-}" ]; then
+        cat > /tmp/fastdds_static_peer.xml <<EOF
+<?xml version="1.0" encoding="UTF-8" ?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
+    <transport_descriptors>
+        <transport_descriptor>
+            <transport_id>UdpTransport</transport_id>
+            <type>UDPv4</type>
+        </transport_descriptor>
+    </transport_descriptors>
+    <participant profile_name="udp_transport_profile" is_default_profile="true">
+        <rtps>
+            <userTransports>
+                <transport_id>UdpTransport</transport_id>
+            </userTransports>
+            <useBuiltinTransports>false</useBuiltinTransports>
+            <builtin>
+                <initialPeersList>
+                    <locator>
+                        <udpv4>
+                            <address>${FASTDDS_STATIC_PEER_IP}</address>
+                        </udpv4>
+                    </locator>
+                </initialPeersList>
+            </builtin>
+        </rtps>
+    </participant>
+</profiles>
+EOF
+        export FASTRTPS_DEFAULT_PROFILES_FILE="/tmp/fastdds_static_peer.xml"
+    fi
+    unset ROS_DISCOVERY_SERVER
+elif [ -n "${DISCOVERY_SERVER_IP:-}" ]; then
+    discovery_server_list="${DISCOVERY_SERVER_IP}:${DISCOVERY_SERVER_PORT:-11811}"
+    if [ -n "${DISCOVERY_SERVER_BACKUP_IPS:-}" ]; then
+        IFS=',' read -r -a discovery_backup_ips <<< "${DISCOVERY_SERVER_BACKUP_IPS}"
+        for backup_ip in "${discovery_backup_ips[@]}"; do
+            if [ -n "${backup_ip}" ]; then
+                discovery_server_list="${discovery_server_list};${backup_ip}:${DISCOVERY_SERVER_PORT:-11811}"
+            fi
+        done
+    fi
+    export ROS_DISCOVERY_SERVER="${discovery_server_list}"
+fi
 # for local development, prevent conflict with other desktops
 export ROS_AUTOMATIC_DISCOVERY_RANGE=SUBNET
 
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index 39734fcc..8310800c 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -94,3 +94,9 @@ services:
     container_name: isaac-sim
     network_mode: host
     networks: !reset null
+    environment:
+      - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
+      - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
+      - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
+      - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
+      - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}

From 4a5019d7316e9a781addeec5c86eaa1ee8b6d0cd Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 22:28:59 -0400
Subject: [PATCH 30/55] fixes to dds

---
 gcs/docker/docker-compose.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/gcs/docker/docker-compose.yaml b/gcs/docker/docker-compose.yaml
index 2ce5165c..28063806 100644
--- a/gcs/docker/docker-compose.yaml
+++ b/gcs/docker/docker-compose.yaml
@@ -50,7 +50,7 @@ services:
   dds-discovery-hitl:
     profiles:
       - hitl
-    image: eprosima/fastdds:latest
+    image: eprosima/discovery-server:latest
     network_mode: host
     command: >
       fastdds discovery

From c101593ecc637e3bfee6c56ab9cebb565f7f09fc Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 22:34:11 -0400
Subject: [PATCH 31/55] put dds hitl under gcs

---
 docs/gcs/docker/index.md                |  2 +-
 docs/real_world/HITL/index.md           |  4 ++--
 gcs/docker/docker-compose.yaml          | 16 ----------------
 gcs/docker/gcs-base-docker-compose.yaml |  8 ++++++++
 4 files changed, 11 insertions(+), 19 deletions(-)

diff --git a/docs/gcs/docker/index.md b/docs/gcs/docker/index.md
index ae6db45d..6c76ee8a 100644
--- a/docs/gcs/docker/index.md
+++ b/docs/gcs/docker/index.md
@@ -56,7 +56,7 @@ For HITL (host networking + scalable DDS discovery), use:
 airstack up gcs-hitl
 ```
 
-`gcs-hitl` automatically starts `dds-discovery-hitl` via `depends_on`, so the discovery server always runs on the GCS host in HITL mode.
+`gcs-hitl` starts the Fast DDS discovery server process inside the same container when `HITL_DISCOVERY_MODE=server`, so discovery is owned by the GCS host without a separate image pull.
 
 **What happens:**
 
diff --git a/docs/real_world/HITL/index.md b/docs/real_world/HITL/index.md
index f7f373d9..ecabf770 100644
--- a/docs/real_world/HITL/index.md
+++ b/docs/real_world/HITL/index.md
@@ -37,7 +37,7 @@ This starts:
 
 - Isaac Sim (`isaac-sim-hitl`)
 - GCS (`gcs-hitl`)
-- Fast DDS discovery server (`dds-discovery-hitl`, auto-started as dependency of `gcs-hitl`)
+- Fast DDS discovery server process inside the `gcs-hitl` container
 
 ### Robot Host
 
@@ -109,6 +109,6 @@ This is usually not discovery failure. Check:
 
 ### Discovery server unreachable
 
-- Verify `DISCOVERY_SERVER_IP` matches the host running `gcs-hitl` (which auto-starts `dds-discovery-hitl`)
+- Verify `DISCOVERY_SERVER_IP` matches the host running `gcs-hitl`
 - Verify no port conflict on `11811`
 - Verify host firewall/network ACLs
\ No newline at end of file
diff --git a/gcs/docker/docker-compose.yaml b/gcs/docker/docker-compose.yaml
index 28063806..98e283b1 100644
--- a/gcs/docker/docker-compose.yaml
+++ b/gcs/docker/docker-compose.yaml
@@ -36,25 +36,9 @@ services:
     extends:
       file: ./docker-compose.yaml
       service: gcs-real
-    depends_on:
-      - dds-discovery-hitl
     environment:
       - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
       - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
       - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
       - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
       - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
-
-  # ===================================================================================================================
-  # Fast DDS discovery server for scalable multi-host HITL discovery.
-  dds-discovery-hitl:
-    profiles:
-      - hitl
-    image: eprosima/discovery-server:latest
-    network_mode: host
-    command: >
-      fastdds discovery
-      -i ${DISCOVERY_SERVER_ID:-0}
-      -l 0.0.0.0
-      -p ${DISCOVERY_SERVER_PORT:-11811}
-    restart: unless-stopped
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index f9b8fd38..235b57e7 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -12,6 +12,14 @@ services:
       ssh service restart;
       python3 /root/AirStack/gcs/foxglove_extensions/install.sh;
       tmux new -d -s bringup;
+      if [ $$HITL_DISCOVERY_MODE = 'server' ]; then
+        if command -v fastdds >/dev/null 2>&1; then
+          tmux new-window -t bringup -n dds_discovery;
+          tmux send-keys -t bringup:dds_discovery 'fastdds discovery -i $${DISCOVERY_SERVER_ID:-0} -l 0.0.0.0 -p $${DISCOVERY_SERVER_PORT:-11811}' ENTER;
+        else
+          echo '[WARN] fastdds CLI not found in gcs container; discovery server not started.';
+        fi;
+      fi;
       if [ $$AUTOLAUNCH = 'true' ]; then
         tmux send-keys -t bringup:0.0 'bws &&  sws; ros2 launch desktop_bringup gcs.launch.xml' ENTER;
       fi;

From 89a572c4312cc06ad746ee653878e13e383f618a Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 23:00:03 -0400
Subject: [PATCH 32/55] fixes to robot hitl

---
 airstack.sh                             | 62 ++++++++++++++++++++++++-
 gcs/docker/gcs-base-docker-compose.yaml |  1 -
 robot/docker/.bashrc                    |  8 +++-
 robot/docker/docker-compose.yaml        |  2 +
 4 files changed, 69 insertions(+), 4 deletions(-)

diff --git a/airstack.sh b/airstack.sh
index b4c91df0..f5199643 100755
--- a/airstack.sh
+++ b/airstack.sh
@@ -119,6 +119,7 @@ function print_command_help {
             echo "Options:"
             echo "  --build       Build images before starting containers"
             echo "  --recreate    Recreate containers even if their configuration and image haven't changed"
+            echo "  --robot_num N Set ROBOT_NAME/ROS_DOMAIN_ID for robot HITL containers (numeric only)"
             ;;
         images)
             echo "Usage: airstack images"
@@ -702,9 +703,60 @@ function classify_compose_args {
 function cmd_up {
     check_docker
 
+    local robot_num=""
+    local filtered_args=()
+    local args=("$@")
+    local i=0
+    while [ $i -lt ${#args[@]} ]; do
+        local arg="${args[$i]}"
+        if [ "$arg" = "--robot_num" ]; then
+            i=$((i+1))
+            if [ $i -ge ${#args[@]} ]; then
+                log_error "--robot_num requires a value"
+                exit 1
+            fi
+            robot_num="${args[$i]}"
+            if ! [[ "$robot_num" =~ ^[0-9]+$ ]]; then
+                log_error "--robot_num must be a number (received: $robot_num)"
+                exit 1
+            fi
+        elif [[ "$arg" == --robot_num=* ]]; then
+            robot_num="${arg#--robot_num=}"
+            if ! [[ "$robot_num" =~ ^[0-9]+$ ]]; then
+                log_error "--robot_num must be a number (received: $robot_num)"
+                exit 1
+            fi
+        else
+            filtered_args+=("$arg")
+        fi
+        i=$((i+1))
+    done
+
     local global_args=()
     local subcmd_args=()
-    classify_compose_args global_args subcmd_args "$@"
+    classify_compose_args global_args subcmd_args "${filtered_args[@]}"
+
+    local service_count=0
+    local target_is_hitl=false
+    for s in "${subcmd_args[@]}"; do
+        if [[ "$s" != -* ]]; then
+            service_count=$((service_count+1))
+            if [ "$s" = "robot-desktop-hitl" ]; then
+                target_is_hitl=true
+            fi
+        fi
+    done
+
+    if [ -n "$robot_num" ]; then
+        if [ "$service_count" -ne 1 ] || [ "$target_is_hitl" = false ]; then
+            log_error "--robot_num can only be used with exactly one service: robot-desktop-hitl"
+            log_error "Example: airstack up robot-desktop-hitl --robot_num 2"
+            exit 1
+        fi
+    elif [ "$service_count" -eq 1 ] && [ "$target_is_hitl" = true ]; then
+        robot_num="1"
+        log_info "No --robot_num provided; defaulting to robot_num=1 for robot-desktop-hitl"
+    fi
 
     # Ensure only one simulator profile is active
     local p="${COMPOSE_PROFILES:-$(sed -n 's/^COMPOSE_PROFILES=//p' "$PROJECT_ROOT/.env" 2>/dev/null | tr -d '"')}"
@@ -723,7 +775,13 @@ function cmd_up {
     xhost + &> /dev/null || true
 
     log_info "Starting services..."
-    run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${global_args[@]}" up "${subcmd_args[@]}" -d
+    if [ -n "$robot_num" ]; then
+        local robot_name="robot_${robot_num}"
+        log_info "Using robot identity from --robot_num: ROBOT_NAME=${robot_name}, ROS_DOMAIN_ID=${robot_num}"
+        ROBOT_NAME="$robot_name" ROS_DOMAIN_ID="$robot_num" run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${global_args[@]}" up "${subcmd_args[@]}" -d
+    else
+        run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${global_args[@]}" up "${subcmd_args[@]}" -d
+    fi
     log_info "Services brought up successfully"
 }
 
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 235b57e7..2291f0d3 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -63,7 +63,6 @@ services:
       - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
       - ../../common/ros_packages:/root/AirStack/gcs/ros_ws/src/common:rw
-      - ../../robot/ros_ws/src/coordination:/root/AirStack/gcs/ros_ws/src/coordination:ro
       - ../../common/fastdds.xml:/root/AirStack/gcs/ros_ws/src/fastdds.xml
       - ../ros_ws:/root/AirStack/gcs/ros_ws:rw
       # bags
diff --git a/robot/docker/.bashrc b/robot/docker/.bashrc
index 14d29b2a..e64f0910 100755
--- a/robot/docker/.bashrc
+++ b/robot/docker/.bashrc
@@ -135,10 +135,16 @@ if [ -n "$name_to_map" ]; then
     script_path="$HOME/AirStack/robot/docker/robot_name_map/resolve_robot_name.py"
     script_dir=$(dirname "$script_path")
 
+    existing_robot_name=${ROBOT_NAME:-}
     existing_robot_domain_id=${ROS_DOMAIN_ID:-}
 
     eval "$($script_path $name_to_map $script_dir/$ROBOT_NAME_MAP_CONFIG_FILE)"
-    export ROBOT_NAME
+    # if ROBOT_NAME was already set in the environment, use it instead of mapped value
+    if [ -n "$existing_robot_name" ]; then
+        export ROBOT_NAME=$existing_robot_name
+    else
+        export ROBOT_NAME
+    fi
 
     # if ROS_DOMAIN_ID was already set in the environment, use that instead of the mapped value
     if [ -z "$existing_robot_domain_id" ]; then
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index 0e46235a..68c6280f 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -65,6 +65,8 @@ services:
     deploy: !reset {}
     environment:
       - AUTONOMY_ROLE=full
+      - ROBOT_NAME=${ROBOT_NAME:-robot_1}
+      - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1}
       - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
       - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
       - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}

From bc67c9ae088020aa132a0832f574958d4b4149e7 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 23:10:16 -0400
Subject: [PATCH 33/55] syncing both computers

---
 gcs/docker/docker-compose.yaml                  | 1 +
 robot/docker/docker-compose.yaml                | 1 +
 simulation/isaac-sim/docker/docker-compose.yaml | 1 +
 3 files changed, 3 insertions(+)

diff --git a/gcs/docker/docker-compose.yaml b/gcs/docker/docker-compose.yaml
index 98e283b1..84ebea2d 100644
--- a/gcs/docker/docker-compose.yaml
+++ b/gcs/docker/docker-compose.yaml
@@ -41,4 +41,5 @@ services:
       - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
       - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
       - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
+      - ROS_DISCOVERY_SERVER=${DISCOVERY_SERVER_IP:-192.168.233.25}:${DISCOVERY_SERVER_PORT:-11811}
       - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index 68c6280f..2cdb9fab 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -71,6 +71,7 @@ services:
       - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
       - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
       - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
+      - ROS_DISCOVERY_SERVER=${DISCOVERY_SERVER_IP:-192.168.233.25}:${DISCOVERY_SERVER_PORT:-11811}
       - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
 
   # ===================================================================================================================
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index 8310800c..c86a1fb5 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -99,4 +99,5 @@ services:
       - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
       - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
       - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
+      - ROS_DISCOVERY_SERVER=${DISCOVERY_SERVER_IP:-192.168.233.25}:${DISCOVERY_SERVER_PORT:-11811}
       - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}

From 6a9db210aa53389df170188e58d88e958d095a49 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 23:31:02 -0400
Subject: [PATCH 34/55]  mimiced robot-l4t for dataflow

---
 gcs/docker/docker-compose.yaml                  | 7 -------
 gcs/docker/gcs-base-docker-compose.yaml         | 2 +-
 robot/docker/docker-compose.yaml                | 6 ------
 simulation/isaac-sim/docker/docker-compose.yaml | 7 -------
 4 files changed, 1 insertion(+), 21 deletions(-)

diff --git a/gcs/docker/docker-compose.yaml b/gcs/docker/docker-compose.yaml
index 84ebea2d..88f5c821 100644
--- a/gcs/docker/docker-compose.yaml
+++ b/gcs/docker/docker-compose.yaml
@@ -36,10 +36,3 @@ services:
     extends:
       file: ./docker-compose.yaml
       service: gcs-real
-    environment:
-      - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
-      - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
-      - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
-      - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
-      - ROS_DISCOVERY_SERVER=${DISCOVERY_SERVER_IP:-192.168.233.25}:${DISCOVERY_SERVER_PORT:-11811}
-      - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 2291f0d3..806b97c9 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -12,7 +12,7 @@ services:
       ssh service restart;
       python3 /root/AirStack/gcs/foxglove_extensions/install.sh;
       tmux new -d -s bringup;
-      if [ $$HITL_DISCOVERY_MODE = 'server' ]; then
+      if [ \"$${HITL_DISCOVERY_MODE:-}\" = 'server' ]; then
         if command -v fastdds >/dev/null 2>&1; then
           tmux new-window -t bringup -n dds_discovery;
           tmux send-keys -t bringup:dds_discovery 'fastdds discovery -i $${DISCOVERY_SERVER_ID:-0} -l 0.0.0.0 -p $${DISCOVERY_SERVER_PORT:-11811}' ENTER;
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index 2cdb9fab..7a3e554a 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -67,12 +67,6 @@ services:
       - AUTONOMY_ROLE=full
       - ROBOT_NAME=${ROBOT_NAME:-robot_1}
       - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1}
-      - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
-      - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
-      - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
-      - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
-      - ROS_DISCOVERY_SERVER=${DISCOVERY_SERVER_IP:-192.168.233.25}:${DISCOVERY_SERVER_PORT:-11811}
-      - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}
 
   # ===================================================================================================================
   # desktop_split: simulates onboard computer on desktop for debugging the split configuration.
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index c86a1fb5..39734fcc 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -94,10 +94,3 @@ services:
     container_name: isaac-sim
     network_mode: host
     networks: !reset null
-    environment:
-      - HITL_DISCOVERY_MODE=${HITL_DISCOVERY_MODE:-server}
-      - DISCOVERY_SERVER_IP=${DISCOVERY_SERVER_IP:-192.168.233.25}
-      - DISCOVERY_SERVER_PORT=${DISCOVERY_SERVER_PORT:-11811}
-      - DISCOVERY_SERVER_ID=${DISCOVERY_SERVER_ID:-0}
-      - ROS_DISCOVERY_SERVER=${DISCOVERY_SERVER_IP:-192.168.233.25}:${DISCOVERY_SERVER_PORT:-11811}
-      - FASTDDS_STATIC_PEER_IP=${FASTDDS_STATIC_PEER_IP:-192.168.233.25}

From e78f29b3ca91d7103a19098e9f924b3b771fdbbe Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 13 Apr 2026 23:50:08 -0400
Subject: [PATCH 35/55] fixed path to ddsrouter_yaml

---
 .../coordination_bringup/launch/gcs_gossip_bridge.launch.py | 6 ++++++
 .../launch/interpolate_dds_router.launch.py                 | 6 ++++++
 2 files changed, 12 insertions(+)

diff --git a/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py b/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
index 091a13f4..499a217b 100644
--- a/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
+++ b/common/ros_packages/coordination/coordination_bringup/launch/gcs_gossip_bridge.launch.py
@@ -15,6 +15,12 @@ def generate_launch_description():
     return LaunchDescription([
         ExecuteProcess(
             cmd=['ddsrouter', '-c', config],
+            env={
+                **os.environ,
+                # ddsrouter runtime libs are installed under /usr/local/lib.
+                # Scope this path to ddsrouter to avoid changing ROS 2 RMW resolution.
+                'LD_LIBRARY_PATH': '/usr/local/lib:' + os.environ.get('LD_LIBRARY_PATH', ''),
+            },
             output='screen',
             name='gcs_gossip_dds_router',
         ),
diff --git a/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py b/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py
index 0f235c55..13885e1a 100644
--- a/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py
+++ b/robot/ros_ws/src/autonomy_bringup/launch/interpolate_dds_router.launch.py
@@ -250,6 +250,12 @@ def replace_var(match):
         LogInfo(msg=f"[dds_router] Final interpolated config:\n{content}"),
         ExecuteProcess(
             cmd=['ddsrouter', '-c', tmp.name],
+            env={
+                **os.environ,
+                # ddsrouter is installed under /usr/local and needs its runtime libs.
+                # Scope this path to ddsrouter only to avoid perturbing ROS 2 RMW loading.
+                'LD_LIBRARY_PATH': '/usr/local/lib:' + os.environ.get('LD_LIBRARY_PATH', ''),
+            },
             output='screen',
         ),
     ]

From 8f06837e4d5fd87c1d00599a4e46149f3710f85e Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 14 Apr 2026 15:19:45 -0400
Subject: [PATCH 36/55] fixed dds server

---
 robot/docker/docker-compose.yaml                      | 3 +++
 simulation/isaac-sim/launch_scripts/two_drone_fire.py | 6 ++++++
 2 files changed, 9 insertions(+)

diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index 7a3e554a..fc0bf1e2 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -67,6 +67,9 @@ services:
       - AUTONOMY_ROLE=full
       - ROBOT_NAME=${ROBOT_NAME:-robot_1}
       - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1}
+      # HITL uses host networking; default bridge IP (172.31.0.200) is not reachable here.
+      # Point SIM_IP at the host running Isaac Sim (override in .env as needed).
+      - SIM_IP=${SIM_IP:-192.168.233.25}
 
   # ===================================================================================================================
   # desktop_split: simulates onboard computer on desktop for debugging the split configuration.
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 1af07ed8..a22a4071 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -178,6 +178,12 @@ def __init__(self):
                 robot_name=f"robot_{i}",
                 vehicle_id=i,
                 domain_id=i,
+                # Match robot interface.launch.py defaults:
+                #   FCU_URL = udp://:<14540+domain>@<SIM_IP>:<14580+domain>
+                # so each vehicle listens on 14580 + vehicle_id.
+                connection_type="udpin",
+                connection_ip="0.0.0.0",
+                connection_baseport=14580,
                 usd_file=DRONE_USD,
                 init_pos=pos,
                 init_orient=cfg["orient"],

From c654bc317642e2894c5cd92cd53c838914fe1f95 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 14 Apr 2026 17:01:46 -0400
Subject: [PATCH 37/55] fixed two_drone_fire

---
 simulation/isaac-sim/launch_scripts/two_drone_fire.py | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index a22a4071..1af07ed8 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -178,12 +178,6 @@ def __init__(self):
                 robot_name=f"robot_{i}",
                 vehicle_id=i,
                 domain_id=i,
-                # Match robot interface.launch.py defaults:
-                #   FCU_URL = udp://:<14540+domain>@<SIM_IP>:<14580+domain>
-                # so each vehicle listens on 14580 + vehicle_id.
-                connection_type="udpin",
-                connection_ip="0.0.0.0",
-                connection_baseport=14580,
                 usd_file=DRONE_USD,
                 init_pos=pos,
                 init_orient=cfg["orient"],

From 66cf6dc74712a74497ebd703c548d441401b758e Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Wed, 15 Apr 2026 15:35:01 -0400
Subject: [PATCH 38/55] rayfronts is now a ros package

---
 .gitmodules                                   |  4 ++
 common/rayfronts                              |  1 +
 .../dataset/ros2isaacsim.yaml                 | 36 +++++++++++
 common/rayfronts_configs/low_memory.yaml      | 37 +++++++++++
 robot/docker/Dockerfile.robot                 | 63 ++++++++++++++++---
 robot/docker/docker-compose.yaml              |  6 +-
 .../launch/rayfronts.launch.xml               | 20 ++++++
 7 files changed, 158 insertions(+), 9 deletions(-)
 create mode 160000 common/rayfronts
 create mode 100644 common/rayfronts_configs/dataset/ros2isaacsim.yaml
 create mode 100644 common/rayfronts_configs/low_memory.yaml
 create mode 100644 robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml

diff --git a/.gitmodules b/.gitmodules
index 0b29f348..aadea5e4 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -14,3 +14,7 @@
 	path = simulation/isaac-sim/extensions/PegasusSimulator
 	url = https://github.com/castacks/PegasusSimulator-AirStack-Integration.git
 
+[submodule "common/rayfronts"]
+	path = common/rayfronts
+	url = https://github.com/RayFronts/RayFronts.git
+	branch = modalai
diff --git a/common/rayfronts b/common/rayfronts
new file mode 160000
index 00000000..b2bbdfde
--- /dev/null
+++ b/common/rayfronts
@@ -0,0 +1 @@
+Subproject commit b2bbdfde73e101bc1243da81ec23a5b3812b31e7
diff --git a/common/rayfronts_configs/dataset/ros2isaacsim.yaml b/common/rayfronts_configs/dataset/ros2isaacsim.yaml
new file mode 100644
index 00000000..ee7b1dbd
--- /dev/null
+++ b/common/rayfronts_configs/dataset/ros2isaacsim.yaml
@@ -0,0 +1,36 @@
+defaults:
+  - base_dataset
+
+_target_: rayfronts.datasets.Ros2Subscriber
+
+# Topic containing RGB images of type sensor_msgs/msg/Image
+rgb_topic: "/robot_1/sensors/front_stereo/left/image_rect"
+
+# Topic containing odometry of type nav_msgs/msg/Odometry
+pose_topic: "/robot_1/odometry_conversion/odometry"
+
+# Message type for the pose topic: "PoseStamped" or "Odometry"
+pose_msg_type: "Odometry"
+
+# Topic containing depth images of type .
+depth_topic: "/robot_1/sensors/front_stereo/left/depth_ground_truth"
+
+# Topic containing intrinsics information from messages
+# of type sensor_msgs/msg/CameraInfo. Will be used at initialization only.
+intrinsics_topic: "/robot_1/sensors/front_stereo/left/camera_info"
+
+# Path to json file containing intrinsics with the following keys
+# fx, fy, cx, cy, w, h. This will be prioritized over the intrinsics topic.
+intrinsics_file: null
+
+# (Optional) Topic containing confidence in depth values.
+# Message type: sensor_msgs/msg/Image.
+confidence_topic: null
+
+# Topic containing point cloud of type sensor_msgs/msg/PointCloud.
+# this is an alternative inefficient depth source to disparity.
+point_cloud_topic: null
+
+# A string of 3 letters describing the camera coordinate system in
+# r/l u/d f/b in any order. (e.g, rdf, flu, rfu)
+src_coord_system: "flu"
\ No newline at end of file
diff --git a/common/rayfronts_configs/low_memory.yaml b/common/rayfronts_configs/low_memory.yaml
new file mode 100644
index 00000000..8644e829
--- /dev/null
+++ b/common/rayfronts_configs/low_memory.yaml
@@ -0,0 +1,37 @@
+defaults:
+  - default
+  - override mapping: semantic_ray_frontiers_map
+  - override encoder: naradio
+  - override vis: ros
+  - override messaging_service: ros
+  - _self_
+
+compile: False
+amp: True
+depth_limit: 20.0
+
+encoder:
+  model_version: radio_v2.5-b
+  lang_model: siglip
+
+dataset:
+  rgb_resolution: [448, 448] #[480, 304]
+  depth_resolution: [448, 448] #[480, 304]
+  frame_skip: 5
+
+mapping:
+  vox_size: 0.50
+  #max_pts_per_frame: 300 #-1
+  #max_empty_pts_per_frame: 20000
+  max_rays_per_frame: 50 #500
+  #fronti_subsampling: 8
+  #fronti_subsampling_min_fronti: 20
+  #ray_tracing: False
+  vox_accum_period: 2 #4
+  ray_accum_period: 4 #4
+  ray_accum_phase: 2 #2
+  #ray_erosion: 0
+  #global_encoding: True
+
+
+
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index 992442ce..864cae50 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -1,5 +1,7 @@
 # either ubuntu:24.04 or l4t. ubuntu:24.04 is default
 ARG BASE_IMAGE
+ARG ENABLE_RAYFRONTS=false
+ARG FINAL_STAGE=runtime
 # ============================================================
 # Stage 1 — builder: compile/download everything
 # ============================================================
@@ -12,6 +14,7 @@ ARG UPDATE_FLAGS="-o Acquire::AllowInsecureRepositories=true -o Acquire::AllowDo
 ARG INSTALL_FLAGS="-o APT::Get::AllowUnauthenticated=true"
 ARG SKIP_MACVO=false
 ARG SKIP_TENSORRT=false
+ARG ENABLE_RAYFRONTS
 
 # from https://github.com/athackst/dockerfiles/blob/main/ros2/jazzy.Dockerfile
 ENV DEBIAN_FRONTEND=noninteractive
@@ -116,7 +119,7 @@ RUN if echo "$BASE_IMAGE" | grep -qE "(nvidia|l4t)" && [ "${SKIP_TENSORRT}" != "
 # Note: numpy>=1.26 required for Python 3.12 compatibility
 # Using --ignore-installed to avoid conflicts with system packages
 RUN pip3 install --break-system-packages --ignore-installed \
-  empy \
+  empy==3.3.4 \
   future \
   lxml \
   matplotlib==3.8.4 \
@@ -155,11 +158,15 @@ RUN pip3 install --break-system-packages --ignore-installed \
 
 # Install MACVO Python dependencies (skipped if SKIP_MACVO=true)
 RUN if [ "${SKIP_MACVO}" != "true" ]; then \
-  pip3 install --break-system-packages \
-  torch \
-  torchvision \
-  onnx \
-  tensorrt; \
+    if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
+      pip3 install --break-system-packages \
+        --index-url https://download.pytorch.org/whl/cu130 \
+        torch==2.9.1 torchvision torchaudio && \
+      pip3 install --break-system-packages onnx tensorrt; \
+    else \
+      pip3 install --break-system-packages \
+        torch torchvision onnx tensorrt; \
+    fi; \
   fi
 
 # Downloading model weights for MACVO (skipped if SKIP_MACVO=true)
@@ -175,8 +182,7 @@ RUN if [ "${SKIP_MACVO}" != "true" ]; then \
 
 # Fixes for MACVO Integration (skipped if SKIP_MACVO=true)
 RUN if [ "${SKIP_MACVO}" != "true" ]; then \
-  pip install --break-system-packages huggingface_hub && \
-  pip uninstall --break-system-packages matplotlib -y; \
+  pip install --break-system-packages huggingface_hub; \
   fi
 
 # TMux config
@@ -196,6 +202,36 @@ RUN mkdir -p /tmp/DDS-Router/src \
   && colcon build --merge-install --install-base /usr/local \
   && rm -rf /tmp/DDS-Router
 
+# RayFronts deps (builder-stage, gated on ENABLE_RAYFRONTS=true)
+RUN if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
+    pip3 install --break-system-packages \
+      hydra-core open_clip_torch "transformers<5" \
+      git+https://github.com/facebookresearch/segment-anything.git \
+      ftfy regex nanobind pandas protobuf \
+      "scipy==1.15.2" "scikit-image" "numpy<2" && \
+    pip3 install --break-system-packages \
+      torch-scatter==2.1.2 && \
+    pip3 install --break-system-packages --force-reinstall --no-deps \
+      setuptools==79.0.1; \
+  fi
+
+# Patched OpenVDB (OasisArtisan fork) — exposes Int8Grid to Python bindings
+RUN if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
+    apt-get ${UPDATE_FLAGS} update && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends \
+      libboost-iostreams-dev libtbb-dev libblosc-dev python3-dev && \
+    git clone --depth 1 https://github.com/OasisArtisan/openvdb /tmp/openvdb && \
+    cmake -S /tmp/openvdb -B /tmp/openvdb/build \
+      -DCMAKE_INSTALL_PREFIX=/usr/local \
+      -DOPENVDB_BUILD_PYTHON_MODULE=ON \
+      -DOPENVDB_PYTHON_WRAP_ALL_GRID_TYPES=ON \
+      -DUSE_NUMPY=ON \
+      -Dnanobind_DIR=$(python3 -c "import nanobind,os;print(os.path.join(os.path.dirname(nanobind.__file__),'cmake'))") && \
+    cmake --build /tmp/openvdb/build -j4 && \
+    cmake --install /tmp/openvdb/build && \
+    rm -rf /tmp/openvdb && \
+    rm -rf /var/lib/apt/lists/*; \
+  fi
+
 # Cleanup
 RUN apt autoremove -y \
   && apt clean -y \
@@ -351,3 +387,14 @@ EXPOSE 22
 
 WORKDIR /root/AirStack/robot/ros_ws
 
+
+# runtime + compiled RayFronts (only reached when FINAL_STAGE=runtime-rayfronts)
+FROM runtime AS runtime-rayfronts
+COPY ./common/rayfronts          /opt/rayfronts
+COPY ./common/rayfronts_configs/ /opt/rayfronts/rayfronts/configs/
+RUN apt-get update && apt-get install -y --no-install-recommends \
+      cmake build-essential python3-dev \
+ && cd /opt/rayfronts && CMAKE_INSTALL_PREFIX=/usr/local ./compile.sh \
+ && rm -rf /var/lib/apt/lists/*
+
+FROM ${FINAL_STAGE} AS final
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index fc0bf1e2..487e061e 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -11,9 +11,13 @@ services:
       service: robot_base
     image: &desktop_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_robot-x86-64_${DOCKER_IMAGE_BUILD_MODE}
     build:
-      dockerfile: ./Dockerfile.robot
+      # context at repo root so runtime-rayfronts stage can COPY ./common/rayfronts
+      context: ../../
+      dockerfile: robot/docker/Dockerfile.robot
       args:
         BASE_IMAGE: nvidia/cuda:13.0.2-base-ubuntu24.04
+        ENABLE_RAYFRONTS: "true"
+        FINAL_STAGE: runtime-rayfronts
       tags:
         - *desktop_image
     # we use tmux sd-keys so that the session stays alive
diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
new file mode 100644
index 00000000..1492161b
--- /dev/null
+++ b/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
@@ -0,0 +1,20 @@
+<launch>
+  <arg name="config_name" default="low_memory"/>
+  <arg name="dataset"     default="ros2isaacsim"/>
+  <arg name="queries_file" default="/opt/rayfronts/queries.txt"/>
+  <arg name="query_period" default="10"/>
+
+  <executable
+    cmd="python3 -m rayfronts.mapping_server
+         --config-name $(var config_name)
+         dataset=$(var dataset)
+         messaging_service=ros
+         querying.text_query_mode=prompts
+         querying.query_file=$(var queries_file)
+         querying.compute_prob=True
+         querying.period=$(var query_period)"
+    cwd="/opt/rayfronts"
+    output="screen">
+    <env name="HYDRA_FULL_ERROR" value="1"/>
+  </executable>
+</launch>

From 715938382ac005f7641222ab36f7cc1e09d83aab Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 16 Apr 2026 20:23:12 -0400
Subject: [PATCH 39/55] added feedback, it's sending success too early though

---
 .env                                          |   2 +-
 .../desktop_bringup/rviz/robot.rviz           | 147 +++++-
 .../rviz/rviz_tasks_panel/src/tasks_panel.cpp |   8 +-
 .../global_bringup/launch/global.launch.xml   |  11 +-
 .../raven_nav/raven_nav/behavior_manager.py   |  37 +-
 .../raven_nav/behaviors/voxel_behavior.py     | 254 ++++++++-
 .../raven_nav/raven_nav/raven_nav_node.py     | 162 +++---
 .../launch/semantic_search_task.launch.xml    |   9 +
 .../planners/semantic_search_task/package.xml |  26 +
 .../resource/semantic_search_task             |   0
 .../semantic_search_task/__init__.py          |   0
 .../semantic_search_task/node.py              | 489 ++++++++++++++++++
 .../planners/semantic_search_task/setup.cfg   |   4 +
 .../planners/semantic_search_task/setup.py    |  26 +
 .../launch/rayfronts.launch.xml               |   3 +-
 .../launch_scripts/two_drone_fire.py          |   2 +-
 16 files changed, 1049 insertions(+), 131 deletions(-)
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/package.xml
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/resource/semantic_search_task
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/__init__.py
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg
 create mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/setup.py

diff --git a/.env b/.env
index 9de69a90..a208d5f1 100644
--- a/.env
+++ b/.env
@@ -24,7 +24,7 @@ COMPOSE_PROFILES="desktop, isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="2"  # Number of robot containers to launch.
+NUM_ROBOTS="1"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== SIMULATION =====================
diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz
index 17abc5bf..00e2f38e 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz
@@ -14,7 +14,7 @@ Panels:
         - /Local1/DROAN1/Global Plan Vis1
         - /Global1/Global Plan1
       Splitter Ratio: 0.5792563557624817
-    Tree Height: 693
+    Tree Height: 622
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -30,10 +30,10 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Foreground Background Cloud
+    SyncSource: StereoImageProc Disparity
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
-    topic: /robot_1/behavior/behavior_tree_graphviz
+    topic: /robot_1/behavior_tree_graphviz
     zoom_factor: 0.1919851303100586
   - Class: rviz_tasks_panel::TasksPanel
     Name: TasksPanel
@@ -44,7 +44,7 @@ Panels:
     executor_4: ""
     executor_5: ""
     executor_6: ""
-    executor_7: ""
+    executor_7: /robot_1/tasks/semantic_search
     executor_8: /robot_1/tasks/fixed_trajectory
     robot: /robot_1
 Visualization Manager:
@@ -472,7 +472,7 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: macvo/odometry
           Value: true
-      Enabled: true
+      Enabled: false
       Name: Perception
     - Class: rviz_common/Group
       Displays:
@@ -482,8 +482,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
@@ -666,8 +665,7 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                global_plan: true
-                global_plan_extra_text: true
+                {}
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -731,10 +729,10 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Marker
-          Enabled: true
+          Enabled: false
           Name: VDB Mapping Marker
           Namespaces:
-            "": true
+            {}
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -742,7 +740,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: vdb_mapping/vdb_map_visualization
-          Value: true
+          Value: false
         - Alpha: 0.800000011920929
           Buffer Length: 1
           Class: rviz_default_plugins/Path
@@ -773,6 +771,117 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Global
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: rays msg_serv
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/msg_serv/rays_sim/q0_red_building
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: filtered rays
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/filtered_rays
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: voxel msg_serv
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/msg_serv/voxels_sim/q0_red_building
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: voxel_rgb
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Boxes
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/voxel_rgb
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -836,16 +945,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 37.564666748046875
+      Distance: 53.187862396240234
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 25.773038864135742
-        Y: -5.22532320022583
-        Z: -18.542400360107422
+        X: 25.142688751220703
+        Y: -7.090958595275879
+        Z: -18.70828628540039
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -865,12 +974,12 @@ Window Geometry:
     collapsed: false
   Front Right RGB:
     collapsed: false
-  Height: 1376
+  Height: 1305
   Hide Left Dock: false
   Hide Right Dock: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 000000ff00000000fd000000040000000000000201000002a9fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000078000002a9000000c700fffffffb0000001e0057006100790070006f0069006e007400200050006c007500670069006e0100000205000001630000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002cb0000009d0000000000000000fb00000028004d004100430056004f00200049006d00610067006500200046006500610074007500720065007300000002ba000000ca00000000000000000000000100000109000002a9fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000780000007e0000005c00fffffffb0000000a0056006900650077007301000000fc00000225000000a000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000028b000000dd0000000000000000fb0000001e004d004100430056004f0020004400690073007000610072006900740079000000028d000001410000001600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000009ba00000037fc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba000001d7fc0100000008fb00000014005400610073006b007300500061006e0065006c010000000000000489000002a400fffffffb0000001e00460072006f006e00740020005200690067006800740020005200470042010000048f000002a10000009400fffffffb0000002200460072006f006e00740020005200690067006800740020004400650070007400680100000736000002840000009e00fffffffb0000002200460072006f006e007400200052006900670068007400200044006500700074006801000006f6000002c40000000000000000fb0000002200460072006f006e00740020005200690067006800740020004400650070007400680100000714000002a60000000000000000fb0000002200460072006f006e007400200052006900670068007400200044006500700074006801000006e2000002d80000000000000000fb00000022004200650068006100760069006f0072005400720065006500500061006e0065006c00000006dc000002de0000023300fffffffb0000000800540069006d00650100000000000004500000000000000000000006a4000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   TasksPanel:
@@ -883,4 +992,4 @@ Window Geometry:
     collapsed: false
   Width: 2490
   X: 1990
-  Y: 27
+  Y: 32
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
index aed06bc4..632ba8a0 100644
--- a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
@@ -1016,13 +1016,7 @@ void TasksPanel::onExecuteClicked()
       goal.confidence_threshold = getFloat(7, "confidence_threshold");
       doSendGoal<task_msgs::action::SemanticSearchTask>(7, goal,
         [](const auto & fb) {
-          return QString("status: %1 | progress: %2 | best_conf: %3 | pos: (%4, %5, %6)")
-            .arg(QString::fromStdString(fb.status))
-            .arg(fb.progress, 0, 'f', 2)
-            .arg(fb.best_confidence_so_far, 0, 'f', 3)
-            .arg(fb.current_position.x, 0, 'f', 1)
-            .arg(fb.current_position.y, 0, 'f', 1)
-            .arg(fb.current_position.z, 0, 'f', 1);
+          return QString::fromStdString(fb.status);
         },
         [](const auto & r) {
           return QString("success: %1\nmessage: %2\nconfidence: %3")
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 490570ff..628af479 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -6,15 +6,6 @@
     </include>
 
     <!-- Planning -->
-    <node pkg="random_walk_planner" exec="random_walk_planner" name="random_walk_planner"
-        output="screen">
-        <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
-        <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
-        <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
-        <remap from="~/exploration_task" to="/$(env ROBOT_NAME)/tasks/exploration" />
-        <remap from="navigate_task" to="/$(env ROBOT_NAME)/tasks/navigate" />
-        <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
-        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
-    </node>
+    <include file="$(find-pkg-share semantic_search_task)/launch/semantic_search_task.launch.xml" />
     
 </launch>
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
index 38ec3606..1493a241 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
@@ -1,5 +1,6 @@
 from raven_nav.behaviors.frontier_behavior import FrontierBehavior
 from raven_nav.behaviors.ray_behavior import RayBehavior
+from raven_nav.behaviors.voxel_behavior import VoxelBehavior
 
 
 class BehaviorManager:
@@ -7,15 +8,25 @@ def __init__(self, get_clock, publisher_dict, score_threshold=0.95):
         self.behavior_mode = 'Frontier-based'
         self.get_clock = get_clock
         self.frontier_behavior = FrontierBehavior(self.get_clock)
-        current_target_publisher = publisher_dict.get('current_target')
-        self.ray_behavior = RayBehavior(self.get_clock, current_target_publisher,
+        current_target_pub = publisher_dict.get('current_target')
+        self.ray_behavior = RayBehavior(self.get_clock, current_target_pub,
                                         score_threshold=score_threshold)
-        self.behaviors = [self.ray_behavior, self.frontier_behavior]
+        self.voxel_behavior = VoxelBehavior(self.get_clock)
+        # Priority: Voxel (navigate to confirmed detections) > Ray > Frontier
+        self.behaviors = [self.voxel_behavior, self.ray_behavior, self.frontier_behavior]
+
+    @property
+    def completed_queries(self) -> set:
+        return self.voxel_behavior.completed_queries
 
     def mode_select(self, ray_origins, ray_dirs, ray_scores,
-                    query_labels, target_objects):
+                    query_labels, target_objects,
+                    vox_xyz=None, vox_scores=None):
         for behavior in self.behaviors:
-            if behavior.name == 'Ray-based':
+            if behavior.name == 'Voxel-based':
+                # Voxel-based navigation disabled — skip
+                continue
+            elif behavior.name == 'Ray-based':
                 if behavior.condition_check(ray_origins, ray_dirs, ray_scores,
                                             query_labels, target_objects):
                     self.behavior_mode = behavior.name
@@ -27,13 +38,17 @@ def mode_select(self, ray_origins, ray_dirs, ray_scores,
 
     def behavior_execute(self, behavior_mode, frontiers, cur_pose_np,
                          waypoint_locked, target_waypoint, target_waypoint2,
-                         publisher_dict):
-        if behavior_mode == 'Frontier-based':
-            return self.frontier_behavior.execute(
-                frontiers, cur_pose_np, waypoint_locked,
-                target_waypoint, target_waypoint2, publisher_dict)
+                         publisher_dict,
+                         vox_xyz=None, vox_scores=None, query_labels=None):
+        if behavior_mode == 'Voxel-based':
+            return self.voxel_behavior.execute(
+                vox_xyz, vox_scores, query_labels, cur_pose_np,
+                waypoint_locked, target_waypoint, target_waypoint2, publisher_dict)
         elif behavior_mode == 'Ray-based':
             return self.ray_behavior.execute(
                 cur_pose_np, waypoint_locked,
                 target_waypoint, target_waypoint2, publisher_dict)
-        return waypoint_locked, target_waypoint, target_waypoint2
+        else:
+            return self.frontier_behavior.execute(
+                frontiers, cur_pose_np, waypoint_locked,
+                target_waypoint, target_waypoint2, publisher_dict)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
index 9b090754..ec21ab00 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
@@ -1,15 +1,239 @@
-# Voxel-based behavior - placeholder for future implementation.
-# Enable once voxel behavior strategy is decided.
-
-# class VoxelBehavior:
-#     def __init__(self, get_clock):
-#         self.get_clock = get_clock
-#         self.name = 'Voxel-based'
-#
-#     def condition_check(self, voxels, voxel_scores, target_labels, target_objects):
-#         return False
-#
-#     def execute(self, voxels, voxel_scores, target_labels, target_objects,
-#                 cur_pose_np, waypoint_locked, target_waypoint, target_waypoint2,
-#                 publisher_dict):
-#         return waypoint_locked, target_waypoint, target_waypoint2
+import time
+
+import numpy as np
+import scipy.ndimage
+from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped
+from visualization_msgs.msg import Marker, MarkerArray
+from std_msgs.msg import ColorRGBA
+
+
+class VoxelBehavior:
+    def __init__(self, get_clock):
+        self.get_clock = get_clock
+        self.name = 'Voxel-based'
+        # {cluster_id: [cx, cy, cz, sx, sy, sz]}
+        self.target_voxel_clusters = {}
+        # {cluster_id: query_label} — which query each cluster belongs to
+        self.cluster_query_map = {}
+        self.visited_clusters = []    # list of [cx,cy,cz,sx,sy,sz]
+        self.unvisited_clusters = []  # list of (cluster_id, [cx,cy,cz,sx,sy,sz])
+        self.completed_queries = set()
+        self.prev_voxel_cluster_ids = 0
+
+    def condition_check(self, vox_xyz, vox_scores, query_labels, target_objects):
+        """
+        vox_xyz:      np.ndarray (N, 3) — voxel world positions
+        vox_scores:   np.ndarray (N, Q) — softmax similarity per query
+        query_labels: list[str]         — ordered label for each sim column
+        target_objects: list[str]       — which labels we're searching for
+        """
+        if vox_xyz is None or vox_scores is None or not target_objects:
+            return False
+        if len(vox_xyz) == 0:
+            return False
+
+        label_indices = [
+            query_labels.index(t) for t in target_objects
+            if t in query_labels
+        ]
+        if not label_indices:
+            return False
+
+        threshold = 0.98
+        relevant_scores = vox_scores[:, label_indices]   # (N, len(label_indices))
+        mask = (relevant_scores > threshold).any(axis=1)
+        indices = np.where(mask)[0]
+
+        if len(indices) == 0:
+            return False
+
+        # Cluster the high-confidence voxels with connected-component labeling
+        filtered_vox = np.round(vox_xyz[indices], 3)
+        vox_size = 0.5
+
+        min_coords = filtered_vox.min(axis=0)
+        norm_coords = ((filtered_vox - min_coords) / vox_size).astype(int)
+        max_coords = norm_coords.max(axis=0) + 1
+        occupancy = np.zeros(tuple(max_coords.tolist()), dtype=np.uint8)
+        for x, y, z in norm_coords:
+            occupancy[x, y, z] = 1
+
+        structure = np.ones((3, 3, 3), dtype=np.uint8)
+        labeled, num_components = scipy.ndimage.label(occupancy, structure=structure)
+
+        label_ids = np.array([labeled[x, y, z] for x, y, z in norm_coords])
+
+        self.target_voxel_clusters.clear()
+        self.cluster_query_map.clear()
+        vox_cluster_count = 0
+
+        for label_val in range(1, num_components + 1):
+            idx = np.where(label_ids == label_val)[0]
+            if len(idx) < 30:
+                continue
+
+            coords = norm_coords[idx]
+            min_voxel = coords.min(axis=0)
+            max_voxel = coords.max(axis=0)
+            min_world = min_voxel * vox_size + min_coords
+            max_world = (max_voxel + 1) * vox_size + min_coords
+            center = (min_world + max_world) / 2
+            size = max_world - min_world
+
+            # Data is already in FLU (raven_nav converts before passing)
+            cx, cy, cz = center[0], center[1], center[2]
+            sx, sy, sz = size[0], size[1], size[2]
+
+            # Tag cluster with best-scoring query label for this cluster
+            cluster_scores = relevant_scores[idx]  # (cluster_size, len(label_indices))
+            best_local = int(cluster_scores.mean(axis=0).argmax())
+            best_label = target_objects[best_local] if best_local < len(target_objects) else target_objects[0]
+
+            self.target_voxel_clusters[vox_cluster_count] = [cx, cy, cz, sx, sy, sz]
+            self.cluster_query_map[vox_cluster_count] = best_label
+            vox_cluster_count += 1
+
+        self.unvisited_clusters = [
+            (idx, cluster) for idx, cluster in self.target_voxel_clusters.items()
+            if not self._is_near_visited(
+                np.array(cluster[:3]), np.array(cluster[3:6]), self.visited_clusters)
+        ]
+
+        return len(self.unvisited_clusters) > 0
+
+    def execute(self, vox_xyz, vox_scores, query_labels, cur_pose_np,
+                waypoint_locked, target_waypoint, target_waypoint2, publisher_dict):
+        voxel_bbox_pub = publisher_dict.get('voxel_bbox')
+        if voxel_bbox_pub:
+            self._visualize_clusters(voxel_bbox_pub)
+
+        path_pub = publisher_dict['path']
+
+        self.unvisited_clusters = [
+            (idx, cluster) for idx, cluster in self.target_voxel_clusters.items()
+            if not self._is_near_visited(
+                np.array(cluster[:3]), np.array(cluster[3:6]), self.visited_clusters)
+        ]
+
+        sorted_clusters = sorted(
+            self.unvisited_clusters,
+            key=lambda item: np.linalg.norm(cur_pose_np - np.array(item[1][:3])))
+
+        path = Path()
+        path.header.stamp = self.get_clock().now().to_msg()
+        path.header.frame_id = 'map'
+
+        # Track whether the waypoint was already locked before this tick.
+        # Arrival detection must NOT fire in the same tick the waypoint is first set.
+        waypoint_was_locked = waypoint_locked
+
+        for i, (idx, cluster) in enumerate(sorted_clusters):
+            center = np.array(cluster[:3])
+            sizes = np.array(cluster[3:])
+            half_sizes = sizes / 2.0
+            direction = center - cur_pose_np
+            dist = np.linalg.norm(direction)
+            if dist < 1e-6:
+                continue
+            dir_norm = direction / dist
+
+            # Find surface point facing the drone
+            ray_origin_local = cur_pose_np - center
+            tmin, tmax = -np.inf, np.inf
+            for axis in range(3):
+                if abs(dir_norm[axis]) > 1e-9:
+                    t1 = (-half_sizes[axis] - ray_origin_local[axis]) / dir_norm[axis]
+                    t2 = (half_sizes[axis] - ray_origin_local[axis]) / dir_norm[axis]
+                    tmin = max(tmin, min(t1, t2))
+                    tmax = min(tmax, max(t1, t2))
+            if tmax < max(tmin, 0):
+                continue
+            t_hit = tmin if tmin > 0 else tmax
+            surface_point = cur_pose_np + dir_norm * t_hit
+            adjacent = surface_point - dir_norm * 1.0   # 1m in front of surface
+
+            if i == 0:
+                if not waypoint_locked:
+                    target_waypoint2 = adjacent
+                    waypoint_locked = True
+
+                alpha = 0.8
+                mid = cur_pose_np * (1 - alpha) + target_waypoint2 * alpha
+                target_waypoint = mid
+
+                for wp_np in [mid, target_waypoint2]:
+                    ps = PoseStamped()
+                    ps.header.stamp = self.get_clock().now().to_msg()
+                    ps.header.frame_id = 'map'
+                    ps.pose.position.x = float(wp_np[0])
+                    ps.pose.position.y = float(wp_np[1])
+                    ps.pose.position.z = float(wp_np[2])
+                    ps.pose.orientation.w = 1.0
+                    path.poses.append(ps)
+
+        path_pub.publish(path)
+
+        # Mark cluster as visited only when the drone has been navigating toward a
+        # previously-locked waypoint (waypoint_was_locked) AND arrives within 3m.
+        # Skipping the check on the tick the waypoint was first set prevents
+        # immediate false-positive "arrived" detections.
+        if waypoint_was_locked and target_waypoint2 is not None and \
+                np.linalg.norm(cur_pose_np - target_waypoint2) < 3.0:
+            if sorted_clusters:
+                arrived_idx, arrived_cluster = sorted_clusters[0]
+                self.visited_clusters.append(arrived_cluster)
+                completed_label = self.cluster_query_map.get(arrived_idx)
+                if completed_label:
+                    self.completed_queries.add(completed_label)
+            waypoint_locked = False
+
+        return waypoint_locked, target_waypoint, target_waypoint2
+
+    # ── helpers ──────────────────────────────────────────────────────────────
+
+    def _is_near_visited(self, center, size, visited_clusters, threshold=10.0):
+        return any(
+            self._cuboid_distance(center, size,
+                                  np.array(v[:3]), np.array(v[3:6])) < threshold
+            for v in visited_clusters)
+
+    def _cuboid_distance(self, ca, sa, cb, sb):
+        ha, hb = sa / 2.0, sb / 2.0
+        dx = max(abs(ca[0] - cb[0]) - (ha[0] + hb[0]), 0)
+        dy = max(abs(ca[1] - cb[1]) - (ha[1] + hb[1]), 0)
+        dz = max(abs(ca[2] - cb[2]) - (ha[2] + hb[2]), 0)
+        return np.sqrt(dx**2 + dy**2 + dz**2)
+
+    def _visualize_clusters(self, pub):
+        markers = MarkerArray()
+        now = self.get_clock().now().to_msg()
+        # Delete old markers
+        for i in range(self.prev_voxel_cluster_ids):
+            m = Marker()
+            m.header.frame_id = 'map'
+            m.header.stamp = now
+            m.ns = 'voxel_clusters'
+            m.id = i
+            m.action = Marker.DELETE
+            markers.markers.append(m)
+        # Add new markers
+        for j, (_, cluster) in enumerate(self.unvisited_clusters):
+            m = Marker()
+            m.header.frame_id = 'map'
+            m.header.stamp = now
+            m.ns = 'voxel_clusters'
+            m.id = j
+            m.type = Marker.CUBE
+            m.action = Marker.ADD
+            m.pose.position.x = cluster[0]
+            m.pose.position.y = cluster[1]
+            m.pose.position.z = cluster[2]
+            m.scale.x = cluster[3]
+            m.scale.y = cluster[4]
+            m.scale.z = cluster[5]
+            m.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.2)
+            m.lifetime.sec = 1
+            markers.markers.append(m)
+        self.prev_voxel_cluster_ids = len(self.unvisited_clusters)
+        pub.publish(markers)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
index 1e14e2ee..d3c75a0e 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
@@ -1,3 +1,4 @@
+import json
 import os
 import numpy as np
 
@@ -19,12 +20,16 @@ def __init__(self):
         robot_name = os.getenv('ROBOT_NAME', 'robot')
         self._prefix = f'/{robot_name}'
 
-        # Ray data parsed from rays_sim/all (all in FLU world frame after conversion)
-        self._ray_origins = None    # (N, 3) FLU
-        self._ray_dirs = None       # (N, 3) FLU
-        self._ray_scores = None     # (N, M) softmax scores
+        # Ray data from rays_sim/all (converted to FLU world frame)
+        self._ray_origins = None    # (N, 3)
+        self._ray_dirs = None       # (N, 3)
+        self._ray_scores = None     # (N, Q)
 
-        self._frontiers = None      # (N, 3) FLU — published already in FLU
+        # Voxel data from voxels_sim/all
+        self._vox_xyz = None        # (N, 3)
+        self._vox_scores = None     # (N, Q)
+
+        self._frontiers = None      # (N, 3)
         self._cur_pose = None       # (3,)
 
         self._waypoint_locked = False
@@ -49,12 +54,17 @@ def __init__(self):
             PointCloud2, f'{self._prefix}/frontier_viewpoints', 10)
         self._current_target_pub = self.create_publisher(
             String, f'{self._prefix}/current_target', 10)
+        self._voxel_bbox_pub = self.create_publisher(
+            MarkerArray, f'{self._prefix}/voxel_clusters', 10)
+        self._completed_targets_pub = self.create_publisher(
+            String, f'{self._prefix}/completed_targets', 10)
 
         self._publisher_dict = {
             'path': self._path_pub,
             'filtered_rays': self._filtered_rays_pub,
             'viewpoint': self._viewpoint_pub,
             'current_target': self._current_target_pub,
+            'voxel_bbox': self._voxel_bbox_pub,
         }
 
         self._behavior_manager = BehaviorManager(
@@ -63,10 +73,17 @@ def __init__(self):
             score_threshold=self._score_threshold,
         )
 
+        # Subscribe to all-queries topics so rayfronts always publishes to them
+        rf_prefix = f'{self._prefix}/rayfronts/msg_serv'
+        self.create_subscription(
+            PointCloud2,
+            f'{rf_prefix}/rays_sim/all',
+            self._ray_all_cb, 10)
         self.create_subscription(
             PointCloud2,
-            f'{self._prefix}/rayfronts/msg_serv/rays_sim/all',
-            self._rays_cb, 10)
+            f'{rf_prefix}/voxels_sim/all',
+            self._vox_all_cb, 10)
+
         self.create_subscription(
             PointCloud2,
             f'{self._prefix}/rayfronts/msg_serv/frontiers',
@@ -88,74 +105,78 @@ def __init__(self):
             f'query_labels={self._query_labels} | '
             f'score_threshold={self._score_threshold}')
 
-    def _rays_cb(self, msg: PointCloud2):
-        """Parse rays_sim/all PointCloud2.
-
-        Fields: x, y, z (RDF origin), theta, phi (degrees), sim_0, sim_1, ...
-        Converts origin + direction to world FLU frame.
-        """
-        field_names = [f.name for f in msg.fields]
-        pts = list(point_cloud2.read_points(msg, field_names=field_names,
-                                            skip_nans=True))
+    def _ray_all_cb(self, msg: PointCloud2):
+        """Receive all-queries ray PointCloud2 (fields: x,y,z,theta,phi,sim_0,sim_1,...)."""
+        Q = len(self._query_labels)
+        if Q == 0:
+            return
+        fields = ('x', 'y', 'z', 'theta', 'phi') + tuple(f'sim_{q}' for q in range(Q))
+        pts = list(point_cloud2.read_points(msg, field_names=fields, skip_nans=True))
         if not pts:
             self._ray_origins = None
             self._ray_dirs = None
             self._ray_scores = None
             return
-
-        pts_arr = np.array([list(p) for p in pts], dtype=np.float32)
-
-        x_idx = field_names.index('x')
-        y_idx = field_names.index('y')
-        z_idx = field_names.index('z')
-        theta_idx = field_names.index('theta')
-        phi_idx = field_names.index('phi')
-
-        rdf_origins = pts_arr[:, [x_idx, y_idx, z_idx]]
-        theta = np.deg2rad(pts_arr[:, theta_idx])
-        phi = np.deg2rad(pts_arr[:, phi_idx])
-
-        # Spherical → Cartesian in RDF frame
-        # Matches geometry3d.spherical_to_cartesian / cartesian_to_spherical:
-        #   theta = azimuthal in xy-plane, phi = polar from +z (forward)
-        # x=cos(theta)*sin(phi), y=sin(theta)*sin(phi), z=cos(phi)
-        dx_rdf = np.cos(theta) * np.sin(phi)
-        dy_rdf = np.sin(theta) * np.sin(phi)
-        dz_rdf = np.cos(phi)
-        rdf_dirs = np.stack([dx_rdf, dy_rdf, dz_rdf], axis=1)
-
-        # RDF → FLU: world_x=rdf_z, world_y=-rdf_x, world_z=-rdf_y
-        self._ray_origins = np.stack([
-            rdf_origins[:, 2], -rdf_origins[:, 0], -rdf_origins[:, 1]], axis=1)
-        self._ray_dirs = np.stack([
-            rdf_dirs[:, 2], -rdf_dirs[:, 0], -rdf_dirs[:, 1]], axis=1)
-
-        # Collect sim_* columns in order
-        sim_cols = [i for i, n in enumerate(field_names) if n.startswith('sim_')]
-        sim_cols.sort(key=lambda i: int(field_names[i].split('_')[1]))
-        if sim_cols:
-            self._ray_scores = pts_arr[:, sim_cols]  # (N, M)
-        else:
-            self._ray_scores = None
+        arr = np.array([list(p) for p in pts], dtype=np.float32)
+        rdf_orig = arr[:, :3]
+        theta = np.deg2rad(arr[:, 3])
+        phi = np.deg2rad(arr[:, 4])
+        sim_all = arr[:, 5:]  # (N, Q)
+        # Spherical → Cartesian in RDF
+        dx = np.cos(theta) * np.sin(phi)
+        dy = np.sin(theta) * np.sin(phi)
+        dz = np.cos(phi)
+        rdf_dirs = np.stack([dx, dy, dz], axis=1)
+        # RDF → FLU
+        flu_orig = np.stack([rdf_orig[:, 2], -rdf_orig[:, 0], -rdf_orig[:, 1]], axis=1)
+        flu_dirs = np.stack([rdf_dirs[:, 2], -rdf_dirs[:, 0], -rdf_dirs[:, 1]], axis=1)
+        self._ray_origins = flu_orig
+        self._ray_dirs = flu_dirs
+        self._ray_scores = sim_all
+
+    def _vox_all_cb(self, msg: PointCloud2):
+        """Receive all-queries voxel PointCloud2 (fields: x,y,z,sim_0,sim_1,...)."""
+        Q = len(self._query_labels)
+        if Q == 0:
+            return
+        fields = ('x', 'y', 'z') + tuple(f'sim_{q}' for q in range(Q))
+        pts = list(point_cloud2.read_points(msg, field_names=fields, skip_nans=True))
+        if not pts:
+            self._vox_xyz = None
+            self._vox_scores = None
+            return
+        arr = np.array([list(p) for p in pts], dtype=np.float32)
+        rdf_xyz = arr[:, :3]
+        sim_all = arr[:, 3:]  # (N, Q)
+        # RDF → FLU
+        flu_xyz = np.stack([rdf_xyz[:, 2], -rdf_xyz[:, 0], -rdf_xyz[:, 1]], axis=1)
+        self._vox_xyz = flu_xyz
+        self._vox_scores = sim_all
 
     def _frontiers_cb(self, msg: PointCloud2):
-        """Parse msg_serv/frontiers — already in FLU world frame."""
         pts = list(point_cloud2.read_points(msg, field_names=('x', 'y', 'z'),
                                             skip_nans=True))
-        if pts:
-            self._frontiers = np.array([[p[0], p[1], p[2]] for p in pts],
-                                       dtype=np.float32)
-        else:
-            self._frontiers = None
+        self._frontiers = (
+            np.array([[p[0], p[1], p[2]] for p in pts], dtype=np.float32)
+            if pts else None)
 
     def _odometry_cb(self, msg: Odometry):
         p = msg.pose.pose.position
         self._cur_pose = np.array([p.x, p.y, p.z], dtype=np.float64)
 
     def _input_prompt_cb(self, msg: String):
-        targets = [t.strip().lower() for t in msg.data.split(',') if t.strip()]
+        targets = [t.strip() for t in msg.data.split(',') if t.strip()]
         if targets:
             self._target_objects = targets
+            # Keep _query_labels in sync so vox/ray callbacks read the right columns
+            if targets != self._query_labels:
+                self._query_labels = targets[:]
+                # Clear cached data so stale multi-column arrays don't confuse behaviors
+                self._vox_xyz = None
+                self._vox_scores = None
+                self._ray_origins = None
+                self._ray_dirs = None
+                self._ray_scores = None
             self.get_logger().info(f'target objects updated: {self._target_objects}')
 
     def _timer_cb(self):
@@ -170,6 +191,8 @@ def _timer_cb(self):
             ray_scores=self._ray_scores,
             query_labels=self._query_labels,
             target_objects=self._target_objects,
+            vox_xyz=self._vox_xyz,
+            vox_scores=self._vox_scores,
         )
         self._behavior_mode = self._behavior_manager.behavior_mode
 
@@ -188,21 +211,30 @@ def _timer_cb(self):
                 target_waypoint=self._target_waypoint,
                 target_waypoint2=self._target_waypoint2,
                 publisher_dict=self._publisher_dict,
+                vox_xyz=self._vox_xyz,
+                vox_scores=self._vox_scores,
+                query_labels=self._query_labels,
             )
 
-        # Throttled status summary
+        # Completed-targets publishing disabled — task stops only on manual cancel
+        completed = list(self._behavior_manager.completed_queries)
+
+        # Status log
         n_frontiers = len(self._frontiers) if self._frontiers is not None else 0
         n_rays = len(self._ray_origins) if self._ray_origins is not None else 0
+        n_voxels = len(self._vox_xyz) if self._vox_xyz is not None else 0
         ray_beh = self._behavior_manager.ray_behavior
-        n_filtered = len(ray_beh._filtered_indices) if ray_beh._filtered_indices is not None else 0
-        publishing = self._target_waypoint is not None
+        n_filtered = (len(ray_beh._filtered_indices)
+                      if ray_beh._filtered_indices is not None else 0)
         wp = (f'({self._target_waypoint[0]:.1f}, {self._target_waypoint[1]:.1f}, '
-              f'{self._target_waypoint[2]:.1f})') if publishing else 'none'
+              f'{self._target_waypoint[2]:.1f})'
+              if self._target_waypoint is not None else 'none')
         self.get_logger().info(
             f'[{self._behavior_mode}] '
-            f'frontiers={n_frontiers} | rays={n_rays} filtered={n_filtered} | '
-            f'target={self._behavior_manager.ray_behavior.current_target} | '
-            f'publishing={publishing} wp={wp}',
+            f'frontiers={n_frontiers} rays={n_rays} filtered={n_filtered} '
+            f'voxels={n_voxels} | '
+            f'target={ray_beh.current_target} | '
+            f'completed={completed} | wp={wp}',
             throttle_duration_sec=2.0,
         )
 
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml b/robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml
new file mode 100644
index 00000000..7ac94ab5
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml
@@ -0,0 +1,9 @@
+<launch>
+    <node pkg="semantic_search_task" exec="semantic_search_task"
+          name="semantic_search_task" output="screen">
+        <remap from="~/semantic_search_task"
+               to="/$(env ROBOT_NAME)/tasks/semantic_search" />
+        <remap from="/$(env ROBOT_NAME)/odometry"
+               to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+    </node>
+</launch>
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/package.xml b/robot/ros_ws/src/global/planners/semantic_search_task/package.xml
new file mode 100644
index 00000000..e9363a32
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>semantic_search_task</name>
+  <version>0.0.0</version>
+  <description>SemanticSearchTask action server: sends natural language queries to rayfronts and monitors voxel similarity output</description>
+  <maintainer email="todo@todo.todo">todo</maintainer>
+  <license>TODO</license>
+
+  <depend>rclpy</depend>
+  <depend>task_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/resource/semantic_search_task b/robot/ros_ws/src/global/planners/semantic_search_task/resource/semantic_search_task
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/__init__.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
new file mode 100644
index 00000000..8f17565e
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
@@ -0,0 +1,489 @@
+import os
+import queue
+import re
+import signal
+import subprocess
+import threading
+import time
+
+import numpy as np
+import rclpy
+import rclpy.executors
+from geometry_msgs.msg import Point, Polygon, Pose
+from nav_msgs.msg import Odometry
+from rclpy.action import ActionServer, ActionClient, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from sensor_msgs_py import point_cloud2
+from std_msgs.msg import String
+from task_msgs.action import SemanticSearchTask, ExplorationTask
+
+# ── ANSI / ROS log stripping ──────────────────────────────────────────────────
+
+_ANSI_RE = re.compile(r'\x1b\[[0-9;]*m')
+_ROS_PREFIX_RE = re.compile(
+    r'^\s*\[?(INFO|WARN|ERROR|DEBUG)\]?\s*\[\d+\.\d+\]\s*\[[^\]]+\]:\s*')
+
+
+def _clean(line: str) -> str:
+    line = _ANSI_RE.sub('', line)
+    line = _ROS_PREFIX_RE.sub('', line)
+    return line.strip()
+
+
+# ── Output filters ────────────────────────────────────────────────────────────
+
+def _filter_rayfronts(line: str) -> str | None:
+    """Return a short human-readable string for key rayfronts events, else None."""
+    raw = line
+    line = _clean(line)
+    low = line.lower()
+
+    if 'error' in low or 'exception' in low or 'traceback' in low:
+        return f'ERROR: {line}'
+    if 'downloading' in low:
+        return 'Downloading model weights...'
+    if 'parsing model identifier' in low or 'loaded built-in' in low:
+        return 'Loading model...'
+    if 'subscribing to' in low or 'subscribed to' in low:
+        return f'Subscribing: {line}'
+    if 'received queries' in low:
+        return f'Encoding queries: {line}'
+    if 'mapping' in low and ('start' in low or 'running' in low or 'loop' in low):
+        return 'Mapping started'
+    if 'process started' in low or 'process has died' in low:
+        return line
+    # Skip noise: warnings, deprecations, progress bars, blank lines
+    if not line or 'warning' in low or 'warn' in low or 'deprecated' in low:
+        return None
+    if '|' in raw and '█' in raw:   # tqdm progress bar
+        return None
+    return None
+
+
+def _filter_raven(line: str) -> str | None:
+    """Reformat raven's status line; skip everything else."""
+    line = _clean(line)
+    if not line:
+        return None
+    # The periodic status line looks like:
+    # [Frontier-based] frontiers=N rays=N filtered=N voxels=N | target=X | completed=[] | wp=(...)
+    m = re.search(r'\[(Frontier-based|Ray-based|Voxel-based)\]', line)
+    if not m:
+        if 'error' in line.lower():
+            return f'ERROR: {line}'
+        return None
+
+    mode = m.group(1)
+    frontiers = re.search(r'frontiers=(\d+)', line)
+    rays = re.search(r'rays=(\d+)', line)
+    voxels = re.search(r'voxels=(\d+)', line)
+    completed = re.search(r'completed=(\[[^\]]*\])', line)
+    wp = re.search(r'wp=(\S+)', line)
+
+    parts = [f'[{mode}]']
+    if frontiers:
+        parts.append(f'frontiers={frontiers.group(1)}')
+    if rays:
+        parts.append(f'rays={rays.group(1)}')
+    if voxels:
+        parts.append(f'voxels={voxels.group(1)}')
+    if completed and completed.group(1) != '[]':
+        parts.append(f'completed={completed.group(1)}')
+    if wp and wp.group(1) != 'none':
+        parts.append(f'wp={wp.group(1)}')
+    return ' '.join(parts)
+
+
+# ── Subprocess helpers ────────────────────────────────────────────────────────
+
+def _pipe_to_queue(proc: subprocess.Popen, q: queue.Queue) -> None:
+    for line in iter(proc.stdout.readline, b''):
+        q.put(line.decode('utf-8', errors='replace').rstrip())
+    q.put(None)
+
+
+def _drain(q: queue.Queue) -> list:
+    lines = []
+    while True:
+        try:
+            item = q.get_nowait()
+            if item is None:
+                break
+            lines.append(item)
+        except queue.Empty:
+            break
+    return lines
+
+
+def _sanitize(label: str) -> str:
+    return re.sub(r'[^a-zA-Z0-9_]', '_', label)
+
+
+# ── Node ──────────────────────────────────────────────────────────────────────
+
+class SemanticSearchTaskNode(Node):
+    def __init__(self):
+        super().__init__('semantic_search_task')
+        robot_name = os.getenv('ROBOT_NAME', 'robot_1')
+        ros_domain = os.getenv('ROS_DOMAIN_ID', '0')
+        self._robot_prefix = f'/{robot_name}'
+        self._rf_prefix = f'/robot_{ros_domain}/rayfronts/msg_serv'
+
+        self._cbg = ReentrantCallbackGroup()
+        self._task_active = False
+        self._cur_pos = None
+
+        self._text_query_pub = self.create_publisher(
+            String, f'{self._rf_prefix}/new_text_query', 10)
+
+        self.create_subscription(
+            Odometry, f'{self._robot_prefix}/odometry',
+            self._odom_cb, 10, callback_group=self._cbg)
+
+        self._action_server = ActionServer(
+            self, SemanticSearchTask, '~/semantic_search_task',
+            execute_callback=self._execute,
+            goal_callback=self._handle_goal,
+            cancel_callback=lambda _: CancelResponse.ACCEPT,
+            callback_group=self._cbg)
+
+        self._cleanup_existing()
+        self.get_logger().info('semantic_search_task ready')
+
+    def _odom_cb(self, msg: Odometry):
+        p = msg.pose.pose.position
+        self._cur_pos = [p.x, p.y, p.z]
+
+    def _handle_goal(self, goal_request):
+        if self._task_active:
+            self.get_logger().warn('Rejecting goal: task already active')
+            return GoalResponse.REJECT
+        self._task_active = True
+        return GoalResponse.ACCEPT
+
+    def _cleanup_existing(self) -> None:
+        """Kill any leftover rayfronts, raven, or random_walk processes."""
+        for pattern in ['rayfronts.mapping_server', 'raven_nav_node',
+                        'random_walk_planner']:
+            result = subprocess.run(
+                ['pkill', '-SIGTERM', '-f', pattern], capture_output=True)
+            if result.returncode == 0:
+                self.get_logger().info(f'Killed existing {pattern} process(es)')
+        time.sleep(2.0)
+        for pattern in ['rayfronts.mapping_server', 'raven_nav_node',
+                        'random_walk_planner']:
+            subprocess.run(['pkill', '-SIGKILL', '-f', pattern], capture_output=True)
+
+    def _spawn(self, cmd: list) -> tuple:
+        proc = subprocess.Popen(
+            cmd,
+            stdout=subprocess.PIPE,
+            stderr=subprocess.STDOUT,
+            start_new_session=True)
+        q = queue.Queue()
+        threading.Thread(target=_pipe_to_queue, args=(proc, q), daemon=True).start()
+        return proc, q
+
+    def _kill(self, name: str, proc: subprocess.Popen) -> None:
+        if proc is None:
+            return
+        self.get_logger().info(f'Stopping {name} (pid={proc.pid})')
+        try:
+            os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
+        except ProcessLookupError:
+            return
+        try:
+            proc.wait(timeout=5.0)
+        except subprocess.TimeoutExpired:
+            try:
+                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
+            except ProcessLookupError:
+                pass
+
+    def _spawn_random_walk(self, robot_name: str) -> subprocess.Popen:
+        # Resolve the config file path via ros2 pkg
+        prefix = subprocess.run(
+            ['ros2', 'pkg', 'prefix', 'random_walk_planner'],
+            capture_output=True, text=True).stdout.strip()
+        config = f'{prefix}/share/random_walk_planner/config/random_walk_config.yaml'
+        return self._spawn([
+            'ros2', 'run', 'random_walk_planner', 'random_walk_planner',
+            '--ros-args',
+            '--params-file', config,
+            '-r', f'~/global_plan:=/{robot_name}/global_plan',
+            '-r', f'~/exploration_task:=/{robot_name}/tasks/exploration',
+            '-r', f'navigate_task:=/{robot_name}/tasks/navigate',
+            '-r', f'odometry:=/{robot_name}/odometry_conversion/odometry',
+            '-r', f'vdb_map_visualization:=/{robot_name}/vdb_mapping/vdb_map_visualization',
+            '-r', f'~/global_plan_toggle:=/{robot_name}/behavior/global_plan_toggle',
+        ])
+
+    def _send_exploration_task(self, robot_name: str) -> None:
+        """Send an unbounded ExplorationTask goal to random_walk_planner to activate droan_gl."""
+        client = ActionClient(
+            self,
+            ExplorationTask,
+            f'/{robot_name}/tasks/exploration',
+            callback_group=self._cbg)
+        if not client.wait_for_server(timeout_sec=10.0):
+            self.get_logger().warn('ExplorationTask server not available after 10s')
+            return
+        goal = ExplorationTask.Goal()
+        goal.min_altitude_agl = 2.0
+        goal.max_altitude_agl = 15.0
+        goal.min_flight_speed = 1.0
+        goal.max_flight_speed = 3.0
+        goal.time_limit_sec = 0.0   # 0 = no limit, runs until cancelled
+        client.send_goal_async(goal)
+        self.get_logger().info('ExplorationTask sent to random_walk_planner')
+
+    def _interruptible_sleep(self, goal_handle, secs: float) -> bool:
+        """Sleep for secs, waking early if cancel requested. Returns True if cancelled."""
+        steps = max(1, int(secs / 0.1))
+        for _ in range(steps):
+            if goal_handle.is_cancel_requested:
+                return True
+            time.sleep(0.1)
+        return False
+
+    def _execute(self, goal_handle):
+        goal = goal_handle.request
+        queries = [q.strip() for q in goal.query.split(',') if q.strip()]
+        if not queries:
+            self._task_active = False
+            goal_handle.abort()
+            result = SemanticSearchTask.Result()
+            result.success = False
+            result.message = 'Empty query'
+            return result
+
+        self.get_logger().info(f'SemanticSearchTask | queries={queries}')
+
+        rayfronts_proc = raven_proc = random_walk_proc = None
+        rayfronts_q = raven_q = queue.Queue()
+
+        # Track last meaningful line from each process for feedback
+        last_rf_status = 'Starting rayfronts...'
+        last_rv_status = 'Starting raven...'
+
+        try:
+            # Kill any leftover processes from a previous run
+            self._cleanup_existing()
+
+            robot_name = os.getenv('ROBOT_NAME', 'robot_1')
+
+            # Wait for old rayfronts DDS subscription to fully unregister
+            deadline = time.time() + 10.0
+            while time.time() < deadline:
+                if goal_handle.is_cancel_requested:
+                    goal_handle.canceled()
+                    result = SemanticSearchTask.Result()
+                    result.success = False
+                    result.message = 'Cancelled'
+                    return result
+                if self.count_subscribers(f'{self._rf_prefix}/new_text_query') == 0:
+                    break
+                time.sleep(0.5)
+
+            # Start raven first so it's subscribed to rays_sim/all and voxels_sim/all
+            # before rayfronts runs its first query cycle (rayfronts lazy-publishes
+            # those topics only when a subscriber exists)
+            labels_yaml = str(queries).replace("'", '"')
+            raven_proc, raven_q = self._spawn([
+                'ros2', 'run', 'raven_nav', 'raven_nav_node',
+                '--ros-args',
+                '-p', f'query_labels:={labels_yaml}',
+                '-r', (f'/{robot_name}/odometry:='
+                       f'/{robot_name}/odometry_conversion/odometry'),
+            ])
+
+            rayfronts_proc, rayfronts_q = self._spawn([
+                'ros2', 'launch', 'perception_bringup', 'rayfronts.launch.xml',
+            ])
+
+
+            start = self.get_clock().now()
+            best_conf = 0.0
+            rayfronts_ready = False
+            prev_rf_sub_count = 0
+            mapping_started = False
+            random_walk_started = False
+            raven_published_waypoint = False
+            completed_targets: set = set()
+
+            # Subscribe to raven's global_plan to detect first waypoint
+            from nav_msgs.msg import Path
+            def _global_plan_cb(msg):
+                nonlocal raven_published_waypoint
+                if msg.poses:
+                    raven_published_waypoint = True
+            self.create_subscription(
+                Path, f'/{robot_name}/global_plan',
+                _global_plan_cb, 1, callback_group=self._cbg)
+
+            # Subscribe to raven's completed_targets
+            def _completed_targets_cb(msg):
+                nonlocal completed_targets
+                import json
+                try:
+                    completed_targets = set(json.loads(msg.data))
+                except Exception:
+                    pass
+            self.create_subscription(
+                String, f'/{robot_name}/completed_targets',
+                _completed_targets_cb, 10, callback_group=self._cbg)
+
+            # Subscribe to voxels_sim/all for best-confidence tracking
+            Q = len(queries)
+            def _vox_all_cb(msg):
+                nonlocal best_conf, mapping_started
+                fields = ('x', 'y', 'z') + tuple(f'sim_{q}' for q in range(Q))
+                from sensor_msgs_py import point_cloud2 as pc2
+                pts = list(pc2.read_points(msg, field_names=fields, skip_nans=True))
+                if pts:
+                    import numpy as _np
+                    arr = _np.array([list(p) for p in pts], dtype=_np.float32)
+                    conf = float(arr[:, 3:].max())
+                    if conf > best_conf:
+                        best_conf = conf
+                    mapping_started = True
+            self.create_subscription(
+                PointCloud2, f'{self._rf_prefix}/voxels_sim/all',
+                _vox_all_cb, 10, callback_group=self._cbg)
+
+            while rclpy.ok():
+                if goal_handle.is_cancel_requested:
+                    goal_handle.canceled()
+                    result = SemanticSearchTask.Result()
+                    result.success = False
+                    result.message = 'Cancelled'
+                    return result
+
+                elapsed = (self.get_clock().now() - start).nanoseconds / 1e9
+                if goal.time_limit_sec > 0 and elapsed >= goal.time_limit_sec:
+                    done = [q for q in queries
+                            if q.lower() in set(c.lower() for c in completed_targets)]
+                    succeeded = len(done) == len(queries)
+                    if succeeded:
+                        goal_handle.succeed()
+                    else:
+                        goal_handle.abort()
+                    result = SemanticSearchTask.Result()
+                    result.success = succeeded
+                    result.message = (f'Timed out. Visited {len(done)}/{len(queries)}: '
+                                      f'{", ".join(done)}')
+                    result.confidence = best_conf
+                    return result
+
+                # Drain and filter rayfronts output
+                for raw in _drain(rayfronts_q):
+                    msg = _filter_rayfronts(raw)
+                    if msg:
+                        last_rf_status = msg
+
+                # Drain and filter raven output
+                for raw in _drain(raven_q):
+                    msg = _filter_raven(raw)
+                    if msg:
+                        last_rv_status = msg
+
+                # Start random_walk once raven publishes its first waypoint
+                if raven_published_waypoint and not random_walk_started:
+                    random_walk_started = True
+                    random_walk_proc, _ = self._spawn_random_walk(robot_name)
+                    self.get_logger().info(
+                        'Raven published first waypoint — starting random_walk_planner')
+                    time.sleep(3.0)
+                    self._send_exploration_task(robot_name)
+
+                # Send queries to rayfronts whenever its subscriber appears (or reappears).
+                # This handles the initial load AND any rayfronts restart mid-task.
+                rf_sub_count = self.count_subscribers(f'{self._rf_prefix}/new_text_query')
+                if rf_sub_count > 0 and prev_rf_sub_count == 0:
+                    rayfronts_ready = True
+                    for q in queries:
+                        self._text_query_pub.publish(String(data=q))
+                    last_rf_status = f'Queries sent: {", ".join(queries)}'
+                    self.get_logger().info(
+                        f'Queries sent to rayfronts: {queries}')
+                if rf_sub_count == 0 and prev_rf_sub_count > 0:
+                    rayfronts_ready = False
+                    self.get_logger().info('rayfronts subscriber lost — will resend on reconnect')
+                prev_rf_sub_count = rf_sub_count
+
+                # Success: raven visited a cluster for every query
+                queries_set = set(q.lower() for q in queries)
+                completed_lower = set(c.lower() for c in completed_targets)
+                if queries_set and queries_set.issubset(completed_lower):
+                    goal_handle.succeed()
+                    result = SemanticSearchTask.Result()
+                    result.success = True
+                    result.message = (
+                        f'All targets visited: {", ".join(sorted(queries_set))}')
+                    result.confidence = best_conf
+                    return result
+
+                # Build clean feedback status
+                pos_str = ''
+                if self._cur_pos:
+                    pos_str = (f'pos=({self._cur_pos[0]:.1f}, '
+                               f'{self._cur_pos[1]:.1f}, '
+                               f'{self._cur_pos[2]:.1f})')
+
+                pending = [q for q in queries
+                           if q.lower() not in set(c.lower() for c in completed_targets)]
+                done = [q for q in queries
+                        if q.lower() in set(c.lower() for c in completed_targets)]
+
+                if not rayfronts_ready:
+                    status = f'[rayfronts] {last_rf_status}'
+                elif not mapping_started:
+                    status = (f'[rayfronts] {last_rf_status}\n'
+                              f'[raven] {last_rv_status}')
+                else:
+                    progress = f'{len(done)}/{len(queries)} targets visited'
+                    done_str = f'done=[{", ".join(done)}]' if done else ''
+                    pending_str = f'pending=[{", ".join(pending)}]' if pending else ''
+                    status = (f'[raven] {last_rv_status}\n'
+                              f'{progress}  {done_str}  {pending_str}  {pos_str}').strip()
+
+                fb = SemanticSearchTask.Feedback()
+                fb.status = status
+                fb.best_confidence_so_far = best_conf
+                if self._cur_pos:
+                    fb.current_position = Point(
+                        x=self._cur_pos[0],
+                        y=self._cur_pos[1],
+                        z=self._cur_pos[2])
+                goal_handle.publish_feedback(fb)
+
+                if self._interruptible_sleep(goal_handle, 1.0):
+                    continue   # cancel was requested, loop back to check it
+
+        finally:
+            self._kill('rayfronts', rayfronts_proc)
+            self._kill('raven', raven_proc)
+            self._kill('random_walk', random_walk_proc)
+            self._task_active = False
+
+        goal_handle.abort()
+        result = SemanticSearchTask.Result()
+        result.success = False
+        result.message = 'Node shutdown'
+        return result
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = SemanticSearchTaskNode()
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg b/robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg
new file mode 100644
index 00000000..f68b1ceb
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/semantic_search_task
+[install]
+install_scripts=$base/lib/semantic_search_task
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/setup.py b/robot/ros_ws/src/global/planners/semantic_search_task/setup.py
new file mode 100644
index 00000000..20006247
--- /dev/null
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'semantic_search_task'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/semantic_search_task.launch.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='todo',
+    maintainer_email='todo@todo.todo',
+    description='SemanticSearchTask action server using rayfronts + raven_nav',
+    license='TODO',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'semantic_search_task = semantic_search_task.node:main',
+        ],
+    },
+)
diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
index 1492161b..9ba28418 100644
--- a/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
+++ b/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
@@ -1,7 +1,6 @@
 <launch>
   <arg name="config_name" default="low_memory"/>
   <arg name="dataset"     default="ros2isaacsim"/>
-  <arg name="queries_file" default="/opt/rayfronts/queries.txt"/>
   <arg name="query_period" default="10"/>
 
   <executable
@@ -10,7 +9,7 @@
          dataset=$(var dataset)
          messaging_service=ros
          querying.text_query_mode=prompts
-         querying.query_file=$(var queries_file)
+         querying.query_file=null
          querying.compute_prob=True
          querying.period=$(var query_period)"
     cwd="/opt/rayfronts"
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 1af07ed8..cfc01e60 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -188,7 +188,7 @@ def __init__(self):
                 drone_prim=f"/World/drone{i}/base_link",
                 robot_name=f"robot_{i}",
                 camera_name="ZEDCamera",
-                camera_offset=[0.1, 0.0, 0.0],
+                camera_offset=[0.21, 0.0, 0.05],
                 camera_rotation_offset=[0.0, 0.0, 0.0],
             )
 

From 6b53e57a10199f0dd5b8d638cfee20a0851fa6f8 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 23 Apr 2026 16:23:49 -0400
Subject: [PATCH 40/55] fixed raven behavior

---
 .../desktop_bringup/launch/gcs.launch.xml     |    1 +
 .../desktop_bringup/params/domain_bridge.yaml |    2 +
 .../desktop_bringup/rviz/robot.rviz           |  101 +-
 .../rviz/rviz_tasks_panel/src/tasks_panel.cpp |    8 +-
 .../action/SemanticSearchTask.action          |   13 +-
 docs/action_bridging.md                       |   58 +
 .../robot-commands/dist/extension.js          | 1013 ++++++++++++++---
 .../robot-commands/package.json               |    6 +-
 .../robot-commands/robot-commands.foxe        |  Bin 2006 -> 6351 bytes
 .../foxglove_visualizer_node.py               |  105 +-
 .../onboard_all/config/domain_bridge.yaml     |    7 +-
 .../planners/raven_nav/config/raven_nav.yaml  |    6 +-
 .../raven_nav/raven_nav/behavior_manager.py   |   11 +-
 .../raven_nav/behaviors/frontier_behavior.py  |    9 +-
 .../raven_nav/behaviors/ray_behavior.py       |   92 +-
 .../raven_nav/raven_nav/raven_nav_node.py     |   35 +-
 .../semantic_search_task/node.py              |   96 +-
 .../launch_scripts/two_drone_fire.py          |    2 +-
 18 files changed, 1256 insertions(+), 309 deletions(-)
 create mode 100644 docs/action_bridging.md

diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index b5693678..2cab8a2d 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -26,6 +26,7 @@
     <param name="address" value="0.0.0.0" />
     <param name="use_sim_time" value="$(var sim)" />
     <param name="send_buffer_limit" value="10000000" />
+    <param name="include_hidden" value="true" />
   </node>
 
   <!-- Gossip bridge: connects GCS domain (0) to gossip bus (domain 99) -->
diff --git a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
index c07a5acb..3eb8c330 100644
--- a/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
+++ b/common/ros_packages/desktop_bringup/params/domain_bridge.yaml
@@ -97,6 +97,8 @@ topics:
     type: sensor_msgs/msg/BatteryState
     from_domain: 3
     to_domain: 0
+  
+
 
   # Bridge "/clock" topic from doman ID 2 to domain ID 3,
   # Override durability to be 'volatile' and override depth to be 1
diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz
index 00e2f38e..4adb19f6 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz
@@ -30,7 +30,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: StereoImageProc Disparity
+    SyncSource: voxel_rgb
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
     topic: /robot_1/behavior_tree_graphviz
@@ -482,7 +482,8 @@ 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
@@ -665,7 +666,8 @@ Visualization Manager:
               Enabled: true
               Name: Global Plan Vis
               Namespaces:
-                {}
+                global_plan: true
+                global_plan_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -694,6 +696,7 @@ Visualization Manager:
               Name: Traj Vis
               Namespaces:
                 traj_controller: true
+                traj_controller_extra_text: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -781,7 +784,7 @@ Visualization Manager:
       Channel Name: intensity
       Class: rviz_default_plugins/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: ""
+      Color Transformer: Intensity
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
@@ -790,7 +793,7 @@ Visualization Manager:
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: rays msg_serv
-      Position Transformer: ""
+      Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.5
@@ -808,7 +811,7 @@ Visualization Manager:
       Enabled: true
       Name: filtered rays
       Namespaces:
-        {}
+        arrows: true
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -826,7 +829,7 @@ Visualization Manager:
       Channel Name: intensity
       Class: rviz_default_plugins/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: ""
+      Color Transformer: Intensity
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
@@ -835,7 +838,7 @@ Visualization Manager:
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: voxel msg_serv
-      Position Transformer: ""
+      Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.5
@@ -859,7 +862,7 @@ Visualization Manager:
       Channel Name: intensity
       Class: rviz_default_plugins/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: ""
+      Color Transformer: RGB8
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
@@ -868,7 +871,7 @@ Visualization Manager:
       Min Color: 0; 0; 0
       Min Intensity: 0
       Name: voxel_rgb
-      Position Transformer: ""
+      Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.5
@@ -882,6 +885,72 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/voxel_feature
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: ""
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_1/rayfronts/queries/red_building/voxels
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -945,25 +1014,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 53.187862396240234
+      Distance: 35.724098205566406
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 25.142688751220703
-        Y: -7.090958595275879
-        Z: -18.70828628540039
+        X: 24.834373474121094
+        Y: -6.7379069328308105
+        Z: -19.230995178222656
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6403977274894714
+      Pitch: 0.6453977227210999
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 2.878566026687622
+      Yaw: 2.873566150665283
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
index 632ba8a0..68db70d0 100644
--- a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
@@ -69,13 +69,13 @@ std::vector<TaskTypeDef> TasksPanel::getTaskDefs()
     }, true},
     {"Semantic Search", "tasks/semantic_search", {
       {"query", "string", 0, 0, 0},
+      {"background_queries", "string", 0, 0, 0},
       {"search_area", "geometry_msgs/Polygon", 0, 0, 0},
       {"min_altitude_agl", "float32", 3.0, 0.0, 500.0},
-      {"max_altitude_agl", "float32", 10.0, 0.0, 500.0},
+      {"max_altitude_agl", "float32", 15.0, 0.0, 500.0},
       {"min_flight_speed", "float32", 1.0, 0.0, 50.0},
       {"max_flight_speed", "float32", 3.0, 0.0, 50.0},
-      {"time_limit_sec", "float32", 120.0, 0.0, 86400.0},
-      {"confidence_threshold", "float32", 0.5, 0.0, 1.0},
+      {"confidence_threshold", "float32", 0.95, 0.0, 1.0},
     }, true},
     {"Fixed Trajectory", "tasks/fixed_trajectory", {
       {"trajectory_spec", "airstack_msgs/FixedTrajectory", 0, 0, 0},
@@ -1007,12 +1007,12 @@ void TasksPanel::onExecuteClicked()
     case 7: {  // Semantic Search
       task_msgs::action::SemanticSearchTask::Goal goal;
       goal.query = getString(7, "query");
+      goal.background_queries = getString(7, "background_queries");
       goal.search_area = getPolygon(7, "search_area");
       goal.min_altitude_agl = getFloat(7, "min_altitude_agl");
       goal.max_altitude_agl = getFloat(7, "max_altitude_agl");
       goal.min_flight_speed = getFloat(7, "min_flight_speed");
       goal.max_flight_speed = getFloat(7, "max_flight_speed");
-      goal.time_limit_sec = getFloat(7, "time_limit_sec");
       goal.confidence_threshold = getFloat(7, "confidence_threshold");
       doSendGoal<task_msgs::action::SemanticSearchTask>(7, goal,
         [](const auto & fb) {
diff --git a/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action b/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action
index 27101625..6f077575 100644
--- a/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action
+++ b/common/ros_packages/msgs/task_msgs/action/SemanticSearchTask.action
@@ -1,16 +1,17 @@
 # Search an area for a location or object described in natural language.
-# confidence_threshold: minimum match confidence to report a result (0.0-1.0)
-# time_limit_sec: maximum task duration in seconds (0 = no limit)
+# confidence_threshold: minimum match confidence to report a result (0.0-1.0, default 0.95)
+# background_queries: comma-separated contrast classes for softmax normalization
+#   (e.g. "building,tree,ground"). Required — softmax over a single query is degenerate.
 
 # Goal
 string query
+string background_queries
 geometry_msgs/Polygon search_area
-float32 min_altitude_agl
-float32 max_altitude_agl
+float32 min_altitude_agl 3.0
+float32 max_altitude_agl 15.0
 float32 min_flight_speed
 float32 max_flight_speed
-float32 time_limit_sec
-float32 confidence_threshold
+float32 confidence_threshold 0.95
 ---
 # Result
 bool success
diff --git a/docs/action_bridging.md b/docs/action_bridging.md
new file mode 100644
index 00000000..a85e3192
--- /dev/null
+++ b/docs/action_bridging.md
@@ -0,0 +1,58 @@
+# ROS 2 action bridging across DDS domains
+
+*Summary for AirStack team discussion.*
+
+## Problem
+
+Each robot runs on its own `ROS_DOMAIN_ID` (robot_1 → 1, robot_2 → 2, …); the GCS runs on domain 0. We bridge with eProsima **`ddsrouter`**. Topics work. **Actions don't.**
+
+From the GCS container:
+- `ros2 service list --include-hidden-services -t` shows `/robot_1/tasks/takeoff/_action/send_goal` — name + type are bridged.
+- `ros2 action info /robot_1/tasks/takeoff` → `Action servers: 0`.
+- `ros2 action send_goal` hangs; Foxglove reports "Service … is not available".
+
+### Root cause
+
+`ddsrouter` bridges the DDS `rq/…Request` / `rr/…Reply` topics underneath a ROS 2 service, but every ROS 2 action client calls `rcl_service_server_is_available()` first. That walks the RMW endpoint graph for a **matched DDS server signature**; ddsrouter's proxy endpoints don't match, so the call aborts. It's an RMW endpoint-matching problem, not a naming/visibility problem.
+
+Same symptom reported in the wild: [RTI community — "ROS2 Router: Service Call Request made, but no Response"](https://community.rti.com/forum-topic/ros2-router-service-call-request-made-no-response).
+
+### "Are actions failing because they're hidden? If we unhide them, will it work?"
+
+No.
+- `_action/*` naming is a **display-only filter** in ROS CLI tools; DDS doesn't care ([ROS 2 Actions Design](https://design.ros2.org/articles/actions.html)).
+- Foxglove already runs with `include_hidden: true` — the services do appear, the liveliness check still fails.
+- The `_action/*` prefix is hardcoded in `rclcpp_action::create_server()`; unhiding would require forking rclcpp and wouldn't change DDS behavior.
+
+---
+
+## Options
+
+### A — Swap ddsrouter for `zenoh_bridge_ros2dds`
+Replace the bridging layer with Eclipse Zenoh's ROS 2 plugin. Zenoh represents services/actions as **Queryables** and side-steps the DDS liveliness-matching problem.
+- **Pros:** native topic + service + action bridging; actively developed; OSRF's current recommendation for distributed ROS 2; same config works for same-host Docker and remote robots.
+- **Cons:** rip-and-replace of the whole bridging layer (compose + launch); new dep (`ros-jazzy-zenoh-bridge-ros2dds`); team has to learn it; cross-machine deploys need explicit `connect:` peer config.
+- **Refs:** [zenoh-plugin-ros2dds](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds) · [Jazzy docs](https://docs.ros.org/en/jazzy/p/zenoh_bridge_dds/) · [OSRF discourse](https://discourse.openrobotics.org/t/new-zenoh-bridge-for-ros-2/34163) · [Autoware writeup](https://autoware.org/driving-autoware-with-zenoh/) · [Humble tutorial](https://medium.com/@piliwilliam0306/use-zenoh-bridge-ros2dds-with-ros2-humble-459ab70ce9c7)
+
+### B — Add `ros2/domain_bridge` alongside ddsrouter
+Keep ddsrouter for topics; run a per-robot `domain_bridge` with an `actions:` block. Supposed to create a real `rclcpp_action::Server` on domain 0 that forwards to the robot.
+- **We tried this in-session.** Bridge ran, config interpolated, but `ros2 action info` still showed `Action servers: 0`. Reverted.
+- **Pros:** smallest config change, no new protocol.
+- **Cons:** action support is a [4-year-old open issue (#11)](https://github.com/ros2/domain_bridge/issues/11); services support similarly thin ([#10](https://github.com/ros2/domain_bridge/issues/10)); [design doc](https://github.com/ros2/domain_bridge/blob/main/doc/design.md) admits QoS/liveliness limits; silent failure mode makes it hard to debug.
+- **Refs:** [ros2/domain_bridge](https://github.com/ros2/domain_bridge) · [#11](https://github.com/ros2/domain_bridge/issues/11) · [#10](https://github.com/ros2/domain_bridge/issues/10) · [design doc](https://github.com/ros2/domain_bridge/blob/main/doc/design.md)
+
+### C — Custom per-robot action relay node
+One new ROS 2 package (`action_relay`). Per robot, one binary with two `rclcpp::Context` objects: a real `rclcpp_action::Server` on domain 0 and a real `rclcpp_action::Client` on domain N. Server forwards goals to client; feedback/result/cancel propagate through. ~150 lines of C++, templated over the 5 action types.
+- **Pros:** bulletproof — it *is* a real server on domain 0, so the liveliness check trivially passes; no new deps; additive (ddsrouter still handles topics/images); scales per-robot.
+- **Cons:** we maintain the code; new task executor → one line in a registry; careful two-executor / two-context threading; no widely-used package to vendor.
+- **Refs:** [ROS 2 Actions Design](https://design.ros2.org/articles/actions.html) · [rclcpp_action API](https://docs.ros.org/en/jazzy/p/rclcpp_action/) · [multi_robot_tf_relay (related pattern)](https://github.com/Gabryss/multi_robot_tf_relay) · [Jazzy action server tutorial](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html)
+
+---
+
+## Related reading
+
+- [eProsima DDS Router docs](https://eprosima-dds-router.readthedocs.io/) — what we use today
+- [ROS on DDS (design)](https://design.ros2.org/articles/ros_on_dds.html) — why services/actions use request/reply DDS topics
+- [ROS 2 QoS — Deadline, Liveliness, Lifespan](https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html) — explains `rcl_service_server_is_available` semantics
+- [Programming Multiple Robots with ROS 2 (OSRF)](https://osrf.github.io/ros2multirobotbook/) — currently recommends Zenoh for cross-domain
+- [Husarnet — Bridge Remote DDS Networks](https://husarnet.com/blog/ros2-dds-router)
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index 20e47441..b890dc07 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -1,178 +1,913 @@
-(()=>{
+(() => {
 "use strict";
 
-const BT_COMMANDS_DATATYPES = new Map([
-  [
-    "behavior_tree_msgs/msg/BehaviorTreeCommands",
-    {
-      definitions: [
-        {
-          name: "commands",
-          type: "behavior_tree_msgs/msg/BehaviorTreeCommand",
-          isArray: true,
-          isComplex: true,
-        },
-      ],
-    },
-  ],
-  [
-    "behavior_tree_msgs/msg/BehaviorTreeCommand",
-    {
-      definitions: [
-        { name: "condition_name", type: "string", isArray: false, isComplex: false },
-        { name: "status", type: "int8", isArray: false, isComplex: false },
-      ],
-    },
-  ],
+// ─────────────────────────── constants ────────────────────────────────────────
+
+const GOAL_STATUS = {
+  UNKNOWN: 0,
+  ACCEPTED: 1,
+  EXECUTING: 2,
+  CANCELING: 3,
+  SUCCEEDED: 4,
+  CANCELED: 5,
+  ABORTED: 6,
+};
+
+const TERMINAL_STATUSES = new Set([
+  GOAL_STATUS.SUCCEEDED,
+  GOAL_STATUS.CANCELED,
+  GOAL_STATUS.ABORTED,
 ]);
 
-const PRESET_COMMANDS = [
-  { label: "Arm & Takeoff",        value: "Auto Takeoff Commanded" },
-  { label: "Global Plan",          value: "Global Plan Commanded" },
-  { label: "Fixed Trajectory",     value: "Fixed Trajectory Commanded" },
-  { label: "Autonomously Explore", value: "Autonomously Explore Commanded" },
-  { label: "Pause",                value: "Pause Commanded" },
-  { label: "Rewind",               value: "Rewind Commanded" },
-  { label: "Land",                 value: "Land Commanded" },
-  { label: "Disarm",               value: "Disarm Commanded" },
-  { label: "Custom",               value: "__custom__" },
+const TRAJECTORY_DEFAULTS = {
+  Circle:    [["frame_id","base_link"],["radius","5.0"],["velocity","2.0"]],
+  Figure8:   [["frame_id","base_link"],["length","10.0"],["width","5.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
+  Racetrack: [["frame_id","base_link"],["length","20.0"],["width","10.0"],["height","0.0"],["velocity","2.0"],["turn_velocity","1.0"],["max_acceleration","1.0"]],
+  Line:      [["frame_id","base_link"],["length","10.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
+  Point:     [["frame_id","base_link"],["x","5.0"],["y","0.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
+  Lawnmower: [["frame_id","base_link"],["length","20.0"],["width","5.0"],["height","10.0"],["velocity","2.0"],["vertical","0"]],
+};
+
+// Shared altitude/speed field sets used across several area tasks.
+const altSpeedFields = [
+  { name: "min_altitude_agl",  kind: "float", default: 3.0,  min: 0,   max: 500, step: 0.1 },
+  { name: "max_altitude_agl",  kind: "float", default: 10.0, min: 0,   max: 500, step: 0.1 },
+  { name: "min_flight_speed",  kind: "float", default: 1.0,  min: 0,   max: 50,  step: 0.1 },
+  { name: "max_flight_speed",  kind: "float", default: 3.0,  min: 0,   max: 50,  step: 0.1 },
 ];
 
-const DEFAULT_STATE = {
-  robot: "robot_1",
-  command: "Auto Takeoff Commanded",
-  custom_command: "",
-  button_label: "Arm & Takeoff",
-  button_color: "#10B981",
-};
+// Tab registry: drives rendering, goal packing, and feedback formatting.
+// Each tab owns its default sub-state (built from `fields`) and a goal builder.
+const TASK_TABS = [
+  {
+    id: "takeoff",
+    label: "Takeoff",
+    actionSuffix: "tasks/takeoff",
+    fields: [
+      { name: "target_altitude_m", kind: "float", default: 10.0, min: 0, max: 500, step: 0.1 },
+      { name: "velocity_m_s",      kind: "float", default: 1.0,  min: 0, max: 50,  step: 0.1 },
+    ],
+    buildGoal: (s) => ({
+      target_altitude_m: numOr(s.target_altitude_m, 0),
+      velocity_m_s:      numOr(s.velocity_m_s, 0),
+    }),
+    formatFeedback: (fb) =>
+      `status: ${fb.status ?? ""} | alt: ${toFixed(fb.current_altitude_m)} / ${toFixed(fb.target_altitude_m)} m`,
+  },
+  {
+    id: "land",
+    label: "Land",
+    actionSuffix: "tasks/land",
+    fields: [
+      { name: "velocity_m_s", kind: "float", default: 0.3, min: 0, max: 10, step: 0.1, hint: "0 = use config default" },
+    ],
+    buildGoal: (s) => ({ velocity_m_s: numOr(s.velocity_m_s, 0) }),
+    formatFeedback: (fb) =>
+      `status: ${fb.status ?? ""} | alt: ${toFixed(fb.current_altitude_m)} m`,
+  },
+  {
+    id: "navigate",
+    label: "Navigate",
+    actionSuffix: "tasks/navigate",
+    fields: [
+      { name: "frame_id",         kind: "string", default: "map" },
+      { name: "waypoints",        kind: "path",   default: "[[0.0, 0.0, 5.0]]",
+        hint: "JSON array of [x, y, z] waypoints in the above frame" },
+      { name: "goal_tolerance_m", kind: "float",  default: 1.0, min: 0, max: 100, step: 0.1 },
+    ],
+    buildGoal: (s) => ({
+      global_plan: buildPath(s.frame_id, s.waypoints),
+      goal_tolerance_m: numOr(s.goal_tolerance_m, 0),
+    }),
+    formatFeedback: genericAreaFeedback,
+  },
+  {
+    id: "exploration",
+    label: "Exploration",
+    actionSuffix: "tasks/exploration",
+    fields: [
+      { name: "search_bounds", kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices ([] = unbounded)" },
+      ...altSpeedFields,
+      { name: "time_limit_sec", kind: "float", default: 120.0, min: 0, max: 86400, step: 1 },
+    ],
+    buildGoal: (s) => ({
+      search_bounds: buildPolygon(s.search_bounds),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      time_limit_sec:   numOr(s.time_limit_sec, 0),
+    }),
+    formatFeedback: genericAreaFeedback,
+  },
+  {
+    id: "coverage",
+    label: "Coverage",
+    actionSuffix: "tasks/coverage",
+    fields: [
+      { name: "coverage_area", kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices" },
+      ...altSpeedFields,
+      { name: "line_spacing_m", kind: "float", default: 5.0, min: 0.1, max: 1000, step: 0.1 },
+      { name: "heading_deg",    kind: "float", default: 0.0, min: 0,   max: 360,  step: 1 },
+    ],
+    buildGoal: (s) => ({
+      coverage_area: buildPolygon(s.coverage_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      line_spacing_m:   numOr(s.line_spacing_m, 0),
+      heading_deg:      numOr(s.heading_deg, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const cov = fb.coverage_percentage;
+      return cov != null ? `${base} | coverage: ${toFixed(cov, 1)}%` : base;
+    },
+  },
+  {
+    id: "object_search",
+    label: "Object Search",
+    actionSuffix: "tasks/object_search",
+    fields: [
+      { name: "object_class", kind: "string", default: "" },
+      { name: "search_area",  kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices" },
+      ...altSpeedFields,
+      { name: "time_limit_sec", kind: "float", default: 120.0, min: 0, max: 86400, step: 1 },
+      { name: "target_count",   kind: "int",   default: 1,     min: 0, max: 10000, step: 1,
+        hint: "0 = find all within area/time" },
+    ],
+    buildGoal: (s) => ({
+      object_class: String(s.object_class ?? ""),
+      search_area:  buildPolygon(s.search_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      time_limit_sec: numOr(s.time_limit_sec, 0),
+      target_count:   intOr(s.target_count, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const n = fb.objects_found_so_far;
+      return n != null ? `${base} | found: ${n}` : base;
+    },
+  },
+  {
+    id: "object_counting",
+    label: "Object Counting",
+    actionSuffix: "tasks/object_counting",
+    fields: [
+      { name: "object_class", kind: "string",  default: "" },
+      { name: "count_area",   kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices" },
+      ...altSpeedFields,
+    ],
+    buildGoal: (s) => ({
+      object_class: String(s.object_class ?? ""),
+      count_area:   buildPolygon(s.count_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const n = fb.current_count;
+      return n != null ? `${base} | count: ${n}` : base;
+    },
+  },
+  {
+    id: "semantic_search",
+    label: "Semantic Search",
+    actionSuffix: "tasks/semantic_search",
+    fields: [
+      { name: "query",              kind: "string", default: "" },
+      { name: "background_queries", kind: "string", default: "",
+        hint: "comma-separated contrast classes, e.g. building,tree,ground" },
+      { name: "search_area",        kind: "polygon", default: "[]",
+        hint: "JSON array of [x, y, z] vertices" },
+      { name: "min_altitude_agl",   kind: "float", default: 3.0,  min: 0, max: 500, step: 0.1 },
+      { name: "max_altitude_agl",   kind: "float", default: 15.0, min: 0, max: 500, step: 0.1 },
+      { name: "min_flight_speed",   kind: "float", default: 1.0,  min: 0, max: 50,  step: 0.1 },
+      { name: "max_flight_speed",   kind: "float", default: 3.0,  min: 0, max: 50,  step: 0.1 },
+      { name: "confidence_threshold", kind: "float", default: 0.95, min: 0, max: 1, step: 0.01 },
+    ],
+    buildGoal: (s) => ({
+      query: String(s.query ?? ""),
+      background_queries: String(s.background_queries ?? ""),
+      search_area: buildPolygon(s.search_area),
+      min_altitude_agl: numOr(s.min_altitude_agl, 0),
+      max_altitude_agl: numOr(s.max_altitude_agl, 0),
+      min_flight_speed: numOr(s.min_flight_speed, 0),
+      max_flight_speed: numOr(s.max_flight_speed, 0),
+      confidence_threshold: numOr(s.confidence_threshold, 0),
+    }),
+    formatFeedback: (fb) => {
+      const base = genericAreaFeedback(fb);
+      const c = fb.best_confidence_so_far;
+      return c != null ? `${base} | best conf: ${toFixed(c, 2)}` : base;
+    },
+  },
+  {
+    id: "fixed_trajectory",
+    label: "Fixed Trajectory",
+    actionSuffix: "tasks/fixed_trajectory",
+    fields: [],  // custom renderer
+    defaultState: () => ({
+      type: "Circle",
+      attributes: TRAJECTORY_DEFAULTS.Circle.map(([k,v]) => [k,v]),
+      loop: false,
+    }),
+    buildGoal: (s) => ({
+      trajectory_spec: {
+        type: s.type,
+        attributes: (s.attributes ?? []).map(([k,v]) => ({ key: String(k), value: String(v) })),
+      },
+      loop: Boolean(s.loop),
+    }),
+    formatFeedback: genericAreaFeedback,
+  },
+];
+
+function tabById(id) {
+  return TASK_TABS.find((t) => t.id === id) ?? TASK_TABS[0];
+}
+
+// ─────────────────────────── utilities ────────────────────────────────────────
+
+function numOr(v, d) { const n = Number(v); return Number.isFinite(n) ? n : d; }
+function intOr(v, d) { const n = parseInt(v, 10); return Number.isFinite(n) ? n : d; }
+function toFixed(v, n = 1) { const x = Number(v); return Number.isFinite(x) ? x.toFixed(n) : "0.0"; }
+
+function genericAreaFeedback(fb) {
+  const p = fb.current_position ?? {};
+  const prog = fb.progress != null ? ` | progress: ${toFixed(fb.progress, 2)}` : "";
+  return `status: ${fb.status ?? ""}${prog} | pos: (${toFixed(p.x)}, ${toFixed(p.y)}, ${toFixed(p.z)})`;
+}
+
+function parseJsonPoints(text) {
+  // Accepts "[[x,y,z], ...]" or "[]". Returns array of {x,y,z} or throws.
+  const parsed = JSON.parse(text);
+  if (!Array.isArray(parsed)) throw new Error("must be a JSON array");
+  return parsed.map((pt, i) => {
+    if (!Array.isArray(pt) || pt.length < 2) {
+      throw new Error(`point ${i} must be [x, y] or [x, y, z]`);
+    }
+    return { x: Number(pt[0]) || 0, y: Number(pt[1]) || 0, z: Number(pt[2]) || 0 };
+  });
+}
 
-function getConditionName(state) {
-  return state.command === "__custom__" ? state.custom_command : state.command;
+function buildPolygon(text) {
+  try {
+    const pts = parseJsonPoints(text ?? "[]");
+    return { points: pts.map((p) => ({ x: p.x, y: p.y, z: p.z })) };
+  } catch {
+    return { points: [] };
+  }
 }
 
+function buildPath(frameId, text) {
+  let pts = [];
+  try { pts = parseJsonPoints(text ?? "[]"); } catch { pts = []; }
+  const header = { stamp: { sec: 0, nanosec: 0 }, frame_id: String(frameId ?? "map") };
+  return {
+    header,
+    poses: pts.map((p) => ({
+      header,
+      pose: {
+        position: { x: p.x, y: p.y, z: p.z },
+        orientation: { x: 0, y: 0, z: 0, w: 1 },
+      },
+    })),
+  };
+}
+
+function randomUuidBytes() {
+  const bytes = new Uint8Array(16);
+  (globalThis.crypto ?? window.crypto).getRandomValues(bytes);
+  return Array.from(bytes);
+}
+
+function uuidEqual(a, b) {
+  if (!a || !b || a.length !== b.length) return false;
+  for (let i = 0; i < a.length; i++) {
+    if (a[i] !== b[i]) return false;
+  }
+  return true;
+}
+
+function actionTopic(robot, suffix, leaf) {
+  return `/${robot}/${suffix}/${leaf}`;
+}
+
+function statusLabel(code) {
+  switch (code) {
+    case GOAL_STATUS.ACCEPTED:  return "Accepted";
+    case GOAL_STATUS.EXECUTING: return "Running";
+    case GOAL_STATUS.CANCELING: return "Cancelling";
+    case GOAL_STATUS.SUCCEEDED: return "Succeeded";
+    case GOAL_STATUS.CANCELED:  return "Canceled";
+    case GOAL_STATUS.ABORTED:   return "Aborted";
+    default:                    return "Unknown";
+  }
+}
+
+function statusColor(code) {
+  switch (code) {
+    case GOAL_STATUS.SUCCEEDED: return "#16a34a";
+    case GOAL_STATUS.ABORTED:   return "#dc2626";
+    case GOAL_STATUS.CANCELED:  return "#ea580c";
+    case GOAL_STATUS.CANCELING: return "#ea580c";
+    default:                    return "#2563eb";
+  }
+}
+
+// Build the default sub-state for a tab from its field list (or explicit defaultState).
+function defaultTabState(tab) {
+  if (tab.defaultState) return tab.defaultState();
+  const s = {};
+  for (const f of tab.fields) s[f.name] = f.default;
+  return s;
+}
+
+// ─────────────────────────── panel ────────────────────────────────────────────
+
 function activate(extensionContext) {
   extensionContext.registerPanel({
-    name: "Robot Command Button",
+    name: "Robot Tasks",
     initPanel: (panelContext) => {
-      let state = Object.assign({}, DEFAULT_STATE, panelContext.initialState ?? {});
-      let advertisedTopic = null;
+      // Build default state.
+      const DEFAULT_STATE = { robot: "robot_1", activeTab: TASK_TABS[0].id };
+      for (const tab of TASK_TABS) DEFAULT_STATE[tab.id] = defaultTabState(tab);
+
+      // Merge persisted state (shallow per tab, so new fields added in code show up).
+      const persisted = panelContext.initialState ?? {};
+      const state = {
+        robot: persisted.robot ?? DEFAULT_STATE.robot,
+        activeTab: persisted.activeTab ?? DEFAULT_STATE.activeTab,
+      };
+      for (const tab of TASK_TABS) {
+        state[tab.id] = { ...DEFAULT_STATE[tab.id], ...(persisted[tab.id] ?? {}) };
+        // Deep-copy attributes array for fixed_trajectory to avoid sharing refs.
+        if (tab.id === "fixed_trajectory" && Array.isArray(state[tab.id].attributes)) {
+          state[tab.id].attributes = state[tab.id].attributes.map(([k,v]) => [k,v]);
+        }
+      }
+
+      // Runtime (not persisted)
+      const runtime = {
+        activeTabId: null,
+        activeGoalUuid: null,
+        feedbackLines: [],
+        resultText: "",
+        statusText: "Idle",
+        statusCode: GOAL_STATUS.UNKNOWN,
+        subscribedTopics: [],
+      };
+
+      const MAX_FEEDBACK_LINES = 100;
 
+      // ── DOM ───────────────────────────────────────────────────────────────
       const root = panelContext.panelElement;
       root.style.cssText =
-        "display:flex;align-items:center;justify-content:center;height:100%;padding:12px;box-sizing:border-box;";
-
-      const btn = document.createElement("button");
-      btn.style.cssText =
-        "width:100%;height:100%;min-height:48px;border:none;border-radius:6px;" +
-        "font-size:16px;font-weight:bold;color:white;cursor:pointer;transition:opacity 0.1s;";
-      btn.addEventListener("mousedown", () => (btn.style.opacity = "0.75"));
-      btn.addEventListener("mouseup", () => (btn.style.opacity = "1"));
-      btn.addEventListener("mouseleave", () => (btn.style.opacity = "1"));
-      btn.addEventListener("click", handleClick);
-      root.appendChild(btn);
-
-      function render() {
-        btn.textContent = state.button_label || "Button";
-        btn.style.backgroundColor = state.button_color || "#10B981";
-        panelContext.setDefaultPanelTitle(state.button_label || "Robot Command Button");
+        "display:flex;flex-direction:column;height:100%;box-sizing:border-box;padding:8px;gap:8px;font-family:sans-serif;color:inherit;overflow-y:auto;overflow-x:hidden;";
+
+      // Robot row
+      const robotRow = document.createElement("div");
+      robotRow.style.cssText = "display:flex;align-items:center;gap:6px;flex-shrink:0;";
+      const robotLabel = document.createElement("span");
+      robotLabel.textContent = "Robot:";
+      robotLabel.style.fontWeight = "bold";
+      const robotInput = document.createElement("input");
+      robotInput.type = "text";
+      robotInput.value = state.robot;
+      robotInput.style.cssText = "flex:1;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      robotInput.addEventListener("change", () => {
+        state.robot = robotInput.value.trim() || "robot_1";
+        persist();
+      });
+      robotRow.appendChild(robotLabel);
+      robotRow.appendChild(robotInput);
+      root.appendChild(robotRow);
+
+      // Tab bar (horizontally scrollable so many tabs fit)
+      const tabBar = document.createElement("div");
+      tabBar.style.cssText = "display:flex;gap:2px;border-bottom:1px solid #444;overflow-x:auto;flex-shrink:0;";
+      const tabButtons = {};
+      for (const tab of TASK_TABS) {
+        const b = document.createElement("button");
+        b.textContent = tab.label;
+        b.style.cssText =
+          "padding:8px 10px;border:none;background:transparent;color:inherit;cursor:pointer;font-size:13px;border-bottom:2px solid transparent;white-space:nowrap;";
+        b.addEventListener("click", () => {
+          state.activeTab = tab.id;
+          persist();
+          renderTabs();
+        });
+        tabButtons[tab.id] = b;
+        tabBar.appendChild(b);
       }
+      root.appendChild(tabBar);
 
-      function handleClick() {
-        const conditionName = getConditionName(state);
-        if (!conditionName) return;
-        const topic = `/${state.robot}/behavior/behavior_tree_commands`;
+      // Tab bodies
+      const tabBodies = {};
+      for (const tab of TASK_TABS) {
+        const body = document.createElement("div");
+        body.style.cssText = "display:flex;flex-direction:column;gap:6px;flex-shrink:0;";
 
-        if (advertisedTopic !== topic) {
-          if (advertisedTopic != null) {
-            panelContext.unadvertise(advertisedTopic);
+        if (tab.id === "fixed_trajectory") {
+          body.appendChild(buildFixedTrajectoryForm(state.fixed_trajectory, persist));
+        } else {
+          // Render simple fields first, then polygon/path (collapsible) at the bottom.
+          const isCollapsible = (f) => f.kind === "polygon" || f.kind === "path";
+          const simple  = tab.fields.filter((f) => !isCollapsible(f));
+          const complex = tab.fields.filter(isCollapsible);
+          for (const field of [...simple, ...complex]) {
+            body.appendChild(buildField(field, state[tab.id], persist));
           }
-          panelContext.advertise(topic, "behavior_tree_msgs/msg/BehaviorTreeCommands", {
-            datatypes: BT_COMMANDS_DATATYPES,
+        }
+
+        tabBodies[tab.id] = body;
+        root.appendChild(body);
+      }
+
+      // Feedback area
+      const feedbackLabel = document.createElement("div");
+      feedbackLabel.textContent = "Feedback";
+      feedbackLabel.style.cssText = "font-weight:bold;margin-top:4px;flex-shrink:0;";
+      const feedbackBox = document.createElement("div");
+      feedbackBox.style.cssText = "min-height:80px;max-height:180px;overflow-y:auto;background:rgba(0,0,0,0.2);border:1px solid #444;border-radius:4px;padding:6px;font-family:monospace;font-size:12px;white-space:pre-wrap;flex-shrink:0;";
+      root.appendChild(feedbackLabel);
+      root.appendChild(feedbackBox);
+
+      const resultLabel = document.createElement("div");
+      resultLabel.textContent = "Result";
+      resultLabel.style.cssText = "font-weight:bold;flex-shrink:0;";
+      const resultBox = document.createElement("div");
+      resultBox.style.cssText = "min-height:32px;background:rgba(0,0,0,0.2);border:1px solid #444;border-radius:4px;padding:6px;font-family:monospace;font-size:12px;white-space:pre-wrap;flex-shrink:0;";
+      root.appendChild(resultLabel);
+      root.appendChild(resultBox);
+
+      // Status + buttons
+      const statusRow = document.createElement("div");
+      statusRow.style.cssText = "display:flex;align-items:center;gap:8px;flex-shrink:0;";
+      const statusLbl = document.createElement("span");
+      statusLbl.style.cssText = "flex:1;font-weight:bold;";
+      const cancelBtn = document.createElement("button");
+      cancelBtn.textContent = "Cancel";
+      cancelBtn.style.cssText = "padding:8px 16px;border-radius:4px;border:none;background:#6b7280;color:white;cursor:pointer;";
+      const executeBtn = document.createElement("button");
+      executeBtn.textContent = "Execute";
+      executeBtn.style.cssText = "padding:8px 16px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-weight:bold;";
+      statusRow.appendChild(statusLbl);
+      statusRow.appendChild(cancelBtn);
+      statusRow.appendChild(executeBtn);
+      root.appendChild(statusRow);
+
+      // ── helpers ──────────────────────────────────────────────────────────
+      function persist() { panelContext.saveState(state); }
+
+      function renderTabs() {
+        for (const tab of TASK_TABS) {
+          const isActive = tab.id === state.activeTab;
+          tabBodies[tab.id].style.display = isActive ? "flex" : "none";
+          tabButtons[tab.id].style.borderBottomColor = isActive ? "#10b981" : "transparent";
+          tabButtons[tab.id].style.fontWeight = isActive ? "bold" : "normal";
+        }
+      }
+
+      function renderStatus() {
+        statusLbl.textContent = `Status: ${runtime.statusText}`;
+        statusLbl.style.color = statusColor(runtime.statusCode);
+        const inProgress = runtime.activeTabId != null && !TERMINAL_STATUSES.has(runtime.statusCode);
+        executeBtn.disabled = inProgress;
+        executeBtn.style.opacity = inProgress ? "0.5" : "1";
+        cancelBtn.disabled = !inProgress || runtime.activeTabId !== state.activeTab;
+        cancelBtn.style.opacity = cancelBtn.disabled ? "0.5" : "1";
+      }
+
+      function renderFeedback() {
+        feedbackBox.textContent = runtime.feedbackLines.join("\n");
+        feedbackBox.scrollTop = feedbackBox.scrollHeight;
+        resultBox.textContent = runtime.resultText;
+      }
+
+      function appendFeedback(line) {
+        const ts = new Date().toLocaleTimeString();
+        runtime.feedbackLines.push(`[${ts}] ${line}`);
+        if (runtime.feedbackLines.length > MAX_FEEDBACK_LINES) {
+          runtime.feedbackLines.splice(0, runtime.feedbackLines.length - MAX_FEEDBACK_LINES);
+        }
+        renderFeedback();
+      }
+
+      function resetRuntimeForNewGoal() {
+        runtime.feedbackLines = [];
+        runtime.resultText = "";
+        runtime.statusText = "Sending...";
+        runtime.statusCode = GOAL_STATUS.UNKNOWN;
+        renderFeedback();
+        renderStatus();
+      }
+
+      // ── subscriptions ────────────────────────────────────────────────────
+      function subscribeForActiveGoal(tab) {
+        const topics = [
+          actionTopic(state.robot, tab.actionSuffix, "_action/feedback"),
+          actionTopic(state.robot, tab.actionSuffix, "_action/status"),
+        ];
+        runtime.subscribedTopics = topics;
+        panelContext.subscribe(topics.map((topic) => ({ topic })));
+      }
+
+      function unsubscribeAll() {
+        runtime.subscribedTopics = [];
+        panelContext.subscribe([]);
+      }
+
+      // ── execute / cancel ─────────────────────────────────────────────────
+      executeBtn.addEventListener("click", async () => {
+        if (runtime.activeTabId != null) return;
+        const tab = tabById(state.activeTab);
+        let goal;
+        try {
+          goal = tab.buildGoal(state[tab.id]);
+        } catch (err) {
+          runtime.statusText = "Invalid goal";
+          runtime.statusCode = GOAL_STATUS.ABORTED;
+          runtime.resultText = `buildGoal failed: ${err?.message ?? err}`;
+          renderStatus();
+          renderFeedback();
+          return;
+        }
+        const goalId = randomUuidBytes();
+
+        runtime.activeTabId = tab.id;
+        runtime.activeGoalUuid = goalId;
+        resetRuntimeForNewGoal();
+        subscribeForActiveGoal(tab);
+
+        const sendGoalService = actionTopic(state.robot, tab.actionSuffix, "_action/send_goal");
+        try {
+          const response = await panelContext.callService(sendGoalService, {
+            goal_id: { uuid: goalId }, goal,
           });
-          advertisedTopic = topic;
+          if (!response?.accepted) {
+            runtime.statusText = "Rejected by server";
+            runtime.statusCode = GOAL_STATUS.ABORTED;
+            runtime.activeTabId = null;
+            runtime.activeGoalUuid = null;
+            unsubscribeAll();
+            renderStatus();
+            return;
+          }
+          runtime.statusText = statusLabel(GOAL_STATUS.ACCEPTED);
+          runtime.statusCode = GOAL_STATUS.ACCEPTED;
+          renderStatus();
+        } catch (err) {
+          runtime.statusText = "Send failed";
+          runtime.statusCode = GOAL_STATUS.ABORTED;
+          runtime.resultText = String(err?.message ?? err);
+          runtime.activeTabId = null;
+          runtime.activeGoalUuid = null;
+          unsubscribeAll();
+          renderStatus();
+          renderFeedback();
         }
+      });
 
-        // Send 2 (SUCCESS) for the selected command, 0 (IDLE) for all others.
-        // Matches rqt behaviour exactly.
-        const commands = [];
-        for (const preset of PRESET_COMMANDS) {
-          if (preset.value === "__custom__") continue;
-          commands.push({
-            condition_name: preset.value,
-            status: preset.value === conditionName ? 2 : 0,
+      cancelBtn.addEventListener("click", async () => {
+        if (runtime.activeGoalUuid == null) return;
+        const tab = tabById(runtime.activeTabId);
+        const cancelService = actionTopic(state.robot, tab.actionSuffix, "_action/cancel_goal");
+        runtime.statusText = statusLabel(GOAL_STATUS.CANCELING);
+        runtime.statusCode = GOAL_STATUS.CANCELING;
+        renderStatus();
+        try {
+          await panelContext.callService(cancelService, {
+            goal_info: { goal_id: { uuid: runtime.activeGoalUuid }, stamp: { sec: 0, nanosec: 0 } },
           });
+        } catch (err) {
+          appendFeedback(`cancel call failed: ${err?.message ?? err}`);
         }
-        if (state.command === "__custom__" && conditionName) {
-          commands.push({ condition_name: conditionName, status: 2 });
+      });
+
+      // ── incoming messages ────────────────────────────────────────────────
+      async function handleStatusMessage(msg) {
+        if (runtime.activeGoalUuid == null) return;
+        const list = msg?.status_list ?? [];
+        for (const entry of list) {
+          const uuid = entry?.goal_info?.goal_id?.uuid;
+          if (!uuid || !uuidEqual(uuid, runtime.activeGoalUuid)) continue;
+          const code = entry.status;
+          runtime.statusCode = code;
+          runtime.statusText = statusLabel(code);
+          renderStatus();
+
+          if (TERMINAL_STATUSES.has(code)) {
+            const tab = tabById(runtime.activeTabId);
+            const getResultService = actionTopic(state.robot, tab.actionSuffix, "_action/get_result");
+            try {
+              const res = await panelContext.callService(getResultService, {
+                goal_id: { uuid: runtime.activeGoalUuid },
+              });
+              const result = res?.result ?? {};
+              runtime.resultText = `success: ${result.success}\nmessage: ${result.message ?? ""}`;
+            } catch (err) {
+              runtime.resultText = `(get_result failed: ${err?.message ?? err})`;
+            }
+            runtime.activeTabId = null;
+            runtime.activeGoalUuid = null;
+            unsubscribeAll();
+            renderStatus();
+            renderFeedback();
+          }
         }
-        panelContext.publish(topic, { commands });
       }
 
-      function updateSettings() {
-        const isCustom = state.command === "__custom__";
-        const fields = {
-          button_label: {
-            label: "Label",
-            input: "string",
-            value: state.button_label,
-          },
-          button_color: {
-            label: "Color",
-            input: "rgb",
-            value: state.button_color,
-          },
-          robot: {
-            label: "Robot Name",
-            input: "string",
-            value: state.robot,
-            placeholder: "robot_1",
-          },
-          command: {
-            label: "Command",
-            input: "select",
-            value: state.command,
-            options: PRESET_COMMANDS,
-          },
-        };
-
-        if (isCustom) {
-          fields.custom_command = {
-            label: "Custom Command Name",
-            input: "string",
-            value: state.custom_command,
-            placeholder: "e.g. My Custom Commanded",
-          };
+      function handleFeedbackMessage(msg) {
+        if (runtime.activeGoalUuid == null) return;
+        const uuid = msg?.goal_id?.uuid;
+        if (!uuid || !uuidEqual(uuid, runtime.activeGoalUuid)) return;
+        const tab = tabById(runtime.activeTabId);
+        appendFeedback(tab.formatFeedback(msg.feedback ?? {}));
+      }
+
+      // ── render loop ──────────────────────────────────────────────────────
+      panelContext.onRender = (renderState, done) => {
+        const frame = renderState.currentFrame;
+        if (frame) {
+          for (const evt of frame) {
+            if (!runtime.subscribedTopics.includes(evt.topic)) continue;
+            if (evt.topic.endsWith("_action/status")) {
+              handleStatusMessage(evt.message);
+            } else if (evt.topic.endsWith("_action/feedback")) {
+              handleFeedbackMessage(evt.message);
+            }
+          }
         }
+        done();
+      };
+      panelContext.watch("currentFrame");
+      panelContext.watch("topics");
 
-        panelContext.updatePanelSettingsEditor({
-          actionHandler: (action) => {
-            if (action.action !== "update") return;
-            const key = action.payload.path[1];
-            state[key] = action.payload.value;
-            panelContext.saveState(state);
-            render();
-            updateSettings();
-          },
-          nodes: {
-            general: {
-              label: "Button",
-              fields,
+      panelContext.updatePanelSettingsEditor({
+        actionHandler: (action) => {
+          if (action.action !== "update") return;
+          const key = action.payload.path[1];
+          if (key === "robot") {
+            state.robot = String(action.payload.value || "robot_1");
+            robotInput.value = state.robot;
+            persist();
+          }
+        },
+        nodes: {
+          general: {
+            label: "Settings",
+            fields: {
+              robot: { label: "Robot Name", input: "string", value: state.robot, placeholder: "robot_1" },
             },
           },
-        });
-      }
+        },
+      });
 
-      render();
-      updateSettings();
+      panelContext.setDefaultPanelTitle("Robot Tasks");
+      renderTabs();
+      renderStatus();
+      renderFeedback();
 
-      return () => {
-        if (advertisedTopic != null) {
-          panelContext.unadvertise(advertisedTopic);
-        }
-      };
+      return () => { unsubscribeAll(); };
     },
   });
 }
 
+// ─────────────────────────── field builders ───────────────────────────────────
+
+function fieldRow(labelText, control, hint) {
+  const wrap = document.createElement("div");
+  wrap.style.cssText = "display:flex;flex-direction:column;gap:2px;";
+  const row = document.createElement("div");
+  row.style.cssText = "display:flex;align-items:center;gap:6px;";
+  const label = document.createElement("span");
+  label.textContent = labelText + ":";
+  label.style.cssText = "flex:1;";
+  row.appendChild(label);
+  row.appendChild(control);
+  wrap.appendChild(row);
+  if (hint) {
+    const h = document.createElement("span");
+    h.textContent = hint;
+    h.style.cssText = "font-size:11px;opacity:0.6;padding-left:4px;";
+    wrap.appendChild(h);
+  }
+  return wrap;
+}
+
+function buildField(field, tabState, persist) {
+  switch (field.kind) {
+    case "float":
+    case "int": {
+      const input = document.createElement("input");
+      input.type = "number";
+      input.step = field.kind === "int" ? "1" : String(field.step ?? 0.1);
+      if (field.min != null) input.min = String(field.min);
+      if (field.max != null) input.max = String(field.max);
+      input.value = String(tabState[field.name]);
+      input.style.cssText = "width:120px;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      input.addEventListener("change", () => {
+        const v = field.kind === "int" ? parseInt(input.value, 10) : Number(input.value);
+        tabState[field.name] = Number.isFinite(v) ? v : 0;
+        persist();
+      });
+      return fieldRow(field.name, input, field.hint);
+    }
+    case "string": {
+      const input = document.createElement("input");
+      input.type = "text";
+      input.value = String(tabState[field.name] ?? "");
+      input.style.cssText = "flex:1;min-width:120px;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      input.addEventListener("change", () => {
+        tabState[field.name] = input.value;
+        persist();
+      });
+      return fieldRow(field.name, input, field.hint);
+    }
+    case "bool": {
+      const cb = document.createElement("input");
+      cb.type = "checkbox";
+      cb.checked = Boolean(tabState[field.name]);
+      cb.addEventListener("change", () => {
+        tabState[field.name] = cb.checked;
+        persist();
+      });
+      return fieldRow(field.name, cb, field.hint);
+    }
+    case "polygon":
+    case "path": {
+      const openKey = `_${field.name}_open`;
+      const details = document.createElement("details");
+      details.style.cssText = "border:1px solid #444;border-radius:4px;padding:4px 6px;margin-top:2px;";
+      details.open = Boolean(tabState[openKey]);
+      const summary = document.createElement("summary");
+      summary.style.cssText = "cursor:pointer;user-select:none;font-size:13px;";
+      const countPoints = (text) => {
+        try {
+          const parsed = JSON.parse(text || "[]");
+          return Array.isArray(parsed) ? parsed.length : 0;
+        } catch { return null; }
+      };
+      const renderSummary = () => {
+        const n = countPoints(tabState[field.name]);
+        const label = n == null ? "invalid" : `${n} pt${n === 1 ? "" : "s"}`;
+        summary.textContent = `${field.name} (${label})`;
+      };
+      renderSummary();
+      details.appendChild(summary);
+
+      details.addEventListener("toggle", () => {
+        tabState[openKey] = details.open;
+        persist();
+      });
+
+      const ta = document.createElement("textarea");
+      ta.rows = 3;
+      ta.value = String(tabState[field.name] ?? "[]");
+      ta.style.cssText = "width:100%;box-sizing:border-box;margin-top:4px;padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-family:monospace;font-size:12px;resize:vertical;";
+      const err = document.createElement("span");
+      err.style.cssText = "display:block;font-size:11px;color:#dc2626;min-height:14px;";
+      const validate = () => {
+        try { JSON.parse(ta.value || "[]"); err.textContent = ""; }
+        catch (e) { err.textContent = "invalid JSON: " + (e?.message ?? e); }
+      };
+      ta.addEventListener("input", () => {
+        tabState[field.name] = ta.value;
+        validate();
+        renderSummary();
+        persist();
+      });
+      validate();
+      details.appendChild(ta);
+      if (field.hint) {
+        const h = document.createElement("span");
+        h.textContent = field.hint;
+        h.style.cssText = "display:block;font-size:11px;opacity:0.6;margin-top:2px;";
+        details.appendChild(h);
+      }
+      details.appendChild(err);
+      return details;
+    }
+  }
+  const fallback = document.createElement("div");
+  fallback.textContent = `(unsupported field kind: ${field.kind})`;
+  return fallback;
+}
+
+function buildFixedTrajectoryForm(tabState, persist) {
+  const wrapper = document.createElement("div");
+  wrapper.style.cssText = "display:flex;flex-direction:column;gap:6px;";
+
+  // Type combo
+  const typeRow = document.createElement("div");
+  typeRow.style.cssText = "display:flex;align-items:center;gap:6px;";
+  const typeLabel = document.createElement("span");
+  typeLabel.textContent = "type:";
+  typeLabel.style.flex = "1";
+  const typeSelect = document.createElement("select");
+  typeSelect.style.cssText = "padding:4px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+  for (const name of Object.keys(TRAJECTORY_DEFAULTS)) {
+    const opt = document.createElement("option");
+    opt.value = name;
+    opt.textContent = name;
+    if (name === tabState.type) opt.selected = true;
+    typeSelect.appendChild(opt);
+  }
+  typeRow.appendChild(typeLabel);
+  typeRow.appendChild(typeSelect);
+  wrapper.appendChild(typeRow);
+
+  // Attributes table
+  const table = document.createElement("div");
+  table.style.cssText = "display:flex;flex-direction:column;gap:2px;";
+  wrapper.appendChild(table);
+
+  function renderAttributes() {
+    table.replaceChildren();
+    tabState.attributes.forEach(([k, v], idx) => {
+      const row = document.createElement("div");
+      row.style.cssText = "display:flex;gap:4px;";
+      const keyIn = document.createElement("input");
+      keyIn.type = "text";
+      keyIn.value = k;
+      keyIn.placeholder = "key";
+      keyIn.style.cssText = "flex:1;padding:3px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      keyIn.addEventListener("change", () => {
+        tabState.attributes[idx][0] = keyIn.value;
+        persist();
+      });
+      const valIn = document.createElement("input");
+      valIn.type = "text";
+      valIn.value = v;
+      valIn.placeholder = "value";
+      valIn.style.cssText = "flex:1;padding:3px 6px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+      valIn.addEventListener("change", () => {
+        tabState.attributes[idx][1] = valIn.value;
+        persist();
+      });
+      const delBtn = document.createElement("button");
+      delBtn.textContent = "✕";
+      delBtn.style.cssText = "width:28px;border-radius:4px;border:none;background:#6b7280;color:white;cursor:pointer;";
+      delBtn.addEventListener("click", () => {
+        tabState.attributes.splice(idx, 1);
+        persist();
+        renderAttributes();
+      });
+      row.appendChild(keyIn);
+      row.appendChild(valIn);
+      row.appendChild(delBtn);
+      table.appendChild(row);
+    });
+  }
+  renderAttributes();
+
+  typeSelect.addEventListener("change", () => {
+    tabState.type = typeSelect.value;
+    tabState.attributes = (TRAJECTORY_DEFAULTS[tabState.type] ?? []).map(([k, v]) => [k, v]);
+    persist();
+    renderAttributes();
+  });
+
+  const addBtn = document.createElement("button");
+  addBtn.textContent = "+ Add Attribute";
+  addBtn.style.cssText = "align-self:flex-start;padding:4px 8px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;";
+  addBtn.addEventListener("click", () => {
+    tabState.attributes.push(["", ""]);
+    persist();
+    renderAttributes();
+  });
+  wrapper.appendChild(addBtn);
+
+  // Loop checkbox
+  const loopRow = document.createElement("label");
+  loopRow.style.cssText = "display:flex;align-items:center;gap:6px;cursor:pointer;";
+  const loopCb = document.createElement("input");
+  loopCb.type = "checkbox";
+  loopCb.checked = Boolean(tabState.loop);
+  loopCb.addEventListener("change", () => {
+    tabState.loop = loopCb.checked;
+    persist();
+  });
+  const loopLabel = document.createElement("span");
+  loopLabel.textContent = "loop";
+  loopRow.appendChild(loopCb);
+  loopRow.appendChild(loopLabel);
+  wrapper.appendChild(loopRow);
+
+  return wrapper;
+}
+
 module.exports = { activate };
 })();
diff --git a/gcs/foxglove_extensions/robot-commands/package.json b/gcs/foxglove_extensions/robot-commands/package.json
index ea84a6c2..24f071be 100644
--- a/gcs/foxglove_extensions/robot-commands/package.json
+++ b/gcs/foxglove_extensions/robot-commands/package.json
@@ -1,9 +1,9 @@
 {
   "name": "robot-commands",
-  "displayName": "Robot Command Button",
-  "description": "Configurable button that publishes a behavior_tree_msgs/BehaviorTreeCommands condition to a robot.",
+  "displayName": "Robot Tasks",
+  "description": "Tabbed panel to send goals to AirStack task executors (ROS 2 actions).",
   "publisher": "AirLab CMU",
-  "version": "1.0.0",
+  "version": "2.0.0",
   "license": "Apache-2.0",
   "main": "./dist/extension.js"
 }
diff --git a/gcs/foxglove_extensions/robot-commands/robot-commands.foxe b/gcs/foxglove_extensions/robot-commands/robot-commands.foxe
index 858ec6a58156576fdedee391a1b73a78588a4382..85b4b201837ca91e54c868437783b6f40f24879a 100644
GIT binary patch
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 361ba685..1de4c2fd 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -6,9 +6,9 @@
 All markers are published to /gcs/robot_markers in the global ENU 'map' frame.
 """
 
-import copy
 import re
 
+import numpy as np
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
@@ -16,8 +16,7 @@
 from nav_msgs.msg import Odometry, Path
 from visualization_msgs.msg import Marker, MarkerArray
 from builtin_interfaces.msg import Duration
-from geometry_msgs.msg import TransformStamped, Point
-from tf2_ros import StaticTransformBroadcaster
+from geometry_msgs.msg import Point
 
 from gcs_visualizer.gcs_utils import (
     gps_to_enu, multiply_quaternions, rotate_vector, transform_marker_array,
@@ -46,6 +45,26 @@
 AXIS_CORRECTION = (-0.5, -0.5, 0.5, 0.5)
 
 
+def _translate_marker(msg: Marker, bx: float, by: float, bz: float) -> Marker:
+    """Return a new Marker with all points translated by (bx, by, bz) via numpy."""
+    m = Marker()
+    m.type = msg.type
+    m.action = msg.action
+    m.pose = msg.pose
+    m.scale = msg.scale
+    m.color = msg.color
+    m.colors = list(msg.colors)
+    m.header.frame_id = 'map'
+    n = len(msg.points)
+    if n > 0:
+        xyz = np.array([(pt.x, pt.y, pt.z) for pt in msg.points], dtype=np.float64)
+        xyz[:, 0] += bx
+        xyz[:, 1] += by
+        xyz[:, 2] += bz
+        m.points = [Point(x=float(r[0]), y=float(r[1]), z=float(r[2])) for r in xyz]
+    return m
+
+
 class FoxgloveVisualizerNode(Node):
     def __init__(self):
         super().__init__('foxglove_visualizer_node')
@@ -64,6 +83,7 @@ def __init__(self):
         self._trajectories   = {}
         self._global_plans   = {}
         self._vdb_markers    = {}
+        self._vdb_global     = {}  # pre-translated global-frame VDB, keyed by robot_name
         self._subscribed_gps  = set()
         self._subscribed_odom = set()
         self._subscribed_traj = set()
@@ -73,14 +93,6 @@ def __init__(self):
 
         self._pub = self.create_publisher(MarkerArray, '/gcs/robot_markers', 10)
 
-        self._static_tf = StaticTransformBroadcaster(self)
-        map_tf = TransformStamped()
-        map_tf.header.stamp = self.get_clock().now().to_msg()
-        map_tf.header.frame_id = 'map'
-        map_tf.child_frame_id = 'enu_origin'
-        map_tf.transform.rotation.w = 1.0
-        self._static_tf.sendTransform(map_tf)
-
         self.create_timer(5.0, self._discover_robots)
         self.create_timer(0.1, self._publish_markers)
         self._discover_robots()
@@ -156,6 +168,10 @@ def _gps_callback(self, msg: NavSatFix, robot_name: str):
         self._gps_positions[robot_name] = pos
         if robot_name not in self._gps_boot:
             self._gps_boot[robot_name] = pos
+            if robot_name in self._vdb_markers:
+                bx, by, bz = pos
+                self._vdb_global[robot_name] = _translate_marker(
+                    self._vdb_markers[robot_name], bx, by, bz)
 
     def _odom_callback(self, msg: Odometry, robot_name: str):
         o = msg.pose.pose.orientation
@@ -169,6 +185,10 @@ def _plan_callback(self, msg: Path, robot_name: str):
 
     def _vdb_callback(self, msg: Marker, robot_name: str):
         self._vdb_markers[robot_name] = msg
+        boot = self._gps_boot.get(robot_name)
+        if boot is not None:
+            bx, by, bz = boot
+            self._vdb_global[robot_name] = _translate_marker(msg, bx, by, bz)
 
     def _publish_markers(self):
         if not self._gps_positions:
@@ -281,40 +301,49 @@ def _publish_markers(self):
             plan = self._global_plans.get(robot_name)
             if plan is not None and boot is not None:
                 bx, by, bz = boot
-                color = ROBOT_COLORS[i % len(ROBOT_COLORS)]
-                line = Marker()
-                line.header.frame_id = 'map'
-                line.header.stamp = now
-                line.ns = f'{robot_name}_global_plan'
-                line.id = i * 10000 + 9999
-                line.type = Marker.LINE_STRIP
-                line.action = Marker.ADD
-                line.pose.orientation.w = 1.0
-                line.scale.x = 0.1
-                line.color.r = color[0]
-                line.color.g = color[1]
-                line.color.b = color[2]
-                line.color.a = 0.8
-                line.lifetime = Duration(sec=2, nanosec=0)
-                for pose_stamped in plan.poses:
-                    p = pose_stamped.pose.position
-                    line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
-                if len(line.points) >= 2:
+                poses = plan.poses
+                n_poses = len(poses)
+                if n_poses >= 2:
+                    color = ROBOT_COLORS[i % len(ROBOT_COLORS)]
+                    xyz = np.array(
+                        [(ps.pose.position.x, ps.pose.position.y, ps.pose.position.z)
+                         for ps in poses], dtype=np.float64)
+                    xyz[:, 0] += bx
+                    xyz[:, 1] += by
+                    xyz[:, 2] += bz
+                    line = Marker()
+                    line.header.frame_id = 'map'
+                    line.header.stamp = now
+                    line.ns = f'{robot_name}_global_plan'
+                    line.id = i * 10000 + 9999
+                    line.type = Marker.LINE_STRIP
+                    line.action = Marker.ADD
+                    line.pose.orientation.w = 1.0
+                    line.scale.x = 0.1
+                    line.color.r = color[0]
+                    line.color.g = color[1]
+                    line.color.b = color[2]
+                    line.color.a = 0.8
+                    line.lifetime = Duration(sec=2, nanosec=0)
+                    line.points = [Point(x=float(r[0]), y=float(r[1]), z=float(r[2]))
+                                   for r in xyz]
                     array.markers.append(line)
 
-            vdb = self._vdb_markers.get(robot_name)
-            if vdb is not None and boot is not None:
-                bx, by, bz = boot
-                m = copy.deepcopy(vdb)
+            vdb = self._vdb_global.get(robot_name)
+            if vdb is not None:
+                m = Marker()
                 m.header.frame_id = 'map'
                 m.header.stamp = now
                 m.ns = f'{robot_name}_vdb'
                 m.id = i * 10000 + 9998
+                m.type = vdb.type
+                m.action = vdb.action
+                m.pose = vdb.pose
+                m.scale = vdb.scale
+                m.color = vdb.color
+                m.colors = vdb.colors
+                m.points = vdb.points  # pre-translated, safe to share read-only
                 m.lifetime = Duration(sec=2, nanosec=0)
-                for pt in m.points:
-                    pt.x += bx
-                    pt.y += by
-                    pt.z += bz
                 array.markers.append(m)
 
         self._pub.publish(array)
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml
index 423554be..326e2225 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/domain_bridge.yaml
@@ -41,9 +41,4 @@ topics:
   bag_record/set_recording_status:
     type: std_msgs/msg/Bool
     from_domain: $(var gcs_domain)
-    to_domain: $(env ROS_DOMAIN_ID)
-
-  /robot_1/current_target:
-    type: std_msgs/msg/String
-    from_domain: 1
-    to_domain: 2
\ No newline at end of file
+    to_domain: $(env ROS_DOMAIN_ID)
\ No newline at end of file
diff --git a/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml b/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
index 8795e894..a2cb2a1d 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
+++ b/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
@@ -2,8 +2,12 @@ raven_nav:
   ros__parameters:
     timer_period: 0.5       # seconds between behavior loop iterations
     score_threshold: 0.95   # minimum softmax sim score to consider a ray
-    # Must match the query order in RayFronts' queries.txt
+    min_altitude_agl: 3.0   # filter rays/frontiers below this altitude (m)
+    max_altitude_agl: 15.0  # filter rays/frontiers above this altitude (m)
+    # Must match the query order registered in RayFronts (target + background)
     query_labels:
       - 'red building'
       - 'water tower'
       - 'radio tower'
+    # Which of the above to actually navigate toward (empty = all query_labels)
+    target_labels: []
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
index 1493a241..8a3ae254 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
@@ -4,13 +4,18 @@
 
 
 class BehaviorManager:
-    def __init__(self, get_clock, publisher_dict, score_threshold=0.95):
+    def __init__(self, get_clock, publisher_dict, score_threshold=0.95,
+                 min_altitude=1.5, max_altitude=100.0):
         self.behavior_mode = 'Frontier-based'
         self.get_clock = get_clock
-        self.frontier_behavior = FrontierBehavior(self.get_clock)
+        self.frontier_behavior = FrontierBehavior(self.get_clock,
+                                                   min_altitude=min_altitude,
+                                                   max_altitude=max_altitude)
         current_target_pub = publisher_dict.get('current_target')
         self.ray_behavior = RayBehavior(self.get_clock, current_target_pub,
-                                        score_threshold=score_threshold)
+                                        score_threshold=score_threshold,
+                                        min_altitude=min_altitude,
+                                        max_altitude=max_altitude)
         self.voxel_behavior = VoxelBehavior(self.get_clock)
         # Priority: Voxel (navigate to confirmed detections) > Ray > Frontier
         self.behaviors = [self.voxel_behavior, self.ray_behavior, self.frontier_behavior]
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
index 08691d8d..4c3dd5a5 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
@@ -9,9 +9,11 @@
 
 
 class FrontierBehavior:
-    def __init__(self, get_clock):
+    def __init__(self, get_clock, min_altitude=1.5, max_altitude=100.0):
         self.get_clock = get_clock
         self.name = 'Frontier-based'
+        self.min_altitude = min_altitude
+        self.max_altitude = max_altitude
 
     def condition_check(self):
         return True
@@ -32,7 +34,8 @@ def execute(self, frontiers_raw, cur_pose_np, waypoint_locked, target_waypoint,
             -frontiers_raw[:, 1],
         ], axis=1)
 
-        frontiers = frontiers[frontiers[:, 2] > 1.5]
+        alt_mask = (frontiers[:, 2] >= self.min_altitude) & (frontiers[:, 2] <= self.max_altitude)
+        frontiers = frontiers[alt_mask]
         if frontiers.shape[0] == 0:
             return waypoint_locked, target_waypoint, target_waypoint2
 
@@ -43,7 +46,7 @@ def execute(self, frontiers_raw, cur_pose_np, waypoint_locked, target_waypoint,
         for l in unique_labels:
             cluster_pts = frontiers[labels == l]
             centroid = cluster_pts.mean(axis=0)
-            if centroid[2] > 4.0 and centroid[2] < 10.0:
+            if self.min_altitude <= centroid[2] <= self.max_altitude:
                 viewpoints.append(centroid)
 
         if len(viewpoints) == 0:
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
index 90815a4b..c0f5f026 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
@@ -10,10 +10,13 @@
 
 
 class RayBehavior:
-    def __init__(self, get_clock, current_target_publisher=None, score_threshold=0.95):
+    def __init__(self, get_clock, current_target_publisher=None, score_threshold=0.95,
+                 min_altitude=1.5, max_altitude=100.0):
         self.get_clock = get_clock
         self.name = 'Ray-based'
         self.score_threshold = score_threshold
+        self.min_altitude = min_altitude
+        self.max_altitude = max_altitude
         self.prev_filtered_marker_ids = 0
         self.current_target = None
         self.current_target_pub = current_target_publisher
@@ -45,6 +48,7 @@ def condition_check(self, ray_origins, ray_dirs, ray_scores,
             return False
 
         relevant_scores = ray_scores[:, label_indices]          # (N, num_targets)
+
         mask = (relevant_scores > self.score_threshold).any(axis=1)
         indices = np.where(mask)[0]
 
@@ -80,61 +84,41 @@ def execute(self, cur_pose_np, waypoint_locked, target_waypoint1,
         dirs = self._ray_dirs[self._filtered_indices]          # (K, 3) FLU
         labels = self._per_ray_label
 
-        # --- Filter rays pointing behind the robot in XY ---
+        # Filter rays outside altitude range (z is altitude in FLU)
+        alt_mask = (origins[:, 2] >= self.min_altitude) & (origins[:, 2] <= self.max_altitude)
+        origins = origins[alt_mask]
+        dirs = dirs[alt_mask]
+        labels = [labels[i] for i in range(len(labels)) if alt_mask[i]]
+
+        if len(origins) == 0:
+            return waypoint_locked, target_waypoint1, target_waypoint2
+
         xy_dirs = dirs[:, :2]
         norms = np.linalg.norm(xy_dirs, axis=1, keepdims=True)
         xy_dirs_normed = xy_dirs / (norms + 1e-6)
 
-        ray_target_xy = origins[:, :2] + xy_dirs_normed
-        to_target = ray_target_xy - cur_pose_np[:2]
-        dot = np.einsum('ij,ij->i', xy_dirs_normed, to_target)
+        # Filter rays pointing behind the robot in XY.
+        # Apply valid mask to all arrays so indices stay consistent (krrish-develop).
+        orig_xy = origins[:, :2]
+        ray_target_xy = orig_xy + xy_dirs_normed
+        to_ray_target = ray_target_xy - cur_pose_np[:2]
+        dot = np.einsum('ij,ij->i', xy_dirs_normed, to_ray_target)
         valid = dot > 0
 
-        _log.warning(
-            f'ray execute: score_filtered={len(origins)} '
-            f'direction_valid={int(valid.sum())} '
-            f'robot_xy=({cur_pose_np[0]:.1f},{cur_pose_np[1]:.1f})')
-
-        if not valid.any():
-            # All rays point behind the robot. Two cases:
-            # 1. Same target + same direction as current travel → already heading
-            #    correctly (e.g. drone passed the ray origin), keep going.
-            # 2. Same target + different direction, OR different target → stay
-            #    committed to active target, keep current waypoint.
-            behind_for_active = (self._active_target is None or
-                                 self.current_target == self._active_target)
-
-            same_direction = False
-            if target_waypoint1 is not None:
-                travel_xy = np.array([target_waypoint1[0], target_waypoint1[1]]) - cur_pose_np[:2]
-                travel_norm = np.linalg.norm(travel_xy)
-                if travel_norm > 1e-6:
-                    travel_dir = travel_xy / travel_norm
-                    avg_behind = xy_dirs_normed.mean(axis=0)
-                    avg_behind = avg_behind / (np.linalg.norm(avg_behind) + 1e-6)
-                    same_direction = float(np.dot(avg_behind, travel_dir)) > np.cos(np.deg2rad(45))
-
-            if behind_for_active and same_direction:
-                _log.warning(
-                    f'ray execute: all rays behind, consistent with active target '
-                    f'({self._active_target}) and direction — keeping waypoint')
-            else:
-                _log.warning(
-                    f'ray execute: all rays behind — staying committed to active '
-                    f'target ({self._active_target}, current={self.current_target})')
-            return waypoint_locked, target_waypoint1, target_waypoint2
+        xy_dirs_normed = xy_dirs_normed[valid]
+        origins = origins[valid]
+        dirs = dirs[valid]
+        labels = [labels[i] for i in range(len(labels)) if valid[i]]
 
-        origins_v = origins[valid]
-        dirs_v = dirs[valid]
-        xy_dirs_v = xy_dirs_normed[valid]
-        labels_v = [labels[i] for i in range(len(labels)) if valid[i]]
+        if len(origins) == 0:
+            return waypoint_locked, target_waypoint1, target_waypoint2
 
-        # --- Greedy spatial clustering per label (45 deg XY threshold) ---
+        # Greedy spatial clustering per label (45 deg XY threshold)
         angle_cos_thresh = np.cos(np.deg2rad(45))
         groups = []
-        for i in range(len(origins_v)):
-            xy_dir = xy_dirs_v[i]
-            label = labels_v[i]
+        for i in range(len(origins)):
+            xy_dir = xy_dirs_normed[i]
+            label = labels[i]
             assigned = False
             for group in groups:
                 if group['label'] != label:
@@ -154,20 +138,19 @@ def execute(self, cur_pose_np, waypoint_locked, target_waypoint1,
                     'label': label,
                 })
 
+        MIN_RAYS_PER_GROUP = 1
+        groups = [g for g in groups if len(g['rays']) >= MIN_RAYS_PER_GROUP]
+
         if not groups:
-            _log.warning('ray execute: no groups formed — no waypoint')
             return waypoint_locked, target_waypoint1, target_waypoint2
 
-        _log.warning(f'ray execute: {len(groups)} groups, publishing waypoint')
-
-        # --- Score groups: lower is better ---
-        # score = dist(avg_origin, robot) - 5 * group_size
+        # Score groups: lower is better (dist - 5 * density)
         k = 5.0
         group_avgs = []
         for group in groups:
             idxs = group['indices']
-            avg_origin = origins_v[idxs].mean(axis=0)
-            avg_dir = dirs_v[idxs].mean(axis=0)
+            avg_origin = origins[idxs].mean(axis=0)
+            avg_dir = dirs[idxs].mean(axis=0)
             avg_dir = avg_dir / (np.linalg.norm(avg_dir) + 1e-6)
             density = len(idxs)
             score = np.linalg.norm(avg_origin - cur_pose_np) - k * density
@@ -198,9 +181,8 @@ def execute(self, cur_pose_np, waypoint_locked, target_waypoint1,
         published_target = best[3]
         if self._active_target is None:
             self._active_target = published_target
-            _log.warning(f'ray execute: active target set to {published_target}')
 
-        self._visualize_filtered_rays(groups, origins_v, dirs_v, publisher_dict)
+        self._visualize_filtered_rays(groups, origins, dirs, publisher_dict)
 
         return waypoint_locked, target_waypoint1, target_waypoint2
 
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
index d3c75a0e..773c600f 100644
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
+++ b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
@@ -42,7 +42,14 @@ def __init__(self):
         query_labels_param = self.declare_parameter(
             'query_labels', ['red building', 'water tower', 'radio tower']).value
         self._query_labels = list(query_labels_param)
-        self._target_objects = self._query_labels[:]
+        # target_labels: which queries to actually navigate toward.
+        # Defaults to query_labels if empty. Must be a subset of query_labels.
+        target_labels_param = self.declare_parameter('target_labels', ['']).value
+        target_labels = [t for t in target_labels_param if t]
+        self._target_objects = target_labels if target_labels else self._query_labels[:]
+
+        self._min_altitude = self.declare_parameter('min_altitude_agl', 1.5).value
+        self._max_altitude = self.declare_parameter('max_altitude_agl', 100.0).value
 
         timer_period = self.declare_parameter('timer_period', 0.5).value
 
@@ -71,6 +78,8 @@ def __init__(self):
             get_clock=self.get_clock,
             publisher_dict=self._publisher_dict,
             score_threshold=self._score_threshold,
+            min_altitude=self._min_altitude,
+            max_altitude=self._max_altitude,
         )
 
         # Subscribe to all-queries topics so rayfronts always publishes to them
@@ -103,14 +112,24 @@ def __init__(self):
             f'raven_nav started | robot={robot_name} | '
             f'timer={timer_period:.2f}s | '
             f'query_labels={self._query_labels} | '
-            f'score_threshold={self._score_threshold}')
+            f'score_threshold={self._score_threshold} | '
+            f'altitude=[{self._min_altitude}, {self._max_altitude}]')
 
     def _ray_all_cb(self, msg: PointCloud2):
         """Receive all-queries ray PointCloud2 (fields: x,y,z,theta,phi,sim_0,sim_1,...)."""
         Q = len(self._query_labels)
         if Q == 0:
             return
-        fields = ('x', 'y', 'z', 'theta', 'phi') + tuple(f'sim_{q}' for q in range(Q))
+        # Discover actual sim_* fields from the message to handle query count mismatches
+        msg_field_names = [f.name for f in msg.fields]
+        sim_fields = sorted([f for f in msg_field_names if f.startswith('sim_')])
+        Q_actual = len(sim_fields)
+        if Q_actual == 0:
+            self._ray_origins = None
+            self._ray_dirs = None
+            self._ray_scores = None
+            return
+        fields = ('x', 'y', 'z', 'theta', 'phi') + tuple(sim_fields)
         pts = list(point_cloud2.read_points(msg, field_names=fields, skip_nans=True))
         if not pts:
             self._ray_origins = None
@@ -139,7 +158,15 @@ def _vox_all_cb(self, msg: PointCloud2):
         Q = len(self._query_labels)
         if Q == 0:
             return
-        fields = ('x', 'y', 'z') + tuple(f'sim_{q}' for q in range(Q))
+        # Discover actual sim_* fields from the message to handle query count mismatches
+        msg_field_names = [f.name for f in msg.fields]
+        sim_fields = sorted([f for f in msg_field_names if f.startswith('sim_')])
+        Q_actual = len(sim_fields)
+        if Q_actual == 0:
+            self._vox_xyz = None
+            self._vox_scores = None
+            return
+        fields = ('x', 'y', 'z') + tuple(sim_fields)
         pts = list(point_cloud2.read_points(msg, field_names=fields, skip_nans=True))
         if not pts:
             self._vox_xyz = None
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
index 8f17565e..9353ac5a 100644
--- a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
@@ -67,12 +67,18 @@ def _filter_raven(line: str) -> str | None:
     line = _clean(line)
     if not line:
         return None
+    low = line.lower()
+    # Surface startup, waiting, and error messages
+    if 'raven_nav started' in low:
+        return 'raven_nav started'
+    if 'waiting for odometry' in low:
+        return 'Waiting for odometry...'
+    if 'error' in low or 'exception' in low or 'traceback' in low:
+        return f'ERROR: {line}'
     # The periodic status line looks like:
     # [Frontier-based] frontiers=N rays=N filtered=N voxels=N | target=X | completed=[] | wp=(...)
     m = re.search(r'\[(Frontier-based|Ray-based|Voxel-based)\]', line)
     if not m:
-        if 'error' in line.lower():
-            return f'ERROR: {line}'
         return None
 
     mode = m.group(1)
@@ -80,13 +86,20 @@ def _filter_raven(line: str) -> str | None:
     rays = re.search(r'rays=(\d+)', line)
     voxels = re.search(r'voxels=(\d+)', line)
     completed = re.search(r'completed=(\[[^\]]*\])', line)
-    wp = re.search(r'wp=(\S+)', line)
+    wp = re.search(r'wp=(\([^)]+\))', line)
+
+    target = re.search(r'target=(.+?)\s*\|', line)
+    filtered = re.search(r'filtered=(\d+)', line)
 
     parts = [f'[{mode}]']
+    if target and target.group(1).strip() != 'None':
+        parts.append(f'current_target="{target.group(1).strip()}"')
     if frontiers:
         parts.append(f'frontiers={frontiers.group(1)}')
     if rays:
         parts.append(f'rays={rays.group(1)}')
+    if filtered:
+        parts.append(f'filtered={filtered.group(1)}')
     if voxels:
         parts.append(f'voxels={voxels.group(1)}')
     if completed and completed.group(1) != '[]':
@@ -235,7 +248,6 @@ def _send_exploration_task(self, robot_name: str) -> None:
         goal.max_altitude_agl = 15.0
         goal.min_flight_speed = 1.0
         goal.max_flight_speed = 3.0
-        goal.time_limit_sec = 0.0   # 0 = no limit, runs until cancelled
         client.send_goal_async(goal)
         self.get_logger().info('ExplorationTask sent to random_walk_planner')
 
@@ -259,7 +271,38 @@ def _execute(self, goal_handle):
             result.message = 'Empty query'
             return result
 
-        self.get_logger().info(f'SemanticSearchTask | queries={queries}')
+        bg_raw = [bq.strip() for bq in goal.background_queries.split(',')
+                  if bq.strip()]
+        if not bg_raw:
+            self._task_active = False
+            goal_handle.abort()
+            result = SemanticSearchTask.Result()
+            result.success = False
+            result.message = (
+                'background_queries is required but was not provided. '
+                'Softmax normalization needs contrast classes (e.g. '
+                '"building,tree,ground") to produce meaningful scores.')
+            return result
+
+        # Build the full query list: target queries + background contrast queries.
+        # Softmax normalization across queries requires N >= 2 to produce
+        # meaningful discriminative scores; with N=1 every score is 1.0.
+        bg = [bq for bq in bg_raw if bq not in queries]
+        all_queries = queries + bg
+
+        if len(all_queries) < 2:
+            self._task_active = False
+            goal_handle.abort()
+            result = SemanticSearchTask.Result()
+            result.success = False
+            result.message = (
+                f'Need at least 2 total queries for softmax normalization, '
+                f'got {len(all_queries)}. Add more background_queries that '
+                f'differ from the target query.')
+            return result
+
+        self.get_logger().info(
+            f'SemanticSearchTask | targets={queries} all_queries={all_queries}')
 
         rayfronts_proc = raven_proc = random_walk_proc = None
         rayfronts_q = raven_q = queue.Queue()
@@ -289,12 +332,18 @@ def _execute(self, goal_handle):
 
             # Start raven first so it's subscribed to rays_sim/all and voxels_sim/all
             # before rayfronts runs its first query cycle (rayfronts lazy-publishes
-            # those topics only when a subscriber exists)
-            labels_yaml = str(queries).replace("'", '"')
+            # those topics only when a subscriber exists).
+            # query_labels = full set (for sim column mapping)
+            # target_labels = only the user's targets (for behavior filtering)
+            all_labels_yaml = str(all_queries).replace("'", '"')
+            target_labels_yaml = str(queries).replace("'", '"')
             raven_proc, raven_q = self._spawn([
                 'ros2', 'run', 'raven_nav', 'raven_nav_node',
                 '--ros-args',
-                '-p', f'query_labels:={labels_yaml}',
+                '-p', f'query_labels:={all_labels_yaml}',
+                '-p', f'target_labels:={target_labels_yaml}',
+                '-p', f'min_altitude_agl:={goal.min_altitude_agl}',
+                '-p', f'max_altitude_agl:={goal.max_altitude_agl}',
                 '-r', (f'/{robot_name}/odometry:='
                        f'/{robot_name}/odometry_conversion/odometry'),
             ])
@@ -304,7 +353,6 @@ def _execute(self, goal_handle):
             ])
 
 
-            start = self.get_clock().now()
             best_conf = 0.0
             rayfronts_ready = False
             prev_rf_sub_count = 0
@@ -336,10 +384,13 @@ def _completed_targets_cb(msg):
                 _completed_targets_cb, 10, callback_group=self._cbg)
 
             # Subscribe to voxels_sim/all for best-confidence tracking
-            Q = len(queries)
             def _vox_all_cb(msg):
                 nonlocal best_conf, mapping_started
-                fields = ('x', 'y', 'z') + tuple(f'sim_{q}' for q in range(Q))
+                msg_field_names = [f.name for f in msg.fields]
+                sim_fields = sorted([f for f in msg_field_names if f.startswith('sim_')])
+                if not sim_fields:
+                    return
+                fields = ('x', 'y', 'z') + tuple(sim_fields)
                 from sensor_msgs_py import point_cloud2 as pc2
                 pts = list(pc2.read_points(msg, field_names=fields, skip_nans=True))
                 if pts:
@@ -361,22 +412,6 @@ def _vox_all_cb(msg):
                     result.message = 'Cancelled'
                     return result
 
-                elapsed = (self.get_clock().now() - start).nanoseconds / 1e9
-                if goal.time_limit_sec > 0 and elapsed >= goal.time_limit_sec:
-                    done = [q for q in queries
-                            if q.lower() in set(c.lower() for c in completed_targets)]
-                    succeeded = len(done) == len(queries)
-                    if succeeded:
-                        goal_handle.succeed()
-                    else:
-                        goal_handle.abort()
-                    result = SemanticSearchTask.Result()
-                    result.success = succeeded
-                    result.message = (f'Timed out. Visited {len(done)}/{len(queries)}: '
-                                      f'{", ".join(done)}')
-                    result.confidence = best_conf
-                    return result
-
                 # Drain and filter rayfronts output
                 for raw in _drain(rayfronts_q):
                     msg = _filter_rayfronts(raw)
@@ -400,14 +435,15 @@ def _vox_all_cb(msg):
 
                 # Send queries to rayfronts whenever its subscriber appears (or reappears).
                 # This handles the initial load AND any rayfronts restart mid-task.
+                # Send ALL queries (target + background) so softmax is meaningful.
                 rf_sub_count = self.count_subscribers(f'{self._rf_prefix}/new_text_query')
                 if rf_sub_count > 0 and prev_rf_sub_count == 0:
                     rayfronts_ready = True
-                    for q in queries:
+                    for q in all_queries:
                         self._text_query_pub.publish(String(data=q))
-                    last_rf_status = f'Queries sent: {", ".join(queries)}'
+                    last_rf_status = f'Queries sent: {", ".join(all_queries)}'
                     self.get_logger().info(
-                        f'Queries sent to rayfronts: {queries}')
+                        f'Queries sent to rayfronts: {all_queries}')
                 if rf_sub_count == 0 and prev_rf_sub_count > 0:
                     rayfronts_ready = False
                     self.get_logger().info('rayfronts subscriber lost — will resend on reconnect')
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index cfc01e60..8ac80223 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -70,7 +70,7 @@
 # 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.07
+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": 3.5},

From 1dcfd6c9cfb84c1592fe0dc591f0f8f399a03ed7 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Fri, 24 Apr 2026 17:37:28 -0400
Subject: [PATCH 41/55] foxglove panel with working executors

---
 .env                                          |   2 +-
 .../desktop_bringup/launch/gcs.launch.xml     |   3 +
 .../ros_packages/desktop_bringup/package.xml  |   1 +
 gcs/docker/gcs-base-docker-compose.yaml       |   2 +
 .../robot-commands/dist/extension.js          | 254 ++++++++-------
 .../src/action_relay/action_relay/__init__.py |   0
 .../action_relay/action_relay/relay_node.py   | 294 ++++++++++++++++++
 .../launch/action_relay.launch.py             |  28 ++
 gcs/ros_ws/src/action_relay/package.xml       |  26 ++
 .../src/action_relay/resource/action_relay    |   0
 gcs/ros_ws/src/action_relay/setup.cfg         |   4 +
 gcs/ros_ws/src/action_relay/setup.py          |  28 ++
 .../onboard_all/config/dds_router.yaml        |   3 +
 .../src/global/world_models/vdb_mapping_ros2  |   2 +-
 14 files changed, 531 insertions(+), 116 deletions(-)
 create mode 100644 gcs/ros_ws/src/action_relay/action_relay/__init__.py
 create mode 100644 gcs/ros_ws/src/action_relay/action_relay/relay_node.py
 create mode 100644 gcs/ros_ws/src/action_relay/launch/action_relay.launch.py
 create mode 100644 gcs/ros_ws/src/action_relay/package.xml
 create mode 100644 gcs/ros_ws/src/action_relay/resource/action_relay
 create mode 100644 gcs/ros_ws/src/action_relay/setup.cfg
 create mode 100644 gcs/ros_ws/src/action_relay/setup.py

diff --git a/.env b/.env
index a208d5f1..9de69a90 100644
--- a/.env
+++ b/.env
@@ -24,7 +24,7 @@ COMPOSE_PROFILES="desktop, isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="1"  # Number of robot containers to launch.
+NUM_ROBOTS="2"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== SIMULATION =====================
diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index 2cab8a2d..794d9bb6 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -35,6 +35,9 @@
   <!-- GCS Visualizer: drone mesh markers in global ENU frame -->
   <include file="$(find-pkg-share gcs_visualizer)/launch/gcs_visualizer.launch.xml" />
 
+  <!-- Action relay: bridges task actions from GCS (domain 0) to each robot (domain N) -->
+  <include file="$(find-pkg-share action_relay)/launch/action_relay.launch.py" />
+
   <!-- ===================== -->
 
 </launch>
diff --git a/common/ros_packages/desktop_bringup/package.xml b/common/ros_packages/desktop_bringup/package.xml
index 8476f6ae..d981954f 100644
--- a/common/ros_packages/desktop_bringup/package.xml
+++ b/common/ros_packages/desktop_bringup/package.xml
@@ -15,6 +15,7 @@
   <depend>urdf</depend>
   <depend>rclpy</depend>
   <depend>visualization_msgs</depend>
+  <depend>action_relay</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 962be52d..852011de 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -39,6 +39,8 @@ services:
       - DISPLAY=${DISPLAY}
       - QT_X11_NO_MITSHM=1
       - NVIDIA_DRIVER_CAPABILITIES=all
+      # Number of robots (used by action_relay to spawn per-robot relays)
+      - NUM_ROBOTS=${NUM_ROBOTS:-1}
       # Record bags
       - RECORD_BAGS=${RECORD_BAGS}
     image: ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_gcs
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index b890dc07..1070fe55 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -43,6 +43,7 @@ const TASK_TABS = [
     id: "takeoff",
     label: "Takeoff",
     actionSuffix: "tasks/takeoff",
+    goalSchema: "task_msgs/TakeoffTask_Goal",
     fields: [
       { name: "target_altitude_m", kind: "float", default: 10.0, min: 0, max: 500, step: 0.1 },
       { name: "velocity_m_s",      kind: "float", default: 1.0,  min: 0, max: 50,  step: 0.1 },
@@ -58,6 +59,7 @@ const TASK_TABS = [
     id: "land",
     label: "Land",
     actionSuffix: "tasks/land",
+    goalSchema: "task_msgs/LandTask_Goal",
     fields: [
       { name: "velocity_m_s", kind: "float", default: 0.3, min: 0, max: 10, step: 0.1, hint: "0 = use config default" },
     ],
@@ -69,6 +71,7 @@ const TASK_TABS = [
     id: "navigate",
     label: "Navigate",
     actionSuffix: "tasks/navigate",
+    goalSchema: "task_msgs/NavigateTask_Goal",
     fields: [
       { name: "frame_id",         kind: "string", default: "map" },
       { name: "waypoints",        kind: "path",   default: "[[0.0, 0.0, 5.0]]",
@@ -85,6 +88,7 @@ const TASK_TABS = [
     id: "exploration",
     label: "Exploration",
     actionSuffix: "tasks/exploration",
+    goalSchema: "task_msgs/ExplorationTask_Goal",
     fields: [
       { name: "search_bounds", kind: "polygon", default: "[]",
         hint: "JSON array of [x, y, z] vertices ([] = unbounded)" },
@@ -105,6 +109,7 @@ const TASK_TABS = [
     id: "coverage",
     label: "Coverage",
     actionSuffix: "tasks/coverage",
+    goalSchema: "task_msgs/CoverageTask_Goal",
     fields: [
       { name: "coverage_area", kind: "polygon", default: "[]",
         hint: "JSON array of [x, y, z] vertices" },
@@ -131,6 +136,7 @@ const TASK_TABS = [
     id: "object_search",
     label: "Object Search",
     actionSuffix: "tasks/object_search",
+    goalSchema: "task_msgs/ObjectSearchTask_Goal",
     fields: [
       { name: "object_class", kind: "string", default: "" },
       { name: "search_area",  kind: "polygon", default: "[]",
@@ -160,6 +166,7 @@ const TASK_TABS = [
     id: "object_counting",
     label: "Object Counting",
     actionSuffix: "tasks/object_counting",
+    goalSchema: "task_msgs/ObjectCountingTask_Goal",
     fields: [
       { name: "object_class", kind: "string",  default: "" },
       { name: "count_area",   kind: "polygon", default: "[]",
@@ -184,6 +191,7 @@ const TASK_TABS = [
     id: "semantic_search",
     label: "Semantic Search",
     actionSuffix: "tasks/semantic_search",
+    goalSchema: "task_msgs/SemanticSearchTask_Goal",
     fields: [
       { name: "query",              kind: "string", default: "" },
       { name: "background_queries", kind: "string", default: "",
@@ -216,6 +224,7 @@ const TASK_TABS = [
     id: "fixed_trajectory",
     label: "Fixed Trajectory",
     actionSuffix: "tasks/fixed_trajectory",
+    goalSchema: "task_msgs/FixedTrajectoryTask_Goal",
     fields: [],  // custom renderer
     defaultState: () => ({
       type: "Circle",
@@ -358,19 +367,27 @@ function activate(extensionContext) {
         }
       }
 
-      // Runtime (not persisted)
+      // Runtime (not persisted) — per-tab feedback/result/status
+      const perTab = {};
+      for (const tab of TASK_TABS) {
+        perTab[tab.id] = {
+          feedbackLines: [],
+          resultText: "",
+          statusText: "Idle",
+          statusCode: GOAL_STATUS.UNKNOWN,
+          active: false,
+        };
+      }
       const runtime = {
-        activeTabId: null,
-        activeGoalUuid: null,
-        feedbackLines: [],
-        resultText: "",
-        statusText: "Idle",
-        statusCode: GOAL_STATUS.UNKNOWN,
         subscribedTopics: [],
       };
 
       const MAX_FEEDBACK_LINES = 100;
 
+      // Helpers to get the active tab's per-tab state
+      function curTab() { return perTab[state.activeTab]; }
+      function anyActive() { return TASK_TABS.some((t) => perTab[t.id].active); }
+
       // ── DOM ───────────────────────────────────────────────────────────────
       const root = panelContext.panelElement;
       root.style.cssText =
@@ -477,164 +494,170 @@ function activate(extensionContext) {
           tabBodies[tab.id].style.display = isActive ? "flex" : "none";
           tabButtons[tab.id].style.borderBottomColor = isActive ? "#10b981" : "transparent";
           tabButtons[tab.id].style.fontWeight = isActive ? "bold" : "normal";
+          // Show active indicator on tab if it has a running goal
+          const pt = perTab[tab.id];
+          tabButtons[tab.id].style.color = pt.active ? "#10b981" : "inherit";
         }
+        renderStatus();
+        renderFeedback();
       }
 
       function renderStatus() {
-        statusLbl.textContent = `Status: ${runtime.statusText}`;
-        statusLbl.style.color = statusColor(runtime.statusCode);
-        const inProgress = runtime.activeTabId != null && !TERMINAL_STATUSES.has(runtime.statusCode);
-        executeBtn.disabled = inProgress;
-        executeBtn.style.opacity = inProgress ? "0.5" : "1";
-        cancelBtn.disabled = !inProgress || runtime.activeTabId !== state.activeTab;
+        const t = curTab();
+        statusLbl.textContent = `Status: ${t.statusText}`;
+        statusLbl.style.color = statusColor(t.statusCode);
+        // Disable execute if ANY tab has an active goal
+        const blocked = anyActive();
+        executeBtn.disabled = blocked;
+        executeBtn.style.opacity = blocked ? "0.5" : "1";
+        cancelBtn.disabled = !t.active;
         cancelBtn.style.opacity = cancelBtn.disabled ? "0.5" : "1";
       }
 
       function renderFeedback() {
-        feedbackBox.textContent = runtime.feedbackLines.join("\n");
+        const t = curTab();
+        feedbackBox.textContent = t.feedbackLines.join("\n");
         feedbackBox.scrollTop = feedbackBox.scrollHeight;
-        resultBox.textContent = runtime.resultText;
+        resultBox.textContent = t.resultText;
       }
 
-      function appendFeedback(line) {
+      function appendFeedback(line, tabId) {
+        const tid = tabId ?? state.activeTab;
+        const t = perTab[tid];
+        if (!t) return;
         const ts = new Date().toLocaleTimeString();
-        runtime.feedbackLines.push(`[${ts}] ${line}`);
-        if (runtime.feedbackLines.length > MAX_FEEDBACK_LINES) {
-          runtime.feedbackLines.splice(0, runtime.feedbackLines.length - MAX_FEEDBACK_LINES);
+        t.feedbackLines.push(`[${ts}] ${line}`);
+        if (t.feedbackLines.length > MAX_FEEDBACK_LINES) {
+          t.feedbackLines.splice(0, t.feedbackLines.length - MAX_FEEDBACK_LINES);
         }
-        renderFeedback();
+        // Only update display if this tab is currently visible
+        if (tid === state.activeTab) renderFeedback();
       }
 
       function resetRuntimeForNewGoal() {
-        runtime.feedbackLines = [];
-        runtime.resultText = "";
-        runtime.statusText = "Sending...";
-        runtime.statusCode = GOAL_STATUS.UNKNOWN;
+        const t = curTab();
+        t.feedbackLines = [];
+        t.resultText = "";
+        t.statusText = "Sending...";
+        t.statusCode = GOAL_STATUS.UNKNOWN;
         renderFeedback();
         renderStatus();
       }
 
-      // ── subscriptions ────────────────────────────────────────────────────
-      function subscribeForActiveGoal(tab) {
-        const topics = [
-          actionTopic(state.robot, tab.actionSuffix, "_action/feedback"),
-          actionTopic(state.robot, tab.actionSuffix, "_action/status"),
-        ];
+      // ── subscriptions (relay uses plain std_msgs/String topics) ────────
+      // Subscribe to ALL relay topics for all tabs so we catch results
+      // even when the user switches tabs during execution.
+      function rebuildSubscriptions() {
+        const topics = [];
+        for (const tab of TASK_TABS) {
+          if (perTab[tab.id].active) {
+            topics.push(`/${state.robot}/${tab.actionSuffix}/relay_feedback`);
+            topics.push(`/${state.robot}/${tab.actionSuffix}/relay_result`);
+          }
+        }
         runtime.subscribedTopics = topics;
         panelContext.subscribe(topics.map((topic) => ({ topic })));
       }
 
-      function unsubscribeAll() {
-        runtime.subscribedTopics = [];
-        panelContext.subscribe([]);
-      }
-
       // ── execute / cancel ─────────────────────────────────────────────────
       executeBtn.addEventListener("click", async () => {
-        if (runtime.activeTabId != null) return;
+        // Block if ANY tab has an active goal
+        if (anyActive()) return;
         const tab = tabById(state.activeTab);
+        const t = perTab[tab.id];
         let goal;
         try {
           goal = tab.buildGoal(state[tab.id]);
         } catch (err) {
-          runtime.statusText = "Invalid goal";
-          runtime.statusCode = GOAL_STATUS.ABORTED;
-          runtime.resultText = `buildGoal failed: ${err?.message ?? err}`;
+          t.statusText = "Invalid goal";
+          t.statusCode = GOAL_STATUS.ABORTED;
+          t.resultText = `buildGoal failed: ${err?.message ?? err}`;
           renderStatus();
           renderFeedback();
           return;
         }
-        const goalId = randomUuidBytes();
 
-        runtime.activeTabId = tab.id;
-        runtime.activeGoalUuid = goalId;
+        t.active = true;
         resetRuntimeForNewGoal();
-        subscribeForActiveGoal(tab);
+        rebuildSubscriptions();
 
-        const sendGoalService = actionTopic(state.robot, tab.actionSuffix, "_action/send_goal");
+        const goalTopic = `/${state.robot}/${tab.actionSuffix}/goal`;
         try {
-          const response = await panelContext.callService(sendGoalService, {
-            goal_id: { uuid: goalId }, goal,
-          });
-          if (!response?.accepted) {
-            runtime.statusText = "Rejected by server";
-            runtime.statusCode = GOAL_STATUS.ABORTED;
-            runtime.activeTabId = null;
-            runtime.activeGoalUuid = null;
-            unsubscribeAll();
-            renderStatus();
-            return;
-          }
-          runtime.statusText = statusLabel(GOAL_STATUS.ACCEPTED);
-          runtime.statusCode = GOAL_STATUS.ACCEPTED;
+          panelContext.advertise(goalTopic, "std_msgs/msg/String");
+          panelContext.publish(goalTopic, { data: JSON.stringify(goal) });
+          t.statusText = "Goal sent";
+          t.statusCode = GOAL_STATUS.EXECUTING;
+          appendFeedback(`Sent goal: ${JSON.stringify(goal)}`);
           renderStatus();
         } catch (err) {
-          runtime.statusText = "Send failed";
-          runtime.statusCode = GOAL_STATUS.ABORTED;
-          runtime.resultText = String(err?.message ?? err);
-          runtime.activeTabId = null;
-          runtime.activeGoalUuid = null;
-          unsubscribeAll();
+          t.statusText = "Publish failed";
+          t.statusCode = GOAL_STATUS.ABORTED;
+          t.resultText = String(err?.message ?? err);
+          t.active = false;
+          rebuildSubscriptions();
           renderStatus();
           renderFeedback();
         }
       });
 
       cancelBtn.addEventListener("click", async () => {
-        if (runtime.activeGoalUuid == null) return;
-        const tab = tabById(runtime.activeTabId);
-        const cancelService = actionTopic(state.robot, tab.actionSuffix, "_action/cancel_goal");
-        runtime.statusText = statusLabel(GOAL_STATUS.CANCELING);
-        runtime.statusCode = GOAL_STATUS.CANCELING;
-        renderStatus();
+        const t = curTab();
+        if (!t.active) return;
+        const tab = tabById(state.activeTab);
+        const cancelTopic = `/${state.robot}/${tab.actionSuffix}/cancel`;
         try {
-          await panelContext.callService(cancelService, {
-            goal_info: { goal_id: { uuid: runtime.activeGoalUuid }, stamp: { sec: 0, nanosec: 0 } },
-          });
+          panelContext.advertise(cancelTopic, "std_msgs/msg/String");
+          panelContext.publish(cancelTopic, { data: "cancel" });
+          appendFeedback("Cancel requested");
         } catch (err) {
-          appendFeedback(`cancel call failed: ${err?.message ?? err}`);
+          appendFeedback(`Cancel failed: ${err?.message ?? err}`);
         }
       });
 
-      // ── incoming messages ────────────────────────────────────────────────
-      async function handleStatusMessage(msg) {
-        if (runtime.activeGoalUuid == null) return;
-        const list = msg?.status_list ?? [];
-        for (const entry of list) {
-          const uuid = entry?.goal_info?.goal_id?.uuid;
-          if (!uuid || !uuidEqual(uuid, runtime.activeGoalUuid)) continue;
-          const code = entry.status;
-          runtime.statusCode = code;
-          runtime.statusText = statusLabel(code);
-          renderStatus();
+      // ── incoming messages (relay publishes std_msgs/String as JSON) ────
+      // Find which tab owns a given relay topic
+      function tabForTopic(topic) {
+        for (const tab of TASK_TABS) {
+          if (topic.includes(`/${tab.actionSuffix}/`)) return tab;
+        }
+        return null;
+      }
 
-          if (TERMINAL_STATUSES.has(code)) {
-            const tab = tabById(runtime.activeTabId);
-            const getResultService = actionTopic(state.robot, tab.actionSuffix, "_action/get_result");
-            try {
-              const res = await panelContext.callService(getResultService, {
-                goal_id: { uuid: runtime.activeGoalUuid },
-              });
-              const result = res?.result ?? {};
-              runtime.resultText = `success: ${result.success}\nmessage: ${result.message ?? ""}`;
-            } catch (err) {
-              runtime.resultText = `(get_result failed: ${err?.message ?? err})`;
-            }
-            runtime.activeTabId = null;
-            runtime.activeGoalUuid = null;
-            unsubscribeAll();
-            renderStatus();
-            renderFeedback();
-          }
+      function handleRelayFeedback(topic, msg) {
+        const tab = tabForTopic(topic);
+        if (!tab) return;
+        const t = perTab[tab.id];
+        if (!t.active) return;
+        const data = msg?.data;
+        if (!data) return;
+        try {
+          const fb = JSON.parse(data);
+          appendFeedback(tab.formatFeedback(fb), tab.id);
+        } catch {
+          appendFeedback(data, tab.id);
         }
       }
 
-      function handleFeedbackMessage(msg) {
-        if (runtime.activeGoalUuid == null) return;
-        const uuid = msg?.goal_id?.uuid;
-        if (!uuid || !uuidEqual(uuid, runtime.activeGoalUuid)) return;
-        const tab = tabById(runtime.activeTabId);
-        appendFeedback(tab.formatFeedback(msg.feedback ?? {}));
+      function handleRelayResult(topic, msg) {
+        const tab = tabForTopic(topic);
+        if (!tab) return;
+        const t = perTab[tab.id];
+        if (!t.active) return;
+        const data = msg?.data;
+        if (!data) return;
+        try {
+          const result = JSON.parse(data);
+          t.resultText = `success: ${result.success}\nmessage: ${result.message ?? ""}`;
+          t.statusText = result.success ? "Succeeded" : "Failed";
+          t.statusCode = result.success ? GOAL_STATUS.SUCCEEDED : GOAL_STATUS.ABORTED;
+        } catch {
+          t.resultText = data;
+          t.statusText = "Done";
+          t.statusCode = GOAL_STATUS.SUCCEEDED;
+        }
+        t.active = false;
+        rebuildSubscriptions();
+        renderTabs();
       }
 
       // ── render loop ──────────────────────────────────────────────────────
@@ -643,10 +666,10 @@ function activate(extensionContext) {
         if (frame) {
           for (const evt of frame) {
             if (!runtime.subscribedTopics.includes(evt.topic)) continue;
-            if (evt.topic.endsWith("_action/status")) {
-              handleStatusMessage(evt.message);
-            } else if (evt.topic.endsWith("_action/feedback")) {
-              handleFeedbackMessage(evt.message);
+            if (evt.topic.endsWith("/relay_feedback")) {
+              handleRelayFeedback(evt.topic, evt.message);
+            } else if (evt.topic.endsWith("/relay_result")) {
+              handleRelayResult(evt.topic, evt.message);
             }
           }
         }
@@ -680,7 +703,10 @@ function activate(extensionContext) {
       renderStatus();
       renderFeedback();
 
-      return () => { unsubscribeAll(); };
+      return () => {
+        runtime.subscribedTopics = [];
+        panelContext.subscribe([]);
+      };
     },
   });
 }
diff --git a/gcs/ros_ws/src/action_relay/action_relay/__init__.py b/gcs/ros_ws/src/action_relay/action_relay/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
new file mode 100644
index 00000000..9caa4cd6
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
@@ -0,0 +1,294 @@
+"""Action relay: bridges ROS 2 actions from GCS (domain 0) to a robot (domain N).
+
+foxglove_bridge drops nested message fields when calling action services via
+callService, so this relay uses plain String topics with JSON payloads instead.
+
+  Foxglove extension                     Robot (domain N)
+    publishes String JSON                  Task executor (ActionServer)
+    on .../goal                            unchanged
+         |                                      ^
+         v                                      |
+  action_relay (this node, domain 0+N)          |
+    subscribes .../goal (String)                |
+    parses JSON -> typed Goal msg               |
+    sends via ActionClient  --------------------+
+    publishes .../relay_feedback (String JSON)
+    publishes .../relay_result   (String JSON)
+
+Usage:
+    ros2 run action_relay action_relay_node \\
+        --ros-args -p robot_name:=robot_1 -p robot_domain:=1
+"""
+
+import json
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, Pose, Polygon, Point32, PoseStamped
+from nav_msgs.msg import Path
+from std_msgs.msg import Header, String
+
+from airstack_msgs.msg import FixedTrajectory
+from diagnostic_msgs.msg import KeyValue
+from task_msgs.action import (
+    TakeoffTask, LandTask, NavigateTask,
+    FixedTrajectoryTask, SemanticSearchTask,
+)
+
+# ── Registry ─────────────────────────────────────────────────────────────────
+RELAYS = [
+    ('takeoff',          TakeoffTask),
+    ('land',             LandTask),
+    ('navigate',         NavigateTask),
+    ('fixed_trajectory', FixedTrajectoryTask),
+    ('semantic_search',  SemanticSearchTask),
+]
+
+_RELIABLE_QOS = QoSProfile(
+    depth=10,
+    reliability=ReliabilityPolicy.RELIABLE,
+    durability=DurabilityPolicy.VOLATILE,
+)
+
+
+# ── Goal builders ────────────────────────────────────────────────────────────
+# Each converts a JSON dict into a typed Goal message.
+
+def _build_takeoff_goal(d):
+    g = TakeoffTask.Goal()
+    g.target_altitude_m = float(d.get('target_altitude_m', 0))
+    g.velocity_m_s = float(d.get('velocity_m_s', 0))
+    return g
+
+
+def _build_land_goal(d):
+    g = LandTask.Goal()
+    g.velocity_m_s = float(d.get('velocity_m_s', 0))
+    return g
+
+
+def _build_navigate_goal(d):
+    g = NavigateTask.Goal()
+    g.goal_tolerance_m = float(d.get('goal_tolerance_m', 1.0))
+    plan_data = d.get('global_plan', {})
+    header = Header()
+    hdr = plan_data.get('header', {})
+    header.frame_id = str(hdr.get('frame_id', 'map'))
+    stamp = hdr.get('stamp', {})
+    header.stamp = Time(sec=int(stamp.get('sec', 0)),
+                        nanosec=int(stamp.get('nanosec', 0)))
+    path = Path(header=header)
+    for pose_data in plan_data.get('poses', []):
+        ps = PoseStamped()
+        ps.header = header
+        pos = pose_data.get('pose', {}).get('position', {})
+        ori = pose_data.get('pose', {}).get('orientation', {})
+        ps.pose.position = Point(
+            x=float(pos.get('x', 0)), y=float(pos.get('y', 0)),
+            z=float(pos.get('z', 0)))
+        ps.pose.orientation.x = float(ori.get('x', 0))
+        ps.pose.orientation.y = float(ori.get('y', 0))
+        ps.pose.orientation.z = float(ori.get('z', 0))
+        ps.pose.orientation.w = float(ori.get('w', 1))
+        path.poses.append(ps)
+    g.global_plan = path
+    return g
+
+
+def _build_fixed_trajectory_goal(d):
+    g = FixedTrajectoryTask.Goal()
+    g.loop = bool(d.get('loop', False))
+    spec_data = d.get('trajectory_spec', {})
+    spec = FixedTrajectory()
+    spec.type = str(spec_data.get('type', ''))
+    for attr in spec_data.get('attributes', []):
+        kv = KeyValue()
+        kv.key = str(attr.get('key', ''))
+        kv.value = str(attr.get('value', ''))
+        spec.attributes.append(kv)
+    g.trajectory_spec = spec
+    return g
+
+
+def _build_semantic_search_goal(d):
+    g = SemanticSearchTask.Goal()
+    g.query = str(d.get('query', ''))
+    g.background_queries = str(d.get('background_queries', ''))
+    g.min_altitude_agl = float(d.get('min_altitude_agl', 3.0))
+    g.max_altitude_agl = float(d.get('max_altitude_agl', 15.0))
+    g.min_flight_speed = float(d.get('min_flight_speed', 0))
+    g.max_flight_speed = float(d.get('max_flight_speed', 0))
+    g.confidence_threshold = float(d.get('confidence_threshold', 0.95))
+    # search_area polygon
+    area = d.get('search_area', {})
+    poly = Polygon()
+    for pt in area.get('points', []):
+        poly.points.append(Point32(
+            x=float(pt.get('x', 0)), y=float(pt.get('y', 0)),
+            z=float(pt.get('z', 0))))
+    g.search_area = poly
+    return g
+
+
+_GOAL_BUILDERS = {
+    'takeoff':          _build_takeoff_goal,
+    'land':             _build_land_goal,
+    'navigate':         _build_navigate_goal,
+    'fixed_trajectory': _build_fixed_trajectory_goal,
+    'semantic_search':  _build_semantic_search_goal,
+}
+
+
+# ── Relay wiring ─────────────────────────────────────────────────────────────
+
+def _make_relay(node0, nodeN, executorN, topic, suffix, action_type, robot_domain):
+    """Wire up goal subscriber (domain 0) + action client (domain N)."""
+
+    client = ActionClient(nodeN, action_type, topic)
+    cbg = ReentrantCallbackGroup()
+
+    feedback_pub = node0.create_publisher(String, f'{topic}/relay_feedback', _RELIABLE_QOS)
+    result_pub = node0.create_publisher(String, f'{topic}/relay_result', _RELIABLE_QOS)
+
+    goal_builder = _GOAL_BUILDERS[suffix]
+    active_goal = {'handle': None}
+
+    def on_goal_str(msg):
+        """Receive JSON goal from Foxglove, parse, forward as action."""
+        try:
+            data = json.loads(msg.data)
+        except json.JSONDecodeError as e:
+            node0.get_logger().error(f'[relay] {topic}: bad JSON: {e}')
+            _publish_result(False, f'Invalid JSON: {e}')
+            return
+
+        goal_msg = goal_builder(data)
+        node0.get_logger().info(f'[relay] {topic}: goal: {goal_msg}')
+
+        if not client.wait_for_server(timeout_sec=5.0):
+            node0.get_logger().warn(f'[relay] {topic}: robot server not available')
+            _publish_result(False, 'Robot action server not available')
+            return
+
+        send_future = client.send_goal_async(
+            goal_msg,
+            feedback_callback=_on_feedback,
+        )
+
+        while not send_future.done():
+            executorN.spin_once(timeout_sec=0.05)
+
+        robot_goal_handle = send_future.result()
+        if not robot_goal_handle.accepted:
+            node0.get_logger().warn(f'[relay] {topic}: robot rejected goal')
+            _publish_result(False, 'Robot rejected goal')
+            return
+
+        active_goal['handle'] = robot_goal_handle
+        node0.get_logger().info(f'[relay] {topic}: goal accepted')
+        feedback_pub.publish(String(data='Goal accepted by robot'))
+
+        result_future = robot_goal_handle.get_result_async()
+        while not result_future.done():
+            executorN.spin_once(timeout_sec=0.05)
+
+        active_goal['handle'] = None
+        wrapped = result_future.result()
+        robot_result = wrapped.result
+        status = wrapped.status
+        status_str = {4: 'SUCCEEDED', 5: 'CANCELED'}.get(status, 'ABORTED')
+        node0.get_logger().info(f'[relay] {topic}: {status_str}')
+
+        success = getattr(robot_result, 'success', status == 4)
+        message = getattr(robot_result, 'message', status_str)
+        _publish_result(success, message)
+
+    def on_cancel(msg):
+        """Cancel the active goal on the robot."""
+        gh = active_goal.get('handle')
+        if gh is None:
+            node0.get_logger().warn(f'[relay] {topic}: cancel requested but no active goal')
+            return
+        node0.get_logger().info(f'[relay] {topic}: forwarding cancel to robot')
+        gh.cancel_goal_async()
+
+    def _on_feedback(fb_msg):
+        fb = fb_msg.feedback
+        fields = fb.get_fields_and_field_types()
+        fb_dict = {}
+        for name in fields:
+            val = getattr(fb, name)
+            # Convert ROS message types to dicts for JSON
+            if hasattr(val, 'get_fields_and_field_types'):
+                sub = {}
+                for sn in val.get_fields_and_field_types():
+                    sub[sn] = getattr(val, sn)
+                fb_dict[name] = sub
+            else:
+                fb_dict[name] = val
+        feedback_pub.publish(String(data=json.dumps(fb_dict, default=str)))
+
+    def _publish_result(success, message):
+        result_pub.publish(String(
+            data=json.dumps({'success': success, 'message': message})))
+
+    node0.create_subscription(
+        String, f'{topic}/goal', on_goal_str, _RELIABLE_QOS,
+        callback_group=cbg,
+    )
+    node0.create_subscription(
+        String, f'{topic}/cancel', on_cancel, _RELIABLE_QOS,
+        callback_group=cbg,
+    )
+
+    if not hasattr(node0, '_relays'):
+        node0._relays = []
+    node0._relays.append((client, feedback_pub, result_pub))
+
+    node0.get_logger().info(
+        f'[relay] {topic}/goal -> client(domain {robot_domain})')
+
+
+def main(args=None):
+    ctx0 = rclpy.Context()
+    ctx0.init(args=args)
+
+    tmp = rclpy.create_node('_action_relay_params', context=ctx0)
+    robot_name = tmp.declare_parameter('robot_name', 'robot_1').value
+    robot_domain = tmp.declare_parameter('robot_domain', 1).value
+    tmp.destroy_node()
+
+    ctxN = rclpy.Context()
+    ctxN.init(args=[], domain_id=int(robot_domain))
+
+    node0 = rclpy.create_node('action_relay', context=ctx0)
+    nodeN = rclpy.create_node('action_relay_client', context=ctxN)
+
+    executorN = SingleThreadedExecutor(context=ctxN)
+    executorN.add_node(nodeN)
+
+    node0.get_logger().info(
+        f'Action relay: robot={robot_name} domain={robot_domain} '
+        f'relaying {len(RELAYS)} action(s)')
+
+    for suffix, action_type in RELAYS:
+        topic = f'/{robot_name}/tasks/{suffix}'
+        _make_relay(node0, nodeN, executorN, topic, suffix, action_type, robot_domain)
+
+    executor0 = MultiThreadedExecutor(context=ctx0)
+    executor0.add_node(node0)
+
+    try:
+        executor0.spin()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node0.destroy_node()
+        nodeN.destroy_node()
+        ctx0.try_shutdown()
+        ctxN.try_shutdown()
diff --git a/gcs/ros_ws/src/action_relay/launch/action_relay.launch.py b/gcs/ros_ws/src/action_relay/launch/action_relay.launch.py
new file mode 100644
index 00000000..1f23f8cb
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/launch/action_relay.launch.py
@@ -0,0 +1,28 @@
+"""Launch one action_relay node per robot.
+
+Reads NUM_ROBOTS from the environment (default 1).
+Robot names are robot_1 .. robot_N with domain IDs 1 .. N.
+"""
+
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    num_robots = int(os.environ.get('NUM_ROBOTS', '1'))
+
+    nodes = []
+    for i in range(1, num_robots + 1):
+        nodes.append(Node(
+            package='action_relay',
+            executable='action_relay_node',
+            name=f'action_relay_robot_{i}',
+            output='screen',
+            parameters=[{
+                'robot_name': f'robot_{i}',
+                'robot_domain': i,
+            }],
+        ))
+
+    return LaunchDescription(nodes)
diff --git a/gcs/ros_ws/src/action_relay/package.xml b/gcs/ros_ws/src/action_relay/package.xml
new file mode 100644
index 00000000..c0bc0c66
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>action_relay</name>
+  <version>0.0.1</version>
+  <description>Relay ROS 2 actions across DDS domains (GCS domain 0 to robot domain N)</description>
+  <maintainer email="airlab@cmu.edu">AirLab CMU</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>task_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>airstack_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/gcs/ros_ws/src/action_relay/resource/action_relay b/gcs/ros_ws/src/action_relay/resource/action_relay
new file mode 100644
index 00000000..e69de29b
diff --git a/gcs/ros_ws/src/action_relay/setup.cfg b/gcs/ros_ws/src/action_relay/setup.cfg
new file mode 100644
index 00000000..cd7b7a99
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/action_relay
+[install]
+install_scripts=$base/lib/action_relay
diff --git a/gcs/ros_ws/src/action_relay/setup.py b/gcs/ros_ws/src/action_relay/setup.py
new file mode 100644
index 00000000..c9f6be51
--- /dev/null
+++ b/gcs/ros_ws/src/action_relay/setup.py
@@ -0,0 +1,28 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'action_relay'
+
+setup(
+    name=package_name,
+    version='0.0.1',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='AirLab CMU',
+    maintainer_email='airlab@cmu.edu',
+    description='Relay ROS 2 actions across DDS domains',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'action_relay_node = action_relay.relay_node:main',
+        ],
+    },
+)
diff --git a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
index a6eda979..6d1c0b75 100644
--- a/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
+++ b/robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml
@@ -26,7 +26,10 @@ allowlist:
 
   # camera streams
   - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/left/image_rect"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/left/camera_info"
   - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/image_rect"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/camera_info"
+  - name: "rt/$(env ROBOT_NAME)/sensors/front_stereo/right/depth_ground_truth"
   - name: "rt/$(env ROBOT_NAME)/perception/stereo_image_proc/point_cloud"
 
   # state information
diff --git a/robot/ros_ws/src/global/world_models/vdb_mapping_ros2 b/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
index 1e9174d3..68fe8dde 160000
--- a/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
+++ b/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
@@ -1 +1 @@
-Subproject commit 1e9174d325e9d1a5af679341161990448decb8be
+Subproject commit 68fe8ddea43365d1c720b07f1d109490e1bd960c

From bdb416980dddf4b9f34131d69fc041ec671c312b Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 27 Apr 2026 12:42:47 -0400
Subject: [PATCH 42/55] random walk fixed

---
 .env                                          |   2 +-
 .../polygon-editor/dist/extension.js          | 287 +++++++++++++++++
 .../polygon-editor/package.json               |   9 +
 .../robot-commands/dist/extension.js          | 126 ++++++--
 .../waypoint-editor/dist/extension.js         | 293 ++++++++++++++++++
 .../waypoint-editor/package.json              |   9 +
 .../action_relay/action_relay/relay_node.py   | 164 ++++++++--
 gcs/ros_ws/src/action_relay/package.xml       |   1 +
 .../gcs_visualizer/polygon_collector_node.py  | 229 ++++++++++++++
 .../gcs_visualizer/waypoint_collector_node.py | 231 ++++++++++++++
 .../launch/gcs_visualizer.launch.xml          |   6 +
 gcs/ros_ws/src/gcs_visualizer/setup.py        |   2 +
 .../global_bringup/launch/global.launch.xml   |  14 +-
 .../random_walk/launch/random_walk_launch.xml |  21 +-
 .../launch_scripts/two_drone_fire.py          |   3 +-
 15 files changed, 1340 insertions(+), 57 deletions(-)
 create mode 100644 gcs/foxglove_extensions/polygon-editor/dist/extension.js
 create mode 100644 gcs/foxglove_extensions/polygon-editor/package.json
 create mode 100644 gcs/foxglove_extensions/waypoint-editor/dist/extension.js
 create mode 100644 gcs/foxglove_extensions/waypoint-editor/package.json
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
 create mode 100644 gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py

diff --git a/.env b/.env
index 9de69a90..bed93ea4 100644
--- a/.env
+++ b/.env
@@ -24,7 +24,7 @@ COMPOSE_PROFILES="desktop, isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="2"  # Number of robot containers to launch.
+NUM_ROBOTS="3"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== SIMULATION =====================
diff --git a/gcs/foxglove_extensions/polygon-editor/dist/extension.js b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
new file mode 100644
index 00000000..cf2a65d3
--- /dev/null
+++ b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
@@ -0,0 +1,287 @@
+(() => {
+"use strict";
+
+// ─────────────────────────── constants ────────────────────────────────────────
+
+const CMD_TOPIC = "/gcs/polygon/command";
+const LIST_TOPIC = "/gcs/polygon/list";
+const CMD_SCHEMA = "std_msgs/msg/String";
+
+// ─────────────────────────── panel ───────────────────────────────────────────
+
+function activate(extensionContext) {
+  extensionContext.registerPanel({
+    name: "Polygon Editor",
+    initPanel: (panelContext) => {
+
+      // ── state ────────────────────────────────────────────────────────
+      const persisted = panelContext.initialState ?? {};
+      const state = {
+        defaultZ: persisted.defaultZ ?? 0.0,
+      };
+      let vertices = [];  // [{x, y, z}, ...]
+      let selectedIdx = -1;
+      let enabled = false;  // synced from /gcs/polygon/list
+
+      function persist() { panelContext.saveState(state); }
+      function sendCmd(cmd) {
+        try {
+          panelContext.advertise(CMD_TOPIC, CMD_SCHEMA);
+          panelContext.publish(CMD_TOPIC, { data: JSON.stringify(cmd) });
+        } catch (err) {
+          statusEl.textContent = "Cmd failed: " + (err?.message ?? err);
+        }
+      }
+
+      // ── DOM ──────────────────────────────────────────────────────────
+      const root = panelContext.panelElement;
+      root.style.cssText =
+        "display:flex;flex-direction:column;height:100%;box-sizing:border-box;" +
+        "padding:8px;gap:6px;font-family:sans-serif;color:inherit;overflow-y:auto;";
+
+      // Title row
+      const titleRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+      const title = el("span", "font-weight:bold;font-size:14px;flex:1;");
+      title.textContent = "Polygon Editor";
+      titleRow.appendChild(title);
+      root.appendChild(titleRow);
+
+      // Altitude + clear row
+      const ctrlRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+
+      const enableBtn = el("button",
+        "padding:6px 14px;border-radius:4px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:13px;");
+      enableBtn.addEventListener("click", () => {
+        sendCmd({ action: "set_enabled", enabled: !enabled });
+      });
+      ctrlRow.appendChild(enableBtn);
+
+      const altLabel = el("span", "font-size:12px;");
+      altLabel.textContent = "Z:";
+      const altInput = el("input",
+        "width:60px;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;");
+      altInput.type = "number";
+      altInput.step = "0.5";
+      altInput.value = String(state.defaultZ);
+      altInput.addEventListener("change", () => {
+        state.defaultZ = Number(altInput.value) || 0.0;
+        persist();
+        sendCmd({ action: "set_altitude", z: state.defaultZ });
+      });
+
+      const clearBtn = el("button",
+        "padding:6px 10px;border-radius:4px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:12px;");
+      clearBtn.textContent = "Clear All";
+      clearBtn.addEventListener("click", () => { sendCmd({ action: "clear" }); });
+
+      ctrlRow.appendChild(altLabel);
+      ctrlRow.appendChild(altInput);
+      ctrlRow.appendChild(el("span", "flex:1;"));  // spacer
+      ctrlRow.appendChild(clearBtn);
+      root.appendChild(ctrlRow);
+
+      // Vertex count
+      const countEl = el("div", "font-size:12px;opacity:0.8;flex-shrink:0;");
+      root.appendChild(countEl);
+
+      // Vertex list container
+      const listContainer = el("div",
+        "flex:1;overflow-y:auto;border:1px solid #444;border-radius:4px;min-height:60px;");
+      root.appendChild(listContainer);
+
+      // Add vertex manually row
+      const addRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const addXIn = numInput("X", "0");
+      const addYIn = numInput("Y", "0");
+      const addZIn = numInput("Z", String(state.defaultZ));
+      const addBtn = el("button",
+        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+      addBtn.textContent = "+ Add";
+      addBtn.addEventListener("click", () => {
+        sendCmd({
+          action: "add",
+          x: Number(addXIn.input.value) || 0,
+          y: Number(addYIn.input.value) || 0,
+          z: Number(addZIn.input.value) || state.defaultZ,
+        });
+      });
+      addRow.appendChild(addXIn.wrap);
+      addRow.appendChild(addYIn.wrap);
+      addRow.appendChild(addZIn.wrap);
+      addRow.appendChild(addBtn);
+      root.appendChild(addRow);
+
+      // Status
+      const statusEl = el("div", "font-size:11px;opacity:0.6;flex-shrink:0;");
+      root.appendChild(statusEl);
+
+      // ── render helpers ───────────────────────────────────────────────
+      function renderEnableBtn() {
+        if (enabled) {
+          enableBtn.textContent = "Capture: ON";
+          enableBtn.style.background = "#10b981";
+        } else {
+          enableBtn.textContent = "Capture: OFF";
+          enableBtn.style.background = "#dc2626";
+        }
+      }
+      renderEnableBtn();
+
+      function renderList() {
+        countEl.textContent = vertices.length + " vert" + (vertices.length === 1 ? "ex" : "ices");
+        listContainer.replaceChildren();
+
+        if (vertices.length === 0) {
+          const empty = el("div", "padding:12px;text-align:center;opacity:0.5;font-size:12px;");
+          empty.textContent = "No vertices. Enable capture and click in the 3D view, or add manually.";
+          listContainer.appendChild(empty);
+          return;
+        }
+
+        for (let i = 0; i < vertices.length; i++) {
+          const v = vertices[i];
+          const row = el("div",
+            "display:flex;align-items:center;gap:4px;padding:4px 6px;" +
+            "border-bottom:1px solid #333;font-size:12px;font-family:monospace;" +
+            (i === selectedIdx ? "background:rgba(220,38,38,0.18);" : ""));
+
+          const idx = el("span", "width:20px;font-weight:bold;color:#dc2626;");
+          idx.textContent = String(i);
+          row.appendChild(idx);
+
+          const xIn = coordInput(v.x, (val) => {
+            sendCmd({ action: "move", index: i, x: val, y: v.y, z: v.z });
+          });
+          const yIn = coordInput(v.y, (val) => {
+            sendCmd({ action: "move", index: i, x: v.x, y: val, z: v.z });
+          });
+          const zIn = coordInput(v.z, (val) => {
+            sendCmd({ action: "move", index: i, x: v.x, y: v.y, z: val });
+          });
+          row.appendChild(xIn);
+          row.appendChild(yIn);
+          row.appendChild(zIn);
+
+          if (i > 0) {
+            const upBtn = smallBtn("▲", () => {
+              sendCmd({ action: "reorder", from: i, to: i - 1 });
+            });
+            upBtn.title = "Move up";
+            row.appendChild(upBtn);
+          } else {
+            row.appendChild(el("span", "width:24px;"));
+          }
+
+          if (i < vertices.length - 1) {
+            const downBtn = smallBtn("▼", () => {
+              sendCmd({ action: "reorder", from: i, to: i + 1 });
+            });
+            downBtn.title = "Move down";
+            row.appendChild(downBtn);
+          } else {
+            row.appendChild(el("span", "width:24px;"));
+          }
+
+          const delBtn = smallBtn("✕", () => {
+            sendCmd({ action: "delete", index: i });
+          });
+          delBtn.style.color = "#dc2626";
+          delBtn.title = "Delete";
+          row.appendChild(delBtn);
+
+          row.addEventListener("click", (e) => {
+            if (e.target.tagName === "INPUT" || e.target.tagName === "BUTTON") return;
+            selectedIdx = (selectedIdx === i) ? -1 : i;
+            renderList();
+          });
+
+          listContainer.appendChild(row);
+        }
+      }
+      renderList();
+
+      // ── subscriptions ────────────────────────────────────────────────
+      panelContext.subscribe([{ topic: LIST_TOPIC }]);
+      panelContext.watch("currentFrame");
+
+      panelContext.onRender = (renderState, done) => {
+        const frame = renderState.currentFrame;
+        if (frame) {
+          for (const evt of frame) {
+            if (evt.topic === LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                vertices = data.vertices ?? [];
+                if (data.default_z != null) {
+                  state.defaultZ = data.default_z;
+                  altInput.value = String(data.default_z);
+                }
+                if (data.enabled != null) {
+                  enabled = Boolean(data.enabled);
+                  renderEnableBtn();
+                }
+                if (selectedIdx >= vertices.length) selectedIdx = -1;
+                renderList();
+              } catch { /* ignore bad data */ }
+            }
+          }
+        }
+        done();
+      };
+
+      panelContext.setDefaultPanelTitle("Polygon Editor");
+
+      return () => {};
+    },
+  });
+}
+
+// ─────────────────────────── DOM helpers ─────────────────────────────────────
+
+function el(tag, style) {
+  const e = document.createElement(tag);
+  if (style) e.style.cssText = style;
+  return e;
+}
+
+function smallBtn(text, onClick) {
+  const b = el("button",
+    "width:24px;height:24px;padding:0;border:none;background:transparent;" +
+    "color:inherit;cursor:pointer;font-size:12px;border-radius:3px;");
+  b.textContent = text;
+  b.addEventListener("mouseenter", () => { b.style.background = "rgba(255,255,255,0.1)"; });
+  b.addEventListener("mouseleave", () => { b.style.background = "transparent"; });
+  b.addEventListener("click", (e) => { e.stopPropagation(); onClick(); });
+  return b;
+}
+
+function coordInput(value, onChange) {
+  const inp = el("input",
+    "width:55px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-family:monospace;font-size:11px;text-align:right;");
+  inp.type = "number";
+  inp.step = "0.5";
+  inp.value = String(value);
+  inp.addEventListener("change", () => {
+    onChange(Number(inp.value) || 0);
+  });
+  return inp;
+}
+
+function numInput(label, defaultVal) {
+  const wrap = el("div", "display:flex;align-items:center;gap:2px;");
+  const lbl = el("span", "font-size:11px;opacity:0.7;");
+  lbl.textContent = label + ":";
+  const input = el("input",
+    "width:50px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-size:11px;");
+  input.type = "number";
+  input.step = "0.5";
+  input.value = defaultVal;
+  wrap.appendChild(lbl);
+  wrap.appendChild(input);
+  return { wrap, input };
+}
+
+module.exports = { activate };
+})();
diff --git a/gcs/foxglove_extensions/polygon-editor/package.json b/gcs/foxglove_extensions/polygon-editor/package.json
new file mode 100644
index 00000000..780c75f6
--- /dev/null
+++ b/gcs/foxglove_extensions/polygon-editor/package.json
@@ -0,0 +1,9 @@
+{
+  "name": "polygon-editor",
+  "displayName": "Polygon Editor",
+  "description": "Click-to-place polygon vertex editor for the Foxglove 3D view.",
+  "publisher": "AirLab CMU",
+  "version": "1.0.0",
+  "license": "Apache-2.0",
+  "main": "./dist/extension.js"
+}
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index 1070fe55..fc0ce384 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -19,6 +19,44 @@ const TERMINAL_STATUSES = new Set([
   GOAL_STATUS.ABORTED,
 ]);
 
+// Topic the Waypoint Editor publishes its current list on (std_msgs/String JSON).
+const EDITOR_LIST_TOPIC = "/gcs/waypoints/list";
+
+// Topic the Polygon Editor publishes its current vertex list on.
+const POLYGON_LIST_TOPIC = "/gcs/polygon/list";
+
+// Module-level caches of latest editor data, refreshed in the panel's onRender.
+// The "Grab from Editor" button on each polygon/path field copies these into
+// the textbox.
+let editorWaypointsCache = [];
+let polygonVerticesCache = [];
+
+// Per-tab runtime (feedback lines, last result, status, active flag) lives at
+// module scope so it survives Foxglove panel re-instantiation (layout swaps,
+// dock changes). It is only cleared explicitly by Execute via
+// resetRuntimeForNewGoal.
+// Keyed by `${robot}::${tabId}` so each robot's panel has its own runtime —
+// otherwise a goal running for robot_1 would color robot_2's panel too.
+const perTabRuntime = {};
+function getPerTab(robot, tabId) {
+  const key = `${robot}::${tabId}`;
+  if (!perTabRuntime[key]) {
+    perTabRuntime[key] = {
+      feedbackLines: [],
+      resultText: "",
+      statusText: "Idle",
+      statusCode: 0,  // GOAL_STATUS.UNKNOWN
+      active: false,
+    };
+  }
+  return perTabRuntime[key];
+}
+
+// JSON-encode the cached editor list as `[[x,y,z], ...]` for the textbox.
+function editorCacheToJson(cache) {
+  return JSON.stringify((cache ?? []).map((p) => [p.x, p.y, p.z]));
+}
+
 const TRAJECTORY_DEFAULTS = {
   Circle:    [["frame_id","base_link"],["radius","5.0"],["velocity","2.0"]],
   Figure8:   [["frame_id","base_link"],["length","10.0"],["width","5.0"],["height","0.0"],["velocity","2.0"],["max_acceleration","1.0"]],
@@ -75,7 +113,7 @@ const TASK_TABS = [
     fields: [
       { name: "frame_id",         kind: "string", default: "map" },
       { name: "waypoints",        kind: "path",   default: "[[0.0, 0.0, 5.0]]",
-        hint: "JSON array of [x, y, z] waypoints in the above frame" },
+        hint: "JSON array of [x, y, z] waypoints. Use 'Grab from Editor' to copy in the Waypoint Editor's current list." },
       { name: "goal_tolerance_m", kind: "float",  default: 1.0, min: 0, max: 100, step: 0.1 },
     ],
     buildGoal: (s) => ({
@@ -91,7 +129,7 @@ const TASK_TABS = [
     goalSchema: "task_msgs/ExplorationTask_Goal",
     fields: [
       { name: "search_bounds", kind: "polygon", default: "[]",
-        hint: "JSON array of [x, y, z] vertices ([] = unbounded)" },
+        hint: "JSON array of [x, y, z] vertices ([] = unbounded). Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
       ...altSpeedFields,
       { name: "time_limit_sec", kind: "float", default: 120.0, min: 0, max: 86400, step: 1 },
     ],
@@ -112,7 +150,7 @@ const TASK_TABS = [
     goalSchema: "task_msgs/CoverageTask_Goal",
     fields: [
       { name: "coverage_area", kind: "polygon", default: "[]",
-        hint: "JSON array of [x, y, z] vertices" },
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
       ...altSpeedFields,
       { name: "line_spacing_m", kind: "float", default: 5.0, min: 0.1, max: 1000, step: 0.1 },
       { name: "heading_deg",    kind: "float", default: 0.0, min: 0,   max: 360,  step: 1 },
@@ -140,7 +178,7 @@ const TASK_TABS = [
     fields: [
       { name: "object_class", kind: "string", default: "" },
       { name: "search_area",  kind: "polygon", default: "[]",
-        hint: "JSON array of [x, y, z] vertices" },
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
       ...altSpeedFields,
       { name: "time_limit_sec", kind: "float", default: 120.0, min: 0, max: 86400, step: 1 },
       { name: "target_count",   kind: "int",   default: 1,     min: 0, max: 10000, step: 1,
@@ -170,7 +208,7 @@ const TASK_TABS = [
     fields: [
       { name: "object_class", kind: "string",  default: "" },
       { name: "count_area",   kind: "polygon", default: "[]",
-        hint: "JSON array of [x, y, z] vertices" },
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
       ...altSpeedFields,
     ],
     buildGoal: (s) => ({
@@ -197,7 +235,7 @@ const TASK_TABS = [
       { name: "background_queries", kind: "string", default: "",
         hint: "comma-separated contrast classes, e.g. building,tree,ground" },
       { name: "search_area",        kind: "polygon", default: "[]",
-        hint: "JSON array of [x, y, z] vertices" },
+        hint: "JSON array of [x, y, z] vertices. Use 'Grab from Editor' to copy in the Polygon Editor's current list." },
       { name: "min_altitude_agl",   kind: "float", default: 3.0,  min: 0, max: 500, step: 0.1 },
       { name: "max_altitude_agl",   kind: "float", default: 15.0, min: 0, max: 500, step: 0.1 },
       { name: "min_flight_speed",   kind: "float", default: 1.0,  min: 0, max: 50,  step: 0.1 },
@@ -367,17 +405,17 @@ function activate(extensionContext) {
         }
       }
 
-      // Runtime (not persisted) — per-tab feedback/result/status
-      const perTab = {};
-      for (const tab of TASK_TABS) {
-        perTab[tab.id] = {
-          feedbackLines: [],
-          resultText: "",
-          statusText: "Idle",
-          statusCode: GOAL_STATUS.UNKNOWN,
-          active: false,
-        };
-      }
+      // Per-tab runtime is module-scoped (perTabRuntime) so it survives panel
+      // re-mounts. Lookups read `state.robot` lazily so changing the robot
+      // input switches the panel to that robot's runtime instantly.
+      const perTab = new Proxy({}, {
+        get(_, key) { return getPerTab(state.robot, String(key)); },
+        has(_, key) {
+          return Boolean(perTabRuntime[`${state.robot}::${String(key)}`]);
+        },
+      });
+      // Pre-create entries for the current robot.
+      for (const tab of TASK_TABS) getPerTab(state.robot, tab.id);
       const runtime = {
         subscribedTopics: [],
       };
@@ -406,6 +444,9 @@ function activate(extensionContext) {
       robotInput.addEventListener("change", () => {
         state.robot = robotInput.value.trim() || "robot_1";
         persist();
+        for (const tab of TASK_TABS) getPerTab(state.robot, tab.id);
+        rebuildSubscriptions();
+        renderTabs();
       });
       robotRow.appendChild(robotLabel);
       robotRow.appendChild(robotInput);
@@ -494,9 +535,19 @@ function activate(extensionContext) {
           tabBodies[tab.id].style.display = isActive ? "flex" : "none";
           tabButtons[tab.id].style.borderBottomColor = isActive ? "#10b981" : "transparent";
           tabButtons[tab.id].style.fontWeight = isActive ? "bold" : "normal";
-          // Show active indicator on tab if it has a running goal
+          // Tab text color: yellow while running, green on success, yellow on
+          // failure/cancel, inherit otherwise.
           const pt = perTab[tab.id];
-          tabButtons[tab.id].style.color = pt.active ? "#10b981" : "inherit";
+          let color = "inherit";
+          if (pt.active) {
+            color = "#eab308";  // yellow — running
+          } else if (pt.statusCode === GOAL_STATUS.SUCCEEDED) {
+            color = "#10b981";  // green — succeeded
+          } else if (pt.statusCode === GOAL_STATUS.ABORTED ||
+                     pt.statusCode === GOAL_STATUS.CANCELED) {
+            color = "#eab308";  // yellow — failed / canceled
+          }
+          tabButtons[tab.id].style.color = color;
         }
         renderStatus();
         renderFeedback();
@@ -542,13 +593,14 @@ function activate(extensionContext) {
         t.statusCode = GOAL_STATUS.UNKNOWN;
         renderFeedback();
         renderStatus();
+        renderTabs();
       }
 
       // ── subscriptions (relay uses plain std_msgs/String topics) ────────
       // Subscribe to ALL relay topics for all tabs so we catch results
       // even when the user switches tabs during execution.
       function rebuildSubscriptions() {
-        const topics = [];
+        const topics = [EDITOR_LIST_TOPIC, POLYGON_LIST_TOPIC];
         for (const tab of TASK_TABS) {
           if (perTab[tab.id].active) {
             topics.push(`/${state.robot}/${tab.actionSuffix}/relay_feedback`);
@@ -558,6 +610,9 @@ function activate(extensionContext) {
         runtime.subscribedTopics = topics;
         panelContext.subscribe(topics.map((topic) => ({ topic })));
       }
+      // Subscribe to the editor topic immediately so the cache is warm before
+      // the user clicks Execute on Navigate.
+      rebuildSubscriptions();
 
       // ── execute / cancel ─────────────────────────────────────────────────
       executeBtn.addEventListener("click", async () => {
@@ -666,7 +721,17 @@ function activate(extensionContext) {
         if (frame) {
           for (const evt of frame) {
             if (!runtime.subscribedTopics.includes(evt.topic)) continue;
-            if (evt.topic.endsWith("/relay_feedback")) {
+            if (evt.topic === EDITOR_LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                editorWaypointsCache = Array.isArray(data.waypoints) ? data.waypoints : [];
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === POLYGON_LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                polygonVerticesCache = Array.isArray(data.vertices) ? data.vertices : [];
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic.endsWith("/relay_feedback")) {
               handleRelayFeedback(evt.topic, evt.message);
             } else if (evt.topic.endsWith("/relay_result")) {
               handleRelayResult(evt.topic, evt.message);
@@ -817,6 +882,25 @@ function buildField(field, tabState, persist) {
       });
       validate();
       details.appendChild(ta);
+
+      // "Grab from Editor" button — snapshots the matching editor cache
+      // (waypoints for path fields, polygon vertices for polygon fields) into
+      // the textbox so the user can capture, modify the editor, capture again
+      // for another command/drone, etc.
+      const grabBtn = document.createElement("button");
+      grabBtn.textContent = "Grab from Editor";
+      grabBtn.style.cssText = "margin-top:4px;padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:12px;";
+      grabBtn.addEventListener("click", () => {
+        const cache = field.kind === "polygon" ? polygonVerticesCache : editorWaypointsCache;
+        const json = editorCacheToJson(cache);
+        ta.value = json;
+        tabState[field.name] = json;
+        validate();
+        renderSummary();
+        persist();
+      });
+      details.appendChild(grabBtn);
+
       if (field.hint) {
         const h = document.createElement("span");
         h.textContent = field.hint;
diff --git a/gcs/foxglove_extensions/waypoint-editor/dist/extension.js b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
new file mode 100644
index 00000000..241d490e
--- /dev/null
+++ b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
@@ -0,0 +1,293 @@
+(() => {
+"use strict";
+
+// ─────────────────────────── constants ────────────────────────────────────────
+
+const CMD_TOPIC = "/gcs/waypoints/command";
+const LIST_TOPIC = "/gcs/waypoints/list";
+const CMD_SCHEMA = "std_msgs/msg/String";
+
+// ─────────────────────────── panel ───────────────────────────────────────────
+
+function activate(extensionContext) {
+  extensionContext.registerPanel({
+    name: "Waypoint Editor",
+    initPanel: (panelContext) => {
+
+      // ── state ────────────────────────────────────────────────────────
+      const persisted = panelContext.initialState ?? {};
+      const state = {
+        defaultZ: persisted.defaultZ ?? 10.0,
+      };
+      let waypoints = [];  // [{x, y, z}, ...]
+      let selectedIdx = -1;
+      let enabled = false;  // synced from /gcs/waypoints/list
+
+      function persist() { panelContext.saveState(state); }
+      function sendCmd(cmd) {
+        try {
+          panelContext.advertise(CMD_TOPIC, CMD_SCHEMA);
+          panelContext.publish(CMD_TOPIC, { data: JSON.stringify(cmd) });
+        } catch (err) {
+          statusEl.textContent = "Cmd failed: " + (err?.message ?? err);
+        }
+      }
+
+      // ── DOM ──────────────────────────────────────────────────────────
+      const root = panelContext.panelElement;
+      root.style.cssText =
+        "display:flex;flex-direction:column;height:100%;box-sizing:border-box;" +
+        "padding:8px;gap:6px;font-family:sans-serif;color:inherit;overflow-y:auto;";
+
+      // Title row
+      const titleRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+      const title = el("span", "font-weight:bold;font-size:14px;flex:1;");
+      title.textContent = "Waypoint Editor";
+      titleRow.appendChild(title);
+      root.appendChild(titleRow);
+
+      // Altitude + clear row
+      const ctrlRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
+
+      const enableBtn = el("button",
+        "padding:6px 14px;border-radius:4px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:13px;");
+      enableBtn.addEventListener("click", () => {
+        sendCmd({ action: "set_enabled", enabled: !enabled });
+      });
+      ctrlRow.appendChild(enableBtn);
+
+      const altLabel = el("span", "font-size:12px;");
+      altLabel.textContent = "Altitude:";
+      const altInput = el("input",
+        "width:60px;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;");
+      altInput.type = "number";
+      altInput.step = "0.5";
+      altInput.value = String(state.defaultZ);
+      altInput.addEventListener("change", () => {
+        state.defaultZ = Number(altInput.value) || 10.0;
+        persist();
+        sendCmd({ action: "set_altitude", z: state.defaultZ });
+      });
+
+      const clearBtn = el("button",
+        "padding:6px 10px;border-radius:4px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:12px;");
+      clearBtn.textContent = "Clear All";
+      clearBtn.addEventListener("click", () => { sendCmd({ action: "clear" }); });
+
+      ctrlRow.appendChild(altLabel);
+      ctrlRow.appendChild(altInput);
+      ctrlRow.appendChild(el("span", "flex:1;"));  // spacer
+      ctrlRow.appendChild(clearBtn);
+      root.appendChild(ctrlRow);
+
+      // Waypoint count
+      const countEl = el("div", "font-size:12px;opacity:0.8;flex-shrink:0;");
+      root.appendChild(countEl);
+
+      // Waypoint list container
+      const listContainer = el("div",
+        "flex:1;overflow-y:auto;border:1px solid #444;border-radius:4px;min-height:60px;");
+      root.appendChild(listContainer);
+
+      // Add waypoint manually row
+      const addRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const addXIn = numInput("X", "0");
+      const addYIn = numInput("Y", "0");
+      const addZIn = numInput("Z", String(state.defaultZ));
+      const addBtn = el("button",
+        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+      addBtn.textContent = "+ Add";
+      addBtn.addEventListener("click", () => {
+        sendCmd({
+          action: "add",
+          x: Number(addXIn.input.value) || 0,
+          y: Number(addYIn.input.value) || 0,
+          z: Number(addZIn.input.value) || state.defaultZ,
+        });
+      });
+      addRow.appendChild(addXIn.wrap);
+      addRow.appendChild(addYIn.wrap);
+      addRow.appendChild(addZIn.wrap);
+      addRow.appendChild(addBtn);
+      root.appendChild(addRow);
+
+      // Status
+      const statusEl = el("div", "font-size:11px;opacity:0.6;flex-shrink:0;");
+      root.appendChild(statusEl);
+
+      // ── render helpers ───────────────────────────────────────────────
+      function renderEnableBtn() {
+        if (enabled) {
+          enableBtn.textContent = "Capture: ON";
+          enableBtn.style.background = "#10b981";
+        } else {
+          enableBtn.textContent = "Capture: OFF";
+          enableBtn.style.background = "#dc2626";
+        }
+      }
+      renderEnableBtn();
+
+      function renderList() {
+        countEl.textContent = waypoints.length + " waypoint" + (waypoints.length === 1 ? "" : "s");
+        listContainer.replaceChildren();
+
+        if (waypoints.length === 0) {
+          const empty = el("div", "padding:12px;text-align:center;opacity:0.5;font-size:12px;");
+          empty.textContent = "No waypoints. Click in the 3D view or add manually below.";
+          listContainer.appendChild(empty);
+          return;
+        }
+
+        for (let i = 0; i < waypoints.length; i++) {
+          const wp = waypoints[i];
+          const row = el("div",
+            "display:flex;align-items:center;gap:4px;padding:4px 6px;" +
+            "border-bottom:1px solid #333;font-size:12px;font-family:monospace;" +
+            (i === selectedIdx ? "background:rgba(16,185,129,0.15);" : ""));
+
+          // Index label
+          const idx = el("span", "width:20px;font-weight:bold;color:#10b981;");
+          idx.textContent = String(i);
+          row.appendChild(idx);
+
+          // Coordinates (editable)
+          const xIn = coordInput(wp.x, (v) => {
+            sendCmd({ action: "move", index: i, x: v, y: wp.y, z: wp.z });
+          });
+          const yIn = coordInput(wp.y, (v) => {
+            sendCmd({ action: "move", index: i, x: wp.x, y: v, z: wp.z });
+          });
+          const zIn = coordInput(wp.z, (v) => {
+            sendCmd({ action: "move", index: i, x: wp.x, y: wp.y, z: v });
+          });
+          row.appendChild(xIn);
+          row.appendChild(yIn);
+          row.appendChild(zIn);
+
+          // Move up
+          if (i > 0) {
+            const upBtn = smallBtn("\u25B2", () => {
+              sendCmd({ action: "reorder", from: i, to: i - 1 });
+            });
+            upBtn.title = "Move up";
+            row.appendChild(upBtn);
+          } else {
+            row.appendChild(el("span", "width:24px;"));
+          }
+
+          // Move down
+          if (i < waypoints.length - 1) {
+            const downBtn = smallBtn("\u25BC", () => {
+              sendCmd({ action: "reorder", from: i, to: i + 1 });
+            });
+            downBtn.title = "Move down";
+            row.appendChild(downBtn);
+          } else {
+            row.appendChild(el("span", "width:24px;"));
+          }
+
+          // Delete
+          const delBtn = smallBtn("\u2715", () => {
+            sendCmd({ action: "delete", index: i });
+          });
+          delBtn.style.color = "#dc2626";
+          delBtn.title = "Delete";
+          row.appendChild(delBtn);
+
+          row.addEventListener("click", (e) => {
+            if (e.target.tagName === "INPUT" || e.target.tagName === "BUTTON") return;
+            selectedIdx = (selectedIdx === i) ? -1 : i;
+            renderList();
+          });
+
+          listContainer.appendChild(row);
+        }
+      }
+      renderList();
+
+      // ── subscriptions ────────────────────────────────────────────────
+      panelContext.subscribe([{ topic: LIST_TOPIC }]);
+      panelContext.watch("currentFrame");
+
+      panelContext.onRender = (renderState, done) => {
+        const frame = renderState.currentFrame;
+        if (frame) {
+          for (const evt of frame) {
+            if (evt.topic === LIST_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                waypoints = data.waypoints ?? [];
+                if (data.default_z != null) {
+                  state.defaultZ = data.default_z;
+                  altInput.value = String(data.default_z);
+                }
+                if (data.enabled != null) {
+                  enabled = Boolean(data.enabled);
+                  renderEnableBtn();
+                }
+                // Clamp selected index
+                if (selectedIdx >= waypoints.length) selectedIdx = -1;
+                renderList();
+              } catch { /* ignore bad data */ }
+            }
+          }
+        }
+        done();
+      };
+
+      panelContext.setDefaultPanelTitle("Waypoint Editor");
+
+      return () => {};
+    },
+  });
+}
+
+// ─────────────────────────── DOM helpers ─────────────────────────────────────
+
+function el(tag, style) {
+  const e = document.createElement(tag);
+  if (style) e.style.cssText = style;
+  return e;
+}
+
+function smallBtn(text, onClick) {
+  const b = el("button",
+    "width:24px;height:24px;padding:0;border:none;background:transparent;" +
+    "color:inherit;cursor:pointer;font-size:12px;border-radius:3px;");
+  b.textContent = text;
+  b.addEventListener("mouseenter", () => { b.style.background = "rgba(255,255,255,0.1)"; });
+  b.addEventListener("mouseleave", () => { b.style.background = "transparent"; });
+  b.addEventListener("click", (e) => { e.stopPropagation(); onClick(); });
+  return b;
+}
+
+function coordInput(value, onChange) {
+  const inp = el("input",
+    "width:55px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-family:monospace;font-size:11px;text-align:right;");
+  inp.type = "number";
+  inp.step = "0.5";
+  inp.value = String(value);
+  inp.addEventListener("change", () => {
+    onChange(Number(inp.value) || 0);
+  });
+  return inp;
+}
+
+function numInput(label, defaultVal) {
+  const wrap = el("div", "display:flex;align-items:center;gap:2px;");
+  const lbl = el("span", "font-size:11px;opacity:0.7;");
+  lbl.textContent = label + ":";
+  const input = el("input",
+    "width:50px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-size:11px;");
+  input.type = "number";
+  input.step = "0.5";
+  input.value = defaultVal;
+  wrap.appendChild(lbl);
+  wrap.appendChild(input);
+  return { wrap, input };
+}
+
+module.exports = { activate };
+})();
diff --git a/gcs/foxglove_extensions/waypoint-editor/package.json b/gcs/foxglove_extensions/waypoint-editor/package.json
new file mode 100644
index 00000000..bad6058d
--- /dev/null
+++ b/gcs/foxglove_extensions/waypoint-editor/package.json
@@ -0,0 +1,9 @@
+{
+  "name": "waypoint-editor",
+  "displayName": "Waypoint Editor",
+  "description": "Click-to-place waypoint editor panel for the Foxglove 3D view.",
+  "publisher": "AirLab CMU",
+  "version": "1.0.0",
+  "license": "Apache-2.0",
+  "main": "./dist/extension.js"
+}
diff --git a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
index 9caa4cd6..fb6495d8 100644
--- a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
+++ b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
@@ -21,16 +21,21 @@
 """
 
 import json
+import math
+import threading
 
 import rclpy
 from rclpy.action import ActionClient
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
-from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+from rclpy.qos import (
+    QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy,
+)
 
 from builtin_interfaces.msg import Time
 from geometry_msgs.msg import Point, Pose, Polygon, Point32, PoseStamped
 from nav_msgs.msg import Path
+from sensor_msgs.msg import NavSatFix
 from std_msgs.msg import Header, String
 
 from airstack_msgs.msg import FixedTrajectory
@@ -38,8 +43,24 @@
 from task_msgs.action import (
     TakeoffTask, LandTask, NavigateTask,
     FixedTrajectoryTask, SemanticSearchTask,
+    ExplorationTask,
 )
 
+# ── Map-frame ENU origin (must match gcs_visualizer/gcs_utils.py) ────────────
+# Foxglove panels publish waypoints/polygons in this same global ENU frame, and
+# the gcs_visualizer renders the robot at gps_to_enu(...) - boot. The robot's
+# task executors expect coordinates relative to its own boot pose, so we
+# subtract the robot's boot ENU position before forwarding.
+ORIGIN_LAT = 38.736832
+ORIGIN_LON = -9.137977
+
+
+def gps_to_enu(lat, lon, alt, alt_ground):
+    x = (lon - ORIGIN_LON) * 111320.0 * math.cos(math.radians(ORIGIN_LAT))
+    y = (lat - ORIGIN_LAT) * 111320.0
+    z = alt - alt_ground
+    return x, y, z
+
 # ── Registry ─────────────────────────────────────────────────────────────────
 RELAYS = [
     ('takeoff',          TakeoffTask),
@@ -47,6 +68,7 @@
     ('navigate',         NavigateTask),
     ('fixed_trajectory', FixedTrajectoryTask),
     ('semantic_search',  SemanticSearchTask),
+    ('exploration',      ExplorationTask),
 ]
 
 _RELIABLE_QOS = QoSProfile(
@@ -59,26 +81,45 @@
 # ── Goal builders ────────────────────────────────────────────────────────────
 # Each converts a JSON dict into a typed Goal message.
 
-def _build_takeoff_goal(d):
+def _map_to_robot(x, y, z, ts):
+    """Subtract the robot's boot ENU position from a global-ENU point."""
+    boot = ts.get('boot') if ts else None
+    if boot is None:
+        return x, y, z
+    bx, by, bz = boot
+    return x - bx, y - by, z - bz
+
+
+def _build_takeoff_goal(d, ts):
     g = TakeoffTask.Goal()
     g.target_altitude_m = float(d.get('target_altitude_m', 0))
     g.velocity_m_s = float(d.get('velocity_m_s', 0))
     return g
 
 
-def _build_land_goal(d):
+def _build_land_goal(d, ts):
     g = LandTask.Goal()
     g.velocity_m_s = float(d.get('velocity_m_s', 0))
     return g
 
 
-def _build_navigate_goal(d):
+def _build_navigate_goal(d, ts):
     g = NavigateTask.Goal()
     g.goal_tolerance_m = float(d.get('goal_tolerance_m', 1.0))
     plan_data = d.get('global_plan', {})
     header = Header()
     hdr = plan_data.get('header', {})
-    header.frame_id = str(hdr.get('frame_id', 'map'))
+    in_frame = str(hdr.get('frame_id', 'map'))
+    # Waypoints from the Foxglove editor are in *global* ENU (gcs_visualizer's
+    # shared map). The robot's own TF tree uses 'map' rooted at its takeoff
+    # position, so we subtract the boot offset and keep the 'map' frame_id —
+    # the on-robot planner (droan_gl, target_frame=map) expects this.
+    if in_frame == 'map':
+        if ts is None or ts.get('boot') is None:
+            raise RuntimeError(
+                'Cannot transform map-frame waypoints: robot GPS boot pose '
+                'not yet received. Wait for first GPS fix and retry.')
+    header.frame_id = in_frame
     stamp = hdr.get('stamp', {})
     header.stamp = Time(sec=int(stamp.get('sec', 0)),
                         nanosec=int(stamp.get('nanosec', 0)))
@@ -88,9 +129,12 @@ def _build_navigate_goal(d):
         ps.header = header
         pos = pose_data.get('pose', {}).get('position', {})
         ori = pose_data.get('pose', {}).get('orientation', {})
-        ps.pose.position = Point(
-            x=float(pos.get('x', 0)), y=float(pos.get('y', 0)),
-            z=float(pos.get('z', 0)))
+        x = float(pos.get('x', 0))
+        y = float(pos.get('y', 0))
+        z = float(pos.get('z', 0))
+        if in_frame == 'map':
+            x, y, z = _map_to_robot(x, y, z, ts)
+        ps.pose.position = Point(x=x, y=y, z=z)
         ps.pose.orientation.x = float(ori.get('x', 0))
         ps.pose.orientation.y = float(ori.get('y', 0))
         ps.pose.orientation.z = float(ori.get('z', 0))
@@ -100,7 +144,7 @@ def _build_navigate_goal(d):
     return g
 
 
-def _build_fixed_trajectory_goal(d):
+def _build_fixed_trajectory_goal(d, ts):
     g = FixedTrajectoryTask.Goal()
     g.loop = bool(d.get('loop', False))
     spec_data = d.get('trajectory_spec', {})
@@ -115,7 +159,7 @@ def _build_fixed_trajectory_goal(d):
     return g
 
 
-def _build_semantic_search_goal(d):
+def _build_semantic_search_goal(d, ts):
     g = SemanticSearchTask.Goal()
     g.query = str(d.get('query', ''))
     g.background_queries = str(d.get('background_queries', ''))
@@ -124,14 +168,38 @@ def _build_semantic_search_goal(d):
     g.min_flight_speed = float(d.get('min_flight_speed', 0))
     g.max_flight_speed = float(d.get('max_flight_speed', 0))
     g.confidence_threshold = float(d.get('confidence_threshold', 0.95))
-    # search_area polygon
     area = d.get('search_area', {})
+    g.search_area = _build_polygon_from_global(area.get('points', []), ts)
+    return g
+
+
+def _build_polygon_from_global(points, ts):
+    """Transform editor-frame polygon points to the robot's local 'map' frame."""
+    if points and (ts is None or ts.get('boot') is None):
+        raise RuntimeError(
+            'Cannot transform polygon vertices: robot GPS boot pose '
+            'not yet received. Wait for first GPS fix and retry.')
     poly = Polygon()
-    for pt in area.get('points', []):
-        poly.points.append(Point32(
-            x=float(pt.get('x', 0)), y=float(pt.get('y', 0)),
-            z=float(pt.get('z', 0))))
-    g.search_area = poly
+    for pt in points:
+        x, y, z = _map_to_robot(
+            float(pt.get('x', 0)),
+            float(pt.get('y', 0)),
+            float(pt.get('z', 0)),
+            ts,
+        )
+        poly.points.append(Point32(x=x, y=y, z=z))
+    return poly
+
+
+def _build_exploration_goal(d, ts):
+    g = ExplorationTask.Goal()
+    g.min_altitude_agl = float(d.get('min_altitude_agl', 0))
+    g.max_altitude_agl = float(d.get('max_altitude_agl', 0))
+    g.min_flight_speed = float(d.get('min_flight_speed', 0))
+    g.max_flight_speed = float(d.get('max_flight_speed', 0))
+    g.time_limit_sec = float(d.get('time_limit_sec', 0))
+    bounds = d.get('search_bounds', {})
+    g.search_bounds = _build_polygon_from_global(bounds.get('points', []), ts)
     return g
 
 
@@ -141,12 +209,14 @@ def _build_semantic_search_goal(d):
     'navigate':         _build_navigate_goal,
     'fixed_trajectory': _build_fixed_trajectory_goal,
     'semantic_search':  _build_semantic_search_goal,
+    'exploration':      _build_exploration_goal,
 }
 
 
 # ── Relay wiring ─────────────────────────────────────────────────────────────
 
-def _make_relay(node0, nodeN, executorN, topic, suffix, action_type, robot_domain):
+def _make_relay(node0, nodeN, executorN, topic, suffix, action_type,
+                robot_domain, transform_state):
     """Wire up goal subscriber (domain 0) + action client (domain N)."""
 
     client = ActionClient(nodeN, action_type, topic)
@@ -157,6 +227,7 @@ def _make_relay(node0, nodeN, executorN, topic, suffix, action_type, robot_domai
 
     goal_builder = _GOAL_BUILDERS[suffix]
     active_goal = {'handle': None}
+    cancel_event = threading.Event()
 
     def on_goal_str(msg):
         """Receive JSON goal from Foxglove, parse, forward as action."""
@@ -167,7 +238,12 @@ def on_goal_str(msg):
             _publish_result(False, f'Invalid JSON: {e}')
             return
 
-        goal_msg = goal_builder(data)
+        try:
+            goal_msg = goal_builder(data, transform_state)
+        except RuntimeError as e:
+            node0.get_logger().warn(f'[relay] {topic}: {e}')
+            _publish_result(False, str(e))
+            return
         node0.get_logger().info(f'[relay] {topic}: goal: {goal_msg}')
 
         if not client.wait_for_server(timeout_sec=5.0):
@@ -190,12 +266,21 @@ def on_goal_str(msg):
             return
 
         active_goal['handle'] = robot_goal_handle
+        cancel_event.clear()
         node0.get_logger().info(f'[relay] {topic}: goal accepted')
         feedback_pub.publish(String(data='Goal accepted by robot'))
 
         result_future = robot_goal_handle.get_result_async()
+        cancel_sent = False
         while not result_future.done():
+            if cancel_event.is_set() and not cancel_sent:
+                # Issue cancel from the same thread that owns executorN to
+                # avoid racing the rcl ActionClient.
+                robot_goal_handle.cancel_goal_async()
+                cancel_sent = True
+                feedback_pub.publish(String(data='Cancel forwarded to robot'))
             executorN.spin_once(timeout_sec=0.05)
+        cancel_event.clear()
 
         active_goal['handle'] = None
         wrapped = result_future.result()
@@ -209,13 +294,12 @@ def on_goal_str(msg):
         _publish_result(success, message)
 
     def on_cancel(msg):
-        """Cancel the active goal on the robot."""
-        gh = active_goal.get('handle')
-        if gh is None:
+        """Signal the goal-handler loop to forward a cancel to the robot."""
+        if active_goal.get('handle') is None:
             node0.get_logger().warn(f'[relay] {topic}: cancel requested but no active goal')
             return
-        node0.get_logger().info(f'[relay] {topic}: forwarding cancel to robot')
-        gh.cancel_goal_async()
+        node0.get_logger().info(f'[relay] {topic}: cancel requested')
+        cancel_event.set()
 
     def _on_feedback(fb_msg):
         fb = fb_msg.feedback
@@ -276,9 +360,41 @@ def main(args=None):
         f'Action relay: robot={robot_name} domain={robot_domain} '
         f'relaying {len(RELAYS)} action(s)')
 
+    # Track this robot's boot ENU position so we can convert global-frame
+    # waypoints/polygons (from the Foxglove editors) to robot-local before
+    # forwarding. First valid GPS fix is treated as boot, matching gcs_visualizer.
+    transform_state = {'boot': None, 'alt_ground': None}
+    gps_qos = QoSProfile(
+        depth=1,
+        reliability=ReliabilityPolicy.BEST_EFFORT,
+        durability=DurabilityPolicy.VOLATILE,
+        history=HistoryPolicy.KEEP_LAST,
+    )
+
+    def _on_gps(msg: NavSatFix):
+        if msg.status.status < 0:
+            return
+        if transform_state['alt_ground'] is None:
+            transform_state['alt_ground'] = msg.altitude
+        if transform_state['boot'] is None:
+            pos = gps_to_enu(msg.latitude, msg.longitude, msg.altitude,
+                             transform_state['alt_ground'])
+            transform_state['boot'] = pos
+            node0.get_logger().info(
+                f"[relay] {robot_name} boot ENU = "
+                f"({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})")
+
+    node0.create_subscription(
+        NavSatFix,
+        f'/{robot_name}/interface/mavros/global_position/global',
+        _on_gps,
+        gps_qos,
+    )
+
     for suffix, action_type in RELAYS:
         topic = f'/{robot_name}/tasks/{suffix}'
-        _make_relay(node0, nodeN, executorN, topic, suffix, action_type, robot_domain)
+        _make_relay(node0, nodeN, executorN, topic, suffix, action_type,
+                    robot_domain, transform_state)
 
     executor0 = MultiThreadedExecutor(context=ctx0)
     executor0.add_node(node0)
diff --git a/gcs/ros_ws/src/action_relay/package.xml b/gcs/ros_ws/src/action_relay/package.xml
index c0bc0c66..f9fe0fdb 100644
--- a/gcs/ros_ws/src/action_relay/package.xml
+++ b/gcs/ros_ws/src/action_relay/package.xml
@@ -14,6 +14,7 @@
   <depend>geometry_msgs</depend>
   <depend>airstack_msgs</depend>
   <depend>diagnostic_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
new file mode 100644
index 00000000..f9e9a323
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
@@ -0,0 +1,229 @@
+"""Polygon collector: receives click points from Foxglove 3D panel,
+manages a polygon vertex list, and publishes markers + vertex list.
+
+Commands arrive as JSON on /gcs/polygon/command (std_msgs/String):
+  {"action": "add", "x": 1.0, "y": 2.0, "z": 0.0}
+  {"action": "delete", "index": 2}
+  {"action": "move", "index": 1, "x": 3.0, "y": 4.0, "z": 0.0}
+  {"action": "reorder", "from": 2, "to": 0}
+  {"action": "clear"}
+  {"action": "set_altitude", "z": 0.0}  (sets default z for clicks)
+
+Publishes:
+  /gcs/polygon/markers  (MarkerArray — red vertex spheres + closed red LINE_STRIP)
+  /gcs/polygon/list     (String — JSON {vertices: [{x,y,z},...], default_z: float})
+
+The polygon is rendered as a closed loop (last vertex connects back to first)
+so the user sees the area they're outlining.
+"""
+
+import json
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, PointStamped
+from std_msgs.msg import ColorRGBA, String
+from visualization_msgs.msg import Marker, MarkerArray
+
+_RELIABLE = QoSProfile(depth=10,
+                        reliability=ReliabilityPolicy.RELIABLE,
+                        durability=DurabilityPolicy.VOLATILE)
+
+_LATCHED = QoSProfile(depth=1,
+                       reliability=ReliabilityPolicy.RELIABLE,
+                       durability=DurabilityPolicy.TRANSIENT_LOCAL)
+
+_LINE_COLOR = ColorRGBA(r=1.0, g=0.15, b=0.15, a=1.0)
+_VERTEX_COLOR = ColorRGBA(r=1.0, g=0.15, b=0.15, a=1.0)
+_TEXT_COLOR = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0)
+_VERTEX_RADIUS = 0.35
+_LINE_WIDTH = 0.15
+
+
+class PolygonCollectorNode(Node):
+    def __init__(self):
+        super().__init__('polygon_collector')
+
+        self._vertices: list[dict] = []  # [{x, y, z}, ...]
+        self._default_z = 0.0
+        self._frame_id = 'map'
+        # Click capture is gated by an Enable toggle so the waypoint and
+        # polygon editors can share /clicked_point without conflicting.
+        self._enabled = False
+
+        self.create_subscription(
+            PointStamped, '/clicked_point', self._on_click, 10)
+        self.create_subscription(
+            String, '/gcs/polygon/command', self._on_command, _RELIABLE)
+
+        self._marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/polygon/markers', _LATCHED)
+        self._list_pub = self.create_publisher(
+            String, '/gcs/polygon/list', _LATCHED)
+
+        self._publish_all()
+        self.get_logger().info('Polygon collector ready')
+
+    def _on_click(self, msg: PointStamped):
+        if not self._enabled:
+            return
+        v = {
+            'x': round(msg.point.x, 2),
+            'y': round(msg.point.y, 2),
+            'z': round(self._default_z, 2),
+        }
+        self._vertices.append(v)
+        self.get_logger().info(
+            f'Added vertex {len(self._vertices)-1}: '
+            f'({v["x"]}, {v["y"]}, {v["z"]})')
+        self._publish_all()
+
+    def _on_command(self, msg: String):
+        try:
+            cmd = json.loads(msg.data)
+        except json.JSONDecodeError:
+            self.get_logger().warn(f'Bad command JSON: {msg.data}')
+            return
+
+        action = cmd.get('action', '')
+
+        if action == 'add':
+            self._vertices.append({
+                'x': round(float(cmd.get('x', 0)), 2),
+                'y': round(float(cmd.get('y', 0)), 2),
+                'z': round(float(cmd.get('z', self._default_z)), 2),
+            })
+
+        elif action == 'delete':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._vertices):
+                self._vertices.pop(idx)
+
+        elif action == 'move':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._vertices):
+                self._vertices[idx] = {
+                    'x': round(float(cmd.get('x', 0)), 2),
+                    'y': round(float(cmd.get('y', 0)), 2),
+                    'z': round(float(cmd.get('z', 0)), 2),
+                }
+
+        elif action == 'reorder':
+            fi = int(cmd.get('from', -1))
+            ti = int(cmd.get('to', -1))
+            if 0 <= fi < len(self._vertices) and 0 <= ti < len(self._vertices):
+                v = self._vertices.pop(fi)
+                self._vertices.insert(ti, v)
+
+        elif action == 'clear':
+            self._vertices.clear()
+
+        elif action == 'set_altitude':
+            self._default_z = float(cmd.get('z', self._default_z))
+            self.get_logger().info(f'Default z set to {self._default_z}')
+            self._publish_list()
+            return
+
+        elif action == 'set_enabled':
+            self._enabled = bool(cmd.get('enabled', False))
+            self.get_logger().info(
+                f'Click capture {"ENABLED" if self._enabled else "disabled"}')
+            self._publish_list()
+            return
+
+        else:
+            self.get_logger().warn(f'Unknown command: {action}')
+            return
+
+        self._publish_all()
+
+    def _publish_all(self):
+        now = self.get_clock().now().to_msg()
+        self._publish_markers(now)
+        self._publish_list()
+
+    def _publish_markers(self, stamp: Time):
+        array = MarkerArray()
+
+        delete = Marker()
+        delete.header.frame_id = self._frame_id
+        delete.header.stamp = stamp
+        delete.ns = 'polygon'
+        delete.action = Marker.DELETEALL
+        array.markers.append(delete)
+
+        for i, v in enumerate(self._vertices):
+            pos = Point(x=v['x'], y=v['y'], z=v['z'])
+
+            sphere = Marker()
+            sphere.header.frame_id = self._frame_id
+            sphere.header.stamp = stamp
+            sphere.ns = 'polygon'
+            sphere.id = i * 2
+            sphere.type = Marker.SPHERE
+            sphere.action = Marker.ADD
+            sphere.pose.position = pos
+            sphere.pose.orientation.w = 1.0
+            sphere.scale.x = _VERTEX_RADIUS
+            sphere.scale.y = _VERTEX_RADIUS
+            sphere.scale.z = _VERTEX_RADIUS
+            sphere.color = _VERTEX_COLOR
+            array.markers.append(sphere)
+
+            label = Marker()
+            label.header.frame_id = self._frame_id
+            label.header.stamp = stamp
+            label.ns = 'polygon'
+            label.id = i * 2 + 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position = Point(
+                x=v['x'], y=v['y'], z=v['z'] + _VERTEX_RADIUS + 0.3)
+            label.pose.orientation.w = 1.0
+            label.scale.z = 0.4
+            label.color = _TEXT_COLOR
+            label.text = str(i)
+            array.markers.append(label)
+
+        # Closed polygon outline (last vertex back to first).
+        if len(self._vertices) >= 2:
+            line = Marker()
+            line.header.frame_id = self._frame_id
+            line.header.stamp = stamp
+            line.ns = 'polygon'
+            line.id = len(self._vertices) * 2 + 100
+            line.type = Marker.LINE_STRIP
+            line.action = Marker.ADD
+            line.scale.x = _LINE_WIDTH
+            line.color = _LINE_COLOR
+            line.pose.orientation.w = 1.0
+            for v in self._vertices:
+                line.points.append(Point(x=v['x'], y=v['y'], z=v['z']))
+            # Close the loop
+            first = self._vertices[0]
+            line.points.append(Point(x=first['x'], y=first['y'], z=first['z']))
+            array.markers.append(line)
+
+        self._marker_pub.publish(array)
+
+    def _publish_list(self):
+        self._list_pub.publish(String(data=json.dumps({
+            'vertices': self._vertices,
+            'default_z': self._default_z,
+            'enabled': self._enabled,
+        })))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PolygonCollectorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
new file mode 100644
index 00000000..27f2af6d
--- /dev/null
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
@@ -0,0 +1,231 @@
+"""Waypoint collector: receives click points from Foxglove 3D panel,
+manages a waypoint list, and publishes markers + path.
+
+Commands arrive as JSON on /gcs/waypoints/command (std_msgs/String):
+  {"action": "add", "x": 1.0, "y": 2.0, "z": 5.0}
+  {"action": "delete", "index": 2}
+  {"action": "move", "index": 1, "x": 3.0, "y": 4.0, "z": 5.0}
+  {"action": "reorder", "from": 2, "to": 0}
+  {"action": "clear"}
+  {"action": "set_altitude", "z": 10.0}  (sets default altitude for clicks)
+
+Publishes:
+  /gcs/waypoints/markers  (MarkerArray — spheres, labels, connecting lines)
+  /gcs/waypoints/list     (String — JSON array of waypoints for the panel)
+  /gcs/waypoints/path     (nav_msgs/Path — for use by navigate task)
+"""
+
+import json
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
+
+from builtin_interfaces.msg import Time
+from geometry_msgs.msg import Point, PointStamped, PoseStamped
+from nav_msgs.msg import Path
+from std_msgs.msg import ColorRGBA, Header, String
+from visualization_msgs.msg import Marker, MarkerArray
+
+_RELIABLE = QoSProfile(depth=10,
+                        reliability=ReliabilityPolicy.RELIABLE,
+                        durability=DurabilityPolicy.VOLATILE)
+
+_LATCHED = QoSProfile(depth=1,
+                       reliability=ReliabilityPolicy.RELIABLE,
+                       durability=DurabilityPolicy.TRANSIENT_LOCAL)
+
+# Marker colors
+_STAR_COLOR = ColorRGBA(r=0.2, g=0.8, b=1.0, a=1.0)
+_TEXT_COLOR = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0)
+_STAR_SCALE = 1.0
+
+
+class WaypointCollectorNode(Node):
+    def __init__(self):
+        super().__init__('waypoint_collector')
+
+        self._waypoints: list[dict] = []  # [{x, y, z}, ...]
+        self._default_z = 10.0
+        self._frame_id = 'map'
+        # Click capture is gated by an Enable toggle from the editor panel so
+        # the waypoint and polygon editors can share /clicked_point without
+        # stealing each other's clicks.
+        self._enabled = False
+
+        # Subscribers
+        self.create_subscription(
+            PointStamped, '/clicked_point', self._on_click, 10)
+        self.create_subscription(
+            String, '/gcs/waypoints/command', self._on_command, _RELIABLE)
+
+        # Publishers
+        self._marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/waypoints/markers', _LATCHED)
+        self._list_pub = self.create_publisher(
+            String, '/gcs/waypoints/list', _LATCHED)
+        self._path_pub = self.create_publisher(
+            Path, '/gcs/waypoints/path', _LATCHED)
+
+        # Publish initial empty state
+        self._publish_all()
+        self.get_logger().info('Waypoint collector ready')
+
+    # ── Click handler ────────────────────────────────────────────────────
+    def _on_click(self, msg: PointStamped):
+        if not self._enabled:
+            return
+        wp = {
+            'x': round(msg.point.x, 2),
+            'y': round(msg.point.y, 2),
+            'z': round(self._default_z, 2),
+        }
+        self._waypoints.append(wp)
+        self.get_logger().info(
+            f'Added waypoint {len(self._waypoints)-1}: '
+            f'({wp["x"]}, {wp["y"]}, {wp["z"]})')
+        self._publish_all()
+
+    # ── Command handler ──────────────────────────────────────────────────
+    def _on_command(self, msg: String):
+        try:
+            cmd = json.loads(msg.data)
+        except json.JSONDecodeError:
+            self.get_logger().warn(f'Bad command JSON: {msg.data}')
+            return
+
+        action = cmd.get('action', '')
+
+        if action == 'add':
+            self._waypoints.append({
+                'x': round(float(cmd.get('x', 0)), 2),
+                'y': round(float(cmd.get('y', 0)), 2),
+                'z': round(float(cmd.get('z', self._default_z)), 2),
+            })
+
+        elif action == 'delete':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._waypoints):
+                self._waypoints.pop(idx)
+
+        elif action == 'move':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._waypoints):
+                self._waypoints[idx] = {
+                    'x': round(float(cmd.get('x', 0)), 2),
+                    'y': round(float(cmd.get('y', 0)), 2),
+                    'z': round(float(cmd.get('z', 0)), 2),
+                }
+
+        elif action == 'reorder':
+            fi = int(cmd.get('from', -1))
+            ti = int(cmd.get('to', -1))
+            if 0 <= fi < len(self._waypoints) and 0 <= ti < len(self._waypoints):
+                wp = self._waypoints.pop(fi)
+                self._waypoints.insert(ti, wp)
+
+        elif action == 'clear':
+            self._waypoints.clear()
+
+        elif action == 'set_altitude':
+            self._default_z = float(cmd.get('z', self._default_z))
+            self.get_logger().info(f'Default altitude set to {self._default_z}')
+            self._publish_list()
+            return
+
+        elif action == 'set_enabled':
+            self._enabled = bool(cmd.get('enabled', False))
+            self.get_logger().info(
+                f'Click capture {"ENABLED" if self._enabled else "disabled"}')
+            self._publish_list()
+            return
+
+        else:
+            self.get_logger().warn(f'Unknown command: {action}')
+            return
+
+        self._publish_all()
+
+    # ── Publishing ───────────────────────────────────────────────────────
+    def _publish_all(self):
+        now = self.get_clock().now().to_msg()
+        self._publish_markers(now)
+        self._publish_list()
+        self._publish_path(now)
+
+    def _publish_markers(self, stamp: Time):
+        array = MarkerArray()
+
+        # Delete all previous markers
+        delete = Marker()
+        delete.header.frame_id = self._frame_id
+        delete.header.stamp = stamp
+        delete.ns = 'waypoints'
+        delete.action = Marker.DELETEALL
+        array.markers.append(delete)
+
+        for i, wp in enumerate(self._waypoints):
+            pos = Point(x=wp['x'], y=wp['y'], z=wp['z'])
+
+            # Star glyph (text marker, always faces camera)
+            star = Marker()
+            star.header.frame_id = self._frame_id
+            star.header.stamp = stamp
+            star.ns = 'waypoints'
+            star.id = i * 2
+            star.type = Marker.TEXT_VIEW_FACING
+            star.action = Marker.ADD
+            star.pose.position = pos
+            star.pose.orientation.w = 1.0
+            star.scale.z = _STAR_SCALE
+            star.color = _STAR_COLOR
+            star.text = '★'
+            array.markers.append(star)
+
+            # Index label
+            label = Marker()
+            label.header.frame_id = self._frame_id
+            label.header.stamp = stamp
+            label.ns = 'waypoints'
+            label.id = i * 2 + 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position = Point(
+                x=wp['x'], y=wp['y'], z=wp['z'] + 0.5)
+            label.pose.orientation.w = 1.0
+            label.scale.z = 0.4
+            label.color = _TEXT_COLOR
+            label.text = str(i)
+            array.markers.append(label)
+
+        self._marker_pub.publish(array)
+
+    def _publish_list(self):
+        self._list_pub.publish(String(data=json.dumps({
+            'waypoints': self._waypoints,
+            'default_z': self._default_z,
+            'enabled': self._enabled,
+        })))
+
+    def _publish_path(self, stamp: Time):
+        path = Path()
+        path.header = Header(frame_id=self._frame_id, stamp=stamp)
+        for wp in self._waypoints:
+            ps = PoseStamped()
+            ps.header = path.header
+            ps.pose.position = Point(x=wp['x'], y=wp['y'], z=wp['z'])
+            ps.pose.orientation.w = 1.0
+            path.poses.append(ps)
+        self._path_pub.publish(path)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WaypointCollectorNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
diff --git a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
index d8f6478d..d5b823d7 100644
--- a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
+++ b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
@@ -9,4 +9,10 @@
   <!-- Payload visualization: project-specific data from PeerProfile gossip payloads -->
   <!-- Remove or replace this node for projects that don't use Rayfronts -->
   <node pkg="gcs_visualizer" exec="payload_visualizer_node" name="payload_visualizer_node" output="screen" />
+
+  <!-- Waypoint collector: click-to-place waypoints from Foxglove 3D panel -->
+  <node pkg="gcs_visualizer" exec="waypoint_collector_node" name="waypoint_collector_node" output="screen" />
+
+  <!-- Polygon collector: click-to-place polygon vertices from Foxglove 3D panel -->
+  <node pkg="gcs_visualizer" exec="polygon_collector_node" name="polygon_collector_node" output="screen" />
 </launch>
diff --git a/gcs/ros_ws/src/gcs_visualizer/setup.py b/gcs/ros_ws/src/gcs_visualizer/setup.py
index 5d89b2bb..9e40163b 100644
--- a/gcs/ros_ws/src/gcs_visualizer/setup.py
+++ b/gcs/ros_ws/src/gcs_visualizer/setup.py
@@ -24,6 +24,8 @@
         'console_scripts': [
             'foxglove_visualizer_node = gcs_visualizer.foxglove_visualizer_node:main',
             'payload_visualizer_node = gcs_visualizer.payload_visualizer_node:main',
+            'waypoint_collector_node = gcs_visualizer.waypoint_collector_node:main',
+            'polygon_collector_node = gcs_visualizer.polygon_collector_node:main',
         ],
     },
 )
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 628af479..0aa9064b 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -7,5 +7,17 @@
 
     <!-- Planning -->
     <include file="$(find-pkg-share semantic_search_task)/launch/semantic_search_task.launch.xml" />
-    
+
+    <node pkg="random_walk_planner" exec="random_walk_planner" name="random_walk_planner"
+        output="screen">
+        <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
+        <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
+        <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
+        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
+        <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+        <!-- Branch additions: ExplorationTask action server + NavigateTask client -->
+        <remap from="~/exploration_task" to="/$(env ROBOT_NAME)/tasks/exploration" />
+        <remap from="navigate_task" to="/$(env ROBOT_NAME)/tasks/navigate" />
+    </node>
+
 </launch>
diff --git a/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml b/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml
index 174bc53f..dfe624ca 100644
--- a/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml
+++ b/robot/ros_ws/src/global/planners/random_walk/launch/random_walk_launch.xml
@@ -1,11 +1,14 @@
 <launch>
-  <node 
-    pkg="random_walk_planner" 
-    exec="random_walk_planner" 
-    name="random_walk_planner">
-    <param 
-      from="$(find random_walk_planner)/config/random_walk_config.yaml" />
-    <param
-      from="$(find global_planner_bringup)/config/global_planner_config.yaml" />
+  <node pkg="random_walk_planner" exec="random_walk_planner"
+        name="random_walk_planner" output="screen">
+    <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
+    <remap from="~/exploration_task"
+           to="/$(env ROBOT_NAME)/tasks/exploration" />
+    <remap from="navigate_task"
+           to="/$(env ROBOT_NAME)/tasks/navigate" />
+    <remap from="vdb_map_visualization"
+           to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
+    <remap from="odometry"
+           to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
   </node>
-</launch>
\ No newline at end of file
+</launch>
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 8ac80223..8b4ff2a8 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -73,7 +73,8 @@
 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": 3.5},
+    {"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": 20.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},
 ]
 # ---------------------------------------------------------
 

From 243292e768accdf31f0758e429147d96864b47e5 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Mon, 27 Apr 2026 16:05:07 -0400
Subject: [PATCH 43/55] fixed random walk bringup. Added saves and viz for
 multiple waypoints and polygons

---
 .env                                          |   2 +-
 .gitignore                                    |   4 +
 gcs/docker/gcs-base-docker-compose.yaml       |   2 +
 .../polygon-editor/dist/extension.js          | 100 ++++++-
 .../robot-commands/dist/extension.js          |  93 ++++++-
 .../waypoint-editor/dist/extension.js         | 100 ++++++-
 .../action_relay/action_relay/relay_node.py   | 103 ++++++--
 .../gcs_visualizer/polygon_collector_node.py  | 218 +++++++++++++++-
 .../gcs_visualizer/waypoint_collector_node.py | 246 +++++++++++++++++-
 gcs/saves/.gitkeep                            |   0
 .../global_bringup/launch/global.launch.xml   |  21 +-
 .../semantic_search_task/node.py              |  14 +-
 .../launch_scripts/two_drone_fire.py          |   1 -
 13 files changed, 842 insertions(+), 62 deletions(-)
 create mode 100644 gcs/saves/.gitkeep

diff --git a/.env b/.env
index bed93ea4..9de69a90 100644
--- a/.env
+++ b/.env
@@ -24,7 +24,7 @@ COMPOSE_PROFILES="desktop, isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="3"  # Number of robot containers to launch.
+NUM_ROBOTS="2"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== SIMULATION =====================
diff --git a/.gitignore b/.gitignore
index cabc5025..1d4a13fd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -82,3 +82,7 @@ simulation/isaac-sim/launch_scripts/AbandonedFactory_Launch.py
 simulation/isaac-sim/launch_scripts/ConstructionSite_Launch.py
 simulation/isaac-sim/launch_scripts/FireAcademy_Launch.py
 simulation/isaac-sim/launch_scripts/RetroNeighborhood_Launch.py
+
+# Persisted waypoint/polygon editor saves (host-side mount target)
+gcs/saves/*
+!gcs/saves/.gitkeep
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index 852011de..74f13918 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -61,6 +61,8 @@ services:
       - ../../.devcontainer/gcs/launch.json:/root/AirStack/.vscode/launch.json:rw
       - ../../.devcontainer/gcs/tasks.json:/root/AirStack/.vscode/tasks.json:rw
       - ./Foxglove:/root/.config/Foxglove:rw
+      # waypoint/polygon editor saves (so they survive container restarts)
+      - ../saves:/root/.airstack:rw
       # foxglove extensions
       - ../foxglove_extensions:/root/AirStack/gcs/foxglove_extensions:ro
       # autonomy stack stuff
diff --git a/gcs/foxglove_extensions/polygon-editor/dist/extension.js b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
index cf2a65d3..5334b9f2 100644
--- a/gcs/foxglove_extensions/polygon-editor/dist/extension.js
+++ b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
@@ -5,6 +5,7 @@
 
 const CMD_TOPIC = "/gcs/polygon/command";
 const LIST_TOPIC = "/gcs/polygon/list";
+const SAVES_TOPIC = "/gcs/polygon/saves";
 const CMD_SCHEMA = "std_msgs/msg/String";
 
 // ─────────────────────────── panel ───────────────────────────────────────────
@@ -22,6 +23,7 @@ function activate(extensionContext) {
       let vertices = [];  // [{x, y, z}, ...]
       let selectedIdx = -1;
       let enabled = false;  // synced from /gcs/polygon/list
+      let saves = [];  // synced from /gcs/polygon/saves
 
       function persist() { panelContext.saveState(state); }
       function sendCmd(cmd) {
@@ -111,10 +113,97 @@ function activate(extensionContext) {
       addRow.appendChild(addBtn);
       root.appendChild(addRow);
 
+      // ── Saves section ────────────────────────────────────────────────
+      const savesLabel = el("div",
+        "font-size:11px;font-weight:bold;opacity:0.8;margin-top:6px;flex-shrink:0;");
+      savesLabel.textContent = "Saves";
+      root.appendChild(savesLabel);
+
+      const saveAddRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const saveNameIn = el("input",
+        "flex:1;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;");
+      saveNameIn.type = "text";
+      saveNameIn.placeholder = "save name…";
+      const saveAddBtn = el("button",
+        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+      saveAddBtn.textContent = "+ Add";
+      saveAddBtn.addEventListener("click", () => {
+        const name = saveNameIn.value.trim();
+        if (!name) return;
+        sendCmd({ action: "add_save", name });
+      });
+      saveAddRow.appendChild(saveNameIn);
+      saveAddRow.appendChild(saveAddBtn);
+      root.appendChild(saveAddRow);
+
+      const saveList = el("div",
+        "border:1px solid #333;border-radius:4px;min-height:0;flex-shrink:0;");
+      root.appendChild(saveList);
+
       // Status
       const statusEl = el("div", "font-size:11px;opacity:0.6;flex-shrink:0;");
       root.appendChild(statusEl);
 
+      function renderSaves() {
+        saveList.replaceChildren();
+        if (saves.length === 0) {
+          const empty = el("div", "padding:6px 8px;opacity:0.5;font-size:11px;");
+          empty.textContent = "No saves yet. Type a name and click + Add.";
+          saveList.appendChild(empty);
+          return;
+        }
+        for (const s of saves) {
+          const row = el("div",
+            "display:flex;align-items:center;gap:6px;padding:4px 6px;border-bottom:1px solid #333;font-size:12px;");
+          const swatch = el("span",
+            "display:inline-block;width:10px;height:10px;border-radius:50%;flex-shrink:0;");
+          const [r, g, b] = s.color || [0.5, 0.5, 0.5];
+          swatch.style.background =
+            `rgb(${Math.round(r*255)},${Math.round(g*255)},${Math.round(b*255)})`;
+          row.appendChild(swatch);
+
+          const nameEl = el("span", "flex:1;font-family:monospace;");
+          nameEl.textContent = `${s.name} (${s.count} vert${s.count === 1 ? "" : "s"})`;
+          row.appendChild(nameEl);
+
+          const loadBtn = el("button",
+            "padding:2px 8px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:11px;");
+          loadBtn.textContent = "Load";
+          loadBtn.addEventListener("click", () => {
+            sendCmd({ action: "load_save", name: s.name });
+          });
+          row.appendChild(loadBtn);
+
+          const saveBtn = el("button", "");
+          if (s.saved) {
+            saveBtn.textContent = "✓ Saved";
+            saveBtn.disabled = true;
+            saveBtn.style.cssText =
+              "padding:2px 8px;border-radius:3px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:11px;";
+          } else {
+            saveBtn.textContent = "Save";
+            saveBtn.style.cssText =
+              "padding:2px 8px;border-radius:3px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:11px;";
+            saveBtn.addEventListener("click", () => {
+              sendCmd({ action: "save_save", name: s.name });
+            });
+          }
+          row.appendChild(saveBtn);
+
+          const delBtn = el("button",
+            "padding:2px 6px;border-radius:3px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:13px;");
+          delBtn.textContent = "✕";
+          delBtn.title = "Delete save";
+          delBtn.addEventListener("click", () => {
+            sendCmd({ action: "delete_save", name: s.name });
+          });
+          row.appendChild(delBtn);
+
+          saveList.appendChild(row);
+        }
+      }
+      renderSaves();
+
       // ── render helpers ───────────────────────────────────────────────
       function renderEnableBtn() {
         if (enabled) {
@@ -201,7 +290,10 @@ function activate(extensionContext) {
       renderList();
 
       // ── subscriptions ────────────────────────────────────────────────
-      panelContext.subscribe([{ topic: LIST_TOPIC }]);
+      panelContext.subscribe([
+        { topic: LIST_TOPIC },
+        { topic: SAVES_TOPIC },
+      ]);
       panelContext.watch("currentFrame");
 
       panelContext.onRender = (renderState, done) => {
@@ -223,6 +315,12 @@ function activate(extensionContext) {
                 if (selectedIdx >= vertices.length) selectedIdx = -1;
                 renderList();
               } catch { /* ignore bad data */ }
+            } else if (evt.topic === SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                saves = Array.isArray(data.saves) ? data.saves : [];
+                renderSaves();
+              } catch { /* ignore bad data */ }
             }
           }
         }
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index fc0ce384..795ea0fb 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -21,15 +21,46 @@ const TERMINAL_STATUSES = new Set([
 
 // Topic the Waypoint Editor publishes its current list on (std_msgs/String JSON).
 const EDITOR_LIST_TOPIC = "/gcs/waypoints/list";
+const EDITOR_SAVES_TOPIC = "/gcs/waypoints/saves";
 
 // Topic the Polygon Editor publishes its current vertex list on.
 const POLYGON_LIST_TOPIC = "/gcs/polygon/list";
+const POLYGON_SAVES_TOPIC = "/gcs/polygon/saves";
 
 // Module-level caches of latest editor data, refreshed in the panel's onRender.
 // The "Grab from Editor" button on each polygon/path field copies these into
 // the textbox.
 let editorWaypointsCache = [];
 let polygonVerticesCache = [];
+// Save caches: {name: {color, vertices}} keyed by save name.
+let editorSavesCache = {};
+let polygonSavesCache = {};
+
+// Registry of (kind, <select>) pairs so we can refresh them when save caches
+// update without rebuilding the whole panel. kind is "polygon" or "path".
+const sourceDropdowns = [];
+
+function refreshSourceDropdowns() {
+  for (const { kind, select } of sourceDropdowns) {
+    const cache = kind === "polygon" ? polygonSavesCache : editorSavesCache;
+    const names = Object.keys(cache).sort();
+    const prev = select.value;
+    select.replaceChildren();
+    const activeOpt = document.createElement("option");
+    activeOpt.value = "";
+    activeOpt.textContent = "active";
+    select.appendChild(activeOpt);
+    for (const name of names) {
+      const opt = document.createElement("option");
+      opt.value = name;
+      opt.textContent = name;
+      select.appendChild(opt);
+    }
+    // Preserve prior selection if the save still exists.
+    if (prev === "" || names.includes(prev)) select.value = prev;
+    else select.value = "";
+  }
+}
 
 // Per-tab runtime (feedback lines, last result, status, active flag) lives at
 // module scope so it survives Foxglove panel re-instantiation (layout swaps,
@@ -600,7 +631,10 @@ function activate(extensionContext) {
       // Subscribe to ALL relay topics for all tabs so we catch results
       // even when the user switches tabs during execution.
       function rebuildSubscriptions() {
-        const topics = [EDITOR_LIST_TOPIC, POLYGON_LIST_TOPIC];
+        const topics = [
+          EDITOR_LIST_TOPIC, EDITOR_SAVES_TOPIC,
+          POLYGON_LIST_TOPIC, POLYGON_SAVES_TOPIC,
+        ];
         for (const tab of TASK_TABS) {
           if (perTab[tab.id].active) {
             topics.push(`/${state.robot}/${tab.actionSuffix}/relay_feedback`);
@@ -731,6 +765,24 @@ function activate(extensionContext) {
                 const data = JSON.parse(evt.message?.data ?? "{}");
                 polygonVerticesCache = Array.isArray(data.vertices) ? data.vertices : [];
               } catch { /* ignore bad data */ }
+            } else if (evt.topic === EDITOR_SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                editorSavesCache = {};
+                for (const s of (data.saves ?? [])) {
+                  editorSavesCache[s.name] = { color: s.color, vertices: s.vertices };
+                }
+                refreshSourceDropdowns();
+              } catch { /* ignore bad data */ }
+            } else if (evt.topic === POLYGON_SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message?.data ?? "{}");
+                polygonSavesCache = {};
+                for (const s of (data.saves ?? [])) {
+                  polygonSavesCache[s.name] = { color: s.color, vertices: s.vertices };
+                }
+                refreshSourceDropdowns();
+              } catch { /* ignore bad data */ }
             } else if (evt.topic.endsWith("/relay_feedback")) {
               handleRelayFeedback(evt.topic, evt.message);
             } else if (evt.topic.endsWith("/relay_result")) {
@@ -883,15 +935,34 @@ function buildField(field, tabState, persist) {
       validate();
       details.appendChild(ta);
 
-      // "Grab from Editor" button — snapshots the matching editor cache
-      // (waypoints for path fields, polygon vertices for polygon fields) into
-      // the textbox so the user can capture, modify the editor, capture again
-      // for another command/drone, etc.
+      // "Grab from <source>" — source dropdown lets the user pick the active
+      // editor or any saved save. The dropdown is registered globally so its
+      // option list refreshes whenever the save caches update.
+      const grabRow = document.createElement("div");
+      grabRow.style.cssText = "display:flex;align-items:center;gap:6px;margin-top:4px;";
+
+      const sourceLabel = document.createElement("span");
+      sourceLabel.textContent = "from:";
+      sourceLabel.style.cssText = "font-size:11px;opacity:0.7;";
+      grabRow.appendChild(sourceLabel);
+
+      const sourceSelect = document.createElement("select");
+      sourceSelect.style.cssText = "flex:1;min-width:80px;padding:3px 4px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;";
+      grabRow.appendChild(sourceSelect);
+      sourceDropdowns.push({ kind: field.kind, select: sourceSelect });
+
       const grabBtn = document.createElement("button");
-      grabBtn.textContent = "Grab from Editor";
-      grabBtn.style.cssText = "margin-top:4px;padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:12px;";
+      grabBtn.textContent = "Grab";
+      grabBtn.style.cssText = "padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:12px;";
       grabBtn.addEventListener("click", () => {
-        const cache = field.kind === "polygon" ? polygonVerticesCache : editorWaypointsCache;
+        const sel = sourceSelect.value;
+        let cache;
+        if (sel === "" || sel == null) {
+          cache = field.kind === "polygon" ? polygonVerticesCache : editorWaypointsCache;
+        } else {
+          const saves = field.kind === "polygon" ? polygonSavesCache : editorSavesCache;
+          cache = saves[sel]?.vertices ?? [];
+        }
         const json = editorCacheToJson(cache);
         ta.value = json;
         tabState[field.name] = json;
@@ -899,7 +970,11 @@ function buildField(field, tabState, persist) {
         renderSummary();
         persist();
       });
-      details.appendChild(grabBtn);
+      grabRow.appendChild(grabBtn);
+      details.appendChild(grabRow);
+
+      // Populate dropdown immediately with whatever is in the cache.
+      refreshSourceDropdowns();
 
       if (field.hint) {
         const h = document.createElement("span");
diff --git a/gcs/foxglove_extensions/waypoint-editor/dist/extension.js b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
index 241d490e..3087a1de 100644
--- a/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
+++ b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
@@ -5,6 +5,7 @@
 
 const CMD_TOPIC = "/gcs/waypoints/command";
 const LIST_TOPIC = "/gcs/waypoints/list";
+const SAVES_TOPIC = "/gcs/waypoints/saves";
 const CMD_SCHEMA = "std_msgs/msg/String";
 
 // ─────────────────────────── panel ───────────────────────────────────────────
@@ -22,6 +23,7 @@ function activate(extensionContext) {
       let waypoints = [];  // [{x, y, z}, ...]
       let selectedIdx = -1;
       let enabled = false;  // synced from /gcs/waypoints/list
+      let saves = [];  // [{name, color, count, saved, vertices}, ...] from /gcs/waypoints/saves
 
       function persist() { panelContext.saveState(state); }
       function sendCmd(cmd) {
@@ -111,10 +113,97 @@ function activate(extensionContext) {
       addRow.appendChild(addBtn);
       root.appendChild(addRow);
 
+      // ── Saves section ────────────────────────────────────────────────
+      const savesLabel = el("div",
+        "font-size:11px;font-weight:bold;opacity:0.8;margin-top:6px;flex-shrink:0;");
+      savesLabel.textContent = "Saves";
+      root.appendChild(savesLabel);
+
+      const saveAddRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const saveNameIn = el("input",
+        "flex:1;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;");
+      saveNameIn.type = "text";
+      saveNameIn.placeholder = "save name…";
+      const saveAddBtn = el("button",
+        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+      saveAddBtn.textContent = "+ Add";
+      saveAddBtn.addEventListener("click", () => {
+        const name = saveNameIn.value.trim();
+        if (!name) return;
+        sendCmd({ action: "add_save", name });
+      });
+      saveAddRow.appendChild(saveNameIn);
+      saveAddRow.appendChild(saveAddBtn);
+      root.appendChild(saveAddRow);
+
+      const saveList = el("div",
+        "border:1px solid #333;border-radius:4px;min-height:0;flex-shrink:0;");
+      root.appendChild(saveList);
+
       // Status
       const statusEl = el("div", "font-size:11px;opacity:0.6;flex-shrink:0;");
       root.appendChild(statusEl);
 
+      function renderSaves() {
+        saveList.replaceChildren();
+        if (saves.length === 0) {
+          const empty = el("div", "padding:6px 8px;opacity:0.5;font-size:11px;");
+          empty.textContent = "No saves yet. Type a name and click + Add.";
+          saveList.appendChild(empty);
+          return;
+        }
+        for (const s of saves) {
+          const row = el("div",
+            "display:flex;align-items:center;gap:6px;padding:4px 6px;border-bottom:1px solid #333;font-size:12px;");
+          const swatch = el("span",
+            "display:inline-block;width:10px;height:10px;border-radius:50%;flex-shrink:0;");
+          const [r, g, b] = s.color || [0.5, 0.5, 0.5];
+          swatch.style.background =
+            `rgb(${Math.round(r*255)},${Math.round(g*255)},${Math.round(b*255)})`;
+          row.appendChild(swatch);
+
+          const nameEl = el("span", "flex:1;font-family:monospace;");
+          nameEl.textContent = `${s.name} (${s.count} wp)`;
+          row.appendChild(nameEl);
+
+          const loadBtn = el("button",
+            "padding:2px 8px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:11px;");
+          loadBtn.textContent = "Load";
+          loadBtn.addEventListener("click", () => {
+            sendCmd({ action: "load_save", name: s.name });
+          });
+          row.appendChild(loadBtn);
+
+          const saveBtn = el("button", "");
+          if (s.saved) {
+            saveBtn.textContent = "✓ Saved";
+            saveBtn.disabled = true;
+            saveBtn.style.cssText =
+              "padding:2px 8px;border-radius:3px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:11px;";
+          } else {
+            saveBtn.textContent = "Save";
+            saveBtn.style.cssText =
+              "padding:2px 8px;border-radius:3px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:11px;";
+            saveBtn.addEventListener("click", () => {
+              sendCmd({ action: "save_save", name: s.name });
+            });
+          }
+          row.appendChild(saveBtn);
+
+          const delBtn = el("button",
+            "padding:2px 6px;border-radius:3px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:13px;");
+          delBtn.textContent = "✕";
+          delBtn.title = "Delete save";
+          delBtn.addEventListener("click", () => {
+            sendCmd({ action: "delete_save", name: s.name });
+          });
+          row.appendChild(delBtn);
+
+          saveList.appendChild(row);
+        }
+      }
+      renderSaves();
+
       // ── render helpers ───────────────────────────────────────────────
       function renderEnableBtn() {
         if (enabled) {
@@ -206,7 +295,10 @@ function activate(extensionContext) {
       renderList();
 
       // ── subscriptions ────────────────────────────────────────────────
-      panelContext.subscribe([{ topic: LIST_TOPIC }]);
+      panelContext.subscribe([
+        { topic: LIST_TOPIC },
+        { topic: SAVES_TOPIC },
+      ]);
       panelContext.watch("currentFrame");
 
       panelContext.onRender = (renderState, done) => {
@@ -229,6 +321,12 @@ function activate(extensionContext) {
                 if (selectedIdx >= waypoints.length) selectedIdx = -1;
                 renderList();
               } catch { /* ignore bad data */ }
+            } else if (evt.topic === SAVES_TOPIC) {
+              try {
+                const data = JSON.parse(evt.message.data);
+                saves = Array.isArray(data.saves) ? data.saves : [];
+                renderSaves();
+              } catch { /* ignore bad data */ }
             }
           }
         }
diff --git a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
index fb6495d8..b7ef096d 100644
--- a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
+++ b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
@@ -23,6 +23,7 @@
 import json
 import math
 import threading
+import time
 
 import rclpy
 from rclpy.action import ActionClient
@@ -229,6 +230,10 @@ def _make_relay(node0, nodeN, executorN, topic, suffix, action_type,
     active_goal = {'handle': None}
     cancel_event = threading.Event()
 
+    # Tasks exempt from the "must be airborne" precondition.
+    AIRBORNE_EXEMPT = {'takeoff', 'land'}
+    MIN_AIRBORNE_Z_M = 5.0
+
     def on_goal_str(msg):
         """Receive JSON goal from Foxglove, parse, forward as action."""
         try:
@@ -238,6 +243,27 @@ def on_goal_str(msg):
             _publish_result(False, f'Invalid JSON: {e}')
             return
 
+        # Airborne precondition: every task except takeoff/land requires the
+        # drone to be at least MIN_AIRBORNE_Z_M above its takeoff altitude.
+        if suffix not in AIRBORNE_EXEMPT:
+            cur_z = transform_state.get('current_z')
+            if cur_z is None:
+                node0.get_logger().warn(
+                    f'[relay] {topic}: rejected — no GPS altitude yet')
+                _publish_result(
+                    False,
+                    'Robot altitude unknown (no GPS fix yet) — takeoff first.')
+                return
+            if cur_z < MIN_AIRBORNE_Z_M:
+                node0.get_logger().warn(
+                    f'[relay] {topic}: rejected — drone at {cur_z:.2f}m AGL, '
+                    f'need >= {MIN_AIRBORNE_Z_M}m')
+                _publish_result(
+                    False,
+                    f'Takeoff first — drone is at {cur_z:.2f}m AGL '
+                    f'(need >= {MIN_AIRBORNE_Z_M:.0f}m).')
+                return
+
         try:
             goal_msg = goal_builder(data, transform_state)
         except RuntimeError as e:
@@ -270,28 +296,55 @@ def on_goal_str(msg):
         node0.get_logger().info(f'[relay] {topic}: goal accepted')
         feedback_pub.publish(String(data='Goal accepted by robot'))
 
-        result_future = robot_goal_handle.get_result_async()
-        cancel_sent = False
-        while not result_future.done():
-            if cancel_event.is_set() and not cancel_sent:
-                # Issue cancel from the same thread that owns executorN to
-                # avoid racing the rcl ActionClient.
-                robot_goal_handle.cancel_goal_async()
-                cancel_sent = True
-                feedback_pub.publish(String(data='Cancel forwarded to robot'))
-            executorN.spin_once(timeout_sec=0.05)
-        cancel_event.clear()
-
-        active_goal['handle'] = None
-        wrapped = result_future.result()
-        robot_result = wrapped.result
-        status = wrapped.status
-        status_str = {4: 'SUCCEEDED', 5: 'CANCELED'}.get(status, 'ABORTED')
-        node0.get_logger().info(f'[relay] {topic}: {status_str}')
-
-        success = getattr(robot_result, 'success', status == 4)
-        message = getattr(robot_result, 'message', status_str)
-        _publish_result(success, message)
+        # Drive the result loop. We always end this block by publishing a
+        # relay_result so the panel can move on, even if the robot's executor
+        # gets stuck on cancel or result_future.result() blows up.
+        try:
+            result_future = robot_goal_handle.get_result_async()
+            cancel_sent = False
+            cancel_deadline = None
+            CANCEL_TIMEOUT_SEC = 10.0
+
+            while not result_future.done():
+                if cancel_event.is_set():
+                    if not cancel_sent:
+                        # Issue cancel from the same thread that owns executorN
+                        # to avoid racing the rcl ActionClient.
+                        robot_goal_handle.cancel_goal_async()
+                        cancel_sent = True
+                        cancel_deadline = time.monotonic() + CANCEL_TIMEOUT_SEC
+                        feedback_pub.publish(String(data='Cancel forwarded to robot'))
+                    elif cancel_deadline is not None and time.monotonic() > cancel_deadline:
+                        node0.get_logger().warn(
+                            f'[relay] {topic}: cancel timed out after '
+                            f'{CANCEL_TIMEOUT_SEC:.0f}s — forcing CANCELED result')
+                        break
+                executorN.spin_once(timeout_sec=0.05)
+
+            if result_future.done():
+                wrapped = result_future.result()
+                robot_result = wrapped.result
+                status = wrapped.status
+                status_str = {4: 'SUCCEEDED', 5: 'CANCELED'}.get(status, 'ABORTED')
+                success = getattr(robot_result, 'success', status == 4)
+                message = getattr(robot_result, 'message', status_str)
+            else:
+                # cancel timed out — server didn't return a result in time
+                status_str = 'CANCELED'
+                success = False
+                message = 'Canceled (server did not respond within timeout)'
+
+            node0.get_logger().info(f'[relay] {topic}: {status_str}')
+            _publish_result(success, message)
+        except Exception as e:
+            node0.get_logger().error(f'[relay] {topic}: result loop failed: {e}')
+            try:
+                _publish_result(False, f'Result handling failed: {e}')
+            except Exception:
+                pass
+        finally:
+            active_goal['handle'] = None
+            cancel_event.clear()
 
     def on_cancel(msg):
         """Signal the goal-handler loop to forward a cancel to the robot."""
@@ -363,7 +416,10 @@ def main(args=None):
     # Track this robot's boot ENU position so we can convert global-frame
     # waypoints/polygons (from the Foxglove editors) to robot-local before
     # forwarding. First valid GPS fix is treated as boot, matching gcs_visualizer.
-    transform_state = {'boot': None, 'alt_ground': None}
+    # current_z is the most recent altitude AGL (relative to alt_ground), used
+    # to gate non-takeoff tasks so the user can't accidentally send a navigate
+    # command while the drone is on the ground.
+    transform_state = {'boot': None, 'alt_ground': None, 'current_z': None}
     gps_qos = QoSProfile(
         depth=1,
         reliability=ReliabilityPolicy.BEST_EFFORT,
@@ -383,6 +439,7 @@ def _on_gps(msg: NavSatFix):
             node0.get_logger().info(
                 f"[relay] {robot_name} boot ENU = "
                 f"({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})")
+        transform_state['current_z'] = msg.altitude - transform_state['alt_ground']
 
     node0.create_subscription(
         NavSatFix,
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
index f9e9a323..47c4fc75 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
@@ -1,5 +1,5 @@
 """Polygon collector: receives click points from Foxglove 3D panel,
-manages a polygon vertex list, and publishes markers + vertex list.
+manages a polygon vertex list and named saves, and publishes markers.
 
 Commands arrive as JSON on /gcs/polygon/command (std_msgs/String):
   {"action": "add", "x": 1.0, "y": 2.0, "z": 0.0}
@@ -8,16 +8,24 @@
   {"action": "reorder", "from": 2, "to": 0}
   {"action": "clear"}
   {"action": "set_altitude", "z": 0.0}  (sets default z for clicks)
+  {"action": "add_save", "name": "north"}   (snapshot active vertices, in-memory)
+  {"action": "save_save", "name": "north"}  (persist that save to disk)
+  {"action": "load_save", "name": "north"}  (replace active list with save)
+  {"action": "delete_save", "name": "north"}
 
 Publishes:
-  /gcs/polygon/markers  (MarkerArray — red vertex spheres + closed red LINE_STRIP)
-  /gcs/polygon/list     (String — JSON {vertices: [{x,y,z},...], default_z: float})
+  /gcs/polygon/markers       (MarkerArray — active editor only, red)
+  /gcs/polygon/list          (String — JSON {vertices, default_z, enabled})
+  /gcs/polygon/save_markers  (MarkerArray — each save rendered in its color)
+  /gcs/polygon/saves         (String JSON — save metadata)
 
-The polygon is rendered as a closed loop (last vertex connects back to first)
-so the user sees the area they're outlining.
+The active polygon is rendered as a closed loop so the user sees the area
+they're outlining; same for each save in its own color.
 """
 
+import copy
 import json
+import os
 
 import rclpy
 from rclpy.node import Node
@@ -41,6 +49,47 @@
 _TEXT_COLOR = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0)
 _VERTEX_RADIUS = 0.35
 _LINE_WIDTH = 0.15
+_SAVE_NUMBER_SCALE = 0.8
+
+_SAVES_PATH = os.path.expanduser('~/.airstack/gcs_polygon_saves.json')
+
+_SAVE_PALETTE = [
+    (0.20, 0.80, 1.00),  # cyan
+    (1.00, 0.30, 0.80),  # magenta
+    (0.50, 1.00, 0.20),  # lime
+    (1.00, 0.60, 0.10),  # orange
+    (1.00, 0.50, 0.70),  # pink
+    (1.00, 0.95, 0.10),  # yellow
+    (0.40, 0.70, 1.00),  # sky
+    (0.65, 0.35, 1.00),  # violet
+    (1.00, 0.45, 0.45),  # coral
+    (0.40, 1.00, 0.75),  # mint
+]
+
+
+def _next_palette_color(used_colors) -> tuple[float, float, float]:
+    """Lowest palette index not currently in use; wrap if all 10 are taken."""
+    used = {tuple(round(x, 6) for x in c) for c in used_colors}
+    for entry in _SAVE_PALETTE:
+        if tuple(round(x, 6) for x in entry) not in used:
+            return entry
+    return _SAVE_PALETTE[len(used) % len(_SAVE_PALETTE)]
+
+
+def _load_saves_from_disk() -> dict:
+    try:
+        with open(_SAVES_PATH, 'r') as f:
+            return json.load(f)
+    except (FileNotFoundError, json.JSONDecodeError):
+        return {}
+
+
+def _atomic_write_saves(saves_to_persist: dict):
+    os.makedirs(os.path.dirname(_SAVES_PATH), exist_ok=True)
+    tmp = _SAVES_PATH + '.tmp'
+    with open(tmp, 'w') as f:
+        json.dump(saves_to_persist, f, indent=2)
+    os.replace(tmp, _SAVES_PATH)
 
 
 class PolygonCollectorNode(Node):
@@ -54,6 +103,21 @@ def __init__(self):
         # polygon editors can share /clicked_point without conflicting.
         self._enabled = False
 
+        # Named saves (in-memory) and a mirror of what's been persisted.
+        self._saves: dict = {}
+        # Track namespaces last published so DELETEALL can wipe them on edit.
+        self._published_save_names: set[str] = set()
+        self._saved_on_disk = _load_saves_from_disk()
+        for name, payload in self._saved_on_disk.items():
+            color = payload.get('color')
+            if color is None:
+                color = list(_next_palette_color(
+                    s['color'] for s in self._saves.values()))
+            self._saves[name] = {
+                'color': color,
+                'vertices': payload.get('vertices', []),
+            }
+
         self.create_subscription(
             PointStamped, '/clicked_point', self._on_click, 10)
         self.create_subscription(
@@ -63,9 +127,15 @@ def __init__(self):
             MarkerArray, '/gcs/polygon/markers', _LATCHED)
         self._list_pub = self.create_publisher(
             String, '/gcs/polygon/list', _LATCHED)
+        self._save_marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/polygon/save_markers', _LATCHED)
+        self._saves_meta_pub = self.create_publisher(
+            String, '/gcs/polygon/saves', _LATCHED)
 
         self._publish_all()
-        self.get_logger().info('Polygon collector ready')
+        self._publish_saves()
+        self.get_logger().info(
+            f'Polygon collector ready ({len(self._saves)} saves loaded)')
 
     def _on_click(self, msg: PointStamped):
         if not self._enabled:
@@ -134,6 +204,62 @@ def _on_command(self, msg: String):
             self._publish_list()
             return
 
+        elif action == 'add_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name:
+                return
+            color = (self._saves[name]['color']
+                     if name in self._saves
+                     else list(_next_palette_color(
+                         s['color'] for s in self._saves.values())))
+            self._saves[name] = {
+                'color': color,
+                'vertices': [dict(v) for v in self._vertices],
+            }
+            self.get_logger().info(
+                f'Added save {name!r} with {len(self._vertices)} vertices')
+            self._publish_saves()
+            return
+
+        elif action == 'save_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name or name not in self._saves:
+                return
+            self._saved_on_disk[name] = copy.deepcopy(self._saves[name])
+            try:
+                _atomic_write_saves(self._saved_on_disk)
+                self.get_logger().info(f'Saved save {name!r} to disk')
+            except OSError as e:
+                self.get_logger().warn(f'Failed to save save {name!r}: {e}')
+                self._saved_on_disk.pop(name, None)
+            self._publish_saves()
+            return
+
+        elif action == 'load_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._vertices = [dict(v) for v in self._saves[name]['vertices']]
+            self.get_logger().info(
+                f'Loaded save {name!r} into active editor '
+                f'({len(self._vertices)} vertices)')
+
+        elif action == 'delete_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._saves.pop(name, None)
+            if name in self._saved_on_disk:
+                self._saved_on_disk.pop(name)
+                try:
+                    _atomic_write_saves(self._saved_on_disk)
+                except OSError as e:
+                    self.get_logger().warn(
+                        f'Failed to update saves file after delete: {e}')
+            self.get_logger().info(f'Deleted save {name!r}')
+            self._publish_saves()
+            return
+
         else:
             self.get_logger().warn(f'Unknown command: {action}')
             return
@@ -216,6 +342,86 @@ def _publish_list(self):
             'enabled': self._enabled,
         })))
 
+    # ── Save publishing ──────────────────────────────────────────────────
+    def _publish_saves(self):
+        """Publish save metadata + save MarkerArray for all named saves."""
+        now = self.get_clock().now().to_msg()
+
+        meta = []
+        for name, save in self._saves.items():
+            saved = (name in self._saved_on_disk and
+                     self._saved_on_disk[name].get('vertices') == save['vertices'])
+            meta.append({
+                'name': name,
+                'color': save['color'],
+                'count': len(save['vertices']),
+                'saved': saved,
+                'vertices': save['vertices'],
+            })
+        self._saves_meta_pub.publish(String(data=json.dumps({'saves': meta})))
+
+        # DELETEALL only clears markers in its own namespace, so wipe both
+        # currently-known and previously-published namespaces explicitly.
+        array = MarkerArray()
+        current_names = set(self._saves.keys())
+        for ns_name in (self._published_save_names | current_names):
+            wipe = Marker()
+            wipe.header.frame_id = self._frame_id
+            wipe.header.stamp = now
+            wipe.ns = f'polygon_saves/{ns_name}'
+            wipe.action = Marker.DELETEALL
+            array.markers.append(wipe)
+
+        next_id = 0
+        for name, save in self._saves.items():
+            r, g, b = save['color']
+            color = ColorRGBA(r=r, g=g, b=b, a=1.0)
+            ns = f'polygon_saves/{name}'
+            verts = save['vertices']
+            if not verts:
+                continue
+
+            # Closed line strip outline (last vertex back to first).
+            if len(verts) >= 2:
+                line = Marker()
+                line.header.frame_id = self._frame_id
+                line.header.stamp = now
+                line.ns = ns
+                line.id = next_id
+                next_id += 1
+                line.type = Marker.LINE_STRIP
+                line.action = Marker.ADD
+                line.scale.x = _LINE_WIDTH
+                line.color = color
+                line.pose.orientation.w = 1.0
+                for v in verts:
+                    line.points.append(Point(x=v['x'], y=v['y'], z=v['z']))
+                line.points.append(
+                    Point(x=verts[0]['x'], y=verts[0]['y'], z=verts[0]['z']))
+                array.markers.append(line)
+
+            # Name label floating above the first vertex so the user can
+            # tell which polygon belongs to which save at a glance.
+            first = verts[0]
+            label = Marker()
+            label.header.frame_id = self._frame_id
+            label.header.stamp = now
+            label.ns = ns
+            label.id = next_id
+            next_id += 1
+            label.type = Marker.TEXT_VIEW_FACING
+            label.action = Marker.ADD
+            label.pose.position = Point(
+                x=first['x'], y=first['y'], z=first['z'] + 0.6)
+            label.pose.orientation.w = 1.0
+            label.scale.z = _SAVE_NUMBER_SCALE
+            label.color = color
+            label.text = name
+            array.markers.append(label)
+
+        self._save_marker_pub.publish(array)
+        self._published_save_names = current_names
+
 
 def main(args=None):
     rclpy.init(args=args)
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
index 27f2af6d..c2ca75f5 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
@@ -1,5 +1,5 @@
 """Waypoint collector: receives click points from Foxglove 3D panel,
-manages a waypoint list, and publishes markers + path.
+manages a waypoint list and named saves, and publishes markers + path.
 
 Commands arrive as JSON on /gcs/waypoints/command (std_msgs/String):
   {"action": "add", "x": 1.0, "y": 2.0, "z": 5.0}
@@ -8,14 +8,22 @@
   {"action": "reorder", "from": 2, "to": 0}
   {"action": "clear"}
   {"action": "set_altitude", "z": 10.0}  (sets default altitude for clicks)
+  {"action": "add_save", "name": "robot_1"}    (snapshot active list, in-memory)
+  {"action": "save_save", "name": "robot_1"}   (persist that save to disk)
+  {"action": "load_save", "name": "robot_1"}   (replace active list with save)
+  {"action": "delete_save", "name": "robot_1"} (remove save)
 
 Publishes:
-  /gcs/waypoints/markers  (MarkerArray — spheres, labels, connecting lines)
-  /gcs/waypoints/list     (String — JSON array of waypoints for the panel)
-  /gcs/waypoints/path     (nav_msgs/Path — for use by navigate task)
+  /gcs/waypoints/markers       (MarkerArray — active editor only)
+  /gcs/waypoints/list          (String — JSON of active waypoints for the panel)
+  /gcs/waypoints/path          (nav_msgs/Path — for use by navigate task)
+  /gcs/waypoints/save_markers  (MarkerArray — every save rendered in its color)
+  /gcs/waypoints/saves         (String JSON — save metadata for editor + tasks)
 """
 
+import copy
 import json
+import os
 
 import rclpy
 from rclpy.node import Node
@@ -40,6 +48,52 @@
 _TEXT_COLOR = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0)
 _STAR_SCALE = 1.0
 
+# Save persistence
+_SAVES_PATH = os.path.expanduser('~/.airstack/gcs_waypoint_saves.json')
+
+# Visually distinct palette for saves — auto-assigned by hashing the save name
+# so the same name keeps the same color across sessions.
+_SAVE_PALETTE = [
+    (0.20, 0.80, 1.00),  # cyan
+    (1.00, 0.30, 0.80),  # magenta
+    (0.50, 1.00, 0.20),  # lime
+    (1.00, 0.60, 0.10),  # orange
+    (1.00, 0.50, 0.70),  # pink
+    (1.00, 0.95, 0.10),  # yellow
+    (0.40, 0.70, 1.00),  # sky
+    (0.65, 0.35, 1.00),  # violet
+    (1.00, 0.45, 0.45),  # coral
+    (0.40, 1.00, 0.75),  # mint
+]
+
+
+def _next_palette_color(used_colors) -> tuple[float, float, float]:
+    """Pick the lowest palette index not currently in use; wrap if all are taken.
+
+    `used_colors` is an iterable of [r, g, b] lists from existing saves.
+    """
+    used = {tuple(round(x, 6) for x in c) for c in used_colors}
+    for entry in _SAVE_PALETTE:
+        if tuple(round(x, 6) for x in entry) not in used:
+            return entry
+    return _SAVE_PALETTE[len(used) % len(_SAVE_PALETTE)]
+
+
+def _load_saves_from_disk() -> dict:
+    try:
+        with open(_SAVES_PATH, 'r') as f:
+            return json.load(f)
+    except (FileNotFoundError, json.JSONDecodeError):
+        return {}
+
+
+def _atomic_write_saves(saves_to_persist: dict):
+    os.makedirs(os.path.dirname(_SAVES_PATH), exist_ok=True)
+    tmp = _SAVES_PATH + '.tmp'
+    with open(tmp, 'w') as f:
+        json.dump(saves_to_persist, f, indent=2)
+    os.replace(tmp, _SAVES_PATH)
+
 
 class WaypointCollectorNode(Node):
     def __init__(self):
@@ -53,6 +107,26 @@ def __init__(self):
         # stealing each other's clicks.
         self._enabled = False
 
+        # Named saves: in-memory by default; the user explicitly Saves to
+        # persist. Disk holds only the saves the user has Saved.
+        # Shape: {name: {'color': [r,g,b], 'vertices': [{x,y,z}, ...]}}
+        self._saves: dict = {}
+        # Track the namespaces we last published so we can DELETEALL stale
+        # ones (Marker.DELETEALL only clears markers in its own namespace).
+        self._published_save_names: set[str] = set()
+        self._saved_on_disk = _load_saves_from_disk()
+        # On startup, in-memory mirrors disk. If a save on disk has no color
+        # (e.g. a hand-edited file), assign the next available palette entry.
+        for name, payload in self._saved_on_disk.items():
+            color = payload.get('color')
+            if color is None:
+                color = list(_next_palette_color(
+                    s['color'] for s in self._saves.values()))
+            self._saves[name] = {
+                'color': color,
+                'vertices': payload.get('vertices', []),
+            }
+
         # Subscribers
         self.create_subscription(
             PointStamped, '/clicked_point', self._on_click, 10)
@@ -66,10 +140,16 @@ def __init__(self):
             String, '/gcs/waypoints/list', _LATCHED)
         self._path_pub = self.create_publisher(
             Path, '/gcs/waypoints/path', _LATCHED)
+        self._save_marker_pub = self.create_publisher(
+            MarkerArray, '/gcs/waypoints/save_markers', _LATCHED)
+        self._saves_meta_pub = self.create_publisher(
+            String, '/gcs/waypoints/saves', _LATCHED)
 
-        # Publish initial empty state
+        # Publish initial empty state (active + saves loaded from disk)
         self._publish_all()
-        self.get_logger().info('Waypoint collector ready')
+        self._publish_saves()
+        self.get_logger().info(
+            f'Waypoint collector ready ({len(self._saves)} saves loaded)')
 
     # ── Click handler ────────────────────────────────────────────────────
     def _on_click(self, msg: PointStamped):
@@ -140,6 +220,64 @@ def _on_command(self, msg: String):
             self._publish_list()
             return
 
+        elif action == 'add_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name:
+                return
+            color = (self._saves[name]['color']
+                     if name in self._saves
+                     else list(_next_palette_color(
+                         s['color'] for s in self._saves.values())))
+            self._saves[name] = {
+                'color': color,
+                'vertices': [dict(wp) for wp in self._waypoints],
+            }
+            self.get_logger().info(
+                f'Added save {name!r} with {len(self._waypoints)} waypoints')
+            self._publish_saves()
+            return
+
+        elif action == 'save_save':
+            name = str(cmd.get('name', '')).strip()
+            if not name or name not in self._saves:
+                return
+            # Deep-copy so further in-memory edits don't silently mutate the
+            # on-disk mirror and break the `saved` comparison.
+            self._saved_on_disk[name] = copy.deepcopy(self._saves[name])
+            try:
+                _atomic_write_saves(self._saved_on_disk)
+                self.get_logger().info(f'Saved save {name!r} to disk')
+            except OSError as e:
+                self.get_logger().warn(f'Failed to save save {name!r}: {e}')
+                self._saved_on_disk.pop(name, None)
+            self._publish_saves()
+            return
+
+        elif action == 'load_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._waypoints = [dict(wp) for wp in self._saves[name]['vertices']]
+            self.get_logger().info(
+                f'Loaded save {name!r} into active editor '
+                f'({len(self._waypoints)} waypoints)')
+
+        elif action == 'delete_save':
+            name = str(cmd.get('name', '')).strip()
+            if name not in self._saves:
+                return
+            self._saves.pop(name, None)
+            if name in self._saved_on_disk:
+                self._saved_on_disk.pop(name)
+                try:
+                    _atomic_write_saves(self._saved_on_disk)
+                except OSError as e:
+                    self.get_logger().warn(
+                        f'Failed to update saves file after delete: {e}')
+            self.get_logger().info(f'Deleted save {name!r}')
+            self._publish_saves()
+            return
+
         else:
             self.get_logger().warn(f'Unknown command: {action}')
             return
@@ -218,6 +356,102 @@ def _publish_path(self, stamp: Time):
             path.poses.append(ps)
         self._path_pub.publish(path)
 
+    # ── Save publishing ──────────────────────────────────────────────────
+    def _publish_saves(self):
+        """Publish both the save metadata JSON and the save MarkerArray."""
+        now = self.get_clock().now().to_msg()
+
+        # Metadata for editor + Robot Tasks panel
+        meta = []
+        for name, save in self._saves.items():
+            saved = (name in self._saved_on_disk and
+                     self._saved_on_disk[name].get('vertices') == save['vertices'])
+            meta.append({
+                'name': name,
+                'color': save['color'],
+                'count': len(save['vertices']),
+                'saved': saved,
+                'vertices': save['vertices'],
+            })
+        self._saves_meta_pub.publish(String(data=json.dumps({'saves': meta})))
+
+        # Marker array — all saves in one publish, each in its own namespace.
+        # DELETEALL only clears markers in its own ns, so emit one DELETEALL
+        # per previously- or currently-known save namespace before re-adding.
+        array = MarkerArray()
+        current_names = set(self._saves.keys())
+        for ns_name in (self._published_save_names | current_names):
+            wipe = Marker()
+            wipe.header.frame_id = self._frame_id
+            wipe.header.stamp = now
+            wipe.ns = f'waypoint_saves/{ns_name}'
+            wipe.action = Marker.DELETEALL
+            array.markers.append(wipe)
+
+        next_id = 0
+        for name, save in self._saves.items():
+            r, g, b = save['color']
+            color = ColorRGBA(r=r, g=g, b=b, a=1.0)
+            ns = f'waypoint_saves/{name}'
+
+            # Big colored index numbers — these ARE the waypoint markers.
+            for i, wp in enumerate(save['vertices']):
+                num = Marker()
+                num.header.frame_id = self._frame_id
+                num.header.stamp = now
+                num.ns = ns
+                num.id = next_id
+                next_id += 1
+                num.type = Marker.TEXT_VIEW_FACING
+                num.action = Marker.ADD
+                num.pose.position = Point(
+                    x=wp['x'], y=wp['y'], z=wp['z'])
+                num.pose.orientation.w = 1.0
+                num.scale.z = _STAR_SCALE
+                num.color = color
+                num.text = str(i)
+                array.markers.append(num)
+
+            # Connecting line through the waypoints (route order)
+            if len(save['vertices']) >= 2:
+                line = Marker()
+                line.header.frame_id = self._frame_id
+                line.header.stamp = now
+                line.ns = ns
+                line.id = next_id
+                next_id += 1
+                line.type = Marker.LINE_STRIP
+                line.action = Marker.ADD
+                line.scale.x = 0.1
+                line.color = color
+                line.pose.orientation.w = 1.0
+                for wp in save['vertices']:
+                    line.points.append(
+                        Point(x=wp['x'], y=wp['y'], z=wp['z']))
+                array.markers.append(line)
+
+            # Save-name label floating above the first waypoint.
+            if save['vertices']:
+                first = save['vertices'][0]
+                label = Marker()
+                label.header.frame_id = self._frame_id
+                label.header.stamp = now
+                label.ns = ns
+                label.id = next_id
+                next_id += 1
+                label.type = Marker.TEXT_VIEW_FACING
+                label.action = Marker.ADD
+                label.pose.position = Point(
+                    x=first['x'], y=first['y'], z=first['z'] + 1.2)
+                label.pose.orientation.w = 1.0
+                label.scale.z = _STAR_SCALE
+                label.color = color
+                label.text = name
+                array.markers.append(label)
+
+        self._save_marker_pub.publish(array)
+        self._published_save_names = current_names
+
 
 def main(args=None):
     rclpy.init(args=args)
diff --git a/gcs/saves/.gitkeep b/gcs/saves/.gitkeep
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index 0aa9064b..cd50c1c5 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -8,16 +8,19 @@
     <!-- Planning -->
     <include file="$(find-pkg-share semantic_search_task)/launch/semantic_search_task.launch.xml" />
 
-    <node pkg="random_walk_planner" exec="random_walk_planner" name="random_walk_planner"
-        output="screen">
+    <!-- The C++ constructor names the node "random_walk_node" (don't set
+         launch name= or it will rename and break the remap source paths).
+         The parent launch pushes a $(env ROBOT_NAME) namespace, so we use
+         RELATIVE remap sources here so they resolve to the namespaced
+         topics at runtime. -->
+    <node pkg="random_walk_planner" exec="random_walk_planner" output="screen">
         <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
-        <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
-        <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
-        <remap from="~/global_plan_toggle" to="/$(env ROBOT_NAME)/behavior/global_plan_toggle" />
-        <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
-        <!-- Branch additions: ExplorationTask action server + NavigateTask client -->
-        <remap from="~/exploration_task" to="/$(env ROBOT_NAME)/tasks/exploration" />
-        <remap from="navigate_task" to="/$(env ROBOT_NAME)/tasks/navigate" />
+        <remap from="vdb_map_visualization" to="vdb_mapping/vdb_map_visualization" />
+        <remap from="odometry" to="odometry_conversion/odometry" />
+        <remap from="random_walk_node/global_plan" to="global_plan" />
+        <remap from="random_walk_node/global_plan_toggle" to="behavior/global_plan_toggle" />
+        <remap from="random_walk_node/exploration_task" to="tasks/exploration" />
+        <remap from="navigate_task" to="tasks/navigate" />
     </node>
 
 </launch>
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
index 9353ac5a..c27b3f4e 100644
--- a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
@@ -177,16 +177,20 @@ def _handle_goal(self, goal_request):
         return GoalResponse.ACCEPT
 
     def _cleanup_existing(self) -> None:
-        """Kill any leftover rayfronts, raven, or random_walk processes."""
-        for pattern in ['rayfronts.mapping_server', 'raven_nav_node',
-                        'random_walk_planner']:
+        """Kill any leftover rayfronts or raven processes.
+
+        random_walk_planner is intentionally NOT killed here: it now runs as
+        a long-lived task executor under global_bringup, and pkill'ing it
+        would tear down the ExplorationTask action server every time this
+        node initializes or starts a goal.
+        """
+        for pattern in ['rayfronts.mapping_server', 'raven_nav_node']:
             result = subprocess.run(
                 ['pkill', '-SIGTERM', '-f', pattern], capture_output=True)
             if result.returncode == 0:
                 self.get_logger().info(f'Killed existing {pattern} process(es)')
         time.sleep(2.0)
-        for pattern in ['rayfronts.mapping_server', 'raven_nav_node',
-                        'random_walk_planner']:
+        for pattern in ['rayfronts.mapping_server', 'raven_nav_node']:
             subprocess.run(['pkill', '-SIGKILL', '-f', pattern], capture_output=True)
 
     def _spawn(self, cmd: list) -> tuple:
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 8b4ff2a8..4dd695b2 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -74,7 +74,6 @@
 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": 20.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},
 ]
 # ---------------------------------------------------------
 

From 5ee9c77d6d24c3610aa75a7897cd8de9dc0e8fc1 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 28 Apr 2026 12:56:29 -0400
Subject: [PATCH 44/55] fixed bounds for exploration task, combined
 waypoint/polygon editor into task panel

---
 .../robot-commands/dist/extension.js          | 275 ++++++++++++++++++
 .../random_walk/include/random_walk_logic.hpp |  20 +-
 .../random_walk/include/random_walk_node.hpp  |   9 +
 .../random_walk/src/random_walk_logic.cpp     |  93 ++++++
 .../random_walk/src/random_walk_node.cpp      | 105 ++++++-
 5 files changed, 499 insertions(+), 3 deletions(-)

diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index 795ea0fb..93c04bef 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -35,11 +35,24 @@ let polygonVerticesCache = [];
 // Save caches: {name: {color, vertices}} keyed by save name.
 let editorSavesCache = {};
 let polygonSavesCache = {};
+// Capture-toggle + default-altitude state per editor (synced from list topics).
+let editorEnabled = false;
+let polygonEnabled = false;
+let editorDefaultZ = 10.0;
+let polygonDefaultZ = 0.0;
 
 // Registry of (kind, <select>) pairs so we can refresh them when save caches
 // update without rebuilding the whole panel. kind is "polygon" or "path".
 const sourceDropdowns = [];
 
+// Registry of inline editor sections — multiple instances live at once (one
+// per tab body) and all reflect the same shared state. Each entry exposes
+// a render() that re-paints from the module caches on every update.
+const editorSections = { waypoint: [], polygon: [] };
+function refreshEditorSections(kind) {
+  for (const section of editorSections[kind]) section.render();
+}
+
 function refreshSourceDropdowns() {
   for (const { kind, select } of sourceDropdowns) {
     const cache = kind === "polygon" ? polygonSavesCache : editorSavesCache;
@@ -518,6 +531,20 @@ function activate(extensionContext) {
           for (const field of [...simple, ...complex]) {
             body.appendChild(buildField(field, state[tab.id], persist));
           }
+          // If this tab takes a path or polygon, embed the matching editor as
+          // a collapsible section so the user doesn't need a separate panel.
+          // The underlying state is shared across all instances via the
+          // /gcs/<editor>/list and /saves topics, so editing in one tab
+          // immediately reflects in another (e.g. polygon drawn in Exploration
+          // shows up live in Semantic Search's editor section).
+          const hasPath = tab.fields.some((f) => f.kind === "path");
+          const hasPolygon = tab.fields.some((f) => f.kind === "polygon");
+          if (hasPath) {
+            body.appendChild(buildEditorSection("waypoint", panelContext));
+          }
+          if (hasPolygon) {
+            body.appendChild(buildEditorSection("polygon", panelContext));
+          }
         }
 
         tabBodies[tab.id] = body;
@@ -759,11 +786,17 @@ function activate(extensionContext) {
               try {
                 const data = JSON.parse(evt.message?.data ?? "{}");
                 editorWaypointsCache = Array.isArray(data.waypoints) ? data.waypoints : [];
+                if (data.default_z != null) editorDefaultZ = Number(data.default_z) || 0;
+                if (data.enabled != null) editorEnabled = Boolean(data.enabled);
+                refreshEditorSections("waypoint");
               } catch { /* ignore bad data */ }
             } else if (evt.topic === POLYGON_LIST_TOPIC) {
               try {
                 const data = JSON.parse(evt.message?.data ?? "{}");
                 polygonVerticesCache = Array.isArray(data.vertices) ? data.vertices : [];
+                if (data.default_z != null) polygonDefaultZ = Number(data.default_z) || 0;
+                if (data.enabled != null) polygonEnabled = Boolean(data.enabled);
+                refreshEditorSections("polygon");
               } catch { /* ignore bad data */ }
             } else if (evt.topic === EDITOR_SAVES_TOPIC) {
               try {
@@ -773,6 +806,7 @@ function activate(extensionContext) {
                   editorSavesCache[s.name] = { color: s.color, vertices: s.vertices };
                 }
                 refreshSourceDropdowns();
+                refreshEditorSections("waypoint");
               } catch { /* ignore bad data */ }
             } else if (evt.topic === POLYGON_SAVES_TOPIC) {
               try {
@@ -782,6 +816,7 @@ function activate(extensionContext) {
                   polygonSavesCache[s.name] = { color: s.color, vertices: s.vertices };
                 }
                 refreshSourceDropdowns();
+                refreshEditorSections("polygon");
               } catch { /* ignore bad data */ }
             } else if (evt.topic.endsWith("/relay_feedback")) {
               handleRelayFeedback(evt.topic, evt.message);
@@ -1094,5 +1129,245 @@ function buildFixedTrajectoryForm(tabState, persist) {
   return wrapper;
 }
 
+// ─────────────────────────── inline editor section ────────────────────────
+
+// Builds a collapsible section that mirrors the standalone Waypoint or
+// Polygon Editor's UI but lives inside a Robot Tasks tab body. Multiple
+// instances can coexist (one per tab); they all share state via the
+// /gcs/<editor>/list and /gcs/<editor>/saves topics, so an edit made in one
+// instance is immediately reflected in the others.
+//
+// kind:    "waypoint" | "polygon"
+// returns: a <details> element to append to a tab body.
+function buildEditorSection(kind, panelContext) {
+  const cfg = kind === "polygon"
+    ? { title: "Polygon Editor", cmdTopic: "/gcs/polygon/command",
+        listField: "vertices", noun: "vert", nounPlural: "vertices",
+        primary: "#dc2626" }
+    : { title: "Waypoint Editor", cmdTopic: "/gcs/waypoints/command",
+        listField: "waypoints", noun: "wp", nounPlural: "waypoints",
+        primary: "#10b981" };
+
+  const sendCmd = (cmd) => {
+    try {
+      panelContext.advertise(cfg.cmdTopic, "std_msgs/msg/String");
+      panelContext.publish(cfg.cmdTopic, { data: JSON.stringify(cmd) });
+    } catch (err) {
+      statusEl.textContent = "Cmd failed: " + (err?.message ?? err);
+    }
+  };
+
+  const details = document.createElement("details");
+  details.style.cssText = "border:1px solid #444;border-radius:4px;padding:6px 8px;margin-top:4px;";
+  const summary = document.createElement("summary");
+  summary.style.cssText = "cursor:pointer;user-select:none;font-weight:bold;font-size:13px;";
+  details.appendChild(summary);
+
+  const wrap = document.createElement("div");
+  wrap.style.cssText = "display:flex;flex-direction:column;gap:6px;margin-top:6px;";
+  details.appendChild(wrap);
+
+  // Capture toggle + altitude + clear
+  const ctrlRow = document.createElement("div");
+  ctrlRow.style.cssText = "display:flex;align-items:center;gap:8px;";
+  const enableBtn = document.createElement("button");
+  enableBtn.style.cssText = "padding:6px 12px;border-radius:4px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:12px;";
+  enableBtn.addEventListener("click", () => {
+    const enabled = (kind === "polygon") ? polygonEnabled : editorEnabled;
+    sendCmd({ action: "set_enabled", enabled: !enabled });
+  });
+  const altLabel = document.createElement("span");
+  altLabel.textContent = "Z:";
+  altLabel.style.fontSize = "12px";
+  const altInput = document.createElement("input");
+  altInput.type = "number";
+  altInput.step = "0.5";
+  altInput.style.cssText = "width:60px;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+  altInput.addEventListener("change", () => {
+    sendCmd({ action: "set_altitude", z: Number(altInput.value) || 0 });
+  });
+  const clearBtn = document.createElement("button");
+  clearBtn.textContent = "Clear";
+  clearBtn.style.cssText = "padding:5px 10px;border-radius:4px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:12px;";
+  clearBtn.addEventListener("click", () => sendCmd({ action: "clear" }));
+  ctrlRow.appendChild(enableBtn);
+  ctrlRow.appendChild(altLabel);
+  ctrlRow.appendChild(altInput);
+  const spacer = document.createElement("span");
+  spacer.style.flex = "1";
+  ctrlRow.appendChild(spacer);
+  ctrlRow.appendChild(clearBtn);
+  wrap.appendChild(ctrlRow);
+
+  // Vertex list
+  const listContainer = document.createElement("div");
+  listContainer.style.cssText = "max-height:160px;overflow-y:auto;border:1px solid #333;border-radius:4px;min-height:40px;";
+  wrap.appendChild(listContainer);
+
+  // Manual add row
+  const addRow = document.createElement("div");
+  addRow.style.cssText = "display:flex;align-items:center;gap:4px;";
+  const mkNumIn = (label, def) => {
+    const w = document.createElement("div");
+    w.style.cssText = "display:flex;align-items:center;gap:2px;";
+    const l = document.createElement("span");
+    l.textContent = label + ":"; l.style.cssText = "font-size:11px;opacity:0.7;";
+    const i = document.createElement("input");
+    i.type = "number"; i.step = "0.5"; i.value = def;
+    i.style.cssText = "width:50px;padding:2px 4px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;font-size:11px;";
+    w.appendChild(l); w.appendChild(i);
+    return { wrap: w, input: i };
+  };
+  const addX = mkNumIn("X", "0"), addY = mkNumIn("Y", "0"), addZ = mkNumIn("Z", "0");
+  const addBtn = document.createElement("button");
+  addBtn.textContent = "+ Add";
+  addBtn.style.cssText = "padding:4px 10px;border-radius:4px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:12px;";
+  addBtn.addEventListener("click", () => {
+    sendCmd({
+      action: "add",
+      x: Number(addX.input.value) || 0,
+      y: Number(addY.input.value) || 0,
+      z: Number(addZ.input.value) || 0,
+    });
+  });
+  addRow.appendChild(addX.wrap); addRow.appendChild(addY.wrap); addRow.appendChild(addZ.wrap);
+  addRow.appendChild(addBtn);
+  wrap.appendChild(addRow);
+
+  // Saves section
+  const savesLabel = document.createElement("div");
+  savesLabel.textContent = "Saves";
+  savesLabel.style.cssText = "font-size:11px;font-weight:bold;opacity:0.8;margin-top:4px;";
+  wrap.appendChild(savesLabel);
+
+  const saveAddRow = document.createElement("div");
+  saveAddRow.style.cssText = "display:flex;align-items:center;gap:4px;";
+  const saveNameIn = document.createElement("input");
+  saveNameIn.type = "text"; saveNameIn.placeholder = "save name…";
+  saveNameIn.style.cssText = "flex:1;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;";
+  const saveAddBtn = document.createElement("button");
+  saveAddBtn.textContent = "+ Add";
+  saveAddBtn.style.cssText = "padding:4px 10px;border-radius:4px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:12px;";
+  saveAddBtn.addEventListener("click", () => {
+    const name = saveNameIn.value.trim();
+    if (!name) return;
+    sendCmd({ action: "add_save", name });
+  });
+  saveAddRow.appendChild(saveNameIn); saveAddRow.appendChild(saveAddBtn);
+  wrap.appendChild(saveAddRow);
+
+  const savesList = document.createElement("div");
+  savesList.style.cssText = "border:1px solid #333;border-radius:4px;min-height:0;";
+  wrap.appendChild(savesList);
+
+  const statusEl = document.createElement("div");
+  statusEl.style.cssText = "font-size:11px;opacity:0.6;";
+  wrap.appendChild(statusEl);
+
+  // ── render ──────────────────────────────────────────────────────
+  const section = {
+    render() {
+      const cache = (kind === "polygon") ? polygonVerticesCache : editorWaypointsCache;
+      const enabled = (kind === "polygon") ? polygonEnabled : editorEnabled;
+      const defaultZ = (kind === "polygon") ? polygonDefaultZ : editorDefaultZ;
+      const savesCache = (kind === "polygon") ? polygonSavesCache : editorSavesCache;
+
+      summary.textContent = `${cfg.title} (${cache.length} ${cache.length === 1 ? cfg.noun : cfg.nounPlural})`;
+
+      enableBtn.textContent = enabled ? "Capture: ON" : "Capture: OFF";
+      enableBtn.style.background = enabled ? "#10b981" : "#dc2626";
+      if (document.activeElement !== altInput) altInput.value = String(defaultZ);
+
+      // Vertex list
+      listContainer.replaceChildren();
+      if (cache.length === 0) {
+        const empty = document.createElement("div");
+        empty.style.cssText = "padding:8px;text-align:center;opacity:0.5;font-size:11px;";
+        empty.textContent = `No ${cfg.nounPlural}. Enable capture and click in 3D, or add manually.`;
+        listContainer.appendChild(empty);
+      } else {
+        for (let i = 0; i < cache.length; i++) {
+          const v = cache[i];
+          const row = document.createElement("div");
+          row.style.cssText = "display:flex;align-items:center;gap:3px;padding:3px 5px;border-bottom:1px solid #333;font-size:11px;font-family:monospace;";
+          const idx = document.createElement("span");
+          idx.textContent = String(i);
+          idx.style.cssText = `width:18px;font-weight:bold;color:${cfg.primary};`;
+          row.appendChild(idx);
+          const mkCoord = (val, axis) => {
+            const inp = document.createElement("input");
+            inp.type = "number"; inp.step = "0.5"; inp.value = String(val);
+            inp.style.cssText = "width:50px;padding:1px 3px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;font-family:monospace;font-size:11px;text-align:right;";
+            inp.addEventListener("change", () => {
+              const nx = axis === "x" ? Number(inp.value) || 0 : v.x;
+              const ny = axis === "y" ? Number(inp.value) || 0 : v.y;
+              const nz = axis === "z" ? Number(inp.value) || 0 : v.z;
+              sendCmd({ action: "move", index: i, x: nx, y: ny, z: nz });
+            });
+            return inp;
+          };
+          row.appendChild(mkCoord(v.x, "x"));
+          row.appendChild(mkCoord(v.y, "y"));
+          row.appendChild(mkCoord(v.z, "z"));
+          const mkBtn = (text, on) => {
+            const b = document.createElement("button");
+            b.textContent = text;
+            b.style.cssText = "width:22px;height:22px;padding:0;border:none;background:transparent;color:inherit;cursor:pointer;font-size:11px;border-radius:3px;";
+            b.addEventListener("click", on);
+            return b;
+          };
+          if (i > 0) row.appendChild(mkBtn("▲", () => sendCmd({ action: "reorder", from: i, to: i - 1 })));
+          else { const sp = document.createElement("span"); sp.style.width = "22px"; row.appendChild(sp); }
+          if (i < cache.length - 1) row.appendChild(mkBtn("▼", () => sendCmd({ action: "reorder", from: i, to: i + 1 })));
+          else { const sp = document.createElement("span"); sp.style.width = "22px"; row.appendChild(sp); }
+          const delBtn = mkBtn("✕", () => sendCmd({ action: "delete", index: i }));
+          delBtn.style.color = "#dc2626";
+          row.appendChild(delBtn);
+          listContainer.appendChild(row);
+        }
+      }
+
+      // Saves
+      savesList.replaceChildren();
+      const names = Object.keys(savesCache).sort();
+      if (names.length === 0) {
+        const empty = document.createElement("div");
+        empty.style.cssText = "padding:5px 8px;opacity:0.5;font-size:11px;";
+        empty.textContent = "No saves yet.";
+        savesList.appendChild(empty);
+      } else {
+        for (const name of names) {
+          const s = savesCache[name];
+          const row = document.createElement("div");
+          row.style.cssText = "display:flex;align-items:center;gap:5px;padding:3px 5px;border-bottom:1px solid #333;font-size:11px;";
+          const sw = document.createElement("span");
+          sw.style.cssText = "display:inline-block;width:9px;height:9px;border-radius:50%;flex-shrink:0;";
+          const c = s.color || [0.5, 0.5, 0.5];
+          sw.style.background = `rgb(${Math.round(c[0]*255)},${Math.round(c[1]*255)},${Math.round(c[2]*255)})`;
+          row.appendChild(sw);
+          const nm = document.createElement("span");
+          nm.textContent = `${name} (${(s.vertices ?? []).length})`;
+          nm.style.cssText = "flex:1;font-family:monospace;";
+          row.appendChild(nm);
+          const small = (text, color, on) => {
+            const b = document.createElement("button");
+            b.textContent = text;
+            b.style.cssText = `padding:1px 6px;border-radius:3px;border:1px solid #555;background:transparent;color:${color};cursor:pointer;font-size:10px;`;
+            b.addEventListener("click", on);
+            return b;
+          };
+          row.appendChild(small("Load", "inherit", () => sendCmd({ action: "load_save", name })));
+          row.appendChild(small("Save", "#2563eb", () => sendCmd({ action: "save_save", name })));
+          row.appendChild(small("✕", "#dc2626", () => sendCmd({ action: "delete_save", name })));
+          savesList.appendChild(row);
+        }
+      }
+    },
+  };
+  editorSections[kind].push(section);
+  section.render();
+  return details;
+}
+
 module.exports = { activate };
 })();
diff --git a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp
index faa9bd40..32968eb9 100644
--- a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp
+++ b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_logic.hpp
@@ -23,6 +23,7 @@
 #include <chrono>
 #include <cmath>
 #include <iostream>
+#include <limits>
 #include <mutex>
 #include <optional>
 #include <random>
@@ -52,11 +53,28 @@ class RandomWalkPlanner {
         std::tuple<float, float, float, float> start_point,
         float timeout_duration);  // x, y, z, yaw
 
+    // Optional XY footprint that bounds where the planner is allowed to
+    // generate goal points. Empty → unbounded (preserves prior behavior).
+    // Set with set_search_bounds; coordinates are in the same frame as
+    // voxel_points and odometry (the robot's local "map", rooted at takeoff).
+    void set_search_bounds(const std::vector<std::pair<float, float>>& xy_polygon);
+    void clear_search_bounds();
+    bool has_search_bounds() const;
+    bool is_inside_search_bounds(float x, float y) const;
+    // Returns the closest point on the polygon boundary, then nudged a small
+    // distance toward the polygon centroid so the result is firmly inside
+    // (avoids edge-case ambiguity with the inside test). If no bounds set
+    // returns (x, y) unchanged.
+    std::pair<float, float> nearest_inside_point(float x, float y) const;
+
     float path_end_threshold_m;
 
     std::vector<std::tuple<float, float, float>> voxel_points;
 
    private:
+    bool point_in_search_bounds(float x, float y) const;
+    std::vector<std::pair<float, float>> search_bounds_xy_;
+
     // Numerical constants
     float max_start_to_goal_dist_m_;
     int checking_point_cnt;
@@ -67,7 +85,7 @@ class RandomWalkPlanner {
     // Variables
     std::tuple<float, float, float> voxel_size_m;
 
-    std::mutex mutex;
+    mutable std::mutex mutex;
 
     // Functions
     bool check_if_collided_single_voxel(const std::tuple<float, float, float>& point,
diff --git a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp
index 55166ea4..bdfdd510 100644
--- a/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp
+++ b/robot/ros_ws/src/global/planners/random_walk/include/random_walk_node.hpp
@@ -25,6 +25,9 @@
 
 #include <atomic>
 #include <cmath>
+#include <mutex>
+#include <utility>
+#include <vector>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -101,6 +104,12 @@ class RandomWalkNode : public rclcpp::Node {
     rclcpp::Time task_start_time_;
     float task_time_limit_sec_ = 0.0f;
 
+    // Latest requested search bounds (XY polygon in robot-local map). Stored
+    // here so we can apply them either when execute() begins or, if the
+    // planner doesn't exist yet, the moment it's constructed in mapCallback.
+    std::vector<std::pair<float, float>> pending_bounds_;
+    std::mutex pending_bounds_mutex_;
+
     // Subscriber callbacks
     void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg);
     void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
diff --git a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp
index 9b82c96a..0bc54e23 100644
--- a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp
+++ b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_logic.cpp
@@ -128,6 +128,93 @@ bool RandomWalkPlanner::check_if_collided_single_voxel(
     }
 }
 
+void RandomWalkPlanner::set_search_bounds(
+    const std::vector<std::pair<float, float>>& xy_polygon)
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    search_bounds_xy_ = xy_polygon;
+}
+
+void RandomWalkPlanner::clear_search_bounds()
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    search_bounds_xy_.clear();
+}
+
+bool RandomWalkPlanner::point_in_search_bounds(float x, float y) const
+{
+    // Empty polygon → unbounded.
+    const auto n = search_bounds_xy_.size();
+    if (n < 3) return true;
+    bool inside = false;
+    for (size_t i = 0, j = n - 1; i < n; j = i++)
+    {
+        const float xi = search_bounds_xy_[i].first;
+        const float yi = search_bounds_xy_[i].second;
+        const float xj = search_bounds_xy_[j].first;
+        const float yj = search_bounds_xy_[j].second;
+        const bool intersects = ((yi > y) != (yj > y)) &&
+                                (x < (xj - xi) * (y - yi) / (yj - yi + 1e-9f) + xi);
+        if (intersects) inside = !inside;
+    }
+    return inside;
+}
+
+bool RandomWalkPlanner::has_search_bounds() const
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    return search_bounds_xy_.size() >= 3;
+}
+
+bool RandomWalkPlanner::is_inside_search_bounds(float x, float y) const
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    return point_in_search_bounds(x, y);
+}
+
+std::pair<float, float> RandomWalkPlanner::nearest_inside_point(float x, float y) const
+{
+    std::lock_guard<std::mutex> lock(this->mutex);
+    const auto n = search_bounds_xy_.size();
+    if (n < 3) return {x, y};
+
+    // Find closest point on any polygon edge.
+    float best_dx = 0.0f, best_dy = 0.0f;
+    float best_d2 = std::numeric_limits<float>::infinity();
+    for (size_t i = 0, j = n - 1; i < n; j = i++)
+    {
+        const float ax = search_bounds_xy_[j].first;
+        const float ay = search_bounds_xy_[j].second;
+        const float bx = search_bounds_xy_[i].first;
+        const float by = search_bounds_xy_[i].second;
+        const float ex = bx - ax;
+        const float ey = by - ay;
+        const float seg_len2 = ex * ex + ey * ey;
+        float t = 0.0f;
+        if (seg_len2 > 1e-9f) t = ((x - ax) * ex + (y - ay) * ey) / seg_len2;
+        if (t < 0.0f) t = 0.0f;
+        if (t > 1.0f) t = 1.0f;
+        const float px = ax + t * ex;
+        const float py = ay + t * ey;
+        const float d2 = (px - x) * (px - x) + (py - y) * (py - y);
+        if (d2 < best_d2) { best_d2 = d2; best_dx = px; best_dy = py; }
+    }
+
+    // Push the closest-edge point toward the polygon centroid so we end up
+    // strictly inside (avoids on-boundary ambiguity for the inside test).
+    float cx = 0.0f, cy = 0.0f;
+    for (const auto& p : search_bounds_xy_) { cx += p.first; cy += p.second; }
+    cx /= static_cast<float>(n);
+    cy /= static_cast<float>(n);
+
+    const float vx = cx - best_dx;
+    const float vy = cy - best_dy;
+    const float vlen = std::sqrt(vx * vx + vy * vy);
+    if (vlen < 1e-3f) return {best_dx, best_dy};
+    const float step = std::min(1.0f, vlen);  // 1 m inward, capped at centroid
+    return {best_dx + vx / vlen * step, best_dy + vy / vlen * step};
+}
+
 bool RandomWalkPlanner::check_if_collided(const std::tuple<float, float, float>& point)
 {
     // make sure the point is within the bounds -30 to 30
@@ -137,6 +224,12 @@ bool RandomWalkPlanner::check_if_collided(const std::tuple<float, float, float>&
         return true;
     }
 
+    // Reject points outside the goal's search_bounds polygon (XY footprint
+    // in robot-local map). Empty polygon → no-op.
+    if (!point_in_search_bounds(std::get<0>(point), std::get<1>(point)))
+    {
+        return true;
+    }
 
     std::lock_guard<std::mutex> lock(this->mutex);
     for (const std::tuple<float, float, float>& voxel : this->voxel_points)
diff --git a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp
index 4027d646..4327ee94 100644
--- a/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp
+++ b/robot/ros_ws/src/global/planners/random_walk/src/random_walk_node.cpp
@@ -141,6 +141,11 @@ void RandomWalkNode::mapCallback(const visualization_msgs::msg::Marker::SharedPt
         this->params.voxel_size_m =
             std::tuple<float, float, float>(msg->scale.x, msg->scale.y, msg->scale.z);
         this->random_walk_planner = std::make_unique<RandomWalkPlanner>(this->params);
+        // Apply any bounds that arrived before the planner existed.
+        {
+            std::lock_guard<std::mutex> lock(pending_bounds_mutex_);
+            this->random_walk_planner->set_search_bounds(pending_bounds_);
+        }
         RCLCPP_INFO(this->get_logger(), "Received first map, initialized planner");
     }
     this->random_walk_planner->voxel_points.clear();
@@ -202,9 +207,26 @@ void RandomWalkNode::execute(std::shared_ptr<GoalHandle> goal_handle) {
     // and will be wired in when the planner supports them.
     RCLCPP_INFO(this->get_logger(),
                 "ExplorationTask executing: alt=[%.1f,%.1f]m AGL, "
-                "speed=[%.1f,%.1f]m/s, time_limit=%.1fs",
+                "speed=[%.1f,%.1f]m/s, time_limit=%.1fs, %zu bounds vertices",
                 goal->min_altitude_agl, goal->max_altitude_agl, goal->min_flight_speed,
-                goal->max_flight_speed, goal->time_limit_sec);
+                goal->max_flight_speed, goal->time_limit_sec,
+                goal->search_bounds.points.size());
+
+    // Snapshot the requested polygon (XY only — Z is governed by altitude_agl
+    // bounds). Stash on the node so it survives a not-yet-constructed planner;
+    // also push to the planner immediately if one exists.
+    {
+        std::vector<std::pair<float, float>> bounds_xy;
+        bounds_xy.reserve(goal->search_bounds.points.size());
+        for (const auto& p : goal->search_bounds.points) {
+            bounds_xy.emplace_back(p.x, p.y);
+        }
+        std::lock_guard<std::mutex> lock(pending_bounds_mutex_);
+        pending_bounds_ = bounds_xy;
+        if (this->random_walk_planner) {
+            this->random_walk_planner->set_search_bounds(pending_bounds_);
+        }
+    }
 
     task_start_time_ = this->now();
     task_time_limit_sec_ = goal->time_limit_sec;
@@ -214,6 +236,85 @@ void RandomWalkNode::execute(std::shared_ptr<GoalHandle> goal_handle) {
     is_path_executing = false;
     generated_paths.clear();
 
+    // If the drone is currently outside the requested polygon, fly it to the
+    // nearest point inside before starting the random walk. Otherwise the
+    // random walker (which generates goals within ~10m of current pose)
+    // would be unable to find any valid candidates and would just stall.
+    if (this->random_walk_planner && this->random_walk_planner->has_search_bounds()) {
+        const float cur_x = static_cast<float>(this->current_location.position.x);
+        const float cur_y = static_cast<float>(this->current_location.position.y);
+        if (!this->random_walk_planner->is_inside_search_bounds(cur_x, cur_y)) {
+            const auto target = this->random_walk_planner->nearest_inside_point(cur_x, cur_y);
+            const float target_z = std::max(
+                static_cast<float>(this->current_location.position.z), 0.5f);
+            RCLCPP_INFO(this->get_logger(),
+                        "Drone at (%.2f,%.2f) is outside search_bounds; "
+                        "navigating to (%.2f,%.2f,%.2f) before exploring",
+                        cur_x, cur_y, target.first, target.second, target_z);
+
+            // Build a single-pose Path in the planner's world frame.
+            nav_msgs::msg::Path approach;
+            approach.header.stamp = this->now();
+            approach.header.frame_id = this->world_frame_id_;
+            geometry_msgs::msg::PoseStamped ps;
+            ps.header = approach.header;
+            ps.pose.position.x = target.first;
+            ps.pose.position.y = target.second;
+            ps.pose.position.z = target_z;
+            ps.pose.orientation.w = 1.0;
+            approach.poses.push_back(ps);
+
+            // Send via the same NavigateTask client used by the random walker.
+            // Reuse navigate_goal_done_/_succeeded so cancel handling and the
+            // result loop remain consistent.
+            navigate_goal_done_ = false;
+            navigate_goal_succeeded_ = false;
+            if (!navigate_client_->wait_for_action_server(std::chrono::seconds(2))) {
+                RCLCPP_WARN(this->get_logger(),
+                            "NavigateTask server unavailable; cannot fly to polygon");
+            } else {
+                auto nav_goal = NavigateTask::Goal();
+                nav_goal.global_plan = approach;
+                nav_goal.goal_tolerance_m = static_cast<float>(
+                    this->random_walk_planner->path_end_threshold_m);
+
+                auto goal_options = rclcpp_action::Client<NavigateTask>::SendGoalOptions();
+                goal_options.goal_response_callback =
+                    [this](const rclcpp_action::ClientGoalHandle<NavigateTask>::SharedPtr& gh) {
+                        navigate_goal_handle_ = gh;
+                        if (!gh) navigate_goal_done_ = true;
+                    };
+                goal_options.result_callback =
+                    [this](const rclcpp_action::ClientGoalHandle<NavigateTask>::WrappedResult& r) {
+                        navigate_goal_succeeded_ =
+                            (r.code == rclcpp_action::ResultCode::SUCCEEDED) && r.result->success;
+                        navigate_goal_done_ = true;
+                    };
+                navigate_client_->async_send_goal(nav_goal, goal_options);
+
+                rclcpp::Rate approach_rate(5.0);
+                while (rclcpp::ok() && !navigate_goal_done_) {
+                    if (cancel_requested_) {
+                        if (navigate_goal_handle_)
+                            navigate_client_->async_cancel_goal(navigate_goal_handle_);
+                        auto result = std::make_shared<ExplorationTask::Result>();
+                        result->success = false;
+                        result->message = "Task cancelled while approaching polygon";
+                        task_active_ = false;
+                        goal_handle->canceled(result);
+                        return;
+                    }
+                    approach_rate.sleep();
+                }
+                if (!navigate_goal_succeeded_) {
+                    RCLCPP_WARN(this->get_logger(),
+                                "Approach to polygon did not succeed; "
+                                "exploration will start from current pose anyway");
+                }
+            }
+        }
+    }
+
     rclcpp::Rate rate(1.0);  // 1 Hz planning loop
 
     while (rclcpp::ok()) {

From f9fe44c31935481b94edb7605084fe489a52ddee Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 28 Apr 2026 14:00:53 -0400
Subject: [PATCH 45/55] made waypoint/polygon gui larger

---
 .../polygon-editor/dist/extension.js          | 82 ++++++++-----------
 .../robot-commands/dist/extension.js          | 60 ++++++++------
 .../waypoint-editor/dist/extension.js         | 61 ++++++++------
 .../foxglove_visualizer_node.py               | 55 ++++++++++++-
 .../gcs_visualizer/polygon_collector_node.py  |  5 ++
 .../gcs_visualizer/waypoint_collector_node.py |  5 ++
 6 files changed, 168 insertions(+), 100 deletions(-)

diff --git a/gcs/foxglove_extensions/polygon-editor/dist/extension.js b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
index 5334b9f2..607a3edf 100644
--- a/gcs/foxglove_extensions/polygon-editor/dist/extension.js
+++ b/gcs/foxglove_extensions/polygon-editor/dist/extension.js
@@ -52,32 +52,20 @@ function activate(extensionContext) {
       const ctrlRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
 
       const enableBtn = el("button",
-        "padding:6px 14px;border-radius:4px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:13px;");
+        "padding:8px 18px;border-radius:5px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:15px;");
       enableBtn.addEventListener("click", () => {
         sendCmd({ action: "set_enabled", enabled: !enabled });
       });
       ctrlRow.appendChild(enableBtn);
 
-      const altLabel = el("span", "font-size:12px;");
-      altLabel.textContent = "Z:";
-      const altInput = el("input",
-        "width:60px;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;");
-      altInput.type = "number";
-      altInput.step = "0.5";
-      altInput.value = String(state.defaultZ);
-      altInput.addEventListener("change", () => {
-        state.defaultZ = Number(altInput.value) || 0.0;
-        persist();
-        sendCmd({ action: "set_altitude", z: state.defaultZ });
-      });
+      // Z is intentionally not exposed for polygons — vertices always carry
+      // z=0 server-side; the consumers treat polygons as 2D footprints.
 
       const clearBtn = el("button",
-        "padding:6px 10px;border-radius:4px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:12px;");
+        "padding:8px 14px;border-radius:5px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:14px;");
       clearBtn.textContent = "Clear All";
       clearBtn.addEventListener("click", () => { sendCmd({ action: "clear" }); });
 
-      ctrlRow.appendChild(altLabel);
-      ctrlRow.appendChild(altInput);
       ctrlRow.appendChild(el("span", "flex:1;"));  // spacer
       ctrlRow.appendChild(clearBtn);
       root.appendChild(ctrlRow);
@@ -91,25 +79,23 @@ function activate(extensionContext) {
         "flex:1;overflow-y:auto;border:1px solid #444;border-radius:4px;min-height:60px;");
       root.appendChild(listContainer);
 
-      // Add vertex manually row
+      // Add vertex manually row (X/Y only — Z is fixed to 0 for polygons)
       const addRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
       const addXIn = numInput("X", "0");
       const addYIn = numInput("Y", "0");
-      const addZIn = numInput("Z", String(state.defaultZ));
       const addBtn = el("button",
-        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
       addBtn.textContent = "+ Add";
       addBtn.addEventListener("click", () => {
         sendCmd({
           action: "add",
           x: Number(addXIn.input.value) || 0,
           y: Number(addYIn.input.value) || 0,
-          z: Number(addZIn.input.value) || state.defaultZ,
+          z: 0,
         });
       });
       addRow.appendChild(addXIn.wrap);
       addRow.appendChild(addYIn.wrap);
-      addRow.appendChild(addZIn.wrap);
       addRow.appendChild(addBtn);
       root.appendChild(addRow);
 
@@ -121,11 +107,11 @@ function activate(extensionContext) {
 
       const saveAddRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
       const saveNameIn = el("input",
-        "flex:1;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;");
+        "flex:1;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;");
       saveNameIn.type = "text";
       saveNameIn.placeholder = "save name…";
       const saveAddBtn = el("button",
-        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
       saveAddBtn.textContent = "+ Add";
       saveAddBtn.addEventListener("click", () => {
         const name = saveNameIn.value.trim();
@@ -154,7 +140,7 @@ function activate(extensionContext) {
         }
         for (const s of saves) {
           const row = el("div",
-            "display:flex;align-items:center;gap:6px;padding:4px 6px;border-bottom:1px solid #333;font-size:12px;");
+            "display:flex;align-items:center;gap:8px;padding:6px 8px;border-bottom:1px solid #333;font-size:14px;");
           const swatch = el("span",
             "display:inline-block;width:10px;height:10px;border-radius:50%;flex-shrink:0;");
           const [r, g, b] = s.color || [0.5, 0.5, 0.5];
@@ -167,7 +153,7 @@ function activate(extensionContext) {
           row.appendChild(nameEl);
 
           const loadBtn = el("button",
-            "padding:2px 8px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:11px;");
+            "padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:13px;");
           loadBtn.textContent = "Load";
           loadBtn.addEventListener("click", () => {
             sendCmd({ action: "load_save", name: s.name });
@@ -179,11 +165,11 @@ function activate(extensionContext) {
             saveBtn.textContent = "✓ Saved";
             saveBtn.disabled = true;
             saveBtn.style.cssText =
-              "padding:2px 8px;border-radius:3px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:11px;";
+              "padding:4px 10px;border-radius:4px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:13px;";
           } else {
             saveBtn.textContent = "Save";
             saveBtn.style.cssText =
-              "padding:2px 8px;border-radius:3px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:11px;";
+              "padding:4px 10px;border-radius:4px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:13px;";
             saveBtn.addEventListener("click", () => {
               sendCmd({ action: "save_save", name: s.name });
             });
@@ -191,7 +177,7 @@ function activate(extensionContext) {
           row.appendChild(saveBtn);
 
           const delBtn = el("button",
-            "padding:2px 6px;border-radius:3px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:13px;");
+            "padding:4px 8px;border-radius:4px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:15px;");
           delBtn.textContent = "✕";
           delBtn.title = "Delete save";
           delBtn.addEventListener("click", () => {
@@ -230,26 +216,22 @@ function activate(extensionContext) {
         for (let i = 0; i < vertices.length; i++) {
           const v = vertices[i];
           const row = el("div",
-            "display:flex;align-items:center;gap:4px;padding:4px 6px;" +
-            "border-bottom:1px solid #333;font-size:12px;font-family:monospace;" +
+            "display:flex;align-items:center;gap:5px;padding:5px 8px;" +
+            "border-bottom:1px solid #333;font-size:14px;font-family:monospace;" +
             (i === selectedIdx ? "background:rgba(220,38,38,0.18);" : ""));
 
-          const idx = el("span", "width:20px;font-weight:bold;color:#dc2626;");
+          const idx = el("span", "width:24px;font-weight:bold;font-size:14px;color:#dc2626;");
           idx.textContent = String(i);
           row.appendChild(idx);
 
           const xIn = coordInput(v.x, (val) => {
-            sendCmd({ action: "move", index: i, x: val, y: v.y, z: v.z });
+            sendCmd({ action: "move", index: i, x: val, y: v.y, z: 0 });
           });
           const yIn = coordInput(v.y, (val) => {
-            sendCmd({ action: "move", index: i, x: v.x, y: val, z: v.z });
-          });
-          const zIn = coordInput(v.z, (val) => {
-            sendCmd({ action: "move", index: i, x: v.x, y: v.y, z: val });
+            sendCmd({ action: "move", index: i, x: v.x, y: val, z: 0 });
           });
           row.appendChild(xIn);
           row.appendChild(yIn);
-          row.appendChild(zIn);
 
           if (i > 0) {
             const upBtn = smallBtn("▲", () => {
@@ -258,7 +240,7 @@ function activate(extensionContext) {
             upBtn.title = "Move up";
             row.appendChild(upBtn);
           } else {
-            row.appendChild(el("span", "width:24px;"));
+            row.appendChild(el("span", "width:32px;"));
           }
 
           if (i < vertices.length - 1) {
@@ -268,9 +250,15 @@ function activate(extensionContext) {
             downBtn.title = "Move down";
             row.appendChild(downBtn);
           } else {
-            row.appendChild(el("span", "width:24px;"));
+            row.appendChild(el("span", "width:32px;"));
           }
 
+          const dupBtn = smallBtn("⧉", () => {
+            sendCmd({ action: "duplicate", index: i });
+          });
+          dupBtn.title = "Duplicate";
+          row.appendChild(dupBtn);
+
           const delBtn = smallBtn("✕", () => {
             sendCmd({ action: "delete", index: i });
           });
@@ -344,8 +332,8 @@ function el(tag, style) {
 
 function smallBtn(text, onClick) {
   const b = el("button",
-    "width:24px;height:24px;padding:0;border:none;background:transparent;" +
-    "color:inherit;cursor:pointer;font-size:12px;border-radius:3px;");
+    "width:32px;height:32px;padding:0;border:none;background:transparent;" +
+    "color:inherit;cursor:pointer;font-size:15px;border-radius:4px;");
   b.textContent = text;
   b.addEventListener("mouseenter", () => { b.style.background = "rgba(255,255,255,0.1)"; });
   b.addEventListener("mouseleave", () => { b.style.background = "transparent"; });
@@ -355,8 +343,8 @@ function smallBtn(text, onClick) {
 
 function coordInput(value, onChange) {
   const inp = el("input",
-    "width:55px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
-    "background:transparent;color:inherit;font-family:monospace;font-size:11px;text-align:right;");
+    "width:72px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-family:monospace;font-size:13px;text-align:right;");
   inp.type = "number";
   inp.step = "0.5";
   inp.value = String(value);
@@ -367,12 +355,12 @@ function coordInput(value, onChange) {
 }
 
 function numInput(label, defaultVal) {
-  const wrap = el("div", "display:flex;align-items:center;gap:2px;");
-  const lbl = el("span", "font-size:11px;opacity:0.7;");
+  const wrap = el("div", "display:flex;align-items:center;gap:4px;");
+  const lbl = el("span", "font-size:13px;opacity:0.75;");
   lbl.textContent = label + ":";
   const input = el("input",
-    "width:50px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
-    "background:transparent;color:inherit;font-size:11px;");
+    "width:64px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-size:13px;");
   input.type = "number";
   input.step = "0.5";
   input.value = defaultVal;
diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index 93c04bef..28e4ad90 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -1171,28 +1171,31 @@ function buildEditorSection(kind, panelContext) {
   const ctrlRow = document.createElement("div");
   ctrlRow.style.cssText = "display:flex;align-items:center;gap:8px;";
   const enableBtn = document.createElement("button");
-  enableBtn.style.cssText = "padding:6px 12px;border-radius:4px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:12px;";
+  enableBtn.style.cssText = "padding:8px 16px;border-radius:5px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:14px;";
   enableBtn.addEventListener("click", () => {
     const enabled = (kind === "polygon") ? polygonEnabled : editorEnabled;
     sendCmd({ action: "set_enabled", enabled: !enabled });
   });
   const altLabel = document.createElement("span");
   altLabel.textContent = "Z:";
-  altLabel.style.fontSize = "12px";
+  altLabel.style.fontSize = "14px";
   const altInput = document.createElement("input");
   altInput.type = "number";
   altInput.step = "0.5";
-  altInput.style.cssText = "width:60px;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;";
+  altInput.style.cssText = "width:80px;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;";
   altInput.addEventListener("change", () => {
     sendCmd({ action: "set_altitude", z: Number(altInput.value) || 0 });
   });
   const clearBtn = document.createElement("button");
   clearBtn.textContent = "Clear";
-  clearBtn.style.cssText = "padding:5px 10px;border-radius:4px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:12px;";
+  clearBtn.style.cssText = "padding:8px 14px;border-radius:5px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:14px;";
   clearBtn.addEventListener("click", () => sendCmd({ action: "clear" }));
   ctrlRow.appendChild(enableBtn);
-  ctrlRow.appendChild(altLabel);
-  ctrlRow.appendChild(altInput);
+  // Z control is only meaningful for waypoints — polygons are 2D.
+  if (kind !== "polygon") {
+    ctrlRow.appendChild(altLabel);
+    ctrlRow.appendChild(altInput);
+  }
   const spacer = document.createElement("span");
   spacer.style.flex = "1";
   ctrlRow.appendChild(spacer);
@@ -1209,28 +1212,31 @@ function buildEditorSection(kind, panelContext) {
   addRow.style.cssText = "display:flex;align-items:center;gap:4px;";
   const mkNumIn = (label, def) => {
     const w = document.createElement("div");
-    w.style.cssText = "display:flex;align-items:center;gap:2px;";
+    w.style.cssText = "display:flex;align-items:center;gap:4px;";
     const l = document.createElement("span");
-    l.textContent = label + ":"; l.style.cssText = "font-size:11px;opacity:0.7;";
+    l.textContent = label + ":"; l.style.cssText = "font-size:13px;opacity:0.75;";
     const i = document.createElement("input");
     i.type = "number"; i.step = "0.5"; i.value = def;
-    i.style.cssText = "width:50px;padding:2px 4px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;font-size:11px;";
+    i.style.cssText = "width:64px;padding:5px 7px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:13px;";
+    l.style.fontSize = "13px";
     w.appendChild(l); w.appendChild(i);
     return { wrap: w, input: i };
   };
-  const addX = mkNumIn("X", "0"), addY = mkNumIn("Y", "0"), addZ = mkNumIn("Z", "0");
+  const addX = mkNumIn("X", "0"), addY = mkNumIn("Y", "0");
+  const addZ = (kind === "polygon") ? null : mkNumIn("Z", "0");
   const addBtn = document.createElement("button");
   addBtn.textContent = "+ Add";
-  addBtn.style.cssText = "padding:4px 10px;border-radius:4px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:12px;";
+  addBtn.style.cssText = "padding:6px 14px;border-radius:5px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:14px;";
   addBtn.addEventListener("click", () => {
     sendCmd({
       action: "add",
       x: Number(addX.input.value) || 0,
       y: Number(addY.input.value) || 0,
-      z: Number(addZ.input.value) || 0,
+      z: addZ ? (Number(addZ.input.value) || 0) : 0,
     });
   });
-  addRow.appendChild(addX.wrap); addRow.appendChild(addY.wrap); addRow.appendChild(addZ.wrap);
+  addRow.appendChild(addX.wrap); addRow.appendChild(addY.wrap);
+  if (addZ) addRow.appendChild(addZ.wrap);
   addRow.appendChild(addBtn);
   wrap.appendChild(addRow);
 
@@ -1244,10 +1250,10 @@ function buildEditorSection(kind, panelContext) {
   saveAddRow.style.cssText = "display:flex;align-items:center;gap:4px;";
   const saveNameIn = document.createElement("input");
   saveNameIn.type = "text"; saveNameIn.placeholder = "save name…";
-  saveNameIn.style.cssText = "flex:1;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;";
+  saveNameIn.style.cssText = "flex:1;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;";
   const saveAddBtn = document.createElement("button");
   saveAddBtn.textContent = "+ Add";
-  saveAddBtn.style.cssText = "padding:4px 10px;border-radius:4px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:12px;";
+  saveAddBtn.style.cssText = "padding:6px 14px;border-radius:5px;border:none;background:" + cfg.primary + ";color:white;cursor:pointer;font-size:14px;";
   saveAddBtn.addEventListener("click", () => {
     const name = saveNameIn.value.trim();
     if (!name) return;
@@ -1289,15 +1295,15 @@ function buildEditorSection(kind, panelContext) {
         for (let i = 0; i < cache.length; i++) {
           const v = cache[i];
           const row = document.createElement("div");
-          row.style.cssText = "display:flex;align-items:center;gap:3px;padding:3px 5px;border-bottom:1px solid #333;font-size:11px;font-family:monospace;";
+          row.style.cssText = "display:flex;align-items:center;gap:5px;padding:5px 8px;border-bottom:1px solid #333;font-size:14px;font-family:monospace;";
           const idx = document.createElement("span");
           idx.textContent = String(i);
-          idx.style.cssText = `width:18px;font-weight:bold;color:${cfg.primary};`;
+          idx.style.cssText = `width:24px;font-weight:bold;font-size:14px;color:${cfg.primary};`;
           row.appendChild(idx);
           const mkCoord = (val, axis) => {
             const inp = document.createElement("input");
             inp.type = "number"; inp.step = "0.5"; inp.value = String(val);
-            inp.style.cssText = "width:50px;padding:1px 3px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;font-family:monospace;font-size:11px;text-align:right;";
+            inp.style.cssText = "width:68px;padding:5px 7px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-family:monospace;font-size:13px;text-align:right;";
             inp.addEventListener("change", () => {
               const nx = axis === "x" ? Number(inp.value) || 0 : v.x;
               const ny = axis === "y" ? Number(inp.value) || 0 : v.y;
@@ -1308,20 +1314,24 @@ function buildEditorSection(kind, panelContext) {
           };
           row.appendChild(mkCoord(v.x, "x"));
           row.appendChild(mkCoord(v.y, "y"));
-          row.appendChild(mkCoord(v.z, "z"));
+          if (kind !== "polygon") row.appendChild(mkCoord(v.z, "z"));
           const mkBtn = (text, on) => {
             const b = document.createElement("button");
             b.textContent = text;
-            b.style.cssText = "width:22px;height:22px;padding:0;border:none;background:transparent;color:inherit;cursor:pointer;font-size:11px;border-radius:3px;";
+            b.style.cssText = "width:30px;height:30px;padding:0;border:none;background:transparent;color:inherit;cursor:pointer;font-size:15px;border-radius:4px;";
             b.addEventListener("click", on);
             return b;
           };
           if (i > 0) row.appendChild(mkBtn("▲", () => sendCmd({ action: "reorder", from: i, to: i - 1 })));
-          else { const sp = document.createElement("span"); sp.style.width = "22px"; row.appendChild(sp); }
+          else { const sp = document.createElement("span"); sp.style.width = "30px"; row.appendChild(sp); }
           if (i < cache.length - 1) row.appendChild(mkBtn("▼", () => sendCmd({ action: "reorder", from: i, to: i + 1 })));
-          else { const sp = document.createElement("span"); sp.style.width = "22px"; row.appendChild(sp); }
+          else { const sp = document.createElement("span"); sp.style.width = "30px"; row.appendChild(sp); }
+          const dupBtn = mkBtn("⧉", () => sendCmd({ action: "duplicate", index: i }));
+          dupBtn.title = "Duplicate";
+          row.appendChild(dupBtn);
           const delBtn = mkBtn("✕", () => sendCmd({ action: "delete", index: i }));
           delBtn.style.color = "#dc2626";
+          delBtn.title = "Delete";
           row.appendChild(delBtn);
           listContainer.appendChild(row);
         }
@@ -1339,9 +1349,9 @@ function buildEditorSection(kind, panelContext) {
         for (const name of names) {
           const s = savesCache[name];
           const row = document.createElement("div");
-          row.style.cssText = "display:flex;align-items:center;gap:5px;padding:3px 5px;border-bottom:1px solid #333;font-size:11px;";
+          row.style.cssText = "display:flex;align-items:center;gap:8px;padding:6px 8px;border-bottom:1px solid #333;font-size:14px;";
           const sw = document.createElement("span");
-          sw.style.cssText = "display:inline-block;width:9px;height:9px;border-radius:50%;flex-shrink:0;";
+          sw.style.cssText = "display:inline-block;width:12px;height:12px;border-radius:50%;flex-shrink:0;";
           const c = s.color || [0.5, 0.5, 0.5];
           sw.style.background = `rgb(${Math.round(c[0]*255)},${Math.round(c[1]*255)},${Math.round(c[2]*255)})`;
           row.appendChild(sw);
@@ -1352,7 +1362,7 @@ function buildEditorSection(kind, panelContext) {
           const small = (text, color, on) => {
             const b = document.createElement("button");
             b.textContent = text;
-            b.style.cssText = `padding:1px 6px;border-radius:3px;border:1px solid #555;background:transparent;color:${color};cursor:pointer;font-size:10px;`;
+            b.style.cssText = `padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:${color};cursor:pointer;font-size:13px;`;
             b.addEventListener("click", on);
             return b;
           };
diff --git a/gcs/foxglove_extensions/waypoint-editor/dist/extension.js b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
index 3087a1de..60e7ef13 100644
--- a/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
+++ b/gcs/foxglove_extensions/waypoint-editor/dist/extension.js
@@ -52,16 +52,16 @@ function activate(extensionContext) {
       const ctrlRow = el("div", "display:flex;align-items:center;gap:8px;flex-shrink:0;");
 
       const enableBtn = el("button",
-        "padding:6px 14px;border-radius:4px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:13px;");
+        "padding:8px 18px;border-radius:5px;border:none;color:white;cursor:pointer;font-weight:bold;font-size:15px;");
       enableBtn.addEventListener("click", () => {
         sendCmd({ action: "set_enabled", enabled: !enabled });
       });
       ctrlRow.appendChild(enableBtn);
 
-      const altLabel = el("span", "font-size:12px;");
+      const altLabel = el("span", "font-size:14px;");
       altLabel.textContent = "Altitude:";
       const altInput = el("input",
-        "width:60px;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;");
+        "width:80px;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;");
       altInput.type = "number";
       altInput.step = "0.5";
       altInput.value = String(state.defaultZ);
@@ -72,7 +72,7 @@ function activate(extensionContext) {
       });
 
       const clearBtn = el("button",
-        "padding:6px 10px;border-radius:4px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:12px;");
+        "padding:8px 14px;border-radius:5px;border:none;background:#dc2626;color:white;cursor:pointer;font-size:14px;");
       clearBtn.textContent = "Clear All";
       clearBtn.addEventListener("click", () => { sendCmd({ action: "clear" }); });
 
@@ -97,7 +97,7 @@ function activate(extensionContext) {
       const addYIn = numInput("Y", "0");
       const addZIn = numInput("Z", String(state.defaultZ));
       const addBtn = el("button",
-        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
       addBtn.textContent = "+ Add";
       addBtn.addEventListener("click", () => {
         sendCmd({
@@ -119,13 +119,13 @@ function activate(extensionContext) {
       savesLabel.textContent = "Saves";
       root.appendChild(savesLabel);
 
-      const saveAddRow = el("div", "display:flex;align-items:center;gap:4px;flex-shrink:0;");
+      const saveAddRow = el("div", "display:flex;align-items:center;gap:6px;flex-shrink:0;");
       const saveNameIn = el("input",
-        "flex:1;padding:3px 5px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;font-size:12px;");
+        "flex:1;padding:6px 8px;border-radius:5px;border:1px solid #555;background:transparent;color:inherit;font-size:14px;");
       saveNameIn.type = "text";
       saveNameIn.placeholder = "save name…";
       const saveAddBtn = el("button",
-        "padding:4px 10px;border-radius:4px;border:none;background:#10b981;color:white;cursor:pointer;font-size:12px;");
+        "padding:6px 14px;border-radius:5px;border:none;background:#10b981;color:white;cursor:pointer;font-size:14px;");
       saveAddBtn.textContent = "+ Add";
       saveAddBtn.addEventListener("click", () => {
         const name = saveNameIn.value.trim();
@@ -154,9 +154,9 @@ function activate(extensionContext) {
         }
         for (const s of saves) {
           const row = el("div",
-            "display:flex;align-items:center;gap:6px;padding:4px 6px;border-bottom:1px solid #333;font-size:12px;");
+            "display:flex;align-items:center;gap:8px;padding:6px 8px;border-bottom:1px solid #333;font-size:14px;");
           const swatch = el("span",
-            "display:inline-block;width:10px;height:10px;border-radius:50%;flex-shrink:0;");
+            "display:inline-block;width:12px;height:12px;border-radius:50%;flex-shrink:0;");
           const [r, g, b] = s.color || [0.5, 0.5, 0.5];
           swatch.style.background =
             `rgb(${Math.round(r*255)},${Math.round(g*255)},${Math.round(b*255)})`;
@@ -167,7 +167,7 @@ function activate(extensionContext) {
           row.appendChild(nameEl);
 
           const loadBtn = el("button",
-            "padding:2px 8px;border-radius:3px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:11px;");
+            "padding:4px 10px;border-radius:4px;border:1px solid #555;background:transparent;color:inherit;cursor:pointer;font-size:13px;");
           loadBtn.textContent = "Load";
           loadBtn.addEventListener("click", () => {
             sendCmd({ action: "load_save", name: s.name });
@@ -179,11 +179,11 @@ function activate(extensionContext) {
             saveBtn.textContent = "✓ Saved";
             saveBtn.disabled = true;
             saveBtn.style.cssText =
-              "padding:2px 8px;border-radius:3px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:11px;";
+              "padding:4px 10px;border-radius:4px;border:1px solid #10b981;background:rgba(16,185,129,0.1);color:#10b981;cursor:default;font-size:13px;";
           } else {
             saveBtn.textContent = "Save";
             saveBtn.style.cssText =
-              "padding:2px 8px;border-radius:3px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:11px;";
+              "padding:4px 10px;border-radius:4px;border:none;background:#2563eb;color:white;cursor:pointer;font-size:13px;";
             saveBtn.addEventListener("click", () => {
               sendCmd({ action: "save_save", name: s.name });
             });
@@ -191,7 +191,7 @@ function activate(extensionContext) {
           row.appendChild(saveBtn);
 
           const delBtn = el("button",
-            "padding:2px 6px;border-radius:3px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:13px;");
+            "padding:4px 8px;border-radius:4px;border:none;background:transparent;color:#dc2626;cursor:pointer;font-size:15px;");
           delBtn.textContent = "✕";
           delBtn.title = "Delete save";
           delBtn.addEventListener("click", () => {
@@ -230,12 +230,12 @@ function activate(extensionContext) {
         for (let i = 0; i < waypoints.length; i++) {
           const wp = waypoints[i];
           const row = el("div",
-            "display:flex;align-items:center;gap:4px;padding:4px 6px;" +
-            "border-bottom:1px solid #333;font-size:12px;font-family:monospace;" +
+            "display:flex;align-items:center;gap:5px;padding:5px 8px;" +
+            "border-bottom:1px solid #333;font-size:14px;font-family:monospace;" +
             (i === selectedIdx ? "background:rgba(16,185,129,0.15);" : ""));
 
           // Index label
-          const idx = el("span", "width:20px;font-weight:bold;color:#10b981;");
+          const idx = el("span", "width:24px;font-weight:bold;font-size:14px;color:#10b981;");
           idx.textContent = String(i);
           row.appendChild(idx);
 
@@ -261,7 +261,7 @@ function activate(extensionContext) {
             upBtn.title = "Move up";
             row.appendChild(upBtn);
           } else {
-            row.appendChild(el("span", "width:24px;"));
+            row.appendChild(el("span", "width:32px;"));
           }
 
           // Move down
@@ -272,9 +272,16 @@ function activate(extensionContext) {
             downBtn.title = "Move down";
             row.appendChild(downBtn);
           } else {
-            row.appendChild(el("span", "width:24px;"));
+            row.appendChild(el("span", "width:32px;"));
           }
 
+          // Duplicate
+          const dupBtn = smallBtn("\u29c9", () => {
+            sendCmd({ action: "duplicate", index: i });
+          });
+          dupBtn.title = "Duplicate";
+          row.appendChild(dupBtn);
+
           // Delete
           const delBtn = smallBtn("\u2715", () => {
             sendCmd({ action: "delete", index: i });
@@ -350,8 +357,8 @@ function el(tag, style) {
 
 function smallBtn(text, onClick) {
   const b = el("button",
-    "width:24px;height:24px;padding:0;border:none;background:transparent;" +
-    "color:inherit;cursor:pointer;font-size:12px;border-radius:3px;");
+    "width:32px;height:32px;padding:0;border:none;background:transparent;" +
+    "color:inherit;cursor:pointer;font-size:15px;border-radius:4px;");
   b.textContent = text;
   b.addEventListener("mouseenter", () => { b.style.background = "rgba(255,255,255,0.1)"; });
   b.addEventListener("mouseleave", () => { b.style.background = "transparent"; });
@@ -361,8 +368,8 @@ function smallBtn(text, onClick) {
 
 function coordInput(value, onChange) {
   const inp = el("input",
-    "width:55px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
-    "background:transparent;color:inherit;font-family:monospace;font-size:11px;text-align:right;");
+    "width:72px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-family:monospace;font-size:13px;text-align:right;");
   inp.type = "number";
   inp.step = "0.5";
   inp.value = String(value);
@@ -373,12 +380,12 @@ function coordInput(value, onChange) {
 }
 
 function numInput(label, defaultVal) {
-  const wrap = el("div", "display:flex;align-items:center;gap:2px;");
-  const lbl = el("span", "font-size:11px;opacity:0.7;");
+  const wrap = el("div", "display:flex;align-items:center;gap:4px;");
+  const lbl = el("span", "font-size:13px;opacity:0.75;");
   lbl.textContent = label + ":";
   const input = el("input",
-    "width:50px;padding:2px 4px;border-radius:3px;border:1px solid #555;" +
-    "background:transparent;color:inherit;font-size:11px;");
+    "width:64px;padding:5px 7px;border-radius:4px;border:1px solid #555;" +
+    "background:transparent;color:inherit;font-size:13px;");
   input.type = "number";
   input.step = "0.5";
   input.value = defaultVal;
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 1de4c2fd..947611e5 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -16,10 +16,12 @@
 from nav_msgs.msg import Odometry, Path
 from visualization_msgs.msg import Marker, MarkerArray
 from builtin_interfaces.msg import Duration
-from geometry_msgs.msg import Point
+from geometry_msgs.msg import Point, TransformStamped
+from tf2_ros import StaticTransformBroadcaster
 
 from gcs_visualizer.gcs_utils import (
     gps_to_enu, multiply_quaternions, rotate_vector, transform_marker_array,
+    ORIGIN_LAT, ORIGIN_LON,
 )
 
 SENSOR_QOS = QoSProfile(
@@ -84,10 +86,33 @@ def __init__(self):
         self._global_plans   = {}
         self._vdb_markers    = {}
         self._vdb_global     = {}  # pre-translated global-frame VDB, keyed by robot_name
+        # Per-robot NavSatFix re-publisher (frame_id rewritten to 'map' so
+        # Foxglove's Map panel will accept it as a location source).
+        self._location_pubs: dict = {}
         self._subscribed_gps  = set()
         self._subscribed_odom = set()
         self._subscribed_traj = set()
         self._subscribed_plan = set()
+
+        # Make 'map' a known frame on the GCS domain so Foxglove resolves it.
+        # The robot side already publishes the same identity static TF
+        # (robot.launch.xml world_to_map_broadcaster).
+        self._static_tf_broadcaster = StaticTransformBroadcaster(self)
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = 'world'
+        t.child_frame_id = 'map'
+        t.transform.rotation.w = 1.0
+        self._static_tf_broadcaster.sendTransform(t)
+
+        # Publish a stationary "map origin" location at the configured
+        # ORIGIN_LAT/LON so the Foxglove Map panel always has a fixed
+        # reference (otherwise the panel auto-centers on the robot, making
+        # the robot appear at the visible map's center). 1 Hz so any newly
+        # opened Map layer picks it up quickly.
+        self._origin_pub = self.create_publisher(
+            NavSatFix, '/gcs/map_origin/location', 10)
+        self.create_timer(1.0, self._publish_map_origin)
         self._subscribed_vdb  = set()
         self._alt_ground      = None
 
@@ -173,6 +198,23 @@ def _gps_callback(self, msg: NavSatFix, robot_name: str):
                 self._vdb_global[robot_name] = _translate_marker(
                     self._vdb_markers[robot_name], bx, by, bz)
 
+        # Re-publish on /gcs/<robot>/location with frame_id='map' so the
+        # Foxglove Map panel will accept it as a location source. lat/lon
+        # are passed through unchanged.
+        if robot_name not in self._location_pubs:
+            self._location_pubs[robot_name] = self.create_publisher(
+                NavSatFix, f'/gcs/{robot_name}/location', 10)
+        out = NavSatFix()
+        out.header.stamp = msg.header.stamp
+        out.header.frame_id = 'map'
+        out.status = msg.status
+        out.latitude = msg.latitude
+        out.longitude = msg.longitude
+        out.altitude = msg.altitude
+        out.position_covariance = msg.position_covariance
+        out.position_covariance_type = msg.position_covariance_type
+        self._location_pubs[robot_name].publish(out)
+
     def _odom_callback(self, msg: Odometry, robot_name: str):
         o = msg.pose.pose.orientation
         self._orientations[robot_name] = (o.x, o.y, o.z, o.w)
@@ -190,6 +232,17 @@ def _vdb_callback(self, msg: Marker, robot_name: str):
             bx, by, bz = boot
             self._vdb_global[robot_name] = _translate_marker(msg, bx, by, bz)
 
+    def _publish_map_origin(self):
+        m = NavSatFix()
+        m.header.stamp = self.get_clock().now().to_msg()
+        m.header.frame_id = 'map'
+        m.status.status = 0  # STATUS_FIX
+        m.status.service = 1  # SERVICE_GPS
+        m.latitude = ORIGIN_LAT
+        m.longitude = ORIGIN_LON
+        m.altitude = 0.0
+        self._origin_pub.publish(m)
+
     def _publish_markers(self):
         if not self._gps_positions:
             return
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
index 47c4fc75..e9dd6487 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
@@ -188,6 +188,11 @@ def _on_command(self, msg: String):
                 v = self._vertices.pop(fi)
                 self._vertices.insert(ti, v)
 
+        elif action == 'duplicate':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._vertices):
+                self._vertices.insert(idx + 1, dict(self._vertices[idx]))
+
         elif action == 'clear':
             self._vertices.clear()
 
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
index c2ca75f5..e99c435c 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
@@ -204,6 +204,11 @@ def _on_command(self, msg: String):
                 wp = self._waypoints.pop(fi)
                 self._waypoints.insert(ti, wp)
 
+        elif action == 'duplicate':
+            idx = int(cmd.get('index', -1))
+            if 0 <= idx < len(self._waypoints):
+                self._waypoints.insert(idx + 1, dict(self._waypoints[idx]))
+
         elif action == 'clear':
             self._waypoints.clear()
 

From b0c822c91430040ac308e564ece713c48d75155d Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 28 Apr 2026 15:23:59 -0400
Subject: [PATCH 46/55] added 2d map to foxglove

---
 .../foxglove_visualizer_node.py               | 158 ++++++++++++++++-
 .../semantic_search_task/node.py              |  36 +++-
 .../launch_scripts/two_drone_fire.py          |  37 +++-
 simulation/isaac-sim/utils/scene_prep.py      | 159 ++++++++++++++++++
 4 files changed, 383 insertions(+), 7 deletions(-)

diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
index 947611e5..bffd5409 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py
@@ -12,11 +12,12 @@
 import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
-from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import NavSatFix, Image
 from nav_msgs.msg import Odometry, Path
 from visualization_msgs.msg import Marker, MarkerArray
 from builtin_interfaces.msg import Duration
 from geometry_msgs.msg import Point, TransformStamped
+from std_msgs.msg import ColorRGBA, Float32
 from tf2_ros import StaticTransformBroadcaster
 
 from gcs_visualizer.gcs_utils import (
@@ -31,6 +32,13 @@
     depth=10,
 )
 
+LATCHED_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.RELIABLE,
+    durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+)
+
 GPS_SUFFIX  = '/interface/mavros/global_position/global'
 ODOM_SUFFIX = '/odometry_conversion/odometry'
 TRAJ_SUFFIX = '/trajectory_controller/trajectory_vis'
@@ -113,6 +121,43 @@ def __init__(self):
         self._origin_pub = self.create_publisher(
             NavSatFix, '/gcs/map_origin/location', 10)
         self.create_timer(1.0, self._publish_map_origin)
+
+        # Sim-mode "textured ground" pipeline. The simulator publishes a
+        # raw overhead Image of its static scene plus a coverage_m spec on a
+        # separate Float32 topic so the GCS doesn't need to be configured
+        # manually. We wait until both have been received, then build a
+        # TRIANGLE_LIST marker with per-vertex colors and publish it once on
+        # a latched topic for Foxglove's 3D panel. Subscriptions are torn
+        # down after the first build to free up bandwidth.
+        self.declare_parameter('overhead_image_topic', '/sim/overhead/image')
+        self.declare_parameter('overhead_spec_topic', '/sim/overhead/spec')
+        self.declare_parameter('overhead_coverage_m', 0.0)        # 0 → wait for spec topic
+        self.declare_parameter('overhead_grid_resolution', 0)     # 0 → derive from coverage
+        self.declare_parameter('overhead_grid_per_m', 0.8)
+        self.declare_parameter('overhead_max_grid_resolution', 384)
+        self._overhead_topic = self.get_parameter(
+            'overhead_image_topic').value
+        self._overhead_spec_topic = self.get_parameter(
+            'overhead_spec_topic').value
+        # 0 → unknown / wait for spec; >0 → manual override.
+        cov_param = float(self.get_parameter('overhead_coverage_m').value)
+        self._overhead_coverage_m = cov_param if cov_param > 0 else None
+        grid_param = int(self.get_parameter('overhead_grid_resolution').value)
+        self._overhead_grid_n_override = grid_param if grid_param > 0 else None
+        self._overhead_grid_per_m = float(self.get_parameter(
+            'overhead_grid_per_m').value)
+        self._overhead_max_grid_n = int(self.get_parameter(
+            'overhead_max_grid_resolution').value)
+        self._ground_published = False
+        self._pending_image = None    # cache image until spec arrives (or vice versa)
+        self._ground_pub = self.create_publisher(
+            Marker, '/gcs/sim_ground', LATCHED_QOS)
+        self._overhead_sub = self.create_subscription(
+            Image, self._overhead_topic,
+            self._on_overhead_image, SENSOR_QOS)
+        self._overhead_spec_sub = self.create_subscription(
+            Float32, self._overhead_spec_topic,
+            self._on_overhead_spec, SENSOR_QOS)
         self._subscribed_vdb  = set()
         self._alt_ground      = None
 
@@ -243,6 +288,117 @@ def _publish_map_origin(self):
         m.altitude = 0.0
         self._origin_pub.publish(m)
 
+    def _on_overhead_spec(self, msg: Float32):
+        """Cache the sim-published coverage_m. We may have already received
+        an image — if so, finish the build now."""
+        if self._ground_published:
+            return
+        if self._overhead_coverage_m is None and float(msg.data) > 0:
+            self._overhead_coverage_m = float(msg.data)
+            self.get_logger().info(
+                f'Sim overhead spec: coverage_m = {self._overhead_coverage_m:.1f}')
+        if self._pending_image is not None:
+            img = self._pending_image
+            self._pending_image = None
+            self._build_sim_ground_marker(img)
+
+    def _on_overhead_image(self, msg: Image):
+        """Cache or process the first valid sim-overhead Image."""
+        if self._ground_published:
+            return
+        if self._overhead_coverage_m is None:
+            # Hold onto the image until the spec arrives.
+            self._pending_image = msg
+            return
+        self._build_sim_ground_marker(msg)
+
+    def _build_sim_ground_marker(self, msg: Image):
+        """Build and publish the TRIANGLE_LIST sim_ground marker, then tear
+        down both overhead subscriptions so we stop pulling traffic."""
+        if self._ground_published:
+            return
+
+        # Decode raw Image: encoding can be rgb8 / rgba8 / bgr* — Isaac Sim's
+        # RGB camera helper publishes rgba8.
+        try:
+            arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
+            arr = arr.reshape((msg.height, msg.width, -1))
+            enc = (msg.encoding or '').lower()
+            if enc.startswith('rgba'):
+                pix = arr[..., :3]
+            elif enc.startswith('bgra'):
+                pix = arr[..., [2, 1, 0]]
+            elif enc.startswith('bgr'):
+                pix = arr[..., ::-1]
+            else:
+                pix = arr[..., :3]
+        except Exception as e:
+            self.get_logger().warn(f'Failed to decode overhead Image: {e}')
+            return
+
+        coverage = float(self._overhead_coverage_m)
+        # Grid resolution: explicit override > coverage × density (capped).
+        if self._overhead_grid_n_override is not None:
+            N = self._overhead_grid_n_override
+        else:
+            N = int(round(coverage * self._overhead_grid_per_m))
+        N = max(8, min(N, int(self._overhead_max_grid_n)))
+
+        # Downsample to N×N grid via simple nearest-neighbor stride.
+        H, W = pix.shape[:2]
+        ys = (np.linspace(0, H - 1, N)).astype(np.int32)
+        xs = (np.linspace(0, W - 1, N)).astype(np.int32)
+        pixels = pix[ys[:, None], xs[None, :], :]  # (N, N, 3)
+
+        cell = coverage / N
+
+        marker = Marker()
+        marker.header.frame_id = 'map'
+        marker.header.stamp = self.get_clock().now().to_msg()
+        marker.ns = 'sim_ground'
+        marker.id = 0
+        marker.type = Marker.TRIANGLE_LIST
+        marker.action = Marker.ADD
+        marker.pose.orientation.w = 1.0
+        marker.scale.x = 1.0
+        marker.scale.y = 1.0
+        marker.scale.z = 1.0
+        marker.color.a = 1.0  # per-vertex colors override .color, but a must be set
+
+        # Build the grid. Image row 0 is the +Y edge (image-up = world-north).
+        # World x = (col / N - 0.5) * coverage; world y = (0.5 - row / N) * coverage.
+        for row in range(N):
+            y_top = (0.5 - row / N) * coverage
+            y_bot = y_top - cell
+            for col in range(N):
+                x_left = (col / N - 0.5) * coverage
+                x_right = x_left + cell
+                r, g, b = pixels[row, col]
+                color = ColorRGBA(r=r/255.0, g=g/255.0, b=b/255.0, a=1.0)
+
+                p_tl = Point(x=x_left,  y=y_top, z=0.0)
+                p_tr = Point(x=x_right, y=y_top, z=0.0)
+                p_bl = Point(x=x_left,  y=y_bot, z=0.0)
+                p_br = Point(x=x_right, y=y_bot, z=0.0)
+
+                # CCW winding (looking down +Z): top-left, bottom-left, bottom-right
+                # then top-left, bottom-right, top-right.
+                marker.points.extend([p_tl, p_bl, p_br,
+                                      p_tl, p_br, p_tr])
+                marker.colors.extend([color] * 6)
+
+        self._ground_pub.publish(marker)
+        self._ground_published = True
+        # Stop pulling raw image bytes (and the now-redundant spec) off the wire.
+        for sub_attr in ('_overhead_sub', '_overhead_spec_sub'):
+            try:
+                self.destroy_subscription(getattr(self, sub_attr))
+            except Exception:
+                pass
+        self.get_logger().info(
+            f'Published sim_ground marker: {N}x{N} cells, '
+            f'{len(marker.points)} verts, coverage {coverage:.1f} m')
+
     def _publish_markers(self):
         if not self._gps_positions:
             return
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
index c27b3f4e..566c66bf 100644
--- a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
+++ b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
@@ -237,8 +237,10 @@ def _spawn_random_walk(self, robot_name: str) -> subprocess.Popen:
             '-r', f'~/global_plan_toggle:=/{robot_name}/behavior/global_plan_toggle',
         ])
 
-    def _send_exploration_task(self, robot_name: str) -> None:
-        """Send an unbounded ExplorationTask goal to random_walk_planner to activate droan_gl."""
+    def _send_exploration_task(self, robot_name: str):
+        """Send an unbounded ExplorationTask goal to random_walk_planner to
+        activate droan_gl. Returns (client, send_future) so the caller can
+        cancel the upstream goal when the semantic_search task ends."""
         client = ActionClient(
             self,
             ExplorationTask,
@@ -246,14 +248,33 @@ def _send_exploration_task(self, robot_name: str) -> None:
             callback_group=self._cbg)
         if not client.wait_for_server(timeout_sec=10.0):
             self.get_logger().warn('ExplorationTask server not available after 10s')
-            return
+            return None, None
         goal = ExplorationTask.Goal()
         goal.min_altitude_agl = 2.0
         goal.max_altitude_agl = 15.0
         goal.min_flight_speed = 1.0
         goal.max_flight_speed = 3.0
-        client.send_goal_async(goal)
+        send_future = client.send_goal_async(goal)
         self.get_logger().info('ExplorationTask sent to random_walk_planner')
+        return client, send_future
+
+    def _cancel_exploration_task(self, send_future):
+        """Best-effort cancel of an ExplorationTask started via
+        _send_exploration_task. Safe to call with None / not-yet-accepted."""
+        if send_future is None or not send_future.done():
+            return
+        try:
+            handle = send_future.result()
+        except Exception:
+            return
+        if handle is None or not getattr(handle, 'accepted', False):
+            return
+        try:
+            handle.cancel_goal_async()
+            self.get_logger().info(
+                'Cancelled upstream ExplorationTask (random_walk_planner)')
+        except Exception as e:
+            self.get_logger().warn(f'Failed to cancel ExplorationTask: {e}')
 
     def _interruptible_sleep(self, goal_handle, secs: float) -> bool:
         """Sleep for secs, waking early if cancel requested. Returns True if cancelled."""
@@ -309,6 +330,7 @@ def _execute(self, goal_handle):
             f'SemanticSearchTask | targets={queries} all_queries={all_queries}')
 
         rayfronts_proc = raven_proc = random_walk_proc = None
+        exploration_send_future = None
         rayfronts_q = raven_q = queue.Queue()
 
         # Track last meaningful line from each process for feedback
@@ -435,7 +457,8 @@ def _vox_all_cb(msg):
                     self.get_logger().info(
                         'Raven published first waypoint — starting random_walk_planner')
                     time.sleep(3.0)
-                    self._send_exploration_task(robot_name)
+                    _, exploration_send_future = self._send_exploration_task(
+                        robot_name)
 
                 # Send queries to rayfronts whenever its subscriber appears (or reappears).
                 # This handles the initial load AND any rayfronts restart mid-task.
@@ -503,6 +526,9 @@ def _vox_all_cb(msg):
                     continue   # cancel was requested, loop back to check it
 
         finally:
+            # Cancel the upstream ExplorationTask so the bringup-launched
+            # random_walk_planner stops wandering when this task ends.
+            self._cancel_exploration_task(exploration_send_future)
             self._kill('rayfronts', rayfronts_proc)
             self._kill('raven', raven_proc)
             self._kill('random_walk', random_walk_proc)
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 4dd695b2..9bd013ff 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -37,7 +37,11 @@
 
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 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
+from scene_prep import (
+    scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit,
+    reference_root_prims_under_world,
+    add_orthographic_camera, add_overhead_camera_publisher,
+)
 
 
 # --------------------- CONFIGURATION ---------------------
@@ -75,6 +79,16 @@
     {"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},
 ]
+
+# 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   # The only per-map knob: world meters per side.
+OVERHEAD_PX_PER_METER  = 4.0     # Source-image density. Bump for sharper texture.
+OVERHEAD_TOPIC         = "/sim/overhead/image"
+OVERHEAD_SPEC_TOPIC    = "/sim/overhead/spec"
+OVERHEAD_FRAME_ID      = "map"
+OVERHEAD_DOMAIN_ID     = 0
 # ---------------------------------------------------------
 
 
@@ -167,6 +181,27 @@ def __init__(self):
         # Units
         mpu, s = get_stage_meters_per_unit(stage)
 
+        # Top-down orthographic camera over (0, 0). Publishes one JPEG aerial
+        # of the static scene at low rate; the GCS visualizer republishes it
+        # as a textured ground for Foxglove's 3D panel.
+        cam_path = add_orthographic_camera(
+            stage,
+            prim_path="/World/MapCamera",
+            altitude_m=OVERHEAD_ALTITUDE_M,
+            coverage_m=OVERHEAD_COVERAGE_M,
+            scene_scale_factor=s,
+        )
+        add_overhead_camera_publisher(
+            parent_graph_path="/World/MapCameraGraph",
+            camera_prim_path=cam_path,
+            topic=OVERHEAD_TOPIC,
+            spec_topic=OVERHEAD_SPEC_TOPIC,
+            frame_id=OVERHEAD_FRAME_ID,
+            coverage_m=OVERHEAD_COVERAGE_M,
+            pixels_per_meter=OVERHEAD_PX_PER_METER,
+            domain_id=OVERHEAD_DOMAIN_ID,
+        )
+
         # Spawn all drones
         for cfg in DRONE_CONFIGS:
             i = cfg["domain_id"]
diff --git a/simulation/isaac-sim/utils/scene_prep.py b/simulation/isaac-sim/utils/scene_prep.py
index 09cd7479..de6b4e0b 100644
--- a/simulation/isaac-sim/utils/scene_prep.py
+++ b/simulation/isaac-sim/utils/scene_prep.py
@@ -124,6 +124,165 @@ def add_dome_light(stage, prim_path: str = "/World/DomeLight", intensity: float
     print(f"[scene_prep] Dome light set at '{prim_path}' (intensity={intensity}, exposure={exposure})")
 
 
+# ---------------------------------------------------------------------------
+# Top-down "map" camera: orthographic, fixed over (0,0), used to publish a
+# one-shot aerial of the static sim scene that the GCS visualizer will
+# convert into a textured ground in Foxglove's 3D panel.
+# ---------------------------------------------------------------------------
+
+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.
+
+    Args:
+        stage:               Active USD stage.
+        prim_path:           Where to place the camera prim.
+        altitude_m:          Camera height above world origin (metric).
+        coverage_m:          Side length of the world square in frame (metric).
+        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.
+
+    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.
+    xform = UsdGeom.Xformable(cam.GetPrim())
+    xform.ClearXformOpOrder()
+    xform.AddTranslateOp().Set(
+        Gf.Vec3d(0.0, 0.0, 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
+    # exactly in frame; scale_factor extends that to non-meter stage units.
+    cam.CreateProjectionAttr(UsdGeom.Tokens.orthographic)
+    aperture = float(coverage_m) * 10.0 * float(scene_scale_factor)
+    cam.CreateHorizontalApertureAttr(aperture)
+    cam.CreateVerticalApertureAttr(aperture)
+    cam.CreateClippingRangeAttr(
+        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)")
+    return prim_path
+
+
+def add_overhead_camera_publisher(parent_graph_path: str,
+                                  camera_prim_path: str,
+                                  topic: str = "/sim/overhead/image",
+                                  spec_topic: str = "/sim/overhead/spec",
+                                  frame_id: str = "map",
+                                  coverage_m: float = 80.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.
+
+    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
+    scene is static, so we only need one valid frame — the GCS visualizer
+    catches it then unsubscribes, making the brief startup burst the only
+    network cost.
+
+    Builds a standalone OmniGraph at ``parent_graph_path`` with its own
+    ROS2Context targeting ``domain_id`` regardless of which domain the
+    drones are using.
+    """
+    import omni.graph.core as og  # lazy so non-sim contexts can import scene_prep
+
+    res = max(64, min(int(round(float(coverage_m) * float(pixels_per_meter))),
+                      int(max_resolution)))
+
+    controller = og.Controller()
+    g = parent_graph_path
+
+    if og.get_graph_by_path(g) is None:
+        og.Controller.create_graph({"graph_path": g, "evaluator_name": "execution"})
+
+    nodes = {
+        "context":      f"{g}/MapCameraROS2Context",
+        "playback":     f"{g}/MapCameraOnPlaybackTick",
+        "create_rp":    f"{g}/MapCameraCreateRenderProduct",
+        "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_value":   f"{g}/MapCameraSpecValue",
+        "spec_topic":   f"{g}/MapCameraSpecTopic",
+        "spec_pub":     f"{g}/MapCameraSpecPublisher",
+    }
+
+    controller.edit(
+        graph_id=g,
+        edit_commands={
+            og.Controller.Keys.CREATE_NODES: [
+                (nodes["context"],    "isaacsim.ros2.bridge.ROS2Context"),
+                (nodes["playback"],   "omni.graph.action.OnPlaybackTick"),
+                (nodes["create_rp"],  "isaacsim.core.nodes.IsaacCreateRenderProduct"),
+                (nodes["rgb"],        "isaacsim.ros2.bridge.ROS2CameraHelper"),
+                (nodes["frame"],      "omni.graph.nodes.ConstantString"),
+                (nodes["topic"],      "omni.graph.nodes.ConstantString"),
+                (nodes["spec_value"], "omni.graph.nodes.ConstantFloat"),
+                (nodes["spec_topic"], "omni.graph.nodes.ConstantString"),
+                (nodes["spec_pub"],   "isaacsim.ros2.bridge.ROS2Publisher"),
+            ],
+            og.Controller.Keys.CONNECT: [
+                # Image branch
+                (f"{nodes['playback']}.outputs:tick",     f"{nodes['create_rp']}.inputs:execIn"),
+                (f"{nodes['create_rp']}.outputs:execOut", f"{nodes['rgb']}.inputs:execIn"),
+                (f"{nodes['create_rp']}.outputs:renderProductPath",
+                 f"{nodes['rgb']}.inputs:renderProductPath"),
+                (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
+                (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"),
+            ],
+            og.Controller.Keys.SET_VALUES: [
+                (("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"),
+            ],
+        },
+    )
+
+    # The ROS2Publisher's value input is dynamically typed — created on the
+    # node after the message type is set. Connect ConstantFloat → publisher.
+    controller.edit(
+        graph_id=g,
+        edit_commands={
+            og.Controller.Keys.CONNECT: [
+                (f"{nodes['spec_value']}.inputs:value",
+                 f"{nodes['spec_pub']}.inputs:data"),
+            ],
+        },
+    )
+
+    print(f"[scene_prep] Overhead camera publisher wired: "
+          f"{topic} ({res}x{res} raw Image), {spec_topic} ({coverage_m} m), "
+          f"domain_id={domain_id}")
+
+
 # ---------------------------------------------------------------------------
 # Consolidate root prims under /World
 # ---------------------------------------------------------------------------

From 62eb1bddeb5f3a5feff3efe696784359a0b61bbb Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Tue, 28 Apr 2026 21:22:42 -0400
Subject: [PATCH 47/55] WIP: pre-merge snapshot

---
 .../desktop_bringup/rviz/robot.rviz           | 53 ++++++++++---------
 1 file changed, 27 insertions(+), 26 deletions(-)

diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz
index 4adb19f6..41ca4014 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz
@@ -11,7 +11,8 @@ Panels:
         - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
         - /Local1/DROAN1/Droan GPU1
         - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
-        - /Local1/DROAN1/Global Plan Vis1
+        - /Local1/Trajectory Controller1
+        - /Global1
         - /Global1/Global Plan1
       Splitter Ratio: 0.5792563557624817
     Tree Height: 622
@@ -30,7 +31,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: voxel_rgb
+    SyncSource: StereoImageProc Disparity
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
     topic: /robot_1/behavior_tree_graphviz
@@ -249,7 +250,7 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Image
-          Enabled: true
+          Enabled: false
           Max Value: 1
           Median window: 5
           Min Value: 0
@@ -261,9 +262,9 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: sensors/front_stereo/right/image_rect
-          Value: true
+          Value: false
         - Class: rviz_default_plugins/Image
-          Enabled: true
+          Enabled: false
           Max Value: 100
           Median window: 5
           Min Value: 0
@@ -275,7 +276,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: sensors/front_stereo/right/depth_ground_truth
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -732,10 +733,10 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Marker
-          Enabled: false
+          Enabled: true
           Name: VDB Mapping Marker
           Namespaces:
-            {}
+            "": true
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -743,7 +744,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: vdb_mapping/vdb_map_visualization
-          Value: false
+          Value: true
         - Alpha: 0.800000011920929
           Buffer Length: 1
           Class: rviz_default_plugins/Path
@@ -786,7 +787,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -806,19 +807,19 @@ Visualization Manager:
         Value: /robot_1/rayfronts/msg_serv/rays_sim/q0_red_building
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
+      Enabled: false
       Name: filtered rays
       Namespaces:
-        arrows: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /robot_1/filtered_rays
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -831,7 +832,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -851,7 +852,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/msg_serv/voxels_sim/q0_red_building
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -864,7 +865,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -884,7 +885,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/voxel_rgb
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -897,7 +898,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: RGB8
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -917,7 +918,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/voxel_feature
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -930,7 +931,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: ""
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 4096
@@ -950,7 +951,7 @@ Visualization Manager:
         Value: /robot_1/rayfronts/queries/red_building/voxels
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -1014,7 +1015,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 35.724098205566406
+      Distance: 72.78426361083984
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -1029,10 +1030,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6453977227210999
+      Pitch: 0.7253975868225098
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 2.873566150665283
+      Yaw: 2.2885656356811523
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
@@ -1048,7 +1049,7 @@ Window Geometry:
   Hide Right Dock: false
   MACVO Disparity:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   TasksPanel:

From 7b190b92a2a0984a1175ef59fad314c0dc941876 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Wed, 29 Apr 2026 11:52:46 -0400
Subject: [PATCH 48/55] added changes from main

---
 .env                          | 4 ++--
 robot/docker/Dockerfile.robot | 6 ++++++
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/.env b/.env
index 48a550bf..4b429431 100644
--- a/.env
+++ b/.env
@@ -25,7 +25,7 @@ COMPOSE_PROFILES="desktop,isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="1"  # Number of robot containers to launch.
+NUM_ROBOTS="2"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
@@ -33,7 +33,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="example_one_px4_pegasus_launch_script.py"
+ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py" #"example_one_px4_pegasus_launch_script.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index d47bddf9..b7c28afa 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -215,6 +215,12 @@ RUN if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
       setuptools==79.0.1; \
   fi
 
+# Re-pin empy to 3.x. The RayFronts block above transitively upgrades it via
+# transformers/bloom/colcon-core to 4.x, which is API-incompatible with
+# rosidl_generator_rs and breaks the workspace build with
+# "TransientParseError: not enough data to read" on rmw.rs.em.
+RUN pip3 install --break-system-packages --force-reinstall empy==3.3.4
+
 # Patched OpenVDB (OasisArtisan fork) — exposes Int8Grid to Python bindings
 RUN if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
     apt-get ${UPDATE_FLAGS} update && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends \

From 5aab17b53ba69d533f4d1498b3c2f10cbb7d3d77 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Wed, 29 Apr 2026 13:16:29 -0400
Subject: [PATCH 49/55] WIP: pre-branch-split snapshot

---
 .../robot-commands/dist/extension.js          | 42 +++++++++++++++----
 .../action_relay/action_relay/relay_node.py   | 11 ++++-
 .../launch_scripts/two_drone_fire.py          |  2 +-
 3 files changed, 44 insertions(+), 11 deletions(-)

diff --git a/gcs/foxglove_extensions/robot-commands/dist/extension.js b/gcs/foxglove_extensions/robot-commands/dist/extension.js
index 28e4ad90..5bac3cef 100644
--- a/gcs/foxglove_extensions/robot-commands/dist/extension.js
+++ b/gcs/foxglove_extensions/robot-commands/dist/extension.js
@@ -738,26 +738,45 @@ function activate(extensionContext) {
         }
         return null;
       }
+      // Extract the robot name from a relay topic like
+      // /robot_2/tasks/takeoff/relay_feedback → "robot_2". Falls back to the
+      // panel's current state.robot if the topic doesn't match the pattern
+      // (defensive — every relay topic follows the convention).
+      function robotForTopic(topic) {
+        const m = /^\/([^/]+)\/tasks\//.exec(topic);
+        return m ? m[1] : state.robot;
+      }
 
       function handleRelayFeedback(topic, msg) {
         const tab = tabForTopic(topic);
         if (!tab) return;
-        const t = perTab[tab.id];
+        const robot = robotForTopic(topic);
+        // Look up the perTab entry by the *message's* robot, not by
+        // state.robot. With multiple panels open (or any race where state.robot
+        // doesn't match the topic), going through the proxy would route to
+        // the wrong robot's state and silently drop the message.
+        const t = getPerTab(robot, tab.id);
         if (!t.active) return;
         const data = msg?.data;
         if (!data) return;
-        try {
-          const fb = JSON.parse(data);
-          appendFeedback(tab.formatFeedback(fb), tab.id);
-        } catch {
-          appendFeedback(data, tab.id);
+        const line = (() => {
+          try { return tab.formatFeedback(JSON.parse(data)); }
+          catch { return data; }
+        })();
+        const ts = new Date().toLocaleTimeString();
+        t.feedbackLines.push(`[${ts}] ${line}`);
+        if (t.feedbackLines.length > MAX_FEEDBACK_LINES) {
+          t.feedbackLines.splice(0, t.feedbackLines.length - MAX_FEEDBACK_LINES);
         }
+        // Update display only when the visible tab + robot match the message.
+        if (robot === state.robot && tab.id === state.activeTab) renderFeedback();
       }
 
       function handleRelayResult(topic, msg) {
         const tab = tabForTopic(topic);
         if (!tab) return;
-        const t = perTab[tab.id];
+        const robot = robotForTopic(topic);
+        const t = getPerTab(robot, tab.id);
         if (!t.active) return;
         const data = msg?.data;
         if (!data) return;
@@ -772,8 +791,13 @@ function activate(extensionContext) {
           t.statusCode = GOAL_STATUS.SUCCEEDED;
         }
         t.active = false;
-        rebuildSubscriptions();
-        renderTabs();
+        // Only rebuild subscriptions / re-render if this affects the
+        // currently-displayed robot. Other panels will see the perTab change
+        // on their next tick.
+        if (robot === state.robot) {
+          rebuildSubscriptions();
+          renderTabs();
+        }
       }
 
       // ── render loop ──────────────────────────────────────────────────────
diff --git a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
index b7ef096d..5a36816a 100644
--- a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
+++ b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
@@ -347,9 +347,18 @@ def on_goal_str(msg):
             cancel_event.clear()
 
     def on_cancel(msg):
-        """Signal the goal-handler loop to forward a cancel to the robot."""
+        """Signal the goal-handler loop to forward a cancel to the robot.
+
+        If there's no active goal at the relay (already finished, or the
+        panel's state desynced from the relay's), republish a synthetic
+        result so any stuck panel UI can flip back to idle. A panel that's
+        already idle will ignore it (its `t.active` is false).
+        """
         if active_goal.get('handle') is None:
             node0.get_logger().warn(f'[relay] {topic}: cancel requested but no active goal')
+            _publish_result(
+                False,
+                'Cancel: no active goal at relay (panel state likely desynced)')
             return
         node0.get_logger().info(f'[relay] {topic}: cancel requested')
         cancel_event.set()
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index 9bd013ff..ac277933 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -83,7 +83,7 @@
 # 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   # The only per-map knob: world meters per side.
+OVERHEAD_COVERAGE_M    = 200.0   # per-map knob: world meters per side.
 OVERHEAD_PX_PER_METER  = 4.0     # Source-image density. Bump for sharper texture.
 OVERHEAD_TOPIC         = "/sim/overhead/image"
 OVERHEAD_SPEC_TOPIC    = "/sim/overhead/spec"

From 4d91ddf2a0294b061125e254d53c8ba1bf2f1312 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 30 Apr 2026 19:09:33 -0400
Subject: [PATCH 50/55] PR for foxglove+multi-robot

---
 .env                                          |   4 +-
 .gitmodules                                   |   4 -
 airstack.sh                                   |  62 +-
 common/rayfronts                              |   1 -
 .../dataset/ros2isaacsim.yaml                 |  36 --
 common/rayfronts_configs/low_memory.yaml      |  37 --
 .../config/gossip_payloads.yaml               |  16 +-
 .../desktop_bringup/CMakeLists.txt            |   7 -
 .../desktop_bringup/robot_1_odom_bridge.py    |  28 -
 .../robot_1_semantic_ray_transposer.py        |  98 ----
 .../robot_2_filtered_ray_transposer.py        |  98 ----
 .../robot_2_relative_position.py              |   0
 .../msgs/airstack_msgs/msg/BeliefMapData.msg  |  10 -
 docs/gcs/docker/index.md                      |  32 +-
 docs/gcs/foxglove.md                          | 134 +++++
 docs/gcs/foxglove_full_screen.png             | Bin 0 -> 2520708 bytes
 docs/gcs/foxglove_publish_point.png           | Bin 0 -> 18939 bytes
 docs/gcs/polygon_editor.png                   | Bin 0 -> 64841 bytes
 docs/gcs/waypoint_editor.png                  | Bin 0 -> 63181 bytes
 docs/gcs/waypoints_and_geofences.md           |  87 +++
 docs/real_world/HITL/index.md                 | 123 +---
 docs/robot/autonomy/coordination/index.md     |   3 -
 docs/robot/autonomy/coordination/payloads.md  |  50 +-
 docs/simulation/index.md                      |  47 --
 docs/simulation/isaac_sim/overhead_camera.md  | 124 ++++
 .../isaac_sim/overhead_in_foxglove.png        | Bin 0 -> 558355 bytes
 docs/simulation/isaac_sim/spawning_drones.md  |  72 +++
 gcs/docker/.bashrc                            |  49 --
 gcs/docker/docker-compose.yaml                |   9 -
 gcs/docker/gcs-base-docker-compose.yaml       |   8 -
 .../src/robot_2_relative_position.py          |  61 --
 .../launch/gcs_visualizer.launch.xml          |   2 +
 mkdocs.yml                                    |   5 +
 robot/docker/.bashrc                          |  49 --
 robot/docker/Dockerfile.robot                 |  63 +-
 robot/docker/docker-compose.yaml              |  26 +-
 .../global_bringup/launch/global.launch.xml   |   2 -
 .../planners/raven_nav/config/raven_nav.yaml  |  13 -
 .../raven_nav/launch/raven_nav.launch.xml     |   8 -
 .../src/global/planners/raven_nav/package.xml |  26 -
 .../planners/raven_nav/raven_nav/__init__.py  |   0
 .../raven_nav/raven_nav/behavior_manager.py   |  59 --
 .../raven_nav/raven_nav/behaviors/__init__.py |   0
 .../raven_nav/behaviors/frontier_behavior.py  | 136 -----
 .../raven_nav/behaviors/ray_behavior.py       | 241 --------
 .../raven_nav/behaviors/voxel_behavior.py     | 239 --------
 .../raven_nav/raven_nav/raven_nav_node.py     | 278 ---------
 .../planners/raven_nav/resource/raven_nav     |   0
 .../src/global/planners/raven_nav/setup.cfg   |   4 -
 .../src/global/planners/raven_nav/setup.py    |  27 -
 .../launch/semantic_search_task.launch.xml    |   9 -
 .../planners/semantic_search_task/package.xml |  26 -
 .../resource/semantic_search_task             |   0
 .../semantic_search_task/__init__.py          |   0
 .../semantic_search_task/node.py              | 555 ------------------
 .../planners/semantic_search_task/setup.cfg   |   4 -
 .../planners/semantic_search_task/setup.py    |  26 -
 robot/ros_ws/src/input_prompt.py              |  64 --
 .../launch/rayfronts.launch.xml               |  19 -
 simulation/isaac-sim/docker/.bashrc           |  48 --
 .../isaac-sim/docker/docker-compose.yaml      |  10 -
 ...py => example_multi_drone_scene_import.py} |   0
 .../launch_scripts/two_drone_fire.py          |   4 +-
 63 files changed, 475 insertions(+), 2668 deletions(-)
 delete mode 160000 common/rayfronts
 delete mode 100644 common/rayfronts_configs/dataset/ros2isaacsim.yaml
 delete mode 100644 common/rayfronts_configs/low_memory.yaml
 delete mode 100755 common/ros_packages/desktop_bringup/robot_1_odom_bridge.py
 delete mode 100644 common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
 delete mode 100644 common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
 delete mode 100755 common/ros_packages/desktop_bringup/robot_2_relative_position.py
 delete mode 100644 common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg
 create mode 100644 docs/gcs/foxglove.md
 create mode 100644 docs/gcs/foxglove_full_screen.png
 create mode 100644 docs/gcs/foxglove_publish_point.png
 create mode 100644 docs/gcs/polygon_editor.png
 create mode 100644 docs/gcs/waypoint_editor.png
 create mode 100644 docs/gcs/waypoints_and_geofences.md
 create mode 100644 docs/simulation/isaac_sim/overhead_camera.md
 create mode 100644 docs/simulation/isaac_sim/overhead_in_foxglove.png
 create mode 100644 docs/simulation/isaac_sim/spawning_drones.md
 delete mode 100644 gcs/gcs_bringup/src/robot_2_relative_position.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/package.xml
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/__init__.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/__init__.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/resource/raven_nav
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/setup.cfg
 delete mode 100644 robot/ros_ws/src/global/planners/raven_nav/setup.py
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/package.xml
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/resource/semantic_search_task
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/__init__.py
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg
 delete mode 100644 robot/ros_ws/src/global/planners/semantic_search_task/setup.py
 delete mode 100644 robot/ros_ws/src/input_prompt.py
 delete mode 100644 robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
 rename simulation/isaac-sim/launch_scripts/{two_drone_scene_import.py => example_multi_drone_scene_import.py} (100%)

diff --git a/.env b/.env
index 4b429431..4a058e18 100644
--- a/.env
+++ b/.env
@@ -25,7 +25,7 @@ COMPOSE_PROFILES="desktop,isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="2"  # Number of robot containers to launch.
+NUM_ROBOTS="4"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
@@ -33,7 +33,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="two_drone_fire.py" #"example_one_px4_pegasus_launch_script.py"
+ISAAC_SIM_SCRIPT_NAME="example_one_px4_pegasus_launch_script.py"
 PLAY_SIM_ON_START="false"
 # ===============================================
 
diff --git a/.gitmodules b/.gitmodules
index b23e919d..8722432a 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -18,7 +18,3 @@
 	path = common/ros_packages/gui/rviz/rviz_polygon_selection_tool
 	url = https://github.com/swri-robotics/rviz_polygon_selection_tool.git
 
-[submodule "common/rayfronts"]
-	path = common/rayfronts
-	url = https://github.com/RayFronts/RayFronts.git
-	branch = modalai
diff --git a/airstack.sh b/airstack.sh
index d408016c..46161306 100755
--- a/airstack.sh
+++ b/airstack.sh
@@ -119,7 +119,6 @@ function print_command_help {
             echo "Options:"
             echo "  --build       Build images before starting containers"
             echo "  --recreate    Recreate containers even if their configuration and image haven't changed"
-            echo "  --robot_num N Set ROBOT_NAME/ROS_DOMAIN_ID for robot HITL containers (numeric only)"
             ;;
         images)
             echo "Usage: airstack images"
@@ -732,60 +731,9 @@ function classify_compose_args {
 function cmd_up {
     check_docker
 
-    local robot_num=""
-    local filtered_args=()
-    local args=("$@")
-    local i=0
-    while [ $i -lt ${#args[@]} ]; do
-        local arg="${args[$i]}"
-        if [ "$arg" = "--robot_num" ]; then
-            i=$((i+1))
-            if [ $i -ge ${#args[@]} ]; then
-                log_error "--robot_num requires a value"
-                exit 1
-            fi
-            robot_num="${args[$i]}"
-            if ! [[ "$robot_num" =~ ^[0-9]+$ ]]; then
-                log_error "--robot_num must be a number (received: $robot_num)"
-                exit 1
-            fi
-        elif [[ "$arg" == --robot_num=* ]]; then
-            robot_num="${arg#--robot_num=}"
-            if ! [[ "$robot_num" =~ ^[0-9]+$ ]]; then
-                log_error "--robot_num must be a number (received: $robot_num)"
-                exit 1
-            fi
-        else
-            filtered_args+=("$arg")
-        fi
-        i=$((i+1))
-    done
-
     local global_args=()
     local subcmd_args=()
-    classify_compose_args global_args subcmd_args "${filtered_args[@]}"
-
-    local service_count=0
-    local target_is_hitl=false
-    for s in "${subcmd_args[@]}"; do
-        if [[ "$s" != -* ]]; then
-            service_count=$((service_count+1))
-            if [ "$s" = "robot-desktop-hitl" ]; then
-                target_is_hitl=true
-            fi
-        fi
-    done
-
-    if [ -n "$robot_num" ]; then
-        if [ "$service_count" -ne 1 ] || [ "$target_is_hitl" = false ]; then
-            log_error "--robot_num can only be used with exactly one service: robot-desktop-hitl"
-            log_error "Example: airstack up robot-desktop-hitl --robot_num 2"
-            exit 1
-        fi
-    elif [ "$service_count" -eq 1 ] && [ "$target_is_hitl" = true ]; then
-        robot_num="1"
-        log_info "No --robot_num provided; defaulting to robot_num=1 for robot-desktop-hitl"
-    fi
+    classify_compose_args global_args subcmd_args "$@"
 
     # Ensure only one simulator profile is active
     local p="${COMPOSE_PROFILES:-$(sed -n 's/^COMPOSE_PROFILES=//p' "$PROJECT_ROOT/.env" 2>/dev/null | tr -d '"')}"
@@ -813,13 +761,7 @@ function cmd_up {
     fi
 
     log_info "Starting services..."
-    if [ -n "$robot_num" ]; then
-        local robot_name="robot_${robot_num}"
-        log_info "Using robot identity from --robot_num: ROBOT_NAME=${robot_name}, ROS_DOMAIN_ID=${robot_num}"
-        ROBOT_NAME="$robot_name" ROS_DOMAIN_ID="$robot_num" run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${global_args[@]}" up "${subcmd_args[@]}" -d
-    else
-        run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${global_args[@]}" up "${subcmd_args[@]}" -d
-    fi
+    run_docker_compose -f "$PROJECT_ROOT/docker-compose.yaml" "${global_args[@]}" up "${subcmd_args[@]}" -d
     log_info "Services brought up successfully"
 }
 
diff --git a/common/rayfronts b/common/rayfronts
deleted file mode 160000
index b2bbdfde..00000000
--- a/common/rayfronts
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit b2bbdfde73e101bc1243da81ec23a5b3812b31e7
diff --git a/common/rayfronts_configs/dataset/ros2isaacsim.yaml b/common/rayfronts_configs/dataset/ros2isaacsim.yaml
deleted file mode 100644
index ee7b1dbd..00000000
--- a/common/rayfronts_configs/dataset/ros2isaacsim.yaml
+++ /dev/null
@@ -1,36 +0,0 @@
-defaults:
-  - base_dataset
-
-_target_: rayfronts.datasets.Ros2Subscriber
-
-# Topic containing RGB images of type sensor_msgs/msg/Image
-rgb_topic: "/robot_1/sensors/front_stereo/left/image_rect"
-
-# Topic containing odometry of type nav_msgs/msg/Odometry
-pose_topic: "/robot_1/odometry_conversion/odometry"
-
-# Message type for the pose topic: "PoseStamped" or "Odometry"
-pose_msg_type: "Odometry"
-
-# Topic containing depth images of type .
-depth_topic: "/robot_1/sensors/front_stereo/left/depth_ground_truth"
-
-# Topic containing intrinsics information from messages
-# of type sensor_msgs/msg/CameraInfo. Will be used at initialization only.
-intrinsics_topic: "/robot_1/sensors/front_stereo/left/camera_info"
-
-# Path to json file containing intrinsics with the following keys
-# fx, fy, cx, cy, w, h. This will be prioritized over the intrinsics topic.
-intrinsics_file: null
-
-# (Optional) Topic containing confidence in depth values.
-# Message type: sensor_msgs/msg/Image.
-confidence_topic: null
-
-# Topic containing point cloud of type sensor_msgs/msg/PointCloud.
-# this is an alternative inefficient depth source to disparity.
-point_cloud_topic: null
-
-# A string of 3 letters describing the camera coordinate system in
-# r/l u/d f/b in any order. (e.g, rdf, flu, rfu)
-src_coord_system: "flu"
\ No newline at end of file
diff --git a/common/rayfronts_configs/low_memory.yaml b/common/rayfronts_configs/low_memory.yaml
deleted file mode 100644
index 8644e829..00000000
--- a/common/rayfronts_configs/low_memory.yaml
+++ /dev/null
@@ -1,37 +0,0 @@
-defaults:
-  - default
-  - override mapping: semantic_ray_frontiers_map
-  - override encoder: naradio
-  - override vis: ros
-  - override messaging_service: ros
-  - _self_
-
-compile: False
-amp: True
-depth_limit: 20.0
-
-encoder:
-  model_version: radio_v2.5-b
-  lang_model: siglip
-
-dataset:
-  rgb_resolution: [448, 448] #[480, 304]
-  depth_resolution: [448, 448] #[480, 304]
-  frame_skip: 5
-
-mapping:
-  vox_size: 0.50
-  #max_pts_per_frame: 300 #-1
-  #max_empty_pts_per_frame: 20000
-  max_rays_per_frame: 50 #500
-  #fronti_subsampling: 8
-  #fronti_subsampling_min_fronti: 20
-  #ray_tracing: False
-  vox_accum_period: 2 #4
-  ray_accum_period: 4 #4
-  ray_accum_phase: 2 #2
-  #ray_erosion: 0
-  #global_encoding: True
-
-
-
diff --git a/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml b/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
index ac7dc162..2db96985 100644
--- a/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
+++ b/common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml
@@ -26,20 +26,6 @@
 
 payload_topics:
 
-  # ── Rayfronts ──────────────────────────────────────────────────────────────
-  # Filtered semantic rays — peer robots consume this as filtered_rays/transposed
-  # to avoid revisiting already-explored areas.
-  - topic: "/{robot_name}/filtered_rays"
-    type: "visualization_msgs/msg/MarkerArray"
-
-  # Frontier viewpoints — candidate exploration targets computed by Rayfronts.
-  # Peers can use these to select non-overlapping goals.
-  - topic: "/{robot_name}/frontier_viewpoints"
-    type: "sensor_msgs/msg/PointCloud2"
-  
-  - topic : "/{robot_name}/rayfronts/voxel_rgb"
-    type: "sensor_msgs/msg/PointCloud2"
-
-  # ── Add more payloads below ────────────────────────────────────────────────
+  # ── Add payloads below ─────────────────────────────────────────────────────
   # - topic: "/{robot_name}/your/topic"
   #   type: "your_msgs/msg/YourType"
diff --git a/common/ros_packages/desktop_bringup/CMakeLists.txt b/common/ros_packages/desktop_bringup/CMakeLists.txt
index 6fc2bfb3..159bf518 100644
--- a/common/ros_packages/desktop_bringup/CMakeLists.txt
+++ b/common/ros_packages/desktop_bringup/CMakeLists.txt
@@ -19,11 +19,4 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
-install(PROGRAMS
-  robot_2_relative_position.py
-  robot_1_semantic_ray_transposer.py
-  robot_2_filtered_ray_transposer.py
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 ament_package()
diff --git a/common/ros_packages/desktop_bringup/robot_1_odom_bridge.py b/common/ros_packages/desktop_bringup/robot_1_odom_bridge.py
deleted file mode 100755
index 0c9b228f..00000000
--- a/common/ros_packages/desktop_bringup/robot_1_odom_bridge.py
+++ /dev/null
@@ -1,28 +0,0 @@
-#!/usr/bin/env python3
-import rclpy
-from rclpy.node import Node
-from nav_msgs.msg import Odometry
-
-class OdomBridgeNode(Node):
-    def __init__(self):
-        super().__init__('robot_1_odom_bridge')
-        self.pub = self.create_publisher(Odometry, '/robot_1/odom_bridge', 10)
-        self.sub = self.create_subscription(
-            Odometry,
-            '/robot_1/odometry_conversion/odometry',
-            self.odom_callback,
-            10
-        )
-
-    def odom_callback(self, msg):
-        self.pub.publish(msg)
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = OdomBridgeNode()
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()
diff --git a/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py b/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
deleted file mode 100644
index 3c4e1b73..00000000
--- a/common/ros_packages/desktop_bringup/robot_1_semantic_ray_transposer.py
+++ /dev/null
@@ -1,98 +0,0 @@
-#!/usr/bin/env python3
-import copy
-
-import rclpy
-from rclpy.node import Node
-from visualization_msgs.msg import MarkerArray
-
-
-class SemanticRayTransposer(Node):
-    def __init__(self):
-        super().__init__('robot_1_semantic_ray_transposer')
-
-        # Taken from simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
-        #old fire academy init:
-        # robot_1_init = (20.0, -7.0, 0.15)
-        # robot_2_init = (17.0, 1.5, 0.15)
-        #RetroNeighborhood init:
-        # robot_1_init = (35.0, -19.0, 0.15)
-        # robot_2_init = (30.0, -19.0, 0.15)
-
-        robot_1_init = (25.0, 7.6, 0.15)
-        robot_2_init = (23.4, 9.8, 0.15)
-
-
-        dx = robot_2_init[0] - robot_1_init[0]
-        dy = robot_2_init[1] - robot_1_init[1]
-        dz = robot_2_init[2] - robot_1_init[2]
-
-        self.declare_parameter('translation_x', dx)
-        self.declare_parameter('translation_y', dy)
-        self.declare_parameter('translation_z', dz)
-        self.declare_parameter('color_r', 0.0)
-        self.declare_parameter('color_g', 0.6)
-        self.declare_parameter('color_b', 1.0)
-        self.declare_parameter('color_a', 1.0)
-
-        self.translation_x = float(self.get_parameter('translation_x').value)
-        self.translation_y = float(self.get_parameter('translation_y').value)
-        self.translation_z = float(self.get_parameter('translation_z').value)
-        self.color_r = float(self.get_parameter('color_r').value)
-        self.color_g = float(self.get_parameter('color_g').value)
-        self.color_b = float(self.get_parameter('color_b').value)
-        self.color_a = float(self.get_parameter('color_a').value)
-
-        self.subscription = self.create_subscription(
-            MarkerArray,
-            '/robot_1/filtered_rays',
-            self.callback,
-            10,
-        )
-        self.publisher = self.create_publisher(
-            MarkerArray,
-            '/robot_1/filtered_rays/transposed',
-            10,
-        )
-
-        self.get_logger().info(
-            f'Publishing transposed rays with translation '
-            f'({self.translation_x:.3f}, {self.translation_y:.3f}, {self.translation_z:.3f})'
-        )
-
-    def callback(self, msg: MarkerArray):
-        out = copy.deepcopy(msg)
-
-        for marker in out.markers:
-            marker.pose.position.x -= self.translation_x
-            marker.pose.position.y -= self.translation_y
-            marker.pose.position.z -= self.translation_z
-            marker.color.r = self.color_r
-            marker.color.g = self.color_g
-            marker.color.b = self.color_b
-            marker.color.a = self.color_a
-
-            for point in marker.points:
-                point.x -= self.translation_x
-                point.y -= self.translation_y
-                point.z -= self.translation_z
-
-            # If marker uses per-point colors, force those to bright blue too.
-            for color in marker.colors:
-                color.r = self.color_r
-                color.g = self.color_g
-                color.b = self.color_b
-                color.a = self.color_a
-
-        self.publisher.publish(out)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = SemanticRayTransposer()
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()
diff --git a/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py b/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
deleted file mode 100644
index 3544346e..00000000
--- a/common/ros_packages/desktop_bringup/robot_2_filtered_ray_transposer.py
+++ /dev/null
@@ -1,98 +0,0 @@
-#!/usr/bin/env python3
-import copy
-
-import rclpy
-from rclpy.node import Node
-from visualization_msgs.msg import MarkerArray
-
-
-class Robot2FilteredRayTransposer(Node):
-    def __init__(self):
-        super().__init__('robot_2_filtered_ray_transposer')
-
-        # Taken from simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
-        #old fire academy init:
-        # robot_1_init = (20.0, -7.0, 0.15)
-        # robot_2_init = (17.0, 1.5, 0.15)
-        #RetroNeighborhood init:
-        # robot_1_init = (35.0, -19.0, 0.15)
-        # robot_2_init = (30.0, -19.0, 0.15)
-
-        robot_1_init = (25.0, 7.6, 0.15)
-        robot_2_init = (23.4, 9.8, 0.15)
-
-        dx = robot_2_init[0] - robot_1_init[0]
-        dy = robot_2_init[1] - robot_1_init[1]
-        dz = robot_2_init[2] - robot_1_init[2]
-
-        self.declare_parameter('translation_x', dx)
-        self.declare_parameter('translation_y', dy)
-        self.declare_parameter('translation_z', dz)
-        self.declare_parameter('color_r', 0.0)
-        self.declare_parameter('color_g', 0.6)
-        self.declare_parameter('color_b', 1.0)
-        self.declare_parameter('color_a', 1.0)
-
-        self.translation_x = float(self.get_parameter('translation_x').value)
-        self.translation_y = float(self.get_parameter('translation_y').value)
-        self.translation_z = float(self.get_parameter('translation_z').value)
-        self.color_r = float(self.get_parameter('color_r').value)
-        self.color_g = float(self.get_parameter('color_g').value)
-        self.color_b = float(self.get_parameter('color_b').value)
-        self.color_a = float(self.get_parameter('color_a').value)
-
-        self.subscription = self.create_subscription(
-            MarkerArray,
-            '/robot_2/filtered_rays',
-            self.callback,
-            10,
-        )
-        self.publisher = self.create_publisher(
-            MarkerArray,
-            '/robot_2/filtered_rays/transposed',
-            10,
-        )
-
-        self.get_logger().info(
-            f'Publishing robot_2 transposed rays with translation '
-            f'({self.translation_x:.3f}, {self.translation_y:.3f}, {self.translation_z:.3f})'
-        )
-
-    def callback(self, msg: MarkerArray):
-        out = copy.deepcopy(msg)
-
-        for marker in out.markers:
-            # Opposite direction from robot_1_semantic_ray_transposer.py:
-            # apply +translation instead of -translation.
-            marker.pose.position.x += self.translation_x
-            marker.pose.position.y += self.translation_y
-            marker.pose.position.z += self.translation_z
-            marker.color.r = self.color_r
-            marker.color.g = self.color_g
-            marker.color.b = self.color_b
-            marker.color.a = self.color_a
-
-            for point in marker.points:
-                point.x += self.translation_x
-                point.y += self.translation_y
-                point.z += self.translation_z
-
-            for color in marker.colors:
-                color.r = self.color_r
-                color.g = self.color_g
-                color.b = self.color_b
-                color.a = self.color_a
-
-        self.publisher.publish(out)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = Robot2FilteredRayTransposer()
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()
diff --git a/common/ros_packages/desktop_bringup/robot_2_relative_position.py b/common/ros_packages/desktop_bringup/robot_2_relative_position.py
deleted file mode 100755
index e69de29b..00000000
diff --git a/common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg b/common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg
deleted file mode 100644
index 22f5162d..00000000
--- a/common/ros_packages/msgs/airstack_msgs/msg/BeliefMapData.msg
+++ /dev/null
@@ -1,10 +0,0 @@
-# serialized BeliefMap data
-# assumes same origin, same grid resolution
-
-int16 x_start # cell number relative to the origin. max int16 is 32767, so this is a reasonable assumption
-int16 y_start
-int32 x_end # if we break things up in the future, could assume always square and remove this
-int32 y_end # if we break things up in the future, could assume always square and remove this
-
-uint16[] map_values # between 0 and 1, 65535 values, precision of 0.000015
-
diff --git a/docs/gcs/docker/index.md b/docs/gcs/docker/index.md
index 6c76ee8a..47c721c6 100644
--- a/docs/gcs/docker/index.md
+++ b/docs/gcs/docker/index.md
@@ -50,20 +50,12 @@ The GCS container is launched with:
 airstack up gcs
 ```
 
-For HITL (host networking + scalable DDS discovery), use:
-
-```bash
-airstack up gcs-hitl
-```
-
-`gcs-hitl` starts the Fast DDS discovery server process inside the same container when `HITL_DISCOVERY_MODE=server`, so discovery is owned by the GCS host without a separate image pull.
-
 **What happens:**
 
 1. Docker Compose starts the GCS container from `gcs/docker/docker-compose.yaml`
 2. The `command:` attribute launches GCS ROS 2 nodes
 3. RQT GUI opens automatically (if `DISPLAY` configured)
-4. Foxglove Studio starts and `foxglove_bridge` serves on port 8765
+4. Foxglove Studio starts on port 8765
 
 **Container command:**
 
@@ -84,7 +76,7 @@ Key variables for GCS configuration:
 
 | Variable | Description | Default |
 |----------|-------------|---------|
-| `VERSION` | Image version tag | Set in `.env` |
+| `DOCKER_IMAGE_TAG` | Image version tag | Set in `.env` |
 | `DOCKER_IMAGE_BUILD_MODE` | Build mode (release/dev) | `dev` |
 | `AUTOLAUNCH` | Auto-start GCS on container launch | `true` |
 | `ROS_DOMAIN_ID` | ROS 2 domain (0 for GCS) | `0` |
@@ -114,14 +106,6 @@ The GCS container connects to the `airstack_network` bridge network, allowing co
 - **ROS_DOMAIN_ID:** 0 (GCS shares domain 0)
 - **Ports:** 8765 (Foxglove), 22 (SSH)
 
-### HITL Host-Network Mode
-
-When using `gcs-hitl`, the container runs in `network_mode: host`:
-
-- Foxglove bridge binds to `0.0.0.0:8765`
-- DDS discovery uses `ROS_DISCOVERY_SERVER` (from `DISCOVERY_SERVER_IP` / `DISCOVERY_SERVER_PORT`)
-- No `airstack_network` bridge isolation; traffic is on the host LAN interfaces
-
 ## Accessing the GCS
 
 ### Via X11 Forwarding
@@ -159,12 +143,6 @@ Access Foxglove Studio in your web browser:
 http://localhost:8765
 ```
 
-For remote visualization from another machine on the same network:
-
-```
-ws://<gcs_host_ip>:8765
-```
-
 Pre-configured layouts are available in `gcs/docker/Foxglove/layouts/`.
 
 ## Foxglove Configuration
@@ -272,12 +250,6 @@ docker compose build --no-cache gcs
 - Check container networking: `docker inspect airstack-gcs-1`
 - Access via container IP if localhost fails
 
-**HITL DDS discovery issues:**
-
-- Verify `ROS_DISCOVERY_SERVER` is set in the GCS container
-- Verify `DISCOVERY_SERVER_IP:DISCOVERY_SERVER_PORT` is reachable from all robot hosts
-- If discovery works but no data flows, verify DDS Router allowlist and QoS compatibility
-
 **ROS 2 communication issues:**
 
 - Verify `ROS_DOMAIN_ID=0` in GCS container
diff --git a/docs/gcs/foxglove.md b/docs/gcs/foxglove.md
new file mode 100644
index 00000000..4e745418
--- /dev/null
+++ b/docs/gcs/foxglove.md
@@ -0,0 +1,134 @@
+# GCS Foxglove Visualization
+
+The GCS runs a **Foxglove Studio** browser interface backed by a single ROS 2 node — `foxglove_visualizer_node` — that gathers per-robot data from the cross-domain bridge and republishes it on a small set of GCS-side topics. Foxglove subscribes to those topics and shows the fleet in 3D.
+
+This page describes what the node visualizes today, the topic naming convention, and where to edit when you want to change or add a marker type. For the gossip payload visualization (filtered rays, voxel maps, etc.) see [Coordination Payloads](../robot/autonomy/coordination/payloads.md).
+
+![Full GCS Foxglove view — overhead-textured 3D panel on top, Robot Tasks panel and per-robot camera + depth feeds along the bottom](foxglove_full_screen.png)
+
+## What gets visualized
+
+The visualizer auto-discovers any robot whose topics match the AirStack convention (default prefix: `robot`). For each discovered robot it subscribes to a fixed set of suffixes:
+
+| Suffix | Type | What it becomes on the GCS |
+|---|---|---|
+| `/interface/mavros/global_position/global` | `NavSatFix` | Robot location pin on the Map panel |
+| `/odometry_conversion/odometry` | `Odometry` | Body-frame pose / orientation arrow |
+| `/trajectory_controller/trajectory_vis` | `MarkerArray` | Live executing trajectory |
+| `/global_plan` | `Path` | Global plan polyline |
+| `/vdb_mapping/vdb_map_visualization` | `Marker` | Per-robot VDB occupancy mesh |
+
+All of these are published by individual robots in their **local `map` frame** (origin = drone boot position). The visualizer translates them into a single global `map` frame on the GCS using each robot's GPS boot offset, and merges everything into one `MarkerArray`.
+
+## Output topics
+
+| Topic | Type | What it carries |
+|---|---|---|
+| `/gcs/robot_markers` | `MarkerArray`  | Combined per-robot markers (mesh, trajectory, plan, VDB) in global ENU |
+| `/gcs/{robot_name}/location` | `NavSatFix` | Per-robot GPS rewritten to `frame_id='map'` — Foxglove's Map panel only accepts it that way |
+| `/gcs/map_origin/location` | `NavSatFix` | Stationary fix at the configured `ORIGIN_LAT/LON` so the Map panel has a fixed reference |
+| `/gcs/sim_ground` | `Marker` | Sim overhead-camera output rendered as a textured ground plane (sim only) |
+| `/gcs/payload/{robot}/{name}` | varies  | Per-robot gossip-payload republish (one topic per registered handler) |
+
+
+## Discovery loop
+
+`_discover_robots` runs every 5 seconds. It calls `get_topic_names_and_types()`, regex-matches each suffix above, and creates a subscription if it sees a topic it doesn't already track. Robots that come online late are picked up on the next tick.
+
+To change which prefix is matched (e.g. you renamed robots from `robot_*` to `drone_*`), set the `robot_name_prefix` parameter on the visualizer node.
+
+## How to modify or add a marker type
+
+The visualizer is designed to be extended in-place. The pattern, taken from `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/foxglove_visualizer_node.py`:
+
+### 1. Add a suffix and regex
+
+```python
+PLAN_SUFFIX = '/global_plan'
+self._plan_pattern = re.compile(rf'^/({re.escape(self._prefix)}_\w+){re.escape(PLAN_SUFFIX)}$')
+```
+
+### 2. Add state
+
+```python
+self._global_plans   = {}   # robot_name -> latest msg
+self._subscribed_plan = set()
+```
+
+### 3. Subscribe in `_discover_robots`
+
+```python
+if topic not in self._subscribed_plan:
+    m = self._plan_pattern.match(topic)
+    if m and 'nav_msgs/msg/Path' in type_list:
+        name = m.group(1)
+        self.create_subscription(
+            Path, topic,
+            lambda msg, n=name: self._plan_callback(msg, n),
+            10,   # 10 = default RELIABLE for planning topics;
+                  # SENSOR_QOS for high-rate sensor streams
+        )
+        self._subscribed_plan.add(topic)
+```
+
+
+### 4. Add a callback
+
+```python
+def _plan_callback(self, msg: Path, robot_name: str):
+    self._global_plans[robot_name] = msg
+```
+
+### 5. Render in `_publish_markers`
+
+```python
+plan = self._global_plans.get(robot_name)
+boot = self._gps_boot.get(robot_name)
+if plan is not None and boot is not None:
+    bx, by, bz = boot
+    line = Marker()
+    line.header.frame_id = 'map'
+    line.ns = f'{robot_name}_global_plan'
+    line.type = Marker.LINE_STRIP
+    for ps in plan.poses:
+        p = ps.pose.position
+        line.points.append(Point(x=p.x + bx, y=p.y + by, z=p.z + bz))
+    array.markers.append(line)
+```
+
+
+### 6. Bridge the source topic across DDS domains
+
+The visualizer can only subscribe to topics that crossed the DDS bridge. Add the source topic to `robot/ros_ws/src/autonomy_bringup/onboard_all/config/dds_router.yaml` under `allowlist`:
+
+```yaml
+allowlist:
+  - name: "rt/$(env ROBOT_NAME)/your/new_topic"
+```
+
+Then restart the robot containers — the router only re-reads its allowlist on startup.
+
+## Bridging a topic without writing a callback
+
+If your topic is already in a Foxglove-native type (`nav_msgs/Path`, `sensor_msgs/PointCloud2`, `visualization_msgs/MarkerArray`) and doesn't need the GPS offset, you can skip the visualizer entirely — just bridge it through the DDS router and add a panel in Foxglove pointing at the topic. The visualizer is only required when you need georeferencing or want everything to flow through the combined `/gcs/robot_markers` namespace.
+
+## Sim-only: textured overhead ground
+
+When running in sim, the visualizer also subscribes to `/sim/overhead/image` + `/sim/overhead/spec`. On receiving both, it builds one `TRIANGLE_LIST` marker on `/gcs/sim_ground` (latched) and tears down its subscriptions. See [2D World Map in Foxglove](../simulation/isaac_sim/overhead_camera.md) for the producer side.
+
+## Troubleshooting
+
+| Symptom | Likely cause |
+|---|---|
+| Robot doesn't appear at all | Source topic isn't in the DDS router allowlist, or the GPS topic isn't publishing yet |
+| Robot appears at the wrong global location | First GPS fix had wrong altitude datum, or PX4 home wasn't set (sim) |
+| Markers double-offset (visibly twice as far from where they should be) | Both `pose.position` and `points[]` were offset in the render loop |
+| New marker added but never shows up | Discovery hasn't fired yet (5 s interval), or topic name doesn't match the regex |
+| Foxglove "frame `map` does not exist" | The static `world → map` TF didn't reach Foxglove — restart the GCS container |
+
+## See also
+
+- [Coordination Payloads](../robot/autonomy/coordination/payloads.md) — extending visualization with gossip-broadcast payloads
+- [Adding Waypoints and Geofences](waypoints_and_geofences.md) — interactive click-to-place editors
+- [Overhead Camera](../simulation/isaac_sim/overhead_camera.md) — sim-side ground texture producer
+- [`.agents/skills/visualize-in-foxglove`](../../.agents/skills/visualize-in-foxglove/SKILL.md) — agent workflow for adding a topic
diff --git a/docs/gcs/foxglove_full_screen.png b/docs/gcs/foxglove_full_screen.png
new file mode 100644
index 0000000000000000000000000000000000000000..cf1e9ef13826fe802b7edd604e4c50835f803e72
GIT binary patch
diff --git a/docs/gcs/foxglove_publish_point.png b/docs/gcs/foxglove_publish_point.png
new file mode 100644
index 0000000000000000000000000000000000000000..6508d1bf1ecd94238ec6abcca736c76a1083d590
GIT binary patch
diff --git a/docs/gcs/polygon_editor.png b/docs/gcs/polygon_editor.png
new file mode 100644
index 0000000000000000000000000000000000000000..7407bb21337dc9c219723787726ef5440264a994
GIT binary patch
diff --git a/docs/gcs/waypoint_editor.png b/docs/gcs/waypoint_editor.png
new file mode 100644
index 0000000000000000000000000000000000000000..774625e15e95ceea4aecdc7d2a9fd6c275863ff9
GIT binary patch
diff --git a/docs/gcs/waypoints_and_geofences.md b/docs/gcs/waypoints_and_geofences.md
new file mode 100644
index 00000000..530235df
--- /dev/null
+++ b/docs/gcs/waypoints_and_geofences.md
@@ -0,0 +1,87 @@
+# Adding Waypoints and Geofences
+
+The GCS has two click-to-place panels in Foxglove:
+
+- **Waypoint Editor** — drop ordered 3D waypoints for the Navigate task.
+- **Polygon Editor** — drop vertices of a 2D area to use as a **geofence** / search bounds for the Exploration and Coverage tasks.
+
+Both panels work the same way: enable click capture then click in the 3D panel to place points.
+
+![Robot Tasks panel — Navigate tab with the embedded Waypoint Editor](waypoint_editor.png)
+
+The two editors live inside the **Robot Tasks** panel — the Waypoint Editor appears under the **Navigate** tab, and the Polygon Editor appears under the **Exploration** and **Coverage** tabs (where it feeds the `search_bounds` field).
+
+<iframe src="https://drive.google.com/file/d/10DYLMBoDlFsCS5n-Sdk2HdDlPbERr_gi/preview" width="100%" height="480" allow="autoplay" allowfullscreen></iframe>
+
+*End-to-end demo: enabling click capture, dropping points, saving the set, then sending it to a robot.*
+
+## Place points
+
+1. In the editor panel, toggle **Enable click capture** on.
+2. (Optional) In the top right of the **3D** panel, switch the camera to a top-down view to make it easier to drop points on the ground plane.
+3. In the 3D panel toolbar, click the **Publish** tool (top right, ▷ icon) and switch its mode to **Publish 2D point (/clicked_point)** — this is what sends clicks to the editor.
+
+    ![3D-mode toggle and the Publish 2D point option in the Foxglove 3D panel toolbar](foxglove_publish_point.png){ width="380" }
+
+4. Click anywhere in the 3D panel. A red marker appears at the click location. The waypoint editor draws spheres in click order; the polygon editor draws a closed loop.
+5. The **Default altitude** field controls the `z` coordinate that gets attached to each click — set it once, then click freely on the ground.
+
+To add a point without clicking — e.g. for a precise coordinate — type values into the **+ Add** row and press Enter.
+
+## Reorder, edit, delete or duplicate
+
+- **Reorder** — drag a row up or down in the active list. The marker numbering updates immediately.
+- **Edit a point** — click the row, edit the `x` / `y` / `z` fields, press Enter.
+- **Delete a point** — click the ✕ on the row.
+- **Clear all** — click **Clear**. Doesn't touch saved sets.
+- **Duplicate** — click the ⧉ icon on a row to insert a copy of that point directly after it. Useful for laying down repeated patterns like a survey grid where each new point is a small offset from the previous one.
+
+For polygons specifically, vertex order defines the perimeter — reorder rows to flip the polygon shape.
+
+
+## Save and load
+
+Saves let you name a set of points and bring them back later — the Robot Tasks panel reads the same saves, so a saved waypoint set can be selected as a Navigate target and a saved polygon can be selected as `search_bounds` for Exploration / Coverage.
+
+Two-step save flow:
+
+1. Type a name into the **save name…** field, then click **+ Add**. The save now exists in memory and shows up in the saves list.
+2. Click **Save** on that row to persist it to disk. The button changes to **✓ Saved** when the file write succeeds.
+
+Other actions per saved row:
+
+- **Load** — replaces the active list with the saved one. Useful for re-editing a previously persisted set.
+- **Delete** — removes the save from both memory and disk.
+
+Saves are written to host-mounted JSON files inside the GCS container:
+
+| Editor | File |
+|---|---|
+| Waypoints | `~/.airstack/gcs_waypoint_saves.json` |
+| Polygons | `~/.airstack/gcs_polygon_saves.json` |
+
+These survive container restarts and can be hand-edited or version-controlled if you want to ship a curated mission set.
+
+## Use them in a task
+
+- **Navigate** — in the Robot Tasks panel, select the **Navigate** tab, pick a saved waypoint set from the **from:** dropdown (or **active** to use whatever's currently in the editor), pick a robot, click **Send**. The **Grab** button copies the current selection into the JSON `waypoints` field below.
+- **Exploration** / **Coverage** — same flow, but the polygon save fills the `search_bounds` field.
+
+![Robot Tasks panel — Exploration tab with `search_bounds` populated from the Polygon Editor](polygon_editor.png)
+
+If a save doesn't appear in the dropdown after creating it, click the dropdown again to refresh — the Tasks panel re-reads the latched saves topic on focus.
+
+## Troubleshooting
+
+| Symptom | Likely cause |
+|---|---|
+| Clicks don't register | Click capture isn't enabled, or the Publish tool isn't set to **Click position** |
+| Clicks register but no marker shows | The 3D panel doesn't have the editor's marker topic enabled — open its Topics list and toggle on `/gcs/waypoints/markers` (or `/gcs/polygon/markers`) |
+| Saves don't persist | Host volume `~/.airstack` not mounted on the GCS container |
+| Save name silently overwrites | Both **+ Add** and **Save** overwrite by name — pick a unique name |
+| Tasks panel doesn't see a new save | Re-open the dropdown to refresh, or restart the panel |
+
+## See also
+
+- [GCS Foxglove Visualization](foxglove.md) — the multi-robot fleet view alongside these editors
+- [Coordination Payloads](../robot/autonomy/coordination/payloads.md) — for sharing custom data fleet-wide
diff --git a/docs/real_world/HITL/index.md b/docs/real_world/HITL/index.md
index ecabf770..bf6057c7 100644
--- a/docs/real_world/HITL/index.md
+++ b/docs/real_world/HITL/index.md
@@ -1,114 +1,25 @@
 # Hardware-In-The-Loop Simulation
+We configure a multi-machine HITL simulation, where a powerful desktop computer runs Isaac Simulator and rendering, and one/multiple jetson compute boards run robot-specific programs (planning, mapping, etc.).
+## Requirement
+A desktop computer configured according to [here](/docs/getting_started). One/multiple ORIN AGX/NX configured according to [here](/docs/real_world/installation/).
 
-AirStack supports multi-machine HITL where simulation and GCS run on one host, and one or more robot compute hosts run autonomy containers. HITL services use `network_mode: host` so DDS traffic is exchanged directly on your LAN.
+## Communication
+All machines should connect to the same network. In our test, all machines are connected to the same router with ethernet cables. Ensure that all machines are able to `ping` others' IP addresses.
 
-## Requirements
-
-- One simulation/GCS host configured per [Getting Started](/docs/getting_started)
-- One or more robot hosts (Jetson or desktop) configured per [Real-World Installation](/docs/real_world/installation/)
-- All hosts on the same routed network with mutual `ping` reachability
-
-## Discovery Model (Default)
-
-HITL defaults to Fast DDS Discovery Server mode for scalability:
-
-- Discovery server endpoint defaults to:
-  - `DISCOVERY_SERVER_IP=192.168.233.25`
-  - `DISCOVERY_SERVER_PORT=11811`
-- Discovery clients:
-  - `isaac-sim-hitl`
-  - `gcs-hitl`
-  - `robot-desktop-hitl`
-- Optional fallback mode:
-  - `HITL_DISCOVERY_MODE=static-peer`
-  - `FASTDDS_STATIC_PEER_IP=<peer_ip>`
-
-## Services
-
-### Simulation/GCS Host
-
-Run:
-
-```bash
-airstack up isaac-sim-hitl gcs-hitl
+### Run
+On the desktop computer, under your Airstack folder, run
 ```
-
-This starts:
-
-- Isaac Sim (`isaac-sim-hitl`)
-- GCS (`gcs-hitl`)
-- Fast DDS discovery server process inside the `gcs-hitl` container
-
-### Robot Host
-
-For a desktop robot host:
-
-```bash
-airstack up robot-desktop-hitl
+docker compose up isaac-sim-hitl
 ```
-
-For a Jetson host:
-
-```bash
-airstack up robot-l4t
+You should see the isaac simulator being launched.
+On the Jetson computer, run
 ```
+docker compose up robot_l4t
+```
+Once the scene is played in the Isaac simulator, the rviz GUI on the Jetson should start displaying sensor data, which means the connection is successful. 
 
-## Two-Computer Validation Checklist
-
-1. Verify discovery variables on both hosts:
-   ```bash
-   echo "$DISCOVERY_SERVER_IP:$DISCOVERY_SERVER_PORT"
-   ```
-2. Verify DDS discovery server visibility from robot host:
-   ```bash
-   ping "$DISCOVERY_SERVER_IP"
-   ```
-3. Verify ROS graph visibility:
-   ```bash
-   ros2 topic list
-   ```
-4. Verify payload flow (not just discovery):
-   ```bash
-   ros2 topic hz /robot_1/odometry
-   ros2 topic echo /robot_1/odometry --once
-   ```
-5. Verify Foxglove:
-   - On GCS host: `ws://localhost:8765`
-   - On remote client (optional): `ws://<gcs_host_ip>:8765`
-
-## Multi-Robot / Multi-Computer Scaling
-
-For `N` robot hosts:
-
-- `gcs-hitl` starts and owns the discovery server on the sim/GCS host.
-- Start one robot container stack per robot host (`robot-desktop-hitl` or `robot-l4t`).
-- Ensure each robot has unique `ROBOT_NAME` and `ROS_DOMAIN_ID` mapping.
-
-Recommended:
-
-- Use discovery server mode for `N >= 3`.
-- Configure backup discovery servers using `DISCOVERY_SERVER_BACKUP_IPS` (comma-separated IP list).
-
-## Troubleshooting
-
-### Topics discovered but no data
-
-This is usually not discovery failure. Check:
-
-1. DDS Router allowlist includes the topic
-2. ROS domain IDs are correct on both sides
-3. QoS compatibility between publisher/subscriber
-4. Firewall allows DDS UDP traffic
-5. Topic namespaced correctly (`/robot_X/...`)
-
-### Foxglove shows no topics
-
-- Confirm `foxglove_bridge` is running in GCS launch
-- Confirm GCS is receiving ROS topics in domain 0
-- Use `ws://localhost:8765` on GCS host first, then test remote URL
-
-### Discovery server unreachable
+Screen record of desktop computer:
+<iframe src="https://drive.google.com/file/d/1sNkEattgDyBAI9xFPVQsXn8sRi6gYQSG/view?usp=sharing" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
 
-- Verify `DISCOVERY_SERVER_IP` matches the host running `gcs-hitl`
-- Verify no port conflict on `11811`
-- Verify host firewall/network ACLs
\ No newline at end of file
+Screen record of Jetson computer:
+<iframe src="https://drive.google.com/file/d/19S8Yceq8t2FPubN8Mly003Up1AbLzCA8/view?usp=sharing" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
\ No newline at end of file
diff --git a/docs/robot/autonomy/coordination/index.md b/docs/robot/autonomy/coordination/index.md
index aab36cdc..cabff92f 100644
--- a/docs/robot/autonomy/coordination/index.md
+++ b/docs/robot/autonomy/coordination/index.md
@@ -22,9 +22,6 @@ Robot 1 (domain 1)          Shared gossip domain (99)          GCS (domain 0)
 
 Each robot builds a local registry of all known peers from incoming messages. The registry never leaves the robot — only each drone's own profile is transmitted.
 
-!!! note "Partial connectivity"
-    Today the system assumes all-to-all visibility via the shared domain. If a drone is out of radio range of another, it will simply not appear in that drone's registry. Relay forwarding is planned but not yet implemented.
-
 ## PeerProfile
 
 Every message on `/gossip/peers` is a `PeerProfile` containing:
diff --git a/docs/robot/autonomy/coordination/payloads.md b/docs/robot/autonomy/coordination/payloads.md
index da9cb678..48917b47 100644
--- a/docs/robot/autonomy/coordination/payloads.md
+++ b/docs/robot/autonomy/coordination/payloads.md
@@ -13,7 +13,7 @@ Payloads are **config-driven** — no changes to `gossip_node.py` are needed.
 
 ## Step 1 — Add to gossip_payloads.yaml
 
-**File:** `robot/ros_ws/src/coordination/coordination_bringup/config/gossip_payloads.yaml`
+**File:** `common/ros_packages/coordination/coordination_bringup/config/gossip_payloads.yaml`
 
 ```yaml
 payload_topics:
@@ -41,20 +41,7 @@ ros2 run coordination_bringup peer_registry_monitor
 
 ## Step 2 — Visualize in Foxglove
 
-Payloads don't appear in Foxglove automatically — you need a handler in `payload_visualizer_node.py` that republishes the payload to its own topic. There are two ways to do this:
-
-### Option A — Use the Claude skill (recommended)
-
-The `attach-gossip-payload` skill handles both the yaml edit and the GCS handler in one go. In Claude Code:
-
-```
-Follow the attach-gossip-payload skill to add /{robot_name}/your/topic
-of type your_msgs/msg/YourType and visualize it in Foxglove
-```
-
-See the full skill at .agents/skills/attach-gossip-payload
-
-### Option B — Manual
+Payloads don't appear in Foxglove automatically — you need a handler in `payload_visualizer_node.py` that republishes the payload to its own topic. The manual steps are below; an [AI-native skill](#ai-native-skill) is also available to do both this and Step 1 in one go.
 
 **File:** `gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/payload_visualizer_node.py`
 
@@ -73,10 +60,7 @@ def _handle_your_payload(self, robot_name, msg, i, now):
 
 ```python
 PAYLOAD_HANDLERS = {
-    'filtered_rays':       ('visualization_msgs/msg/MarkerArray', _handle_filtered_rays),
-    'frontier_viewpoints': ('sensor_msgs/msg/PointCloud2',        _handle_frontier_viewpoints),
-    'rgb_voxels':          ('sensor_msgs/msg/PointCloud2',        _handle_rgb_voxels),
-    'your_name':           ('your_msgs/msg/YourType',             _handle_your_payload),  # ← add
+    'your_name': ('your_msgs/msg/YourType', _handle_your_payload),
 }
 ```
 
@@ -85,23 +69,35 @@ PAYLOAD_HANDLERS = {
 ```bash
 docker exec airstack-gcs-1 bash -c "bws --packages-select gcs_visualizer && sws"
 ```
-or restart Airstack
+
+or restart AirStack.
 
 Foxglove will now show `/gcs/payload/{robot_name}/your_name` as a subscribable topic with full visualization controls.
 
+### AI-native skill
+
+The `attach-gossip-payload` skill automates Step 1 and Step 2 — yaml edit and the GCS handler in a single pass. In Claude Code:
+
+```
+Follow the attach-gossip-payload skill to add /{robot_name}/your/topic
+of type your_msgs/msg/YourType and visualize it in Foxglove
+```
+
+See the full skill at `.agents/skills/attach-gossip-payload`.
+
 ## Visualization options
 
 For `PointCloud2` payloads, you have two options:
 
-**Default — Foxglove GUI:** Publish as raw `PointCloud2`. Foxglove's panel settings control point size, shape, and color per-user. No code changes needed.
+**Default — Foxglove GUI:** Publish as raw. Foxglove's panel settings control point size, shape, and color per-user. No code changes needed.
 
-**Preconfigured — fixed shape/size/color in code:** Convert to a `MarkerArray` in the handler. An example to set`voxel_rgb` to render as 0.5 m cubes with per-point RGB colors:
+**Preconfigured — fixed shape/size/color in code:** Convert to a `MarkerArray` in the handler. An example that renders a point cloud as 0.5 m cubes with per-point RGB colors:
 
 ```python
-def _handle_rgb_voxels(self, robot_name, msg, i, now):
+def _handle_my_payload(self, robot_name, msg, i, now):
     marker = point_cloud2_to_cube_marker(
         msg, 0.0, 0.0, self._display_z_offset(),
-        ns=f'{robot_name}_voxel_rgb',
+        ns=f'{robot_name}_my_payload',
         marker_id=i * 100000,
         stamp=now,
         lifetime=Duration(sec=2, nanosec=0),
@@ -111,9 +107,5 @@ def _handle_rgb_voxels(self, robot_name, msg, i, now):
     if marker is not None:
         out = MarkerArray()
         out.markers.append(marker)
-        self._pub_for(f'/gcs/payload/{robot_name}/voxel_rgb', MarkerArray).publish(out)
+        self._pub_for(f'/gcs/payload/{robot_name}/my_payload', MarkerArray).publish(out)
 ```
-
-## Bandwidth note
-
-Payloads are re-serialized and sent in full every tick. A 500 KB PointCloud2 at 1 Hz is ~4 Mbps per robot — keep an eye on this for large maps. Reduce the gossip `publish_rate` parameter or only attach payloads when needed for bandwidth-constrained deployments.
diff --git a/docs/simulation/index.md b/docs/simulation/index.md
index ddde7f33..0a3aa935 100644
--- a/docs/simulation/index.md
+++ b/docs/simulation/index.md
@@ -17,12 +17,6 @@ simulation/
 │   ├── extensions/                # Custom Isaac Sim extensions
 │   ├── launch_scripts/            # Python launch scripts
 │   └── standalone_examples/       # Example scenes and scripts
-├── ms-airsim/
-│   ├── docker/                    # Microsoft AirSim (legacy) containerization
-│   │   ├── docker-compose.yaml    # Launch configuration
-│   │   └── Dockerfile             # Image definition
-│   ├── config/                    # Microsoft AirSim (legacy) settings.json
-│   └── ros_ws/                    # Depth bridge ROS package
 └── simple-sim/
     ├── docker/                    # Simple simulator container
     │   └── docker-compose.yaml    # Launch configuration
@@ -35,7 +29,6 @@ simulation/
 Simulation components are launched via Docker Compose. Each simulator has its own configuration:
 
 - **Isaac Sim:** `simulation/isaac-sim/docker/docker-compose.yaml`
-- **Microsoft AirSim (legacy):** `simulation/ms-airsim/docker/docker-compose.yaml`
 - **Simple Sim:** `simulation/simple-sim/docker/docker-compose.yaml`
 
 **Key launch points:**
@@ -76,20 +69,6 @@ Isaac Sim is our primary simulation platform, offering:
 - [Ascent SITL Extension](isaac_sim/ascent_sitl_extension.md)
 - [Export from Unreal Engine](isaac_sim/export_stages_from_unreal.md)
 
-### Microsoft AirSim (legacy) (Unreal Engine)
-
-An open-source drone simulator built on Unreal Engine with native PX4 SITL integration.
-
-**Use cases:**
-
-- PX4-in-the-loop testing with photorealistic environments
-- Depth-based obstacle avoidance testing (DROAN)
-- Environments from the Unreal Engine ecosystem
-
-**Launch:** `airstack up --env-file overrides/ms-airsim.env`
-
-**Location:** `simulation/ms-airsim/`
-
 ### Simple Sim (Lightweight)
 
 A lightweight 2D/3D simulator for basic testing and development when full Isaac Sim fidelity isn't needed.
@@ -133,26 +112,6 @@ A lightweight 2D/3D simulator for basic testing and development when full Isaac
 
 **Learn more:** [Docker Workflow](../development/beginner/airstack-cli/docker_usage.md#robot)
 
-### Multi-Computer HITL Simulation
-
-For cross-machine HITL, run simulation + GCS on one host and robot containers on other hosts:
-
-```bash
-# Sim/GCS host
-airstack up isaac-sim-hitl gcs-hitl
-
-# Robot host
-airstack up robot-desktop-hitl
-```
-
-Discovery defaults:
-
-- `HITL_DISCOVERY_MODE=server`
-- `DISCOVERY_SERVER_IP=192.168.233.25`
-- `DISCOVERY_SERVER_PORT=11811`
-
-For larger fleets, set `DISCOVERY_SERVER_BACKUP_IPS` with backup server IPs.
-
 ### Custom Scenes
 
 Create custom Isaac Sim scenes with:
@@ -204,12 +163,6 @@ PLAY_SIM_ON_START=false airstack up isaac-sim
 - Check `ROS_DOMAIN_ID` settings in containers
 - See [Docker Workflow](../development/beginner/airstack-cli/docker_usage.md)
 
-**HITL discovery but no payload data:**
-
-- Verify `ROS_DISCOVERY_SERVER` in all HITL containers
-- Verify DDS Router allowlist includes required topics
-- Verify QoS compatibility (`ros2 topic info -v <topic>`)
-
 **Performance issues:**
 
 - Reduce scene complexity
diff --git a/docs/simulation/isaac_sim/overhead_camera.md b/docs/simulation/isaac_sim/overhead_camera.md
new file mode 100644
index 00000000..86fb91e1
--- /dev/null
+++ b/docs/simulation/isaac_sim/overhead_camera.md
@@ -0,0 +1,124 @@
+# 2D World Map in Foxglove
+
+There are two ways to render a 2D ground reference under your fleet in Foxglove's 3D panel:
+
+1. **Real-world satellite images** — for outdoor flights. Foxglove's built-in **Map** panel fetches satellite/road tiles from an online tile provider and pins your robots on them via GPS.
+2. **Simulated overhead camera** — for sim. A static top-down orthographic camera captures the scene once and publishes it as an aerial image; the GCS renders it as a textured ground plane.
+
+Both paths render into the same global `map` frame, so robot markers, trajectories, and gossip payloads sit correctly on top in either case.
+
+## Real-World Satellite Images
+
+For outdoor flights, no special AirStack configuration is needed — the GCS publishes everything Foxglove's Map panel needs:
+
+- Each robot's GPS gets republished on `/gcs/{robot_name}/location` (with `frame_id='map'` so the Map panel accepts it).
+- A stationary fix at `ORIGIN_LAT/LON` is published on `/gcs/map_origin/location` so the panel has a fixed anchor and doesn't auto-recenter on whichever robot moves first.
+
+In Foxglove:
+
+1. Add a **Map** panel.
+2. Open its settings and pick a tile layer (e.g. **Custom (URL template)** with a satellite tile provider — Foxglove ships with OpenStreetMap by default; for satellite imagery you can use any standard `{z}/{x}/{y}` URL such as Esri's World Imagery).
+3. Add `/gcs/map_origin/location` and each `/gcs/{robot}/location` topic.
+
+The Map panel will draw satellite tiles around the origin and show robot pins as they move.
+
+## Simulated Overhead Camera
+
+The overhead camera is a static, top-down orthographic camera that renders the simulated scene once and publishes it as an aerial image. The GCS picks it up and renders it as a textured ground plane in Foxglove's 3D panel — useful as a visual reference behind the robot markers, especially in scenes that don't have ground-truth satellite imagery.
+
+The scene is static, so the camera publishes briefly at startup, the GCS catches one valid frame, and both sides tear down their subscriptions. After that, the overhead is essentially free.
+
+![Foxglove 3D panel showing the textured overhead under the simulated scene](overhead_in_foxglove.png)
+
+## Enabling it in a sim launch script
+
+Two helpers from `simulation/isaac-sim/utils/scene_prep.py` do all the work. Call both inside the post-load callback (after the stage is loaded but before drones spawn):
+
+```python
+from utils.scene_prep import (
+    add_orthographic_camera, add_overhead_camera_publisher,
+    get_stage_meters_per_unit,
+)
+
+mpu, scene_scale_factor = get_stage_meters_per_unit(stage)
+
+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,
+)
+
+add_overhead_camera_publisher(
+    parent_graph_path="/World/MapCameraGraph",
+    camera_prim_path=cam_path,
+    topic="/sim/overhead/image",
+    spec_topic="/sim/overhead/spec",
+    frame_id="map",
+    coverage_m=OVERHEAD_COVERAGE_M,
+    pixels_per_meter=OVERHEAD_PX_PER_METER,
+    domain_id=0,
+)
+```
+
+The three constants (`OVERHEAD_ALTITUDE_M`, `OVERHEAD_COVERAGE_M`, `OVERHEAD_PX_PER_METER`) are the only knobs you typically need to adjust. Defaults and effect:
+
+| Constant | Default | What it controls |
+|---|---|---|
+| `OVERHEAD_ALTITUDE_M` | `150.0` | Camera height above world origin (m). |
+| `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:
+
+```python
+from pxr import Gf, UsdGeom
+
+cam_path = add_orthographic_camera(stage, prim_path="/World/MapCamera", ...)
+
+# 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,
+))
+```
+
+
+## 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:
+
+1. Downsamples the source image to a coarse triangle grid (default 0.8 cells/m, capped at 384×384) — Foxglove's 3D panel struggles with dense per-pixel meshes, but a coarse triangle grid renders smoothly.
+2. Publishes one `Marker` of type `TRIANGLE_LIST` on `/gcs/sim_ground`.
+
+### Hide the overhead in Foxglove
+
+The ground plane is just a marker on `/gcs/sim_ground`, so you can toggle it on or off in the 3D panel settings without touching any code:
+
+1. Click the gear icon on the **3D** panel.
+2. Open **Topics → `/gcs/sim_ground`**.
+3. Toggle visibility off to hide it.
+
+### Sharper rendering
+
+The default downsample (0.8 cells/m, cap 384) is conservative. To raise the rendered resolution, override two parameters on `foxglove_visualizer_node` in `gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml`:
+
+```xml
+<param name="overhead_grid_per_m"           value="3.0" />
+<param name="overhead_max_grid_resolution"  value="1024" />
+```
+
+`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
+
+- [Spawning Drones](spawning_drones.md) — authoring a full multi-drone launch script
+- [Pegasus Scene Setup](pegasus_scene_setup.md) — single-drone scene authoring
+- [GCS Foxglove Visualization](../../gcs/foxglove.md) — what the visualizer publishes alongside the ground marker
diff --git a/docs/simulation/isaac_sim/overhead_in_foxglove.png b/docs/simulation/isaac_sim/overhead_in_foxglove.png
new file mode 100644
index 0000000000000000000000000000000000000000..6d73704fd70b1f80521eb6022059e5961ee8b72e
GIT binary patch
diff --git a/docs/simulation/isaac_sim/spawning_drones.md b/docs/simulation/isaac_sim/spawning_drones.md
new file mode 100644
index 00000000..692c621a
--- /dev/null
+++ b/docs/simulation/isaac_sim/spawning_drones.md
@@ -0,0 +1,72 @@
+# Spawning Drones
+
+The reference launch scripts under `simulation/isaac-sim/launch_scripts/` cover the progression from a single drone in an empty world up to multiple drones in a custom imported scene with per-drone GPS origins:
+
+| Script | Purpose |
+|---|---|
+| `barebones_pegasus_launch.py` | Minimal Pegasus boilerplate. Single drone, default environment, no scene import. Use as a template for new launch scripts. |
+| `example_one_px4_pegasus_launch_script.py` | One PX4 drone with the standard sensor stack (ZED stereo, optional Ouster lidar) in the default environment. |
+| `example_multi_px4_pegasus_launch_script.py` | `NUM_ROBOTS` drones spawned in a row in the default environment. Each drone gets its own ROS domain id (`1..N`). |
+| `example_multi_drone_scene_import.py` | `NUM_ROBOTS` drones in an **imported scene** (USD from a Nucleus server) with per-drone GPS homes set via `gps_utils.set_gps_origins`. Use this as the starting point for any custom scene. |
+
+The first three are vanilla Pegasus patterns; this page focuses on the multi-drone + custom-scene case where you also need correct GPS homes.
+
+## The DRONE_CONFIGS pattern
+
+`example_multi_drone_scene_import.py` declares all per-drone settings in a single list:
+
+```python
+DRONE_CONFIGS = [
+    {"domain_id": 1, "x_m": -3.0, "y_m": 3.5, "z_m": 0.15, "orient": [0, 0, 0, 1]},
+    {"domain_id": 2, "x_m":  3.0, "y_m": 3.0, "z_m": 0.15, "orient": [0, 0, 0, 1]},
+]
+```
+
+| Field | Purpose |
+|---|---|
+| `domain_id` | ROS domain id and PX4 vehicle id. The robot container with `ROS_DOMAIN_ID=1` will see this drone. |
+| `x_m`, `y_m`, `z_m` | World-frame spawn position in metres. Convention: `+X = East`, `+Y = North`, `+Z = Up`. |
+| `orient` | Spawn orientation quaternion `[x, y, z, w]`. |
+
+To add another drone, append an entry with a fresh `domain_id` and a non-overlapping spawn position. Make sure the corresponding robot container is launched with the same `ROS_DOMAIN_ID` (`NUM_ROBOTS=N airstack up robot-desktop`).
+
+## Per-drone GPS home — `gps_utils`
+
+PX4 needs a GPS home per vehicle. `simulation/isaac-sim/launch_scripts/gps_utils.py` derives one from each drone's world-frame spawn position so all drones share a consistent geographic anchor and end up at distinct GPS coordinates spaced according to their spawn offsets.
+
+```python
+from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+
+set_gps_origins(DRONE_CONFIGS)   # call once before spawning vehicles
+```
+
+`set_gps_origins` does two things per drone:
+
+1. Calls `compute_gps_origin(x_m, y_m, z_m, world_origin)` to convert the spawn offset into `(lat, lon, alt)`. The conversion is a flat-Earth approximation — accurate at scene scales (a few hundred metres), not at continental scale.
+2. Writes `PX4_HOME_LAT_<domain_id>`, `PX4_HOME_LON_<domain_id>`, `PX4_HOME_ALT_<domain_id>` into the process environment. The Pegasus PX4 OmniGraph node reads these when building each drone's `PX4MavlinkBackendConfig`, which passes them to the PX4 SITL subprocess as `PX4_HOME_LAT/LON/ALT`.
+
+### World anchor
+
+The world origin maps to `DEFAULT_WORLD_ORIGIN = (38.736832, -9.137977, 90.0)` — Lisbon, matching the Pegasus default. Override it for a scene set elsewhere:
+
+```python
+set_gps_origins(DRONE_CONFIGS, world_origin=(40.4433, -79.9436, 280.0))  # Pittsburgh
+```
+
+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.
+
+## Common issues
+
+| Symptom | Likely cause | Fix |
+|---|---|---|
+| Drone shows up at the world origin in Foxglove despite being elsewhere in sim | `set_gps_origins` not called, or called *after* spawn | Move the call before vehicle spawning |
+| All drones share one GPS coordinate | `domain_id` collision in `DRONE_CONFIGS` | Give each drone a unique `domain_id` |
+| Map panel centers on the wrong city | Wrong `world_origin` | Override the second arg to `set_gps_origins` |
+| Drone position drifts in the wrong compass direction | Stage axis mismatch | Swap `x_m` ↔ `y_m` in `gps_utils.compute_gps_origin` |
+| Robot container can't see the drone's topics | `ROS_DOMAIN_ID` ≠ `domain_id` in DRONE_CONFIGS | Match them, or set `NUM_ROBOTS` correctly |
+
+## See also
+
+- [Pegasus Scene Setup](pegasus_scene_setup.md) — single-drone authoring background
+- [Overhead Camera](overhead_camera.md) — topdown ground texture
+- [GCS Foxglove Visualization](../../gcs/foxglove.md) — how multi-robot poses render on the GCS
diff --git a/gcs/docker/.bashrc b/gcs/docker/.bashrc
index 03ffeb85..ba851a34 100644
--- a/gcs/docker/.bashrc
+++ b/gcs/docker/.bashrc
@@ -10,55 +10,6 @@ ROS2_WS_DIR="$HOME/AirStack/gcs/ros_ws"
 # needed for communication with Isaac Sim ROS2  # https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#enabling-the-ros-bridge-extension
 export FASTRTPS_DEFAULT_PROFILES_FILE="$ROS2_WS_DIR/src/fastdds.xml"
 
-# HITL DDS mode:
-# - server (default): use Fast DDS Discovery Server via ROS_DISCOVERY_SERVER.
-# - static-peer: generate a temporary Fast DDS profile with one initial peer.
-if [ "${HITL_DISCOVERY_MODE:-}" = "static-peer" ]; then
-    if [ -n "${FASTDDS_STATIC_PEER_IP:-}" ]; then
-        cat > /tmp/fastdds_static_peer.xml <<EOF
-<?xml version="1.0" encoding="UTF-8" ?>
-<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
-    <transport_descriptors>
-        <transport_descriptor>
-            <transport_id>UdpTransport</transport_id>
-            <type>UDPv4</type>
-        </transport_descriptor>
-    </transport_descriptors>
-    <participant profile_name="udp_transport_profile" is_default_profile="true">
-        <rtps>
-            <userTransports>
-                <transport_id>UdpTransport</transport_id>
-            </userTransports>
-            <useBuiltinTransports>false</useBuiltinTransports>
-            <builtin>
-                <initialPeersList>
-                    <locator>
-                        <udpv4>
-                            <address>${FASTDDS_STATIC_PEER_IP}</address>
-                        </udpv4>
-                    </locator>
-                </initialPeersList>
-            </builtin>
-        </rtps>
-    </participant>
-</profiles>
-EOF
-        export FASTRTPS_DEFAULT_PROFILES_FILE="/tmp/fastdds_static_peer.xml"
-    fi
-    unset ROS_DISCOVERY_SERVER
-elif [ -n "${DISCOVERY_SERVER_IP:-}" ]; then
-    discovery_server_list="${DISCOVERY_SERVER_IP}:${DISCOVERY_SERVER_PORT:-11811}"
-    if [ -n "${DISCOVERY_SERVER_BACKUP_IPS:-}" ]; then
-        IFS=',' read -r -a discovery_backup_ips <<< "${DISCOVERY_SERVER_BACKUP_IPS}"
-        for backup_ip in "${discovery_backup_ips[@]}"; do
-            if [ -n "${backup_ip}" ]; then
-                discovery_server_list="${discovery_server_list};${backup_ip}:${DISCOVERY_SERVER_PORT:-11811}"
-            fi
-        done
-    fi
-    export ROS_DISCOVERY_SERVER="${discovery_server_list}"
-fi
-
 # fix ROS2 jazzy setuptools deprecation warning https://robotics.stackexchange.com/questions/24230/setuptoolsdeprecationwarning-in-ros2-jazzy/24349#24349
 PYTHONWARNINGS="ignore:easy_install command is deprecated,ignore:setup.py install is deprecated"
 export PYTHONWARNINGS
diff --git a/gcs/docker/docker-compose.yaml b/gcs/docker/docker-compose.yaml
index 88f5c821..b73f9bc8 100644
--- a/gcs/docker/docker-compose.yaml
+++ b/gcs/docker/docker-compose.yaml
@@ -27,12 +27,3 @@ services:
       - $HOME/bags:/bags
       # TODO: Update rviz config location if needed
       - ../plot:/plot
-
-  # ===================================================================================================================
-  # hitl alias for host-network GCS with discovery-server client defaults.
-  gcs-hitl:
-    profiles: !override
-      - hitl
-    extends:
-      file: ./docker-compose.yaml
-      service: gcs-real
diff --git a/gcs/docker/gcs-base-docker-compose.yaml b/gcs/docker/gcs-base-docker-compose.yaml
index e495d3c4..13bc4422 100644
--- a/gcs/docker/gcs-base-docker-compose.yaml
+++ b/gcs/docker/gcs-base-docker-compose.yaml
@@ -14,14 +14,6 @@ services:
       ssh service restart;
       python3 /root/AirStack/gcs/foxglove_extensions/install.sh;
       tmux new -d -s bringup;
-      if [ \"$${HITL_DISCOVERY_MODE:-}\" = 'server' ]; then
-        if command -v fastdds >/dev/null 2>&1; then
-          tmux new-window -t bringup -n dds_discovery;
-          tmux send-keys -t bringup:dds_discovery 'fastdds discovery -i $${DISCOVERY_SERVER_ID:-0} -l 0.0.0.0 -p $${DISCOVERY_SERVER_PORT:-11811}' ENTER;
-        else
-          echo '[WARN] fastdds CLI not found in gcs container; discovery server not started.';
-        fi;
-      fi;
       if [ $$AUTOLAUNCH = 'true' ]; then
         tmux send-keys -t bringup:0.0 'bws &&  sws; ros2 launch desktop_bringup gcs.launch.xml' ENTER;
       fi;
diff --git a/gcs/gcs_bringup/src/robot_2_relative_position.py b/gcs/gcs_bringup/src/robot_2_relative_position.py
deleted file mode 100644
index f85ad2ed..00000000
--- a/gcs/gcs_bringup/src/robot_2_relative_position.py
+++ /dev/null
@@ -1,61 +0,0 @@
-#!/usr/bin/env python3
-import rclpy
-from rclpy.node import Node
-from nav_msgs.msg import Odometry
-
-class RelativePositionNode(Node):
-    def __init__(self, robot_1_init):
-        super().__init__('robot_2_relative_position')
-        self.robot_1_init = robot_1_init  # (x, y, z)
-        self.robot_1_pos = None
-        self.robot_2_pos = None
-        self.rel_pub = self.create_publisher(Odometry, '/robot_2/relative_position', 10)
-        self.create_subscription(
-            Odometry,
-            '/robot_1/odometry_conversion/odometry',
-            self.robot_1_callback,
-            10
-        )
-        self.create_subscription(
-            Odometry,
-            '/robot_2/odometry_conversion/odometry',
-            self.robot_2_callback,
-            10
-        )
-
-    def robot_1_callback(self, msg):
-        self.robot_1_pos = (
-            msg.pose.pose.position.x,
-            msg.pose.pose.position.y,
-            msg.pose.pose.position.z
-        )
-
-    def robot_2_callback(self, msg):
-        self.robot_2_pos = (
-            msg.pose.pose.position.x,
-            msg.pose.pose.position.y,
-            msg.pose.pose.position.z
-        )
-        self.publish_relative(msg)
-
-    def publish_relative(self, msg):
-        if self.robot_1_pos is None or self.robot_2_pos is None:
-            return
-        rel = [self.robot_2_pos[i] - self.robot_1_pos[i] for i in range(3)]
-        rel_msg = Odometry()
-        rel_msg.header = msg.header
-        rel_msg.pose.pose.position.x = rel[0]
-        rel_msg.pose.pose.position.y = rel[1]
-        rel_msg.pose.pose.position.z = rel[2]
-        self.rel_pub.publish(rel_msg)
-
-def main(args=None):
-    rclpy.init(args=args)
-    robot_1_init = [20.0, -7, 0.0015]
-    node = RelativePositionNode(robot_1_init)
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()
diff --git a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
index d5b823d7..ba3bba4f 100644
--- a/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
+++ b/gcs/ros_ws/src/gcs_visualizer/launch/gcs_visualizer.launch.xml
@@ -4,6 +4,8 @@
   <!-- Standard visualization: mesh, labels, axes, trajectory, global plan, VDB -->
   <node pkg="gcs_visualizer" exec="foxglove_visualizer_node" name="foxglove_visualizer_node" output="screen">
     <param name="robot_name_prefix" value="$(var robot_name_prefix)" />
+    <param name="overhead_grid_per_m" value="3.0" />
+    <param name="overhead_max_grid_resolution" value="1024" />
   </node>
 
   <!-- Payload visualization: project-specific data from PeerProfile gossip payloads -->
diff --git a/mkdocs.yml b/mkdocs.yml
index dbddf243..46cee943 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -82,6 +82,8 @@ nav:
           - docs/simulation/isaac_sim/index.md
           - Docker: docs/simulation/isaac_sim/docker.md
           - docs/simulation/isaac_sim/pegasus_scene_setup.md
+          - Spawning Drones: docs/simulation/isaac_sim/spawning_drones.md
+          - Overhead Camera: docs/simulation/isaac_sim/overhead_camera.md
           - docs/simulation/isaac_sim/ascent_sitl_extension.md
           - docs/simulation/isaac_sim/export_stages_from_unreal.md
       - Microsoft AirSim (legacy):
@@ -160,6 +162,9 @@ nav:
           - docs/gcs/docker/index.md
       - Usage:
           - User Interface: docs/gcs/usage/user_interface.md
+      - Foxglove:
+          - Visualization: docs/gcs/foxglove.md
+          - Adding Waypoints & Geofences: docs/gcs/waypoints_and_geofences.md
       - Command Center:
           - docs/gcs/command_center/command_center.md
       - Casualty Assessment:
diff --git a/robot/docker/.bashrc b/robot/docker/.bashrc
index e64f0910..20c01270 100755
--- a/robot/docker/.bashrc
+++ b/robot/docker/.bashrc
@@ -10,55 +10,6 @@ ROS2_WS_DIR="$HOME/AirStack/robot/ros_ws"
 # needed for communication with Isaac Sim ROS2  # https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#enabling-the-ros-bridge-extension
 export FASTRTPS_DEFAULT_PROFILES_FILE="$ROS2_WS_DIR/src/fastdds.xml"
 
-# HITL DDS mode:
-# - server (default): use Fast DDS Discovery Server via ROS_DISCOVERY_SERVER.
-# - static-peer: generate a temporary Fast DDS profile with one initial peer.
-if [ "${HITL_DISCOVERY_MODE:-}" = "static-peer" ]; then
-    if [ -n "${FASTDDS_STATIC_PEER_IP:-}" ]; then
-        cat > /tmp/fastdds_static_peer.xml <<EOF
-<?xml version="1.0" encoding="UTF-8" ?>
-<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
-    <transport_descriptors>
-        <transport_descriptor>
-            <transport_id>UdpTransport</transport_id>
-            <type>UDPv4</type>
-        </transport_descriptor>
-    </transport_descriptors>
-    <participant profile_name="udp_transport_profile" is_default_profile="true">
-        <rtps>
-            <userTransports>
-                <transport_id>UdpTransport</transport_id>
-            </userTransports>
-            <useBuiltinTransports>false</useBuiltinTransports>
-            <builtin>
-                <initialPeersList>
-                    <locator>
-                        <udpv4>
-                            <address>${FASTDDS_STATIC_PEER_IP}</address>
-                        </udpv4>
-                    </locator>
-                </initialPeersList>
-            </builtin>
-        </rtps>
-    </participant>
-</profiles>
-EOF
-        export FASTRTPS_DEFAULT_PROFILES_FILE="/tmp/fastdds_static_peer.xml"
-    fi
-    unset ROS_DISCOVERY_SERVER
-elif [ -n "${DISCOVERY_SERVER_IP:-}" ]; then
-    discovery_server_list="${DISCOVERY_SERVER_IP}:${DISCOVERY_SERVER_PORT:-11811}"
-    if [ -n "${DISCOVERY_SERVER_BACKUP_IPS:-}" ]; then
-        IFS=',' read -r -a discovery_backup_ips <<< "${DISCOVERY_SERVER_BACKUP_IPS}"
-        for backup_ip in "${discovery_backup_ips[@]}"; do
-            if [ -n "${backup_ip}" ]; then
-                discovery_server_list="${discovery_server_list};${backup_ip}:${DISCOVERY_SERVER_PORT:-11811}"
-            fi
-        done
-    fi
-    export ROS_DISCOVERY_SERVER="${discovery_server_list}"
-fi
-
 # fix ROS2 jazzy setuptools deprecation warning https://robotics.stackexchange.com/questions/24230/setuptoolsdeprecationwarning-in-ros2-humble/24349#24349
 PYTHONWARNINGS="ignore:easy_install command is deprecated,ignore:setup.py install is deprecated"
 export PYTHONWARNINGS
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index b7c28afa..7b0e4a28 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -1,7 +1,5 @@
 # either ubuntu:24.04 or l4t. ubuntu:24.04 is default
 ARG BASE_IMAGE
-ARG ENABLE_RAYFRONTS=false
-ARG FINAL_STAGE=runtime
 # ============================================================
 # Stage 1 — builder: compile/download everything
 # ============================================================
@@ -14,7 +12,6 @@ ARG UPDATE_FLAGS="-o Acquire::AllowInsecureRepositories=true -o Acquire::AllowDo
 ARG INSTALL_FLAGS="-o APT::Get::AllowUnauthenticated=true"
 ARG SKIP_MACVO=false
 ARG SKIP_TENSORRT=false
-ARG ENABLE_RAYFRONTS
 
 # from https://github.com/athackst/dockerfiles/blob/main/ros2/jazzy.Dockerfile
 ENV DEBIAN_FRONTEND=noninteractive
@@ -137,7 +134,6 @@ RUN pip3 install --break-system-packages --ignore-installed \
   six \
   toml \
   scipy \
-  scikit-learn \
   pypose \
   rich \
   tqdm \
@@ -158,15 +154,8 @@ RUN pip3 install --break-system-packages --ignore-installed \
 
 # Install MACVO Python dependencies (skipped if SKIP_MACVO=true)
 RUN if [ "${SKIP_MACVO}" != "true" ]; then \
-    if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
-      pip3 install --break-system-packages \
-        --index-url https://download.pytorch.org/whl/cu130 \
-        torch==2.9.1 torchvision torchaudio && \
-      pip3 install --break-system-packages onnx tensorrt; \
-    else \
-      pip3 install --break-system-packages \
-        torch torchvision onnx tensorrt; \
-    fi; \
+    pip3 install --break-system-packages \
+      torch torchvision onnx tensorrt; \
   fi
 
 # Downloading model weights for MACVO (skipped if SKIP_MACVO=true)
@@ -202,42 +191,6 @@ RUN mkdir -p /tmp/DDS-Router/src \
   && colcon build --merge-install --install-base /usr/local \
   && rm -rf /tmp/DDS-Router
 
-# RayFronts deps (builder-stage, gated on ENABLE_RAYFRONTS=true)
-RUN if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
-    pip3 install --break-system-packages \
-      hydra-core open_clip_torch "transformers<5" \
-      git+https://github.com/facebookresearch/segment-anything.git \
-      ftfy regex nanobind pandas protobuf \
-      "scipy==1.15.2" "scikit-image" "numpy<2" && \
-    pip3 install --break-system-packages \
-      torch-scatter==2.1.2 && \
-    pip3 install --break-system-packages --force-reinstall --no-deps \
-      setuptools==79.0.1; \
-  fi
-
-# Re-pin empy to 3.x. The RayFronts block above transitively upgrades it via
-# transformers/bloom/colcon-core to 4.x, which is API-incompatible with
-# rosidl_generator_rs and breaks the workspace build with
-# "TransientParseError: not enough data to read" on rmw.rs.em.
-RUN pip3 install --break-system-packages --force-reinstall empy==3.3.4
-
-# Patched OpenVDB (OasisArtisan fork) — exposes Int8Grid to Python bindings
-RUN if [ "${ENABLE_RAYFRONTS}" = "true" ]; then \
-    apt-get ${UPDATE_FLAGS} update && apt-get ${INSTALL_FLAGS} install -y --no-install-recommends \
-      libboost-iostreams-dev libtbb-dev libblosc-dev python3-dev && \
-    git clone --depth 1 https://github.com/OasisArtisan/openvdb /tmp/openvdb && \
-    cmake -S /tmp/openvdb -B /tmp/openvdb/build \
-      -DCMAKE_INSTALL_PREFIX=/usr/local \
-      -DOPENVDB_BUILD_PYTHON_MODULE=ON \
-      -DOPENVDB_PYTHON_WRAP_ALL_GRID_TYPES=ON \
-      -DUSE_NUMPY=ON \
-      -Dnanobind_DIR=$(python3 -c "import nanobind,os;print(os.path.join(os.path.dirname(nanobind.__file__),'cmake'))") && \
-    cmake --build /tmp/openvdb/build -j4 && \
-    cmake --install /tmp/openvdb/build && \
-    rm -rf /tmp/openvdb && \
-    rm -rf /var/lib/apt/lists/*; \
-  fi
-
 # Cleanup
 RUN apt autoremove -y \
   && apt clean -y \
@@ -399,15 +352,3 @@ RUN echo 'root:airstack' | chpasswd \
 EXPOSE 22
 
 WORKDIR /root/AirStack/robot/ros_ws
-
-
-# runtime + compiled RayFronts (only reached when FINAL_STAGE=runtime-rayfronts)
-FROM runtime AS runtime-rayfronts
-COPY ./common/rayfronts          /opt/rayfronts
-COPY ./common/rayfronts_configs/ /opt/rayfronts/rayfronts/configs/
-RUN apt-get update && apt-get install -y --no-install-recommends \
-      cmake build-essential python3-dev \
- && cd /opt/rayfronts && CMAKE_INSTALL_PREFIX=/usr/local ./compile.sh \
- && rm -rf /var/lib/apt/lists/*
-
-FROM ${FINAL_STAGE} AS final
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index b2f5c444..888fa2ef 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -11,13 +11,9 @@ services:
       service: robot_base
     image: &desktop_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${VERSION}_robot-x86-64_${DOCKER_IMAGE_BUILD_MODE}
     build:
-      # context at repo root so runtime-rayfronts stage can COPY ./common/rayfronts
-      context: ../../
-      dockerfile: robot/docker/Dockerfile.robot
+      dockerfile: ./Dockerfile.robot
       args:
         BASE_IMAGE: nvidia/cuda:13.0.2-base-ubuntu24.04
-        ENABLE_RAYFRONTS: "true"
-        FINAL_STAGE: runtime-rayfronts
       tags:
         - *desktop_image
       cache_from:
@@ -57,26 +53,6 @@ services:
               count: 1
               capabilities: [gpu]
 
-  # ===================================================================================================================
-  # hitl desktop: run full robot stack on a desktop host network for multi-machine HITL.
-  robot-desktop-hitl:
-    profiles: !override
-      - hitl
-    extends:
-      file: ./docker-compose.yaml
-      service: robot-desktop
-    network_mode: host
-    networks: !reset null
-    ports: !reset {}
-    deploy: !reset {}
-    environment:
-      - AUTONOMY_ROLE=full
-      - ROBOT_NAME=${ROBOT_NAME:-robot_1}
-      - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1}
-      # HITL uses host networking; default bridge IP (172.31.0.200) is not reachable here.
-      # Point SIM_IP at the host running Isaac Sim (override in .env as needed).
-      - SIM_IP=${SIM_IP:-192.168.233.25}
-
   # ===================================================================================================================
   # desktop_split: simulates onboard computer on desktop for debugging the split configuration.
   # Same image as robot-desktop; role=onboard means only lite autonomy modules launch.
diff --git a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
index cd50c1c5..c0e7dcf4 100644
--- a/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/global/global_bringup/launch/global.launch.xml
@@ -6,8 +6,6 @@
     </include>
 
     <!-- Planning -->
-    <include file="$(find-pkg-share semantic_search_task)/launch/semantic_search_task.launch.xml" />
-
     <!-- The C++ constructor names the node "random_walk_node" (don't set
          launch name= or it will rename and break the remap source paths).
          The parent launch pushes a $(env ROBOT_NAME) namespace, so we use
diff --git a/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml b/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
deleted file mode 100644
index a2cb2a1d..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/config/raven_nav.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-raven_nav:
-  ros__parameters:
-    timer_period: 0.5       # seconds between behavior loop iterations
-    score_threshold: 0.95   # minimum softmax sim score to consider a ray
-    min_altitude_agl: 3.0   # filter rays/frontiers below this altitude (m)
-    max_altitude_agl: 15.0  # filter rays/frontiers above this altitude (m)
-    # Must match the query order registered in RayFronts (target + background)
-    query_labels:
-      - 'red building'
-      - 'water tower'
-      - 'radio tower'
-    # Which of the above to actually navigate toward (empty = all query_labels)
-    target_labels: []
diff --git a/robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml b/robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml
deleted file mode 100644
index 81b3152f..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/launch/raven_nav.launch.xml
+++ /dev/null
@@ -1,8 +0,0 @@
-<launch>
-    <node pkg="raven_nav" exec="raven_nav_node" name="raven_nav" output="screen">
-        <param from="$(find-pkg-share raven_nav)/config/raven_nav.yaml"
-               allow_substs="true" />
-        <remap from="/$(env ROBOT_NAME)/odometry"
-               to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
-    </node>
-</launch>
diff --git a/robot/ros_ws/src/global/planners/raven_nav/package.xml b/robot/ros_ws/src/global/planners/raven_nav/package.xml
deleted file mode 100644
index be054d48..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/package.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>raven_nav</name>
-  <version>0.0.0</version>
-  <description>Navigation planner that consumes RayFronts mapper outputs to produce waypoints</description>
-  <maintainer email="todo@todo.todo">todo</maintainer>
-  <license>TODO</license>
-
-  <depend>rclpy</depend>
-  <depend>python3-sklearn</depend>
-  <depend>nav_msgs</depend>
-  <depend>sensor_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>visualization_msgs</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/__init__.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
deleted file mode 100644
index 8a3ae254..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behavior_manager.py
+++ /dev/null
@@ -1,59 +0,0 @@
-from raven_nav.behaviors.frontier_behavior import FrontierBehavior
-from raven_nav.behaviors.ray_behavior import RayBehavior
-from raven_nav.behaviors.voxel_behavior import VoxelBehavior
-
-
-class BehaviorManager:
-    def __init__(self, get_clock, publisher_dict, score_threshold=0.95,
-                 min_altitude=1.5, max_altitude=100.0):
-        self.behavior_mode = 'Frontier-based'
-        self.get_clock = get_clock
-        self.frontier_behavior = FrontierBehavior(self.get_clock,
-                                                   min_altitude=min_altitude,
-                                                   max_altitude=max_altitude)
-        current_target_pub = publisher_dict.get('current_target')
-        self.ray_behavior = RayBehavior(self.get_clock, current_target_pub,
-                                        score_threshold=score_threshold,
-                                        min_altitude=min_altitude,
-                                        max_altitude=max_altitude)
-        self.voxel_behavior = VoxelBehavior(self.get_clock)
-        # Priority: Voxel (navigate to confirmed detections) > Ray > Frontier
-        self.behaviors = [self.voxel_behavior, self.ray_behavior, self.frontier_behavior]
-
-    @property
-    def completed_queries(self) -> set:
-        return self.voxel_behavior.completed_queries
-
-    def mode_select(self, ray_origins, ray_dirs, ray_scores,
-                    query_labels, target_objects,
-                    vox_xyz=None, vox_scores=None):
-        for behavior in self.behaviors:
-            if behavior.name == 'Voxel-based':
-                # Voxel-based navigation disabled — skip
-                continue
-            elif behavior.name == 'Ray-based':
-                if behavior.condition_check(ray_origins, ray_dirs, ray_scores,
-                                            query_labels, target_objects):
-                    self.behavior_mode = behavior.name
-                    return
-            else:
-                if behavior.condition_check():
-                    self.behavior_mode = behavior.name
-                    return
-
-    def behavior_execute(self, behavior_mode, frontiers, cur_pose_np,
-                         waypoint_locked, target_waypoint, target_waypoint2,
-                         publisher_dict,
-                         vox_xyz=None, vox_scores=None, query_labels=None):
-        if behavior_mode == 'Voxel-based':
-            return self.voxel_behavior.execute(
-                vox_xyz, vox_scores, query_labels, cur_pose_np,
-                waypoint_locked, target_waypoint, target_waypoint2, publisher_dict)
-        elif behavior_mode == 'Ray-based':
-            return self.ray_behavior.execute(
-                cur_pose_np, waypoint_locked,
-                target_waypoint, target_waypoint2, publisher_dict)
-        else:
-            return self.frontier_behavior.execute(
-                frontiers, cur_pose_np, waypoint_locked,
-                target_waypoint, target_waypoint2, publisher_dict)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/__init__.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
deleted file mode 100644
index 4c3dd5a5..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/frontier_behavior.py
+++ /dev/null
@@ -1,136 +0,0 @@
-import torch
-import numpy as np
-from sklearn.cluster import DBSCAN
-from std_msgs.msg import Header
-from sensor_msgs.msg import PointField
-from sensor_msgs_py import point_cloud2
-from nav_msgs.msg import Path
-from geometry_msgs.msg import PoseStamped
-
-
-class FrontierBehavior:
-    def __init__(self, get_clock, min_altitude=1.5, max_altitude=100.0):
-        self.get_clock = get_clock
-        self.name = 'Frontier-based'
-        self.min_altitude = min_altitude
-        self.max_altitude = max_altitude
-
-    def condition_check(self):
-        return True
-
-    def execute(self, frontiers_raw, cur_pose_np, waypoint_locked, target_waypoint,
-                target_waypoint2, publisher_dict):
-        viewpoint_publisher = publisher_dict['viewpoint']
-        path_publisher = publisher_dict['path']
-
-        if frontiers_raw is None or len(frontiers_raw) == 0:
-            return waypoint_locked, target_waypoint, target_waypoint2
-
-        # msg_serv/frontiers are published in RDF frame (no transform applied by RayFronts)
-        # Convert RDF → FLU: world_x=rdf_z, world_y=-rdf_x, world_z=-rdf_y
-        frontiers = np.stack([
-            frontiers_raw[:, 2],
-            -frontiers_raw[:, 0],
-            -frontiers_raw[:, 1],
-        ], axis=1)
-
-        alt_mask = (frontiers[:, 2] >= self.min_altitude) & (frontiers[:, 2] <= self.max_altitude)
-        frontiers = frontiers[alt_mask]
-        if frontiers.shape[0] == 0:
-            return waypoint_locked, target_waypoint, target_waypoint2
-
-        clustering = DBSCAN(eps=2.7, min_samples=3).fit(frontiers)
-        labels = clustering.labels_
-        unique_labels = [l for l in set(labels) if l != -1]
-        viewpoints = []
-        for l in unique_labels:
-            cluster_pts = frontiers[labels == l]
-            centroid = cluster_pts.mean(axis=0)
-            if self.min_altitude <= centroid[2] <= self.max_altitude:
-                viewpoints.append(centroid)
-
-        if len(viewpoints) == 0:
-            return waypoint_locked, target_waypoint, target_waypoint2
-
-        viewpoints = np.stack(viewpoints)
-        cent_msg = self._create_pointcloud2_msg(viewpoints)
-        viewpoint_publisher.publish(cent_msg)
-
-        robot_pos = cur_pose_np
-        distances = np.linalg.norm(viewpoints - robot_pos, axis=1)
-
-        if target_waypoint is not None:
-            cur_motion_vec = target_waypoint - robot_pos
-            cur_motion_vec = cur_motion_vec / (np.linalg.norm(cur_motion_vec) + 1e-6)
-            candidate_vecs = viewpoints - robot_pos
-            norms = np.linalg.norm(candidate_vecs, axis=1, keepdims=True)
-            candidate_vecs = candidate_vecs / (norms + 1e-6)
-            cos_sim = candidate_vecs @ cur_motion_vec
-            momentum_weight = 5.0
-            scores = distances + momentum_weight * (1.0 - cos_sim)
-        else:
-            scores = distances
-
-        top_n = 5
-        num_candidates = min(top_n, viewpoints.shape[0])
-        if num_candidates == 0:
-            return waypoint_locked, target_waypoint, target_waypoint2
-
-        top_indices = np.argsort(scores)[:num_candidates]
-        best_idx = top_indices[np.random.randint(0, num_candidates)]
-        best_cent = viewpoints[best_idx]
-
-        path = Path()
-        path.header.stamp = self.get_clock().now().to_msg()
-        path.header.frame_id = 'map'
-
-        if not waypoint_locked:
-            target_waypoint = best_cent
-            direction = target_waypoint - cur_pose_np
-            dir_norm = np.linalg.norm(direction)
-            if dir_norm > 1e-6:
-                direction = direction / dir_norm
-                target_waypoint2 = target_waypoint + 2.0 * direction
-            else:
-                target_waypoint2 = target_waypoint.copy()
-            waypoint_locked = True
-
-        target_pose = PoseStamped()
-        target_pose.header.stamp = self.get_clock().now().to_msg()
-        target_pose.header.frame_id = 'map'
-        target_pose.pose.position.x = float(target_waypoint[0])
-        target_pose.pose.position.y = float(target_waypoint[1])
-        target_pose.pose.position.z = float(target_waypoint[2])
-        target_pose.pose.orientation.w = 1.0
-        path.poses.append(target_pose)
-
-        target_pose2 = PoseStamped()
-        target_pose2.header.stamp = self.get_clock().now().to_msg()
-        target_pose2.header.frame_id = 'map'
-        target_pose2.pose.position.x = float(target_waypoint2[0])
-        target_pose2.pose.position.y = float(target_waypoint2[1])
-        target_pose2.pose.position.z = float(target_waypoint2[2])
-        target_pose2.pose.orientation.w = 1.0
-        path.poses.append(target_pose2)
-
-        path_publisher.publish(path)
-
-        if np.linalg.norm(cur_pose_np - target_waypoint) < 5.0:
-            waypoint_locked = False
-
-        return waypoint_locked, target_waypoint, target_waypoint2
-
-    def _create_pointcloud2_msg(self, xyz):
-        if isinstance(xyz, np.ndarray):
-            pass
-        else:
-            xyz = np.array(xyz, dtype=np.float32)
-        header = Header()
-        header.stamp = self.get_clock().now().to_msg()
-        header.frame_id = 'map'
-        fields = [PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
-                  PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
-                  PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1)]
-        points = [[float(xyz[i, 0]), float(xyz[i, 1]), float(xyz[i, 2])]
-                  for i in range(xyz.shape[0])]
-        return point_cloud2.create_cloud(header, fields, points)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
deleted file mode 100644
index c0f5f026..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/ray_behavior.py
+++ /dev/null
@@ -1,241 +0,0 @@
-import numpy as np
-from nav_msgs.msg import Path
-from geometry_msgs.msg import PoseStamped, Point
-from visualization_msgs.msg import Marker, MarkerArray
-from std_msgs.msg import String
-
-
-import logging
-_log = logging.getLogger(__name__)
-
-
-class RayBehavior:
-    def __init__(self, get_clock, current_target_publisher=None, score_threshold=0.95,
-                 min_altitude=1.5, max_altitude=100.0):
-        self.get_clock = get_clock
-        self.name = 'Ray-based'
-        self.score_threshold = score_threshold
-        self.min_altitude = min_altitude
-        self.max_altitude = max_altitude
-        self.prev_filtered_marker_ids = 0
-        self.current_target = None
-        self.current_target_pub = current_target_publisher
-
-        # Set by condition_check, consumed by execute
-        self._filtered_indices = None   # indices into ray arrays that pass threshold
-        self._ray_origins = None        # (N, 3) FLU — full ray set
-        self._ray_dirs = None           # (N, 3) FLU
-        self._per_ray_label = None      # list[str] — best label per filtered ray
-
-        # Active target: the target the drone is committed to pursuing.
-        # Set on first acquisition; stays fixed until explicitly reset.
-        self._active_target = None
-
-    def condition_check(self, ray_origins, ray_dirs, ray_scores,
-                        query_labels, target_objects):
-        """Returns True if any ray scores above threshold for a target object."""
-        self.current_target = None
-        self._filtered_indices = None
-
-        if (ray_origins is None or ray_scores is None or
-                len(ray_origins) == 0 or len(target_objects) == 0):
-            return False
-
-        # Find column indices in ray_scores for each target object
-        label_indices = [query_labels.index(t) for t in target_objects
-                         if t in query_labels]
-        if not label_indices:
-            return False
-
-        relevant_scores = ray_scores[:, label_indices]          # (N, num_targets)
-
-        mask = (relevant_scores > self.score_threshold).any(axis=1)
-        indices = np.where(mask)[0]
-
-        if indices.size == 0:
-            return False
-
-        # Assign each passing ray its best-scoring target label
-        per_ray_label = []
-        targets_found = []
-        for idx in indices:
-            scores_i = relevant_scores[idx]
-            best_col = int(np.argmax(scores_i))
-            label = target_objects[best_col]
-            per_ray_label.append(label)
-            targets_found.append(label)
-
-        from collections import Counter
-        self.current_target = Counter(targets_found).most_common(1)[0][0]
-        if self.current_target_pub is not None:
-            self.current_target_pub.publish(String(data=self.current_target))
-
-        self._filtered_indices = indices
-        self._ray_origins = ray_origins
-        self._ray_dirs = ray_dirs
-        self._per_ray_label = per_ray_label
-        return True
-
-    def execute(self, cur_pose_np, waypoint_locked, target_waypoint1,
-                target_waypoint2, publisher_dict):
-        path_publisher = publisher_dict['path']
-
-        origins = self._ray_origins[self._filtered_indices]   # (K, 3) FLU
-        dirs = self._ray_dirs[self._filtered_indices]          # (K, 3) FLU
-        labels = self._per_ray_label
-
-        # Filter rays outside altitude range (z is altitude in FLU)
-        alt_mask = (origins[:, 2] >= self.min_altitude) & (origins[:, 2] <= self.max_altitude)
-        origins = origins[alt_mask]
-        dirs = dirs[alt_mask]
-        labels = [labels[i] for i in range(len(labels)) if alt_mask[i]]
-
-        if len(origins) == 0:
-            return waypoint_locked, target_waypoint1, target_waypoint2
-
-        xy_dirs = dirs[:, :2]
-        norms = np.linalg.norm(xy_dirs, axis=1, keepdims=True)
-        xy_dirs_normed = xy_dirs / (norms + 1e-6)
-
-        # Filter rays pointing behind the robot in XY.
-        # Apply valid mask to all arrays so indices stay consistent (krrish-develop).
-        orig_xy = origins[:, :2]
-        ray_target_xy = orig_xy + xy_dirs_normed
-        to_ray_target = ray_target_xy - cur_pose_np[:2]
-        dot = np.einsum('ij,ij->i', xy_dirs_normed, to_ray_target)
-        valid = dot > 0
-
-        xy_dirs_normed = xy_dirs_normed[valid]
-        origins = origins[valid]
-        dirs = dirs[valid]
-        labels = [labels[i] for i in range(len(labels)) if valid[i]]
-
-        if len(origins) == 0:
-            return waypoint_locked, target_waypoint1, target_waypoint2
-
-        # Greedy spatial clustering per label (45 deg XY threshold)
-        angle_cos_thresh = np.cos(np.deg2rad(45))
-        groups = []
-        for i in range(len(origins)):
-            xy_dir = xy_dirs_normed[i]
-            label = labels[i]
-            assigned = False
-            for group in groups:
-                if group['label'] != label:
-                    continue
-                if np.dot(xy_dir, group['centroid']) >= angle_cos_thresh:
-                    group['indices'].append(i)
-                    group['rays'].append(xy_dir)
-                    c = np.mean(group['rays'], axis=0)
-                    group['centroid'] = c / (np.linalg.norm(c) + 1e-6)
-                    assigned = True
-                    break
-            if not assigned:
-                groups.append({
-                    'centroid': xy_dir.copy(),
-                    'rays': [xy_dir],
-                    'indices': [i],
-                    'label': label,
-                })
-
-        MIN_RAYS_PER_GROUP = 1
-        groups = [g for g in groups if len(g['rays']) >= MIN_RAYS_PER_GROUP]
-
-        if not groups:
-            return waypoint_locked, target_waypoint1, target_waypoint2
-
-        # Score groups: lower is better (dist - 5 * density)
-        k = 5.0
-        group_avgs = []
-        for group in groups:
-            idxs = group['indices']
-            avg_origin = origins[idxs].mean(axis=0)
-            avg_dir = dirs[idxs].mean(axis=0)
-            avg_dir = avg_dir / (np.linalg.norm(avg_dir) + 1e-6)
-            density = len(idxs)
-            score = np.linalg.norm(avg_origin - cur_pose_np) - k * density
-            group_avgs.append((avg_origin, avg_dir, density, group['label'], score))
-
-        group_avgs.sort(key=lambda g: g[4])
-        best = group_avgs[0]
-        best_origin, best_dir = best[0], best[1]
-        direction = best_dir / (np.linalg.norm(best_dir) + 1e-6)
-
-        target_waypoint1 = best_origin + direction * 6.0
-        target_waypoint2 = best_origin + direction * 12.0
-
-        path = Path()
-        path.header.stamp = self.get_clock().now().to_msg()
-        path.header.frame_id = 'map'
-        for wp in (target_waypoint1, target_waypoint2):
-            ps = PoseStamped()
-            ps.header.stamp = self.get_clock().now().to_msg()
-            ps.header.frame_id = 'map'
-            ps.pose.position.x = float(wp[0])
-            ps.pose.position.y = float(wp[1])
-            ps.pose.position.z = float(wp[2])
-            ps.pose.orientation.w = 1.0
-            path.poses.append(ps)
-        path_publisher.publish(path)
-
-        published_target = best[3]
-        if self._active_target is None:
-            self._active_target = published_target
-
-        self._visualize_filtered_rays(groups, origins, dirs, publisher_dict)
-
-        return waypoint_locked, target_waypoint1, target_waypoint2
-
-    def _visualize_filtered_rays(self, groups, origins, dirs, publisher_dict):
-        pub = publisher_dict['filtered_rays']
-        self._clear_filtered_rays(pub)
-        colors = [
-            (1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0),
-            (1.0, 1.0, 0.0), (0.0, 1.0, 1.0), (1.0, 0.0, 1.0),
-            (0.5, 0.5, 0.5), (1.0, 0.5, 0.0), (0.5, 0.0, 1.0),
-            (0.0, 0.5, 0.5),
-        ]
-        arrow_length = 2.0
-        marker_array = MarkerArray()
-        j = 0
-        for i, group in enumerate(groups):
-            rr, gg, bb = colors[i % len(colors)]
-            for idx in group['indices']:
-                p0 = origins[idx]
-                d = dirs[idx]
-                p1 = p0 + arrow_length * (d / (np.linalg.norm(d) + 1e-6))
-                arrow = Marker()
-                arrow.header.frame_id = 'map'
-                arrow.header.stamp = self.get_clock().now().to_msg()
-                arrow.ns = 'arrows'
-                arrow.id = j
-                arrow.type = Marker.ARROW
-                arrow.action = Marker.ADD
-                arrow.points = [
-                    Point(x=float(p0[0]), y=float(p0[1]), z=float(p0[2])),
-                    Point(x=float(p1[0]), y=float(p1[1]), z=float(p1[2])),
-                ]
-                arrow.scale.x = 0.6
-                arrow.scale.y = 1.2
-                arrow.scale.z = 0.75
-                arrow.color.r = rr
-                arrow.color.g = gg
-                arrow.color.b = bb
-                arrow.color.a = 0.5
-                marker_array.markers.append(arrow)
-                j += 1
-        self.prev_filtered_marker_ids = j
-        pub.publish(marker_array)
-
-    def _clear_filtered_rays(self, pub):
-        if self.prev_filtered_marker_ids > 0:
-            clear_array = MarkerArray()
-            for i in range(self.prev_filtered_marker_ids):
-                m = Marker()
-                m.header.frame_id = 'map'
-                m.header.stamp = self.get_clock().now().to_msg()
-                m.ns = 'arrows'
-                m.id = i
-                m.action = Marker.DELETE
-                clear_array.markers.append(m)
-            pub.publish(clear_array)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
deleted file mode 100644
index ec21ab00..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/behaviors/voxel_behavior.py
+++ /dev/null
@@ -1,239 +0,0 @@
-import time
-
-import numpy as np
-import scipy.ndimage
-from nav_msgs.msg import Path
-from geometry_msgs.msg import PoseStamped
-from visualization_msgs.msg import Marker, MarkerArray
-from std_msgs.msg import ColorRGBA
-
-
-class VoxelBehavior:
-    def __init__(self, get_clock):
-        self.get_clock = get_clock
-        self.name = 'Voxel-based'
-        # {cluster_id: [cx, cy, cz, sx, sy, sz]}
-        self.target_voxel_clusters = {}
-        # {cluster_id: query_label} — which query each cluster belongs to
-        self.cluster_query_map = {}
-        self.visited_clusters = []    # list of [cx,cy,cz,sx,sy,sz]
-        self.unvisited_clusters = []  # list of (cluster_id, [cx,cy,cz,sx,sy,sz])
-        self.completed_queries = set()
-        self.prev_voxel_cluster_ids = 0
-
-    def condition_check(self, vox_xyz, vox_scores, query_labels, target_objects):
-        """
-        vox_xyz:      np.ndarray (N, 3) — voxel world positions
-        vox_scores:   np.ndarray (N, Q) — softmax similarity per query
-        query_labels: list[str]         — ordered label for each sim column
-        target_objects: list[str]       — which labels we're searching for
-        """
-        if vox_xyz is None or vox_scores is None or not target_objects:
-            return False
-        if len(vox_xyz) == 0:
-            return False
-
-        label_indices = [
-            query_labels.index(t) for t in target_objects
-            if t in query_labels
-        ]
-        if not label_indices:
-            return False
-
-        threshold = 0.98
-        relevant_scores = vox_scores[:, label_indices]   # (N, len(label_indices))
-        mask = (relevant_scores > threshold).any(axis=1)
-        indices = np.where(mask)[0]
-
-        if len(indices) == 0:
-            return False
-
-        # Cluster the high-confidence voxels with connected-component labeling
-        filtered_vox = np.round(vox_xyz[indices], 3)
-        vox_size = 0.5
-
-        min_coords = filtered_vox.min(axis=0)
-        norm_coords = ((filtered_vox - min_coords) / vox_size).astype(int)
-        max_coords = norm_coords.max(axis=0) + 1
-        occupancy = np.zeros(tuple(max_coords.tolist()), dtype=np.uint8)
-        for x, y, z in norm_coords:
-            occupancy[x, y, z] = 1
-
-        structure = np.ones((3, 3, 3), dtype=np.uint8)
-        labeled, num_components = scipy.ndimage.label(occupancy, structure=structure)
-
-        label_ids = np.array([labeled[x, y, z] for x, y, z in norm_coords])
-
-        self.target_voxel_clusters.clear()
-        self.cluster_query_map.clear()
-        vox_cluster_count = 0
-
-        for label_val in range(1, num_components + 1):
-            idx = np.where(label_ids == label_val)[0]
-            if len(idx) < 30:
-                continue
-
-            coords = norm_coords[idx]
-            min_voxel = coords.min(axis=0)
-            max_voxel = coords.max(axis=0)
-            min_world = min_voxel * vox_size + min_coords
-            max_world = (max_voxel + 1) * vox_size + min_coords
-            center = (min_world + max_world) / 2
-            size = max_world - min_world
-
-            # Data is already in FLU (raven_nav converts before passing)
-            cx, cy, cz = center[0], center[1], center[2]
-            sx, sy, sz = size[0], size[1], size[2]
-
-            # Tag cluster with best-scoring query label for this cluster
-            cluster_scores = relevant_scores[idx]  # (cluster_size, len(label_indices))
-            best_local = int(cluster_scores.mean(axis=0).argmax())
-            best_label = target_objects[best_local] if best_local < len(target_objects) else target_objects[0]
-
-            self.target_voxel_clusters[vox_cluster_count] = [cx, cy, cz, sx, sy, sz]
-            self.cluster_query_map[vox_cluster_count] = best_label
-            vox_cluster_count += 1
-
-        self.unvisited_clusters = [
-            (idx, cluster) for idx, cluster in self.target_voxel_clusters.items()
-            if not self._is_near_visited(
-                np.array(cluster[:3]), np.array(cluster[3:6]), self.visited_clusters)
-        ]
-
-        return len(self.unvisited_clusters) > 0
-
-    def execute(self, vox_xyz, vox_scores, query_labels, cur_pose_np,
-                waypoint_locked, target_waypoint, target_waypoint2, publisher_dict):
-        voxel_bbox_pub = publisher_dict.get('voxel_bbox')
-        if voxel_bbox_pub:
-            self._visualize_clusters(voxel_bbox_pub)
-
-        path_pub = publisher_dict['path']
-
-        self.unvisited_clusters = [
-            (idx, cluster) for idx, cluster in self.target_voxel_clusters.items()
-            if not self._is_near_visited(
-                np.array(cluster[:3]), np.array(cluster[3:6]), self.visited_clusters)
-        ]
-
-        sorted_clusters = sorted(
-            self.unvisited_clusters,
-            key=lambda item: np.linalg.norm(cur_pose_np - np.array(item[1][:3])))
-
-        path = Path()
-        path.header.stamp = self.get_clock().now().to_msg()
-        path.header.frame_id = 'map'
-
-        # Track whether the waypoint was already locked before this tick.
-        # Arrival detection must NOT fire in the same tick the waypoint is first set.
-        waypoint_was_locked = waypoint_locked
-
-        for i, (idx, cluster) in enumerate(sorted_clusters):
-            center = np.array(cluster[:3])
-            sizes = np.array(cluster[3:])
-            half_sizes = sizes / 2.0
-            direction = center - cur_pose_np
-            dist = np.linalg.norm(direction)
-            if dist < 1e-6:
-                continue
-            dir_norm = direction / dist
-
-            # Find surface point facing the drone
-            ray_origin_local = cur_pose_np - center
-            tmin, tmax = -np.inf, np.inf
-            for axis in range(3):
-                if abs(dir_norm[axis]) > 1e-9:
-                    t1 = (-half_sizes[axis] - ray_origin_local[axis]) / dir_norm[axis]
-                    t2 = (half_sizes[axis] - ray_origin_local[axis]) / dir_norm[axis]
-                    tmin = max(tmin, min(t1, t2))
-                    tmax = min(tmax, max(t1, t2))
-            if tmax < max(tmin, 0):
-                continue
-            t_hit = tmin if tmin > 0 else tmax
-            surface_point = cur_pose_np + dir_norm * t_hit
-            adjacent = surface_point - dir_norm * 1.0   # 1m in front of surface
-
-            if i == 0:
-                if not waypoint_locked:
-                    target_waypoint2 = adjacent
-                    waypoint_locked = True
-
-                alpha = 0.8
-                mid = cur_pose_np * (1 - alpha) + target_waypoint2 * alpha
-                target_waypoint = mid
-
-                for wp_np in [mid, target_waypoint2]:
-                    ps = PoseStamped()
-                    ps.header.stamp = self.get_clock().now().to_msg()
-                    ps.header.frame_id = 'map'
-                    ps.pose.position.x = float(wp_np[0])
-                    ps.pose.position.y = float(wp_np[1])
-                    ps.pose.position.z = float(wp_np[2])
-                    ps.pose.orientation.w = 1.0
-                    path.poses.append(ps)
-
-        path_pub.publish(path)
-
-        # Mark cluster as visited only when the drone has been navigating toward a
-        # previously-locked waypoint (waypoint_was_locked) AND arrives within 3m.
-        # Skipping the check on the tick the waypoint was first set prevents
-        # immediate false-positive "arrived" detections.
-        if waypoint_was_locked and target_waypoint2 is not None and \
-                np.linalg.norm(cur_pose_np - target_waypoint2) < 3.0:
-            if sorted_clusters:
-                arrived_idx, arrived_cluster = sorted_clusters[0]
-                self.visited_clusters.append(arrived_cluster)
-                completed_label = self.cluster_query_map.get(arrived_idx)
-                if completed_label:
-                    self.completed_queries.add(completed_label)
-            waypoint_locked = False
-
-        return waypoint_locked, target_waypoint, target_waypoint2
-
-    # ── helpers ──────────────────────────────────────────────────────────────
-
-    def _is_near_visited(self, center, size, visited_clusters, threshold=10.0):
-        return any(
-            self._cuboid_distance(center, size,
-                                  np.array(v[:3]), np.array(v[3:6])) < threshold
-            for v in visited_clusters)
-
-    def _cuboid_distance(self, ca, sa, cb, sb):
-        ha, hb = sa / 2.0, sb / 2.0
-        dx = max(abs(ca[0] - cb[0]) - (ha[0] + hb[0]), 0)
-        dy = max(abs(ca[1] - cb[1]) - (ha[1] + hb[1]), 0)
-        dz = max(abs(ca[2] - cb[2]) - (ha[2] + hb[2]), 0)
-        return np.sqrt(dx**2 + dy**2 + dz**2)
-
-    def _visualize_clusters(self, pub):
-        markers = MarkerArray()
-        now = self.get_clock().now().to_msg()
-        # Delete old markers
-        for i in range(self.prev_voxel_cluster_ids):
-            m = Marker()
-            m.header.frame_id = 'map'
-            m.header.stamp = now
-            m.ns = 'voxel_clusters'
-            m.id = i
-            m.action = Marker.DELETE
-            markers.markers.append(m)
-        # Add new markers
-        for j, (_, cluster) in enumerate(self.unvisited_clusters):
-            m = Marker()
-            m.header.frame_id = 'map'
-            m.header.stamp = now
-            m.ns = 'voxel_clusters'
-            m.id = j
-            m.type = Marker.CUBE
-            m.action = Marker.ADD
-            m.pose.position.x = cluster[0]
-            m.pose.position.y = cluster[1]
-            m.pose.position.z = cluster[2]
-            m.scale.x = cluster[3]
-            m.scale.y = cluster[4]
-            m.scale.z = cluster[5]
-            m.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=0.2)
-            m.lifetime.sec = 1
-            markers.markers.append(m)
-        self.prev_voxel_cluster_ids = len(self.unvisited_clusters)
-        pub.publish(markers)
diff --git a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py b/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
deleted file mode 100644
index 773c600f..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/raven_nav/raven_nav_node.py
+++ /dev/null
@@ -1,278 +0,0 @@
-import json
-import os
-import numpy as np
-
-import rclpy
-from rclpy.node import Node
-from nav_msgs.msg import Odometry, Path
-from sensor_msgs.msg import PointCloud2
-from sensor_msgs_py import point_cloud2
-from std_msgs.msg import String
-from visualization_msgs.msg import MarkerArray
-
-from raven_nav.behavior_manager import BehaviorManager
-
-
-class RavenNavNode(Node):
-    def __init__(self):
-        super().__init__('raven_nav')
-
-        robot_name = os.getenv('ROBOT_NAME', 'robot')
-        self._prefix = f'/{robot_name}'
-
-        # Ray data from rays_sim/all (converted to FLU world frame)
-        self._ray_origins = None    # (N, 3)
-        self._ray_dirs = None       # (N, 3)
-        self._ray_scores = None     # (N, Q)
-
-        # Voxel data from voxels_sim/all
-        self._vox_xyz = None        # (N, 3)
-        self._vox_scores = None     # (N, Q)
-
-        self._frontiers = None      # (N, 3)
-        self._cur_pose = None       # (3,)
-
-        self._waypoint_locked = False
-        self._target_waypoint = None
-        self._target_waypoint2 = None
-        self._behavior_mode = 'Frontier-based'
-
-        # Parameters
-        self._score_threshold = self.declare_parameter('score_threshold', 0.95).value
-        query_labels_param = self.declare_parameter(
-            'query_labels', ['red building', 'water tower', 'radio tower']).value
-        self._query_labels = list(query_labels_param)
-        # target_labels: which queries to actually navigate toward.
-        # Defaults to query_labels if empty. Must be a subset of query_labels.
-        target_labels_param = self.declare_parameter('target_labels', ['']).value
-        target_labels = [t for t in target_labels_param if t]
-        self._target_objects = target_labels if target_labels else self._query_labels[:]
-
-        self._min_altitude = self.declare_parameter('min_altitude_agl', 1.5).value
-        self._max_altitude = self.declare_parameter('max_altitude_agl', 100.0).value
-
-        timer_period = self.declare_parameter('timer_period', 0.5).value
-
-        self._path_pub = self.create_publisher(
-            Path, f'{self._prefix}/global_plan', 10)
-        self._filtered_rays_pub = self.create_publisher(
-            MarkerArray, f'{self._prefix}/filtered_rays', 10)
-        self._viewpoint_pub = self.create_publisher(
-            PointCloud2, f'{self._prefix}/frontier_viewpoints', 10)
-        self._current_target_pub = self.create_publisher(
-            String, f'{self._prefix}/current_target', 10)
-        self._voxel_bbox_pub = self.create_publisher(
-            MarkerArray, f'{self._prefix}/voxel_clusters', 10)
-        self._completed_targets_pub = self.create_publisher(
-            String, f'{self._prefix}/completed_targets', 10)
-
-        self._publisher_dict = {
-            'path': self._path_pub,
-            'filtered_rays': self._filtered_rays_pub,
-            'viewpoint': self._viewpoint_pub,
-            'current_target': self._current_target_pub,
-            'voxel_bbox': self._voxel_bbox_pub,
-        }
-
-        self._behavior_manager = BehaviorManager(
-            get_clock=self.get_clock,
-            publisher_dict=self._publisher_dict,
-            score_threshold=self._score_threshold,
-            min_altitude=self._min_altitude,
-            max_altitude=self._max_altitude,
-        )
-
-        # Subscribe to all-queries topics so rayfronts always publishes to them
-        rf_prefix = f'{self._prefix}/rayfronts/msg_serv'
-        self.create_subscription(
-            PointCloud2,
-            f'{rf_prefix}/rays_sim/all',
-            self._ray_all_cb, 10)
-        self.create_subscription(
-            PointCloud2,
-            f'{rf_prefix}/voxels_sim/all',
-            self._vox_all_cb, 10)
-
-        self.create_subscription(
-            PointCloud2,
-            f'{self._prefix}/rayfronts/msg_serv/frontiers',
-            self._frontiers_cb, 10)
-        self.create_subscription(
-            Odometry,
-            f'{self._prefix}/odometry',
-            self._odometry_cb, 10)
-        self.create_subscription(
-            String,
-            '/input_prompt',
-            self._input_prompt_cb, 10)
-
-        self.create_timer(timer_period, self._timer_cb)
-
-        self.get_logger().info(
-            f'raven_nav started | robot={robot_name} | '
-            f'timer={timer_period:.2f}s | '
-            f'query_labels={self._query_labels} | '
-            f'score_threshold={self._score_threshold} | '
-            f'altitude=[{self._min_altitude}, {self._max_altitude}]')
-
-    def _ray_all_cb(self, msg: PointCloud2):
-        """Receive all-queries ray PointCloud2 (fields: x,y,z,theta,phi,sim_0,sim_1,...)."""
-        Q = len(self._query_labels)
-        if Q == 0:
-            return
-        # Discover actual sim_* fields from the message to handle query count mismatches
-        msg_field_names = [f.name for f in msg.fields]
-        sim_fields = sorted([f for f in msg_field_names if f.startswith('sim_')])
-        Q_actual = len(sim_fields)
-        if Q_actual == 0:
-            self._ray_origins = None
-            self._ray_dirs = None
-            self._ray_scores = None
-            return
-        fields = ('x', 'y', 'z', 'theta', 'phi') + tuple(sim_fields)
-        pts = list(point_cloud2.read_points(msg, field_names=fields, skip_nans=True))
-        if not pts:
-            self._ray_origins = None
-            self._ray_dirs = None
-            self._ray_scores = None
-            return
-        arr = np.array([list(p) for p in pts], dtype=np.float32)
-        rdf_orig = arr[:, :3]
-        theta = np.deg2rad(arr[:, 3])
-        phi = np.deg2rad(arr[:, 4])
-        sim_all = arr[:, 5:]  # (N, Q)
-        # Spherical → Cartesian in RDF
-        dx = np.cos(theta) * np.sin(phi)
-        dy = np.sin(theta) * np.sin(phi)
-        dz = np.cos(phi)
-        rdf_dirs = np.stack([dx, dy, dz], axis=1)
-        # RDF → FLU
-        flu_orig = np.stack([rdf_orig[:, 2], -rdf_orig[:, 0], -rdf_orig[:, 1]], axis=1)
-        flu_dirs = np.stack([rdf_dirs[:, 2], -rdf_dirs[:, 0], -rdf_dirs[:, 1]], axis=1)
-        self._ray_origins = flu_orig
-        self._ray_dirs = flu_dirs
-        self._ray_scores = sim_all
-
-    def _vox_all_cb(self, msg: PointCloud2):
-        """Receive all-queries voxel PointCloud2 (fields: x,y,z,sim_0,sim_1,...)."""
-        Q = len(self._query_labels)
-        if Q == 0:
-            return
-        # Discover actual sim_* fields from the message to handle query count mismatches
-        msg_field_names = [f.name for f in msg.fields]
-        sim_fields = sorted([f for f in msg_field_names if f.startswith('sim_')])
-        Q_actual = len(sim_fields)
-        if Q_actual == 0:
-            self._vox_xyz = None
-            self._vox_scores = None
-            return
-        fields = ('x', 'y', 'z') + tuple(sim_fields)
-        pts = list(point_cloud2.read_points(msg, field_names=fields, skip_nans=True))
-        if not pts:
-            self._vox_xyz = None
-            self._vox_scores = None
-            return
-        arr = np.array([list(p) for p in pts], dtype=np.float32)
-        rdf_xyz = arr[:, :3]
-        sim_all = arr[:, 3:]  # (N, Q)
-        # RDF → FLU
-        flu_xyz = np.stack([rdf_xyz[:, 2], -rdf_xyz[:, 0], -rdf_xyz[:, 1]], axis=1)
-        self._vox_xyz = flu_xyz
-        self._vox_scores = sim_all
-
-    def _frontiers_cb(self, msg: PointCloud2):
-        pts = list(point_cloud2.read_points(msg, field_names=('x', 'y', 'z'),
-                                            skip_nans=True))
-        self._frontiers = (
-            np.array([[p[0], p[1], p[2]] for p in pts], dtype=np.float32)
-            if pts else None)
-
-    def _odometry_cb(self, msg: Odometry):
-        p = msg.pose.pose.position
-        self._cur_pose = np.array([p.x, p.y, p.z], dtype=np.float64)
-
-    def _input_prompt_cb(self, msg: String):
-        targets = [t.strip() for t in msg.data.split(',') if t.strip()]
-        if targets:
-            self._target_objects = targets
-            # Keep _query_labels in sync so vox/ray callbacks read the right columns
-            if targets != self._query_labels:
-                self._query_labels = targets[:]
-                # Clear cached data so stale multi-column arrays don't confuse behaviors
-                self._vox_xyz = None
-                self._vox_scores = None
-                self._ray_origins = None
-                self._ray_dirs = None
-                self._ray_scores = None
-            self.get_logger().info(f'target objects updated: {self._target_objects}')
-
-    def _timer_cb(self):
-        if self._cur_pose is None:
-            self.get_logger().warn('waiting for odometry...', throttle_duration_sec=5.0)
-            return
-
-        prev_mode = self._behavior_mode
-        self._behavior_manager.mode_select(
-            ray_origins=self._ray_origins,
-            ray_dirs=self._ray_dirs,
-            ray_scores=self._ray_scores,
-            query_labels=self._query_labels,
-            target_objects=self._target_objects,
-            vox_xyz=self._vox_xyz,
-            vox_scores=self._vox_scores,
-        )
-        self._behavior_mode = self._behavior_manager.behavior_mode
-
-        if self._behavior_mode != prev_mode:
-            self.get_logger().info(f'behavior mode: {prev_mode} -> {self._behavior_mode}')
-            self._waypoint_locked = False
-            self._target_waypoint = None
-            self._target_waypoint2 = None
-
-        self._waypoint_locked, self._target_waypoint, self._target_waypoint2 = \
-            self._behavior_manager.behavior_execute(
-                behavior_mode=self._behavior_mode,
-                frontiers=self._frontiers,
-                cur_pose_np=self._cur_pose,
-                waypoint_locked=self._waypoint_locked,
-                target_waypoint=self._target_waypoint,
-                target_waypoint2=self._target_waypoint2,
-                publisher_dict=self._publisher_dict,
-                vox_xyz=self._vox_xyz,
-                vox_scores=self._vox_scores,
-                query_labels=self._query_labels,
-            )
-
-        # Completed-targets publishing disabled — task stops only on manual cancel
-        completed = list(self._behavior_manager.completed_queries)
-
-        # Status log
-        n_frontiers = len(self._frontiers) if self._frontiers is not None else 0
-        n_rays = len(self._ray_origins) if self._ray_origins is not None else 0
-        n_voxels = len(self._vox_xyz) if self._vox_xyz is not None else 0
-        ray_beh = self._behavior_manager.ray_behavior
-        n_filtered = (len(ray_beh._filtered_indices)
-                      if ray_beh._filtered_indices is not None else 0)
-        wp = (f'({self._target_waypoint[0]:.1f}, {self._target_waypoint[1]:.1f}, '
-              f'{self._target_waypoint[2]:.1f})'
-              if self._target_waypoint is not None else 'none')
-        self.get_logger().info(
-            f'[{self._behavior_mode}] '
-            f'frontiers={n_frontiers} rays={n_rays} filtered={n_filtered} '
-            f'voxels={n_voxels} | '
-            f'target={ray_beh.current_target} | '
-            f'completed={completed} | wp={wp}',
-            throttle_duration_sec=2.0,
-        )
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = RavenNavNode()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
diff --git a/robot/ros_ws/src/global/planners/raven_nav/resource/raven_nav b/robot/ros_ws/src/global/planners/raven_nav/resource/raven_nav
deleted file mode 100644
index e69de29b..00000000
diff --git a/robot/ros_ws/src/global/planners/raven_nav/setup.cfg b/robot/ros_ws/src/global/planners/raven_nav/setup.cfg
deleted file mode 100644
index 1b9b84ea..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/raven_nav
-[install]
-install_scripts=$base/lib/raven_nav
diff --git a/robot/ros_ws/src/global/planners/raven_nav/setup.py b/robot/ros_ws/src/global/planners/raven_nav/setup.py
deleted file mode 100644
index ce503284..00000000
--- a/robot/ros_ws/src/global/planners/raven_nav/setup.py
+++ /dev/null
@@ -1,27 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'raven_nav'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name + '/launch', ['launch/raven_nav.launch.xml']),
-        ('share/' + package_name + '/config', ['config/raven_nav.yaml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='todo',
-    maintainer_email='todo@todo.todo',
-    description='Navigation planner consuming RayFronts mapper outputs',
-    license='TODO',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-            'raven_nav_node = raven_nav.raven_nav_node:main',
-        ],
-    },
-)
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml b/robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml
deleted file mode 100644
index 7ac94ab5..00000000
--- a/robot/ros_ws/src/global/planners/semantic_search_task/launch/semantic_search_task.launch.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
-    <node pkg="semantic_search_task" exec="semantic_search_task"
-          name="semantic_search_task" output="screen">
-        <remap from="~/semantic_search_task"
-               to="/$(env ROBOT_NAME)/tasks/semantic_search" />
-        <remap from="/$(env ROBOT_NAME)/odometry"
-               to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
-    </node>
-</launch>
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/package.xml b/robot/ros_ws/src/global/planners/semantic_search_task/package.xml
deleted file mode 100644
index e9363a32..00000000
--- a/robot/ros_ws/src/global/planners/semantic_search_task/package.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>semantic_search_task</name>
-  <version>0.0.0</version>
-  <description>SemanticSearchTask action server: sends natural language queries to rayfronts and monitors voxel similarity output</description>
-  <maintainer email="todo@todo.todo">todo</maintainer>
-  <license>TODO</license>
-
-  <depend>rclpy</depend>
-  <depend>task_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>sensor_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>visualization_msgs</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/resource/semantic_search_task b/robot/ros_ws/src/global/planners/semantic_search_task/resource/semantic_search_task
deleted file mode 100644
index e69de29b..00000000
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/__init__.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/__init__.py
deleted file mode 100644
index e69de29b..00000000
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py b/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
deleted file mode 100644
index 566c66bf..00000000
--- a/robot/ros_ws/src/global/planners/semantic_search_task/semantic_search_task/node.py
+++ /dev/null
@@ -1,555 +0,0 @@
-import os
-import queue
-import re
-import signal
-import subprocess
-import threading
-import time
-
-import numpy as np
-import rclpy
-import rclpy.executors
-from geometry_msgs.msg import Point, Polygon, Pose
-from nav_msgs.msg import Odometry
-from rclpy.action import ActionServer, ActionClient, CancelResponse, GoalResponse
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.node import Node
-from sensor_msgs.msg import PointCloud2
-from sensor_msgs_py import point_cloud2
-from std_msgs.msg import String
-from task_msgs.action import SemanticSearchTask, ExplorationTask
-
-# ── ANSI / ROS log stripping ──────────────────────────────────────────────────
-
-_ANSI_RE = re.compile(r'\x1b\[[0-9;]*m')
-_ROS_PREFIX_RE = re.compile(
-    r'^\s*\[?(INFO|WARN|ERROR|DEBUG)\]?\s*\[\d+\.\d+\]\s*\[[^\]]+\]:\s*')
-
-
-def _clean(line: str) -> str:
-    line = _ANSI_RE.sub('', line)
-    line = _ROS_PREFIX_RE.sub('', line)
-    return line.strip()
-
-
-# ── Output filters ────────────────────────────────────────────────────────────
-
-def _filter_rayfronts(line: str) -> str | None:
-    """Return a short human-readable string for key rayfronts events, else None."""
-    raw = line
-    line = _clean(line)
-    low = line.lower()
-
-    if 'error' in low or 'exception' in low or 'traceback' in low:
-        return f'ERROR: {line}'
-    if 'downloading' in low:
-        return 'Downloading model weights...'
-    if 'parsing model identifier' in low or 'loaded built-in' in low:
-        return 'Loading model...'
-    if 'subscribing to' in low or 'subscribed to' in low:
-        return f'Subscribing: {line}'
-    if 'received queries' in low:
-        return f'Encoding queries: {line}'
-    if 'mapping' in low and ('start' in low or 'running' in low or 'loop' in low):
-        return 'Mapping started'
-    if 'process started' in low or 'process has died' in low:
-        return line
-    # Skip noise: warnings, deprecations, progress bars, blank lines
-    if not line or 'warning' in low or 'warn' in low or 'deprecated' in low:
-        return None
-    if '|' in raw and '█' in raw:   # tqdm progress bar
-        return None
-    return None
-
-
-def _filter_raven(line: str) -> str | None:
-    """Reformat raven's status line; skip everything else."""
-    line = _clean(line)
-    if not line:
-        return None
-    low = line.lower()
-    # Surface startup, waiting, and error messages
-    if 'raven_nav started' in low:
-        return 'raven_nav started'
-    if 'waiting for odometry' in low:
-        return 'Waiting for odometry...'
-    if 'error' in low or 'exception' in low or 'traceback' in low:
-        return f'ERROR: {line}'
-    # The periodic status line looks like:
-    # [Frontier-based] frontiers=N rays=N filtered=N voxels=N | target=X | completed=[] | wp=(...)
-    m = re.search(r'\[(Frontier-based|Ray-based|Voxel-based)\]', line)
-    if not m:
-        return None
-
-    mode = m.group(1)
-    frontiers = re.search(r'frontiers=(\d+)', line)
-    rays = re.search(r'rays=(\d+)', line)
-    voxels = re.search(r'voxels=(\d+)', line)
-    completed = re.search(r'completed=(\[[^\]]*\])', line)
-    wp = re.search(r'wp=(\([^)]+\))', line)
-
-    target = re.search(r'target=(.+?)\s*\|', line)
-    filtered = re.search(r'filtered=(\d+)', line)
-
-    parts = [f'[{mode}]']
-    if target and target.group(1).strip() != 'None':
-        parts.append(f'current_target="{target.group(1).strip()}"')
-    if frontiers:
-        parts.append(f'frontiers={frontiers.group(1)}')
-    if rays:
-        parts.append(f'rays={rays.group(1)}')
-    if filtered:
-        parts.append(f'filtered={filtered.group(1)}')
-    if voxels:
-        parts.append(f'voxels={voxels.group(1)}')
-    if completed and completed.group(1) != '[]':
-        parts.append(f'completed={completed.group(1)}')
-    if wp and wp.group(1) != 'none':
-        parts.append(f'wp={wp.group(1)}')
-    return ' '.join(parts)
-
-
-# ── Subprocess helpers ────────────────────────────────────────────────────────
-
-def _pipe_to_queue(proc: subprocess.Popen, q: queue.Queue) -> None:
-    for line in iter(proc.stdout.readline, b''):
-        q.put(line.decode('utf-8', errors='replace').rstrip())
-    q.put(None)
-
-
-def _drain(q: queue.Queue) -> list:
-    lines = []
-    while True:
-        try:
-            item = q.get_nowait()
-            if item is None:
-                break
-            lines.append(item)
-        except queue.Empty:
-            break
-    return lines
-
-
-def _sanitize(label: str) -> str:
-    return re.sub(r'[^a-zA-Z0-9_]', '_', label)
-
-
-# ── Node ──────────────────────────────────────────────────────────────────────
-
-class SemanticSearchTaskNode(Node):
-    def __init__(self):
-        super().__init__('semantic_search_task')
-        robot_name = os.getenv('ROBOT_NAME', 'robot_1')
-        ros_domain = os.getenv('ROS_DOMAIN_ID', '0')
-        self._robot_prefix = f'/{robot_name}'
-        self._rf_prefix = f'/robot_{ros_domain}/rayfronts/msg_serv'
-
-        self._cbg = ReentrantCallbackGroup()
-        self._task_active = False
-        self._cur_pos = None
-
-        self._text_query_pub = self.create_publisher(
-            String, f'{self._rf_prefix}/new_text_query', 10)
-
-        self.create_subscription(
-            Odometry, f'{self._robot_prefix}/odometry',
-            self._odom_cb, 10, callback_group=self._cbg)
-
-        self._action_server = ActionServer(
-            self, SemanticSearchTask, '~/semantic_search_task',
-            execute_callback=self._execute,
-            goal_callback=self._handle_goal,
-            cancel_callback=lambda _: CancelResponse.ACCEPT,
-            callback_group=self._cbg)
-
-        self._cleanup_existing()
-        self.get_logger().info('semantic_search_task ready')
-
-    def _odom_cb(self, msg: Odometry):
-        p = msg.pose.pose.position
-        self._cur_pos = [p.x, p.y, p.z]
-
-    def _handle_goal(self, goal_request):
-        if self._task_active:
-            self.get_logger().warn('Rejecting goal: task already active')
-            return GoalResponse.REJECT
-        self._task_active = True
-        return GoalResponse.ACCEPT
-
-    def _cleanup_existing(self) -> None:
-        """Kill any leftover rayfronts or raven processes.
-
-        random_walk_planner is intentionally NOT killed here: it now runs as
-        a long-lived task executor under global_bringup, and pkill'ing it
-        would tear down the ExplorationTask action server every time this
-        node initializes or starts a goal.
-        """
-        for pattern in ['rayfronts.mapping_server', 'raven_nav_node']:
-            result = subprocess.run(
-                ['pkill', '-SIGTERM', '-f', pattern], capture_output=True)
-            if result.returncode == 0:
-                self.get_logger().info(f'Killed existing {pattern} process(es)')
-        time.sleep(2.0)
-        for pattern in ['rayfronts.mapping_server', 'raven_nav_node']:
-            subprocess.run(['pkill', '-SIGKILL', '-f', pattern], capture_output=True)
-
-    def _spawn(self, cmd: list) -> tuple:
-        proc = subprocess.Popen(
-            cmd,
-            stdout=subprocess.PIPE,
-            stderr=subprocess.STDOUT,
-            start_new_session=True)
-        q = queue.Queue()
-        threading.Thread(target=_pipe_to_queue, args=(proc, q), daemon=True).start()
-        return proc, q
-
-    def _kill(self, name: str, proc: subprocess.Popen) -> None:
-        if proc is None:
-            return
-        self.get_logger().info(f'Stopping {name} (pid={proc.pid})')
-        try:
-            os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
-        except ProcessLookupError:
-            return
-        try:
-            proc.wait(timeout=5.0)
-        except subprocess.TimeoutExpired:
-            try:
-                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
-            except ProcessLookupError:
-                pass
-
-    def _spawn_random_walk(self, robot_name: str) -> subprocess.Popen:
-        # Resolve the config file path via ros2 pkg
-        prefix = subprocess.run(
-            ['ros2', 'pkg', 'prefix', 'random_walk_planner'],
-            capture_output=True, text=True).stdout.strip()
-        config = f'{prefix}/share/random_walk_planner/config/random_walk_config.yaml'
-        return self._spawn([
-            'ros2', 'run', 'random_walk_planner', 'random_walk_planner',
-            '--ros-args',
-            '--params-file', config,
-            '-r', f'~/global_plan:=/{robot_name}/global_plan',
-            '-r', f'~/exploration_task:=/{robot_name}/tasks/exploration',
-            '-r', f'navigate_task:=/{robot_name}/tasks/navigate',
-            '-r', f'odometry:=/{robot_name}/odometry_conversion/odometry',
-            '-r', f'vdb_map_visualization:=/{robot_name}/vdb_mapping/vdb_map_visualization',
-            '-r', f'~/global_plan_toggle:=/{robot_name}/behavior/global_plan_toggle',
-        ])
-
-    def _send_exploration_task(self, robot_name: str):
-        """Send an unbounded ExplorationTask goal to random_walk_planner to
-        activate droan_gl. Returns (client, send_future) so the caller can
-        cancel the upstream goal when the semantic_search task ends."""
-        client = ActionClient(
-            self,
-            ExplorationTask,
-            f'/{robot_name}/tasks/exploration',
-            callback_group=self._cbg)
-        if not client.wait_for_server(timeout_sec=10.0):
-            self.get_logger().warn('ExplorationTask server not available after 10s')
-            return None, None
-        goal = ExplorationTask.Goal()
-        goal.min_altitude_agl = 2.0
-        goal.max_altitude_agl = 15.0
-        goal.min_flight_speed = 1.0
-        goal.max_flight_speed = 3.0
-        send_future = client.send_goal_async(goal)
-        self.get_logger().info('ExplorationTask sent to random_walk_planner')
-        return client, send_future
-
-    def _cancel_exploration_task(self, send_future):
-        """Best-effort cancel of an ExplorationTask started via
-        _send_exploration_task. Safe to call with None / not-yet-accepted."""
-        if send_future is None or not send_future.done():
-            return
-        try:
-            handle = send_future.result()
-        except Exception:
-            return
-        if handle is None or not getattr(handle, 'accepted', False):
-            return
-        try:
-            handle.cancel_goal_async()
-            self.get_logger().info(
-                'Cancelled upstream ExplorationTask (random_walk_planner)')
-        except Exception as e:
-            self.get_logger().warn(f'Failed to cancel ExplorationTask: {e}')
-
-    def _interruptible_sleep(self, goal_handle, secs: float) -> bool:
-        """Sleep for secs, waking early if cancel requested. Returns True if cancelled."""
-        steps = max(1, int(secs / 0.1))
-        for _ in range(steps):
-            if goal_handle.is_cancel_requested:
-                return True
-            time.sleep(0.1)
-        return False
-
-    def _execute(self, goal_handle):
-        goal = goal_handle.request
-        queries = [q.strip() for q in goal.query.split(',') if q.strip()]
-        if not queries:
-            self._task_active = False
-            goal_handle.abort()
-            result = SemanticSearchTask.Result()
-            result.success = False
-            result.message = 'Empty query'
-            return result
-
-        bg_raw = [bq.strip() for bq in goal.background_queries.split(',')
-                  if bq.strip()]
-        if not bg_raw:
-            self._task_active = False
-            goal_handle.abort()
-            result = SemanticSearchTask.Result()
-            result.success = False
-            result.message = (
-                'background_queries is required but was not provided. '
-                'Softmax normalization needs contrast classes (e.g. '
-                '"building,tree,ground") to produce meaningful scores.')
-            return result
-
-        # Build the full query list: target queries + background contrast queries.
-        # Softmax normalization across queries requires N >= 2 to produce
-        # meaningful discriminative scores; with N=1 every score is 1.0.
-        bg = [bq for bq in bg_raw if bq not in queries]
-        all_queries = queries + bg
-
-        if len(all_queries) < 2:
-            self._task_active = False
-            goal_handle.abort()
-            result = SemanticSearchTask.Result()
-            result.success = False
-            result.message = (
-                f'Need at least 2 total queries for softmax normalization, '
-                f'got {len(all_queries)}. Add more background_queries that '
-                f'differ from the target query.')
-            return result
-
-        self.get_logger().info(
-            f'SemanticSearchTask | targets={queries} all_queries={all_queries}')
-
-        rayfronts_proc = raven_proc = random_walk_proc = None
-        exploration_send_future = None
-        rayfronts_q = raven_q = queue.Queue()
-
-        # Track last meaningful line from each process for feedback
-        last_rf_status = 'Starting rayfronts...'
-        last_rv_status = 'Starting raven...'
-
-        try:
-            # Kill any leftover processes from a previous run
-            self._cleanup_existing()
-
-            robot_name = os.getenv('ROBOT_NAME', 'robot_1')
-
-            # Wait for old rayfronts DDS subscription to fully unregister
-            deadline = time.time() + 10.0
-            while time.time() < deadline:
-                if goal_handle.is_cancel_requested:
-                    goal_handle.canceled()
-                    result = SemanticSearchTask.Result()
-                    result.success = False
-                    result.message = 'Cancelled'
-                    return result
-                if self.count_subscribers(f'{self._rf_prefix}/new_text_query') == 0:
-                    break
-                time.sleep(0.5)
-
-            # Start raven first so it's subscribed to rays_sim/all and voxels_sim/all
-            # before rayfronts runs its first query cycle (rayfronts lazy-publishes
-            # those topics only when a subscriber exists).
-            # query_labels = full set (for sim column mapping)
-            # target_labels = only the user's targets (for behavior filtering)
-            all_labels_yaml = str(all_queries).replace("'", '"')
-            target_labels_yaml = str(queries).replace("'", '"')
-            raven_proc, raven_q = self._spawn([
-                'ros2', 'run', 'raven_nav', 'raven_nav_node',
-                '--ros-args',
-                '-p', f'query_labels:={all_labels_yaml}',
-                '-p', f'target_labels:={target_labels_yaml}',
-                '-p', f'min_altitude_agl:={goal.min_altitude_agl}',
-                '-p', f'max_altitude_agl:={goal.max_altitude_agl}',
-                '-r', (f'/{robot_name}/odometry:='
-                       f'/{robot_name}/odometry_conversion/odometry'),
-            ])
-
-            rayfronts_proc, rayfronts_q = self._spawn([
-                'ros2', 'launch', 'perception_bringup', 'rayfronts.launch.xml',
-            ])
-
-
-            best_conf = 0.0
-            rayfronts_ready = False
-            prev_rf_sub_count = 0
-            mapping_started = False
-            random_walk_started = False
-            raven_published_waypoint = False
-            completed_targets: set = set()
-
-            # Subscribe to raven's global_plan to detect first waypoint
-            from nav_msgs.msg import Path
-            def _global_plan_cb(msg):
-                nonlocal raven_published_waypoint
-                if msg.poses:
-                    raven_published_waypoint = True
-            self.create_subscription(
-                Path, f'/{robot_name}/global_plan',
-                _global_plan_cb, 1, callback_group=self._cbg)
-
-            # Subscribe to raven's completed_targets
-            def _completed_targets_cb(msg):
-                nonlocal completed_targets
-                import json
-                try:
-                    completed_targets = set(json.loads(msg.data))
-                except Exception:
-                    pass
-            self.create_subscription(
-                String, f'/{robot_name}/completed_targets',
-                _completed_targets_cb, 10, callback_group=self._cbg)
-
-            # Subscribe to voxels_sim/all for best-confidence tracking
-            def _vox_all_cb(msg):
-                nonlocal best_conf, mapping_started
-                msg_field_names = [f.name for f in msg.fields]
-                sim_fields = sorted([f for f in msg_field_names if f.startswith('sim_')])
-                if not sim_fields:
-                    return
-                fields = ('x', 'y', 'z') + tuple(sim_fields)
-                from sensor_msgs_py import point_cloud2 as pc2
-                pts = list(pc2.read_points(msg, field_names=fields, skip_nans=True))
-                if pts:
-                    import numpy as _np
-                    arr = _np.array([list(p) for p in pts], dtype=_np.float32)
-                    conf = float(arr[:, 3:].max())
-                    if conf > best_conf:
-                        best_conf = conf
-                    mapping_started = True
-            self.create_subscription(
-                PointCloud2, f'{self._rf_prefix}/voxels_sim/all',
-                _vox_all_cb, 10, callback_group=self._cbg)
-
-            while rclpy.ok():
-                if goal_handle.is_cancel_requested:
-                    goal_handle.canceled()
-                    result = SemanticSearchTask.Result()
-                    result.success = False
-                    result.message = 'Cancelled'
-                    return result
-
-                # Drain and filter rayfronts output
-                for raw in _drain(rayfronts_q):
-                    msg = _filter_rayfronts(raw)
-                    if msg:
-                        last_rf_status = msg
-
-                # Drain and filter raven output
-                for raw in _drain(raven_q):
-                    msg = _filter_raven(raw)
-                    if msg:
-                        last_rv_status = msg
-
-                # Start random_walk once raven publishes its first waypoint
-                if raven_published_waypoint and not random_walk_started:
-                    random_walk_started = True
-                    random_walk_proc, _ = self._spawn_random_walk(robot_name)
-                    self.get_logger().info(
-                        'Raven published first waypoint — starting random_walk_planner')
-                    time.sleep(3.0)
-                    _, exploration_send_future = self._send_exploration_task(
-                        robot_name)
-
-                # Send queries to rayfronts whenever its subscriber appears (or reappears).
-                # This handles the initial load AND any rayfronts restart mid-task.
-                # Send ALL queries (target + background) so softmax is meaningful.
-                rf_sub_count = self.count_subscribers(f'{self._rf_prefix}/new_text_query')
-                if rf_sub_count > 0 and prev_rf_sub_count == 0:
-                    rayfronts_ready = True
-                    for q in all_queries:
-                        self._text_query_pub.publish(String(data=q))
-                    last_rf_status = f'Queries sent: {", ".join(all_queries)}'
-                    self.get_logger().info(
-                        f'Queries sent to rayfronts: {all_queries}')
-                if rf_sub_count == 0 and prev_rf_sub_count > 0:
-                    rayfronts_ready = False
-                    self.get_logger().info('rayfronts subscriber lost — will resend on reconnect')
-                prev_rf_sub_count = rf_sub_count
-
-                # Success: raven visited a cluster for every query
-                queries_set = set(q.lower() for q in queries)
-                completed_lower = set(c.lower() for c in completed_targets)
-                if queries_set and queries_set.issubset(completed_lower):
-                    goal_handle.succeed()
-                    result = SemanticSearchTask.Result()
-                    result.success = True
-                    result.message = (
-                        f'All targets visited: {", ".join(sorted(queries_set))}')
-                    result.confidence = best_conf
-                    return result
-
-                # Build clean feedback status
-                pos_str = ''
-                if self._cur_pos:
-                    pos_str = (f'pos=({self._cur_pos[0]:.1f}, '
-                               f'{self._cur_pos[1]:.1f}, '
-                               f'{self._cur_pos[2]:.1f})')
-
-                pending = [q for q in queries
-                           if q.lower() not in set(c.lower() for c in completed_targets)]
-                done = [q for q in queries
-                        if q.lower() in set(c.lower() for c in completed_targets)]
-
-                if not rayfronts_ready:
-                    status = f'[rayfronts] {last_rf_status}'
-                elif not mapping_started:
-                    status = (f'[rayfronts] {last_rf_status}\n'
-                              f'[raven] {last_rv_status}')
-                else:
-                    progress = f'{len(done)}/{len(queries)} targets visited'
-                    done_str = f'done=[{", ".join(done)}]' if done else ''
-                    pending_str = f'pending=[{", ".join(pending)}]' if pending else ''
-                    status = (f'[raven] {last_rv_status}\n'
-                              f'{progress}  {done_str}  {pending_str}  {pos_str}').strip()
-
-                fb = SemanticSearchTask.Feedback()
-                fb.status = status
-                fb.best_confidence_so_far = best_conf
-                if self._cur_pos:
-                    fb.current_position = Point(
-                        x=self._cur_pos[0],
-                        y=self._cur_pos[1],
-                        z=self._cur_pos[2])
-                goal_handle.publish_feedback(fb)
-
-                if self._interruptible_sleep(goal_handle, 1.0):
-                    continue   # cancel was requested, loop back to check it
-
-        finally:
-            # Cancel the upstream ExplorationTask so the bringup-launched
-            # random_walk_planner stops wandering when this task ends.
-            self._cancel_exploration_task(exploration_send_future)
-            self._kill('rayfronts', rayfronts_proc)
-            self._kill('raven', raven_proc)
-            self._kill('random_walk', random_walk_proc)
-            self._task_active = False
-
-        goal_handle.abort()
-        result = SemanticSearchTask.Result()
-        result.success = False
-        result.message = 'Node shutdown'
-        return result
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = SemanticSearchTaskNode()
-    executor = rclpy.executors.MultiThreadedExecutor()
-    executor.add_node(node)
-    try:
-        executor.spin()
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg b/robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg
deleted file mode 100644
index f68b1ceb..00000000
--- a/robot/ros_ws/src/global/planners/semantic_search_task/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/semantic_search_task
-[install]
-install_scripts=$base/lib/semantic_search_task
diff --git a/robot/ros_ws/src/global/planners/semantic_search_task/setup.py b/robot/ros_ws/src/global/planners/semantic_search_task/setup.py
deleted file mode 100644
index 20006247..00000000
--- a/robot/ros_ws/src/global/planners/semantic_search_task/setup.py
+++ /dev/null
@@ -1,26 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'semantic_search_task'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name + '/launch', ['launch/semantic_search_task.launch.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='todo',
-    maintainer_email='todo@todo.todo',
-    description='SemanticSearchTask action server using rayfronts + raven_nav',
-    license='TODO',
-    tests_require=['pytest'],
-    entry_points={
-        'console_scripts': [
-            'semantic_search_task = semantic_search_task.node:main',
-        ],
-    },
-)
diff --git a/robot/ros_ws/src/input_prompt.py b/robot/ros_ws/src/input_prompt.py
deleted file mode 100644
index 8bcc56cb..00000000
--- a/robot/ros_ws/src/input_prompt.py
+++ /dev/null
@@ -1,64 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-import threading
-import time
-
-class InputPublisher(Node):
-    def __init__(self):
-        super().__init__('input_publisher')
-        self.publisher_ = self.create_publisher(String, '/input_prompt', 10)
-        self.current_input = ""
-        self.running = True
-        self.publish_thread = threading.Thread(target=self.publish_loop)
-        self.publish_thread.start()
-
-    def publish_loop(self):
-        msg = String()
-        while self.running:
-            msg.data = self.current_input
-            self.publisher_.publish(msg)
-            time.sleep(0.2)  # 5Hz
-
-    def set_input(self, new_input):
-        self.current_input = new_input
-
-    def clear_input(self):
-        self.current_input = ""
-
-    def stop(self):
-        self.running = False
-        self.publish_thread.join()
-
-def main():
-    rclpy.init()
-    node = InputPublisher()
-
-    try:
-        while True:
-            print("\n=== Input Publisher ===")
-            print(f"Current input: '{node.current_input}'")
-            print("1. Set new input")
-            print("2. Clear input")
-            print("3. Exit")
-            choice = input("Choose option: ").strip()
-            if choice == '1':
-                new_input = input("Enter new input: ")
-                node.set_input(new_input)
-            elif choice == '2':
-                node.clear_input()
-                print("Input cleared.")
-            elif choice == '3':
-                break
-            else:
-                print("Invalid choice.")
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.stop()
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()
-
diff --git a/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml b/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
deleted file mode 100644
index 9ba28418..00000000
--- a/robot/ros_ws/src/perception/perception_bringup/launch/rayfronts.launch.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-<launch>
-  <arg name="config_name" default="low_memory"/>
-  <arg name="dataset"     default="ros2isaacsim"/>
-  <arg name="query_period" default="10"/>
-
-  <executable
-    cmd="python3 -m rayfronts.mapping_server
-         --config-name $(var config_name)
-         dataset=$(var dataset)
-         messaging_service=ros
-         querying.text_query_mode=prompts
-         querying.query_file=null
-         querying.compute_prob=True
-         querying.period=$(var query_period)"
-    cwd="/opt/rayfronts"
-    output="screen">
-    <env name="HYDRA_FULL_ERROR" value="1"/>
-  </executable>
-</launch>
diff --git a/simulation/isaac-sim/docker/.bashrc b/simulation/isaac-sim/docker/.bashrc
index 66b3c9c9..256b3587 100644
--- a/simulation/isaac-sim/docker/.bashrc
+++ b/simulation/isaac-sim/docker/.bashrc
@@ -127,54 +127,6 @@ source /isaac-sim/jazzy_ws/install/setup.bash  # isaacsim ros2 package
 # needed for communication with Isaac Sim ROS2  # https://docs.omniverse.nvidia.com/isaacsim/latest/installation/install_ros.html#enabling-the-ros-bridge-extension
 export FASTRTPS_DEFAULT_PROFILES_FILE="/isaac-sim/.ros/fastdds.xml"
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-# HITL DDS mode:
-# - server (default): use Fast DDS Discovery Server via ROS_DISCOVERY_SERVER.
-# - static-peer: generate a temporary Fast DDS profile with one initial peer.
-if [ "${HITL_DISCOVERY_MODE:-}" = "static-peer" ]; then
-    if [ -n "${FASTDDS_STATIC_PEER_IP:-}" ]; then
-        cat > /tmp/fastdds_static_peer.xml <<EOF
-<?xml version="1.0" encoding="UTF-8" ?>
-<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
-    <transport_descriptors>
-        <transport_descriptor>
-            <transport_id>UdpTransport</transport_id>
-            <type>UDPv4</type>
-        </transport_descriptor>
-    </transport_descriptors>
-    <participant profile_name="udp_transport_profile" is_default_profile="true">
-        <rtps>
-            <userTransports>
-                <transport_id>UdpTransport</transport_id>
-            </userTransports>
-            <useBuiltinTransports>false</useBuiltinTransports>
-            <builtin>
-                <initialPeersList>
-                    <locator>
-                        <udpv4>
-                            <address>${FASTDDS_STATIC_PEER_IP}</address>
-                        </udpv4>
-                    </locator>
-                </initialPeersList>
-            </builtin>
-        </rtps>
-    </participant>
-</profiles>
-EOF
-        export FASTRTPS_DEFAULT_PROFILES_FILE="/tmp/fastdds_static_peer.xml"
-    fi
-    unset ROS_DISCOVERY_SERVER
-elif [ -n "${DISCOVERY_SERVER_IP:-}" ]; then
-    discovery_server_list="${DISCOVERY_SERVER_IP}:${DISCOVERY_SERVER_PORT:-11811}"
-    if [ -n "${DISCOVERY_SERVER_BACKUP_IPS:-}" ]; then
-        IFS=',' read -r -a discovery_backup_ips <<< "${DISCOVERY_SERVER_BACKUP_IPS}"
-        for backup_ip in "${discovery_backup_ips[@]}"; do
-            if [ -n "${backup_ip}" ]; then
-                discovery_server_list="${discovery_server_list};${backup_ip}:${DISCOVERY_SERVER_PORT:-11811}"
-            fi
-        done
-    fi
-    export ROS_DISCOVERY_SERVER="${discovery_server_list}"
-fi
 # for local development, prevent conflict with other desktops
 export ROS_AUTOMATIC_DISCOVERY_RANGE=SUBNET
 
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index 7a996dea..4043da32 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -89,13 +89,3 @@ services:
         tmux send-keys -t isaac '/isaac-sim/runapp.sh' ENTER; 
       sleep infinity"
     networks: !reset null
-  # ===================================================================================================================
-  # for running over ethernet for hitl to stream to the NVIDIA jetson device
-  isaac-sim-hitl:
-    extends:
-      service: isaac-sim
-    profiles: !override
-      - hitl
-    container_name: isaac-sim
-    network_mode: host
-    networks: !reset null
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
similarity index 100%
rename from simulation/isaac-sim/launch_scripts/two_drone_scene_import.py
rename to simulation/isaac-sim/launch_scripts/example_multi_drone_scene_import.py
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
index ac277933..f64ad2a7 100644
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
@@ -78,13 +78,15 @@
 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": 20.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": 17.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  = 4.0     # Source-image density. Bump for sharper texture.
+OVERHEAD_PX_PER_METER  = 12.0     # Source-image density. Bump for sharper texture.
 OVERHEAD_TOPIC         = "/sim/overhead/image"
 OVERHEAD_SPEC_TOPIC    = "/sim/overhead/spec"
 OVERHEAD_FRAME_ID      = "map"

From 432e423bd76147120d5296d05c9037204cc92b72 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 30 Apr 2026 19:21:10 -0400
Subject: [PATCH 51/55] merged with main

---
 .env                                        |   4 ++--
 scenes/fire_academy_faro_with_sky.scene.usd | Bin 24731 -> 0 bytes
 2 files changed, 2 insertions(+), 2 deletions(-)
 delete mode 100644 scenes/fire_academy_faro_with_sky.scene.usd

diff --git a/.env b/.env
index 2aff3d9e..63c6f910 100644
--- a/.env
+++ b/.env
@@ -12,7 +12,7 @@ PROJECT_NAME="airstack"
 # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made 
 # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version.
 # auto-generated from git commit hash
-VERSION="0.18.0-alpha.8"
+VERSION="0.18.0-alpha.9"
 # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image
 DOCKER_IMAGE_BUILD_MODE="dev"  
 # Where to push and pull images from. Can replace with your docker hub username if using docker hub.
@@ -25,7 +25,7 @@ COMPOSE_PROFILES="desktop,isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="4"  # Number of robot containers to launch.
+NUM_ROBOTS="1"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
diff --git a/scenes/fire_academy_faro_with_sky.scene.usd b/scenes/fire_academy_faro_with_sky.scene.usd
deleted file mode 100644
index 622e7a360040a1320b8e5675bd7f2f475e36ddea..0000000000000000000000000000000000000000
GIT binary patch

From ed3cacfc9a4f6ccdcfb7121720e0a159d136061a Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 30 Apr 2026 19:54:41 -0400
Subject: [PATCH 52/55] PR cleanup: revert unrelated changes and drop extra
 files

- Restore main's robot.rviz (drop redundant robot_1/robot_2.rviz)
- Restore ms-airsim include in root docker-compose.yaml
- Restore airsim sections in docs/simulation/index.md
- Restore docs/gcs/docker/index.md (VERSION env name)
- Restore robot/docker/{.bashrc, Dockerfile.robot} to main
- Restore SIM_IP in robot/docker/docker-compose.yaml
- Restore takeoff_landing_planner takeoff_height: 8.0
- Drop docs/action_bridging.md (internal design memo)
- Drop personal launch scripts (two_drone_fire*, three_drone_scene_import, two_drone_RetroNeighbourhood)
- Trim verbose comments in gps_utils.py and example_multi_drone_scene_import.py
---
 .../desktop_bringup/rviz/robot_1.rviz         | 1112 -----------------
 .../desktop_bringup/rviz/robot_2.rviz         | 1073 ----------------
 docker-compose.yaml                           |    1 +
 docs/action_bridging.md                       |   58 -
 docs/gcs/docker/index.md                      |    2 +-
 docs/simulation/index.md                      |   21 +
 robot/docker/.bashrc                          |   12 +-
 robot/docker/Dockerfile.robot                 |   11 +-
 robot/docker/docker-compose.yaml              |    1 +
 .../config/takeoff_landing_planner.yaml       |    2 +-
 .../example_multi_drone_scene_import.py       |   63 +-
 .../isaac-sim/launch_scripts/gps_utils.py     |   72 +-
 .../three_drone_scene_import.py               |  229 ----
 .../two_drone_RetroNeighbourhood.py           |  284 -----
 .../launch_scripts/two_drone_fire.py          |  279 -----
 .../launch_scripts/two_drone_fire_academy.py  |  303 -----
 16 files changed, 65 insertions(+), 3458 deletions(-)
 delete mode 100644 common/ros_packages/desktop_bringup/rviz/robot_1.rviz
 delete mode 100644 common/ros_packages/desktop_bringup/rviz/robot_2.rviz
 delete mode 100644 docs/action_bridging.md
 delete mode 100644 simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
 delete mode 100644 simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
 delete mode 100644 simulation/isaac-sim/launch_scripts/two_drone_fire.py
 delete mode 100644 simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py

diff --git a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz b/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
deleted file mode 100644
index 955e9351..00000000
--- a/common/ros_packages/desktop_bringup/rviz/robot_1.rviz
+++ /dev/null
@@ -1,1112 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 70
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /TF1/Frames1
-        - /RobotModel1
-        - /Sensors1
-        - /Sensors1/Lidar1
-        - /Perception1
-        - /Local1
-        - /Local1/DROAN1
-        - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
-        - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
-        - /Global1
-        - /Global1/Global Plan1
-        - /PointCloud21
-        - /PointCloud22
-        - /PointCloud23
-        - /MarkerArray1
-        - /MarkerArray1/Topic1
-        - /MarkerArray2
-        - /MarkerArray2/Topic1
-        - /Odometry1
-        - /MarkerArray3
-        - /MarkerArray4
-        - /MarkerArray4/Topic1
-      Splitter Ratio: 0.590062141418457
-    Tree Height: 373
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz_common/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-  - Class: rviz_behavior_tree_panel::BehaviorTreePanel
-    Name: BehaviorTreePanel
-    topic: /robot_1/behavior/behavior_tree_graphviz
-    zoom_factor: 0.1919851303100586
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 100
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        OS1_REV6_128_10hz___512_resolution:
-          Value: false
-        base_link:
-          Value: true
-        base_link_ZED_X:
-          Value: false
-        base_link_body_body_link:
-          Value: false
-        base_link_frd:
-          Value: true
-        base_link_stabilized:
-          Value: false
-        camera_left:
-          Value: true
-        camera_right:
-          Value: true
-        imu:
-          Value: false
-        lidar:
-          Value: false
-        look_ahead_point:
-          Value: false
-        look_ahead_point_stabilized:
-          Value: false
-        macvo_ned:
-          Value: false
-        map:
-          Value: true
-        map_ned:
-          Value: false
-        odom:
-          Value: true
-        odom_ned:
-          Value: true
-        rotor0:
-          Value: true
-        rotor1:
-          Value: true
-        rotor2:
-          Value: true
-        rotor3:
-          Value: true
-        tracking_point:
-          Value: false
-        tracking_point_stabilized:
-          Value: false
-        world:
-          Value: false
-      Marker Scale: 0.30000001192092896
-      Name: TF
-      Show Arrows: false
-      Show Axes: true
-      Show Names: true
-      Tree:
-        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:
-                    {}
-                rotor0:
-                  {}
-                rotor1:
-                  {}
-                rotor2:
-                  {}
-                rotor3:
-                  {}
-              base_link_frd:
-                {}
-            base_link_stabilized:
-              {}
-            look_ahead_point:
-              {}
-            look_ahead_point_stabilized:
-              {}
-            map_ned:
-              {}
-            tracking_point:
-              {}
-            tracking_point_stabilized:
-              {}
-      Update Interval: 0
-      Value: true
-    - Alpha: 0.5
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        OS1_REV6_128_10hz___512_resolution:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        base_link_ZED_X:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link_body_body_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        camera_left:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_right:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        lidar:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rotor0:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rotor1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rotor2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rotor3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Image
-          Enabled: true
-          Max Value: 1
-          Median window: 5
-          Min Value: 0
-          Name: Front Right RGB
-          Normalize Range: true
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/sensors/front_stereo/right/image_rect
-          Value: true
-        - Class: rviz_default_plugins/Image
-          Enabled: false
-          Max Value: 100
-          Median window: 5
-          Min Value: 0
-          Name: Front Right Depth
-          Normalize Range: false
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: sensors/front_stereo/right/depth_ground_truth
-          Value: false
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 4.254574298858643
-            Min Value: -0.06456756591796875
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud2
-          Color: 170; 170; 255
-          Color Transformer: AxisColor
-          Decay Time: 1
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 4096
-          Min Color: 0; 0; 0
-          Min Intensity: 0
-          Name: Lidar
-          Position Transformer: XYZ
-          Selectable: true
-          Size (Pixels): 5
-          Size (m): 0.009999999776482582
-          Style: Points
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: sensors/lidar/point_cloud
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-        - Angle Tolerance: 0
-          Class: rviz_default_plugins/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: false
-          Keep: 1
-          Name: Odometry
-          Position Tolerance: 0
-          Shape:
-            Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.10000000149011612
-            Color: 255; 25; 0
-            Head Length: 0.30000001192092896
-            Head Radius: 0.10000000149011612
-            Shaft Length: 1
-            Shaft Radius: 0.05000000074505806
-            Value: Axes
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: odometry_conversion/odometry
-          Value: false
-        - Class: rviz_default_plugins/Image
-          Enabled: false
-          Max Value: 100
-          Median window: 5
-          Min Value: 0
-          Name: Front Right Depth
-          Normalize Range: false
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/sensors/front_stereo/right/depth_ground_truth
-          Value: false
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 18.48733139038086
-            Min Value: -5.648834705352783
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud2
-          Color: 170; 170; 255
-          Color Transformer: RGB8
-          Decay Time: 1
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 4096
-          Min Color: 0; 0; 0
-          Min Intensity: 0
-          Name: Disparity
-          Position Transformer: XYZ
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.10000000149011612
-          Style: Points
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/sensors/front_stereo/pointcloud
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-      Enabled: true
-      Name: Sensors
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Image
-          Enabled: true
-          Max Value: 1
-          Median window: 5
-          Min Value: 0
-          Name: MACVO Disparity
-          Normalize Range: true
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/perception/macvo/disparity
-          Value: true
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud
-          Color: 255; 255; 255
-          Color Transformer: RGBF32
-          Decay Time: 5
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 4096
-          Min Color: 0; 0; 0
-          Min Intensity: 0
-          Name: MACVO PointCloud
-          Position Transformer: XYZ
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.009999999776482582
-          Style: Flat Squares
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/perception/macvo/point_cloud
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-        - Angle Tolerance: 0.10000000149011612
-          Class: rviz_default_plugins/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: true
-          Keep: 100
-          Name: MACVO Odometry
-          Position Tolerance: 0.10000000149011612
-          Shape:
-            Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.10000000149011612
-            Color: 255; 25; 0
-            Head Length: 0.30000001192092896
-            Head Radius: 0.10000000149011612
-            Shaft Length: 1
-            Shaft Radius: 0.05000000074505806
-            Value: Arrow
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/macvo/odometry
-          Value: true
-      Enabled: false
-      Name: Perception
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_common/Group
-          Displays:
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Trimmed Global Plan for DROAN
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: droan/local_planner_global_plan_vis
-              Value: true
-            - Class: rviz_common/Group
-              Displays:
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: false
-                  Name: ExpansionPoly
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: droan/expansion_poly
-                  Value: false
-                - Class: rviz_default_plugins/Marker
-                  Enabled: false
-                  Name: Disparity Frustum
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    Filter size: 10
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_1/droan/frustum
-                  Value: false
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: false
-                  Name: Disparity Map Collision Checking
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_1/droan/disparity_map_debug
-                  Value: false
-                - Alpha: 1
-                  Autocompute Intensity Bounds: true
-                  Autocompute Value Bounds:
-                    Max Value: 10
-                    Min Value: -10
-                    Value: true
-                  Axis: Z
-                  Channel Name: intensity
-                  Class: rviz_default_plugins/PointCloud2
-                  Color: 255; 255; 255
-                  Color Transformer: Intensity
-                  Decay Time: 0
-                  Enabled: false
-                  Invert Rainbow: false
-                  Max Color: 255; 255; 255
-                  Max Intensity: 220
-                  Min Color: 0; 0; 0
-                  Min Intensity: 120
-                  Name: Expansion Cloud
-                  Position Transformer: XYZ
-                  Selectable: true
-                  Size (Pixels): 3
-                  Size (m): 0.009999999776482582
-                  Style: Flat Squares
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    Filter size: 10
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: droan/expansion_cloud
-                  Use Fixed Frame: true
-                  Use rainbow: true
-                  Value: false
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: false
-                  Name: Traj Library
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: droan/trajectory_library_vis
-                  Value: false
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: true
-                  Name: Disparity Graph Poses
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_1/droan/disparity_graph
-                  Value: true
-              Enabled: false
-              Name: Droan CPU
-            - Class: rviz_common/Group
-              Displays:
-                - Alpha: 1
-                  Autocompute Intensity Bounds: true
-                  Autocompute Value Bounds:
-                    Max Value: 10
-                    Min Value: -10
-                    Value: true
-                  Axis: Z
-                  Channel Name: intensity
-                  Class: rviz_default_plugins/PointCloud2
-                  Color: 255; 255; 255
-                  Color Transformer: Intensity
-                  Decay Time: 0
-                  Enabled: true
-                  Invert Rainbow: false
-                  Max Color: 255; 255; 255
-                  Max Intensity: 45
-                  Min Color: 0; 0; 0
-                  Min Intensity: 0
-                  Name: Foreground Background Cloud
-                  Position Transformer: XYZ
-                  Selectable: true
-                  Size (Pixels): 3
-                  Size (m): 0.009999999776482582
-                  Style: Points
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    Filter size: 10
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_1/droan/fg_bg_cloud
-                  Use Fixed Frame: true
-                  Use rainbow: false
-                  Value: true
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: true
-                  Name: Traj Debug
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_1/droan/traj_debug
-                  Value: true
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: true
-                  Name: Graph Vis
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_1/droan/graph_vis
-                  Value: true
-              Enabled: false
-              Name: Droan GPU
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Virtual Obstacles
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: droan/virtual_obstacles
-              Value: true
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Global Plan Vis
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: /robot_1/droan/local_planner_global_plan_vis
-              Value: true
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Rewind Info
-              Namespaces:
-                rewind_info: true
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: /robot_1/droan/rewind_info
-              Value: true
-          Enabled: true
-          Name: DROAN
-        - Class: rviz_common/Group
-          Displays:
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Traj Vis
-              Namespaces:
-                traj_controller: true
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: trajectory_controller/trajectory_vis
-              Value: true
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: false
-              Name: Traj Debug
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: trajectory_controller/trajectory_controller_debug_markers
-              Value: false
-          Enabled: true
-          Name: Trajectory Controller
-      Enabled: true
-      Name: Local
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Marker
-          Enabled: false
-          Name: VDB Mapping Marker
-          Namespaces:
-            {}
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: vdb_mapping/vdb_map_visualization
-          Value: false
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz_default_plugins/Path
-          Color: 0; 255; 255
-          Enabled: true
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Billboards
-          Line Width: 0.30000001192092896
-          Name: Global Plan
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_1/global_plan
-          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_1/sensors/front_stereo/left/image_rect
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 0
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.5
-      Style: Boxes
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/frontier_viewpoints
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz_default_plugins/Image
-      Enabled: false
-      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_1/sensors/front_stereo/left/image_rect
-      Value: false
-    - Class: rviz_default_plugins/Image
-      Enabled: false
-      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_1/sensors/front_stereo/left/image_rect
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.5
-      Style: Boxes
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/rayfronts/voxel_rgb
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.20000000298023224
-      Style: Points
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/rayfronts/frontiers
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: false
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/rayfronts/semantic_ray_frontiers
-      Value: false
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/filtered_rays
-      Value: true
-    - Angle Tolerance: 0.10000000149011612
-      Class: rviz_default_plugins/Odometry
-      Covariance:
-        Orientation:
-          Alpha: 0.5
-          Color: 255; 255; 127
-          Color Style: Unique
-          Frame: Local
-          Offset: 1
-          Scale: 1
-          Value: true
-        Position:
-          Alpha: 0.30000001192092896
-          Color: 204; 51; 204
-          Scale: 1
-          Value: true
-        Value: true
-      Enabled: true
-      Keep: 100
-      Name: Odometry
-      Position Tolerance: 0.10000000149011612
-      Shape:
-        Alpha: 1
-        Axes Length: 1
-        Axes Radius: 0.10000000149011612
-        Color: 255; 25; 0
-        Head Length: 0.30000001192092896
-        Head Radius: 0.10000000149011612
-        Shaft Length: 1
-        Shaft Radius: 0.05000000074505806
-        Value: Axes
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/relative_position
-      Value: true
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: false
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/rayfronts/semantic_ray_frontiers
-      Value: false
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/filtered_rays/transposed
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: map
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 30.03053092956543
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: -5.642820358276367
-        Y: -0.14288032054901123
-        Z: -2.2876358032226562
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5275139212608337
-      Target Frame: base_link
-      Value: Orbit (rviz)
-      Yaw: 6.182468891143799
-    Saved: ~
-Window Geometry:
-  BehaviorTreePanel:
-    collapsed: false
-  Displays:
-    collapsed: true
-  Front Right Depth:
-    collapsed: false
-  Front Right RGB:
-    collapsed: false
-  Height: 911
-  Hide Left Dock: true
-  Hide Right Dock: false
-  Image:
-    collapsed: false
-  MACVO Disparity:
-    collapsed: false
-  QMainWindow State: 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
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1930
-  X: 1510
-  Y: 631
diff --git a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz b/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
deleted file mode 100644
index 1b953c8e..00000000
--- a/common/ros_packages/desktop_bringup/rviz/robot_2.rviz
+++ /dev/null
@@ -1,1073 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 70
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /TF1/Frames1
-        - /RobotModel1
-        - /Sensors1
-        - /Sensors1/Lidar1
-        - /Perception1
-        - /Local1
-        - /Local1/DROAN1
-        - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
-        - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
-        - /Global1
-        - /Global1/Global Plan1
-        - /PointCloud21
-        - /PointCloud22
-        - /PointCloud23
-        - /MarkerArray1
-        - /MarkerArray1/Topic1
-        - /MarkerArray2
-        - /MarkerArray2/Topic1
-        - /MarkerArray3
-        - /MarkerArray3/Topic1
-        - /MarkerArray3/Namespaces1
-        - /MarkerArray4
-      Splitter Ratio: 0.590062141418457
-    Tree Height: 373
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz_common/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-  - Class: rviz_behavior_tree_panel::BehaviorTreePanel
-    Name: BehaviorTreePanel
-    topic: /robot_2/behavior/behavior_tree_graphviz
-    zoom_factor: 0.1919851303100586
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 100
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        OS1_REV6_128_10hz___512_resolution:
-          Value: false
-        base_link:
-          Value: true
-        base_link_ZED_X:
-          Value: false
-        base_link_body_body_link:
-          Value: false
-        base_link_frd:
-          Value: true
-        base_link_stabilized:
-          Value: false
-        camera_left:
-          Value: true
-        camera_right:
-          Value: true
-        imu:
-          Value: false
-        lidar:
-          Value: false
-        look_ahead_point:
-          Value: false
-        look_ahead_point_stabilized:
-          Value: false
-        macvo_ned:
-          Value: false
-        map:
-          Value: true
-        map_ned:
-          Value: false
-        odom:
-          Value: true
-        odom_ned:
-          Value: true
-        rotor0:
-          Value: true
-        rotor1:
-          Value: true
-        rotor2:
-          Value: true
-        rotor3:
-          Value: true
-        tracking_point:
-          Value: false
-        tracking_point_stabilized:
-          Value: false
-        world:
-          Value: false
-      Marker Scale: 0.30000001192092896
-      Name: TF
-      Show Arrows: false
-      Show Axes: true
-      Show Names: true
-      Tree:
-        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:
-                    {}
-                rotor0:
-                  {}
-                rotor1:
-                  {}
-                rotor2:
-                  {}
-                rotor3:
-                  {}
-              base_link_frd:
-                {}
-            base_link_stabilized:
-              {}
-            look_ahead_point:
-              {}
-            look_ahead_point_stabilized:
-              {}
-            map_ned:
-              {}
-            tracking_point:
-              {}
-            tracking_point_stabilized:
-              {}
-      Update Interval: 0
-      Value: true
-    - Alpha: 0.5
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        OS1_REV6_128_10hz___512_resolution:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        base_link_ZED_X:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link_body_body_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        camera_left:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_right:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        lidar:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rotor0:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rotor1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rotor2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rotor3:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Image
-          Enabled: true
-          Max Value: 1
-          Median window: 5
-          Min Value: 0
-          Name: Front Right RGB
-          Normalize Range: true
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/sensors/front_stereo/right/image_rect
-          Value: true
-        - Class: rviz_default_plugins/Image
-          Enabled: false
-          Max Value: 100
-          Median window: 5
-          Min Value: 0
-          Name: Front Right Depth
-          Normalize Range: false
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: sensors/front_stereo/right/depth_ground_truth
-          Value: false
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 4.254574298858643
-            Min Value: -0.06456756591796875
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud2
-          Color: 170; 170; 255
-          Color Transformer: AxisColor
-          Decay Time: 1
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 4096
-          Min Color: 0; 0; 0
-          Min Intensity: 0
-          Name: Lidar
-          Position Transformer: XYZ
-          Selectable: true
-          Size (Pixels): 5
-          Size (m): 0.009999999776482582
-          Style: Points
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: sensors/lidar/point_cloud
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-        - Angle Tolerance: 0
-          Class: rviz_default_plugins/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: false
-          Keep: 1
-          Name: Odometry
-          Position Tolerance: 0
-          Shape:
-            Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.10000000149011612
-            Color: 255; 25; 0
-            Head Length: 0.30000001192092896
-            Head Radius: 0.10000000149011612
-            Shaft Length: 1
-            Shaft Radius: 0.05000000074505806
-            Value: Axes
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: odometry_conversion/odometry
-          Value: false
-        - Class: rviz_default_plugins/Image
-          Enabled: false
-          Max Value: 100
-          Median window: 5
-          Min Value: 0
-          Name: Front Right Depth
-          Normalize Range: false
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/sensors/front_stereo/right/depth_ground_truth
-          Value: false
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 18.48733139038086
-            Min Value: -5.648834705352783
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud2
-          Color: 170; 170; 255
-          Color Transformer: RGB8
-          Decay Time: 1
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 4096
-          Min Color: 0; 0; 0
-          Min Intensity: 0
-          Name: Disparity
-          Position Transformer: XYZ
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.10000000149011612
-          Style: Points
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/sensors/front_stereo/pointcloud
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-      Enabled: true
-      Name: Sensors
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Image
-          Enabled: true
-          Max Value: 1
-          Median window: 5
-          Min Value: 0
-          Name: MACVO Disparity
-          Normalize Range: true
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/perception/macvo/disparity
-          Value: true
-        - Alpha: 1
-          Autocompute Intensity Bounds: true
-          Autocompute Value Bounds:
-            Max Value: 10
-            Min Value: -10
-            Value: true
-          Axis: Z
-          Channel Name: intensity
-          Class: rviz_default_plugins/PointCloud
-          Color: 255; 255; 255
-          Color Transformer: RGBF32
-          Decay Time: 5
-          Enabled: false
-          Invert Rainbow: false
-          Max Color: 255; 255; 255
-          Max Intensity: 4096
-          Min Color: 0; 0; 0
-          Min Intensity: 0
-          Name: MACVO PointCloud
-          Position Transformer: XYZ
-          Selectable: true
-          Size (Pixels): 3
-          Size (m): 0.009999999776482582
-          Style: Flat Squares
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/perception/macvo/point_cloud
-          Use Fixed Frame: true
-          Use rainbow: true
-          Value: false
-        - Angle Tolerance: 0.10000000149011612
-          Class: rviz_default_plugins/Odometry
-          Covariance:
-            Orientation:
-              Alpha: 0.5
-              Color: 255; 255; 127
-              Color Style: Unique
-              Frame: Local
-              Offset: 1
-              Scale: 1
-              Value: true
-            Position:
-              Alpha: 0.30000001192092896
-              Color: 204; 51; 204
-              Scale: 1
-              Value: true
-            Value: true
-          Enabled: true
-          Keep: 100
-          Name: MACVO Odometry
-          Position Tolerance: 0.10000000149011612
-          Shape:
-            Alpha: 1
-            Axes Length: 1
-            Axes Radius: 0.10000000149011612
-            Color: 255; 25; 0
-            Head Length: 0.30000001192092896
-            Head Radius: 0.10000000149011612
-            Shaft Length: 1
-            Shaft Radius: 0.05000000074505806
-            Value: Arrow
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/macvo/odometry
-          Value: true
-      Enabled: false
-      Name: Perception
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_common/Group
-          Displays:
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Trimmed Global Plan for DROAN
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: droan/local_planner_global_plan_vis
-              Value: true
-            - Class: rviz_common/Group
-              Displays:
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: false
-                  Name: ExpansionPoly
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: droan/expansion_poly
-                  Value: false
-                - Class: rviz_default_plugins/Marker
-                  Enabled: false
-                  Name: Disparity Frustum
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    Filter size: 10
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_2/droan/frustum
-                  Value: false
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: false
-                  Name: Disparity Map Collision Checking
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_2/droan/disparity_map_debug
-                  Value: false
-                - Alpha: 1
-                  Autocompute Intensity Bounds: true
-                  Autocompute Value Bounds:
-                    Max Value: 10
-                    Min Value: -10
-                    Value: true
-                  Axis: Z
-                  Channel Name: intensity
-                  Class: rviz_default_plugins/PointCloud2
-                  Color: 255; 255; 255
-                  Color Transformer: Intensity
-                  Decay Time: 0
-                  Enabled: false
-                  Invert Rainbow: false
-                  Max Color: 255; 255; 255
-                  Max Intensity: 220
-                  Min Color: 0; 0; 0
-                  Min Intensity: 120
-                  Name: Expansion Cloud
-                  Position Transformer: XYZ
-                  Selectable: true
-                  Size (Pixels): 3
-                  Size (m): 0.009999999776482582
-                  Style: Flat Squares
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    Filter size: 10
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: droan/expansion_cloud
-                  Use Fixed Frame: true
-                  Use rainbow: true
-                  Value: false
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: false
-                  Name: Traj Library
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: droan/trajectory_library_vis
-                  Value: false
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: true
-                  Name: Disparity Graph Poses
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_2/droan/disparity_graph
-                  Value: true
-              Enabled: false
-              Name: Droan CPU
-            - Class: rviz_common/Group
-              Displays:
-                - Alpha: 1
-                  Autocompute Intensity Bounds: true
-                  Autocompute Value Bounds:
-                    Max Value: 10
-                    Min Value: -10
-                    Value: true
-                  Axis: Z
-                  Channel Name: intensity
-                  Class: rviz_default_plugins/PointCloud2
-                  Color: 255; 255; 255
-                  Color Transformer: Intensity
-                  Decay Time: 0
-                  Enabled: true
-                  Invert Rainbow: false
-                  Max Color: 255; 255; 255
-                  Max Intensity: 45
-                  Min Color: 0; 0; 0
-                  Min Intensity: 0
-                  Name: Foreground Background Cloud
-                  Position Transformer: XYZ
-                  Selectable: true
-                  Size (Pixels): 3
-                  Size (m): 0.009999999776482582
-                  Style: Points
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    Filter size: 10
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_2/droan/fg_bg_cloud
-                  Use Fixed Frame: true
-                  Use rainbow: false
-                  Value: true
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: true
-                  Name: Traj Debug
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_2/droan/traj_debug
-                  Value: true
-                - Class: rviz_default_plugins/MarkerArray
-                  Enabled: true
-                  Name: Graph Vis
-                  Namespaces:
-                    {}
-                  Topic:
-                    Depth: 5
-                    Durability Policy: Volatile
-                    History Policy: Keep Last
-                    Reliability Policy: Reliable
-                    Value: /robot_2/droan/graph_vis
-                  Value: true
-              Enabled: false
-              Name: Droan GPU
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Virtual Obstacles
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: droan/virtual_obstacles
-              Value: true
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Global Plan Vis
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: /robot_2/droan/local_planner_global_plan_vis
-              Value: true
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Rewind Info
-              Namespaces:
-                rewind_info: true
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: /robot_2/droan/rewind_info
-              Value: true
-          Enabled: true
-          Name: DROAN
-        - Class: rviz_common/Group
-          Displays:
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: true
-              Name: Traj Vis
-              Namespaces:
-                traj_controller: true
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: trajectory_controller/trajectory_vis
-              Value: true
-            - Class: rviz_default_plugins/MarkerArray
-              Enabled: false
-              Name: Traj Debug
-              Namespaces:
-                {}
-              Topic:
-                Depth: 5
-                Durability Policy: Volatile
-                History Policy: Keep Last
-                Reliability Policy: Reliable
-                Value: trajectory_controller/trajectory_controller_debug_markers
-              Value: false
-          Enabled: true
-          Name: Trajectory Controller
-      Enabled: true
-      Name: Local
-    - Class: rviz_common/Group
-      Displays:
-        - Class: rviz_default_plugins/Marker
-          Enabled: false
-          Name: VDB Mapping Marker
-          Namespaces:
-            {}
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: vdb_mapping/vdb_map_visualization
-          Value: false
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz_default_plugins/Path
-          Color: 0; 255; 255
-          Enabled: true
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Billboards
-          Line Width: 0.30000001192092896
-          Name: Global Plan
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            Filter size: 10
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: /robot_2/global_plan
-          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
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 0
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.5
-      Style: Boxes
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/frontier_viewpoints
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz_default_plugins/Image
-      Enabled: false
-      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: false
-    - Class: rviz_default_plugins/Image
-      Enabled: false
-      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: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.5
-      Style: Boxes
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/rayfronts/voxel_rgb
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: RGB8
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.20000000298023224
-      Style: Points
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/rayfronts/frontiers
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: false
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/rayfronts/semantic_ray_frontiers
-      Value: false
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_2/filtered_rays
-      Value: true
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/filtered_rays/transposed
-      Value: true
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_1/filtered_rays
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: map
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 26.823678970336914
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: -8.390859603881836
-        Y: 3.887568235397339
-        Z: 0.3204301595687866
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.23979705572128296
-      Target Frame: base_link
-      Value: Orbit (rviz)
-      Yaw: 6.241737365722656
-    Saved: ~
-Window Geometry:
-  BehaviorTreePanel:
-    collapsed: false
-  Displays:
-    collapsed: true
-  Front Right Depth:
-    collapsed: false
-  Front Right RGB:
-    collapsed: false
-  Height: 1122
-  Hide Left Dock: true
-  Hide Right Dock: false
-  Image:
-    collapsed: false
-  MACVO Disparity:
-    collapsed: false
-  QMainWindow State: 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
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1931
-  X: 1509
-  Y: 27
diff --git a/docker-compose.yaml b/docker-compose.yaml
index e2e26c03..9511e696 100644
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -6,6 +6,7 @@
 include:
   - simulation/isaac-sim/docker/docker-compose.yaml
   - simulation/simple-sim/docker/docker-compose.yaml
+  - simulation/ms-airsim/docker/docker-compose.yaml
   - robot/docker/docker-compose.yaml
   - gcs/docker/docker-compose.yaml
   - docs/docker/docker-compose.yaml
diff --git a/docs/action_bridging.md b/docs/action_bridging.md
deleted file mode 100644
index a85e3192..00000000
--- a/docs/action_bridging.md
+++ /dev/null
@@ -1,58 +0,0 @@
-# ROS 2 action bridging across DDS domains
-
-*Summary for AirStack team discussion.*
-
-## Problem
-
-Each robot runs on its own `ROS_DOMAIN_ID` (robot_1 → 1, robot_2 → 2, …); the GCS runs on domain 0. We bridge with eProsima **`ddsrouter`**. Topics work. **Actions don't.**
-
-From the GCS container:
-- `ros2 service list --include-hidden-services -t` shows `/robot_1/tasks/takeoff/_action/send_goal` — name + type are bridged.
-- `ros2 action info /robot_1/tasks/takeoff` → `Action servers: 0`.
-- `ros2 action send_goal` hangs; Foxglove reports "Service … is not available".
-
-### Root cause
-
-`ddsrouter` bridges the DDS `rq/…Request` / `rr/…Reply` topics underneath a ROS 2 service, but every ROS 2 action client calls `rcl_service_server_is_available()` first. That walks the RMW endpoint graph for a **matched DDS server signature**; ddsrouter's proxy endpoints don't match, so the call aborts. It's an RMW endpoint-matching problem, not a naming/visibility problem.
-
-Same symptom reported in the wild: [RTI community — "ROS2 Router: Service Call Request made, but no Response"](https://community.rti.com/forum-topic/ros2-router-service-call-request-made-no-response).
-
-### "Are actions failing because they're hidden? If we unhide them, will it work?"
-
-No.
-- `_action/*` naming is a **display-only filter** in ROS CLI tools; DDS doesn't care ([ROS 2 Actions Design](https://design.ros2.org/articles/actions.html)).
-- Foxglove already runs with `include_hidden: true` — the services do appear, the liveliness check still fails.
-- The `_action/*` prefix is hardcoded in `rclcpp_action::create_server()`; unhiding would require forking rclcpp and wouldn't change DDS behavior.
-
----
-
-## Options
-
-### A — Swap ddsrouter for `zenoh_bridge_ros2dds`
-Replace the bridging layer with Eclipse Zenoh's ROS 2 plugin. Zenoh represents services/actions as **Queryables** and side-steps the DDS liveliness-matching problem.
-- **Pros:** native topic + service + action bridging; actively developed; OSRF's current recommendation for distributed ROS 2; same config works for same-host Docker and remote robots.
-- **Cons:** rip-and-replace of the whole bridging layer (compose + launch); new dep (`ros-jazzy-zenoh-bridge-ros2dds`); team has to learn it; cross-machine deploys need explicit `connect:` peer config.
-- **Refs:** [zenoh-plugin-ros2dds](https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds) · [Jazzy docs](https://docs.ros.org/en/jazzy/p/zenoh_bridge_dds/) · [OSRF discourse](https://discourse.openrobotics.org/t/new-zenoh-bridge-for-ros-2/34163) · [Autoware writeup](https://autoware.org/driving-autoware-with-zenoh/) · [Humble tutorial](https://medium.com/@piliwilliam0306/use-zenoh-bridge-ros2dds-with-ros2-humble-459ab70ce9c7)
-
-### B — Add `ros2/domain_bridge` alongside ddsrouter
-Keep ddsrouter for topics; run a per-robot `domain_bridge` with an `actions:` block. Supposed to create a real `rclcpp_action::Server` on domain 0 that forwards to the robot.
-- **We tried this in-session.** Bridge ran, config interpolated, but `ros2 action info` still showed `Action servers: 0`. Reverted.
-- **Pros:** smallest config change, no new protocol.
-- **Cons:** action support is a [4-year-old open issue (#11)](https://github.com/ros2/domain_bridge/issues/11); services support similarly thin ([#10](https://github.com/ros2/domain_bridge/issues/10)); [design doc](https://github.com/ros2/domain_bridge/blob/main/doc/design.md) admits QoS/liveliness limits; silent failure mode makes it hard to debug.
-- **Refs:** [ros2/domain_bridge](https://github.com/ros2/domain_bridge) · [#11](https://github.com/ros2/domain_bridge/issues/11) · [#10](https://github.com/ros2/domain_bridge/issues/10) · [design doc](https://github.com/ros2/domain_bridge/blob/main/doc/design.md)
-
-### C — Custom per-robot action relay node
-One new ROS 2 package (`action_relay`). Per robot, one binary with two `rclcpp::Context` objects: a real `rclcpp_action::Server` on domain 0 and a real `rclcpp_action::Client` on domain N. Server forwards goals to client; feedback/result/cancel propagate through. ~150 lines of C++, templated over the 5 action types.
-- **Pros:** bulletproof — it *is* a real server on domain 0, so the liveliness check trivially passes; no new deps; additive (ddsrouter still handles topics/images); scales per-robot.
-- **Cons:** we maintain the code; new task executor → one line in a registry; careful two-executor / two-context threading; no widely-used package to vendor.
-- **Refs:** [ROS 2 Actions Design](https://design.ros2.org/articles/actions.html) · [rclcpp_action API](https://docs.ros.org/en/jazzy/p/rclcpp_action/) · [multi_robot_tf_relay (related pattern)](https://github.com/Gabryss/multi_robot_tf_relay) · [Jazzy action server tutorial](https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Writing-an-Action-Server-Client/Cpp.html)
-
----
-
-## Related reading
-
-- [eProsima DDS Router docs](https://eprosima-dds-router.readthedocs.io/) — what we use today
-- [ROS on DDS (design)](https://design.ros2.org/articles/ros_on_dds.html) — why services/actions use request/reply DDS topics
-- [ROS 2 QoS — Deadline, Liveliness, Lifespan](https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html) — explains `rcl_service_server_is_available` semantics
-- [Programming Multiple Robots with ROS 2 (OSRF)](https://osrf.github.io/ros2multirobotbook/) — currently recommends Zenoh for cross-domain
-- [Husarnet — Bridge Remote DDS Networks](https://husarnet.com/blog/ros2-dds-router)
diff --git a/docs/gcs/docker/index.md b/docs/gcs/docker/index.md
index 47c721c6..4d94181f 100644
--- a/docs/gcs/docker/index.md
+++ b/docs/gcs/docker/index.md
@@ -76,7 +76,7 @@ Key variables for GCS configuration:
 
 | Variable | Description | Default |
 |----------|-------------|---------|
-| `DOCKER_IMAGE_TAG` | Image version tag | Set in `.env` |
+| `VERSION` | Image version tag | Set in `.env` |
 | `DOCKER_IMAGE_BUILD_MODE` | Build mode (release/dev) | `dev` |
 | `AUTOLAUNCH` | Auto-start GCS on container launch | `true` |
 | `ROS_DOMAIN_ID` | ROS 2 domain (0 for GCS) | `0` |
diff --git a/docs/simulation/index.md b/docs/simulation/index.md
index 0a3aa935..6927aaa4 100644
--- a/docs/simulation/index.md
+++ b/docs/simulation/index.md
@@ -17,6 +17,12 @@ simulation/
 │   ├── extensions/                # Custom Isaac Sim extensions
 │   ├── launch_scripts/            # Python launch scripts
 │   └── standalone_examples/       # Example scenes and scripts
+├── ms-airsim/
+│   ├── docker/                    # Microsoft AirSim (legacy) containerization
+│   │   ├── docker-compose.yaml    # Launch configuration
+│   │   └── Dockerfile             # Image definition
+│   ├── config/                    # Microsoft AirSim (legacy) settings.json
+│   └── ros_ws/                    # Depth bridge ROS package
 └── simple-sim/
     ├── docker/                    # Simple simulator container
     │   └── docker-compose.yaml    # Launch configuration
@@ -29,6 +35,7 @@ simulation/
 Simulation components are launched via Docker Compose. Each simulator has its own configuration:
 
 - **Isaac Sim:** `simulation/isaac-sim/docker/docker-compose.yaml`
+- **Microsoft AirSim (legacy):** `simulation/ms-airsim/docker/docker-compose.yaml`
 - **Simple Sim:** `simulation/simple-sim/docker/docker-compose.yaml`
 
 **Key launch points:**
@@ -69,6 +76,20 @@ Isaac Sim is our primary simulation platform, offering:
 - [Ascent SITL Extension](isaac_sim/ascent_sitl_extension.md)
 - [Export from Unreal Engine](isaac_sim/export_stages_from_unreal.md)
 
+### Microsoft AirSim (legacy) (Unreal Engine)
+
+An open-source drone simulator built on Unreal Engine with native PX4 SITL integration.
+
+**Use cases:**
+
+- PX4-in-the-loop testing with photorealistic environments
+- Depth-based obstacle avoidance testing (DROAN)
+- Environments from the Unreal Engine ecosystem
+
+**Launch:** `airstack up --env-file overrides/ms-airsim.env`
+
+**Location:** `simulation/ms-airsim/`
+
 ### Simple Sim (Lightweight)
 
 A lightweight 2D/3D simulator for basic testing and development when full Isaac Sim fidelity isn't needed.
diff --git a/robot/docker/.bashrc b/robot/docker/.bashrc
index 20c01270..d2c3c877 100755
--- a/robot/docker/.bashrc
+++ b/robot/docker/.bashrc
@@ -17,8 +17,10 @@ export PYTHONWARNINGS
 # Convenience functions for ROS2 workspace
 
 function bws(){
+    # Since multiple robot containers may mount a shared volume, we need to use flock to prevent race conditions.
     echo "Running \`colcon build $@\` in $ROS2_WS_DIR"
-    COLCON_LOG_PATH="$ROS2_WS_DIR"/log colcon build --symlink-install --base-paths "$ROS2_WS_DIR"/ --build-base "$ROS2_WS_DIR"/build/ --install-base "$ROS2_WS_DIR"/install/ "$@"
+    COLCON_LOG_PATH="$ROS2_WS_DIR"/log flock "$ROS2_WS_DIR/.build.lock" \
+        colcon build --symlink-install --base-paths "$ROS2_WS_DIR"/ --build-base "$ROS2_WS_DIR"/build/ --install-base "$ROS2_WS_DIR"/install/ "$@"
 }
 function sws(){
     if [ -f "$ROS2_WS_DIR/install/local_setup.bash" ]; then
@@ -86,16 +88,10 @@ if [ -n "$name_to_map" ]; then
     script_path="$HOME/AirStack/robot/docker/robot_name_map/resolve_robot_name.py"
     script_dir=$(dirname "$script_path")
 
-    existing_robot_name=${ROBOT_NAME:-}
     existing_robot_domain_id=${ROS_DOMAIN_ID:-}
 
     eval "$($script_path $name_to_map $script_dir/$ROBOT_NAME_MAP_CONFIG_FILE)"
-    # if ROBOT_NAME was already set in the environment, use it instead of mapped value
-    if [ -n "$existing_robot_name" ]; then
-        export ROBOT_NAME=$existing_robot_name
-    else
-        export ROBOT_NAME
-    fi
+    export ROBOT_NAME
 
     # if ROS_DOMAIN_ID was already set in the environment, use that instead of the mapped value
     if [ -z "$existing_robot_domain_id" ]; then
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index 43233e85..55b1ad40 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -154,8 +154,11 @@ RUN pip3 install --break-system-packages --ignore-installed \
 
 # Install MACVO Python dependencies (skipped if SKIP_MACVO=true)
 RUN if [ "${SKIP_MACVO}" != "true" ]; then \
-    pip3 install --break-system-packages \
-      torch torchvision onnx tensorrt; \
+  pip3 install --break-system-packages \
+  torch \
+  torchvision \
+  onnx \
+  tensorrt; \
   fi
 
 # Downloading model weights for MACVO (skipped if SKIP_MACVO=true)
@@ -171,7 +174,8 @@ RUN if [ "${SKIP_MACVO}" != "true" ]; then \
 
 # Fixes for MACVO Integration (skipped if SKIP_MACVO=true)
 RUN if [ "${SKIP_MACVO}" != "true" ]; then \
-  pip install --break-system-packages huggingface_hub; \
+  pip install --break-system-packages huggingface_hub && \
+  pip uninstall --break-system-packages matplotlib -y; \
   fi
 
 # TMux config
@@ -352,3 +356,4 @@ RUN echo 'root:airstack' | chpasswd \
 EXPOSE 22
 
 WORKDIR /root/AirStack/robot/ros_ws
+
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index 888fa2ef..a825dbea 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -25,6 +25,7 @@ services:
       - NVIDIA_DRIVER_CAPABILITIES=all
       - LAUNCH_PACKAGE=desktop_bringup  # desktop_bringup adds RViz; real robots use autonomy_bringup
       - AUTONOMY_ROLE=full
+      - SIM_IP=${SIM_IP:-172.31.0.200}
       # FCU_URL and TGT_SYSTEM not set, dynamically calculated in interface.launch.py
     # 'command' uses variables so that it can be shared across robot-desktop and robot-l4t, with different launch packages and roles.
     command: >
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml
index 557efa3c..1a7fc8ed 100644
--- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml
@@ -1,6 +1,6 @@
 /**:
   ros__parameters:
-    takeoff_height: 5.0
+    takeoff_height: 8.0
     high_takeoff_height: 8.0
     takeoff_velocity: 1.0
     landing_velocity: 0.3
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 28f8ca8a..38ac7619 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
@@ -3,7 +3,7 @@
 import carb
 from isaacsim import SimulationApp
 
-# Start Isaac Sim's simulation environment (Must start this before importing omni modules)
+# SimulationApp must be created before importing any omni modules.
 simulation_app = SimulationApp({"headless": False})
 
 import os
@@ -17,13 +17,11 @@
 
 from omni.isaac.core.world import World
 
-# 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
 
-# gps_utils lives in the same directory as this script
 _LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
 if _LAUNCH_SCRIPTS_DIR not in sys.path:
     sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
@@ -34,59 +32,42 @@
 from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
 
 
-# --------------------- 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"
 ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/ACFA/FireAcademyFaro/fire_academy_fixed_textures.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"
 
-# Lighting
 ADD_DOME_LIGHT = True
 DOME_LIGHT_PATH = "/World/DomeLight"
 DOME_LIGHT_INTENSITY = 3500.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.
 WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
 
-# Drone spawn configs.
-# x_m = East offset from world origin (meters)
-# y_m = North offset from world origin (meters)
-# z_m = Up offset / height above floor (meters)
-# orient = initial quaternion [x, y, z, w]
 SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
 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.0, 1.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.0, 1.0]},
+    {"domain_id": 1, "x_m": 27.0, "y_m": 7.6, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.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.0, 1.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
+    "omni.graph.core",
+    "omni.graph.action",
+    "omni.graph.action_nodes",
+    "omni.graph.ui",
+    "omni.graph.visualization.nodes",
+    "omni.graph.scriptnode",
+    "omni.graph.window.action",
+    "omni.graph.window.generic",
+    "omni.graph.ui_nodes",
+    "airlab.pegasus",
     "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:
@@ -99,7 +80,6 @@ def nucleus_stat(url: str) -> bool:
 
 
 def wait_for_stage(stage, timeout_s: float = 10.0):
-    """Pump the Kit app loop until /World has content (scene fully loaded)."""
     for _ in range(int(timeout_s / 0.1)):
         omni.kit.app.get_app().update()
         world_prim = stage.GetPrimAtPath("/World")
@@ -114,23 +94,19 @@ def wait_for_stage(stage, timeout_s: float = 10.0):
 class PegasusApp:
 
     def __init__(self):
-        # Write GPS origins immediately so robot containers can read them
-        # before this container finishes its heavy USD loading.
+        # Write GPS origins before USD loading so robot containers can read them
+        # while this container is still busy.
         set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
 
         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()
@@ -140,7 +116,6 @@ def __init__(self):
         if not wait_for_stage(stage):
             carb.log_warn("Stage load timed out — continuing anyway.")
 
-        # ----- Scene preparation -----
         stage_prim = stage.GetPrimAtPath("/World/stage")
         if stage_prim.IsValid():
             scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
@@ -153,10 +128,8 @@ def __init__(self):
         if ADD_DOME_LIGHT:
             add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
 
-        # Units
         mpu, s = get_stage_meters_per_unit(stage)
 
-        # Spawn all drones
         for cfg in DRONE_CONFIGS:
             i = cfg["domain_id"]
             pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
@@ -191,12 +164,10 @@ def __init__(self):
                 lidar_min_range=0.75,
             )
 
-        # Give physics time to register the spawned drone prims before reset.
-        # Complex scenes (e.g. fire academy) need more ticks than simpler ones.
+        # Complex scenes (e.g. fire academy) need ~30 ticks before reset.
         for _ in range(30):
             omni.kit.app.get_app().update()
 
-        # Reset so physics/articulations are ready
         self.world.reset()
 
         self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
@@ -208,7 +179,6 @@ def run(self):
         else:
             self.timeline.stop()
 
-        # Main loop
         while simulation_app.is_running() and not self.stop_sim:
             try:
                 self.world.step(render=True)
@@ -216,7 +186,6 @@ def run(self):
                 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()
diff --git a/simulation/isaac-sim/launch_scripts/gps_utils.py b/simulation/isaac-sim/launch_scripts/gps_utils.py
index d17e8516..f1b68393 100644
--- a/simulation/isaac-sim/launch_scripts/gps_utils.py
+++ b/simulation/isaac-sim/launch_scripts/gps_utils.py
@@ -1,48 +1,21 @@
-"""
-GPS origin utilities for AirStack multi-drone simulation.
-
-Computes per-robot GPS origins from Isaac Sim world-frame spawn positions and
-sets them as process environment variables so the OmniGraph PX4 node picks them
-up when building each vehicle's PX4MavlinkBackendConfig.
-
-Usage in a scene script:
-    from utils.gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
+"""GPS origin utilities for multi-drone simulation.
 
-    DRONE_CONFIGS = [
-        {"domain_id": 1, "x_m": -3.0, "y_m":  3.5, "z_m": 0.15, "orient": [0, 0, 0, 1]},
-        {"domain_id": 2, "x_m":  3.0, "y_m":  3.0, "z_m": 0.15, "orient": [0, 0, 0, 1]},
-    ]
-    set_gps_origins(DRONE_CONFIGS)
+Computes per-drone GPS origins from world-frame spawn positions and sets them
+as PX4_HOME_LAT_<id>/LON_<id>/ALT_<id> env vars before the Pegasus PX4 node
+spawns each vehicle.
 """
 
 import math
 import os
 
-# GPS coordinates that correspond to world origin (0, 0, 0) in the simulation.
-# Lisbon default matches the Pegasus configs.yaml default.
-DEFAULT_WORLD_ORIGIN = (38.736832, -9.137977, 90.0)  # (lat_deg, lon_deg, alt_m WGS-84) — Lisbon, matches Pegasus configs.yaml
+# Lisbon — matches the Pegasus configs.yaml default.
+DEFAULT_WORLD_ORIGIN = (38.736832, -9.137977, 90.0)
 
 
 def compute_gps_origin(x_m, y_m, z_m, world_origin=DEFAULT_WORLD_ORIGIN):
-    """
-    Convert a world-frame spawn position (meters) to a GPS geo_origin.
-
-    Axis convention assumed (Isaac Sim stage, Z-up ENU):
-        +X = East  →  affects longitude
-        +Y = North →  affects latitude
-        +Z = Up    →  affects altitude
-
-    NOTE: If drones appear shifted in the wrong compass direction after testing,
-    swap the x_m and y_m assignments in delta_lat / delta_lon below.
+    """Convert a world-frame spawn (+X=East, +Y=North, +Z=Up, metres) to (lat, lon, alt).
 
-    Args:
-        x_m: spawn X position in world-frame meters
-        y_m: spawn Y position in world-frame meters
-        z_m: spawn Z position in world-frame meters (height above world origin)
-        world_origin: (lat_deg, lon_deg, alt_m) that world (0,0,0) maps to
-
-    Returns:
-        (lat_deg, lon_deg, alt_m) — the GPS home position for this robot
+    Flat-Earth approximation; accurate at scene scales (a few hundred metres).
     """
     base_lat, base_lon, base_alt = world_origin
     delta_lat = y_m / 111320.0
@@ -51,38 +24,17 @@ def compute_gps_origin(x_m, y_m, z_m, world_origin=DEFAULT_WORLD_ORIGIN):
 
 
 def set_gps_origins(drone_configs, world_origin=DEFAULT_WORLD_ORIGIN):
-    """
-    Compute GPS origins for all drones and set them as process environment variables.
-
-    Called once at the start of a scene script before spawning vehicles.
-    Sets PX4_HOME_LAT_<domain_id>, PX4_HOME_LON_<domain_id>, PX4_HOME_ALT_<domain_id>
-    in the current process so the OmniGraph PX4 node reads them when building
-    PX4MavlinkBackendConfig, which passes them to PX4LaunchTool as PX4_HOME_LAT/LON/ALT
-    on the PX4 subprocess environment.
+    """Set PX4_HOME_*_<domain_id> for each drone in drone_configs.
 
-    Args:
-        drone_configs: list of dicts, one per drone:
-            [
-                {
-                    "domain_id": int,      # matches vehicle_id passed to spawn_px4_multirotor_node
-                    "x_m":       float,    # world-frame spawn East  (meters)
-                    "y_m":       float,    # world-frame spawn North (meters)
-                    "z_m":       float,    # world-frame spawn Up    (meters)
-                    "orient":    list,     # quaternion [x, y, z, w]
-                },
-                ...
-            ]
-        world_origin: (lat_deg, lon_deg, alt_m) for world (0, 0, 0).
+    Each entry needs ``domain_id``, ``x_m``, ``y_m``, ``z_m``. Call before
+    spawning vehicles so the Pegasus OmniGraph PX4 node picks the values up.
     """
     print(f"[gps_utils] World anchor: lat={world_origin[0]}, lon={world_origin[1]}, alt={world_origin[2]}")
 
     for cfg in drone_configs:
         domain_id = cfg["domain_id"]
         lat, lon, alt = compute_gps_origin(cfg["x_m"], cfg["y_m"], cfg["z_m"], world_origin)
-
         os.environ[f"PX4_HOME_LAT_{domain_id}"] = str(lat)
         os.environ[f"PX4_HOME_LON_{domain_id}"] = str(lon)
         os.environ[f"PX4_HOME_ALT_{domain_id}"] = str(alt)
-
-        print(f"[gps_utils]   domain_id={domain_id}: "
-              f"lat={lat:.7f}, lon={lon:.7f}, alt={alt:.2f}")
+        print(f"[gps_utils]   domain_id={domain_id}: lat={lat:.7f}, lon={lon:.7f}, alt={alt:.2f}")
diff --git a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py b/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
deleted file mode 100644
index 6a38e848..00000000
--- a/simulation/isaac-sim/launch_scripts/three_drone_scene_import.py
+++ /dev/null
@@ -1,229 +0,0 @@
-#!/usr/bin/env python3
-
-import sys
-import os
-
-import carb
-from isaacsim import SimulationApp
-
-# Start Isaac Sim's simulation environment (Must start this before importing omni modules)
-simulation_app = SimulationApp({"headless": False})
-
-# Set local Nucleus as asset root before importing Pegasus (which resolves it at import time)
-carb.settings.get_settings().set(
-    "/persistent/isaac/asset_root/default",
-    "omniverse://airlab-nucleus.andrew.cmu.edu/NVIDIA/Assets/Isaac/5.1"
-)
-
-import time
-
-import omni.kit.app
-import omni.timeline
-import omni.usd
-import omni.client
-
-from omni.isaac.core.world import World
-
-# 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
-
-# gps_utils lives in the same directory as this script
-_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
-if _LAUNCH_SCRIPTS_DIR not in sys.path:
-    sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
-from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
-
-sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
-import scene_prep
-from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
-
-
-# --------------------- 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"
-STAGE_SCALE = 0.01
-
-DRONE_USD = "~/.local/share/ov/data/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
-
-# GPS world anchor: what world (0, 0, 0) maps to in real GPS coordinates.
-# Matches the Zürich default in px4_config.yaml — change here to relocate the sim world.
-WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
-
-# Drone spawn configs.
-# x_m = East offset from world origin (meters)
-# y_m = North offset from world origin (meters)
-# z_m = Up offset / height above floor (meters)
-# orient = initial quaternion [x, y, z, w]
-SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
-DRONE_CONFIGS = [
-    {"domain_id": 1, "x_m": -3.0, "y_m":  3.5, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0]},
-    {"domain_id": 2, "x_m":  3.0, "y_m":  3.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.0]},
-    {"domain_id": 3, "x_m":  0.0, "y_m": -3.0, "z_m": SPAWN_HEIGHT_ABOVE_FLOOR_M, "orient": [0.0, 0.0, 0.0, 1.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:
-            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 wait_for_stage(stage, timeout_s: float = 10.0):
-    """Pump the Kit app loop until /World has content (scene fully loaded)."""
-    for _ in range(int(timeout_s / 0.1)):
-        omni.kit.app.get_app().update()
-        world_prim = stage.GetPrimAtPath("/World")
-        if world_prim.IsValid():
-            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
-            if non_physics:
-                return True
-        time.sleep(0.1)
-    return False
-
-
-class PegasusApp:
-
-    def __init__(self):
-        # Write GPS origins immediately so robot containers can read them
-        # before this container finishes its heavy USD loading.
-        set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
-
-        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")
-
-        if not wait_for_stage(stage):
-            carb.log_warn("Stage load timed out — continuing anyway.")
-
-        # ----- Scene preparation -----
-        stage_prim = stage.GetPrimAtPath("/World/stage")
-        if stage_prim.IsValid():
-            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
-            add_colliders(stage_prim)
-            for _ in range(10):
-                omni.kit.app.get_app().update()
-        else:
-            carb.log_warn("/World/stage not found — skipping scale and collision.")
-
-        if ADD_DOME_LIGHT:
-            add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
-
-        # Units
-        mpu, s = get_stage_meters_per_unit(stage)
-
-        # Spawn all drones
-        for cfg in DRONE_CONFIGS:
-            i = cfg["domain_id"]
-            pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
-
-            graph_handle = spawn_px4_multirotor_node(
-                pegasus_node_name=f"PX4Multirotor_{i}",
-                drone_prim=f"/World/drone{i}/base_link",
-                robot_name=f"robot_{i}",
-                vehicle_id=i,
-                domain_id=i,
-                usd_file=DRONE_USD,
-                init_pos=pos,
-                init_orient=cfg["orient"],
-            )
-
-            add_zed_stereo_camera_subgraph(
-                parent_graph_handle=graph_handle,
-                drone_prim=f"/World/drone{i}/base_link",
-                robot_name=f"robot_{i}",
-                camera_name="ZEDCamera",
-                camera_offset=[0.2, 0.0, -0.05],
-                camera_rotation_offset=[0.0, 0.0, 0.0],
-            )
-
-            add_ouster_lidar_subgraph(
-                parent_graph_handle=graph_handle,
-                drone_prim=f"/World/drone{i}/abase_link",
-                robot_name=f"robot_{i}",
-                lidar_name="OS1_REV6_128_10hz___512_resolution",
-                lidar_offset=[0.0, 0.0, 0.025],
-                lidar_rotation_offset=[0.0, 0.0, 0.0],
-                lidar_min_range=3.0,
-            )
-
-        # Reset so physics/articulations are ready
-        self.world.reset()
-
-        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
-        self.stop_sim = False
-
-    def run(self):
-        if self.play_on_start:
-            self.timeline.play()
-        else:
-            self.timeline.stop()
-
-        # 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()
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py b/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
deleted file mode 100644
index 34073e52..00000000
--- a/simulation/isaac-sim/launch_scripts/two_drone_RetroNeighbourhood.py
+++ /dev/null
@@ -1,284 +0,0 @@
-#!/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 = (35.0, -19)
-DRONE2_XY_M = (30, -19)
-# ---------------------------------------------------------
-
-
-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 = 1.5, # 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
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire.py b/simulation/isaac-sim/launch_scripts/two_drone_fire.py
deleted file mode 100644
index f64ad2a7..00000000
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire.py
+++ /dev/null
@@ -1,279 +0,0 @@
-#!/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})
-
-# Set local Nucleus as asset root before importing Pegasus (which resolves it at import time)
-carb.settings.get_settings().set(
-    "/persistent/isaac/asset_root/default",
-    "omniverse://airlab-nucleus.andrew.cmu.edu/NVIDIA/Assets/Isaac/5.1"
-)
-
-import os
-import sys
-import time
-
-import omni.kit.app
-import omni.timeline
-import omni.usd
-import omni.client
-
-from omni.isaac.core.world import World
-
-# 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
-
-# gps_utils lives in the same directory as this script
-_LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
-if _LAUNCH_SCRIPTS_DIR not in sys.path:
-    sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
-from gps_utils import set_gps_origins, DEFAULT_WORLD_ORIGIN
-
-sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
-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,
-    add_orthographic_camera, add_overhead_camera_publisher,
-)
-
-
-# --------------------- CONFIGURATION ---------------------
-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"
-STAGE_SCALE = 0.01
-
-DRONE_USD = "~/.local/share/ov/data/documents/Kit/shared/exts/pegasus.simulator/pegasus/simulator/assets/Robots/Iris/iris.usd"
-
-# Lighting
-ADD_DOME_LIGHT = False
-DOME_LIGHT_PATH = "/World/DomeLight"
-DOME_LIGHT_INTENSITY = 3500.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.
-WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
-
-# Drone spawn configs.
-# x_m = East offset from world origin (meters)
-# y_m = North offset from world origin (meters)
-# z_m = Up offset / height above floor (meters)
-# orient = initial quaternion [x, y, z, w]
-# 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": 20.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": 17.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.
-OVERHEAD_TOPIC         = "/sim/overhead/image"
-OVERHEAD_SPEC_TOPIC    = "/sim/overhead/spec"
-OVERHEAD_FRAME_ID      = "map"
-OVERHEAD_DOMAIN_ID     = 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 wait_for_stage(stage, timeout_s: float = 10.0):
-    """Pump the Kit app loop until /World has content (scene fully loaded)."""
-    for _ in range(int(timeout_s / 0.1)):
-        omni.kit.app.get_app().update()
-        world_prim = stage.GetPrimAtPath("/World")
-        if world_prim.IsValid():
-            non_physics = [c for c in world_prim.GetChildren() if c.GetName() != "PhysicsScene"]
-            if non_physics:
-                return True
-        time.sleep(0.1)
-    return False
-
-
-class PegasusApp:
-
-    def __init__(self):
-        # Write GPS origins immediately so robot containers can read them
-        # before this container finishes its heavy USD loading.
-        set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
-
-        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 (places geometry at /World/stage)
-        self.pg.load_environment(ENV_URL)
-
-        stage = omni.usd.get_context().get_stage()
-        if stage is None:
-            raise RuntimeError("Stage failed to load")
-
-        if not wait_for_stage(stage):
-            carb.log_warn("Stage load timed out — continuing anyway.")
-
-        # ----- Scene preparation -----
-        # Bring in sky/sun/environment prims that sit outside /World in the source USD
-        reference_root_prims_under_world(stage, ENV_URL)
-
-        stage_prim = stage.GetPrimAtPath("/World/stage")
-        if stage_prim.IsValid():
-            scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
-            add_colliders(stage_prim)
-            for _ in range(10):
-                omni.kit.app.get_app().update()
-        else:
-            carb.log_warn("/World/stage not found — skipping scale and collision.")
-
-        if ADD_DOME_LIGHT:
-            add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
-
-        # Units
-        mpu, s = get_stage_meters_per_unit(stage)
-
-        # Top-down orthographic camera over (0, 0). Publishes one JPEG aerial
-        # of the static scene at low rate; the GCS visualizer republishes it
-        # as a textured ground for Foxglove's 3D panel.
-        cam_path = add_orthographic_camera(
-            stage,
-            prim_path="/World/MapCamera",
-            altitude_m=OVERHEAD_ALTITUDE_M,
-            coverage_m=OVERHEAD_COVERAGE_M,
-            scene_scale_factor=s,
-        )
-        add_overhead_camera_publisher(
-            parent_graph_path="/World/MapCameraGraph",
-            camera_prim_path=cam_path,
-            topic=OVERHEAD_TOPIC,
-            spec_topic=OVERHEAD_SPEC_TOPIC,
-            frame_id=OVERHEAD_FRAME_ID,
-            coverage_m=OVERHEAD_COVERAGE_M,
-            pixels_per_meter=OVERHEAD_PX_PER_METER,
-            domain_id=OVERHEAD_DOMAIN_ID,
-        )
-
-        # Spawn all drones
-        for cfg in DRONE_CONFIGS:
-            i = cfg["domain_id"]
-            pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
-
-            graph_handle = spawn_px4_multirotor_node(
-                pegasus_node_name=f"PX4Multirotor_{i}",
-                drone_prim=f"/World/drone{i}/base_link",
-                robot_name=f"robot_{i}",
-                vehicle_id=i,
-                domain_id=i,
-                usd_file=DRONE_USD,
-                init_pos=pos,
-                init_orient=cfg["orient"],
-            )
-
-            add_zed_stereo_camera_subgraph(
-                parent_graph_handle=graph_handle,
-                drone_prim=f"/World/drone{i}/base_link",
-                robot_name=f"robot_{i}",
-                camera_name="ZEDCamera",
-                camera_offset=[0.21, 0.0, 0.05],
-                camera_rotation_offset=[0.0, 0.0, 0.0],
-            )
-
-            add_ouster_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_offset=[0.0, 0.0, 0.025],
-                lidar_rotation_offset=[0.0, 0.0, 0.0],
-                lidar_min_range=cfg["lidar_min_range"],
-            )
-
-        # Give physics time to register the spawned drone prims before reset.
-        # Complex scenes (e.g. fire academy) need more ticks than simpler ones.
-        for _ in range(30):
-            omni.kit.app.get_app().update()
-
-        # Reset so physics/articulations are ready
-        self.world.reset()
-
-        self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
-        self.stop_sim = False
-
-    def run(self):
-        if self.play_on_start:
-            self.timeline.play()
-        else:
-            self.timeline.stop()
-
-        # 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()
diff --git a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py b/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
deleted file mode 100644
index 33c1082a..00000000
--- a/simulation/isaac-sim/launch_scripts/two_drone_fire_academy.py
+++ /dev/null
@@ -1,303 +0,0 @@
-#!/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/Assets/FireAcademyFaro/fire_academy_fixed_textures.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.07
-
-# spawn location for /Assets/Fire_Academy_Digital_Twin/fire_academy.usd
-# DRONE1_XY_M = (20.0, -7.0)
-# DRONE2_XY_M = (17, 1.5)
-
-DRONE1_XY_M = (27.0, 7.6)
-DRONE2_XY_M = (23, 9.8)
-# ---------------------------------------------------------
-
-
-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)
-
-            translate_attr = stage_prim.GetAttribute("xformOp:translate")
-            if translate_attr.IsValid():
-                if str(translate_attr.GetTypeName()) == "float3":
-                    translate_attr.Set(Gf.Vec3f(0.0, 0.0, 0.0))
-                else:
-                    translate_attr.Set(Gf.Vec3d(0.0, 0.0, 0.0))
-            else:
-                # If missing, create a new translate op.
-                translate_op = xformable.AddTranslateOp(UsdGeom.XformOp.PrecisionDouble)
-                translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0))
-
-            scale_attr = stage_prim.GetAttribute("xformOp:scale")
-            if scale_attr.IsValid():
-                if str(scale_attr.GetTypeName()) == "double3":
-                    scale_attr.Set(Gf.Vec3d(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
-                else:
-                    scale_attr.Set(Gf.Vec3f(STAGE_SCALE, STAGE_SCALE, STAGE_SCALE))
-            else:
-                # If missing, create a new scale op.
-                scale_op = xformable.AddScaleOp(UsdGeom.XformOp.PrecisionFloat)
-                scale_op.Set(Gf.Vec3f(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.937, 0.35], #[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 = 4.0, # 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.937, 0.35], #[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 = 3.5 
-        )
-
-        # 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()

From e4a56d56bb6ce4454777a2790ad9351375986ea8 Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 30 Apr 2026 19:54:41 -0400
Subject: [PATCH 53/55] Trim noisy inline comments in PR-added Python files

---
 gcs/ros_ws/src/action_relay/action_relay/relay_node.py         | 1 -
 .../gcs_visualizer/gcs_visualizer/polygon_collector_node.py    | 3 ---
 .../gcs_visualizer/gcs_visualizer/waypoint_collector_node.py   | 2 --
 3 files changed, 6 deletions(-)

diff --git a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
index 5a36816a..00491633 100644
--- a/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
+++ b/gcs/ros_ws/src/action_relay/action_relay/relay_node.py
@@ -369,7 +369,6 @@ def _on_feedback(fb_msg):
         fb_dict = {}
         for name in fields:
             val = getattr(fb, name)
-            # Convert ROS message types to dicts for JSON
             if hasattr(val, 'get_fields_and_field_types'):
                 sub = {}
                 for sn in val.get_fields_and_field_types():
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
index e9dd6487..b2f33f84 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/polygon_collector_node.py
@@ -319,7 +319,6 @@ def _publish_markers(self, stamp: Time):
             label.text = str(i)
             array.markers.append(label)
 
-        # Closed polygon outline (last vertex back to first).
         if len(self._vertices) >= 2:
             line = Marker()
             line.header.frame_id = self._frame_id
@@ -333,7 +332,6 @@ def _publish_markers(self, stamp: Time):
             line.pose.orientation.w = 1.0
             for v in self._vertices:
                 line.points.append(Point(x=v['x'], y=v['y'], z=v['z']))
-            # Close the loop
             first = self._vertices[0]
             line.points.append(Point(x=first['x'], y=first['y'], z=first['z']))
             array.markers.append(line)
@@ -386,7 +384,6 @@ def _publish_saves(self):
             if not verts:
                 continue
 
-            # Closed line strip outline (last vertex back to first).
             if len(verts) >= 2:
                 line = Marker()
                 line.header.frame_id = self._frame_id
diff --git a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
index e99c435c..4db71d7d 100644
--- a/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
+++ b/gcs/ros_ws/src/gcs_visualizer/gcs_visualizer/waypoint_collector_node.py
@@ -310,7 +310,6 @@ def _publish_markers(self, stamp: Time):
         for i, wp in enumerate(self._waypoints):
             pos = Point(x=wp['x'], y=wp['y'], z=wp['z'])
 
-            # Star glyph (text marker, always faces camera)
             star = Marker()
             star.header.frame_id = self._frame_id
             star.header.stamp = stamp
@@ -325,7 +324,6 @@ def _publish_markers(self, stamp: Time):
             star.text = '★'
             array.markers.append(star)
 
-            # Index label
             label = Marker()
             label.header.frame_id = self._frame_id
             label.header.stamp = stamp

From 957f5089e29e9df6dc2661d81aff1a4018d882ce Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 30 Apr 2026 20:10:49 -0400
Subject: [PATCH 54/55] Pin vdb_mapping_ros2 to public main (was at unpushed
 68fe8dde)

---
 robot/ros_ws/src/global/world_models/vdb_mapping_ros2 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/robot/ros_ws/src/global/world_models/vdb_mapping_ros2 b/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
index 68fe8dde..2d6d171c 160000
--- a/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
+++ b/robot/ros_ws/src/global/world_models/vdb_mapping_ros2
@@ -1 +1 @@
-Subproject commit 68fe8ddea43365d1c720b07f1d109490e1bd960c
+Subproject commit 2d6d171c186cfbef8dbe8d17416e4f9618511db6

From 824d911a3daf53545f0541d4e0ae1a9b7a8cbd0c Mon Sep 17 00:00:00 2001
From: krrishj18 <krrishj@andrew.cmu.edu>
Date: Thu, 30 Apr 2026 23:06:27 -0400
Subject: [PATCH 55/55] fixed launch script

---
 .env                                          |   6 +-
 .../example_multi_drone_scene_import.py       | 140 +++++++++++++-----
 2 files changed, 110 insertions(+), 36 deletions(-)

diff --git a/.env b/.env
index 63c6f910..48c1e06b 100644
--- a/.env
+++ b/.env
@@ -25,7 +25,7 @@ COMPOSE_PROFILES="desktop,isaac-sim"
 
 # ================= Common ===================
 AUTOLAUNCH="true"  # If false, the docker-compose will just spawn idle docker containers with no launch command.
-NUM_ROBOTS="1"  # Number of robot containers to launch.
+NUM_ROBOTS="4"  # Number of robot containers to launch.
 RECORD_BAGS="false"  # "true" or "false"
 
 # ============== ISAAC SIM =====================
@@ -33,7 +33,7 @@ ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_peg
 # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file
 ISAAC_SIM_USE_STANDALONE="true" # "true" or "false"
 # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/)
-ISAAC_SIM_SCRIPT_NAME="example_one_px4_pegasus_launch_script.py"
+ISAAC_SIM_SCRIPT_NAME="example_multi_drone_scene_import.py"
 PLAY_SIM_ON_START="false"
 
 # ===============================================
@@ -42,7 +42,7 @@ PLAY_SIM_ON_START="false"
 # Do not set if you want airstack to fetch a simple blocks world.
 # MS_AIRSIM_ENV_DIR=./simulation/ms-airsim/assets/scenes
 # MS_AIRSIM_BINARY_PATH="/ms-airsim-env/Blocks/LinuxNoEditor/Blocks.sh"
-# =================================================
+# =================================================airst
 
 # ================= ROBOT =====================
 ROBOT_NAME_MAP_CONFIG_FILE="default_robot_name_map.yaml"  # Determines how to set ROBOT_NAME and ROS_DOMAIN_ID. See robot/docker/robot_name_map/
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 38ac7619..144b0410 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
@@ -3,9 +3,15 @@
 import carb
 from isaacsim import SimulationApp
 
-# SimulationApp must be created before importing any omni modules.
+# Start Isaac Sim's simulation environment (Must start this before importing omni modules)
 simulation_app = SimulationApp({"headless": False})
 
+# Set local Nucleus as asset root before importing Pegasus (which resolves it at import time)
+carb.settings.get_settings().set(
+    "/persistent/isaac/asset_root/default",
+    "omniverse://airlab-nucleus.andrew.cmu.edu/NVIDIA/Assets/Isaac/5.1"
+)
+
 import os
 import sys
 import time
@@ -17,11 +23,13 @@
 
 from omni.isaac.core.world import World
 
+# 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
 
+# gps_utils lives in the same directory as this script
 _LAUNCH_SCRIPTS_DIR = os.path.dirname(os.path.abspath(__file__))
 if _LAUNCH_SCRIPTS_DIR not in sys.path:
     sys.path.insert(0, _LAUNCH_SCRIPTS_DIR)
@@ -29,45 +37,80 @@
 
 sys.path.insert(0, os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "utils")))
 import scene_prep
-from scene_prep import scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit
+from scene_prep import (
+    scale_stage_prim, add_colliders, add_dome_light, get_stage_meters_per_unit,
+    reference_root_prims_under_world,
+    add_orthographic_camera, add_overhead_camera_publisher,
+)
 
 
+# --------------------- CONFIGURATION ---------------------
 NUCLEUS_SERVER = "airlab-nucleus.andrew.cmu.edu"
-ENV_URL = f"omniverse://{NUCLEUS_SERVER}/Library/Assets/ACFA/FireAcademyFaro/fire_academy_fixed_textures.usd"
+
+#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"
 
-ADD_DOME_LIGHT = True
+# Lighting
+ADD_DOME_LIGHT = False
 DOME_LIGHT_PATH = "/World/DomeLight"
 DOME_LIGHT_INTENSITY = 3500.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.
 WORLD_GPS_ORIGIN = DEFAULT_WORLD_ORIGIN
 
-SPAWN_HEIGHT_ABOVE_FLOOR_M = 0.15
+# Drone spawn configs.
+# x_m = East offset from world origin (meters)
+# y_m = North offset from world origin (meters)
+# z_m = Up offset / height above floor (meters)
+# orient = initial quaternion [x, y, z, w]
+# 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.0, 1.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.0, 1.0]},
+    {"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.
+OVERHEAD_TOPIC         = "/sim/overhead/image"
+OVERHEAD_SPEC_TOPIC    = "/sim/overhead/spec"
+OVERHEAD_FRAME_ID      = "map"
+OVERHEAD_DOMAIN_ID     = 0
+# ---------------------------------------------------------
+
 
 ext_manager = omni.kit.app.get_app().get_extension_manager()
 for ext in [
     "omni.physx.forcefields",
-    "omni.graph.core",
-    "omni.graph.action",
-    "omni.graph.action_nodes",
-    "omni.graph.ui",
-    "omni.graph.visualization.nodes",
-    "omni.graph.scriptnode",
-    "omni.graph.window.action",
-    "omni.graph.window.generic",
-    "omni.graph.ui_nodes",
-    "airlab.pegasus",
+    "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:
@@ -80,6 +123,7 @@ def nucleus_stat(url: str) -> bool:
 
 
 def wait_for_stage(stage, timeout_s: float = 10.0):
+    """Pump the Kit app loop until /World has content (scene fully loaded)."""
     for _ in range(int(timeout_s / 0.1)):
         omni.kit.app.get_app().update()
         world_prim = stage.GetPrimAtPath("/World")
@@ -94,19 +138,24 @@ def wait_for_stage(stage, timeout_s: float = 10.0):
 class PegasusApp:
 
     def __init__(self):
-        # Write GPS origins before USD loading so robot containers can read them
-        # while this container is still busy.
+        # Write GPS origins immediately so robot containers can read them
+        # before this container finishes its heavy USD loading.
         set_gps_origins(DRONE_CONFIGS, world_origin=WORLD_GPS_ORIGIN)
 
         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
+
+        self.timeline.stop()
+
         self.pg.load_environment(ENV_URL)
 
         stage = omni.usd.get_context().get_stage()
@@ -116,6 +165,10 @@ def __init__(self):
         if not wait_for_stage(stage):
             carb.log_warn("Stage load timed out — continuing anyway.")
 
+        # ----- Scene preparation -----
+        # Bring in sky/sun/environment prims that sit outside /World in the source USD
+        reference_root_prims_under_world(stage, ENV_URL)
+
         stage_prim = stage.GetPrimAtPath("/World/stage")
         if stage_prim.IsValid():
             scale_stage_prim(stage, "/World/stage", STAGE_SCALE)
@@ -128,8 +181,31 @@ def __init__(self):
         if ADD_DOME_LIGHT:
             add_dome_light(stage, DOME_LIGHT_PATH, DOME_LIGHT_INTENSITY, DOME_LIGHT_EXPOSURE)
 
+        # Units
         mpu, s = get_stage_meters_per_unit(stage)
 
+        # Top-down orthographic camera over (0, 0). Publishes one JPEG aerial
+        # of the static scene at low rate; the GCS visualizer republishes it
+        # as a textured ground for Foxglove's 3D panel.
+        cam_path = add_orthographic_camera(
+            stage,
+            prim_path="/World/MapCamera",
+            altitude_m=OVERHEAD_ALTITUDE_M,
+            coverage_m=OVERHEAD_COVERAGE_M,
+            scene_scale_factor=s,
+        )
+        add_overhead_camera_publisher(
+            parent_graph_path="/World/MapCameraGraph",
+            camera_prim_path=cam_path,
+            topic=OVERHEAD_TOPIC,
+            spec_topic=OVERHEAD_SPEC_TOPIC,
+            frame_id=OVERHEAD_FRAME_ID,
+            coverage_m=OVERHEAD_COVERAGE_M,
+            pixels_per_meter=OVERHEAD_PX_PER_METER,
+            domain_id=OVERHEAD_DOMAIN_ID,
+        )
+
+        # Spawn all drones
         for cfg in DRONE_CONFIGS:
             i = cfg["domain_id"]
             pos = [cfg["x_m"] * s, cfg["y_m"] * s, cfg["z_m"] * s]
@@ -150,7 +226,7 @@ def __init__(self):
                 drone_prim=f"/World/drone{i}/base_link",
                 robot_name=f"robot_{i}",
                 camera_name="ZEDCamera",
-                camera_offset=[0.2, 0.0, -0.05],
+                camera_offset=[0.21, 0.0, 0.05],
                 camera_rotation_offset=[0.0, 0.0, 0.0],
             )
 
@@ -161,15 +237,9 @@ def __init__(self):
                 lidar_name="OS1_REV6_128_10hz___512_resolution",
                 lidar_offset=[0.0, 0.0, 0.025],
                 lidar_rotation_offset=[0.0, 0.0, 0.0],
-                lidar_min_range=0.75,
+                lidar_min_range=cfg["lidar_min_range"],
             )
 
-        # Complex scenes (e.g. fire academy) need ~30 ticks before reset.
-        for _ in range(30):
-            omni.kit.app.get_app().update()
-
-        self.world.reset()
-
         self.play_on_start = os.environ.get("PLAY_SIM_ON_START", "true").lower() == "true"
         self.stop_sim = False
 
@@ -179,12 +249,16 @@ def run(self):
         else:
             self.timeline.stop()
 
+        app = omni.kit.app.get_app()
         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
+            world = World.instance()
+            if world is not None and hasattr(world, '_scene'):
+                world.step(render=True)
+                if world is not self.world:
+                    self.world = world
+                    self.pg._world = world
+            else:
+                app.update()
 
         carb.log_warn("PegasusApp Simulation App is closing.")
         self.timeline.stop()
@@ -197,4 +271,4 @@ def main():
 
 
 if __name__ == "__main__":
-    main()
+    main()
\ No newline at end of file
