diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 09d89a4d..c6bd9d23 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -1,15 +1,37 @@
 {
-    "configurations": [
-      {
-        "name": "Linux",
-        "includePath": ["${workspaceFolder}/ros_ws/**", "/opt/ros/humble/include/**"],
-        "defines": [],
-        "compilerPath": "/usr/bin/gcc",
-        "cStandard": "c11",
-        "cppStandard": "c++17",
-        "intelliSenseMode": "clang-x64"
-      }
-    ],
-    "version": 4
-  }
-  
\ No newline at end of file
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/root/AirStack/ros_ws/install/vdb_mapping_interfaces/include/**",
+        "/root/AirStack/ros_ws/install/trajectory_library/include/**",
+        "/root/AirStack/ros_ws/install/mavros_interface/include/**",
+        "/root/AirStack/ros_ws/install/robot_interface/include/**",
+        "/root/AirStack/ros_ws/install/px4_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_system_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_state_machine_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_planning_msgs/include/**",
+        "/root/AirStack/ros_ws/install/mav_msgs/include/**",
+        "/root/AirStack/ros_ws/install/disparity_map_representation/include/**",
+        "/root/AirStack/ros_ws/install/map_representation_interface/include/**",
+        "/root/AirStack/ros_ws/install/disparity_graph/include/**",
+        "/root/AirStack/ros_ws/install/behavior_tree/include/**",
+        "/root/AirStack/ros_ws/install/behavior_tree_msgs/include/**",
+        "/root/AirStack/ros_ws/install/airstack_common/include/**",
+        "/root/AirStack/ros_ws/install/airstack_msgs/include/**",
+        "/opt/ros/humble/include/**",
+        "/usr/include/**",
+        "/root/AirStack/ros_ws/src/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/extensions.json b/.vscode/extensions.json
index 4376032f..c4792536 100644
--- a/.vscode/extensions.json
+++ b/.vscode/extensions.json
@@ -1,5 +1,9 @@
 {
     "recommendations": [
         "ms-python.black-formatter",
+        "ms-iot.vscode-ros",
+        "ms-vscode-remote.remote-ssh",
+        "ms-python.python",
+        "ms-vscode-remote.remote-containers"
     ]
 }
\ No newline at end of file
diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 00000000..b112a2d9
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,11 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "ROS2: Launch robot.launch.py",
+      "type": "ros",
+      "request": "launch",
+      "target": "${workspaceFolder}/ros_ws/install/robot_bringup/share/robot_bringup/launch/robot.launch.xml",
+    }
+  ]
+}
\ No newline at end of file
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
index 61dbf971..04dac578 100644
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@@ -2,20 +2,32 @@
   "version": "2.0.0",
   "tasks": [
     {
-      "label": "build",
+      "label": "build ros_ws",
       "type": "shell",
       "options": {
         "cwd": "${workspaceFolder}/ros_ws"
       },
-      "command": "colcon build --cmake-args '-DCMAKE_BUILD_TYPE=Debug'"
+      "command": [
+        "source /root/.bashrc &&",
+        "bws --cmake-args '-DCMAKE_BUILD_TYPE=Debug'"
+      ],
+      "problemMatcher": [],
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      }
     },
     {
-      "label": "test",
+      "label": "test ros_ws",
       "type": "shell",
       "options": {
         "cwd": "${workspaceFolder}/ros_ws"
       },
-      "command": "colcon test && colcon test-result"
+      "command": [
+        "source /opt/ros/humble/setup.bash;",
+        "source ${workspaceFolder}/ros_ws/install/setup.bash;",
+        "colcon test && colcon test-result"
+      ],
     }
   ]
-}
+}
\ No newline at end of file
diff --git a/docker/Dockerfile.airstack-dev b/docker/Dockerfile.airstack-dev
index f38d921b..d60334aa 100644
--- a/docker/Dockerfile.airstack-dev
+++ b/docker/Dockerfile.airstack-dev
@@ -9,7 +9,8 @@ RUN apt install -y \
     vim nano wget curl tree \
     cmake build-essential \
     less htop jq \
-    python3-pip
+    python3-pip \
+    gdb
 
 # Package dependencies
 # perform ROS dependency installation for our workspace.
diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml
index e3fe78fb..93e209f4 100644
--- a/docker/docker-compose.yaml
+++ b/docker/docker-compose.yaml
@@ -2,12 +2,12 @@
 services:
   # ==============================================================================
   isaac-sim:
+    image: &isaac-sim-image airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.0
     build:
       context: ../
       dockerfile: docker/Dockerfile.isaac-ros
       tags:
-        - airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.0
-    image: airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.0
+        - *isaac-sim-image
     container_name: isaac-sim
     entrypoint: bash
     # Interactive shell
@@ -58,16 +58,21 @@ services:
       - ./extras/config:/root/.nvidia-omniverse/config:rw
 
   # ==============================================================================
-  ground-control-station:
-    image: airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:latest
-    container_name: ground-control-station
+  robot:
+    image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
+    build:
+      context: ../
+      dockerfile: docker/Dockerfile.airstack-dev
+      tags:
+        - *airstack-dev-image
     entrypoint: bash -c "service ssh restart && bash"
     # Interactive shell
     stdin_open: true # docker run -i
     tty: true # docker run -t
     # Needed to display graphical applications
-    ipc: host
     privileged: true
+    networks:
+      - airstack_network
     environment:
       - DISPLAY
       - QT_X11_NO_MITSHM=1
@@ -80,37 +85,30 @@ services:
               count: 1
               capabilities: [ gpu ]
     ports:
-      # for ssh
-      - 2222:22
+      # for ssh, starting from 2223-2243 on the host port all map to 22 in the container. Assumes no more than 21 robots
+      - "2223-2243:22"
     volumes:
       # display stuff
       - $HOME/.Xauthority:/root/.Xauthority
       - /tmp/.X11-unix:/tmp/.X11-unix
       # developer stuff
-      - ./ground_control_station/.bashrc:/root/.bashrc:rw # bash config
+      - ./robot/.bashrc:/root/.bashrc:rw # bash config
       - ~/.bash_history:/root/.bash_history:rw # save cmdline history
       - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
       # autonomy stack stuff
       - ./..:/root/AirStack:rw
 
   # ==============================================================================
-  robot:
-    build:
-      context: ../
-      dockerfile: docker/Dockerfile.airstack-dev
-      tags:
-        - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:latest
-        - airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
-    image: airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.1
-    # container_name: robot-1
+  ground-control-station:
+    image: *airstack-dev-image
+    container_name: ground-control-station
     entrypoint: bash -c "service ssh restart && bash"
     # Interactive shell
     stdin_open: true # docker run -i
     tty: true # docker run -t
     # Needed to display graphical applications
+    ipc: host
     privileged: true
-    networks:
-      - airstack_network
     environment:
       - DISPLAY
       - QT_X11_NO_MITSHM=1
@@ -123,14 +121,14 @@ services:
               count: 1
               capabilities: [ gpu ]
     ports:
-      # for ssh, starting from 2223-2243 on the host port all map to 22 in the container. Assumes no more than 21 robots
-      - "2223-2243:22"
+      # for ssh
+      - 2222:22
     volumes:
       # display stuff
       - $HOME/.Xauthority:/root/.Xauthority
       - /tmp/.X11-unix:/tmp/.X11-unix
       # developer stuff
-      - ./robot/.bashrc:/root/.bashrc:rw # bash config
+      - ./ground_control_station/.bashrc:/root/.bashrc:rw # bash config
       - ~/.bash_history:/root/.bash_history:rw # save cmdline history
       - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name
       # autonomy stack stuff
diff --git a/docker/extras/config/omniverse.toml b/docker/extras/config/omniverse.toml
index f1e42127..acd5cb4b 100644
--- a/docker/extras/config/omniverse.toml
+++ b/docker/extras/config/omniverse.toml
@@ -1,3 +1,4 @@
 
 [bookmarks]
 "AirLab Nucleus Server" = "omniverse://airlab-storage.andrew.cmu.edu:8443"
+"AirStack" = "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/"
diff --git a/docker/robot/.bashrc b/docker/robot/.bashrc
index 3cd80547..bff515e3 100644
--- a/docker/robot/.bashrc
+++ b/docker/robot/.bashrc
@@ -4,7 +4,7 @@
 # for examples
 
 # If not running interactively, don't do anything
-[ -z "$PS1" ] && return
+# [ -z "$PS1" ] && return
 
 # don't put duplicate lines in the history. See bash(1) for more options
 # ... or force ignoredups and ignorespace
@@ -120,7 +120,7 @@ export PYTHONWARNINGS
 # Convenience functions for ROS2 workspace
 
 function bws(){
-    echo "Running \`colcon build\` in $ROS2_WS_DIR"
+    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/ "$@"
 }
 function sws(){
diff --git a/docs/development/development_environment.md b/docs/development/development_environment.md
new file mode 100644
index 00000000..e69de29b
diff --git a/docs/development/docker_usage.md b/docs/development/docker_usage.md
index 36483b75..25c98636 100644
--- a/docs/development/docker_usage.md
+++ b/docs/development/docker_usage.md
@@ -1,3 +1,4 @@
+# General Usage with Docker Compose
 
 AirStack is designed for multi-robot development, and is setup to run multiple robots in simulation.
 
@@ -18,6 +19,29 @@ This lets them communicate with ROS2 on the same network.
 
 Each robot has its own ROS_DOMAIN_ID.
 
+## Pull Images
+To use the AirLab docker registry:
+```bash
+cd AirStack/docker/
+docker login airlab-storage.andrew.cmu.edu:5001
+## <Enter your andrew id (without @andrew.cmu.edu)>
+## <Enter your andrew password>
+
+## Pull the images in the docker compose file
+docker compose pull 
+```
+
+Catelog: [AirLab Registry Images](https://airlab-storage.andrew.cmu.edu:5001/v2/_catalog).
+
+Available image tags: 
+[airstack-dev](https://airlab-storage.andrew.cmu.edu:5001/v2/shared/airstack-dev/tags/list),
+[isaac-sim_ros-humble](https://airlab-storage.andrew.cmu.edu:5001/v2/shared/isaac-sim_ros-humble/tags/list)
+
+## Build Images
+```bash
+docker compose build
+```
+
 ## Start and Stop
 Start
 ```bash
@@ -53,6 +77,20 @@ The ssh password is `airstack`.
 
 ## Container Details
 
+```mermaid
+graph TD
+    A(Isaac Sim) <-- Sensors and Actuation --> B
+    A <-- Sensors and Actuation --> C
+    B(Robot 1) <-- Global Info --> D(Ground Control Station)
+    C(Robot 2) <-- Global Info --> D
+
+    style A fill:#76B900,stroke:#333,stroke-width:2px
+    style B fill:#fbb,stroke:#333,stroke-width:2px
+    style C fill:#fbb,stroke:#333,stroke-width:2px
+    style D fill:#fbf,stroke:#333,stroke-width:2px
+
+```
+
 
 ### Isaac Sim
 Start a bash shell in the Isaac Sim container:
@@ -64,7 +102,7 @@ docker exec -it isaac-sim bash
 `runapp` launches Isaac Sim.
 The `--path` argument can be passed with a path to a `.usd` file to load a scene.
 
-It can also be run in headless mode with `./runheadless.native.sh` to stream to Omniverse Streaming Client or `./runheadless.webrtc.sh` to stream to a web browser.
+It can also be run in headless mode with `./runheadless.native.sh` to stream to [Omniverse Streaming Client](https://docs.omniverse.nvidia.com/streaming-client/latest/user-manual.html) or `./runheadless.webrtc.sh` to [stream to a web browser](https://docs.omniverse.nvidia.com/extensions/latest/ext_livestream/webrtc.html).
 
 The container also has the isaacsim ROS2 package within that can be launched with `ros2 launch isaacsim run_isaacsim.launch.py`. 
 
@@ -78,6 +116,7 @@ docker exec -it docker-robot-1 bash
 # in robot docker
 cws  # cleans workspace
 bws  # builds workspace
+bws --packages-select [your_packages] # builds only desired packages
 sws  # sources workspace
 ros2 launch robot_bringup robot.launch.xml  # top-level launch 
 ```
@@ -96,4 +135,4 @@ docker exec -it ground-control-station bash
 
 The commands are currently the same.
 
-`ROS_DOMAIN_ID` is set to 0.
\ No newline at end of file
+On the GCS `ROS_DOMAIN_ID` is set to 0.
\ No newline at end of file
diff --git a/docs/development/testing/ci_cd.md b/docs/development/testing/ci_cd.md
index e69de29b..a5afaf26 100644
--- a/docs/development/testing/ci_cd.md
+++ b/docs/development/testing/ci_cd.md
@@ -0,0 +1 @@
+# CI/CD Pipeline
\ No newline at end of file
diff --git a/docs/development/testing/integration_testing.md b/docs/development/testing/integration_testing.md
index e69de29b..00c8e31f 100644
--- a/docs/development/testing/integration_testing.md
+++ b/docs/development/testing/integration_testing.md
@@ -0,0 +1 @@
+# Integration Testing
\ No newline at end of file
diff --git a/docs/development/testing/testing_frameworks.md b/docs/development/testing/testing_frameworks.md
index e69de29b..60ba2d65 100644
--- a/docs/development/testing/testing_frameworks.md
+++ b/docs/development/testing/testing_frameworks.md
@@ -0,0 +1 @@
+# Testing Frameworks
\ No newline at end of file
diff --git a/docs/development/vscode/attach_container.png b/docs/development/vscode/attach_container.png
new file mode 100644
index 00000000..e42ebdf8
Binary files /dev/null and b/docs/development/vscode/attach_container.png differ
diff --git a/docs/development/vscode/debugger.png b/docs/development/vscode/debugger.png
new file mode 100644
index 00000000..84149f73
Binary files /dev/null and b/docs/development/vscode/debugger.png differ
diff --git a/docs/development/vscode/extensions.png b/docs/development/vscode/extensions.png
new file mode 100644
index 00000000..39a5ac7f
Binary files /dev/null and b/docs/development/vscode/extensions.png differ
diff --git a/docs/development/vscode/index.md b/docs/development/vscode/index.md
new file mode 100644
index 00000000..0935c11b
--- /dev/null
+++ b/docs/development/vscode/index.md
@@ -0,0 +1,50 @@
+# VS Code: Docker Integration and Debugger Setup
+
+Start containers
+```bash
+# optionally pass the --scale robot=N argument to start N robots
+dc compose up -d  # --scale robot=2
+```
+
+Open AirStack folder
+
+```bash
+cd AirStack
+code .
+```
+
+Install the ["Dev Containers" extension](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers).
+
+Now click the "Remote Explorer" icon on the left side bar, hover over a robot container, and attach to the container.
+
+![attach_container](attach_container.png)
+
+Install recommended extensions within the image. This installs the `ROS`, `C++`, and `Python` extensions in the container.
+![extensions.png](extensions.png)
+
+## Build ROS Workspace
+Hit `Ctrl-Shift-B` to build the project. This is a shortcut for `bws --cmake-args '-DCMAKE_BUILD_TYPE=Debug'`, which adds debug symbols to the build.
+
+Build tasks are defined in `.vscode/tasks.json`.
+
+
+## Launch
+
+Hit `F5` to launch `robot.launch.xml`, or click the "Run and Debug" button on the left side of the screen and click the green play button.
+
+Launch tasks are defined in `.vscode/launch.json`.
+
+![launch](launch.png)
+
+You can now set breakpoints, view variables, step-through code, and debug [as usual in VSCode](https://code.visualstudio.com/docs/editor/debugging).
+
+![debugger](debugger.png)
+
+
+!!! warning "Warning about file permissions"
+
+    Folders and files created within the attached docker container will be owned by root. This can cause issues when trying to edit files from the host machine, especially when using git to switch branches.
+    If you accidentally create files as root, you can change the owner to your user with the following command:
+    ```bash
+    sudo chown -R $USER:$USER .
+    ```
\ No newline at end of file
diff --git a/docs/development/vscode/launch.png b/docs/development/vscode/launch.png
new file mode 100644
index 00000000..bd865144
Binary files /dev/null and b/docs/development/vscode/launch.png differ
diff --git a/docs/development/vscode/open_folder.png b/docs/development/vscode/open_folder.png
new file mode 100644
index 00000000..05c8f369
Binary files /dev/null and b/docs/development/vscode/open_folder.png differ
diff --git a/docs/robot/autonomy/0_interface/index.md b/docs/robot/autonomy/0_interface/index.md
index 495a0c64..43afef5c 100644
--- a/docs/robot/autonomy/0_interface/index.md
+++ b/docs/robot/autonomy/0_interface/index.md
@@ -14,44 +14,49 @@ Launch files are under `src/robot/autonomy/0_interface/interface_bringup/launch`
 
 The main launch command is `ros2 launch interface_bringup interface.launch.xml`.
 
-
 ## RobotInterface
 
 Package `robot_interface` is a ROS2 node that interfaces with the robot's hardware.
-The `RobotInterface` _gets robot state_ and forwards it to the autonomy stack, 
+The `RobotInterface` _gets robot state_ and forwards it to the autonomy stack,
 and also _translates control commands_ from the autonomy stack into the command for the underlying hardware.
 Note the base class is unimplemented.
 Specific implementations should extend `class RobotInterface` in `robot_interface.hpp`, for example `class MAVROSInterface`.
 
 ### State
+
 The `RobotInterface` class broadcasts the robot's pose as a TF2 transform.
-It also publishes the robot's odometry as a `nav_msgs/Odometry` message to `$(arg robot_name)/0_interface/robot_0_interface/odometry`.
+It also publishes the robot's odometry as a `nav_msgs/Odometry` message to `$(env ROBOT_NAME)/0_interface/robot_0_interface/odometry`.
 
 ### Commands
+
 The commands are variations of the two main command modes: Attitude control and Position control.
 These are reflected in [MAVLink](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED) and supported by both PX4 and [Ardupilot](https://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#movement-commands).
 
-The Robot0_interface node subscribes to:
+The RobotInterface node subscribes to:
 
-- `/$(arg robot_name)/interface/cmd_attitude_thrust` of type `mav_msgs/AttitudeThrust.msg`
-- `/$(arg robot_name)/interface/cmd_rate_thrust` of type `mav_msgs/RateThrust.msg`
-- `/$(arg robot_name)/interface/cmd_roll_pitch_yawrate_thrust` of type `mav_msgs/RollPitchYawrateThrust.msg`
-- `/$(arg robot_name)/interface/cmd_torque_thrust` of type `mav_msgs/TorqueThrust.msg`
-- `/$(arg robot_name)/interface/cmd_velocity` of type `geometry_msgs/TwistStamped.msg`
-- `/$(arg robot_name)/interface/cmd_position` of type `geometry_msgs/PoseStamped.msg`
+- `/$(env ROBOT_NAME)/interface/cmd_attitude_thrust` of type `mav_msgs/AttitudeThrust.msg`
+- `/$(env ROBOT_NAME)/interface/cmd_rate_thrust` of type `mav_msgs/RateThrust.msg`
+- `/$(env ROBOT_NAME)/interface/cmd_roll_pitch_yawrate_thrust` of type `mav_msgs/RollPitchYawrateThrust.msg`
+- `/$(env ROBOT_NAME)/interface/cmd_torque_thrust` of type `mav_msgs/TorqueThrust.msg`
+- `/$(env ROBOT_NAME)/interface/cmd_velocity` of type `geometry_msgs/TwistStamped.msg`
+- `/$(env ROBOT_NAME)/interface/cmd_position` of type `geometry_msgs/PoseStamped.msg`
 
 All messages are in the robot's body frame, except `velocity` and `position` which use the frame specified by the message header.
 
 ## MAVROSInterface
+
 The available implementation in AirStack is called `MAVROSInterface` implemented in `mavros_interface.cpp`. It simply forwards the control commands to the Ascent flight controller (based on Ardupilot) using MAVROS.
 
 ## Custom Robot Interface
+
 If you're using a different robot control unit with its own custom API, then you need to create an associated RobotInterface. Implementations should do the following:
 
 ### Broadcast State
+
 Implementations of `RobotInterface` should obtain the robot's pose and broadcast it as a TF2 transform.
 
 Should look something like:
+
 ```c++
 // callback function triggered by some loop
 void your_callback_function(){
@@ -68,9 +73,11 @@ void your_callback_function(){
     // ...
 }
 ```
+
 ==TODO: our code doesn't currently do it like this, it instead uses an external odometry_conversion node.==
 
 ### Override Command Handling
+
 Should override all `virtual` functions in `robot_interface.hpp`:
 
 - `cmd_attitude_thrust_callback`
diff --git a/docs/robot/autonomy/4_global/planning.md b/docs/robot/autonomy/4_global/planning.md
index 6355ace1..edb5fc1e 100644
--- a/docs/robot/autonomy/4_global/planning.md
+++ b/docs/robot/autonomy/4_global/planning.md
@@ -20,49 +20,39 @@ Feel free to implement your own through the following interfaces.
 
 Global planners are meant to be modules that can be swapped out easily. 
 They can be thought of as different high level behaviors for the robot to follow.
-The Behavior Executive may run multiple global planners in parallel and choose the best plan for the current situation.
+Consider that multiple global planners may be run in parallel, for example by some ensemble planner node that chooses the best plan for the current situation.
 
-As such, the global planner should be implemented as a ROS2 action server that can be queried for a plan.
-The Behavior Executive will then publish the best plan to `/$(arg robot_name)/global/trajectory` for the local planner to follow.
+As such, the global planner should be implemented as a ROS2 node that accepts runtime mission parameters in a custom `PlanRequest.msg` and 
+publishes a plan to its local `~/global_plan` topic.
+
+The best global plan should then be forwarded or remapped to `/$(env ROBOT_NAME)/global_plan` for the local planner to follow.
 
 ``` mermaid
 sequenceDiagram
   autonumber
-  Behavior Executive->>Global Planner: GetPlan.action: goal
+  Some Node->>Global Planner: ~/plan_request (your_planner/PlanRequest.msg)
   loop Planning
-      Global Planner->>Behavior Executive: GetPlan.action: feedback
+      Global Planner-->>Some Node: heartbeat feedback
   end
-  Global Planner-->>Behavior Executive: GetPlan.action: result (nav_msgs/Path)
-  Behavior Executive-->>Local Planner: /$ROBOT_NAME/global/trajectory (nav_msgs/Path)
+  Global Planner->>Some Node: ~/global_plan (nav_msgs/Path.msg)
+  Some Node->>Local Planner: /$ROBOT_NAME/global_plan (nav_msgs/Path.msg)
 ```
 
-### Actions Interface
-
-Global Planner implementations should define a custom **GetPlan** action server and associated `GetPlan.action` message. 
-The action message may be defined with whatever input parameters necessary for the planner to generate a plan.
-Your `GetPlan.action` _must_ return a `nav_msgs/Path` message.
+### Subscribe: Plan Request
+Your custom `PlanRequest.msg` defines the parameters that your global planner needs to generate a plan. 
+It will be sent on the `~/plan_request` topic.
 
-An example `GetPlan.action` message is shown below.
+Some common parameters may be the following:
 ```
-# Define a goal
+# PlanRequest.msg
 std_msgs/Duration timeout  # maximum time to spend planning
 geometry_msgs/Polygon bounds # boundary that the plan must stay within
----
-# Define the result that will be published after the action execution ends.
-{==nav_msgs/Path trajectory  # REQUIRED FIELD==}
----
-# Define a feedback message that will be published during action execution.
-float32 percent_complete
 ```
 
 
-#### Goal
-The goal defines the parameters that the global planner needs to generate a plan. All fields are optional.
-
-
-#### Result
-The global planner must have a return field `trajectory` of message type `nav_msgs/Path`.
-`trajectory` defines high level waypoints to reach by a given time.
+### Publish: Global Plan
+The global planner must publish a message of type `nav_msgs/Path` to `~/global_plan`.
+The message defines high level waypoints to reach by a given time.
 
 The `nav_msgs/Path` message type contains a `header` field and `poses` field.
 
@@ -81,14 +71,10 @@ nav_msgs/Path.msg
                 - string frame_id: the coordinate frame of the waypoint
             - geometry_msgs/Pose pose: the position and orientation of the waypoint
 ```
-#### Feedback
-All other fields are optional.
-
-
-More info about ROS2 actions may be found in the official [tutorial](https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Actions/Understanding-ROS2-Actions.html) and [design philosophy](https://design.ros2.org/articles/actions.html) documents.
-
+### Publish: Heartbeat
+For long-running global planners, it's recommended to publish a heartbeat message to `~/heartbeat`. This way the calling node can know that the global planner is still running and hasn't crashed.
 
-### Subscribers
+### Additional Subscribers
 In general, the global planner needs to access components of the world model such as the map and drone state.
 
 The most common map is Occupancy Grids that is published by {==TODO==} node.
diff --git a/mkdocs.yml b/mkdocs.yml
index 039c7129..6489a717 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -10,6 +10,7 @@ extra:
 extra_css:
   - stylesheets/extra.css
 markdown_extensions:
+  - admonition
   - pymdownx.highlight:
       anchor_linenums: true
       line_spans: __span
@@ -32,14 +33,15 @@ nav:
   - Getting Started: getting_started.md
   - Development:
       - development/index.md
-      - General Usage with Docker Compose: development/docker_usage.md
+      - development/docker_usage.md
+      - development/vscode/index.md
       - Testing:
           - development/testing/index.md
-          - Testing Frameworks: development/testing/testing_frameworks.md
-          - Integration Testing: development/testing/integration_testing.md
-          - Unit Testing: development/testing/unit_testing.md
-          - System Testing: development/testing/system_testing.md
-          - CI/CD Pipeline: development/testing/ci_cd.md
+          - development/testing/testing_frameworks.md
+          - development/testing/integration_testing.md
+          - development/testing/unit_testing.md
+          - development/testing/system_testing.md
+          - development/testing/ci_cd.md
       - Contributing: development/contributing.md
       - ROS Frame Conventions: development/frame_conventions.md
   - Robot:
diff --git a/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz b/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz
new file mode 100644
index 00000000..f0537cce
--- /dev/null
+++ b/ros_ws/src/robot/autonomy/3_local/local_bringup/rviz/droan.rviz
@@ -0,0 +1,772 @@
+---
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1
+        - /TF1/Frames1
+        - /Odometry1/Topic1
+        - /ExpansionPoly1
+        - /Left Depth1
+      Splitter Ratio: 0.35953420400619507
+    Tree Height: 1085
+  - 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: Expansion Cloud
+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
+        base_link:
+          Value: true
+        base_link_frd:
+          Value: false
+        base_link_stabilized:
+          Value: false
+        front_stereo:
+          Value: false
+        left_camera:
+          Value: true
+        look_ahead_point:
+          Value: false
+        look_ahead_point_stabilized:
+          Value: false
+        map:
+          Value: true
+        map_FLU:
+          Value: false
+        map_ned:
+          Value: false
+        odom:
+          Value: false
+        odom_ned:
+          Value: false
+        ouster:
+          Value: false
+        right_camera:
+          Value: true
+        tracking_point:
+          Value: true
+        tracking_point_stabilized:
+          Value: false
+        world:
+          Value: false
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        world:
+          map_FLU:
+            map:
+              base_link:
+                base_link_frd: {}
+                front_stereo:
+                  left_camera: {}
+                  right_camera: {}
+                ouster: {}
+              base_link_stabilized: {}
+              look_ahead_point: {}
+              look_ahead_point_stabilized: {}
+              map_ned: {}
+              tracking_point: {}
+              tracking_point_stabilized: {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_default_plugins/MarkerArray
+      Enabled: true
+      Name: Traj Vis
+      Namespaces:
+        trajectory_0: 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
+    - 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/Marker
+      Enabled: true
+      Name: VDB Mapping Marker
+      Namespaces:
+        '': true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: vdb_mapping/vdb_map_visualization
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 6.571824073791504
+        Min Value: -0.5682187080383301
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 170; 170; 255
+      Color Transformer: FlatColor
+      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: Lidar
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/ouster/point_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      Max Value: 100
+      Median window: 5
+      Min Value: 0
+      Name: Front Left Depth
+      Normalize Range: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/front_stereo/left/depth
+      Value: false
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Front Left RGB
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/front_stereo/left/image_rect
+      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
+    - 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: 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: true
+    - 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_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/MarkerArray
+      Enabled: true
+      Name: DisparityMarker
+      Namespaces: {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: droan/disparity_marker
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Left Camera
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/front_stereo/left/image_rect
+      Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 100
+      Median window: 5
+      Min Value: 0
+      Name: Left Depth
+      Normalize Range: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/front_stereo/left/depth
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    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: 43.695953369140625
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -1.6335676908493042
+        'Y': -2.40643310546875
+        Z: -2.289210796356201
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6753983497619629
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.6135711669921875
+    Saved: null
+Window Geometry:
+  Displays:
+    collapsed: false
+  Front Left Depth:
+    collapsed: false
+  Front Left RGB:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Left Camera:
+    collapsed: false
+  Left Depth:
+    collapsed: false
+  QMainWindow State: >-
+    000000ff00000000fd0000000400000000000001f5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c900000000000000000000000100000201000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000002ee0000002800fffffffb00000014004c006500660074002000440065007000740068010000032f000001d20000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042000000003b000001360000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068000000003b0000026b0000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b8000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2490
+  X: 1990
+  'Y': 27        trajectory_164305: true
+        trajectory_164794: true
+        trajectory_165283: true
+        trajectory_165772: true
+        trajectory_166261: true
+        trajectory_16627: true
+        trajectory_166750: true
+        trajectory_167239: true
+        trajectory_167728: true
+        trajectory_168217: true
+        trajectory_168706: true
+        trajectory_169195: true
+        trajectory_169684: true
+        trajectory_17116: true
+        trajectory_17605: true
+        trajectory_18094: true
+        trajectory_18583: true
+        trajectory_19072: true
+        trajectory_19561: true
+        trajectory_1957: true
+        trajectory_20050: true
+        trajectory_20539: true
+        trajectory_21028: true
+        trajectory_21517: true
+        trajectory_22006: true
+        trajectory_22495: true
+        trajectory_22984: true
+        trajectory_23473: true
+        trajectory_23962: true
+        trajectory_24451: true
+        trajectory_2446: true
+        trajectory_24940: true
+        trajectory_25429: true
+        trajectory_25918: true
+        trajectory_26407: true
+        trajectory_26896: true
+        trajectory_27385: true
+        trajectory_27874: true
+        trajectory_28363: true
+        trajectory_28852: true
+        trajectory_29341: true
+        trajectory_2935: true
+        trajectory_29830: true
+        trajectory_30319: true
+        trajectory_30808: true
+        trajectory_31297: true
+        trajectory_31786: true
+        trajectory_32275: true
+        trajectory_32764: true
+        trajectory_33253: true
+        trajectory_33742: true
+        trajectory_34231: true
+        trajectory_3424: true
+        trajectory_34720: true
+        trajectory_35209: true
+        trajectory_35698: true
+        trajectory_36187: true
+        trajectory_36676: true
+        trajectory_37165: true
+        trajectory_37654: true
+        trajectory_38143: true
+        trajectory_38632: true
+        trajectory_39121: true
+        trajectory_3913: true
+        trajectory_39610: true
+        trajectory_40099: true
+        trajectory_40588: true
+        trajectory_41077: true
+        trajectory_41566: true
+        trajectory_42055: true
+        trajectory_42544: true
+        trajectory_43033: true
+        trajectory_43522: true
+        trajectory_44011: true
+        trajectory_4402: true
+        trajectory_44500: true
+        trajectory_44989: true
+        trajectory_45478: true
+        trajectory_45967: true
+        trajectory_46456: true
+        trajectory_46945: true
+        trajectory_47434: true
+        trajectory_47923: true
+        trajectory_48412: true
+        trajectory_48901: true
+        trajectory_4891: true
+        trajectory_490: true
+        trajectory_49390: true
+        trajectory_49879: true
+        trajectory_50368: true
+        trajectory_50857: true
+        trajectory_51346: true
+        trajectory_51835: true
+        trajectory_52324: true
+        trajectory_52813: true
+        trajectory_53302: true
+        trajectory_53791: true
+        trajectory_5380: true
+        trajectory_54280: true
+        trajectory_54769: true
+        trajectory_55258: true
+        trajectory_55747: true
+        trajectory_56236: true
+        trajectory_56725: true
+        trajectory_57214: true
+        trajectory_57703: true
+        trajectory_58192: true
+        trajectory_58681: true
+        trajectory_5869: true
+        trajectory_59170: true
+        trajectory_59659: true
+        trajectory_60148: true
+        trajectory_60637: true
+        trajectory_61126: true
+        trajectory_61615: true
+        trajectory_62104: true
+        trajectory_62593: true
+        trajectory_63082: true
+        trajectory_63571: true
+        trajectory_6358: true
+        trajectory_64060: true
+        trajectory_64549: true
+        trajectory_65038: true
+        trajectory_65527: true
+        trajectory_66016: true
+        trajectory_66505: true
+        trajectory_66994: true
+        trajectory_67483: true
+        trajectory_67972: true
+        trajectory_68461: true
+        trajectory_6847: true
+        trajectory_68950: true
+        trajectory_69439: true
+        trajectory_69928: true
+        trajectory_70417: true
+        trajectory_70906: true
+        trajectory_71395: true
+        trajectory_71884: true
+        trajectory_72373: true
+        trajectory_72862: true
+        trajectory_73351: true
+        trajectory_7336: true
+        trajectory_73840: true
+        trajectory_74329: true
+        trajectory_74818: true
+        trajectory_75307: true
+        trajectory_75796: true
+        trajectory_76285: true
+        trajectory_76774: true
+        trajectory_77263: true
+        trajectory_77752: true
+        trajectory_78241: true
+        trajectory_7825: true
+        trajectory_78730: true
+        trajectory_79219: true
+        trajectory_79708: true
+        trajectory_80197: true
+        trajectory_80686: true
+        trajectory_81175: true
+        trajectory_81664: true
+        trajectory_82153: true
+        trajectory_82642: true
+        trajectory_83131: true
+        trajectory_8314: true
+        trajectory_83620: true
+        trajectory_84109: true
+        trajectory_84598: true
+        trajectory_85087: true
+        trajectory_85576: true
+        trajectory_86065: true
+        trajectory_86554: true
+        trajectory_87043: true
+        trajectory_87532: true
+        trajectory_88021: true
+        trajectory_8803: true
+        trajectory_88510: true
+        trajectory_88999: true
+        trajectory_89488: true
+        trajectory_89977: true
+        trajectory_90466: true
+        trajectory_90955: true
+        trajectory_91444: true
+        trajectory_91933: true
+        trajectory_92422: true
+        trajectory_92911: true
+        trajectory_9292: true
+        trajectory_93400: true
+        trajectory_93889: true
+        trajectory_94378: true
+        trajectory_94867: true
+        trajectory_95356: true
+        trajectory_95845: true
+        trajectory_96334: true
+        trajectory_96823: true
+        trajectory_97312: true
+        trajectory_97801: true
+        trajectory_9781: true
+        trajectory_979: true
+        trajectory_98290: true
+        trajectory_98779: true
+        trajectory_99268: true
+        trajectory_99757: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: droan/local_planner_global_plan_vis
+      Value: true
+    - 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/MarkerArray
+      Enabled: true
+      Name: DisparityMarker
+      Namespaces:
+        {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: droan/disparity_marker
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    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: 18.647010803222656
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 5.373880863189697
+        Y: -3.36837100982666
+        Z: -1.463265299797058
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.32039859890937805
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.983567476272583
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Front Left Depth:
+    collapsed: false
+  Front Left RGB:
+    collapsed: false
+  Height: 773
+  Hide Left Dock: true
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002b1000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c9000000000000000000000001000002010000026bfc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042000000003b000001360000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068000000003b0000026b0000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005a00000026b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1440
+  X: 2431
+  Y: 309
diff --git a/ros_ws/src/robot/robot_bringup/config/core.rviz b/ros_ws/src/robot/robot_bringup/config/core.rviz
index 645d54e6..b3620767 100644
--- a/ros_ws/src/robot/robot_bringup/config/core.rviz
+++ b/ros_ws/src/robot/robot_bringup/config/core.rviz
@@ -1,3 +1,4 @@
+---
 Panels:
   - Class: rviz_common/Displays
     Help Height: 78
@@ -5,10 +6,13 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /Grid1
         - /TF1/Frames1
         - /Odometry1/Topic1
-      Splitter Ratio: 0.48235294222831726
-    Tree Height: 752
+        - /ExpansionPoly1
+        - /Left Depth1
+      Splitter Ratio: 0.35953420400619507
+    Tree Height: 1085
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -26,9 +30,9 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Lidar
+    SyncSource: Expansion Cloud
 Visualization Manager:
-  Class: ""
+  Class: ''
   Displays:
     - Alpha: 0.5
       Cell Size: 1
@@ -42,51 +46,51 @@ Visualization Manager:
       Normal Cell Count: 0
       Offset:
         X: 0
-        Y: 0
+        'Y': 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 10
+      Plane Cell Count: 100
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz_default_plugins/TF
       Enabled: true
       Frame Timeout: 15
       Frames:
-        All Enabled: true
+        All Enabled: false
         base_link:
           Value: true
         base_link_frd:
-          Value: true
+          Value: false
         base_link_stabilized:
-          Value: true
+          Value: false
         front_stereo:
-          Value: true
+          Value: false
         left_camera:
           Value: true
         look_ahead_point:
-          Value: true
+          Value: false
         look_ahead_point_stabilized:
-          Value: true
+          Value: false
         map:
           Value: true
         map_FLU:
-          Value: true
+          Value: false
         map_ned:
-          Value: true
+          Value: false
         odom:
-          Value: true
+          Value: false
         odom_ned:
-          Value: true
+          Value: false
         ouster:
-          Value: true
+          Value: false
         right_camera:
           Value: true
         tracking_point:
           Value: true
         tracking_point_stabilized:
-          Value: true
+          Value: false
         world:
-          Value: true
+          Value: false
       Marker Scale: 1
       Name: TF
       Show Arrows: true
@@ -97,27 +101,17 @@ Visualization Manager:
           map_FLU:
             map:
               base_link:
-                base_link_frd:
-                  {}
+                base_link_frd: {}
                 front_stereo:
-                  left_camera:
-                    {}
-                  right_camera:
-                    {}
-                ouster:
-                  {}
-              base_link_stabilized:
-                {}
-              look_ahead_point:
-                {}
-              look_ahead_point_stabilized:
-                {}
-              map_ned:
-                {}
-              tracking_point:
-                {}
-              tracking_point_stabilized:
-                {}
+                  left_camera: {}
+                  right_camera: {}
+                ouster: {}
+              base_link_stabilized: {}
+              look_ahead_point: {}
+              look_ahead_point_stabilized: {}
+              map_ned: {}
+              tracking_point: {}
+              tracking_point_stabilized: {}
       Update Interval: 0
       Value: true
     - Class: rviz_default_plugins/MarkerArray
@@ -133,17 +127,16 @@ Visualization Manager:
         Value: trajectory_controller/trajectory_vis
       Value: true
     - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
+      Enabled: false
       Name: Traj Debug
-      Namespaces:
-        default: true
+      Namespaces: {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: trajectory_controller/trajectory_controller_debug_markers
-      Value: true
+      Value: false
     - Angle Tolerance: 0
       Class: rviz_default_plugins/Odometry
       Covariance:
@@ -161,7 +154,7 @@ Visualization Manager:
           Scale: 1
           Value: true
         Value: true
-      Enabled: true
+      Enabled: false
       Keep: 1
       Name: Odometry
       Position Tolerance: 0
@@ -182,19 +175,19 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: odometry_conversion/odometry
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/Marker
       Enabled: true
       Name: VDB Mapping Marker
       Namespaces:
-        "": true
+        '': true
       Topic:
         Depth: 5
         Durability Policy: Volatile
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /robot_1/vdb_mapping/vdb_map_visualization
+        Value: vdb_mapping/vdb_map_visualization
       Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -226,12 +219,12 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /robot_1/sensors/ouster/point_cloud
+        Value: sensors/ouster/point_cloud
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
     - Class: rviz_default_plugins/Image
-      Enabled: true
+      Enabled: false
       Max Value: 100
       Median window: 5
       Min Value: 0
@@ -242,14 +235,106 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /robot_1/sensors/front_stereo/left/depth
+        Value: sensors/front_stereo/left/depth
+      Value: false
+    - Class: rviz_default_plugins/Image
+      Enabled: false
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Front Left RGB
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/front_stereo/left/image_rect
+      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
+    - 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: 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: true
+    - 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_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/MarkerArray
+      Enabled: true
+      Name: DisparityMarker
+      Namespaces: {}
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: droan/disparity_marker
       Value: true
     - Class: rviz_default_plugins/Image
       Enabled: true
       Max Value: 1
       Median window: 5
       Min Value: 0
-      Name: Front Left RGB
+      Name: Left Camera
       Normalize Range: true
       Topic:
         Depth: 5
@@ -258,6 +343,20 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: sensors/front_stereo/left/image_rect
       Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 100
+      Median window: 5
+      Min Value: 0
+      Name: Left Depth
+      Normalize Range: false
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: sensors/front_stereo/left/depth
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -304,26 +403,26 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 23.79106903076172
+      Distance: 43.695953369140625
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -2.7142093181610107
-        Y: 2.349794387817383
-        Z: -0.7724144458770752
+        X: -1.6335676908493042
+        'Y': -2.40643310546875
+        Z: -2.289210796356201
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.49539828300476074
+      Pitch: 0.6753983497619629
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.3035807609558105
-    Saved: ~
+      Yaw: 0.6135711669921875
+    Saved: null
 Window Geometry:
   Displays:
     collapsed: false
@@ -331,10 +430,15 @@ Window Geometry:
     collapsed: false
   Front Left RGB:
     collapsed: false
-  Height: 1043
+  Height: 1376
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c90000000000000000000000010000020100000379fc0200000005fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042010000003b000001be0000002800fffffffb0000002000460072006f006e00740020004c00650066007400200044006500700074006801000001ff000001b50000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000041d0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Left Camera:
+    collapsed: false
+  Left Depth:
+    collapsed: false
+  QMainWindow State: >-
+    000000ff00000000fd0000000400000000000001f5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c900000000000000000000000100000201000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000002ee0000002800fffffffb00000014004c006500660074002000440065007000740068010000032f000001d20000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042000000003b000001360000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068000000003b0000026b0000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005b8000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -343,6 +447,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 0
-  Y: 360
+  Width: 2490
+  X: 1990
+  'Y': 27
\ No newline at end of file