diff --git a/.env b/.env
index 6c47f55cf..23c886d92 100644
--- a/.env
+++ b/.env
@@ -11,7 +11,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.
-VERSION="0.18.0-alpha.3"
+VERSION="0.18.0-alpha.4"
 # 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.
@@ -30,7 +30,7 @@ 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"
 PLAY_SIM_ON_START="false"
diff --git a/.gitmodules b/.gitmodules
index 0b29f3486..4314795a4 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -8,9 +8,12 @@
 	path = robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo
 	url = https://github.com/MAC-VO/MAC-VO.git
 [submodule "robot/ros_ws/src/rviz_behavior_tree_panel/xdot_cpp"]
-	path = common/ros_packages/gui/rviz_behavior_tree_panel/xdot_cpp
+	path = common/ros_packages/gui/rviz/rviz_behavior_tree_panel/xdot_cpp
 	url = https://github.com/castacks/xdot_cpp.git
 [submodule "simulation/isaac-sim/extensions/PegasusSimulator"]
 	path = simulation/isaac-sim/extensions/PegasusSimulator
 	url = https://github.com/castacks/PegasusSimulator-AirStack-Integration.git
 
+[submodule "common/ros_packages/gui/rviz/rviz_polygon_selection_tool"]
+	path = common/ros_packages/gui/rviz/rviz_polygon_selection_tool
+	url = https://github.com/swri-robotics/rviz_polygon_selection_tool.git
diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
index ac3dd2f2a..f0876e990 100644
--- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml
@@ -12,24 +12,6 @@
   <?ignore
   <node pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share desktop_bringup)/rviz/gcs.rviz --ros-args --log-level INFO" output="screen" respawn="true" respawn_delay="1" />
     ?>
-  
-
-  <node pkg="rqt_gui" exec="rqt_gui"
-    args="--perspective-file $(find-pkg-share rqt_gcs)/config/gcs.perspective --ros-args --log-level INFO"
-    output="screen" respawn="true" respawn_delay="1" />
-
-  <?ignore
-  <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="use_sim_time" value="$(var sim)" />
-  </node>
-  ?>
-
-
-   
-
   <!-- ===================== -->
 
 </launch>
\ No newline at end of file
diff --git a/common/ros_packages/desktop_bringup/launch/robot.launch.xml b/common/ros_packages/desktop_bringup/launch/robot.launch.xml
index 53bf44922..2d92337bb 100644
--- a/common/ros_packages/desktop_bringup/launch/robot.launch.xml
+++ b/common/ros_packages/desktop_bringup/launch/robot.launch.xml
@@ -16,13 +16,6 @@
     <node pkg="rviz2" exec="rviz2"
       args="-d $(find-pkg-share desktop_bringup)/rviz/robot.rviz --ros-args --log-level INFO"
       output="screen" respawn="true" respawn_delay="1" />
-
-    <push-ros-namespace namespace="behavior" />
-    <node pkg="rqt_gui" exec="rqt_gui"
-      args="--perspective-file $(find-pkg-share desktop_bringup)/config/core.perspective">
-      <remap from="fixed_trajectory_command"
-        to="/$(env ROBOT_NAME)/fixed_trajectory_generator/fixed_trajectory_command" />
-    </node>
   </group>
 
 
diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz
index 183df82e9..6ded1ee37 100644
--- a/common/ros_packages/desktop_bringup/rviz/robot.rviz
+++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz
@@ -1,27 +1,24 @@
 Panels:
   - Class: rviz_common/Displays
-    Help Height: 70
+    Help Height: 0
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-        - /Sensors1
-        - /Perception1/StereoImageProc Disparity1
+        - /Local1
         - /Local1/DROAN1
         - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
         - /Local1/DROAN1/Droan GPU1
         - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1
-        - /Global1
+        - /Local1/DROAN1/Global Plan Vis1
         - /Global1/Global Plan1
-      Splitter Ratio: 0.3440559506416321
-    Tree Height: 480
+      Splitter Ratio: 0.5792563557624817
+    Tree Height: 693
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
+    Expanded: ~
     Name: Tool Properties
     Splitter Ratio: 0.5886790156364441
   - Class: rviz_common/Views
@@ -33,11 +30,23 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: StereoImageProc Disparity
+    SyncSource: Foreground Background Cloud
   - Class: rviz_behavior_tree_panel::BehaviorTreePanel
     Name: BehaviorTreePanel
-    topic: behavior/behavior_tree_graphviz
+    topic: /robot_1/behavior/behavior_tree_graphviz
     zoom_factor: 0.1919851303100586
+  - Class: rviz_tasks_panel::TasksPanel
+    Name: TasksPanel
+    executor_0: /robot_1/tasks/takeoff
+    executor_1: /robot_1/tasks/land
+    executor_2: /robot_1/tasks/navigate
+    executor_3: /robot_1/tasks/exploration
+    executor_4: ""
+    executor_5: ""
+    executor_6: ""
+    executor_7: ""
+    executor_8: /robot_1/tasks/fixed_trajectory
+    robot: /robot_1
 Visualization Manager:
   Class: ""
   Displays:
@@ -77,7 +86,7 @@ Visualization Manager:
         base_link_frd:
           Value: true
         base_link_stabilized:
-          Value: false
+          Value: true
         camera_left:
           Value: true
         camera_right:
@@ -87,11 +96,9 @@ Visualization Manager:
         lidar:
           Value: false
         look_ahead_point:
-          Value: false
+          Value: true
         look_ahead_point_stabilized:
-          Value: false
-        macvo_ned:
-          Value: false
+          Value: true
         map:
           Value: true
         map_ned:
@@ -109,9 +116,9 @@ Visualization Manager:
         rotor3:
           Value: true
         tracking_point:
-          Value: false
+          Value: true
         tracking_point_stabilized:
-          Value: false
+          Value: true
         world:
           Value: false
       Marker Scale: 0.30000001192092896
@@ -129,8 +136,7 @@ Visualization Manager:
                     {}
                 base_link_ZED_X:
                   camera_left:
-                    macvo_ned:
-                      {}
+                    {}
                   camera_right:
                     {}
                   imu:
@@ -476,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
@@ -641,7 +648,7 @@ Visualization Manager:
                     Reliability Policy: Reliable
                     Value: droan/graph_vis
                   Value: true
-              Enabled: true
+              Enabled: false
               Name: Droan GPU
             - Class: rviz_default_plugins/MarkerArray
               Enabled: true
@@ -659,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
@@ -671,7 +679,7 @@ Visualization Manager:
               Enabled: true
               Name: Rewind Info
               Namespaces:
-                {}
+                rewind_info: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -710,6 +718,15 @@ Visualization Manager:
               Value: false
           Enabled: true
           Name: Trajectory Controller
+        - Class: rviz_default_plugins/InteractiveMarkers
+          Enable Transparency: true
+          Enabled: true
+          Interactive Markers Namespace: /robot_1/waypoint_plugin
+          Name: Waypoints InteractiveMarkers
+          Show Axes: true
+          Show Descriptions: true
+          Show Visual Aids: false
+          Value: true
       Enabled: true
       Name: Local
     - Class: rviz_common/Group
@@ -757,20 +774,6 @@ Visualization Manager:
           Value: true
       Enabled: true
       Name: Global
-    - Class: rviz_default_plugins/Image
-      Enabled: true
-      Max Value: 1
-      Median window: 5
-      Min Value: 0
-      Name: Image
-      Normalize Range: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: sensors/front_stereo/left/image_rect
-      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -810,6 +813,23 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /clicked_point
+    - Class: rviz_polygon_selection_tool/PolygonSelectionTool
+      Close loop: true
+      Lasso mode: true
+      Line Color: 255; 0; 0
+      Patch size: 15
+      Point Color: 255; 0; 0
+      Point Generation Gap: 0.0020000000949949026
+      Point Size: 50
+      Render as Overlay: false
+      Service name: get_selection
+      Show Text: true
+      Text Size: 0.014999999664723873
+    - Class: waypoint_rviz2_plugin/WaypointTool
+      WaypointTool:
+        default_height: 10
+        frame_id: map
+        topic: waypoints
   Transformation:
     Current:
       Class: rviz_default_plugins/TF
@@ -817,25 +837,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 26.301408767700195
+      Distance: 37.564666748046875
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 6.924289703369141
-        Y: -7.847400188446045
-        Z: -4.3323540687561035
+        X: 25.773038864135742
+        Y: -5.22532320022583
+        Z: -18.542400360107422
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5103968977928162
+      Pitch: 0.6403977274894714
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 2.279876232147217
+      Yaw: 2.878566026687622
     Saved: ~
 Window Geometry:
   BehaviorTreePanel:
@@ -849,13 +869,13 @@ Window Geometry:
   Height: 1376
   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
+  TasksPanel:
+    collapsed: false
   Time:
     collapsed: false
   Tool Properties:
diff --git a/common/ros_packages/gui/rqt_behavior_tree/resource/rqt_behavior_tree b/common/ros_packages/gui/foxglove/.gitkeep
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/resource/rqt_behavior_tree
rename to common/ros_packages/gui/foxglove/.gitkeep
diff --git a/common/ros_packages/gui/rqt_behavior_tree/.gitignore b/common/ros_packages/gui/rqt/rqt_behavior_tree/.gitignore
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/.gitignore
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/.gitignore
diff --git a/common/ros_packages/gui/rqt_behavior_tree/CHANGELOG.rst b/common/ros_packages/gui/rqt/rqt_behavior_tree/CHANGELOG.rst
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/CHANGELOG.rst
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/CHANGELOG.rst
diff --git a/common/ros_packages/gui/rqt_behavior_tree/package.xml b/common/ros_packages/gui/rqt/rqt_behavior_tree/package.xml
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/package.xml
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/package.xml
diff --git a/common/ros_packages/gui/rqt_behavior_tree/plugin.xml b/common/ros_packages/gui/rqt/rqt_behavior_tree/plugin.xml
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/plugin.xml
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/plugin.xml
diff --git a/common/ros_packages/gui/rqt_behavior_tree/resource/py_console_widget.ui b/common/ros_packages/gui/rqt/rqt_behavior_tree/resource/py_console_widget.ui
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/resource/py_console_widget.ui
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/resource/py_console_widget.ui
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/resource/rqt_behavior_tree
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/resource/rqt_behavior_tree
diff --git a/common/ros_packages/gui/rqt_behavior_tree/setup.cfg b/common/ros_packages/gui/rqt/rqt_behavior_tree/setup.cfg
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/setup.cfg
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/setup.cfg
diff --git a/common/ros_packages/gui/rqt_behavior_tree/setup.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/setup.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/setup.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/setup.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/main.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/main.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/main.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/main.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/test.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/test.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/test.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/test.py
diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/__init__.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/__init__.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py
diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py
similarity index 100%
rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py
rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py
diff --git a/common/ros_packages/gui/rqt_py_template/CHANGELOG.rst b/common/ros_packages/gui/rqt/rqt_py_template/CHANGELOG.rst
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/CHANGELOG.rst
rename to common/ros_packages/gui/rqt/rqt_py_template/CHANGELOG.rst
diff --git a/common/ros_packages/gui/rqt_py_template/README.md b/common/ros_packages/gui/rqt/rqt_py_template/README.md
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/README.md
rename to common/ros_packages/gui/rqt/rqt_py_template/README.md
diff --git a/common/ros_packages/gui/rqt_py_template/generate_rqt_py_package.sh b/common/ros_packages/gui/rqt/rqt_py_template/generate_rqt_py_package.sh
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/generate_rqt_py_package.sh
rename to common/ros_packages/gui/rqt/rqt_py_template/generate_rqt_py_package.sh
diff --git a/common/ros_packages/gui/rqt_py_template/package.xml b/common/ros_packages/gui/rqt/rqt_py_template/package.xml
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/package.xml
rename to common/ros_packages/gui/rqt/rqt_py_template/package.xml
diff --git a/common/ros_packages/gui/rqt_py_template/plugin.xml b/common/ros_packages/gui/rqt/rqt_py_template/plugin.xml
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/plugin.xml
rename to common/ros_packages/gui/rqt/rqt_py_template/plugin.xml
diff --git a/common/ros_packages/gui/rqt_py_template/resource/py_console_widget.ui b/common/ros_packages/gui/rqt/rqt_py_template/resource/py_console_widget.ui
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/resource/py_console_widget.ui
rename to common/ros_packages/gui/rqt/rqt_py_template/resource/py_console_widget.ui
diff --git a/common/ros_packages/gui/rqt_py_template/resource/rqt_py_template b/common/ros_packages/gui/rqt/rqt_py_template/resource/rqt_py_template
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/resource/rqt_py_template
rename to common/ros_packages/gui/rqt/rqt_py_template/resource/rqt_py_template
diff --git a/common/ros_packages/gui/rqt_py_template/setup.cfg b/common/ros_packages/gui/rqt/rqt_py_template/setup.cfg
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/setup.cfg
rename to common/ros_packages/gui/rqt/rqt_py_template/setup.cfg
diff --git a/common/ros_packages/gui/rqt_py_template/setup.py b/common/ros_packages/gui/rqt/rqt_py_template/setup.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/setup.py
rename to common/ros_packages/gui/rqt/rqt_py_template/setup.py
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/__init__.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/__init__.py
similarity index 100%
rename from robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/__init__.py
rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/__init__.py
diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/main.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/main.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/main.py
rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/main.py
diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_text_edit.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_text_edit.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_text_edit.py
rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_text_edit.py
diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_widget.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_widget.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_widget.py
rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_widget.py
diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/spyder_console_widget.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/spyder_console_widget.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/spyder_console_widget.py
rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/spyder_console_widget.py
diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/template.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/template.py
similarity index 100%
rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/template.py
rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/template.py
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/.gitignore b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/.gitignore
new file mode 100644
index 000000000..88cd635a1
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/.gitignore
@@ -0,0 +1,39 @@
+# Build artifacts
+build/
+install/
+log/
+
+# IDE files
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# Qt generated files
+*.autosave
+moc_*.cpp
+moc_*.h
+ui_*.h
+
+# Bag files
+*.bag
+*.db3
+*.db3-shm
+*.db3-wal
+
+# CMake
+CMakeCache.txt
+CMakeFiles/
+cmake_install.cmake
+Makefile
+
+# Python
+__pycache__/
+*.pyc
+*.pyo
+*.pyd
+
+# OS files
+.DS_Store
+Thumbs.db
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/CMakeLists.txt b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/CMakeLists.txt
new file mode 100644
index 000000000..c19c09c71
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/CMakeLists.txt
@@ -0,0 +1,119 @@
+cmake_minimum_required(VERSION 3.5)
+project(waypoint_rviz2_plugin)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(rviz_ogre_vendor REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(interactive_markers REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(pluginlib REQUIRED)
+
+# Qt5
+find_package(Qt5 REQUIRED COMPONENTS Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+
+# Qt signals and slots to avoid defining "emit", "slots"
+add_definitions(-DQT_NO_KEYWORDS)
+
+# Header files that need Qt Moc pre-processing
+qt5_wrap_cpp(MOC_FILES
+  include/waypoint_rviz2_plugin/waypoint_manager.hpp
+  include/waypoint_rviz2_plugin/waypoint_tool.hpp
+)
+
+# Plugin source files
+set(SOURCES
+  src/waypoint_manager.cpp
+  src/waypoint_tool.cpp
+  ${MOC_FILES}
+)
+
+# Declare the library
+add_library(${PROJECT_NAME} SHARED
+  ${SOURCES}
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Dependencies
+set(DEPENDENCIES
+  rclcpp
+  rviz_common
+  rviz_default_plugins
+  rviz_rendering
+  rviz_ogre_vendor
+  geometry_msgs
+  visualization_msgs
+  interactive_markers
+  nav_msgs
+  tf2
+  tf2_geometry_msgs
+  rosbag2_cpp
+  pluginlib
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  ${DEPENDENCIES}
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
+)
+
+# Prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+# Export plugin description file for pluginlib
+pluginlib_export_plugin_description_file(rviz_common plugin_description.xml)
+
+# Install
+install(TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(FILES plugin_description.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(DIRECTORY media/
+  DESTINATION share/${PROJECT_NAME}/media
+)
+
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+# Export
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${DEPENDENCIES})
+
+ament_package()
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/README.md b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/README.md
new file mode 100644
index 000000000..57ecd587b
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/README.md
@@ -0,0 +1,142 @@
+# 3D Waypoint RViz2 Plugin
+
+An interactive RViz2 tool plugin for creating, manipulating, and managing 3D waypoints directly in the RViz2 viewport. Waypoint state is managed by a shared `WaypointManager` singleton so that both this tool (3D interaction) and the Tasks Panel's Navigate tab (UI controls) operate on the same data.
+
+![Waypoint Tool Demo](rviz_task_panel_waypoints.png)
+
+## Architecture
+
+```
+ WaypointTool (rviz_common::Tool)        Tasks Panel Navigate tab
+   mouse click in 3D viewport              UI: height, X/Y/Z/Yaw,
+         |                                 Clear, Save, Load, Execute
+         v                                        |
+     +------------------------------------------+
+     |          WaypointManager (singleton)      |
+     |                                          |
+     |  - InteractiveMarkerServer               |
+     |  - Ogre SceneNode map (waypoint visuals) |
+     |  - add / remove / clear / reorder        |
+     |  - save / load (.db3 bag files)          |
+     |  - getPath() -> nav_msgs/Path            |
+     |  - Qt signals for UI updates             |
+     +------------------------------------------+
+```
+
+- **WaypointTool** (`rviz_common::Tool`) -- handles mouse events in the 3D viewport. Left-click to add, right-click near an existing waypoint to delete. Delegates all state to `WaypointManager`.
+- **WaypointManager** (`QObject` singleton) -- owns all waypoint state: the interactive marker server, the Ogre scene nodes, persistence, and publishing. Emits Qt signals (`waypointCountChanged`, `selectedMarkerChanged`, `waypointsCleared`) so external UI can stay in sync.
+- The **Tasks Panel** (separate `rviz_tasks_panel` package) holds a `shared_ptr` to the same `WaypointManager` instance and provides the full control UI in its Navigate tab.
+
+## Features
+
+- **Interactive Waypoint Creation**: Click to add waypoints at any position in the 3D environment
+- **3D Manipulation**: Drag and move waypoints using interactive markers (MOVE_ROTATE, MOVE_AXIS)
+- **Orientation Control**: Set and adjust waypoint yaw via interactive markers or Navigate tab spinboxes
+- **Visual Feedback**: Real-time axis indicator meshes at each waypoint
+- **Waypoint Management**:
+  - Add waypoints with left-click
+  - Delete waypoints with right-click or context menu
+  - "Set manual" context menu selects the waypoint for editing in the Navigate tab
+  - Automatic reordering after deletion (no gaps in numbering)
+  - Clear all waypoints at once
+- **Data Persistence**: Save and load waypoint configurations using ROS 2 bag files (.db3)
+- **Publishing**: Publish waypoints as `nav_msgs/msg/Path` messages
+- **Configurable**: Default height, frame ID, and topic name (persisted in RViz config)
+
+## Requirements
+
+- ROS 2 Jazzy (or Humble and above)
+- RViz2
+- Qt5
+- C++17
+
+## Dependencies
+
+- rclcpp
+- rviz_common
+- rviz_default_plugins
+- rviz_rendering
+- rviz_ogre_vendor
+- geometry_msgs
+- visualization_msgs
+- interactive_markers
+- nav_msgs
+- tf2
+- tf2_geometry_msgs
+- rosbag2_cpp
+- pluginlib
+
+## Building
+
+```bash
+colcon build --packages-select waypoint_rviz2_plugin
+```
+
+## Usage
+
+### Adding the Tool to RViz2
+
+1. Open RViz2
+2. In the toolbar, click the "+" icon to add a new tool
+3. Select **waypoint_rviz2_plugin / WaypointTool** from the list
+4. The tool appears in the toolbar with keyboard shortcut **1**
+
+### Placing Waypoints
+
+1. **Select the Tool**: Click the Waypoint Tool icon or press **1**
+2. **Add Waypoints**: Left-click anywhere in the 3D view
+3. **Move Waypoints**: Add the interactive marker display (Add > By topic > `/waypoint_plugin/update/InteractiveMarkers`), then drag markers to reposition
+4. **Delete Waypoints**: Right-click near a waypoint, or use the context menu "delete"
+5. **Select for Editing**: Right-click a marker and choose "set manual" to select it in the Navigate tab
+
+### Navigate Tab Controls (in Tasks Panel)
+
+All waypoint management UI lives in the Tasks Panel's **Navigate** tab:
+
+- **Default Height** -- Z-height for newly placed waypoints
+- **X / Y / Z / Yaw spinboxes** -- edit the currently selected waypoint's pose
+- **Clear All** -- remove all waypoints
+- **Save / Load** -- persist waypoints to/from `.db3` bag files
+- **Waypoint count** and **selected waypoint** labels update in real time
+
+### Listening to Published Waypoints
+
+```bash
+ros2 topic echo /waypoints
+```
+
+## File Structure
+
+```
+waypoint_rviz2_plugin/
+├── CMakeLists.txt
+├── package.xml
+├── plugin_description.xml
+├── README.md
+├── include/
+│   └── waypoint_rviz2_plugin/
+│       ├── waypoint_manager.hpp   # Shared singleton (state + operations)
+│       └── waypoint_tool.hpp      # RViz Tool (mouse events only)
+├── src/
+│   ├── waypoint_manager.cpp
+│   └── waypoint_tool.cpp
+├── media/
+│   └── axis.dae                   # 3D axis indicator mesh
+└── launch/
+    └── rviz2.launch.py
+```
+
+## License
+
+Apache 2.0
+
+## Credits
+
+Original ROS 1 version by KoKoLates (the21515@gmail.com).
+
+ROS 2 port and WaypointManager refactor for AirStack (Andrew Jong ajong@andrew.cmu.edu).
+
+## References
+
+- [RViz2 Plugin Tutorial](https://docs.ros.org/en/jazzy/Tutorials/Advanced/RViz-Plugins.html)
+- [Interactive Markers](https://docs.ros.org/en/jazzy/Tutorials/Advanced/RViz/Interactive-Markers.html)
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_manager.hpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_manager.hpp
new file mode 100644
index 000000000..9001d1592
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_manager.hpp
@@ -0,0 +1,128 @@
+/**
+ * @file waypoint_manager.hpp
+ *
+ * Singleton that owns all waypoint state (interactive markers, Ogre scene
+ * nodes, persistence).  Both WaypointTool (3-D mouse interaction) and the
+ * Tasks Panel Navigate tab (UI controls) hold a shared_ptr to the same
+ * instance so they share a single source of truth.
+ */
+
+#pragma once
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <OgreQuaternion.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreVector3.h>
+#endif
+
+#include <QObject>
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <visualization_msgs/msg/interactive_marker_feedback.hpp>
+#include <interactive_markers/interactive_marker_server.hpp>
+#include <interactive_markers/menu_handler.hpp>
+
+namespace rviz_common {
+class DisplayContext;
+}
+
+namespace waypoint_rviz2_plugin {
+
+constexpr char kWaypointPrefix[] = "point#";
+
+class WaypointManager : public QObject
+{
+  Q_OBJECT
+
+public:
+  // ── singleton ──────────────────────────────────────────────────────────────
+  static std::shared_ptr<WaypointManager> instance();
+
+  ~WaypointManager() override;
+
+  /** Called once by WaypointTool::onInitialize() which has the DisplayContext. */
+  void initialize(rviz_common::DisplayContext * context,
+                  rclcpp::Node::SharedPtr node);
+  bool isInitialized() const { return initialized_; }
+
+  // ── waypoint operations ────────────────────────────────────────────────────
+  void addWaypoint(const Ogre::Vector3 & position, const Ogre::Quaternion & quaternion);
+  void removeWaypoint(int idx);
+  void clearAll();
+
+  /** Build a nav_msgs/Path from current waypoint state. */
+  nav_msgs::msg::Path getPath() const;
+
+  /** Publish current waypoints on the configured topic. */
+  void publishPath();
+
+  void saveBag(const std::string & filename);
+  void loadBag(const std::string & filename);
+
+  /** Update pose of the currently-selected marker from spinbox values. */
+  void updateSelectedPose(double x, double y, double z, double yaw);
+
+  // ── accessors / mutators ───────────────────────────────────────────────────
+  std::string frameId() const;
+  void setFrameId(const std::string & frame);
+
+  std::string outputTopic() const;
+  void setOutputTopic(const std::string & topic);
+
+  double defaultHeight() const;
+  void setDefaultHeight(double h);
+
+  int waypointCount() const;
+
+  /** Read-only access to the node map (used by WaypointTool for proximity). */
+  const std::map<int, Ogre::SceneNode *> & nodeMap() const { return node_map_; }
+
+  /** Expose the interactive marker server (needed by WaypointTool for menu). */
+  std::shared_ptr<interactive_markers::InteractiveMarkerServer> server() const { return server_; }
+
+  interactive_markers::MenuHandler & menuHandler() { return menu_handler_; }
+
+Q_SIGNALS:
+  void waypointCountChanged(int count);
+  void selectedMarkerChanged(const QString & name, double x, double y, double z, double yaw);
+  void waypointsCleared();
+
+private:
+  WaypointManager();  // private – use instance()
+
+  void processFeedback(
+    const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback);
+
+  /** Rebuild node_map_ with sequential 1-based keys and rename interactive
+   *  markers so there are no gaps after a deletion. */
+  void reorderWaypoints();
+
+  bool initialized_{false};
+  rclcpp::Node::SharedPtr node_;
+  rviz_common::DisplayContext * context_{nullptr};
+  Ogre::SceneManager * scene_manager_{nullptr};
+
+  std::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
+  interactive_markers::MenuHandler menu_handler_;
+
+  std::map<int, Ogre::SceneNode *> node_map_;
+  int unique_idx_{0};
+
+  std::string frame_id_{"map"};
+  std::string output_topic_{"/waypoints"};
+  double default_height_{0.0};
+  std::string selected_marker_name_;
+
+  std::string axis_resource_;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
+
+  mutable std::mutex mtx_;
+};
+
+}  // namespace waypoint_rviz2_plugin
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_tool.hpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_tool.hpp
new file mode 100644
index 000000000..b599df90f
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_tool.hpp
@@ -0,0 +1,52 @@
+/**
+ * @file waypoint_tool.hpp
+ * @author KoKoLates (the21515@gmail.com)
+ * @version 2.0.0
+ *
+ * RViz2 Tool for placing/removing waypoints via mouse clicks in the 3-D
+ * viewport.  All state is owned by WaypointManager; this class only converts
+ * mouse events into manager calls.
+ */
+
+#pragma once
+
+#ifndef Q_MOC_RUN
+#include <OgreSceneNode.h>
+#include <OgreVector3.h>
+#endif
+
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/tool.hpp>
+
+#include "waypoint_manager.hpp"
+
+namespace rviz_common {
+class DisplayContext;
+class ViewportMouseEvent;
+}
+
+namespace waypoint_rviz2_plugin {
+
+class WaypointTool : public rviz_common::Tool
+{
+  Q_OBJECT
+public:
+  WaypointTool();
+  ~WaypointTool() override;
+
+  void onInitialize() override;
+  void activate() override;
+  void deactivate() override;
+  int processMouseEvent(rviz_common::ViewportMouseEvent & event) override;
+
+  void load(const rviz_common::Config & config) override;
+  void save(rviz_common::Config config) const override;
+
+private:
+  Ogre::SceneNode * move_axis_node_{nullptr};
+  std::shared_ptr<WaypointManager> manager_;
+  rclcpp::Node::SharedPtr node_;
+};
+
+}  // namespace waypoint_rviz2_plugin
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_widget.hpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_widget.hpp
new file mode 100644
index 000000000..89544e07d
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_widget.hpp
@@ -0,0 +1,108 @@
+/**
+ * @file waypoint_widget.hpp
+ * @author KoKoLates (the21515@gmail.com)
+ * @version 1.0.0
+ * @date 2023-12-10
+ * 
+ * ROS2 port of the interactive waypoint widget for RViz2
+ */
+
+#pragma once
+
+#ifndef Q_MOC_RUN
+#include <mutex>
+#include <OgreVector3.h>
+#include <OgreQuaternion.h>
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+#endif
+
+#include <QWidget>
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/path.hpp>
+
+#include "ui_waypoint_plugin.h"
+
+namespace rviz_common {
+    class DisplayContext;
+}
+
+namespace interactive_markers {
+    class InteractiveMarkerServer;
+}
+
+namespace Ui {
+    class PluginWidget;
+}
+
+namespace waypoint_rviz2_plugin {
+    class WaypointTool;
+}
+
+namespace waypoint_rviz2_plugin {
+    constexpr char waypoint_name_prefix[] = "point#";
+    
+    class WaypointWidget: public QWidget {
+        friend class WaypointTool;
+        Q_OBJECT
+
+    public: 
+        WaypointWidget(rviz_common::DisplayContext *context, 
+                       rclcpp::Node::SharedPtr node,
+                       std::map<int, Ogre::SceneNode*> *map_ptr, 
+                       std::shared_ptr<interactive_markers::InteractiveMarkerServer> server, 
+                       int *unique_idx, 
+                       QWidget *parent=nullptr, 
+                       WaypointTool *waypoint_tool=nullptr);
+        ~WaypointWidget();
+
+        void enable();
+        void disable();
+    
+        void setPose(const Ogre::Vector3&, const Ogre::Quaternion&);
+        void setConfig(QString topic, QString frame, float height);
+        void setWaypointLabel(Ogre::Vector3 position);
+        void setWaypointCount(int size);
+        void setSelectedMarkerName(std::string name);
+
+        void getPose(Ogre::Vector3&, Ogre::Quaternion&);
+        QString getFrameId();
+        QString getOutputTopic();
+        double getDefaultHeight();
+
+        void saveBag(const std::string& filename);
+        void loadBag(const std::string& filename);
+
+    protected:
+        Ui::PluginWidget *ui_;
+        rviz_common::DisplayContext *context_;
+
+    private:
+        rclcpp::Node::SharedPtr node_;
+        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
+
+        WaypointTool *waypoint_tool_;
+        std::map<int, Ogre::SceneNode*> *map_ptr_;
+        Ogre::SceneManager *scene_manager_;
+        std::shared_ptr<interactive_markers::InteractiveMarkerServer> server_;
+
+        int* unique_idx_;
+        double default_height_;
+
+        QString output_topic_;
+        QString frame_id_;
+
+        std::mutex frame_updates_mutex_;
+        std::string selected_marker_name_;
+
+    private Q_SLOTS:
+        void saveHandler();
+        void loadHandler();
+        void clearHandler();
+        void publishHandler();
+        void frameChange();
+        void topicChange();
+        void poseChange(double value);
+        void heightChange(double height);
+    };
+}
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/launch/rviz2.launch.py b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/launch/rviz2.launch.py
new file mode 100644
index 000000000..39284886d
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/launch/rviz2.launch.py
@@ -0,0 +1,28 @@
+"""
+RViz2 launch file for waypoint plugin demonstration
+"""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('waypoint_rviz2_plugin')
+    
+    rviz_config_file = os.path.join(pkg_share, 'config', 'waypoint_demo.rviz')
+    
+    use_rviz_config = os.path.exists(rviz_config_file)
+    
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        arguments=['-d', rviz_config_file] if use_rviz_config else [],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        rviz_node
+    ])
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/media/axis.dae b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/media/axis.dae
new file mode 100644
index 000000000..a2f7b2ddc
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/media/axis.dae
@@ -0,0 +1,400 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 3.5.1 commit date:2023-04-24, commit time:18:11, hash:e1ccd9d4a1d3</authoring_tool>
+    </contributor>
+    <created>2023-06-25T21:02:12</created>
+    <modified>2023-06-25T21:02:12</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_cameras>
+    <camera id="Camera_001-camera" name="Camera.001">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+    <camera id="Camera-camera" name="Camera">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+  </library_cameras>
+  <library_lights>
+    <light id="Light_001-light" name="Light.001">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0.00111109</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">1</mode>
+          <gamma sid="blender_gamma" type="float">1</gamma>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <shadow_r sid="blender_shadow_r" type="float">0</shadow_r>
+          <shadow_g sid="blender_shadow_g" type="float">0</shadow_g>
+          <shadow_b sid="blender_shadow_b" type="float">0</shadow_b>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <dist sid="blender_dist" type="float">29.99998</dist>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <att1 sid="att1" type="float">0</att1>
+          <att2 sid="att2" type="float">1</att2>
+          <falloff_type sid="falloff_type" type="int">2</falloff_type>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">30.002</clipend>
+          <bias sid="bias" type="float">1</bias>
+          <bufsize sid="bufsize" type="int">2880</bufsize>
+          <samp sid="samp" type="int">3</samp>
+          <buffers sid="buffers" type="int">1</buffers>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+    <light id="Light-light" name="Light">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0.00111109</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">1</mode>
+          <gamma sid="blender_gamma" type="float">1</gamma>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <shadow_r sid="blender_shadow_r" type="float">0</shadow_r>
+          <shadow_g sid="blender_shadow_g" type="float">0</shadow_g>
+          <shadow_b sid="blender_shadow_b" type="float">0</shadow_b>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <dist sid="blender_dist" type="float">29.99998</dist>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <att1 sid="att1" type="float">0</att1>
+          <att2 sid="att2" type="float">1</att2>
+          <falloff_type sid="falloff_type" type="int">2</falloff_type>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">30.002</clipend>
+          <bias sid="bias" type="float">1</bias>
+          <bufsize sid="bufsize" type="int">2880</bufsize>
+          <samp sid="samp" type="int">3</samp>
+          <buffers sid="buffers" type="int">1</buffers>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+  </library_lights>
+  <library_effects>
+    <effect id="材質_001-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">0 0 1 1</color>
+            </diffuse>
+            <reflectivity>
+              <float sid="specular">0.5</float>
+            </reflectivity>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+    <effect id="材質-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">1 0 0 1</color>
+            </diffuse>
+            <reflectivity>
+              <float sid="specular">0.5</float>
+            </reflectivity>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+    <effect id="材質_002-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">0 1 0 1</color>
+            </diffuse>
+            <reflectivity>
+              <float sid="specular">0.5</float>
+            </reflectivity>
+            <index_of_refraction>
+              <float sid="ior">1.45</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images/>
+  <library_materials>
+    <material id="材質_001-material" name="材質.001">
+      <instance_effect url="#材質_001-effect"/>
+    </material>
+    <material id="材質-material" name="材質">
+      <instance_effect url="#材質-effect"/>
+    </material>
+    <material id="材質_002-material" name="材質.002">
+      <instance_effect url="#材質_002-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="圓柱體-mesh" name="圓柱體">
+      <mesh>
+        <source id="圓柱體-mesh-positions">
+          <float_array id="圓柱體-mesh-positions-array" count="192">0 1 -1 0 1 1 0.1950903 0.9807853 -1 0.1950903 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314696 0.5555703 -1 0.8314696 0.5555703 1 0.9238795 0.3826835 -1 0.9238795 0.3826835 1 0.9807853 0.1950903 -1 0.9807853 0.1950903 1 1 0 -1 1 0 1 0.9807853 -0.1950903 -1 0.9807853 -0.1950903 1 0.9238795 -0.3826835 -1 0.9238795 -0.3826835 1 0.8314696 -0.5555703 -1 0.8314696 -0.5555703 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555703 -0.8314696 -1 0.5555703 -0.8314696 1 0.3826835 -0.9238795 -1 0.3826835 -0.9238795 1 0.1950903 -0.9807853 -1 0.1950903 -0.9807853 1 0 -1 -1 0 -1 1 -0.1950903 -0.9807853 -1 -0.1950903 -0.9807853 1 -0.3826835 -0.9238795 -1 -0.3826835 -0.9238795 1 -0.5555703 -0.8314696 -1 -0.5555703 -0.8314696 1 -0.7071068 -0.7071068 -1 -0.7071068 -0.7071068 1 -0.8314696 -0.5555703 -1 -0.8314696 -0.5555703 1 -0.9238795 -0.3826835 -1 -0.9238795 -0.3826835 1 -0.9807853 -0.1950903 -1 -0.9807853 -0.1950903 1 -1 0 -1 -1 0 1 -0.9807853 0.1950903 -1 -0.9807853 0.1950903 1 -0.9238795 0.3826835 -1 -0.9238795 0.3826835 1 -0.8314696 0.5555703 -1 -0.8314696 0.5555703 1 -0.7071068 0.7071068 -1 -0.7071068 0.7071068 1 -0.5555703 0.8314696 -1 -0.5555703 0.8314696 1 -0.3826835 0.9238795 -1 -0.3826835 0.9238795 1 -0.1950903 0.9807853 -1 -0.1950903 0.9807853 1</float_array>
+          <technique_common>
+            <accessor source="#圓柱體-mesh-positions-array" count="64" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="圓柱體-mesh-normals">
+          <float_array id="圓柱體-mesh-normals-array" count="192">0.09801697 0.9951848 0 0.2902848 0.9569403 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730108 0.634393 0 0.8819212 0.471397 0 0.9569405 0.2902846 0 0.9951848 0.09801727 0 0.9951848 -0.09801667 0 0.9569403 -0.2902851 0 0.8819215 -0.4713965 0 0.7730105 -0.6343934 0 0.6343933 -0.7730104 0 0.4713967 -0.8819213 0 0.2902848 -0.9569404 0 0.09801697 -0.9951848 0 -0.09801697 -0.9951848 0 -0.2902848 -0.9569403 0 -0.4713967 -0.8819213 0 -0.634393 -0.7730107 0 -0.7730108 -0.634393 0 -0.8819212 -0.471397 0 -0.9569405 -0.2902846 0 -0.9951848 -0.09801727 0 -0.9951848 0.09801667 0 -0.9569403 0.2902851 0 -0.8819215 0.4713965 0 -0.7730105 0.6343934 0 -0.6343933 0.7730104 0 -0.4713967 0.8819213 0 0 0 1 -0.2902848 0.9569404 0 -0.09801697 0.9951848 0 0 0 -1 0.2902848 0.9569404 0 0.6343933 0.7730104 0 0.7730105 0.6343934 0 0.8819215 0.4713965 0 0.9569403 0.2902851 0 0.9951848 0.09801667 0 0.9951848 -0.09801727 0 0.9569405 -0.2902846 0 0.8819212 -0.471397 0 0.7730108 -0.634393 0 0.634393 -0.7730107 0 0.2902848 -0.9569403 0 -0.2902848 -0.9569404 0 -0.6343933 -0.7730104 0 -0.7730105 -0.6343934 0 -0.8819215 -0.4713965 0 -0.9569403 -0.2902851 0 -0.9951848 -0.09801667 0 -0.9951848 0.09801727 0 -0.9569405 0.2902846 0 -0.8819212 0.471397 0 -0.7730108 0.634393 0 -0.634393 0.7730107 0 -0.2902848 0.9569403 0 3.97512e-6 0 -1 3.97512e-6 0 -1 -3.97512e-6 0 -1 -3.97512e-6 0 -1 3.88857e-7 0 -1 -3.88857e-7 0 -1</float_array>
+          <technique_common>
+            <accessor source="#圓柱體-mesh-normals-array" count="64" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="圓柱體-mesh-map-0">
+          <float_array id="圓柱體-mesh-map-0-array" count="744">1 1 0.96875 0.5 1 0.5 0.96875 1 0.9375 0.5 0.96875 0.5 0.9375 1 0.90625 0.5 0.9375 0.5 0.90625 1 0.8750001 0.5 0.90625 0.5 0.8750001 1 0.84375 0.5 0.8750001 0.5 0.84375 1 0.8125 0.5 0.84375 0.5 0.8125 1 0.78125 0.5 0.8125 0.5 0.78125 1 0.75 0.5 0.78125 0.5 0.75 1 0.71875 0.5 0.75 0.5 0.71875 1 0.6875 0.5 0.71875 0.5 0.6875 1 0.65625 0.5 0.6875 0.5 0.65625 1 0.625 0.5 0.65625 0.5 0.625 1 0.59375 0.5 0.625 0.5 0.59375 1 0.5625 0.5 0.59375 0.5 0.5625 1 0.53125 0.5 0.5625 0.5 0.53125 1 0.5 0.5 0.53125 0.5 0.5 1 0.46875 0.5 0.5 0.5 0.46875 1 0.4375 0.5 0.46875 0.5 0.4375 1 0.40625 0.5 0.4375 0.5 0.40625 1 0.375 0.5 0.40625 0.5 0.375 1 0.34375 0.5 0.375 0.5 0.34375 1 0.3125 0.5 0.34375 0.5 0.3125 1 0.28125 0.5 0.3125 0.5 0.28125 1 0.25 0.5 0.28125 0.5 0.25 1 0.21875 0.5 0.25 0.5 0.21875 1 0.1875 0.5 0.21875 0.5 0.1875 1 0.15625 0.5 0.1875 0.5 0.15625 1 0.125 0.5 0.15625 0.5 0.125 1 0.09375 0.5 0.125 0.5 0.09375 1 0.0625 0.5 0.09375 0.5 0.1581559 0.02826887 0.4717311 0.1581559 0.341844 0.4717311 0.0625 1 0.03125 0.5 0.0625 0.5 0.03125 1 0 0.5 0.03125 0.5 0.7968217 0.01461154 0.5146116 0.2031783 0.7031784 0.4853885 1 1 0.96875 1 0.96875 0.5 0.96875 1 0.9375 1 0.9375 0.5 0.9375 1 0.90625 1 0.90625 0.5 0.90625 1 0.8750001 1 0.8750001 0.5 0.8750001 1 0.84375 1 0.84375 0.5 0.84375 1 0.8125 1 0.8125 0.5 0.8125 1 0.78125 1 0.78125 0.5 0.78125 1 0.75 1 0.75 0.5 0.75 1 0.71875 1 0.71875 0.5 0.71875 1 0.6875 1 0.6875 0.5 0.6875 1 0.65625 1 0.65625 0.5 0.65625 1 0.625 1 0.625 0.5 0.625 1 0.59375 1 0.59375 0.5 0.59375 1 0.5625 1 0.5625 0.5 0.5625 1 0.53125 1 0.53125 0.5 0.53125 1 0.5 1 0.5 0.5 0.5 1 0.46875 1 0.46875 0.5 0.46875 1 0.4375 1 0.4375 0.5 0.4375 1 0.40625 1 0.40625 0.5 0.40625 1 0.375 1 0.375 0.5 0.375 1 0.34375 1 0.34375 0.5 0.34375 1 0.3125 1 0.3125 0.5 0.3125 1 0.28125 1 0.28125 0.5 0.28125 1 0.25 1 0.25 0.5 0.25 1 0.21875 1 0.21875 0.5 0.21875 1 0.1875 1 0.1875 0.5 0.1875 1 0.15625 1 0.15625 0.5 0.15625 1 0.125 1 0.125 0.5 0.125 1 0.09375 1 0.09375 0.5 0.09375 1 0.0625 1 0.0625 0.5 0.341844 0.4717311 0.2968217 0.4853885 0.25 0.49 0.25 0.49 0.2031783 0.4853885 0.341844 0.4717311 0.2031783 0.4853885 0.1581559 0.4717311 0.341844 0.4717311 0.1581559 0.4717311 0.116663 0.4495527 0.08029437 0.4197056 0.08029437 0.4197056 0.05044728 0.3833369 0.02826887 0.341844 0.02826887 0.341844 0.01461154 0.2968217 0.00999999 0.25 0.00999999 0.25 0.01461154 0.2031783 0.02826887 0.341844 0.01461154 0.2031783 0.02826887 0.1581559 0.02826887 0.341844 0.02826887 0.1581559 0.05044728 0.116663 0.08029437 0.08029437 0.08029437 0.08029437 0.116663 0.05044728 0.1581559 0.02826887 0.1581559 0.02826887 0.2031783 0.01461154 0.341844 0.02826887 0.2031783 0.01461154 0.25 0.00999999 0.341844 0.02826887 0.25 0.00999999 0.2968217 0.01461154 0.341844 0.02826887 0.341844 0.02826887 0.3833369 0.05044728 0.4197056 0.08029437 0.4197056 0.08029437 0.4495527 0.116663 0.4717311 0.1581559 0.4717311 0.1581559 0.4853885 0.2031783 0.4717311 0.341844 0.4853885 0.2031783 0.49 0.25 0.4717311 0.341844 0.49 0.25 0.4853885 0.2968217 0.4717311 0.341844 0.4717311 0.341844 0.4495527 0.3833369 0.4197056 0.4197056 0.4197056 0.4197056 0.3833369 0.4495527 0.341844 0.4717311 0.1581559 0.4717311 0.08029437 0.4197056 0.341844 0.4717311 0.08029437 0.4197056 0.02826887 0.341844 0.341844 0.4717311 0.02826887 0.1581559 0.08029437 0.08029437 0.02826887 0.341844 0.08029437 0.08029437 0.1581559 0.02826887 0.02826887 0.341844 0.341844 0.02826887 0.4197056 0.08029437 0.1581559 0.02826887 0.4197056 0.08029437 0.4717311 0.1581559 0.1581559 0.02826887 0.4717311 0.341844 0.4197056 0.4197056 0.4717311 0.1581559 0.4197056 0.4197056 0.341844 0.4717311 0.4717311 0.1581559 0.341844 0.4717311 0.02826887 0.341844 0.1581559 0.02826887 0.0625 1 0.03125 1 0.03125 0.5 0.03125 1 0 1 0 0.5 0.7031784 0.4853885 0.75 0.49 0.7968217 0.4853885 0.7968217 0.4853885 0.841844 0.4717311 0.8833369 0.4495527 0.8833369 0.4495527 0.9197056 0.4197056 0.9495527 0.3833369 0.9495527 0.3833369 0.9717311 0.341844 0.9853885 0.2968217 0.9853885 0.2968217 0.99 0.25 0.9853885 0.2031783 0.9853885 0.2031783 0.9717311 0.1581559 0.9495527 0.116663 0.9495527 0.116663 0.9197056 0.08029437 0.8833369 0.05044728 0.8833369 0.05044728 0.841844 0.02826887 0.7968217 0.01461154 0.7968217 0.01461154 0.75 0.00999999 0.7031784 0.01461154 0.7031784 0.01461154 0.658156 0.02826887 0.6166632 0.05044728 0.6166632 0.05044728 0.5802944 0.08029437 0.5504473 0.116663 0.5504473 0.116663 0.5282689 0.1581559 0.5146116 0.2031783 0.5146116 0.2031783 0.51 0.25 0.5146116 0.2968217 0.5146116 0.2968217 0.5282689 0.341844 0.5504473 0.3833369 0.5504473 0.3833369 0.5802944 0.4197056 0.6166632 0.4495527 0.6166632 0.4495527 0.658156 0.4717311 0.7031784 0.4853885 0.7031784 0.4853885 0.7968217 0.4853885 0.9853885 0.2968217 0.7968217 0.4853885 0.8833369 0.4495527 0.9853885 0.2968217 0.8833369 0.4495527 0.9495527 0.3833369 0.9853885 0.2968217 0.9853885 0.2968217 0.9853885 0.2031783 0.7968217 0.01461154 0.9853885 0.2031783 0.9495527 0.116663 0.7968217 0.01461154 0.9495527 0.116663 0.8833369 0.05044728 0.7968217 0.01461154 0.7968217 0.01461154 0.7031784 0.01461154 0.5146116 0.2031783 0.7031784 0.01461154 0.6166632 0.05044728 0.5146116 0.2031783 0.6166632 0.05044728 0.5504473 0.116663 0.5146116 0.2031783 0.5146116 0.2031783 0.5146116 0.2968217 0.7031784 0.4853885 0.5146116 0.2968217 0.5504473 0.3833369 0.7031784 0.4853885 0.5504473 0.3833369 0.6166632 0.4495527 0.7031784 0.4853885 0.7031784 0.4853885 0.9853885 0.2968217 0.7968217 0.01461154</float_array>
+          <technique_common>
+            <accessor source="#圓柱體-mesh-map-0-array" count="372" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="圓柱體-mesh-vertices">
+          <input semantic="POSITION" source="#圓柱體-mesh-positions"/>
+        </vertices>
+        <triangles material="材質_001-material" count="124">
+          <input semantic="VERTEX" source="#圓柱體-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#圓柱體-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#圓柱體-mesh-map-0" offset="2" set="2"/>
+          <p>1 0 0 2 0 1 0 0 2 3 1 3 4 1 4 2 1 5 5 2 6 6 2 7 4 2 8 7 3 9 8 3 10 6 3 11 9 4 12 10 4 13 8 4 14 11 5 15 12 5 16 10 5 17 13 6 18 14 6 19 12 6 20 15 7 21 16 7 22 14 7 23 17 8 24 18 8 25 16 8 26 19 9 27 20 9 28 18 9 29 21 10 30 22 10 31 20 10 32 23 11 33 24 11 34 22 11 35 25 12 36 26 12 37 24 12 38 27 13 39 28 13 40 26 13 41 29 14 42 30 14 43 28 14 44 31 15 45 32 15 46 30 15 47 33 16 48 34 16 49 32 16 50 35 17 51 36 17 52 34 17 53 37 18 54 38 18 55 36 18 56 39 19 57 40 19 58 38 19 59 41 20 60 42 20 61 40 20 62 43 21 63 44 21 64 42 21 65 45 22 66 46 22 67 44 22 68 47 23 69 48 23 70 46 23 71 49 24 72 50 24 73 48 24 74 51 25 75 52 25 76 50 25 77 53 26 78 54 26 79 52 26 80 55 27 81 56 27 82 54 27 83 57 28 84 58 28 85 56 28 86 59 29 87 60 29 88 58 29 89 37 30 90 21 30 91 5 30 92 61 31 93 62 31 94 60 31 95 63 32 96 0 32 97 62 32 98 30 33 99 46 33 100 62 33 101 1 0 102 3 0 103 2 0 104 3 34 105 5 34 106 4 34 107 5 2 108 7 2 109 6 2 110 7 35 111 9 35 112 8 35 113 9 36 114 11 36 115 10 36 116 11 37 117 13 37 118 12 37 119 13 38 120 15 38 121 14 38 122 15 39 123 17 39 124 16 39 125 17 40 126 19 40 127 18 40 128 19 41 129 21 41 130 20 41 131 21 42 132 23 42 133 22 42 134 23 43 135 25 43 136 24 43 137 25 44 138 27 44 139 26 44 140 27 13 141 29 13 142 28 13 143 29 45 144 31 45 145 30 45 146 31 15 147 33 15 148 32 15 149 33 16 150 35 16 151 34 16 152 35 46 153 37 46 154 36 46 155 37 18 156 39 18 157 38 18 158 39 47 159 41 47 160 40 47 161 41 48 162 43 48 163 42 48 164 43 49 165 45 49 166 44 49 167 45 50 168 47 50 169 46 50 170 47 51 171 49 51 172 48 51 173 49 52 174 51 52 175 50 52 176 51 53 177 53 53 178 52 53 179 53 54 180 55 54 181 54 54 182 55 55 183 57 55 184 56 55 185 57 56 186 59 56 187 58 56 188 59 29 189 61 29 190 60 29 191 5 30 192 3 30 193 1 30 194 1 30 195 63 30 196 5 30 197 63 30 198 61 30 199 5 30 200 61 30 201 59 30 202 57 30 203 57 30 204 55 30 205 53 30 206 53 30 207 51 30 208 49 30 209 49 30 210 47 30 211 53 30 212 47 30 213 45 30 214 53 30 215 45 30 216 43 30 217 41 30 218 41 30 219 39 30 220 37 30 221 37 30 222 35 30 223 29 30 224 35 30 225 33 30 226 29 30 227 33 30 228 31 30 229 29 30 230 29 30 231 27 30 232 25 30 233 25 30 234 23 30 235 21 30 236 21 30 237 19 30 238 13 30 239 19 30 240 17 30 241 13 30 242 17 30 243 15 30 244 13 30 245 13 30 246 11 30 247 9 30 248 9 30 249 7 30 250 5 30 251 61 30 252 57 30 253 5 30 254 57 30 255 53 30 256 5 30 257 45 30 258 41 30 259 53 30 260 41 30 261 37 30 262 53 30 263 29 30 264 25 30 265 37 30 266 25 30 267 21 30 268 37 30 269 13 30 270 9 30 271 21 30 272 9 30 273 5 30 274 21 30 275 5 30 276 53 30 277 37 30 278 61 57 279 63 57 280 62 57 281 63 32 282 1 32 283 0 32 284 62 33 285 0 33 286 2 33 287 2 33 288 4 33 289 6 33 290 6 33 291 8 33 292 10 33 293 10 58 294 12 58 295 14 58 296 14 33 297 16 33 298 18 33 299 18 59 300 20 59 301 22 59 302 22 33 303 24 33 304 26 33 305 26 33 306 28 33 307 30 33 308 30 33 309 32 33 310 34 33 311 34 33 312 36 33 313 38 33 314 38 33 315 40 33 316 42 33 317 42 60 318 44 60 319 46 60 320 46 33 321 48 33 322 50 33 323 50 61 324 52 61 325 54 61 326 54 33 327 56 33 328 58 33 329 58 33 330 60 33 331 62 33 332 62 33 333 2 33 334 14 33 335 2 33 336 6 33 337 14 33 338 6 33 339 10 33 340 14 33 341 14 62 342 18 62 343 30 62 344 18 33 345 22 33 346 30 33 347 22 33 348 26 33 349 30 33 350 30 33 351 34 33 352 46 33 353 34 33 354 38 33 355 46 33 356 38 33 357 42 33 358 46 33 359 46 63 360 50 63 361 62 63 362 50 33 363 54 33 364 62 33 365 54 33 366 58 33 367 62 33 368 62 33 369 14 33 370 30 33 371</p>
+        </triangles>
+      </mesh>
+    </geometry>
+    <geometry id="圓柱體_001-mesh" name="圓柱體.001">
+      <mesh>
+        <source id="圓柱體_001-mesh-positions">
+          <float_array id="圓柱體_001-mesh-positions-array" count="192">0 1 -1 0 1 1 0.1950903 0.9807853 -1 0.1950903 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314696 0.5555703 -1 0.8314696 0.5555703 1 0.9238795 0.3826835 -1 0.9238795 0.3826835 1 0.9807853 0.1950903 -1 0.9807853 0.1950903 1 1 0 -1 1 0 1 0.9807853 -0.1950903 -1 0.9807853 -0.1950903 1 0.9238795 -0.3826835 -1 0.9238795 -0.3826835 1 0.8314696 -0.5555703 -1 0.8314696 -0.5555703 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555703 -0.8314696 -1 0.5555703 -0.8314696 1 0.3826835 -0.9238795 -1 0.3826835 -0.9238795 1 0.1950903 -0.9807853 -1 0.1950903 -0.9807853 1 0 -1 -1 0 -1 1 -0.1950903 -0.9807853 -1 -0.1950903 -0.9807853 1 -0.3826835 -0.9238795 -1 -0.3826835 -0.9238795 1 -0.5555703 -0.8314696 -1 -0.5555703 -0.8314696 1 -0.7071068 -0.7071068 -1 -0.7071068 -0.7071068 1 -0.8314696 -0.5555703 -1 -0.8314696 -0.5555703 1 -0.9238795 -0.3826835 -1 -0.9238795 -0.3826835 1 -0.9807853 -0.1950903 -1 -0.9807853 -0.1950903 1 -1 0 -1 -1 0 1 -0.9807853 0.1950903 -1 -0.9807853 0.1950903 1 -0.9238795 0.3826835 -1 -0.9238795 0.3826835 1 -0.8314696 0.5555703 -1 -0.8314696 0.5555703 1 -0.7071068 0.7071068 -1 -0.7071068 0.7071068 1 -0.5555703 0.8314696 -1 -0.5555703 0.8314696 1 -0.3826835 0.9238795 -1 -0.3826835 0.9238795 1 -0.1950903 0.9807853 -1 -0.1950903 0.9807853 1</float_array>
+          <technique_common>
+            <accessor source="#圓柱體_001-mesh-positions-array" count="64" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="圓柱體_001-mesh-normals">
+          <float_array id="圓柱體_001-mesh-normals-array" count="192">0.09801697 0.9951848 0 0.2902848 0.9569403 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730108 0.634393 0 0.8819212 0.471397 0 0.9569405 0.2902846 0 0.9951848 0.09801727 0 0.9951848 -0.09801667 0 0.9569403 -0.2902851 0 0.8819215 -0.4713965 0 0.7730105 -0.6343934 0 0.6343933 -0.7730104 0 0.4713967 -0.8819213 0 0.2902848 -0.9569404 0 0.09801697 -0.9951848 0 -0.09801697 -0.9951848 0 -0.2902848 -0.9569403 0 -0.4713967 -0.8819213 0 -0.634393 -0.7730107 0 -0.7730108 -0.634393 0 -0.8819212 -0.471397 0 -0.9569405 -0.2902846 0 -0.9951848 -0.09801727 0 -0.9951848 0.09801667 0 -0.9569403 0.2902851 0 -0.8819215 0.4713965 0 -0.7730105 0.6343934 0 -0.6343933 0.7730104 0 -0.4713967 0.8819213 0 0 0 1 -0.2902848 0.9569404 0 -0.09801697 0.9951848 0 0 0 -1 0.2902848 0.9569404 0 0.6343933 0.7730104 0 0.7730105 0.6343934 0 0.8819215 0.4713965 0 0.9569403 0.2902851 0 0.9951848 0.09801667 0 0.9951848 -0.09801727 0 0.9569405 -0.2902846 0 0.8819212 -0.471397 0 0.7730108 -0.634393 0 0.634393 -0.7730107 0 0.2902848 -0.9569403 0 -0.2902848 -0.9569404 0 -0.6343933 -0.7730104 0 -0.7730105 -0.6343934 0 -0.8819215 -0.4713965 0 -0.9569403 -0.2902851 0 -0.9951848 -0.09801667 0 -0.9951848 0.09801727 0 -0.9569405 0.2902846 0 -0.8819212 0.471397 0 -0.7730108 0.634393 0 -0.634393 0.7730107 0 -0.2902848 0.9569403 0 3.97512e-6 0 -1 3.97512e-6 0 -1 -3.97512e-6 0 -1 -3.97512e-6 0 -1 3.88857e-7 0 -1 -3.88857e-7 0 -1</float_array>
+          <technique_common>
+            <accessor source="#圓柱體_001-mesh-normals-array" count="64" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="圓柱體_001-mesh-map-0">
+          <float_array id="圓柱體_001-mesh-map-0-array" count="744">1 1 0.96875 0.5 1 0.5 0.96875 1 0.9375 0.5 0.96875 0.5 0.9375 1 0.90625 0.5 0.9375 0.5 0.90625 1 0.8750001 0.5 0.90625 0.5 0.8750001 1 0.84375 0.5 0.8750001 0.5 0.84375 1 0.8125 0.5 0.84375 0.5 0.8125 1 0.78125 0.5 0.8125 0.5 0.78125 1 0.75 0.5 0.78125 0.5 0.75 1 0.71875 0.5 0.75 0.5 0.71875 1 0.6875 0.5 0.71875 0.5 0.6875 1 0.65625 0.5 0.6875 0.5 0.65625 1 0.625 0.5 0.65625 0.5 0.625 1 0.59375 0.5 0.625 0.5 0.59375 1 0.5625 0.5 0.59375 0.5 0.5625 1 0.53125 0.5 0.5625 0.5 0.53125 1 0.5 0.5 0.53125 0.5 0.5 1 0.46875 0.5 0.5 0.5 0.46875 1 0.4375 0.5 0.46875 0.5 0.4375 1 0.40625 0.5 0.4375 0.5 0.40625 1 0.375 0.5 0.40625 0.5 0.375 1 0.34375 0.5 0.375 0.5 0.34375 1 0.3125 0.5 0.34375 0.5 0.3125 1 0.28125 0.5 0.3125 0.5 0.28125 1 0.25 0.5 0.28125 0.5 0.25 1 0.21875 0.5 0.25 0.5 0.21875 1 0.1875 0.5 0.21875 0.5 0.1875 1 0.15625 0.5 0.1875 0.5 0.15625 1 0.125 0.5 0.15625 0.5 0.125 1 0.09375 0.5 0.125 0.5 0.09375 1 0.0625 0.5 0.09375 0.5 0.1581559 0.02826887 0.4717311 0.1581559 0.341844 0.4717311 0.0625 1 0.03125 0.5 0.0625 0.5 0.03125 1 0 0.5 0.03125 0.5 0.7968217 0.01461154 0.5146116 0.2031783 0.7031784 0.4853885 1 1 0.96875 1 0.96875 0.5 0.96875 1 0.9375 1 0.9375 0.5 0.9375 1 0.90625 1 0.90625 0.5 0.90625 1 0.8750001 1 0.8750001 0.5 0.8750001 1 0.84375 1 0.84375 0.5 0.84375 1 0.8125 1 0.8125 0.5 0.8125 1 0.78125 1 0.78125 0.5 0.78125 1 0.75 1 0.75 0.5 0.75 1 0.71875 1 0.71875 0.5 0.71875 1 0.6875 1 0.6875 0.5 0.6875 1 0.65625 1 0.65625 0.5 0.65625 1 0.625 1 0.625 0.5 0.625 1 0.59375 1 0.59375 0.5 0.59375 1 0.5625 1 0.5625 0.5 0.5625 1 0.53125 1 0.53125 0.5 0.53125 1 0.5 1 0.5 0.5 0.5 1 0.46875 1 0.46875 0.5 0.46875 1 0.4375 1 0.4375 0.5 0.4375 1 0.40625 1 0.40625 0.5 0.40625 1 0.375 1 0.375 0.5 0.375 1 0.34375 1 0.34375 0.5 0.34375 1 0.3125 1 0.3125 0.5 0.3125 1 0.28125 1 0.28125 0.5 0.28125 1 0.25 1 0.25 0.5 0.25 1 0.21875 1 0.21875 0.5 0.21875 1 0.1875 1 0.1875 0.5 0.1875 1 0.15625 1 0.15625 0.5 0.15625 1 0.125 1 0.125 0.5 0.125 1 0.09375 1 0.09375 0.5 0.09375 1 0.0625 1 0.0625 0.5 0.341844 0.4717311 0.2968217 0.4853885 0.25 0.49 0.25 0.49 0.2031783 0.4853885 0.341844 0.4717311 0.2031783 0.4853885 0.1581559 0.4717311 0.341844 0.4717311 0.1581559 0.4717311 0.116663 0.4495527 0.08029437 0.4197056 0.08029437 0.4197056 0.05044728 0.3833369 0.02826887 0.341844 0.02826887 0.341844 0.01461154 0.2968217 0.00999999 0.25 0.00999999 0.25 0.01461154 0.2031783 0.02826887 0.341844 0.01461154 0.2031783 0.02826887 0.1581559 0.02826887 0.341844 0.02826887 0.1581559 0.05044728 0.116663 0.08029437 0.08029437 0.08029437 0.08029437 0.116663 0.05044728 0.1581559 0.02826887 0.1581559 0.02826887 0.2031783 0.01461154 0.341844 0.02826887 0.2031783 0.01461154 0.25 0.00999999 0.341844 0.02826887 0.25 0.00999999 0.2968217 0.01461154 0.341844 0.02826887 0.341844 0.02826887 0.3833369 0.05044728 0.4197056 0.08029437 0.4197056 0.08029437 0.4495527 0.116663 0.4717311 0.1581559 0.4717311 0.1581559 0.4853885 0.2031783 0.4717311 0.341844 0.4853885 0.2031783 0.49 0.25 0.4717311 0.341844 0.49 0.25 0.4853885 0.2968217 0.4717311 0.341844 0.4717311 0.341844 0.4495527 0.3833369 0.4197056 0.4197056 0.4197056 0.4197056 0.3833369 0.4495527 0.341844 0.4717311 0.1581559 0.4717311 0.08029437 0.4197056 0.341844 0.4717311 0.08029437 0.4197056 0.02826887 0.341844 0.341844 0.4717311 0.02826887 0.1581559 0.08029437 0.08029437 0.02826887 0.341844 0.08029437 0.08029437 0.1581559 0.02826887 0.02826887 0.341844 0.341844 0.02826887 0.4197056 0.08029437 0.1581559 0.02826887 0.4197056 0.08029437 0.4717311 0.1581559 0.1581559 0.02826887 0.4717311 0.341844 0.4197056 0.4197056 0.4717311 0.1581559 0.4197056 0.4197056 0.341844 0.4717311 0.4717311 0.1581559 0.341844 0.4717311 0.02826887 0.341844 0.1581559 0.02826887 0.0625 1 0.03125 1 0.03125 0.5 0.03125 1 0 1 0 0.5 0.7031784 0.4853885 0.75 0.49 0.7968217 0.4853885 0.7968217 0.4853885 0.841844 0.4717311 0.8833369 0.4495527 0.8833369 0.4495527 0.9197056 0.4197056 0.9495527 0.3833369 0.9495527 0.3833369 0.9717311 0.341844 0.9853885 0.2968217 0.9853885 0.2968217 0.99 0.25 0.9853885 0.2031783 0.9853885 0.2031783 0.9717311 0.1581559 0.9495527 0.116663 0.9495527 0.116663 0.9197056 0.08029437 0.8833369 0.05044728 0.8833369 0.05044728 0.841844 0.02826887 0.7968217 0.01461154 0.7968217 0.01461154 0.75 0.00999999 0.7031784 0.01461154 0.7031784 0.01461154 0.658156 0.02826887 0.6166632 0.05044728 0.6166632 0.05044728 0.5802944 0.08029437 0.5504473 0.116663 0.5504473 0.116663 0.5282689 0.1581559 0.5146116 0.2031783 0.5146116 0.2031783 0.51 0.25 0.5146116 0.2968217 0.5146116 0.2968217 0.5282689 0.341844 0.5504473 0.3833369 0.5504473 0.3833369 0.5802944 0.4197056 0.6166632 0.4495527 0.6166632 0.4495527 0.658156 0.4717311 0.7031784 0.4853885 0.7031784 0.4853885 0.7968217 0.4853885 0.9853885 0.2968217 0.7968217 0.4853885 0.8833369 0.4495527 0.9853885 0.2968217 0.8833369 0.4495527 0.9495527 0.3833369 0.9853885 0.2968217 0.9853885 0.2968217 0.9853885 0.2031783 0.7968217 0.01461154 0.9853885 0.2031783 0.9495527 0.116663 0.7968217 0.01461154 0.9495527 0.116663 0.8833369 0.05044728 0.7968217 0.01461154 0.7968217 0.01461154 0.7031784 0.01461154 0.5146116 0.2031783 0.7031784 0.01461154 0.6166632 0.05044728 0.5146116 0.2031783 0.6166632 0.05044728 0.5504473 0.116663 0.5146116 0.2031783 0.5146116 0.2031783 0.5146116 0.2968217 0.7031784 0.4853885 0.5146116 0.2968217 0.5504473 0.3833369 0.7031784 0.4853885 0.5504473 0.3833369 0.6166632 0.4495527 0.7031784 0.4853885 0.7031784 0.4853885 0.9853885 0.2968217 0.7968217 0.01461154</float_array>
+          <technique_common>
+            <accessor source="#圓柱體_001-mesh-map-0-array" count="372" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="圓柱體_001-mesh-vertices">
+          <input semantic="POSITION" source="#圓柱體_001-mesh-positions"/>
+        </vertices>
+        <triangles material="材質-material" count="124">
+          <input semantic="VERTEX" source="#圓柱體_001-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#圓柱體_001-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#圓柱體_001-mesh-map-0" offset="2" set="2"/>
+          <p>1 0 0 2 0 1 0 0 2 3 1 3 4 1 4 2 1 5 5 2 6 6 2 7 4 2 8 7 3 9 8 3 10 6 3 11 9 4 12 10 4 13 8 4 14 11 5 15 12 5 16 10 5 17 13 6 18 14 6 19 12 6 20 15 7 21 16 7 22 14 7 23 17 8 24 18 8 25 16 8 26 19 9 27 20 9 28 18 9 29 21 10 30 22 10 31 20 10 32 23 11 33 24 11 34 22 11 35 25 12 36 26 12 37 24 12 38 27 13 39 28 13 40 26 13 41 29 14 42 30 14 43 28 14 44 31 15 45 32 15 46 30 15 47 33 16 48 34 16 49 32 16 50 35 17 51 36 17 52 34 17 53 37 18 54 38 18 55 36 18 56 39 19 57 40 19 58 38 19 59 41 20 60 42 20 61 40 20 62 43 21 63 44 21 64 42 21 65 45 22 66 46 22 67 44 22 68 47 23 69 48 23 70 46 23 71 49 24 72 50 24 73 48 24 74 51 25 75 52 25 76 50 25 77 53 26 78 54 26 79 52 26 80 55 27 81 56 27 82 54 27 83 57 28 84 58 28 85 56 28 86 59 29 87 60 29 88 58 29 89 37 30 90 21 30 91 5 30 92 61 31 93 62 31 94 60 31 95 63 32 96 0 32 97 62 32 98 30 33 99 46 33 100 62 33 101 1 0 102 3 0 103 2 0 104 3 34 105 5 34 106 4 34 107 5 2 108 7 2 109 6 2 110 7 35 111 9 35 112 8 35 113 9 36 114 11 36 115 10 36 116 11 37 117 13 37 118 12 37 119 13 38 120 15 38 121 14 38 122 15 39 123 17 39 124 16 39 125 17 40 126 19 40 127 18 40 128 19 41 129 21 41 130 20 41 131 21 42 132 23 42 133 22 42 134 23 43 135 25 43 136 24 43 137 25 44 138 27 44 139 26 44 140 27 13 141 29 13 142 28 13 143 29 45 144 31 45 145 30 45 146 31 15 147 33 15 148 32 15 149 33 16 150 35 16 151 34 16 152 35 46 153 37 46 154 36 46 155 37 18 156 39 18 157 38 18 158 39 47 159 41 47 160 40 47 161 41 48 162 43 48 163 42 48 164 43 49 165 45 49 166 44 49 167 45 50 168 47 50 169 46 50 170 47 51 171 49 51 172 48 51 173 49 52 174 51 52 175 50 52 176 51 53 177 53 53 178 52 53 179 53 54 180 55 54 181 54 54 182 55 55 183 57 55 184 56 55 185 57 56 186 59 56 187 58 56 188 59 29 189 61 29 190 60 29 191 5 30 192 3 30 193 1 30 194 1 30 195 63 30 196 5 30 197 63 30 198 61 30 199 5 30 200 61 30 201 59 30 202 57 30 203 57 30 204 55 30 205 53 30 206 53 30 207 51 30 208 49 30 209 49 30 210 47 30 211 53 30 212 47 30 213 45 30 214 53 30 215 45 30 216 43 30 217 41 30 218 41 30 219 39 30 220 37 30 221 37 30 222 35 30 223 29 30 224 35 30 225 33 30 226 29 30 227 33 30 228 31 30 229 29 30 230 29 30 231 27 30 232 25 30 233 25 30 234 23 30 235 21 30 236 21 30 237 19 30 238 13 30 239 19 30 240 17 30 241 13 30 242 17 30 243 15 30 244 13 30 245 13 30 246 11 30 247 9 30 248 9 30 249 7 30 250 5 30 251 61 30 252 57 30 253 5 30 254 57 30 255 53 30 256 5 30 257 45 30 258 41 30 259 53 30 260 41 30 261 37 30 262 53 30 263 29 30 264 25 30 265 37 30 266 25 30 267 21 30 268 37 30 269 13 30 270 9 30 271 21 30 272 9 30 273 5 30 274 21 30 275 5 30 276 53 30 277 37 30 278 61 57 279 63 57 280 62 57 281 63 32 282 1 32 283 0 32 284 62 33 285 0 33 286 2 33 287 2 33 288 4 33 289 6 33 290 6 33 291 8 33 292 10 33 293 10 58 294 12 58 295 14 58 296 14 33 297 16 33 298 18 33 299 18 59 300 20 59 301 22 59 302 22 33 303 24 33 304 26 33 305 26 33 306 28 33 307 30 33 308 30 33 309 32 33 310 34 33 311 34 33 312 36 33 313 38 33 314 38 33 315 40 33 316 42 33 317 42 60 318 44 60 319 46 60 320 46 33 321 48 33 322 50 33 323 50 61 324 52 61 325 54 61 326 54 33 327 56 33 328 58 33 329 58 33 330 60 33 331 62 33 332 62 33 333 2 33 334 14 33 335 2 33 336 6 33 337 14 33 338 6 33 339 10 33 340 14 33 341 14 62 342 18 62 343 30 62 344 18 33 345 22 33 346 30 33 347 22 33 348 26 33 349 30 33 350 30 33 351 34 33 352 46 33 353 34 33 354 38 33 355 46 33 356 38 33 357 42 33 358 46 33 359 46 63 360 50 63 361 62 63 362 50 33 363 54 33 364 62 33 365 54 33 366 58 33 367 62 33 368 62 33 369 14 33 370 30 33 371</p>
+        </triangles>
+      </mesh>
+    </geometry>
+    <geometry id="圓柱體_002-mesh" name="圓柱體.002">
+      <mesh>
+        <source id="圓柱體_002-mesh-positions">
+          <float_array id="圓柱體_002-mesh-positions-array" count="192">0 1 -1 0 1 1 0.1950903 0.9807853 -1 0.1950903 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314696 0.5555703 -1 0.8314696 0.5555703 1 0.9238795 0.3826835 -1 0.9238795 0.3826835 1 0.9807853 0.1950903 -1 0.9807853 0.1950903 1 1 0 -1 1 0 1 0.9807853 -0.1950903 -1 0.9807853 -0.1950903 1 0.9238795 -0.3826835 -1 0.9238795 -0.3826835 1 0.8314696 -0.5555703 -1 0.8314696 -0.5555703 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555703 -0.8314696 -1 0.5555703 -0.8314696 1 0.3826835 -0.9238795 -1 0.3826835 -0.9238795 1 0.1950903 -0.9807853 -1 0.1950903 -0.9807853 1 0 -1 -1 0 -1 1 -0.1950903 -0.9807853 -1 -0.1950903 -0.9807853 1 -0.3826835 -0.9238795 -1 -0.3826835 -0.9238795 1 -0.5555703 -0.8314696 -1 -0.5555703 -0.8314696 1 -0.7071068 -0.7071068 -1 -0.7071068 -0.7071068 1 -0.8314696 -0.5555703 -1 -0.8314696 -0.5555703 1 -0.9238795 -0.3826835 -1 -0.9238795 -0.3826835 1 -0.9807853 -0.1950903 -1 -0.9807853 -0.1950903 1 -1 0 -1 -1 0 1 -0.9807853 0.1950903 -1 -0.9807853 0.1950903 1 -0.9238795 0.3826835 -1 -0.9238795 0.3826835 1 -0.8314696 0.5555703 -1 -0.8314696 0.5555703 1 -0.7071068 0.7071068 -1 -0.7071068 0.7071068 1 -0.5555703 0.8314696 -1 -0.5555703 0.8314696 1 -0.3826835 0.9238795 -1 -0.3826835 0.9238795 1 -0.1950903 0.9807853 -1 -0.1950903 0.9807853 1</float_array>
+          <technique_common>
+            <accessor source="#圓柱體_002-mesh-positions-array" count="64" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="圓柱體_002-mesh-normals">
+          <float_array id="圓柱體_002-mesh-normals-array" count="192">0.09801697 0.9951848 0 0.2902848 0.9569403 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730108 0.634393 0 0.8819212 0.471397 0 0.9569405 0.2902846 0 0.9951848 0.09801727 0 0.9951848 -0.09801667 0 0.9569403 -0.2902851 0 0.8819215 -0.4713965 0 0.7730105 -0.6343934 0 0.6343933 -0.7730104 0 0.4713967 -0.8819213 0 0.2902848 -0.9569404 0 0.09801697 -0.9951848 0 -0.09801697 -0.9951848 0 -0.2902848 -0.9569403 0 -0.4713967 -0.8819213 0 -0.634393 -0.7730107 0 -0.7730108 -0.634393 0 -0.8819212 -0.471397 0 -0.9569405 -0.2902846 0 -0.9951848 -0.09801727 0 -0.9951848 0.09801667 0 -0.9569403 0.2902851 0 -0.8819215 0.4713965 0 -0.7730105 0.6343934 0 -0.6343933 0.7730104 0 -0.4713967 0.8819213 0 0 0 1 -0.2902848 0.9569404 0 -0.09801697 0.9951848 0 0 0 -1 0.2902848 0.9569404 0 0.6343933 0.7730104 0 0.7730105 0.6343934 0 0.8819215 0.4713965 0 0.9569403 0.2902851 0 0.9951848 0.09801667 0 0.9951848 -0.09801727 0 0.9569405 -0.2902846 0 0.8819212 -0.471397 0 0.7730108 -0.634393 0 0.634393 -0.7730107 0 0.2902848 -0.9569403 0 -0.2902848 -0.9569404 0 -0.6343933 -0.7730104 0 -0.7730105 -0.6343934 0 -0.8819215 -0.4713965 0 -0.9569403 -0.2902851 0 -0.9951848 -0.09801667 0 -0.9951848 0.09801727 0 -0.9569405 0.2902846 0 -0.8819212 0.471397 0 -0.7730108 0.634393 0 -0.634393 0.7730107 0 -0.2902848 0.9569403 0 3.97512e-6 0 -1 3.97512e-6 0 -1 -3.97512e-6 0 -1 -3.97512e-6 0 -1 3.88857e-7 0 -1 -3.88857e-7 0 -1</float_array>
+          <technique_common>
+            <accessor source="#圓柱體_002-mesh-normals-array" count="64" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="圓柱體_002-mesh-map-0">
+          <float_array id="圓柱體_002-mesh-map-0-array" count="744">1 1 0.96875 0.5 1 0.5 0.96875 1 0.9375 0.5 0.96875 0.5 0.9375 1 0.90625 0.5 0.9375 0.5 0.90625 1 0.8750001 0.5 0.90625 0.5 0.8750001 1 0.84375 0.5 0.8750001 0.5 0.84375 1 0.8125 0.5 0.84375 0.5 0.8125 1 0.78125 0.5 0.8125 0.5 0.78125 1 0.75 0.5 0.78125 0.5 0.75 1 0.71875 0.5 0.75 0.5 0.71875 1 0.6875 0.5 0.71875 0.5 0.6875 1 0.65625 0.5 0.6875 0.5 0.65625 1 0.625 0.5 0.65625 0.5 0.625 1 0.59375 0.5 0.625 0.5 0.59375 1 0.5625 0.5 0.59375 0.5 0.5625 1 0.53125 0.5 0.5625 0.5 0.53125 1 0.5 0.5 0.53125 0.5 0.5 1 0.46875 0.5 0.5 0.5 0.46875 1 0.4375 0.5 0.46875 0.5 0.4375 1 0.40625 0.5 0.4375 0.5 0.40625 1 0.375 0.5 0.40625 0.5 0.375 1 0.34375 0.5 0.375 0.5 0.34375 1 0.3125 0.5 0.34375 0.5 0.3125 1 0.28125 0.5 0.3125 0.5 0.28125 1 0.25 0.5 0.28125 0.5 0.25 1 0.21875 0.5 0.25 0.5 0.21875 1 0.1875 0.5 0.21875 0.5 0.1875 1 0.15625 0.5 0.1875 0.5 0.15625 1 0.125 0.5 0.15625 0.5 0.125 1 0.09375 0.5 0.125 0.5 0.09375 1 0.0625 0.5 0.09375 0.5 0.1581559 0.02826887 0.4717311 0.1581559 0.341844 0.4717311 0.0625 1 0.03125 0.5 0.0625 0.5 0.03125 1 0 0.5 0.03125 0.5 0.7968217 0.01461154 0.5146116 0.2031783 0.7031784 0.4853885 1 1 0.96875 1 0.96875 0.5 0.96875 1 0.9375 1 0.9375 0.5 0.9375 1 0.90625 1 0.90625 0.5 0.90625 1 0.8750001 1 0.8750001 0.5 0.8750001 1 0.84375 1 0.84375 0.5 0.84375 1 0.8125 1 0.8125 0.5 0.8125 1 0.78125 1 0.78125 0.5 0.78125 1 0.75 1 0.75 0.5 0.75 1 0.71875 1 0.71875 0.5 0.71875 1 0.6875 1 0.6875 0.5 0.6875 1 0.65625 1 0.65625 0.5 0.65625 1 0.625 1 0.625 0.5 0.625 1 0.59375 1 0.59375 0.5 0.59375 1 0.5625 1 0.5625 0.5 0.5625 1 0.53125 1 0.53125 0.5 0.53125 1 0.5 1 0.5 0.5 0.5 1 0.46875 1 0.46875 0.5 0.46875 1 0.4375 1 0.4375 0.5 0.4375 1 0.40625 1 0.40625 0.5 0.40625 1 0.375 1 0.375 0.5 0.375 1 0.34375 1 0.34375 0.5 0.34375 1 0.3125 1 0.3125 0.5 0.3125 1 0.28125 1 0.28125 0.5 0.28125 1 0.25 1 0.25 0.5 0.25 1 0.21875 1 0.21875 0.5 0.21875 1 0.1875 1 0.1875 0.5 0.1875 1 0.15625 1 0.15625 0.5 0.15625 1 0.125 1 0.125 0.5 0.125 1 0.09375 1 0.09375 0.5 0.09375 1 0.0625 1 0.0625 0.5 0.341844 0.4717311 0.2968217 0.4853885 0.25 0.49 0.25 0.49 0.2031783 0.4853885 0.341844 0.4717311 0.2031783 0.4853885 0.1581559 0.4717311 0.341844 0.4717311 0.1581559 0.4717311 0.116663 0.4495527 0.08029437 0.4197056 0.08029437 0.4197056 0.05044728 0.3833369 0.02826887 0.341844 0.02826887 0.341844 0.01461154 0.2968217 0.00999999 0.25 0.00999999 0.25 0.01461154 0.2031783 0.02826887 0.341844 0.01461154 0.2031783 0.02826887 0.1581559 0.02826887 0.341844 0.02826887 0.1581559 0.05044728 0.116663 0.08029437 0.08029437 0.08029437 0.08029437 0.116663 0.05044728 0.1581559 0.02826887 0.1581559 0.02826887 0.2031783 0.01461154 0.341844 0.02826887 0.2031783 0.01461154 0.25 0.00999999 0.341844 0.02826887 0.25 0.00999999 0.2968217 0.01461154 0.341844 0.02826887 0.341844 0.02826887 0.3833369 0.05044728 0.4197056 0.08029437 0.4197056 0.08029437 0.4495527 0.116663 0.4717311 0.1581559 0.4717311 0.1581559 0.4853885 0.2031783 0.4717311 0.341844 0.4853885 0.2031783 0.49 0.25 0.4717311 0.341844 0.49 0.25 0.4853885 0.2968217 0.4717311 0.341844 0.4717311 0.341844 0.4495527 0.3833369 0.4197056 0.4197056 0.4197056 0.4197056 0.3833369 0.4495527 0.341844 0.4717311 0.1581559 0.4717311 0.08029437 0.4197056 0.341844 0.4717311 0.08029437 0.4197056 0.02826887 0.341844 0.341844 0.4717311 0.02826887 0.1581559 0.08029437 0.08029437 0.02826887 0.341844 0.08029437 0.08029437 0.1581559 0.02826887 0.02826887 0.341844 0.341844 0.02826887 0.4197056 0.08029437 0.1581559 0.02826887 0.4197056 0.08029437 0.4717311 0.1581559 0.1581559 0.02826887 0.4717311 0.341844 0.4197056 0.4197056 0.4717311 0.1581559 0.4197056 0.4197056 0.341844 0.4717311 0.4717311 0.1581559 0.341844 0.4717311 0.02826887 0.341844 0.1581559 0.02826887 0.0625 1 0.03125 1 0.03125 0.5 0.03125 1 0 1 0 0.5 0.7031784 0.4853885 0.75 0.49 0.7968217 0.4853885 0.7968217 0.4853885 0.841844 0.4717311 0.8833369 0.4495527 0.8833369 0.4495527 0.9197056 0.4197056 0.9495527 0.3833369 0.9495527 0.3833369 0.9717311 0.341844 0.9853885 0.2968217 0.9853885 0.2968217 0.99 0.25 0.9853885 0.2031783 0.9853885 0.2031783 0.9717311 0.1581559 0.9495527 0.116663 0.9495527 0.116663 0.9197056 0.08029437 0.8833369 0.05044728 0.8833369 0.05044728 0.841844 0.02826887 0.7968217 0.01461154 0.7968217 0.01461154 0.75 0.00999999 0.7031784 0.01461154 0.7031784 0.01461154 0.658156 0.02826887 0.6166632 0.05044728 0.6166632 0.05044728 0.5802944 0.08029437 0.5504473 0.116663 0.5504473 0.116663 0.5282689 0.1581559 0.5146116 0.2031783 0.5146116 0.2031783 0.51 0.25 0.5146116 0.2968217 0.5146116 0.2968217 0.5282689 0.341844 0.5504473 0.3833369 0.5504473 0.3833369 0.5802944 0.4197056 0.6166632 0.4495527 0.6166632 0.4495527 0.658156 0.4717311 0.7031784 0.4853885 0.7031784 0.4853885 0.7968217 0.4853885 0.9853885 0.2968217 0.7968217 0.4853885 0.8833369 0.4495527 0.9853885 0.2968217 0.8833369 0.4495527 0.9495527 0.3833369 0.9853885 0.2968217 0.9853885 0.2968217 0.9853885 0.2031783 0.7968217 0.01461154 0.9853885 0.2031783 0.9495527 0.116663 0.7968217 0.01461154 0.9495527 0.116663 0.8833369 0.05044728 0.7968217 0.01461154 0.7968217 0.01461154 0.7031784 0.01461154 0.5146116 0.2031783 0.7031784 0.01461154 0.6166632 0.05044728 0.5146116 0.2031783 0.6166632 0.05044728 0.5504473 0.116663 0.5146116 0.2031783 0.5146116 0.2031783 0.5146116 0.2968217 0.7031784 0.4853885 0.5146116 0.2968217 0.5504473 0.3833369 0.7031784 0.4853885 0.5504473 0.3833369 0.6166632 0.4495527 0.7031784 0.4853885 0.7031784 0.4853885 0.9853885 0.2968217 0.7968217 0.01461154</float_array>
+          <technique_common>
+            <accessor source="#圓柱體_002-mesh-map-0-array" count="372" stride="2">
+              <param name="S" type="float"/>
+              <param name="T" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="圓柱體_002-mesh-vertices">
+          <input semantic="POSITION" source="#圓柱體_002-mesh-positions"/>
+        </vertices>
+        <triangles material="材質_002-material" count="124">
+          <input semantic="VERTEX" source="#圓柱體_002-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#圓柱體_002-mesh-normals" offset="1"/>
+          <input semantic="TEXCOORD" source="#圓柱體_002-mesh-map-0" offset="2" set="2"/>
+          <p>1 0 0 2 0 1 0 0 2 3 1 3 4 1 4 2 1 5 5 2 6 6 2 7 4 2 8 7 3 9 8 3 10 6 3 11 9 4 12 10 4 13 8 4 14 11 5 15 12 5 16 10 5 17 13 6 18 14 6 19 12 6 20 15 7 21 16 7 22 14 7 23 17 8 24 18 8 25 16 8 26 19 9 27 20 9 28 18 9 29 21 10 30 22 10 31 20 10 32 23 11 33 24 11 34 22 11 35 25 12 36 26 12 37 24 12 38 27 13 39 28 13 40 26 13 41 29 14 42 30 14 43 28 14 44 31 15 45 32 15 46 30 15 47 33 16 48 34 16 49 32 16 50 35 17 51 36 17 52 34 17 53 37 18 54 38 18 55 36 18 56 39 19 57 40 19 58 38 19 59 41 20 60 42 20 61 40 20 62 43 21 63 44 21 64 42 21 65 45 22 66 46 22 67 44 22 68 47 23 69 48 23 70 46 23 71 49 24 72 50 24 73 48 24 74 51 25 75 52 25 76 50 25 77 53 26 78 54 26 79 52 26 80 55 27 81 56 27 82 54 27 83 57 28 84 58 28 85 56 28 86 59 29 87 60 29 88 58 29 89 37 30 90 21 30 91 5 30 92 61 31 93 62 31 94 60 31 95 63 32 96 0 32 97 62 32 98 30 33 99 46 33 100 62 33 101 1 0 102 3 0 103 2 0 104 3 34 105 5 34 106 4 34 107 5 2 108 7 2 109 6 2 110 7 35 111 9 35 112 8 35 113 9 36 114 11 36 115 10 36 116 11 37 117 13 37 118 12 37 119 13 38 120 15 38 121 14 38 122 15 39 123 17 39 124 16 39 125 17 40 126 19 40 127 18 40 128 19 41 129 21 41 130 20 41 131 21 42 132 23 42 133 22 42 134 23 43 135 25 43 136 24 43 137 25 44 138 27 44 139 26 44 140 27 13 141 29 13 142 28 13 143 29 45 144 31 45 145 30 45 146 31 15 147 33 15 148 32 15 149 33 16 150 35 16 151 34 16 152 35 46 153 37 46 154 36 46 155 37 18 156 39 18 157 38 18 158 39 47 159 41 47 160 40 47 161 41 48 162 43 48 163 42 48 164 43 49 165 45 49 166 44 49 167 45 50 168 47 50 169 46 50 170 47 51 171 49 51 172 48 51 173 49 52 174 51 52 175 50 52 176 51 53 177 53 53 178 52 53 179 53 54 180 55 54 181 54 54 182 55 55 183 57 55 184 56 55 185 57 56 186 59 56 187 58 56 188 59 29 189 61 29 190 60 29 191 5 30 192 3 30 193 1 30 194 1 30 195 63 30 196 5 30 197 63 30 198 61 30 199 5 30 200 61 30 201 59 30 202 57 30 203 57 30 204 55 30 205 53 30 206 53 30 207 51 30 208 49 30 209 49 30 210 47 30 211 53 30 212 47 30 213 45 30 214 53 30 215 45 30 216 43 30 217 41 30 218 41 30 219 39 30 220 37 30 221 37 30 222 35 30 223 29 30 224 35 30 225 33 30 226 29 30 227 33 30 228 31 30 229 29 30 230 29 30 231 27 30 232 25 30 233 25 30 234 23 30 235 21 30 236 21 30 237 19 30 238 13 30 239 19 30 240 17 30 241 13 30 242 17 30 243 15 30 244 13 30 245 13 30 246 11 30 247 9 30 248 9 30 249 7 30 250 5 30 251 61 30 252 57 30 253 5 30 254 57 30 255 53 30 256 5 30 257 45 30 258 41 30 259 53 30 260 41 30 261 37 30 262 53 30 263 29 30 264 25 30 265 37 30 266 25 30 267 21 30 268 37 30 269 13 30 270 9 30 271 21 30 272 9 30 273 5 30 274 21 30 275 5 30 276 53 30 277 37 30 278 61 57 279 63 57 280 62 57 281 63 32 282 1 32 283 0 32 284 62 33 285 0 33 286 2 33 287 2 33 288 4 33 289 6 33 290 6 33 291 8 33 292 10 33 293 10 58 294 12 58 295 14 58 296 14 33 297 16 33 298 18 33 299 18 59 300 20 59 301 22 59 302 22 33 303 24 33 304 26 33 305 26 33 306 28 33 307 30 33 308 30 33 309 32 33 310 34 33 311 34 33 312 36 33 313 38 33 314 38 33 315 40 33 316 42 33 317 42 60 318 44 60 319 46 60 320 46 33 321 48 33 322 50 33 323 50 61 324 52 61 325 54 61 326 54 33 327 56 33 328 58 33 329 58 33 330 60 33 331 62 33 332 62 33 333 2 33 334 14 33 335 2 33 336 6 33 337 14 33 338 6 33 339 10 33 340 14 33 341 14 62 342 18 62 343 30 62 344 18 33 345 22 33 346 30 33 347 22 33 348 26 33 349 30 33 350 30 33 351 34 33 352 46 33 353 34 33 354 38 33 355 46 33 356 38 33 357 42 33 358 46 33 359 46 63 360 50 63 361 62 63 362 50 33 363 54 33 364 62 33 365 54 33 366 58 33 367 62 33 368 62 33 369 14 33 370 30 33 371</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Light_001" name="Light.001" type="NODE">
+        <matrix sid="transform">-0.2908648 -0.7711008 0.5663932 4.076245 0.9551711 -0.1998834 0.2183913 1.005454 -0.05518911 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light_001-light"/>
+      </node>
+      <node id="Camera_001" name="Camera.001" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054209 -0.6141704 -6.925791 -4.01133e-9 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera_001-camera"/>
+      </node>
+      <node id="圓柱體" name="圓柱體" type="NODE">
+        <matrix sid="transform">0.025 0 0 0 0 0.025 0 0 0 0 0.25 0.255 0 0 0 1</matrix>
+        <instance_geometry url="#圓柱體-mesh" name="圓柱體">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="材質_001-material" target="#材質_001-material">
+                <bind_vertex_input semantic="圓柱體-mesh-map-0" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+      <node id="圓柱體_001" name="圓柱體.001" type="NODE">
+        <matrix sid="transform">1.88745e-9 0 0.25 0.25 0 0.025 0 0 -0.025 0 1.88745e-8 0 0 0 0 1</matrix>
+        <instance_geometry url="#圓柱體_001-mesh" name="圓柱體.001">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="材質-material" target="#材質-material">
+                <bind_vertex_input semantic="圓柱體_001-mesh-map-0" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+      <node id="圓柱體_002" name="圓柱體.002" type="NODE">
+        <matrix sid="transform">0.025 0 0 0 0 1.88745e-9 -0.25 0.25 0 0.025 1.88745e-8 0 0 0 0 1</matrix>
+        <instance_geometry url="#圓柱體_002-mesh" name="圓柱體.002">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="材質_002-material" target="#材質_002-material">
+                <bind_vertex_input semantic="圓柱體_002-mesh-map-0" input_semantic="TEXCOORD" input_set="0"/>
+              </instance_material>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+      <node id="Camera" name="Camera" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera-camera"/>
+      </node>
+      <node id="Light" name="Light" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light-light"/>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>
\ No newline at end of file
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/package.xml b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/package.xml
new file mode 100644
index 000000000..f3e2934ba
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/package.xml
@@ -0,0 +1,37 @@
+<?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>waypoint_rviz2_plugin</name>
+  <version>1.0.0</version>
+  <description>RViz2 plugin for creating and manipulating interactive 3D waypoints</description>
+  
+  <maintainer email="the21515@gmail.com">koko</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>rviz_rendering</depend>
+  <depend>rviz_ogre_vendor</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>interactive_markers</depend>
+  <depend>nav_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>rosbag2_cpp</depend>
+  <depend>pluginlib</depend>
+  <depend>qtbase5-dev</depend>
+
+  <build_depend>qt5-qmake</build_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <rviz plugin="${prefix}/plugin_description.xml"/>
+  </export>
+</package>
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/plugin_description.xml b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/plugin_description.xml
new file mode 100644
index 000000000..3aa60feef
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/plugin_description.xml
@@ -0,0 +1,10 @@
+<library path="waypoint_rviz2_plugin">
+  <class name="waypoint_rviz2_plugin/WaypointTool"
+         type="waypoint_rviz2_plugin::WaypointTool"
+         base_class_type="rviz_common::Tool">
+    <description>
+      Interactive waypoint creation and manipulation tool for RViz2.
+      Click to add waypoints, drag to move them, right-click to delete.
+    </description>
+  </class>
+</library>
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/rviz_task_panel_waypoints.png b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/rviz_task_panel_waypoints.png
new file mode 100644
index 000000000..f58a48502
Binary files /dev/null and b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/rviz_task_panel_waypoints.png differ
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_manager.cpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_manager.cpp
new file mode 100644
index 000000000..4f9374e83
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_manager.cpp
@@ -0,0 +1,664 @@
+/**
+ * @file waypoint_manager.cpp
+ *
+ * Implementation of the shared WaypointManager singleton.
+ */
+
+#include "waypoint_rviz2_plugin/waypoint_manager.hpp"
+
+#include <sstream>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/serialization.hpp>
+#include <rclcpp/serialized_message.hpp>
+#include <rosbag2_cpp/reader.hpp>
+#include <rosbag2_cpp/writer.hpp>
+#include <rosbag2_storage/storage_options.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_rendering/mesh_loader.hpp>
+
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <OgreEntity.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+
+namespace waypoint_rviz2_plugin {
+
+// ─────────────────────────── singleton ─────────────────────────────────────────
+
+std::shared_ptr<WaypointManager> WaypointManager::instance()
+{
+  static std::weak_ptr<WaypointManager> weak_instance;
+  static std::mutex singleton_mtx;
+  std::lock_guard<std::mutex> lock(singleton_mtx);
+
+  auto sp = weak_instance.lock();
+  if (!sp) {
+    sp = std::shared_ptr<WaypointManager>(new WaypointManager());
+    weak_instance = sp;
+  }
+  return sp;
+}
+
+WaypointManager::WaypointManager()
+{
+  selected_marker_name_ = std::string(kWaypointPrefix) + "1";
+}
+
+WaypointManager::~WaypointManager()
+{
+  if (scene_manager_) {
+    for (auto & [idx, node_ptr] : node_map_) {
+      scene_manager_->destroySceneNode(node_ptr);
+    }
+  }
+}
+
+// ─────────────────────────── initialization ───────────────────────────────────
+
+void WaypointManager::initialize(rviz_common::DisplayContext * context,
+                                 rclcpp::Node::SharedPtr node)
+{
+  if (initialized_) { return; }
+
+  context_ = context;
+  node_ = node;
+  scene_manager_ = context_->getSceneManager();
+
+  axis_resource_ = "package://waypoint_rviz2_plugin/media/axis.dae";
+  if (rviz_rendering::loadMeshFromResource(axis_resource_).isNull()) {
+    RCLCPP_ERROR(node_->get_logger(),
+                 "WaypointManager: failed to load model resource '%s'.",
+                 axis_resource_.c_str());
+    return;
+  }
+
+  server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(
+    "waypoint_plugin", node_);
+
+  path_publisher_ =
+    node_->create_publisher<nav_msgs::msg::Path>(output_topic_, 1);
+
+  menu_handler_.insert(
+    "delete",
+    std::bind(&WaypointManager::processFeedback, this, std::placeholders::_1));
+  menu_handler_.insert(
+    "set manual",
+    std::bind(&WaypointManager::processFeedback, this, std::placeholders::_1));
+
+  initialized_ = true;
+}
+
+// ─────────────────────────── waypoint operations ──────────────────────────────
+
+void WaypointManager::addWaypoint(const Ogre::Vector3 & position,
+                                  const Ogre::Quaternion & quaternion)
+{
+  if (!initialized_) { return; }
+
+  unique_idx_++;
+  std::stringstream wp_name;
+  wp_name << kWaypointPrefix << unique_idx_;
+  std::string str_name(wp_name.str());
+
+  if (rviz_rendering::loadMeshFromResource(axis_resource_).isNull()) {
+    RCLCPP_ERROR(node_->get_logger(),
+                 "WaypointManager: failed to load model resource '%s'.",
+                 axis_resource_.c_str());
+    return;
+  }
+
+  Ogre::SceneNode * node_ptr =
+    scene_manager_->getRootSceneNode()->createChildSceneNode();
+  Ogre::Entity * entity = scene_manager_->createEntity(axis_resource_);
+  node_ptr->attachObject(entity);
+  node_ptr->setVisible(true);
+  node_ptr->setPosition(position);
+  node_ptr->setOrientation(quaternion);
+
+  auto node_entry = node_map_.find(unique_idx_);
+  if (node_entry == node_map_.end()) {
+    node_map_.insert(std::make_pair(unique_idx_, node_ptr));
+  } else {
+    RCLCPP_WARN(node_->get_logger(), "%s already in map", str_name.c_str());
+    return;
+  }
+
+  // Build interactive marker
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = position.x;
+  pose.pose.position.y = position.y;
+  pose.pose.position.z = position.z;
+  pose.pose.orientation.x = quaternion.x;
+  pose.pose.orientation.y = quaternion.y;
+  pose.pose.orientation.z = quaternion.z;
+  pose.pose.orientation.w = quaternion.w;
+
+  visualization_msgs::msg::InteractiveMarker int_marker;
+  int_marker.header.stamp = node_->now();
+  {
+    std::lock_guard<std::mutex> lock(mtx_);
+    int_marker.header.frame_id = frame_id_;
+  }
+  int_marker.pose = pose.pose;
+  int_marker.scale = 1.0;
+  int_marker.name = str_name;
+
+  // Invisible cylinder for picking
+  visualization_msgs::msg::Marker marker;
+  marker.type = visualization_msgs::msg::Marker::CYLINDER;
+  marker.scale.x = 0.5;
+  marker.scale.y = 0.5;
+  marker.scale.z = 0.1;
+  marker.color.r = 0.0;
+  marker.color.g = 0.0;
+  marker.color.b = 0.0;
+  marker.color.a = 0.0;
+
+  visualization_msgs::msg::InteractiveMarkerControl c_control;
+  c_control.always_visible = true;
+  c_control.markers.push_back(marker);
+  int_marker.controls.push_back(c_control);
+
+  visualization_msgs::msg::InteractiveMarkerControl control;
+  control.orientation.w = 0.707106781;
+  control.orientation.x = 0;
+  control.orientation.y = 0.707106781;
+  control.orientation.z = 0;
+  control.interaction_mode =
+    visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE;
+  int_marker.controls.push_back(control);
+
+  control.interaction_mode =
+    visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
+  int_marker.controls.push_back(control);
+
+  control.interaction_mode =
+    visualization_msgs::msg::InteractiveMarkerControl::MENU;
+  control.name = "menu_delete";
+  control.description = str_name;
+  int_marker.controls.push_back(control);
+
+  server_->insert(int_marker);
+  server_->setCallback(
+    int_marker.name,
+    std::bind(&WaypointManager::processFeedback, this, std::placeholders::_1));
+  menu_handler_.apply(*server_, int_marker.name);
+  server_->applyChanges();
+
+  selected_marker_name_ = str_name;
+
+  // Convert quaternion to yaw for signal
+  tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
+  tf2::Matrix3x3 m(qt);
+  double roll, pitch, yaw;
+  m.getRPY(roll, pitch, yaw);
+
+  Q_EMIT waypointCountChanged(static_cast<int>(node_map_.size()));
+  Q_EMIT selectedMarkerChanged(
+    QString::fromStdString(str_name), position.x, position.y, position.z, yaw);
+}
+
+void WaypointManager::removeWaypoint(int idx)
+{
+  if (!initialized_) { return; }
+
+  auto it = node_map_.find(idx);
+  if (it == node_map_.end()) { return; }
+
+  // Destroy the scene node for the deleted waypoint
+  it->second->detachAllObjects();
+  scene_manager_->destroySceneNode(it->second);
+
+  std::stringstream wp_name;
+  wp_name << kWaypointPrefix << idx;
+  server_->erase(wp_name.str());
+  node_map_.erase(it);
+
+  // Reorder: rebuild node_map_ with sequential 1-based keys and rename
+  // interactive markers so there are no gaps.
+  reorderWaypoints();
+
+  server_->applyChanges();
+
+  // Set unique_idx_ so the next added waypoint gets the right number
+  unique_idx_ = node_map_.empty() ? 0 : node_map_.rbegin()->first;
+
+  Q_EMIT waypointCountChanged(static_cast<int>(node_map_.size()));
+}
+
+void WaypointManager::reorderWaypoints()
+{
+  // Collect entries in current order (std::map iterates by key, ascending)
+  std::vector<std::pair<int, Ogre::SceneNode *>> entries(
+    node_map_.begin(), node_map_.end());
+
+  // Erase all interactive markers from the server (but keep scene nodes alive)
+  for (const auto & [old_idx, node_ptr] : entries) {
+    std::stringstream old_name;
+    old_name << kWaypointPrefix << old_idx;
+    server_->erase(old_name.str());
+  }
+
+  // Rebuild node_map_ with sequential 1-based keys
+  node_map_.clear();
+  for (size_t i = 0; i < entries.size(); ++i) {
+    int new_idx = static_cast<int>(i) + 1;
+    Ogre::SceneNode * node_ptr = entries[i].second;
+    node_map_[new_idx] = node_ptr;
+
+    // Re-create the interactive marker with the new name
+    std::stringstream new_name;
+    new_name << kWaypointPrefix << new_idx;
+    std::string str_name = new_name.str();
+
+    Ogre::Vector3 pos = node_ptr->getPosition();
+    Ogre::Quaternion quat = node_ptr->getOrientation();
+
+    visualization_msgs::msg::InteractiveMarker int_marker;
+    int_marker.header.stamp = node_->now();
+    {
+      std::lock_guard<std::mutex> lock(mtx_);
+      int_marker.header.frame_id = frame_id_;
+    }
+    int_marker.pose.position.x = pos.x;
+    int_marker.pose.position.y = pos.y;
+    int_marker.pose.position.z = pos.z;
+    int_marker.pose.orientation.x = quat.x;
+    int_marker.pose.orientation.y = quat.y;
+    int_marker.pose.orientation.z = quat.z;
+    int_marker.pose.orientation.w = quat.w;
+    int_marker.scale = 1.0;
+    int_marker.name = str_name;
+
+    // Invisible cylinder for picking
+    visualization_msgs::msg::Marker marker;
+    marker.type = visualization_msgs::msg::Marker::CYLINDER;
+    marker.scale.x = 0.5;
+    marker.scale.y = 0.5;
+    marker.scale.z = 0.1;
+    marker.color.a = 0.0;
+
+    visualization_msgs::msg::InteractiveMarkerControl c_control;
+    c_control.always_visible = true;
+    c_control.markers.push_back(marker);
+    int_marker.controls.push_back(c_control);
+
+    visualization_msgs::msg::InteractiveMarkerControl control;
+    control.orientation.w = 0.707106781;
+    control.orientation.x = 0;
+    control.orientation.y = 0.707106781;
+    control.orientation.z = 0;
+    control.interaction_mode =
+      visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE;
+    int_marker.controls.push_back(control);
+
+    control.interaction_mode =
+      visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
+    int_marker.controls.push_back(control);
+
+    control.interaction_mode =
+      visualization_msgs::msg::InteractiveMarkerControl::MENU;
+    control.name = "menu_delete";
+    control.description = str_name;
+    int_marker.controls.push_back(control);
+
+    server_->insert(int_marker);
+    server_->setCallback(
+      int_marker.name,
+      std::bind(&WaypointManager::processFeedback, this,
+                std::placeholders::_1));
+    menu_handler_.apply(*server_, int_marker.name);
+  }
+}
+
+void WaypointManager::clearAll()
+{
+  if (!initialized_) { return; }
+
+  for (auto & [idx, node_ptr] : node_map_) {
+    scene_manager_->destroySceneNode(node_ptr);
+  }
+  node_map_.clear();
+  unique_idx_ = 0;
+
+  server_->clear();
+  server_->applyChanges();
+
+  Q_EMIT waypointCountChanged(0);
+  Q_EMIT waypointsCleared();
+}
+
+nav_msgs::msg::Path WaypointManager::getPath() const
+{
+  nav_msgs::msg::Path path;
+  for (const auto & [idx, node_ptr] : node_map_) {
+    Ogre::Vector3 pos = node_ptr->getPosition();
+    Ogre::Quaternion quat = node_ptr->getOrientation();
+
+    geometry_msgs::msg::PoseStamped pose;
+    pose.pose.position.x = pos.x;
+    pose.pose.position.y = pos.y;
+    pose.pose.position.z = pos.z;
+    pose.pose.orientation.x = quat.x;
+    pose.pose.orientation.y = quat.y;
+    pose.pose.orientation.z = quat.z;
+    pose.pose.orientation.w = quat.w;
+    path.poses.push_back(pose);
+  }
+  {
+    std::lock_guard<std::mutex> lock(mtx_);
+    path.header.frame_id = frame_id_;
+  }
+  if (node_) {
+    path.header.stamp = node_->now();
+  }
+  return path;
+}
+
+void WaypointManager::publishPath()
+{
+  if (!path_publisher_) { return; }
+  path_publisher_->publish(getPath());
+}
+
+void WaypointManager::updateSelectedPose(double x, double y, double z,
+                                         double yaw)
+{
+  if (!initialized_) { return; }
+
+  int selected_idx = 0;
+  try {
+    selected_idx = std::stoi(
+      selected_marker_name_.substr(strlen(kWaypointPrefix)));
+  } catch (const std::logic_error & e) {
+    RCLCPP_ERROR_STREAM(node_->get_logger(), e.what());
+    return;
+  }
+
+  auto it = node_map_.find(selected_idx);
+  if (it == node_map_.end()) {
+    RCLCPP_ERROR(node_->get_logger(), "%s not found in map",
+                 selected_marker_name_.c_str());
+    return;
+  }
+
+  Ogre::Vector3 position(x, y, z);
+  tf2::Quaternion qt;
+  qt.setRPY(0.0, 0.0, yaw);
+  Ogre::Quaternion quaternion(qt.w(), qt.x(), qt.y(), qt.z());
+
+  it->second->setPosition(position);
+  it->second->setOrientation(quaternion);
+
+  std::stringstream wp_name;
+  wp_name << kWaypointPrefix << it->first;
+  std::string wp_name_str(wp_name.str());
+
+  visualization_msgs::msg::InteractiveMarker int_marker;
+  if (server_->get(wp_name_str, int_marker)) {
+    int_marker.pose.position.x = x;
+    int_marker.pose.position.y = y;
+    int_marker.pose.position.z = z;
+    int_marker.pose.orientation.x = qt.x();
+    int_marker.pose.orientation.y = qt.y();
+    int_marker.pose.orientation.z = qt.z();
+    int_marker.pose.orientation.w = qt.w();
+    server_->setPose(wp_name_str, int_marker.pose, int_marker.header);
+  }
+  server_->applyChanges();
+}
+
+// ─────────────────────────── persistence ──────────────────────────────────────
+
+void WaypointManager::saveBag(const std::string & filename)
+{
+  nav_msgs::msg::Path path = getPath();
+
+  try {
+    rosbag2_cpp::Writer writer;
+
+    rosbag2_storage::StorageOptions storage_options;
+    storage_options.uri = filename;
+    storage_options.storage_id = "sqlite3";
+
+    rosbag2_cpp::ConverterOptions converter_options;
+    converter_options.input_serialization_format = "cdr";
+    converter_options.output_serialization_format = "cdr";
+
+    writer.open(storage_options, converter_options);
+
+    rosbag2_storage::TopicMetadata topic_metadata;
+    topic_metadata.name = "waypoints";
+    topic_metadata.type = "nav_msgs/msg/Path";
+    topic_metadata.serialization_format = rmw_get_serialization_format();
+    writer.create_topic(topic_metadata);
+
+    auto serialized_msg = std::make_shared<rclcpp::SerializedMessage>();
+    rclcpp::Serialization<nav_msgs::msg::Path> serialization;
+    serialization.serialize_message(&path, serialized_msg.get());
+
+    auto bag_message =
+      std::make_shared<rosbag2_storage::SerializedBagMessage>();
+    bag_message->topic_name = "waypoints";
+    bag_message->recv_timestamp = node_->now().nanoseconds();
+    bag_message->serialized_data = std::shared_ptr<rcutils_uint8_array_t>(
+      new rcutils_uint8_array_t,
+      [](rcutils_uint8_array_t * data) {
+        auto fini_return = rcutils_uint8_array_fini(data);
+        delete data;
+        if (fini_return != RCUTILS_RET_OK) {
+          RCUTILS_LOG_ERROR_NAMED(
+            "waypoint_rviz2_plugin",
+            "Failed to destroy serialized message %s",
+            rcutils_get_error_string().str);
+        }
+      });
+    *bag_message->serialized_data =
+      serialized_msg->release_rcl_serialized_message();
+
+    writer.write(bag_message);
+
+    RCLCPP_INFO(node_->get_logger(), "Waypoints saved successfully");
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to save bag: %s", e.what());
+  }
+}
+
+void WaypointManager::loadBag(const std::string & filename)
+{
+  try {
+    rosbag2_cpp::Reader reader;
+
+    rosbag2_storage::StorageOptions storage_options;
+    storage_options.uri = filename;
+    storage_options.storage_id = "sqlite3";
+
+    rosbag2_cpp::ConverterOptions converter_options;
+    converter_options.input_serialization_format = "cdr";
+    converter_options.output_serialization_format = "cdr";
+
+    reader.open(storage_options, converter_options);
+
+    while (reader.has_next()) {
+      auto bag_message = reader.read_next();
+
+      if (bag_message->topic_name == "waypoints") {
+        rclcpp::SerializedMessage serialized_msg(
+          *bag_message->serialized_data);
+        nav_msgs::msg::Path path;
+
+        rclcpp::Serialization<nav_msgs::msg::Path> serialization;
+        serialization.deserialize_message(&serialized_msg, &path);
+
+        RCLCPP_INFO(node_->get_logger(), "Loading %zu waypoints",
+                    path.poses.size());
+
+        for (size_t i = 0; i < path.poses.size(); i++) {
+          const auto & pos = path.poses[i];
+          Ogre::Vector3 position(
+            pos.pose.position.x, pos.pose.position.y, pos.pose.position.z);
+          Ogre::Quaternion quaternion(
+            pos.pose.orientation.w, pos.pose.orientation.x,
+            pos.pose.orientation.y, pos.pose.orientation.z);
+          addWaypoint(position, quaternion);
+        }
+      }
+    }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to load bag: %s", e.what());
+  }
+}
+
+// ─────────────────────────── accessors ────────────────────────────────────────
+
+std::string WaypointManager::frameId() const
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  return frame_id_;
+}
+
+void WaypointManager::setFrameId(const std::string & frame)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  frame_id_ = frame;
+}
+
+std::string WaypointManager::outputTopic() const
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  return output_topic_;
+}
+
+void WaypointManager::setOutputTopic(const std::string & topic)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  if (topic == output_topic_) { return; }
+  output_topic_ = topic;
+  if (node_ && !topic.empty() && topic != "/") {
+    path_publisher_.reset();
+    path_publisher_ =
+      node_->create_publisher<nav_msgs::msg::Path>(output_topic_, 1);
+  }
+}
+
+double WaypointManager::defaultHeight() const
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  return default_height_;
+}
+
+void WaypointManager::setDefaultHeight(double h)
+{
+  std::lock_guard<std::mutex> lock(mtx_);
+  default_height_ = h;
+}
+
+int WaypointManager::waypointCount() const
+{
+  return static_cast<int>(node_map_.size());
+}
+
+// ─────────────────────────── feedback ─────────────────────────────────────────
+
+void WaypointManager::processFeedback(
+  const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &
+    feedback)
+{
+  switch (feedback->event_type) {
+    case visualization_msgs::msg::InteractiveMarkerFeedback::MENU_SELECT: {
+      int idx = 0;
+      try {
+        idx = std::stoi(
+          feedback->marker_name.substr(strlen(kWaypointPrefix)));
+      } catch (const std::logic_error & e) {
+        RCLCPP_ERROR_STREAM(node_->get_logger(), e.what());
+        return;
+      }
+
+      auto node_entry = node_map_.find(idx);
+      if (node_entry == node_map_.end()) {
+        RCLCPP_ERROR(node_->get_logger(), "%s not found in map",
+                     feedback->marker_name.c_str());
+        return;
+      }
+
+      if (feedback->menu_entry_id == 1) {
+        // delete
+        node_entry->second->detachAllObjects();
+        scene_manager_->destroySceneNode(node_entry->second);
+
+        std::stringstream wp_name;
+        wp_name << kWaypointPrefix << node_entry->first;
+        server_->erase(wp_name.str());
+        node_map_.erase(node_entry);
+
+        reorderWaypoints();
+        server_->applyChanges();
+
+        unique_idx_ = node_map_.empty() ? 0 : node_map_.rbegin()->first;
+
+        Q_EMIT waypointCountChanged(static_cast<int>(node_map_.size()));
+      } else {
+        // "set manual" – select this marker for editing in the Navigate panel
+        selected_marker_name_ = feedback->marker_name;
+
+        Ogre::Vector3 position = node_entry->second->getPosition();
+        Ogre::Quaternion quaternion = node_entry->second->getOrientation();
+
+        tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z,
+                           quaternion.w);
+        tf2::Matrix3x3 m(qt);
+        double roll, pitch, yaw;
+        m.getRPY(roll, pitch, yaw);
+
+        Q_EMIT selectedMarkerChanged(
+          QString::fromStdString(feedback->marker_name),
+          position.x, position.y, position.z, yaw);
+      }
+      break;
+    }
+    case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE: {
+      int idx = 0;
+      try {
+        idx = std::stoi(
+          feedback->marker_name.substr(strlen(kWaypointPrefix)));
+      } catch (const std::logic_error & e) {
+        RCLCPP_ERROR_STREAM(node_->get_logger(), e.what());
+        return;
+      }
+
+      auto node_entry = node_map_.find(idx);
+      if (node_entry == node_map_.end()) {
+        RCLCPP_ERROR(node_->get_logger(), "%s not found in map",
+                     feedback->marker_name.c_str());
+        return;
+      }
+
+      const auto & p = feedback->pose;
+      Ogre::Vector3 position(p.position.x, p.position.y, p.position.z);
+      Ogre::Quaternion quaternion(
+        p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
+
+      node_entry->second->setPosition(position);
+      node_entry->second->setOrientation(quaternion);
+
+      selected_marker_name_ = feedback->marker_name;
+
+      tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z,
+                         quaternion.w);
+      tf2::Matrix3x3 m(qt);
+      double roll, pitch, yaw;
+      m.getRPY(roll, pitch, yaw);
+
+      Q_EMIT selectedMarkerChanged(
+        QString::fromStdString(feedback->marker_name),
+        position.x, position.y, position.z, yaw);
+      break;
+    }
+  }
+}
+
+}  // namespace waypoint_rviz2_plugin
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_tool.cpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_tool.cpp
new file mode 100644
index 000000000..f41882d53
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_tool.cpp
@@ -0,0 +1,184 @@
+/**
+ * @file waypoint_tool.cpp
+ * @author KoKoLates (the21515@gmail.com)
+ * @version 2.0.0
+ *
+ * RViz2 Tool for placing/removing waypoints via mouse clicks.
+ * All waypoint state is delegated to WaypointManager.
+ */
+
+#include "waypoint_rviz2_plugin/waypoint_tool.hpp"
+
+#ifndef Q_MOC_RUN
+#include <rclcpp/logging.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/render_panel.hpp>
+#include <rviz_common/viewport_mouse_event.hpp>
+#include <rviz_rendering/mesh_loader.hpp>
+
+#include <OgreCamera.h>
+#include <OgreEntity.h>
+#include <OgrePlane.h>
+#include <OgreRay.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+#include <OgreViewport.h>
+#endif
+
+// Define slots macro for render_window.hpp compatibility
+#ifndef slots
+#define slots Q_SLOTS
+#define signals Q_SIGNALS
+#endif
+
+#include <rviz_rendering/render_window.hpp>
+
+namespace waypoint_rviz2_plugin {
+
+WaypointTool::WaypointTool()
+{
+  shortcut_key_ = '1';
+}
+
+WaypointTool::~WaypointTool()
+{
+  // move_axis_node_ is owned by the Ogre SceneManager; it is cleaned up when
+  // RViz destroys the scene.
+}
+
+void WaypointTool::onInitialize()
+{
+  node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
+
+  std::string axis_resource = "package://waypoint_rviz2_plugin/media/axis.dae";
+  if (rviz_rendering::loadMeshFromResource(axis_resource).isNull()) {
+    RCLCPP_ERROR(node_->get_logger(),
+                 "WaypointTool: failed to load model resource '%s'.",
+                 axis_resource.c_str());
+    return;
+  }
+
+  move_axis_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+  Ogre::Entity * entity = scene_manager_->createEntity(axis_resource);
+  move_axis_node_->attachObject(entity);
+  move_axis_node_->setVisible(false);
+
+  // Create / retrieve the shared WaypointManager singleton
+  manager_ = WaypointManager::instance();
+  if (!manager_->isInitialized()) {
+    manager_->initialize(context_, node_);
+  }
+}
+
+void WaypointTool::activate()
+{
+  if (move_axis_node_) {
+    move_axis_node_->setVisible(true);
+  }
+}
+
+void WaypointTool::deactivate()
+{
+  if (move_axis_node_) {
+    move_axis_node_->setVisible(false);
+  }
+}
+
+int WaypointTool::processMouseEvent(rviz_common::ViewportMouseEvent & event)
+{
+  if (!move_axis_node_ || !manager_ || !manager_->isInitialized()) {
+    return Render;
+  }
+
+  double height = manager_->defaultHeight();
+  Ogre::Vector3 intersection;
+  Ogre::Quaternion quaternion;
+  Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, height);
+
+  auto render_window = event.panel->getRenderWindow();
+  Ogre::Camera * camera =
+    rviz_rendering::RenderWindowOgreAdapter::getOgreCamera(render_window);
+  Ogre::Viewport * viewport =
+    rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_window);
+
+  int width = viewport->getActualWidth();
+  int height_px = viewport->getActualHeight();
+  float screen_x = static_cast<float>(event.x) / static_cast<float>(width);
+  float screen_y = static_cast<float>(event.y) / static_cast<float>(height_px);
+
+  Ogre::Ray mouse_ray = camera->getCameraToViewportRay(screen_x, screen_y);
+  std::pair<bool, Ogre::Real> result = mouse_ray.intersects(ground_plane);
+
+  if (result.first) {
+    intersection = mouse_ray.getPoint(result.second);
+    move_axis_node_->setVisible(true);
+    move_axis_node_->setPosition(intersection);
+
+    // Check proximity to existing waypoints
+    const auto & node_map = manager_->nodeMap();
+    for (auto it = node_map.begin(); it != node_map.end(); ++it) {
+      Ogre::Vector3 stored_pose = it->second->getPosition();
+      double distance = std::sqrt(
+        std::pow(stored_pose.x - intersection.x, 2) +
+        std::pow(stored_pose.y - intersection.y, 2));
+
+      if (distance < 0.4) {
+        move_axis_node_->setVisible(false);
+
+        if (event.rightDown()) {
+          manager_->removeWaypoint(it->first);
+          move_axis_node_->setVisible(true);
+          return Render | Finished;
+        }
+      }
+    }
+
+    if (event.leftDown()) {
+      manager_->addWaypoint(intersection, quaternion);
+      return Render | Finished;
+    }
+  } else {
+    move_axis_node_->setVisible(false);
+  }
+  return Render;
+}
+
+void WaypointTool::save(rviz_common::Config config) const
+{
+  config.mapSetValue("Class", getClassId());
+  rviz_common::Config wp_config = config.mapMakeChild("WaypointTool");
+
+  if (manager_) {
+    wp_config.mapSetValue("topic",
+                          QString::fromStdString(manager_->outputTopic()));
+    wp_config.mapSetValue("frame_id",
+                          QString::fromStdString(manager_->frameId()));
+    wp_config.mapSetValue("default_height", manager_->defaultHeight());
+  }
+}
+
+void WaypointTool::load(const rviz_common::Config & config)
+{
+  rviz_common::Config wp_config = config.mapGetChild("WaypointTool");
+  QString topic, frame;
+  float height = 0.0f;
+
+  if (!wp_config.mapGetString("topic", &topic)) {
+    topic = "/waypoints";
+  }
+  if (!wp_config.mapGetString("frame_id", &frame)) {
+    frame = "map";
+  }
+  wp_config.mapGetFloat("default_height", &height);
+
+  if (manager_) {
+    manager_->setOutputTopic(topic.toStdString());
+    manager_->setFrameId(frame.toStdString());
+    manager_->setDefaultHeight(height);
+  }
+}
+
+}  // namespace waypoint_rviz2_plugin
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(waypoint_rviz2_plugin::WaypointTool, rviz_common::Tool)
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_widget.cpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_widget.cpp
new file mode 100644
index 000000000..2fc25c6d8
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_widget.cpp
@@ -0,0 +1,503 @@
+/**
+ * @file waypoint_widget.cpp
+ * @author KoKoLates (the21515@gmail.com)
+ * @version 1.0.0
+ * @date 2023-06-18
+ * 
+ * ROS2 port of the interactive waypoint widget implementation
+ */
+
+#include "waypoint_rviz2_plugin/waypoint_widget.hpp"
+#include "waypoint_rviz2_plugin/waypoint_tool.hpp"
+
+#include <rosbag2_cpp/reader.hpp>
+#include <rosbag2_cpp/writer.hpp>
+#include <rosbag2_cpp/writers/sequential_writer.hpp>
+#include <rosbag2_storage/storage_options.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <QFileDialog>
+#include <rviz_common/display_context.hpp>
+#include <interactive_markers/interactive_marker_server.hpp>
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+
+#include <rclcpp/serialization.hpp>
+#include <rclcpp/serialized_message.hpp>
+
+namespace waypoint_rviz2_plugin {
+
+WaypointWidget::WaypointWidget(rviz_common::DisplayContext *context, 
+                               rclcpp::Node::SharedPtr node,
+                               std::map<int, Ogre::SceneNode*> *map_ptr,
+                               std::shared_ptr<interactive_markers::InteractiveMarkerServer> server, 
+                               int *unique_idx,
+                               QWidget *parent, 
+                               WaypointTool *waypoint_tool): QWidget(parent) 
+{
+    context_ = context;
+    node_ = node;
+    ui_ = new Ui::PluginWidget();
+    map_ptr_ = map_ptr;
+    unique_idx_ = unique_idx;
+    server_ = server;
+    frame_id_ = "map";
+    waypoint_tool_ = waypoint_tool;
+    default_height_ = 0.0;
+    selected_marker_name_ = std::string(waypoint_name_prefix) + "1";
+
+    scene_manager_ = context_->getSceneManager();
+    ui_->setupUi(this);
+    
+    path_publisher_ = node_->create_publisher<nav_msgs::msg::Path>("waypoints", 1);
+
+    connect(ui_->button_save,    SIGNAL(clicked()), this, SLOT(saveHandler()));
+    connect(ui_->button_load,    SIGNAL(clicked()), this, SLOT(loadHandler()));
+    connect(ui_->button_clear,   SIGNAL(clicked()), this, SLOT(clearHandler()));
+    connect(ui_->button_publish, SIGNAL(clicked()), this, SLOT(publishHandler()));
+
+    connect(ui_->x_spinbox,      SIGNAL(valueChanged(double)), this, SLOT(poseChange(double)));
+    connect(ui_->y_spinbox,      SIGNAL(valueChanged(double)), this, SLOT(poseChange(double)));
+    connect(ui_->z_spinbox,      SIGNAL(valueChanged(double)), this, SLOT(poseChange(double)));
+    connect(ui_->yaw_spinbox,    SIGNAL(valueChanged(double)), this, SLOT(poseChange(double)));
+    connect(ui_->height_spinbox, SIGNAL(valueChanged(double)), this, SLOT(heightChange(double)));
+    
+    connect(ui_->topic_input,    SIGNAL(editingFinished()), this, SLOT(topicChange()));
+    connect(ui_->frame_input,    SIGNAL(editingFinished()), this, SLOT(frameChange()));
+}
+
+WaypointWidget::~WaypointWidget() {
+    delete ui_;
+    map_ptr_ = nullptr;
+}
+
+void WaypointWidget::enable() {
+    show();
+}
+
+void WaypointWidget::disable() {
+    path_publisher_.reset();
+    hide();
+}
+
+void WaypointWidget::saveHandler() {
+    QString filename = QFileDialog::getSaveFileName(
+        0, tr("Waypoints Save"), "waypoints", 
+        tr("Save Files (*.db3)")
+    );
+    if (filename.isEmpty()) {
+        RCLCPP_ERROR(node_->get_logger(), "No saving file selected");
+        return;
+    }
+    const std::string str_filename = filename.toStdString();
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Saving the waypoints into " << str_filename);
+
+    if (filename.endsWith(".db3")) {
+        saveBag(str_filename);
+    }
+    else {
+        RCLCPP_INFO_STREAM(node_->get_logger(), "Invalid saving file format: " << str_filename);
+    }
+}
+
+void WaypointWidget::loadHandler() {
+    const QString filename = QFileDialog::getOpenFileName(
+        0, tr("Waypoint load"), "~/", 
+        tr("Load Files (*.db3)")
+    );
+    if (filename.isEmpty()) {
+        RCLCPP_ERROR(node_->get_logger(), "No loading file selected");
+        return;
+    }
+    const std::string str_filename = filename.toStdString();
+    RCLCPP_INFO(node_->get_logger(), "loading waypoints from %s", str_filename.c_str());
+    if (filename.endsWith(".db3")) {
+        loadBag(str_filename);
+    } 
+    else {
+        RCLCPP_INFO_STREAM(node_->get_logger(), "Invalid loading file format: " << str_filename);
+    }
+}
+
+void WaypointWidget::clearHandler() {
+    std::map<int, Ogre::SceneNode*>::iterator node_itr;
+    for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); node_itr++) {
+        scene_manager_->destroySceneNode(node_itr->second);
+    }
+    map_ptr_->clear();
+    *unique_idx_ = 0;
+
+    server_->clear();
+    server_->applyChanges();
+}
+
+void WaypointWidget::publishHandler() {
+    nav_msgs::msg::Path path;
+    std::map<int, Ogre::SceneNode*>::iterator node_itr;
+
+    for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); node_itr++) {
+        Ogre::Vector3 position;
+        position = node_itr->second->getPosition();
+
+        geometry_msgs::msg::PoseStamped pose;
+        pose.pose.position.x = position.x;
+        pose.pose.position.y = position.y;
+        pose.pose.position.z = position.z;
+
+        Ogre::Quaternion quat;
+        quat = node_itr->second->getOrientation();
+        pose.pose.orientation.x = quat.x;
+        pose.pose.orientation.y = quat.y;
+        pose.pose.orientation.z = quat.z;
+        pose.pose.orientation.w = quat.w;
+
+        path.poses.push_back(pose);
+    }
+    path.header.frame_id = frame_id_.toStdString();
+    path.header.stamp = node_->now();
+    path_publisher_->publish(path);
+}
+
+void WaypointWidget::poseChange(double value) {
+    auto node_entry = map_ptr_->end();
+    try {
+        const int selected_marker_idx = std::stoi(selected_marker_name_.substr(strlen(waypoint_name_prefix)));
+        node_entry = map_ptr_->find(selected_marker_idx);
+    }
+    catch (const std::logic_error& e) {
+        RCLCPP_ERROR_STREAM(node_->get_logger(), e.what());
+        return;
+    }
+
+    if (node_entry == map_ptr_->end())
+        RCLCPP_ERROR(node_->get_logger(), "%s not found in map", selected_marker_name_.c_str());
+    else
+    {
+        Ogre::Vector3 position;
+        Ogre::Quaternion quaternion;
+        getPose(position, quaternion);
+
+        node_entry->second->setPosition(position);
+        node_entry->second->setOrientation(quaternion);
+
+        std::stringstream waypoint_name;
+        waypoint_name << waypoint_name_prefix << node_entry->first;
+        std::string waypoint_name_str(waypoint_name.str());
+
+        visualization_msgs::msg::InteractiveMarker int_marker;
+        if(server_->get(waypoint_name_str, int_marker)) {
+            int_marker.pose.position.x = position.x;
+            int_marker.pose.position.y = position.y;
+            int_marker.pose.position.z = position.z;
+
+            int_marker.pose.orientation.x = quaternion.x;
+            int_marker.pose.orientation.y = quaternion.y;
+            int_marker.pose.orientation.z = quaternion.z;
+            int_marker.pose.orientation.w = quaternion.w;
+
+            server_->setPose(waypoint_name_str, int_marker.pose, int_marker.header);
+        }
+        server_->applyChanges();
+    }
+}
+
+void WaypointWidget::frameChange() {
+    std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+    QString new_frame = ui_->frame_input->text();
+
+    if ((new_frame != frame_id_) && (new_frame != "")) {
+        frame_id_ = new_frame;
+        RCLCPP_INFO(node_->get_logger(), "new frame: %s", frame_id_.toStdString().c_str());
+
+        std::map<int, Ogre::SceneNode *>::iterator node_itr;
+        for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); node_itr++) {
+            std::stringstream waypoint_name;
+            waypoint_name << "waypoint" << node_itr->first;
+            std::string waypoint_name_str(waypoint_name.str());
+
+            visualization_msgs::msg::InteractiveMarker int_marker;
+            if(server_->get(waypoint_name_str, int_marker)) {
+                int_marker.header.frame_id = new_frame.toStdString();
+                server_->setPose(waypoint_name_str, int_marker.pose, int_marker.header);
+            }
+        }
+        server_->applyChanges();
+    }
+}
+
+void WaypointWidget::topicChange() {
+    QString new_topic = ui_->topic_input->text();
+
+    if(new_topic != output_topic_) {
+        path_publisher_.reset();
+        output_topic_ = new_topic;
+        if((output_topic_ != "") && (output_topic_ != "/")) {
+            path_publisher_ = node_->create_publisher<nav_msgs::msg::Path>(
+                output_topic_.toStdString(), 1);
+        }
+    }
+}
+
+void WaypointWidget::heightChange(double value) {
+    auto sn_entry = map_ptr_->end();
+    try {
+        const int selected_marker_idx = std::stoi(
+            selected_marker_name_.substr((strlen((waypoint_name_prefix))))
+        );
+        sn_entry = map_ptr_->find(selected_marker_idx);
+    } catch(const std::logic_error &error) {
+        RCLCPP_ERROR_STREAM(node_->get_logger(), error.what());
+        return;
+    }
+
+    if (sn_entry == map_ptr_->end()) {
+        RCLCPP_ERROR(node_->get_logger(), "%s not found in map", selected_marker_name_.c_str());
+    } else {
+        Ogre::Vector3 position;
+        Ogre::Quaternion quaternion;
+        getPose(position, quaternion);
+
+        sn_entry->second->setPosition(position);
+        sn_entry->second->setOrientation(quaternion);
+
+        std::stringstream wp_name;
+        wp_name << waypoint_name_prefix << sn_entry->first;
+        std::string wp_name_str(wp_name.str());
+
+        visualization_msgs::msg::InteractiveMarker int_marker;
+        if (server_->get(wp_name_str, int_marker)) {
+            int_marker.pose.position.x = position.x;
+            int_marker.pose.position.y = position.y;
+            int_marker.pose.position.z = position.z;
+
+            int_marker.pose.orientation.x = quaternion.x;
+            int_marker.pose.orientation.y = quaternion.y;
+            int_marker.pose.orientation.z = quaternion.z;
+            int_marker.pose.orientation.w = quaternion.w;
+
+            server_->setPose(wp_name_str, int_marker.pose, int_marker.header);
+        }
+        server_->applyChanges();
+    }
+}
+
+void WaypointWidget::saveBag(const std::string& filename) {
+    nav_msgs::msg::Path path;
+    std::map<int, Ogre::SceneNode*>::iterator node_itr;
+    for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); ++node_itr) {
+        Ogre::Vector3 position;
+        position = node_itr->second->getPosition();
+
+        geometry_msgs::msg::PoseStamped pose;
+        pose.pose.position.x = position.x;
+        pose.pose.position.y = position.y;
+        pose.pose.position.z = position.z;
+
+        Ogre::Quaternion quat;
+        quat = node_itr->second->getOrientation();
+        pose.pose.orientation.x = quat.x;
+        pose.pose.orientation.y = quat.y;
+        pose.pose.orientation.z = quat.z;
+        pose.pose.orientation.w = quat.w;
+
+        path.poses.push_back(pose);
+    }
+    path.header.frame_id = frame_id_.toStdString();
+    path.header.stamp = node_->now();
+
+    try {
+        rosbag2_cpp::Writer writer;
+        
+        rosbag2_storage::StorageOptions storage_options;
+        storage_options.uri = filename;
+        storage_options.storage_id = "sqlite3";
+        
+        rosbag2_cpp::ConverterOptions converter_options;
+        converter_options.input_serialization_format = "cdr";
+        converter_options.output_serialization_format = "cdr";
+        
+        writer.open(storage_options, converter_options);
+
+        rosbag2_storage::TopicMetadata topic_metadata;
+        topic_metadata.name = "waypoints";
+        topic_metadata.type = "nav_msgs/msg/Path";
+        topic_metadata.serialization_format = rmw_get_serialization_format();
+        writer.create_topic(topic_metadata);
+
+        auto serialized_msg = std::make_shared<rclcpp::SerializedMessage>();
+        rclcpp::Serialization<nav_msgs::msg::Path> serialization;
+        serialization.serialize_message(&path, serialized_msg.get());
+
+        auto bag_message = std::make_shared<rosbag2_storage::SerializedBagMessage>();
+        bag_message->topic_name = "waypoints";
+        bag_message->recv_timestamp = node_->now().nanoseconds();
+        bag_message->serialized_data = std::shared_ptr<rcutils_uint8_array_t>(
+            new rcutils_uint8_array_t,
+            [](rcutils_uint8_array_t* data) {
+                auto fini_return = rcutils_uint8_array_fini(data);
+                delete data;
+                if (fini_return != RCUTILS_RET_OK) {
+                    RCUTILS_LOG_ERROR_NAMED(
+                        "waypoint_rviz2_plugin",
+                        "Failed to destroy serialized message %s", rcutils_get_error_string().str);
+                }
+            });
+        *bag_message->serialized_data = serialized_msg->release_rcl_serialized_message();
+
+        writer.write(bag_message);
+        
+        RCLCPP_INFO(node_->get_logger(), "Waypoints saved successfully");
+    }
+    catch (const std::exception& e) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to save bag: %s", e.what());
+    }
+}
+
+void WaypointWidget::loadBag(const std::string& filename) {
+    try {
+        rosbag2_cpp::Reader reader;
+        
+        rosbag2_storage::StorageOptions storage_options;
+        storage_options.uri = filename;
+        storage_options.storage_id = "sqlite3";
+        
+        rosbag2_cpp::ConverterOptions converter_options;
+        converter_options.input_serialization_format = "cdr";
+        converter_options.output_serialization_format = "cdr";
+        
+        reader.open(storage_options, converter_options);
+
+        while (reader.has_next()) {
+            auto bag_message = reader.read_next();
+            
+            if (bag_message->topic_name == "waypoints") {
+                rclcpp::SerializedMessage serialized_msg(*bag_message->serialized_data);
+                nav_msgs::msg::Path path;
+                
+                rclcpp::Serialization<nav_msgs::msg::Path> serialization;
+                serialization.deserialize_message(&serialized_msg, &path);
+
+                RCLCPP_INFO(node_->get_logger(), "n waypoints %zu", path.poses.size());
+
+                for (size_t i = 0; i < path.poses.size(); i++) {
+                    geometry_msgs::msg::PoseStamped pos = path.poses[i];
+                    Ogre::Vector3 position;
+                    position.x = pos.pose.position.x;
+                    position.y = pos.pose.position.y;
+                    position.z = pos.pose.position.z;
+
+                    Ogre::Quaternion quaternion;
+                    quaternion.x = pos.pose.orientation.x;
+                    quaternion.y = pos.pose.orientation.y;
+                    quaternion.z = pos.pose.orientation.z;
+                    quaternion.w = pos.pose.orientation.w;
+
+                    waypoint_tool_->makeItem(position, quaternion);
+                }
+            }
+        }
+    }
+    catch (const std::exception& e) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to load bag: %s", e.what());
+    }
+}
+
+void WaypointWidget::setPose(const Ogre::Vector3& position, const Ogre::Quaternion& quaternion) {
+    ui_->x_spinbox->blockSignals(true);
+    ui_->y_spinbox->blockSignals(true);
+    ui_->z_spinbox->blockSignals(true);
+    ui_->yaw_spinbox->blockSignals(true);
+
+    ui_->x_spinbox->setValue(position.x);
+    ui_->y_spinbox->setValue(position.y);
+    ui_->z_spinbox->setValue(position.z);
+
+    tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
+    tf2::Matrix3x3 m(qt);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+    ui_->yaw_spinbox->setValue(yaw);
+
+    ui_->x_spinbox->blockSignals(false);
+    ui_->y_spinbox->blockSignals(false);
+    ui_->z_spinbox->blockSignals(false);
+    ui_->yaw_spinbox->blockSignals(false);
+}
+
+void WaypointWidget::setConfig(QString topic, QString frame, float height) {
+    {
+        std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+        ui_->topic_input->blockSignals(true);
+        ui_->frame_input->blockSignals(true);
+        ui_->height_spinbox->blockSignals(true);
+
+        ui_->topic_input->setText(topic);
+        ui_->frame_input->setText(frame);
+        ui_->height_spinbox->setValue(height);
+
+        ui_->topic_input->blockSignals(false);
+        ui_->frame_input->blockSignals(false);
+        ui_->height_spinbox->blockSignals(false);
+    }
+
+    topicChange();
+    frameChange();
+    heightChange(height);
+}
+
+void WaypointWidget::setWaypointLabel(Ogre::Vector3 position) {
+    std::ostringstream string_stream;
+    string_stream.precision(2);
+    string_stream << selected_marker_name_;
+    std::string label = string_stream.str();
+    ui_->text_selected->setText(QString::fromStdString(label));
+}
+
+void WaypointWidget::setWaypointCount(int size) {
+    std::ostringstream string_stream;
+    string_stream << "Total waypoints: " << size;
+    std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+    ui_->text_total->setText(
+        QString::fromStdString(string_stream.str())
+    );
+}
+
+void WaypointWidget::setSelectedMarkerName(std::string name) {
+    selected_marker_name_ = name;
+}
+
+void WaypointWidget::getPose(Ogre::Vector3& position, Ogre::Quaternion& quaternion) {
+    std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+    position.x = ui_->x_spinbox->value();
+    position.y = ui_->y_spinbox->value();
+    position.z = ui_->z_spinbox->value();
+    double yaw = ui_->yaw_spinbox->value();
+
+    tf2::Quaternion qt;
+    qt.setRPY(0.0, 0.0, yaw);
+    quaternion.x = qt.x();
+    quaternion.y = qt.y();
+    quaternion.z = qt.z();
+    quaternion.w = qt.w();
+}
+
+double WaypointWidget::getDefaultHeight() {
+    std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+    return default_height_;
+}
+
+QString WaypointWidget::getFrameId() {
+    std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+    return frame_id_;
+}
+
+QString WaypointWidget::getOutputTopic() {
+    std::lock_guard<std::mutex> lock(frame_updates_mutex_);
+    return output_topic_;
+}
+
+}
diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/ui/waypoint_plugin.ui b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/ui/waypoint_plugin.ui
new file mode 100644
index 000000000..bce84c609
--- /dev/null
+++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/ui/waypoint_plugin.ui
@@ -0,0 +1,253 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>PluginWidget</class>
+ <widget class="QWidget" name="PluginWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>469</width>
+    <height>175</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>Quadrotor Steering</string>
+  </property>
+  <layout class="QGridLayout" name="gridLayout">
+   <item row="5" column="1" colspan="3">
+    <layout class="QHBoxLayout" name="horizontalLayout_3">
+     <item>
+      <widget class="QLabel" name="label_topic">
+       <property name="text">
+        <string>Topic</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="topic_input">
+       <property name="text">
+        <string>/waypoints</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="label_frame">
+       <property name="text">
+        <string>Frame</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLineEdit" name="frame_input">
+       <property name="text">
+        <string>map</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QCheckBox" name="sixDcheckBox">
+       <property name="text">
+        <string>6D</string>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item row="6" column="1" colspan="3">
+    <layout class="QHBoxLayout" name="horizontalLayout_2">
+     <item>
+      <widget class="QLabel" name="wp_height_label">
+       <property name="text">
+        <string>Default Height</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QDoubleSpinBox" name="height_spinbox">
+       <property name="minimum">
+        <double>-5.000000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.100000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="text_total">
+       <property name="text">
+        <string>Total Waypoints: </string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="text_selected">
+       <property name="text">
+        <string>Selected wp: </string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QLabel" name="sel_wp_label">
+       <property name="text">
+        <string/>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item row="10" column="2" colspan="2">
+    <widget class="QPushButton" name="button_load">
+     <property name="text">
+      <string>Load Waypoints</string>
+     </property>
+    </widget>
+   </item>
+   <item row="8" column="1" colspan="3">
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <item>
+      <widget class="QLabel" name="x_label">
+       <property name="text">
+        <string>X:</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QDoubleSpinBox" name="x_spinbox">
+       <property name="minimum">
+        <double>-1000.000000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>1000.000000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.500000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QLabel" name="y_label">
+       <property name="text">
+        <string>Y:</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QDoubleSpinBox" name="y_spinbox">
+       <property name="minimum">
+        <double>-1000.000000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>1000.000000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.500000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer_2">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QLabel" name="z_label">
+       <property name="text">
+        <string>Z:</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QDoubleSpinBox" name="z_spinbox">
+       <property name="minimum">
+        <double>-1000.000000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>1000.000000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.500000000000000</double>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer_3">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item>
+      <widget class="QLabel" name="yaw_label">
+       <property name="text">
+        <string>Yaw:</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QDoubleSpinBox" name="yaw_spinbox">
+       <property name="minimum">
+        <double>-3.150000000000000</double>
+       </property>
+       <property name="maximum">
+        <double>3.150000000000000</double>
+       </property>
+       <property name="singleStep">
+        <double>0.100000000000000</double>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
+   <item row="9" column="1">
+    <widget class="QPushButton" name="button_clear">
+     <property name="text">
+      <string>Clear All</string>
+     </property>
+    </widget>
+   </item>
+   <item row="10" column="1">
+    <widget class="QPushButton" name="button_save">
+     <property name="text">
+      <string>Save Waypoints</string>
+     </property>
+    </widget>
+   </item>
+   <item row="9" column="2" colspan="2">
+    <widget class="QPushButton" name="button_publish">
+     <property name="text">
+      <string>Publish Waypoints</string>
+     </property>
+    </widget>
+   </item>
+  </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/.clang-format b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.clang-format
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/.clang-format
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.clang-format
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/.gitignore b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.gitignore
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/.gitignore
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.gitignore
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/.pre-commit-config.yaml b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.pre-commit-config.yaml
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/.pre-commit-config.yaml
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.pre-commit-config.yaml
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/CMakeLists.txt b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/CMakeLists.txt
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/CMakeLists.txt
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/CMakeLists.txt
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/LICENSE b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/LICENSE
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/LICENSE
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/LICENSE
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/README.md b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/README.md
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/README.md
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/README.md
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/DemoPanel.png b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/DemoPanel.png
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/DemoPanel.png
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/DemoPanel.png
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/package.xml b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/package.xml
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/package.xml
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/package.xml
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/rviz_common_plugins.xml b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/rviz_common_plugins.xml
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/rviz_common_plugins.xml
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/rviz_common_plugins.xml
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/scripts/simple_test_publisher.py b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/simple_test_publisher.py
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/scripts/simple_test_publisher.py
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/simple_test_publisher.py
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp
diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/xdot_cpp b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/xdot_cpp
similarity index 100%
rename from common/ros_packages/gui/rviz_behavior_tree_panel/xdot_cpp
rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/xdot_cpp
diff --git a/common/ros_packages/gui/rviz/rviz_polygon_selection_tool b/common/ros_packages/gui/rviz/rviz_polygon_selection_tool
new file mode 160000
index 000000000..018c74644
--- /dev/null
+++ b/common/ros_packages/gui/rviz/rviz_polygon_selection_tool
@@ -0,0 +1 @@
+Subproject commit 018c74644f1c1b96dde48b4bf118c3bcf0d70877
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/CMakeLists.txt b/common/ros_packages/gui/rviz/rviz_tasks_panel/CMakeLists.txt
new file mode 100644
index 000000000..e6f81b9cc
--- /dev/null
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/CMakeLists.txt
@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.8)
+project(rviz_tasks_panel)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_AUTOMOC ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(Qt5 REQUIRED COMPONENTS Core Widgets Gui)
+find_package(task_msgs REQUIRED)
+find_package(airstack_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(diagnostic_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(rviz_polygon_selection_tool REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(waypoint_rviz2_plugin REQUIRED)
+
+qt5_wrap_cpp(MOC_FILES
+  include/rviz_tasks_panel/tasks_panel.hpp
+)
+
+add_library(tasks_panel SHARED
+  src/tasks_panel.cpp
+  ${MOC_FILES}
+)
+
+target_include_directories(tasks_panel PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(tasks_panel
+  pluginlib
+  rviz_common
+  rclcpp
+  rclcpp_action
+  task_msgs
+  airstack_msgs
+  geometry_msgs
+  nav_msgs
+  std_msgs
+  diagnostic_msgs
+  action_msgs
+  rviz_polygon_selection_tool
+  waypoint_rviz2_plugin
+)
+
+target_link_libraries(tasks_panel
+  Qt5::Core
+  Qt5::Widgets
+  Qt5::Gui
+)
+
+install(TARGETS tasks_panel
+  EXPORT export_rviz_tasks_panel
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(FILES plugin_description.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_include_directories(include)
+ament_export_targets(export_rviz_tasks_panel)
+pluginlib_export_plugin_description_file(rviz_common plugin_description.xml)
+
+ament_package()
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md b/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md
new file mode 100644
index 000000000..1822cb492
--- /dev/null
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md
@@ -0,0 +1,185 @@
+# RViz Tasks Panel
+
+RViz2 panel plugin for dispatching and monitoring ROS 2 task
+action goals. Provides a tabbed GUI where operators can
+parameterize, execute, and cancel tasks on any discovered robot,
+with live feedback and result display.
+
+![Screenshot of the RViz Tasks Panel showing multiple tabs and parameter widgets](./media/rviz_tasks_panel.png)
+![takeoff](./media/tasks_panel_takeoff.png)
+![navigate](./media/tasks_panel_navigate.png)
+
+## Overview
+
+The Tasks Panel replaces CLI-based action goal dispatch with a
+graphical interface for all 9 AirStack task types. Each task type
+gets its own tab with auto-generated parameter widgets, an
+executor selector, and a feedback/result view.
+
+The **Navigate** tab integrates directly with the
+`waypoint_rviz2_plugin` package's shared `WaypointManager`
+singleton, providing inline controls for placing, editing,
+saving/loading, and executing waypoint-based navigation -- no
+separate waypoint panel needed.
+
+```text
++-----------------------------------------------------+
+|  Tasks Panel                      Robot: [robot_1]   |
++-----------------------------------------------------+
+| [Takeoff] [Land] [Navigate] [Exploration] [Coverage] |
++-----------------------------------------------------+
+|  +- Goal Parameters -----+  +- Feedback & Result -+ |
+|  | Executor: [/robot1/..] |  | Feedback:           | |
+|  | Height: [2.0]  Wp: 3   |  | [live stream]       | |
+|  | X:[1] Y:[2] Z:[2] Y:[0]|  |                     | |
+|  | [Clear] [Save] [Load]  |  | Result:             | |
+|  | goal_tolerance_m: [1.0] |  | [goal outcome]      | |
+|  | [Cancel]     [Execute]  |  |                     | |
+|  +-------------------------+  +---------------------+ |
++-----------------------------------------------------+
+```
+
+## Features
+
+- **9 task tabs** with auto-generated goal parameter widgets
+- **Executor discovery** -- scans ROS 2 topics every 5 seconds
+  to find running action servers
+- **Robot namespace selector** -- auto-populated from discovered
+  action server namespaces
+- **Polygon input** -- integrates with
+  `rviz_polygon_selection_tool` to capture 3D polygon selections
+- **Integrated waypoint controls** -- the Navigate tab embeds
+  waypoint management (height, X/Y/Z/Yaw editing, Clear, Save,
+  Load) via the shared `WaypointManager` singleton from
+  `waypoint_rviz2_plugin`
+- **Fixed Trajectory editor** -- type dropdown with
+  auto-populated default attributes in an editable key-value
+  table
+- **Live feedback** -- timestamped feedback messages stream in
+  real time
+- **Result display** -- color-coded status (green = succeeded,
+  red = aborted, orange = canceled)
+- **Config persistence** -- robot and executor selections are
+  saved/restored with the RViz config
+- **Single-task lock** -- only one action may execute at a time
+  to prevent conflicts
+- **Airborne-only tasks** -- Land, Navigate, and all mission
+  tasks are disabled until the drone is airborne (subscribes to
+  `is_airborne` from `takeoff_landing_planner`); Takeoff is
+  always available while idle
+
+## Supported Task Types
+
+Tasks marked **airborne-only** are disabled until the drone is
+in the air.
+
+| Tab | Action Type | Key Parameters | Airborne-only |
+|-----|-------------|----------------|---------------|
+| Takeoff | `TakeoffTask` | `target_altitude_m`, `velocity_m_s` | |
+| Land | `LandTask` | `velocity_m_s` | ✓ |
+| Navigate | `NavigateTask` | `global_plan` (Path), `goal_tolerance_m` | ✓ |
+| Exploration | `ExplorationTask` | `search_bounds` (Polygon), altitude/speed, `time_limit_sec` | ✓ |
+| Coverage | `CoverageTask` | `coverage_area` (Polygon), `line_spacing_m`, `heading_deg` | ✓ |
+| Object Search | `ObjectSearchTask` | `object_class`, `search_area`, `target_count` | ✓ |
+| Object Counting | `ObjectCountingTask` | `object_class`, `count_area` | ✓ |
+| Semantic Search | `SemanticSearchTask` | `query`, `search_area`, `confidence_threshold` | ✓ |
+| Fixed Trajectory | `FixedTrajectoryTask` | `trajectory_spec`, `loop` | ✓ |
+
+## Widget Type Mapping
+
+| ROS Type | Widget | Notes |
+|----------|--------|-------|
+| `float32`/`float64` | `QDoubleSpinBox` | Range from registry |
+| `int32` | `QSpinBox` | Range from registry |
+| `string` | `QLineEdit` | Free-text input |
+| `bool` | `QCheckBox` | Toggle |
+| `geometry_msgs/Polygon` | `QPushButton` | Calls `get_selection` service |
+| `nav_msgs/Path` | Waypoint controls | Inline height/pose/buttons via WaypointManager |
+| `airstack_msgs/FixedTrajectory` | `QComboBox` + `QTableWidget` | Type + attributes |
+
+## Dependencies
+
+- `rviz_common` -- RViz2 panel base class
+- `pluginlib` -- plugin loading
+- `rclcpp` / `rclcpp_action` -- ROS 2 node and action client
+- `task_msgs` -- action definitions for all 9 task types
+- `airstack_msgs` -- `FixedTrajectory` message
+- `geometry_msgs` / `nav_msgs` / `std_msgs` -- standard message types
+- `diagnostic_msgs` / `action_msgs` -- status introspection
+- `rviz_polygon_selection_tool` -- polygon selection service
+- `waypoint_rviz2_plugin` -- shared `WaypointManager` singleton
+- Qt5 (Core, Widgets, Gui)
+
+## Build
+
+```bash
+colcon build --packages-select waypoint_rviz2_plugin rviz_tasks_panel
+```
+
+## Usage
+
+1. Launch RViz2
+2. Go to **Panels > Add New Panel**
+3. Select **rviz_tasks_panel / TasksPanel**
+4. The panel auto-discovers running task action servers and
+   populates the Robot dropdown
+5. Select a tab, configure goal parameters, and click **Execute**
+6. Monitor feedback in the right pane; click **Cancel** to abort
+
+### Waypoint Navigation (Navigate tab)
+
+1. Activate the **Waypoint Tool** (key **1**) in the RViz toolbar
+2. Left-click in the 3D viewport to place waypoints
+3. In the Navigate tab, adjust **Default Height**, or edit the
+   selected waypoint's **X/Y/Z/Yaw** spinboxes
+4. Use **Save** / **Load** to persist waypoints as `.db3` files
+5. Use **Clear All** to remove all waypoints
+6. Set **goal_tolerance_m** and click **Execute** to navigate
+
+### Polygon Selection
+
+For tasks requiring a polygon boundary:
+
+1. Activate the **Polygon Selection Tool** in the toolbar
+2. Draw a polygon in the 3D viewport
+3. Click **Get Polygon from RViz** in the relevant task tab
+
+### Fixed Trajectory
+
+1. Select a trajectory type from the dropdown
+2. Edit default attributes in the key-value table
+3. Click **Execute**
+
+## Executor Discovery
+
+The panel scans `get_topic_names_and_types()` for topics matching
+`*/<action_topic_suffix>/_action/status`. For each match, it
+extracts the robot namespace and populates:
+
+- The top-level **Robot** dropdown
+- Each tab's **Executor** dropdown
+
+Discovery runs every 5 seconds and can be triggered manually with
+the **Refresh** button.
+
+## Architecture
+
+- **Compile-time task registry**: `getTaskDefs()` returns a
+  static vector of `TaskTypeDef` structs defining all task types
+- **Shared waypoint state**: The Navigate tab holds a
+  `shared_ptr<WaypointManager>` (singleton from
+  `waypoint_rviz2_plugin`), the same instance used by
+  `WaypointTool` for 3D mouse interaction
+- **Type-erased action clients**: `std::any` stores templated
+  `rclcpp_action::Client` instances per tab
+- **Thread safety**: ROS 2 callbacks use
+  `QMetaObject::invokeMethod` with `Qt::QueuedConnection` to
+  marshal UI updates to the Qt main thread
+- **Airborne gate**: subscribes to `{robot}/is_airborne`
+  (std_msgs/Bool from `takeoff_landing_planner`); per-tab Execute
+  buttons are enabled only when `task_idle && airborne_ok`, where
+  `airborne_ok = !requires_airborne || is_airborne_`
+
+## License
+
+MIT -- Carnegie Mellon University
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/include/rviz_tasks_panel/tasks_panel.hpp b/common/ros_packages/gui/rviz/rviz_tasks_panel/include/rviz_tasks_panel/tasks_panel.hpp
new file mode 100644
index 000000000..0c4f93809
--- /dev/null
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/include/rviz_tasks_panel/tasks_panel.hpp
@@ -0,0 +1,189 @@
+// Copyright (c) 2024 Carnegie Mellon University
+// MIT License
+
+#pragma once
+
+#include <any>
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <QComboBox>
+#include <QCheckBox>
+#include <QDoubleSpinBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
+#include <QPushButton>
+#include <QSpinBox>
+#include <QSplitter>
+#include <QTabWidget>
+#include <QTableWidget>
+#include <QTextEdit>
+#include <QTimer>
+#include <QVBoxLayout>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rviz_common/panel.hpp>
+
+#include <geometry_msgs/msg/polygon.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <airstack_msgs/msg/fixed_trajectory.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+#include <rviz_polygon_selection_tool/srv/get_selection.hpp>
+
+#include <task_msgs/action/takeoff_task.hpp>
+#include <task_msgs/action/land_task.hpp>
+#include <task_msgs/action/navigate_task.hpp>
+#include <task_msgs/action/exploration_task.hpp>
+#include <task_msgs/action/coverage_task.hpp>
+#include <task_msgs/action/object_search_task.hpp>
+#include <task_msgs/action/object_counting_task.hpp>
+#include <task_msgs/action/semantic_search_task.hpp>
+#include <task_msgs/action/fixed_trajectory_task.hpp>
+
+#include <waypoint_rviz2_plugin/waypoint_manager.hpp>
+
+namespace rviz_tasks_panel
+{
+
+// Goal field definition for auto-generating widgets
+struct GoalFieldDef
+{
+  std::string name;
+  std::string ros_type;  // "float32", "int32", "string", "bool",
+                         // "geometry_msgs/Polygon", "nav_msgs/Path",
+                         // "airstack_msgs/FixedTrajectory"
+  double default_value{0.0};
+  double min_value{0.0};
+  double max_value{10000.0};
+};
+
+// Task type definition
+struct TaskTypeDef
+{
+  std::string display_name;
+  std::string action_topic_suffix;  // e.g. "tasks/takeoff"
+  std::vector<GoalFieldDef> goal_fields;
+  bool requires_airborne{false};  // if true, Execute is disabled while drone is on the ground
+};
+
+// Per-tab runtime state
+struct TaskTabState
+{
+  QComboBox * executor_combo{nullptr};
+  std::map<std::string, QWidget *> field_widgets;
+  std::map<std::string, std::any> field_data;  // complex types (Polygon, Path)
+  QPushButton * cancel_btn{nullptr};
+  QPushButton * execute_btn{nullptr};
+  QTextEdit * feedback_display{nullptr};
+  QTextEdit * result_display{nullptr};
+  QLabel * status_label{nullptr};
+  std::any action_client;
+  std::any goal_handle;
+  bool goal_active{false};
+};
+
+class TasksPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit TasksPanel(QWidget * parent = nullptr);
+  ~TasksPanel() override = default;
+  void onInitialize() override;
+  void save(rviz_common::Config config) const override;
+  void load(const rviz_common::Config & config) override;
+
+private Q_SLOTS:
+  void onRefreshExecutors();
+  void onExecuteClicked();
+  void onCancelClicked();
+  void onGetPolygon(int tab_index, const std::string & field_name);
+  void onTrajectoryTypeChanged(int tab_index, const QString & type);
+
+  // Waypoint UI slots
+  void onWaypointCountChanged(int count);
+  void onSelectedMarkerChanged(
+    const QString & name, double x, double y, double z, double yaw);
+  void onClearWaypoints();
+  void onSaveWaypoints();
+  void onLoadWaypoints();
+  void onWaypointPoseChanged(double value);
+
+private:
+  rclcpp::Node::SharedPtr raw_node_;
+
+  // Top bar
+  QComboBox * robot_combo_{nullptr};
+
+  // Tabs
+  QTabWidget * tab_widget_{nullptr};
+  std::vector<TaskTypeDef> task_defs_;
+  std::vector<TaskTabState> tab_states_;
+
+  // Polygon selection service
+  rclcpp::Client<rviz_polygon_selection_tool::srv::GetSelection>::SharedPtr polygon_client_;
+
+  // Shared waypoint manager (singleton, also held by WaypointTool)
+  std::shared_ptr<waypoint_rviz2_plugin::WaypointManager> waypoint_manager_;
+
+  // Waypoint UI widgets (Navigate tab)
+  QLabel * wp_count_label_{nullptr};
+  QLabel * wp_selected_label_{nullptr};
+  QDoubleSpinBox * wp_height_spin_{nullptr};
+  QDoubleSpinBox * wp_x_spin_{nullptr};
+  QDoubleSpinBox * wp_y_spin_{nullptr};
+  QDoubleSpinBox * wp_z_spin_{nullptr};
+  QDoubleSpinBox * wp_yaw_spin_{nullptr};
+  bool wp_updating_spinboxes_{false};  // guard against signal loops
+
+  // Global task lock: only one task may run at a time
+  int active_task_tab_{-1};  // -1 = no task running, else = tab index of active task
+
+  // Airborne state (fed by is_airborne topic from takeoff_landing_planner)
+  bool is_airborne_{false};
+  QString is_airborne_robot_;  // robot namespace currently subscribed to
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr is_airborne_sub_;
+
+  // Discovery timer
+  QTimer * refresh_timer_{nullptr};
+
+  static std::vector<TaskTypeDef> getTaskDefs();
+  void buildTabs();
+  QWidget * buildGoalFieldWidget(const GoalFieldDef & def, int tab_index);
+  void updateAvailableExecutors();
+
+  // Widget value extraction helpers
+  double getFloat(int tab_index, const std::string & field_name);
+  int32_t getInt(int tab_index, const std::string & field_name);
+  std::string getString(int tab_index, const std::string & field_name);
+  bool getBool(int tab_index, const std::string & field_name);
+  geometry_msgs::msg::Polygon getPolygon(int tab_index, const std::string & field_name);
+  nav_msgs::msg::Path getPath(int tab_index, const std::string & field_name);
+  airstack_msgs::msg::FixedTrajectory getFixedTrajectory(int tab_index, const std::string & field_name);
+
+  // Template for sending goals with type-specific callbacks
+  template<typename ActionT>
+  void doSendGoal(
+    int tab_index,
+    const typename ActionT::Goal & goal,
+    std::function<QString(const typename ActionT::Feedback &)> fmt_feedback,
+    std::function<QString(const typename ActionT::Result::SharedPtr)> fmt_result);
+
+  template<typename ActionT>
+  void doCancelGoal(int tab_index);
+
+  void setGoalActive(int tab_index, bool active);
+  void updateExecuteButtons();
+  void renewAirborneSubscription();
+  void setTabStatus(int tab_index, const QString & icon, const QColor & text_color);
+  void clearTabStatus(int tab_index);
+  QString currentRobot() const;
+};
+
+}  // namespace rviz_tasks_panel
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/media/rviz_tasks_panel.png b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/rviz_tasks_panel.png
new file mode 100644
index 000000000..e87af772d
Binary files /dev/null and b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/rviz_tasks_panel.png differ
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_navigate.png b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_navigate.png
new file mode 100644
index 000000000..46b03cbd0
Binary files /dev/null and b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_navigate.png differ
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_takeoff.png b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_takeoff.png
new file mode 100644
index 000000000..01639d0bb
Binary files /dev/null and b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_takeoff.png differ
diff --git a/robot/ros_ws/src/behavior/behavior_executive/package.xml b/common/ros_packages/gui/rviz/rviz_tasks_panel/package.xml
similarity index 50%
rename from robot/ros_ws/src/behavior/behavior_executive/package.xml
rename to common/ros_packages/gui/rviz/rviz_tasks_panel/package.xml
index d1ae9fce8..8c0c91c21 100644
--- a/robot/ros_ws/src/behavior/behavior_executive/package.xml
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/package.xml
@@ -1,27 +1,28 @@
 <?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>behavior_executive</name>
+  <name>rviz_tasks_panel</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="uav@todo.todo">uav</maintainer>
-  <license>TODO: License declaration</license>
+  <description>RViz2 Panel for dispatching and monitoring task action goals</description>
+  <maintainer email="ajong@andrew.cmu.edu">andrew</maintainer>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>pluginlib</depend>
+  <depend>rviz_common</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
-  <depend>std_msgs</depend>
-  <depend>nav_msgs</depend>
   <depend>task_msgs</depend>
-  <depend>behavior_tree</depend>
-  <depend>behavior_tree_msgs</depend>
   <depend>airstack_msgs</depend>
-  <depend>airstack_common</depend>
-  <depend>mavros_msgs</depend>
-  
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>rviz_polygon_selection_tool</depend>
+  <depend>waypoint_rviz2_plugin</depend>
+  <depend>qtbase5-dev</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/plugin_description.xml b/common/ros_packages/gui/rviz/rviz_tasks_panel/plugin_description.xml
new file mode 100644
index 000000000..8298c7dc1
--- /dev/null
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/plugin_description.xml
@@ -0,0 +1,5 @@
+<library path="tasks_panel">
+  <class type="rviz_tasks_panel::TasksPanel" base_class_type="rviz_common::Panel">
+    <description>Panel for dispatching and monitoring ROS 2 task action goals</description>
+  </class>
+</library>
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
new file mode 100644
index 000000000..aed06bc47
--- /dev/null
+++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp
@@ -0,0 +1,1163 @@
+// Copyright (c) 2024 Carnegie Mellon University
+// MIT License
+
+#include "rviz_tasks_panel/tasks_panel.hpp"
+
+#include <QFileDialog>
+#include <QHeaderView>
+#include <QMetaObject>
+#include <QTabBar>
+#include <QTime>
+#include <pluginlib/class_list_macros.hpp>
+#include <rviz_common/display_context.hpp>
+#include <set>
+
+namespace rviz_tasks_panel
+{
+
+// ─────────────────────────── static task registry ─────────────────────────────
+
+std::vector<TaskTypeDef> TasksPanel::getTaskDefs()
+{
+  // default, min, max
+  return {
+    {"Takeoff", "tasks/takeoff", {
+      {"target_altitude_m", "float32", 10.0, 0.0, 500.0},
+      {"velocity_m_s", "float32", 1.0, 0.0, 50.0},
+    }},
+    {"Land", "tasks/land", {
+      {"velocity_m_s", "float32", 0.3, 0.0, 10.0},
+    }, true},
+    {"Navigate", "tasks/navigate", {
+      {"global_plan", "nav_msgs/Path", 0, 0, 0},
+      {"goal_tolerance_m", "float32", 1.0, 0.0, 100.0},
+    }, true},
+    {"Exploration", "tasks/exploration", {
+      {"search_bounds", "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},
+      {"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},
+    }, true},
+    {"Coverage", "tasks/coverage", {
+      {"coverage_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},
+      {"min_flight_speed", "float32", 1.0, 0.0, 50.0},
+      {"max_flight_speed", "float32", 3.0, 0.0, 50.0},
+      {"line_spacing_m", "float32", 5.0, 0.1, 1000.0},
+      {"heading_deg", "float32", 0.0, 0.0, 360.0},
+    }, true},
+    {"Object Search", "tasks/object_search", {
+      {"object_class", "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},
+      {"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},
+      {"target_count", "int32", 1, 0, 10000},
+    }, true},
+    {"Object Counting", "tasks/object_counting", {
+      {"object_class", "string", 0, 0, 0},
+      {"count_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},
+      {"min_flight_speed", "float32", 1.0, 0.0, 50.0},
+      {"max_flight_speed", "float32", 3.0, 0.0, 50.0},
+    }, true},
+    {"Semantic Search", "tasks/semantic_search", {
+      {"query", "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},
+      {"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},
+    }, true},
+    {"Fixed Trajectory", "tasks/fixed_trajectory", {
+      {"trajectory_spec", "airstack_msgs/FixedTrajectory", 0, 0, 0},
+      {"loop", "bool", 0, 0, 0},
+    }, true},
+  };
+}
+
+// ─────────────────────────── constructor / init ───────────────────────────────
+
+TasksPanel::TasksPanel(QWidget * parent)
+: rviz_common::Panel(parent)
+{
+  task_defs_ = getTaskDefs();
+}
+
+void TasksPanel::onInitialize()
+{
+  auto node_abs = getDisplayContext()->getRosNodeAbstraction().lock();
+  raw_node_ = node_abs->get_raw_node();
+
+  // Polygon selection service client
+  polygon_client_ =
+    raw_node_->create_client<rviz_polygon_selection_tool::srv::GetSelection>("get_selection");
+
+  // Shared waypoint manager (singleton created by WaypointTool)
+  waypoint_manager_ = waypoint_rviz2_plugin::WaypointManager::instance();
+  if (!waypoint_manager_->isInitialized()) {
+    // WaypointTool hasn't initialized yet; try again shortly
+    QTimer::singleShot(1000, [this]() {
+      if (waypoint_manager_ && !waypoint_manager_->isInitialized()) {
+        waypoint_manager_->initialize(getDisplayContext(),
+          getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node());
+      }
+    });
+  }
+
+  // Connect waypoint manager signals
+  connect(waypoint_manager_.get(),
+    &waypoint_rviz2_plugin::WaypointManager::waypointCountChanged,
+    this, &TasksPanel::onWaypointCountChanged, Qt::QueuedConnection);
+  connect(waypoint_manager_.get(),
+    &waypoint_rviz2_plugin::WaypointManager::selectedMarkerChanged,
+    this, &TasksPanel::onSelectedMarkerChanged, Qt::QueuedConnection);
+  connect(waypoint_manager_.get(),
+    &waypoint_rviz2_plugin::WaypointManager::waypointsCleared,
+    this, [this]() {
+      onWaypointCountChanged(0);
+    }, Qt::QueuedConnection);
+
+  // Build UI
+  auto * main_layout = new QVBoxLayout();
+
+  // Robot selector bar
+  auto * top_bar = new QHBoxLayout();
+  top_bar->addWidget(new QLabel("Robot:"));
+  robot_combo_ = new QComboBox();
+  robot_combo_->setEditable(true);
+  robot_combo_->setMinimumWidth(120);
+  top_bar->addWidget(robot_combo_);
+  auto * refresh_btn = new QPushButton("Refresh");
+  connect(refresh_btn, &QPushButton::clicked, this, &TasksPanel::onRefreshExecutors);
+  top_bar->addWidget(refresh_btn);
+  top_bar->addStretch();
+  main_layout->addLayout(top_bar);
+
+  // Tab widget
+  tab_widget_ = new QTabWidget();
+  tab_widget_->setFont(QFont("Noto Sans", 10));
+  main_layout->addWidget(tab_widget_);
+
+  setLayout(main_layout);
+
+  buildTabs();
+
+  // Discovery timer
+  refresh_timer_ = new QTimer(this);
+  connect(refresh_timer_, &QTimer::timeout, this, &TasksPanel::onRefreshExecutors);
+  refresh_timer_->start(5000);
+
+  // Initial discovery
+  QTimer::singleShot(500, this, &TasksPanel::onRefreshExecutors);
+}
+
+// ─────────────────────────── build tabs ───────────────────────────────────────
+
+void TasksPanel::buildTabs()
+{
+  tab_states_.resize(task_defs_.size());
+
+  for (size_t i = 0; i < task_defs_.size(); ++i) {
+    const auto & def = task_defs_[i];
+    auto & state = tab_states_[i];
+
+    auto * tab = new QWidget();
+    auto * splitter = new QSplitter(Qt::Horizontal);
+
+    // ── LEFT: Goal Parameters ──
+    auto * left_widget = new QWidget();
+    auto * left_layout = new QVBoxLayout(left_widget);
+
+    // Executor selector
+    auto * exec_layout = new QHBoxLayout();
+    exec_layout->addWidget(new QLabel("Executor:"));
+    state.executor_combo = new QComboBox();
+    state.executor_combo->setEditable(true);
+    state.executor_combo->setMinimumWidth(200);
+    exec_layout->addWidget(state.executor_combo);
+    left_layout->addLayout(exec_layout);
+
+    // Goal fields
+    auto * form = new QFormLayout();
+    for (const auto & field : def.goal_fields) {
+      auto * widget = buildGoalFieldWidget(field, static_cast<int>(i));
+      state.field_widgets[field.name] = widget;
+      form->addRow(QString::fromStdString(field.name) + ":", widget);
+    }
+    left_layout->addLayout(form);
+
+    left_layout->addStretch();
+
+    // Cancel / Execute buttons
+    auto * btn_layout = new QHBoxLayout();
+    state.cancel_btn = new QPushButton("Cancel");
+    state.cancel_btn->setEnabled(false);
+    state.execute_btn = new QPushButton("Execute");
+    btn_layout->addWidget(state.cancel_btn);
+    btn_layout->addStretch();
+    btn_layout->addWidget(state.execute_btn);
+    left_layout->addLayout(btn_layout);
+
+    int tab_idx = static_cast<int>(i);
+    connect(state.execute_btn, &QPushButton::clicked, [this, tab_idx]() {
+      // store which tab triggered it, then call handler
+      tab_widget_->setCurrentIndex(tab_idx);
+      onExecuteClicked();
+    });
+    connect(state.cancel_btn, &QPushButton::clicked, [this, tab_idx]() {
+      tab_widget_->setCurrentIndex(tab_idx);
+      onCancelClicked();
+    });
+
+    // ── RIGHT: Feedback & Result ──
+    auto * right_widget = new QWidget();
+    auto * right_layout = new QVBoxLayout(right_widget);
+
+    right_layout->addWidget(new QLabel("Feedback:"));
+    state.feedback_display = new QTextEdit();
+    state.feedback_display->setReadOnly(true);
+    state.feedback_display->setMaximumHeight(200);
+    right_layout->addWidget(state.feedback_display);
+
+    right_layout->addWidget(new QLabel("Result:"));
+    state.result_display = new QTextEdit();
+    state.result_display->setReadOnly(true);
+    state.result_display->setMaximumHeight(100);
+    right_layout->addWidget(state.result_display);
+
+    state.status_label = new QLabel("Idle");
+    right_layout->addWidget(state.status_label);
+    right_layout->addStretch();
+
+    splitter->addWidget(left_widget);
+    splitter->addWidget(right_widget);
+    splitter->setStretchFactor(0, 1);
+    splitter->setStretchFactor(1, 1);
+
+    auto * tab_layout = new QVBoxLayout(tab);
+    tab_layout->addWidget(splitter);
+
+    tab_widget_->addTab(tab, QString::fromStdString(def.display_name));
+  }
+}
+
+QWidget * TasksPanel::buildGoalFieldWidget(const GoalFieldDef & def, int tab_index)
+{
+  if (def.ros_type == "float32" || def.ros_type == "float64") {
+    auto * spin = new QDoubleSpinBox();
+    spin->setRange(def.min_value, def.max_value);
+    spin->setValue(def.default_value);
+    spin->setDecimals(2);
+    spin->setSingleStep(0.1);
+    return spin;
+  }
+  if (def.ros_type == "int32") {
+    auto * spin = new QSpinBox();
+    spin->setRange(static_cast<int>(def.min_value), static_cast<int>(def.max_value));
+    spin->setValue(static_cast<int>(def.default_value));
+    return spin;
+  }
+  if (def.ros_type == "string") {
+    return new QLineEdit();
+  }
+  if (def.ros_type == "bool") {
+    auto * cb = new QCheckBox();
+    cb->setChecked(def.default_value != 0.0);
+    return cb;
+  }
+  if (def.ros_type == "geometry_msgs/Polygon") {
+    auto * container = new QWidget();
+    auto * layout = new QVBoxLayout(container);
+    layout->setContentsMargins(0, 0, 0, 0);
+    auto * btn = new QPushButton("Get Polygon from RViz");
+    auto * label = new QLabel("No polygon selected");
+    layout->addWidget(btn);
+    layout->addWidget(label);
+    std::string field_name = def.name;
+    connect(btn, &QPushButton::clicked, [this, tab_index, field_name]() {
+      onGetPolygon(tab_index, field_name);
+    });
+    return container;
+  }
+  if (def.ros_type == "nav_msgs/Path") {
+    auto * container = new QWidget();
+    auto * layout = new QVBoxLayout(container);
+    layout->setContentsMargins(0, 0, 0, 0);
+
+    // Instruction label
+    layout->addWidget(new QLabel("Activate Waypoint Tool (key: 1) to place waypoints"));
+
+    // Height / count / selected row
+    auto * info_row = new QHBoxLayout();
+    info_row->addWidget(new QLabel("Height:"));
+    wp_height_spin_ = new QDoubleSpinBox();
+    wp_height_spin_->setRange(-100.0, 500.0);
+    wp_height_spin_->setSingleStep(0.1);
+    wp_height_spin_->setValue(waypoint_manager_ ? waypoint_manager_->defaultHeight() : 0.0);
+    wp_height_spin_->setMaximumWidth(80);
+    info_row->addWidget(wp_height_spin_);
+
+    wp_count_label_ = new QLabel("Waypoints: 0");
+    info_row->addWidget(wp_count_label_);
+
+    wp_selected_label_ = new QLabel("Selected: none");
+    info_row->addWidget(wp_selected_label_);
+    info_row->addStretch();
+    layout->addLayout(info_row);
+
+    // X / Y / Z / Yaw spinboxes for editing selected waypoint
+    auto * pose_row = new QHBoxLayout();
+
+    pose_row->addWidget(new QLabel("X:"));
+    wp_x_spin_ = new QDoubleSpinBox();
+    wp_x_spin_->setRange(-1000.0, 1000.0);
+    wp_x_spin_->setSingleStep(0.5);
+    pose_row->addWidget(wp_x_spin_);
+
+    pose_row->addWidget(new QLabel("Y:"));
+    wp_y_spin_ = new QDoubleSpinBox();
+    wp_y_spin_->setRange(-1000.0, 1000.0);
+    wp_y_spin_->setSingleStep(0.5);
+    pose_row->addWidget(wp_y_spin_);
+
+    pose_row->addWidget(new QLabel("Z:"));
+    wp_z_spin_ = new QDoubleSpinBox();
+    wp_z_spin_->setRange(-1000.0, 1000.0);
+    wp_z_spin_->setSingleStep(0.5);
+    pose_row->addWidget(wp_z_spin_);
+
+    pose_row->addWidget(new QLabel("Yaw:"));
+    wp_yaw_spin_ = new QDoubleSpinBox();
+    wp_yaw_spin_->setRange(-3.15, 3.15);
+    wp_yaw_spin_->setSingleStep(0.1);
+    pose_row->addWidget(wp_yaw_spin_);
+    layout->addLayout(pose_row);
+
+    // Action buttons
+    auto * btn_row = new QHBoxLayout();
+    auto * clear_btn = new QPushButton("Clear All");
+    auto * save_btn = new QPushButton("Save");
+    auto * load_btn = new QPushButton("Load");
+    btn_row->addWidget(clear_btn);
+    btn_row->addWidget(save_btn);
+    btn_row->addWidget(load_btn);
+    btn_row->addStretch();
+    layout->addLayout(btn_row);
+
+    // Connect height spinbox to manager
+    connect(wp_height_spin_, QOverload<double>::of(&QDoubleSpinBox::valueChanged),
+      [this](double val) {
+        if (waypoint_manager_) { waypoint_manager_->setDefaultHeight(val); }
+      });
+
+    // Connect pose spinboxes to update selected waypoint
+    auto pose_changed = [this](double) { onWaypointPoseChanged(0.0); };
+    connect(wp_x_spin_, QOverload<double>::of(&QDoubleSpinBox::valueChanged), pose_changed);
+    connect(wp_y_spin_, QOverload<double>::of(&QDoubleSpinBox::valueChanged), pose_changed);
+    connect(wp_z_spin_, QOverload<double>::of(&QDoubleSpinBox::valueChanged), pose_changed);
+    connect(wp_yaw_spin_, QOverload<double>::of(&QDoubleSpinBox::valueChanged), pose_changed);
+
+    // Connect buttons
+    connect(clear_btn, &QPushButton::clicked, this, &TasksPanel::onClearWaypoints);
+    connect(save_btn, &QPushButton::clicked, this, &TasksPanel::onSaveWaypoints);
+    connect(load_btn, &QPushButton::clicked, this, &TasksPanel::onLoadWaypoints);
+
+    return container;
+  }
+  if (def.ros_type == "airstack_msgs/FixedTrajectory") {
+    auto * container = new QWidget();
+    auto * layout = new QVBoxLayout(container);
+    layout->setContentsMargins(0, 0, 0, 0);
+
+    auto * type_combo = new QComboBox();
+    type_combo->addItems({"Circle", "Figure8", "Racetrack", "Line", "Point", "Lawnmower"});
+    layout->addWidget(type_combo);
+
+    auto * table = new QTableWidget(0, 2);
+    table->setHorizontalHeaderLabels({"Key", "Value"});
+    table->horizontalHeader()->setStretchLastSection(true);
+    table->setMaximumHeight(150);
+    layout->addWidget(table);
+
+    auto * add_btn = new QPushButton("+ Add Attribute");
+    layout->addWidget(add_btn);
+
+    connect(add_btn, &QPushButton::clicked, [table]() {
+      int row = table->rowCount();
+      table->insertRow(row);
+      table->setItem(row, 0, new QTableWidgetItem(""));
+      table->setItem(row, 1, new QTableWidgetItem(""));
+    });
+
+    // Store combo and table for later retrieval
+    tab_states_[tab_index].field_data[def.name + "_type_combo"] = type_combo;
+    tab_states_[tab_index].field_data[def.name + "_table"] = table;
+
+    connect(type_combo, &QComboBox::currentTextChanged,
+      [this, tab_index, field_name = def.name](const QString & type) {
+        onTrajectoryTypeChanged(tab_index, type);
+      });
+
+    // Trigger initial population
+    QTimer::singleShot(0, [type_combo]() {
+      Q_EMIT type_combo->currentTextChanged(type_combo->currentText());
+    });
+
+    return container;
+  }
+
+  return new QLabel("(unsupported type: " + QString::fromStdString(def.ros_type) + ")");
+}
+
+// ─────────────────────────── executor discovery ──────────────────────────────
+
+void TasksPanel::onRefreshExecutors()
+{
+  if (!raw_node_) {return;}
+
+  auto topic_map = raw_node_->get_topic_names_and_types();
+
+  // Collect robot namespaces and per-task executors
+  std::set<std::string> robots;
+
+  for (size_t i = 0; i < task_defs_.size(); ++i) {
+    const std::string suffix = task_defs_[i].action_topic_suffix + "/_action/status";
+    QStringList executors;
+
+    for (const auto & [topic, types] : topic_map) {
+      if (topic.size() > suffix.size() &&
+        topic.substr(topic.size() - suffix.size()) == suffix)
+      {
+        // Extract action name (remove /_action/status)
+        std::string action_name = topic.substr(0, topic.size() - std::string("/_action/status").size());
+        executors.append(QString::fromStdString(action_name));
+
+        // Extract robot namespace (everything before /tasks/...)
+        std::string task_suffix = task_defs_[i].action_topic_suffix;
+        size_t pos = action_name.rfind(task_suffix);
+        if (pos != std::string::npos && pos > 1) {
+          robots.insert(action_name.substr(0, pos - 1));  // -1 to remove trailing /
+        }
+      }
+    }
+
+    // Update executor combo preserving current selection
+    auto & combo = tab_states_[i].executor_combo;
+    QString current = combo->currentText();
+    combo->clear();
+    combo->addItems(executors);
+    if (!current.isEmpty()) {
+      int idx = combo->findText(current);
+      if (idx >= 0) {
+        combo->setCurrentIndex(idx);
+      } else {
+        combo->setCurrentText(current);
+      }
+    }
+  }
+
+  // Update robot combo
+  QString current_robot = robot_combo_->currentText();
+  robot_combo_->clear();
+  for (const auto & r : robots) {
+    robot_combo_->addItem(QString::fromStdString(r));
+  }
+  if (!current_robot.isEmpty()) {
+    int idx = robot_combo_->findText(current_robot);
+    if (idx >= 0) {
+      robot_combo_->setCurrentIndex(idx);
+    } else {
+      robot_combo_->setCurrentText(current_robot);
+    }
+  }
+
+  renewAirborneSubscription();
+}
+
+// ─────────────────────────── special widgets ──────────────────────────────────
+
+void TasksPanel::onGetPolygon(int tab_index, const std::string & field_name)
+{
+  if (!polygon_client_->wait_for_service(std::chrono::seconds(0))) {
+    tab_states_[tab_index].status_label->setText("Polygon tool not active - press 'p' first");
+    return;
+  }
+
+  auto request = std::make_shared<rviz_polygon_selection_tool::srv::GetSelection::Request>();
+  polygon_client_->async_send_request(request,
+    [this, tab_index, field_name](
+      rclcpp::Client<rviz_polygon_selection_tool::srv::GetSelection>::SharedFuture future)
+    {
+      auto response = future.get();
+      if (response->selection.empty()) {
+        QMetaObject::invokeMethod(this, [this, tab_index]() {
+          tab_states_[tab_index].status_label->setText("No polygons drawn");
+        }, Qt::QueuedConnection);
+        return;
+      }
+
+      // Take first polygon
+      geometry_msgs::msg::Polygon poly;
+      for (const auto & pt : response->selection[0].polygon.points) {
+        geometry_msgs::msg::Point32 p;
+        p.x = pt.x;
+        p.y = pt.y;
+        p.z = pt.z;
+        poly.points.push_back(p);
+      }
+
+      QMetaObject::invokeMethod(this, [this, tab_index, field_name, poly]() {
+        tab_states_[tab_index].field_data[field_name] = poly;
+        // Update label in the polygon widget
+        auto * container = tab_states_[tab_index].field_widgets[field_name];
+        auto * label = container->findChild<QLabel *>();
+        if (label) {
+          label->setText(QString("Polygon: %1 points").arg(poly.points.size()));
+        }
+      }, Qt::QueuedConnection);
+    });
+}
+
+void TasksPanel::onTrajectoryTypeChanged(int tab_index, const QString & type)
+{
+  std::string key = "trajectory_spec_table";
+  auto it = tab_states_[tab_index].field_data.find(key);
+  if (it == tab_states_[tab_index].field_data.end()) {return;}
+
+  auto * table = std::any_cast<QTableWidget *>(it->second);
+  table->setRowCount(0);
+
+  // Default attributes per trajectory type
+  std::vector<std::pair<std::string, std::string>> attrs;
+  if (type == "Circle") {
+    attrs = {{"frame_id", "base_link"}, {"radius", "5.0"}, {"velocity", "2.0"}};
+  } else if (type == "Figure8") {
+    attrs = {{"frame_id", "base_link"}, {"length", "10.0"}, {"width", "5.0"},
+             {"height", "0.0"}, {"velocity", "2.0"}, {"max_acceleration", "1.0"}};
+  } else if (type == "Racetrack") {
+    attrs = {{"frame_id", "base_link"}, {"length", "20.0"}, {"width", "10.0"},
+             {"height", "0.0"}, {"velocity", "2.0"}, {"turn_velocity", "1.0"},
+             {"max_acceleration", "1.0"}};
+  } else if (type == "Line") {
+    attrs = {{"frame_id", "base_link"}, {"length", "10.0"}, {"height", "0.0"},
+             {"velocity", "2.0"}, {"max_acceleration", "1.0"}};
+  } else if (type == "Point") {
+    attrs = {{"frame_id", "base_link"}, {"x", "5.0"}, {"y", "0.0"},
+             {"height", "0.0"}, {"velocity", "2.0"}, {"max_acceleration", "1.0"}};
+  } else if (type == "Lawnmower") {
+    attrs = {{"frame_id", "base_link"}, {"length", "20.0"}, {"width", "5.0"},
+             {"height", "10.0"}, {"velocity", "2.0"}, {"vertical", "0"}};
+  }
+
+  for (const auto & [k, v] : attrs) {
+    int row = table->rowCount();
+    table->insertRow(row);
+    table->setItem(row, 0, new QTableWidgetItem(QString::fromStdString(k)));
+    table->setItem(row, 1, new QTableWidgetItem(QString::fromStdString(v)));
+  }
+}
+
+// ─────────────────────────── value extraction ─────────────────────────────────
+
+double TasksPanel::getFloat(int tab_index, const std::string & field_name)
+{
+  auto * w = tab_states_[tab_index].field_widgets[field_name];
+  if (auto * spin = qobject_cast<QDoubleSpinBox *>(w)) {return spin->value();}
+  return 0.0;
+}
+
+int32_t TasksPanel::getInt(int tab_index, const std::string & field_name)
+{
+  auto * w = tab_states_[tab_index].field_widgets[field_name];
+  if (auto * spin = qobject_cast<QSpinBox *>(w)) {return spin->value();}
+  return 0;
+}
+
+std::string TasksPanel::getString(int tab_index, const std::string & field_name)
+{
+  auto * w = tab_states_[tab_index].field_widgets[field_name];
+  if (auto * edit = qobject_cast<QLineEdit *>(w)) {return edit->text().toStdString();}
+  return "";
+}
+
+bool TasksPanel::getBool(int tab_index, const std::string & field_name)
+{
+  auto * w = tab_states_[tab_index].field_widgets[field_name];
+  if (auto * cb = qobject_cast<QCheckBox *>(w)) {return cb->isChecked();}
+  return false;
+}
+
+geometry_msgs::msg::Polygon TasksPanel::getPolygon(int tab_index, const std::string & field_name)
+{
+  auto it = tab_states_[tab_index].field_data.find(field_name);
+  if (it != tab_states_[tab_index].field_data.end()) {
+    return std::any_cast<geometry_msgs::msg::Polygon>(it->second);
+  }
+  return geometry_msgs::msg::Polygon();
+}
+
+nav_msgs::msg::Path TasksPanel::getPath(int, const std::string &)
+{
+  if (waypoint_manager_ && waypoint_manager_->isInitialized()) {
+    return waypoint_manager_->getPath();
+  }
+  return nav_msgs::msg::Path();
+}
+
+airstack_msgs::msg::FixedTrajectory TasksPanel::getFixedTrajectory(
+  int tab_index, const std::string & field_name)
+{
+  airstack_msgs::msg::FixedTrajectory ft;
+
+  auto combo_it = tab_states_[tab_index].field_data.find(field_name + "_type_combo");
+  if (combo_it != tab_states_[tab_index].field_data.end()) {
+    auto * combo = std::any_cast<QComboBox *>(combo_it->second);
+    ft.type = combo->currentText().toStdString();
+  }
+
+  auto table_it = tab_states_[tab_index].field_data.find(field_name + "_table");
+  if (table_it != tab_states_[tab_index].field_data.end()) {
+    auto * table = std::any_cast<QTableWidget *>(table_it->second);
+    for (int row = 0; row < table->rowCount(); ++row) {
+      diagnostic_msgs::msg::KeyValue kv;
+      kv.key = table->item(row, 0)->text().toStdString();
+      kv.value = table->item(row, 1)->text().toStdString();
+      ft.attributes.push_back(kv);
+    }
+  }
+
+  return ft;
+}
+
+// ─────────────────────────── UI state helpers ─────────────────────────────────
+
+void TasksPanel::setGoalActive(int tab_index, bool active)
+{
+  auto & state = tab_states_[tab_index];
+  state.goal_active = active;
+  state.cancel_btn->setEnabled(active);
+
+  if (active) {
+    active_task_tab_ = tab_index;
+    state.feedback_display->clear();
+    state.result_display->clear();
+    state.status_label->setText("Running...");
+    state.status_label->setStyleSheet("color: blue;");
+    setTabStatus(tab_index, QString::fromUtf8(u8"⏳"), Qt::blue);
+  } else {
+    active_task_tab_ = -1;
+  }
+
+  updateExecuteButtons();
+}
+
+void TasksPanel::updateExecuteButtons()
+{
+  for (size_t i = 0; i < tab_states_.size(); ++i) {
+    bool task_idle = (active_task_tab_ == -1);
+    bool airborne_ok = !task_defs_[i].requires_airborne || is_airborne_;
+    tab_states_[i].execute_btn->setEnabled(task_idle && airborne_ok);
+  }
+}
+
+void TasksPanel::renewAirborneSubscription()
+{
+  QString robot = robot_combo_->currentText();
+  if (robot == is_airborne_robot_) {return;}
+  is_airborne_robot_ = robot;
+
+  is_airborne_sub_.reset();
+  is_airborne_ = false;
+  updateExecuteButtons();
+
+  if (robot.isEmpty() || !raw_node_) {return;}
+
+  std::string topic = robot.toStdString() + "/takeoff_landing_planner/is_airborne";
+  is_airborne_sub_ = raw_node_->create_subscription<std_msgs::msg::Bool>(
+    topic, 1,
+    [this](std_msgs::msg::Bool::SharedPtr msg) {
+      bool airborne = msg->data;
+      QMetaObject::invokeMethod(this, [this, airborne]() {
+        is_airborne_ = airborne;
+        updateExecuteButtons();
+      }, Qt::QueuedConnection);
+    });
+}
+
+void TasksPanel::setTabStatus(int tab_index, const QString & icon, const QColor & text_color)
+{
+  tab_widget_->tabBar()->setTabTextColor(tab_index, text_color);
+  const auto & def = task_defs_[tab_index];
+  tab_widget_->setTabText(tab_index, icon + " " + QString::fromStdString(def.display_name));
+}
+
+void TasksPanel::clearTabStatus(int tab_index)
+{
+  tab_widget_->tabBar()->setTabTextColor(tab_index, QColor());
+  const auto & def = task_defs_[tab_index];
+  tab_widget_->setTabText(tab_index, QString::fromStdString(def.display_name));
+}
+
+QString TasksPanel::currentRobot() const
+{
+  return robot_combo_->currentText();
+}
+
+// ─────────────────────────── template: send/cancel ────────────────────────────
+
+template<typename ActionT>
+void TasksPanel::doSendGoal(
+  int tab_index,
+  const typename ActionT::Goal & goal,
+  std::function<QString(const typename ActionT::Feedback &)> fmt_feedback,
+  std::function<QString(const typename ActionT::Result::SharedPtr)> fmt_result)
+{
+  auto & state = tab_states_[tab_index];
+  std::string action_name = state.executor_combo->currentText().toStdString();
+  if (action_name.empty()) {
+    state.status_label->setText("No executor selected");
+    state.status_label->setStyleSheet("color: red;");
+    return;
+  }
+
+  auto client = rclcpp_action::create_client<ActionT>(raw_node_, action_name);
+  state.action_client = client;
+
+  if (!client->wait_for_action_server(std::chrono::seconds(2))) {
+    state.status_label->setText("Action server not available");
+    state.status_label->setStyleSheet("color: red;");
+    return;
+  }
+
+  // Reset all tab colors from previous task results
+  for (size_t i = 0; i < tab_states_.size(); ++i) {
+    clearTabStatus(static_cast<int>(i));
+  }
+  setGoalActive(tab_index, true);
+
+  auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+
+  // Feedback callback
+  send_goal_options.feedback_callback =
+    [this, tab_index, fmt_feedback](
+      typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
+      const std::shared_ptr<const typename ActionT::Feedback> feedback)
+    {
+      QString text = fmt_feedback(*feedback);
+      QMetaObject::invokeMethod(this, [this, tab_index, text]() {
+        auto & state = tab_states_[tab_index];
+        state.feedback_display->append(
+          "[" + QTime::currentTime().toString("HH:mm:ss") + "] " + text);
+        // Auto-scroll to bottom
+        auto cursor = state.feedback_display->textCursor();
+        cursor.movePosition(QTextCursor::End);
+        state.feedback_display->setTextCursor(cursor);
+      }, Qt::QueuedConnection);
+    };
+
+  // Result callback
+  send_goal_options.result_callback =
+    [this, tab_index, fmt_result](
+      const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & wrapped_result)
+    {
+      QString result_text;
+      QString status_text;
+      QString color;
+      QString tab_icon;
+      QColor tab_text_color;
+
+      switch (wrapped_result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+          result_text = fmt_result(wrapped_result.result);
+          status_text = "Succeeded";
+          color = "color: darkGreen;";
+          tab_icon = QString::fromUtf8(u8"✅");
+          tab_text_color = Qt::darkGreen;
+          break;
+        case rclcpp_action::ResultCode::ABORTED:
+          result_text = fmt_result(wrapped_result.result);
+          status_text = "Aborted";
+          color = "color: red;";
+          tab_icon = QString::fromUtf8(u8"❌");
+          tab_text_color = Qt::red;
+          break;
+        case rclcpp_action::ResultCode::CANCELED:
+          result_text = "Goal canceled";
+          status_text = "Canceled";
+          color = "color: orange;";
+          tab_icon = QString::fromUtf8(u8"🚫");
+          tab_text_color = QColor("orange");
+          break;
+        default:
+          result_text = "Unknown result";
+          status_text = "Unknown";
+          color = "color: gray;";
+          tab_icon = "?";
+          tab_text_color = Qt::gray;
+          break;
+      }
+
+      QMetaObject::invokeMethod(this, [this, tab_index, result_text, status_text, color,
+                                        tab_icon, tab_text_color]() {
+        auto & state = tab_states_[tab_index];
+        state.result_display->setText(result_text);
+        state.status_label->setText(status_text);
+        state.status_label->setStyleSheet(color);
+        setTabStatus(tab_index, tab_icon, tab_text_color);
+        setGoalActive(tab_index, false);
+      }, Qt::QueuedConnection);
+    };
+
+  auto goal_future = client->async_send_goal(goal, send_goal_options);
+}
+
+template<typename ActionT>
+void TasksPanel::doCancelGoal(int tab_index)
+{
+  auto & state = tab_states_[tab_index];
+  try {
+    auto client = std::any_cast<typename rclcpp_action::Client<ActionT>::SharedPtr>(
+      state.action_client);
+    client->async_cancel_all_goals();
+    state.status_label->setText("Cancelling...");
+    state.status_label->setStyleSheet("color: orange;");
+  } catch (const std::bad_any_cast &) {
+    state.status_label->setText("No active goal to cancel");
+  }
+}
+
+// ─────────────────────────── execute dispatch ─────────────────────────────────
+
+void TasksPanel::onExecuteClicked()
+{
+  int idx = tab_widget_->currentIndex();
+  if (idx < 0 || idx >= static_cast<int>(tab_states_.size())) {return;}
+  if (active_task_tab_ >= 0) {return;}  // only one task at a time
+  if (task_defs_[idx].requires_airborne && !is_airborne_) {return;}  // airborne required
+
+  switch (idx) {
+    case 0: {  // Takeoff
+      task_msgs::action::TakeoffTask::Goal goal;
+      goal.target_altitude_m = getFloat(0, "target_altitude_m");
+      goal.velocity_m_s = getFloat(0, "velocity_m_s");
+      doSendGoal<task_msgs::action::TakeoffTask>(0, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | alt: %2 / %3 m")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.current_altitude_m, 0, 'f', 1)
+            .arg(fb.target_altitude_m, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message));
+        });
+      break;
+    }
+    case 1: {  // Land
+      task_msgs::action::LandTask::Goal goal;
+      goal.velocity_m_s = getFloat(1, "velocity_m_s");
+      doSendGoal<task_msgs::action::LandTask>(1, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | alt: %2 m")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.current_altitude_m, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message));
+        });
+      break;
+    }
+    case 2: {  // Navigate
+      task_msgs::action::NavigateTask::Goal goal;
+      goal.global_plan = getPath(2, "global_plan");
+      goal.goal_tolerance_m = getFloat(2, "goal_tolerance_m");
+      doSendGoal<task_msgs::action::NavigateTask>(2, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | dist: %2 m | pos: (%3, %4, %5)")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.distance_to_goal, 0, 'f', 1)
+            .arg(fb.current_position.x, 0, 'f', 1)
+            .arg(fb.current_position.y, 0, 'f', 1)
+            .arg(fb.current_position.z, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message));
+        });
+      break;
+    }
+    case 3: {  // Exploration
+      task_msgs::action::ExplorationTask::Goal goal;
+      goal.search_bounds = getPolygon(3, "search_bounds");
+      goal.min_altitude_agl = getFloat(3, "min_altitude_agl");
+      goal.max_altitude_agl = getFloat(3, "max_altitude_agl");
+      goal.min_flight_speed = getFloat(3, "min_flight_speed");
+      goal.max_flight_speed = getFloat(3, "max_flight_speed");
+      goal.time_limit_sec = getFloat(3, "time_limit_sec");
+      doSendGoal<task_msgs::action::ExplorationTask>(3, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | progress: %2 | pos: (%3, %4, %5)")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.progress, 0, 'f', 2)
+            .arg(fb.current_position.x, 0, 'f', 1)
+            .arg(fb.current_position.y, 0, 'f', 1)
+            .arg(fb.current_position.z, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message));
+        });
+      break;
+    }
+    case 4: {  // Coverage
+      task_msgs::action::CoverageTask::Goal goal;
+      goal.coverage_area = getPolygon(4, "coverage_area");
+      goal.min_altitude_agl = getFloat(4, "min_altitude_agl");
+      goal.max_altitude_agl = getFloat(4, "max_altitude_agl");
+      goal.min_flight_speed = getFloat(4, "min_flight_speed");
+      goal.max_flight_speed = getFloat(4, "max_flight_speed");
+      goal.line_spacing_m = getFloat(4, "line_spacing_m");
+      goal.heading_deg = getFloat(4, "heading_deg");
+      doSendGoal<task_msgs::action::CoverageTask>(4, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | progress: %2 | coverage: %3% | pos: (%4, %5, %6)")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.progress, 0, 'f', 2)
+            .arg(fb.coverage_percentage, 0, 'f', 1)
+            .arg(fb.current_position.x, 0, 'f', 1)
+            .arg(fb.current_position.y, 0, 'f', 1)
+            .arg(fb.current_position.z, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2\ncoverage: %3%")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message))
+            .arg(r->coverage_percentage, 0, 'f', 1);
+        });
+      break;
+    }
+    case 5: {  // Object Search
+      task_msgs::action::ObjectSearchTask::Goal goal;
+      goal.object_class = getString(5, "object_class");
+      goal.search_area = getPolygon(5, "search_area");
+      goal.min_altitude_agl = getFloat(5, "min_altitude_agl");
+      goal.max_altitude_agl = getFloat(5, "max_altitude_agl");
+      goal.min_flight_speed = getFloat(5, "min_flight_speed");
+      goal.max_flight_speed = getFloat(5, "max_flight_speed");
+      goal.time_limit_sec = getFloat(5, "time_limit_sec");
+      goal.target_count = getInt(5, "target_count");
+      doSendGoal<task_msgs::action::ObjectSearchTask>(5, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | progress: %2 | found: %3 | pos: (%4, %5, %6)")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.progress, 0, 'f', 2)
+            .arg(fb.objects_found_so_far)
+            .arg(fb.current_position.x, 0, 'f', 1)
+            .arg(fb.current_position.y, 0, 'f', 1)
+            .arg(fb.current_position.z, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2\nobjects_found: %3")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message))
+            .arg(r->objects_found);
+        });
+      break;
+    }
+    case 6: {  // Object Counting
+      task_msgs::action::ObjectCountingTask::Goal goal;
+      goal.object_class = getString(6, "object_class");
+      goal.count_area = getPolygon(6, "count_area");
+      goal.min_altitude_agl = getFloat(6, "min_altitude_agl");
+      goal.max_altitude_agl = getFloat(6, "max_altitude_agl");
+      goal.min_flight_speed = getFloat(6, "min_flight_speed");
+      goal.max_flight_speed = getFloat(6, "max_flight_speed");
+      doSendGoal<task_msgs::action::ObjectCountingTask>(6, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | progress: %2 | count: %3 | pos: (%4, %5, %6)")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.progress, 0, 'f', 2)
+            .arg(fb.current_count)
+            .arg(fb.current_position.x, 0, 'f', 1)
+            .arg(fb.current_position.y, 0, 'f', 1)
+            .arg(fb.current_position.z, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2\ncount: %3")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message))
+            .arg(r->count);
+        });
+      break;
+    }
+    case 7: {  // Semantic Search
+      task_msgs::action::SemanticSearchTask::Goal goal;
+      goal.query = getString(7, "query");
+      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) {
+          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);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2\nconfidence: %3")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message))
+            .arg(r->confidence, 0, 'f', 3);
+        });
+      break;
+    }
+    case 8: {  // Fixed Trajectory
+      task_msgs::action::FixedTrajectoryTask::Goal goal;
+      goal.trajectory_spec = getFixedTrajectory(8, "trajectory_spec");
+      goal.loop = getBool(8, "loop");
+      doSendGoal<task_msgs::action::FixedTrajectoryTask>(8, goal,
+        [](const auto & fb) {
+          return QString("status: %1 | progress: %2 | pos: (%3, %4, %5)")
+            .arg(QString::fromStdString(fb.status))
+            .arg(fb.progress, 0, 'f', 2)
+            .arg(fb.current_position.x, 0, 'f', 1)
+            .arg(fb.current_position.y, 0, 'f', 1)
+            .arg(fb.current_position.z, 0, 'f', 1);
+        },
+        [](const auto & r) {
+          return QString("success: %1\nmessage: %2")
+            .arg(r->success ? "true" : "false")
+            .arg(QString::fromStdString(r->message));
+        });
+      break;
+    }
+  }
+}
+
+// ─────────────────────────── cancel dispatch ──────────────────────────────────
+
+void TasksPanel::onCancelClicked()
+{
+  int idx = tab_widget_->currentIndex();
+  if (idx < 0 || idx >= static_cast<int>(tab_states_.size())) {return;}
+  if (!tab_states_[idx].goal_active) {return;}
+
+  switch (idx) {
+    case 0: doCancelGoal<task_msgs::action::TakeoffTask>(0); break;
+    case 1: doCancelGoal<task_msgs::action::LandTask>(1); break;
+    case 2: doCancelGoal<task_msgs::action::NavigateTask>(2); break;
+    case 3: doCancelGoal<task_msgs::action::ExplorationTask>(3); break;
+    case 4: doCancelGoal<task_msgs::action::CoverageTask>(4); break;
+    case 5: doCancelGoal<task_msgs::action::ObjectSearchTask>(5); break;
+    case 6: doCancelGoal<task_msgs::action::ObjectCountingTask>(6); break;
+    case 7: doCancelGoal<task_msgs::action::SemanticSearchTask>(7); break;
+    case 8: doCancelGoal<task_msgs::action::FixedTrajectoryTask>(8); break;
+  }
+}
+
+// ─────────────────────────── config persistence ──────────────────────────────
+
+void TasksPanel::save(rviz_common::Config config) const
+{
+  rviz_common::Panel::save(config);
+  config.mapSetValue("robot", robot_combo_->currentText());
+  for (size_t i = 0; i < tab_states_.size(); ++i) {
+    config.mapSetValue(
+      QString("executor_%1").arg(i),
+      tab_states_[i].executor_combo->currentText());
+  }
+}
+
+void TasksPanel::load(const rviz_common::Config & config)
+{
+  rviz_common::Panel::load(config);
+  QString robot;
+  if (config.mapGetString("robot", &robot)) {
+    robot_combo_->setCurrentText(robot);
+  }
+  for (size_t i = 0; i < tab_states_.size(); ++i) {
+    QString executor;
+    if (config.mapGetString(QString("executor_%1").arg(i), &executor)) {
+      tab_states_[i].executor_combo->setCurrentText(executor);
+    }
+  }
+}
+
+// ─────────────────────────── waypoint slots ──────────────────────────────────
+
+void TasksPanel::onWaypointCountChanged(int count)
+{
+  if (wp_count_label_) {
+    wp_count_label_->setText(QString("Waypoints: %1").arg(count));
+  }
+}
+
+void TasksPanel::onSelectedMarkerChanged(
+  const QString & name, double x, double y, double z, double yaw)
+{
+  if (wp_selected_label_) {
+    wp_selected_label_->setText(QString("Selected: %1").arg(name));
+  }
+
+  // Update spinboxes without triggering poseChanged
+  wp_updating_spinboxes_ = true;
+  if (wp_x_spin_) { wp_x_spin_->setValue(x); }
+  if (wp_y_spin_) { wp_y_spin_->setValue(y); }
+  if (wp_z_spin_) { wp_z_spin_->setValue(z); }
+  if (wp_yaw_spin_) { wp_yaw_spin_->setValue(yaw); }
+  wp_updating_spinboxes_ = false;
+}
+
+void TasksPanel::onClearWaypoints()
+{
+  if (waypoint_manager_) { waypoint_manager_->clearAll(); }
+}
+
+void TasksPanel::onSaveWaypoints()
+{
+  QString filename = QFileDialog::getSaveFileName(
+    this, tr("Save Waypoints"), "waypoints", tr("Bag Files (*.db3)"));
+  if (filename.isEmpty()) { return; }
+  if (waypoint_manager_) { waypoint_manager_->saveBag(filename.toStdString()); }
+}
+
+void TasksPanel::onLoadWaypoints()
+{
+  QString filename = QFileDialog::getOpenFileName(
+    this, tr("Load Waypoints"), "~/", tr("Bag Files (*.db3)"));
+  if (filename.isEmpty()) { return; }
+  if (waypoint_manager_) { waypoint_manager_->loadBag(filename.toStdString()); }
+}
+
+void TasksPanel::onWaypointPoseChanged(double)
+{
+  if (wp_updating_spinboxes_ || !waypoint_manager_) { return; }
+  waypoint_manager_->updateSelectedPose(
+    wp_x_spin_->value(), wp_y_spin_->value(),
+    wp_z_spin_->value(), wp_yaw_spin_->value());
+}
+
+}  // namespace rviz_tasks_panel
+
+PLUGINLIB_EXPORT_CLASS(rviz_tasks_panel::TasksPanel, rviz_common::Panel)
diff --git a/common/ros_packages/msgs/task_msgs/CMakeLists.txt b/common/ros_packages/msgs/task_msgs/CMakeLists.txt
index add09d32e..852f3e2b7 100644
--- a/common/ros_packages/msgs/task_msgs/CMakeLists.txt
+++ b/common/ros_packages/msgs/task_msgs/CMakeLists.txt
@@ -16,10 +16,12 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/CoverageTask.action"
   "action/ExplorationTask.action"
   "action/FixedTrajectoryTask.action"
+  "action/LandTask.action"
   "action/NavigateTask.action"
   "action/ObjectCountingTask.action"
   "action/ObjectSearchTask.action"
   "action/SemanticSearchTask.action"
+  "action/TakeoffTask.action"
   DEPENDENCIES action_msgs geometry_msgs nav_msgs airstack_msgs
 )
 
diff --git a/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action b/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action
index d8abff82b..72dfce8cb 100644
--- a/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action
+++ b/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action
@@ -1,8 +1,8 @@
-# Follow a pre-defined trajectory.
+# Follow a fixed trajectory specified by type and parameters.
 # loop: if true, repeat the trajectory indefinitely until cancelled
 
 # Goal
-airstack_msgs/TrajectoryXYZVYaw trajectory
+airstack_msgs/FixedTrajectory trajectory_spec
 bool loop
 ---
 # Result
@@ -12,6 +12,4 @@ string message
 # Feedback
 string status
 float32 progress
-int32 current_waypoint_index
-int32 total_waypoints
 geometry_msgs/Point current_position
diff --git a/common/ros_packages/msgs/task_msgs/action/LandTask.action b/common/ros_packages/msgs/task_msgs/action/LandTask.action
new file mode 100644
index 000000000..4e81d5fcd
--- /dev/null
+++ b/common/ros_packages/msgs/task_msgs/action/LandTask.action
@@ -0,0 +1,12 @@
+# Land the robot.
+
+# Goal
+float32 velocity_m_s      # 0.0 = use default from config
+---
+# Result
+bool success
+string message
+---
+# Feedback
+string status
+float32 current_altitude_m
diff --git a/common/ros_packages/msgs/task_msgs/action/TakeoffTask.action b/common/ros_packages/msgs/task_msgs/action/TakeoffTask.action
new file mode 100644
index 000000000..6d78d4daf
--- /dev/null
+++ b/common/ros_packages/msgs/task_msgs/action/TakeoffTask.action
@@ -0,0 +1,15 @@
+# Takeoff to a target altitude.
+# Rejects if not armed, no offboard control, or state estimate timed out.
+
+# Goal
+float32 target_altitude_m
+float32 velocity_m_s
+---
+# Result
+bool success
+string message
+---
+# Feedback
+string status
+float32 current_altitude_m
+float32 target_altitude_m
diff --git a/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml b/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml
index 7e14ec054..cef093702 100644
--- a/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml
+++ b/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml
@@ -2,35 +2,14 @@
     <group>
         <push-ros-namespace namespace="behavior" />
 
-        <!-- Behavior Tree -->
-        <node pkg="behavior_tree" exec="behavior_tree_implementation">
-            <param name="config" value="$(find-pkg-share behavior_tree)/config/drone.tree" />
-            <param name="timeout" value="1.0" />
+        <!-- Safety Monitor: state estimate watchdog + pause/resume/rewind commands -->
+        <node pkg="drone_safety_monitor" exec="drone_safety_monitor"
+              namespace="drone_safety_monitor" output="screen">
+            <param name="state_estimate_timeout" value="1.0" />
+            <remap from="state_estimate"
+                   to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+            <remap from="set_trajectory_mode"
+                   to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
         </node>
-
-        <!-- Behavior Executive -->
-        <node pkg="behavior_executive" exec="behavior_executive"
-            output="screen">
-            <!-- remap glue to AirStack -->
-            <remap from="is_armed" to="/$(env ROBOT_NAME)/interface/is_armed" />
-            <!-- topics -->
-            <remap from="has_control" to="/$(env ROBOT_NAME)/interface/has_control" />
-            <!-- services -->
-            <remap from="robot_command" to="/$(env ROBOT_NAME)/interface/robot_command" />
-            <remap from="set_trajectory_mode" to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
-            <remap from="set_takeoff_landing_command" to="/$(env ROBOT_NAME)/takeoff_landing_planner/set_takeoff_landing_command" />
-            <remap from="/global/trajectory" to="/$(env ROBOT_NAME)/global/trajectory" />
-            <!-- <remap from="get_random_walk_plan" to="/$(env ROBOT_NAME)/global/get_random_walk_plan" /> -->
-            <remap from="takeoff_state" to="/$(env ROBOT_NAME)/takeoff_landing_planner/takeoff_state" />
-            <remap from="landing_state" to="/$(env ROBOT_NAME)/takeoff_landing_planner/landing_state" />
-            <remap from="state_estimate_timed_out" to="/$(env ROBOT_NAME)/drone_safety_monitor/state_estimate_timed_out" />
-            <remap from="set_recording_status" to="/$(env ROBOT_NAME)/bag_record/set_recording_status" />
-            <remap from="reset_stuck" to="/$(env ROBOT_NAME)/droan/reset_stuck" />
-            <remap from="clear_map" to="/$(env ROBOT_NAME)/droan/clear_map" />
-            <remap from="stuck" to="/$(env ROBOT_NAME)/droan/stuck" />
-            <remap from="exploration_task" to="/$(env ROBOT_NAME)/tasks/exploration" />
-
-        </node>
-
     </group>
 </launch>
diff --git a/robot/ros_ws/src/behavior/behavior_executive/CMakeLists.txt b/robot/ros_ws/src/behavior/behavior_executive/CMakeLists.txt
deleted file mode 100644
index b84f62f77..000000000
--- a/robot/ros_ws/src/behavior/behavior_executive/CMakeLists.txt
+++ /dev/null
@@ -1,54 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(behavior_executive)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_action REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(task_msgs REQUIRED)
-find_package(behavior_tree REQUIRED)
-find_package(behavior_tree_msgs REQUIRED)
-find_package(airstack_msgs REQUIRED)
-find_package(airstack_common REQUIRED)
-find_package(mavros_msgs REQUIRED)
-
-add_executable(behavior_executive src/behavior_executive.cpp)
-target_include_directories(behavior_executive PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-target_compile_features(behavior_executive PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-ament_target_dependencies(behavior_executive
-  rclcpp
-  rclcpp_action
-  std_msgs
-  nav_msgs
-  task_msgs
-  behavior_tree
-  behavior_tree_msgs
-  airstack_msgs
-  airstack_common
-  mavros_msgs
-)
-
-install(TARGETS behavior_executive
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
diff --git a/robot/ros_ws/src/behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/robot/ros_ws/src/behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp
deleted file mode 100644
index b71896cf7..000000000
--- a/robot/ros_ws/src/behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp
+++ /dev/null
@@ -1,120 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <airstack_common/ros2_helper.hpp>
-#include <airstack_msgs/srv/robot_command.hpp>
-#include <airstack_msgs/srv/takeoff_landing_command.hpp>
-#include <airstack_msgs/srv/trajectory_mode.hpp>
-#include <behavior_tree/behavior_tree.hpp>
-#include <behavior_tree_msgs/msg/behavior_tree_commands.hpp>
-#include <nav_msgs/msg/path.hpp>
-#include <std_msgs/msg/empty.hpp>
-#include <vector>
-
-#include "rclcpp_action/rclcpp_action.hpp"
-#include "task_msgs/action/exploration_task.hpp"
-
-class BehaviorExecutive : public rclcpp::Node {
-   private:
-    // parameters
-    bool ascent_takeoff;
-
-    // variables
-    std::string takeoff_state, landing_state;
-
-    // Condition variables
-    bt::Condition* auto_takeoff_commanded_condition;
-    bt::Condition* takeoff_commanded_condition;
-    bt::Condition* armed_condition;
-    bt::Condition* offboard_mode_condition;
-    bt::Condition* stationary_condition;
-    bt::Condition* land_commanded_condition;
-    bt::Condition* pause_commanded_condition;
-    bt::Condition* rewind_commanded_condition;
-    bt::Condition* fixed_trajectory_condition;
-    bt::Condition* global_plan_condition;
-    bt::Condition* offboard_commanded_condition;
-    bt::Condition* arm_commanded_condition;
-    bt::Condition* disarm_commanded_condition;
-    bt::Condition* takeoff_complete_condition;
-    bt::Condition* landing_complete_condition;
-    bt::Condition* in_air_condition;
-    bt::Condition* state_estimate_timed_out_condition;
-    bt::Condition* stuck_condition;
-    bt::Condition* autonomously_explore_condition;
-    std::vector<bt::Condition*> conditions;
-
-    // Action variables
-    bt::Action* arm_action;
-    bt::Action* takeoff_action;
-    bt::Action* land_action;
-    bt::Action* pause_action;
-    bt::Action* rewind_action;
-    bt::Action* follow_fixed_trajectory_action;
-    bt::Action* global_plan_action;
-    bt::Action* request_control_action;
-    bt::Action* disarm_action;
-    std::vector<bt::Action*> actions;
-
-    // subscribers
-    rclcpp::Subscription<behavior_tree_msgs::msg::BehaviorTreeCommands>::SharedPtr
-        behavior_tree_commands_sub;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr is_armed_sub;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr has_control_sub;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr takeoff_state_sub;
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr landing_state_sub;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr state_estimate_timed_out_sub;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr stuck_sub;
-
-    // publishers
-    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr recording_pub;
-    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr reset_stuck_pub;
-    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clear_map_pub;
-
-    // services
-    rclcpp::CallbackGroup::SharedPtr service_callback_group;
-    rclcpp::Client<airstack_msgs::srv::RobotCommand>::SharedPtr robot_command_client;
-    rclcpp::Client<airstack_msgs::srv::TrajectoryMode>::SharedPtr trajectory_mode_client;
-    rclcpp::Client<airstack_msgs::srv::TakeoffLandingCommand>::SharedPtr
-        takeoff_landing_command_client;
-
-    // action clients
-    using ExplorationTask = task_msgs::action::ExplorationTask;
-    rclcpp_action::Client<ExplorationTask>::SharedPtr exploration_client_;
-    rclcpp_action::ClientGoalHandle<ExplorationTask>::SharedPtr exploration_goal_handle_;
-    bool exploration_goal_done_ = true;
-    bool exploration_goal_succeeded_ = false;
-
-    // timers
-    rclcpp::TimerBase::SharedPtr timer;
-
-    // callbacks
-    void timer_callback();
-    void bt_commands_callback(behavior_tree_msgs::msg::BehaviorTreeCommands msg);
-    void is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg);
-    void has_control_callback(const std_msgs::msg::Bool::SharedPtr msg);
-    void takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg);
-    void landing_state_callback(const std_msgs::msg::String::SharedPtr msg);
-    void state_estimate_timed_out_callback(const std_msgs::msg::Bool::SharedPtr msg);
-    void stuck_callback(const std_msgs::msg::Bool::SharedPtr msg);
-
-   public:
-    BehaviorExecutive();
-}; 
diff --git a/robot/ros_ws/src/behavior/behavior_executive/src/behavior_executive.cpp b/robot/ros_ws/src/behavior/behavior_executive/src/behavior_executive.cpp
deleted file mode 100644
index 9dc0900ba..000000000
--- a/robot/ros_ws/src/behavior/behavior_executive/src/behavior_executive.cpp
+++ /dev/null
@@ -1,467 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <behavior_executive/behavior_executive.hpp>
-
-BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive")
-{
-    // conditions
-    auto_takeoff_commanded_condition = new bt::Condition("Auto Takeoff Commanded", this);
-    takeoff_commanded_condition = new bt::Condition("Takeoff Commanded", this);
-    armed_condition = new bt::Condition("Armed", this);
-    offboard_mode_condition = new bt::Condition("Offboard Mode", this);
-    stationary_condition = new bt::Condition("Stationary", this);
-    land_commanded_condition = new bt::Condition("Land Commanded", this);
-    pause_commanded_condition = new bt::Condition("Pause Commanded", this);
-    rewind_commanded_condition = new bt::Condition("Rewind Commanded", this);
-    fixed_trajectory_condition = new bt::Condition("Fixed Trajectory Commanded", this);
-    global_plan_condition = new bt::Condition("Global Plan Commanded", this);
-    offboard_commanded_condition = new bt::Condition("Offboard Commanded", this);
-    arm_commanded_condition = new bt::Condition("Arm Commanded", this);
-    disarm_commanded_condition = new bt::Condition("Disarm Commanded", this);
-    takeoff_complete_condition = new bt::Condition("Takeoff Complete", this);
-    landing_complete_condition = new bt::Condition("Landing Complete", this);
-    in_air_condition = new bt::Condition("In Air", this);
-    state_estimate_timed_out_condition = new bt::Condition("State Estimate Timed Out", this);
-    stuck_condition = new bt::Condition("Stuck", this);
-    autonomously_explore_condition = new bt::Condition("Autonomously Explore Commanded", this);
-    conditions.push_back(auto_takeoff_commanded_condition);
-    conditions.push_back(takeoff_commanded_condition);
-    conditions.push_back(armed_condition);
-    conditions.push_back(offboard_mode_condition);
-    conditions.push_back(stationary_condition);
-    conditions.push_back(land_commanded_condition);
-    conditions.push_back(pause_commanded_condition);
-    conditions.push_back(rewind_commanded_condition);
-    conditions.push_back(fixed_trajectory_condition);
-    conditions.push_back(global_plan_condition);
-    conditions.push_back(offboard_commanded_condition);
-    conditions.push_back(arm_commanded_condition);
-    conditions.push_back(disarm_commanded_condition);
-    conditions.push_back(takeoff_complete_condition);
-    conditions.push_back(landing_complete_condition);
-    conditions.push_back(in_air_condition);
-    conditions.push_back(state_estimate_timed_out_condition);
-    conditions.push_back(stuck_condition);
-    conditions.push_back(autonomously_explore_condition);
-
-    // actions
-    arm_action = new bt::Action("Arm", this);
-    takeoff_action = new bt::Action("Takeoff", this);
-    land_action = new bt::Action("Land", this);
-    pause_action = new bt::Action("Pause", this);
-    rewind_action = new bt::Action("Rewind", this);
-    follow_fixed_trajectory_action = new bt::Action("Follow Fixed Trajectory", this);
-    global_plan_action = new bt::Action("Follow Global Plan", this);
-    request_control_action = new bt::Action("Request Control", this);
-    disarm_action = new bt::Action("Disarm", this);
-    actions.push_back(arm_action);
-    actions.push_back(takeoff_action);
-    actions.push_back(land_action);
-    actions.push_back(pause_action);
-    actions.push_back(rewind_action);
-    actions.push_back(follow_fixed_trajectory_action);
-    actions.push_back(global_plan_action);
-    actions.push_back(request_control_action);
-    actions.push_back(disarm_action);
-
-    // subscribers
-    behavior_tree_commands_sub =
-        this->create_subscription<behavior_tree_msgs::msg::BehaviorTreeCommands>(
-            "behavior_tree_commands", 1,
-            std::bind(&BehaviorExecutive::bt_commands_callback, this, std::placeholders::_1));
-    is_armed_sub = this->create_subscription<std_msgs::msg::Bool>(
-        "is_armed", 1,
-        std::bind(&BehaviorExecutive::is_armed_callback, this, std::placeholders::_1));
-    has_control_sub = this->create_subscription<std_msgs::msg::Bool>(
-        "has_control", 1,
-        std::bind(&BehaviorExecutive::has_control_callback, this, std::placeholders::_1));
-    takeoff_state_sub = this->create_subscription<std_msgs::msg::String>("takeoff_state", 1,
-                                                                         std::bind(&BehaviorExecutive::takeoff_state_callback,
-                                                                                   this, std::placeholders::_1));
-    landing_state_sub = this->create_subscription<std_msgs::msg::String>("landing_state", 1,
-                                                                         std::bind(&BehaviorExecutive::landing_state_callback,
-                                                                                   this, std::placeholders::_1));
-    state_estimate_timed_out_sub =
-        this->create_subscription<std_msgs::msg::Bool>("state_estimate_timed_out", 1,
-                                                       std::bind(&BehaviorExecutive::state_estimate_timed_out_callback,
-                                                                 this, std::placeholders::_1));
-    stuck_sub =
-        this->create_subscription<std_msgs::msg::Bool>("stuck", 1,
-                                                       std::bind(&BehaviorExecutive::stuck_callback,
-                                                                 this, std::placeholders::_1));
-
-    // publishers
-    recording_pub = this->create_publisher<std_msgs::msg::Bool>("set_recording_status", 1);
-    reset_stuck_pub = this->create_publisher<std_msgs::msg::Empty>("reset_stuck", 1);
-    clear_map_pub = this->create_publisher<std_msgs::msg::Empty>("clear_map", 1);
-
-    // services
-    service_callback_group =
-        this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-    robot_command_client = this->create_client<airstack_msgs::srv::RobotCommand>(
-        "robot_command", rmw_qos_profile_services_default, service_callback_group);
-    trajectory_mode_client = this->create_client<airstack_msgs::srv::TrajectoryMode>(
-        "set_trajectory_mode", rmw_qos_profile_services_default, service_callback_group);
-    takeoff_landing_command_client = this->create_client<airstack_msgs::srv::TakeoffLandingCommand>(
-        "set_takeoff_landing_command", rmw_qos_profile_services_default, service_callback_group);
-
-    // action clients
-    exploration_client_ = rclcpp_action::create_client<ExplorationTask>(this, "exploration_task");
-
-    // timers
-    timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.),
-                                 std::bind(&BehaviorExecutive::timer_callback, this));
-}
-
-void BehaviorExecutive::timer_callback()
-{
-    if (request_control_action->is_active())
-    {
-        if (request_control_action->active_has_changed())
-        {
-            airstack_msgs::srv::RobotCommand::Request::SharedPtr request =
-                std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
-            request->command = airstack_msgs::srv::RobotCommand::Request::REQUEST_CONTROL;
-
-            auto result = robot_command_client->async_send_request(request);
-            std::cout << "waiting be rc" << std::endl;
-            result.wait();
-            std::cout << "done be rc" << std::endl;
-            if (result.get()->success)
-                request_control_action->set_success();
-            else
-                request_control_action->set_failure();
-        }
-    }
-
-    if (arm_action->is_active())
-    {
-        if (arm_action->active_has_changed())
-        {
-            std_msgs::msg::Bool start_msg;
-            start_msg.data = true;
-            recording_pub->publish(start_msg);
-
-            airstack_msgs::srv::RobotCommand::Request::SharedPtr request =
-                std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
-            request->command = airstack_msgs::srv::RobotCommand::Request::ARM;
-
-            auto result = robot_command_client->async_send_request(request);
-            std::cout << "waiting be arm" << std::endl;
-            result.wait();
-            std::cout << "done be arm" << std::endl;
-            if (result.get()->success)
-                arm_action->set_success();
-            else
-                arm_action->set_failure();
-        }
-    }
-
-    if (disarm_action->is_active())
-    {
-        if (disarm_action->active_has_changed())
-        {
-            std_msgs::msg::Bool stop_msg;
-            stop_msg.data = false;
-            recording_pub->publish(stop_msg);
-
-            in_air_condition->set(false);
-            airstack_msgs::srv::RobotCommand::Request::SharedPtr request =
-                std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
-            request->command = airstack_msgs::srv::RobotCommand::Request::DISARM;
-
-            auto result = robot_command_client->async_send_request(request);
-            std::cout << "waiting be arm" << std::endl;
-            result.wait();
-            std::cout << "done be arm" << std::endl;
-            if (result.get()->success)
-                disarm_action->set_success();
-            else
-                disarm_action->set_failure();
-        }
-    }
-
-    if (takeoff_action->is_active())
-    {
-        // std::cout << "takeoff" << std::endl;
-        takeoff_action->set_running();
-        in_air_condition->set(true);
-        reset_stuck_pub->publish(std_msgs::msg::Empty());
-
-        if (takeoff_action->active_has_changed())
-        {
-            // clear the local planner's map
-            clear_map_pub->publish(std_msgs::msg::Empty());
-
-            // put trajectory controller in track mode
-            airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request =
-                std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
-            mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK;
-            auto mode_result = trajectory_mode_client->async_send_request(mode_request);
-            std::cout << "trajectory mode request sent" << std::endl;
-            mode_result.wait();
-            std::cout << "trajectory mode request confirmed received" << std::endl;
-
-            if (mode_result.get()->success)
-            {
-                // send a takeoff command to ardupilot
-                // TODO clean up variable names
-                airstack_msgs::srv::RobotCommand::Request::SharedPtr request =
-                    std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
-                request->command = airstack_msgs::srv::RobotCommand::Request::TAKEOFF;
-
-                auto result = robot_command_client->async_send_request(request);
-                std::cout << "waiting robot command takeoff" << std::endl;
-                result.wait();
-                std::cout << "done robot command takeoff" << std::endl;
-
-                // send the takeoff trajectory
-                if (result.get()->success)
-                {
-                    airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr takeoff_request =
-                        std::make_shared<airstack_msgs::srv::TakeoffLandingCommand::Request>();
-                    takeoff_request->command =
-                        airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF;
-                    auto takeoff_result =
-                        takeoff_landing_command_client->async_send_request(takeoff_request);
-                    std::cout << "takeoff request sent" << std::endl;
-                    takeoff_result.wait();
-                    std::cout << "takeoff request confirmed received" << std::endl;
-                    if (!takeoff_result.get()->accepted)
-                        takeoff_action->set_failure();
-                }
-                else
-                    takeoff_action->set_failure();
-            }
-            else
-                takeoff_action->set_failure();
-        }
-
-        if (takeoff_state == "COMPLETE")
-        {
-            takeoff_complete_condition->set(true);
-            takeoff_action->set_success();
-        }
-    }
-
-    if (land_action->is_active())
-    {
-        // std::cout << "land" << std::endl;
-        land_action->set_running();
-        if (land_action->active_has_changed())
-        {
-            // put trajectory controller in track mode
-            airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request =
-                std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
-            mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK;
-            auto mode_result = trajectory_mode_client->async_send_request(mode_request);
-            std::cout << "trajectory mode request sent" << std::endl;
-            mode_result.wait();
-            std::cout << "trajectory mode request confirmed received" << std::endl;
-
-            if (mode_result.get()->success)
-            {
-                airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr land_request =
-                    std::make_shared<airstack_msgs::srv::TakeoffLandingCommand::Request>();
-                land_request->command = airstack_msgs::srv::TakeoffLandingCommand::Request::LAND;
-                auto land_result = takeoff_landing_command_client->async_send_request(land_request);
-                std::cout << "land 1" << std::endl;
-                land_result.wait();
-                std::cout << "land 2" << std::endl;
-                if (land_result.get()->accepted)
-                    land_action->set_success();
-                else
-                    land_action->set_failure();
-            }
-            else
-                land_action->set_failure();
-        }
-
-        if (landing_state == "COMPLETE")
-        {
-            landing_complete_condition->set(true);
-            land_action->set_success();
-        }
-    }
-
-    if (pause_action->is_active())
-    {
-        pause_action->set_running();
-        if (pause_action->active_has_changed())
-        {
-            airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request =
-                std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
-            mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::PAUSE;
-            auto mode_result = trajectory_mode_client->async_send_request(mode_request);
-            mode_result.wait();
-        }
-    }
-
-    if (rewind_action->is_active())
-    {
-        rewind_action->set_running();
-        if (rewind_action->active_has_changed())
-        {
-            airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request =
-                std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
-            mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::REWIND;
-            auto mode_result = trajectory_mode_client->async_send_request(mode_request);
-            mode_result.wait();
-        }
-    }
-
-    // follow fixed trajectory action
-    if (follow_fixed_trajectory_action->is_active())
-    {
-        follow_fixed_trajectory_action->set_running();
-
-        if (follow_fixed_trajectory_action->active_has_changed())
-        {
-            // put trajectory controller in track mode
-            airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request =
-                std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
-            mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK;
-            auto mode_result = trajectory_mode_client->async_send_request(mode_request);
-            std::cout << "trajectory mode request sent" << std::endl;
-            mode_result.wait();
-            std::cout << "trajectory mode request confirmed received" << std::endl;
-        }
-    }
-
-    if (global_plan_action->is_active())
-    {
-        global_plan_action->set_running();
-        if (global_plan_action->active_has_changed())
-        {
-            auto goal = ExplorationTask::Goal();
-            goal.min_altitude_agl = 2.0f;
-            goal.max_altitude_agl = 20.0f;
-            goal.min_flight_speed = 1.0f;
-            goal.max_flight_speed = 5.0f;
-            goal.time_limit_sec = 0.0f;  // no limit
-
-            exploration_goal_done_ = false;
-            exploration_goal_succeeded_ = false;
-
-            auto options = rclcpp_action::Client<ExplorationTask>::SendGoalOptions();
-            options.goal_response_callback =
-                [this](const rclcpp_action::ClientGoalHandle<ExplorationTask>::SharedPtr& gh) {
-                    exploration_goal_handle_ = gh;
-                    if (!gh) {
-                        RCLCPP_WARN(this->get_logger(), "ExplorationTask goal rejected");
-                        exploration_goal_done_ = true;
-                    }
-                };
-            options.result_callback =
-                [this](const rclcpp_action::ClientGoalHandle<ExplorationTask>::WrappedResult& result) {
-                    exploration_goal_succeeded_ =
-                        (result.code == rclcpp_action::ResultCode::SUCCEEDED) && result.result->success;
-                    exploration_goal_done_ = true;
-                };
-            exploration_client_->async_send_goal(goal, options);
-            RCLCPP_INFO(this->get_logger(), "Sent ExplorationTask goal");
-        }
-
-        if (exploration_goal_done_)
-        {
-            if (exploration_goal_succeeded_)
-                global_plan_action->set_success();
-            else
-                global_plan_action->set_failure();
-        }
-    }
-
-    for (bt::Condition *condition : conditions)
-        condition->publish();
-    for (bt::Action *action : actions)
-        action->publish();
-}
-
-// callbacks
-
-/**
- * @brief Parses behavior tree commands and updates conditions accordingly
- *
- * @param msg
- */
-void BehaviorExecutive::bt_commands_callback(behavior_tree_msgs::msg::BehaviorTreeCommands msg)
-{
-    for (int i = 0; i < msg.commands.size(); i++)
-    {
-        std::string condition_name = msg.commands[i].condition_name;
-        int status = msg.commands[i].status;
-
-        for (int j = 0; j < conditions.size(); j++)
-        {
-            bt::Condition *condition = conditions[j];
-            if (condition_name == condition->get_label())
-            {
-                if (status == behavior_tree_msgs::msg::Status::SUCCESS)
-                    condition->set(true);
-                else if (status == behavior_tree_msgs::msg::Status::FAILURE)
-                    condition->set(false);
-            }
-        }
-    }
-}
-
-void BehaviorExecutive::is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    armed_condition->set(msg->data);
-}
-
-void BehaviorExecutive::has_control_callback(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    offboard_mode_condition->set(msg->data);
-}
-
-void BehaviorExecutive::takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg)
-{
-    takeoff_state = msg->data;
-}
-
-void BehaviorExecutive::landing_state_callback(const std_msgs::msg::String::SharedPtr msg)
-{
-    landing_state = msg->data;
-}
-
-void BehaviorExecutive::state_estimate_timed_out_callback(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    state_estimate_timed_out_condition->set(msg->data);
-}
-
-void BehaviorExecutive::stuck_callback(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    stuck_condition->set(msg->data);
-}
-
-int main(int argc, char **argv)
-{
-    rclcpp::init(argc, argv);
-    std::shared_ptr<rclcpp::Node> node = std::make_shared<BehaviorExecutive>();
-
-    rclcpp::executors::MultiThreadedExecutor executor;
-    executor.add_node(node);
-    executor.spin();
-    rclcpp::shutdown();
-
-    return 0;
-}
diff --git a/robot/ros_ws/src/behavior/behavior_tree/.gitignore b/robot/ros_ws/src/behavior/behavior_tree/.gitignore
deleted file mode 100644
index e6458330d..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-*~
-*.pyc
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree/CMakeLists.txt b/robot/ros_ws/src/behavior/behavior_tree/CMakeLists.txt
deleted file mode 100644
index 00372fad8..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/CMakeLists.txt
+++ /dev/null
@@ -1,75 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(behavior_tree)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(behavior_tree_msgs REQUIRED)
-find_package(Boost REQUIRED COMPONENTS iostreams)
-
-include_directories(include ${rclcpp_INCLUDE_DIRS} ${behavior_tree_msgs_INCLUDE_DIRS})
-
-add_executable(behavior_tree_implementation src/behavior_tree_implementation.cpp)
-target_compile_features(behavior_tree_implementation PUBLIC c_std_99 cxx_std_17)
-target_link_libraries(behavior_tree_implementation
-  ${Boost_LIBRARIES}
-  ${rclcpp_LIBRARIES}
-  ${std_msgs_LIBRARIES}
-  ${behavior_tree_msgs_LIBRARIES})
-#ament_target_dependencies(behavior_tree_implementation rclcpp std_msgs behavior_tree_msgs)
-
-add_library(behavior_tree src/behavior_tree.cpp)
-ament_target_dependencies(behavior_tree rclcpp std_msgs behavior_tree_msgs)
-ament_export_targets(behavior_tree HAS_LIBRARY_TARGET)
-
-install(
-  DIRECTORY include/behavior_tree
-  DESTINATION include
-  )
-
-install(
-  TARGETS behavior_tree
-  EXPORT behavior_tree
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION bin
-  INCLUDES DESTINATION include
-)
-
-install(TARGETS
-  behavior_tree_implementation
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-# Install files.
-# install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
-# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
-install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
-# install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
-
-ament_package()
diff --git a/robot/ros_ws/src/behavior/behavior_tree/config/drone.tree b/robot/ros_ws/src/behavior/behavior_tree/config/drone.tree
deleted file mode 100644
index bf28299ed..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/config/drone.tree
+++ /dev/null
@@ -1,82 +0,0 @@
-?
-	->
-		(Auto Takeoff Commanded)
-		->
-			<!>
-				(State Estimate Timed Out)
-			?
-				(In Air)
-				(Armed)
-				(Offboard Mode)
-				[Request Control]
-			?
-				(In Air)
-				(Armed)
-				[Arm]
-			?
-				(Takeoff Complete)
-				[Takeoff]
-	->
-		(Takeoff Commanded)
-		?
-			(Takeoff Complete)
-			[Takeoff]
-	->
-		(Land Commanded)
-		?
-			(Landing Complete)
-			[Land]
-	->
-		(Pause Commanded)
-		?
-			[Pause]
-	->
-		(Rewind Commanded)
-		?
-			[Rewind]
-	->
-		(Fixed Trajectory Commanded)
-		?
-			[Follow Fixed Trajectory]
-	->
-		(Global Plan Commanded)
-		?
-			[Follow Global Plan]
-	->
-		(Offboard Commanded)
-		?
-			(Offboard Mode)
-			[Request Control]
-	->
-		(Arm Commanded)
-		?
-			(Armed)
-			[Arm]
-	->
-		(Disarm Commanded)
-		?
-			<!>
-				(Armed)
-			[Disarm]
-	->
-		(Autonomously Explore Commanded)
-		->
-			<!>
-				(State Estimate Timed Out)
-			?
-				(In Air)
-				(Armed)
-				(Offboard Mode)
-				[Request Control]
-			?
-				(In Air)
-				(Armed)
-				[Arm]
-			?
-				(Takeoff Complete)
-				[Takeoff]
-		?
-			->
-				(Stuck)
-				[Rewind]
-			[Follow Global Plan]
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp b/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp
deleted file mode 100644
index 2c1b9a6ed..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp
+++ /dev/null
@@ -1,139 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#pragma once
-
-#include <algorithm>
-#include <behavior_tree_msgs/msg/active.hpp>
-#include <behavior_tree_msgs/msg/status.hpp>
-#include <mutex>
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <string>
-#include <thread>
-
-namespace bt {
-
-class Condition {
-   private:
-    std_msgs::msg::Bool success;
-    std::string label;
-
-    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr success_pub;
-
-   public:
-    Condition(std::string label, rclcpp::Node* node);
-    void set(bool b);
-    void publish();
-    bool get();
-    std::string get_label();
-
-    static std::string get_published_topic_name(std::string label);
-};
-
-class Action {
-   private:
-    bool active, prev_active, active_changed;
-    uint64_t current_id;
-    behavior_tree_msgs::msg::Status status;
-    std::string label;
-
-    rclcpp::Subscription<behavior_tree_msgs::msg::Active>::SharedPtr active_sub;
-    rclcpp::Publisher<behavior_tree_msgs::msg::Status>::SharedPtr status_pub;
-
-    void active_callback(const behavior_tree_msgs::msg::Active::SharedPtr msg);
-
-    std::function<void()> active_callback_func;
-    std::function<void()> inactive_callback_func;
-
-    void init(std::string label, rclcpp::Node* node);
-
-   public:
-    Action(std::string label, rclcpp::Node* node);
-
-    // constructor for active and inactive callback that are class members
-    template <class ActiveClassType, class InactiveClassType>
-    Action(std::string label, rclcpp::Node* node, void (ActiveClassType::*active_callback)(),
-           ActiveClassType* active_class_object, void (InactiveClassType::*inactive_callback)(),
-           InactiveClassType* inactive_class_object) {
-        init(label, node);
-        active_callback_func = std::bind(active_callback, active_class_object);
-        inactive_callback_func = std::bind(inactive_callback, inactive_class_object);
-    }
-
-    // constructor for an active callback that is a class member
-    template <class ActiveClassType>
-    Action(std::string label, rclcpp::Node* node, void (ActiveClassType::*active_callback)(),
-           ActiveClassType* active_class_object) {
-        init(label, node);
-        active_callback_func = std::bind(active_callback, active_class_object);
-    }
-
-    // constructor for active and inactive callbacks that are not class members. The inactive
-    // callback is optional
-    Action(std::string label, rclcpp::Node* node, void (*active_callback)(),
-           void (*inactive_callback)() = NULL) {
-        init(label, node);
-        active_callback_func = active_callback;
-        inactive_callback_func = inactive_callback;
-    }
-
-    // constructor for an active callback that is not a class member and inactive callback that is a
-    // class member
-    template <class InactiveClassType>
-    Action(std::string label, rclcpp::Node* node, void (*active_callback)(),
-           void (InactiveClassType::*inactive_callback)(),
-           InactiveClassType* inactive_class_object) {
-        init(label, node);
-        active_callback_func = active_callback;
-        inactive_callback_func = std::bind(inactive_callback, inactive_class_object);
-    }
-
-    // constructor for an active callback that is a class member and inactive callback that is not a
-    // class member
-    template <class ActiveClassType>
-    Action(std::string label, rclcpp::Node* node, void (ActiveClassType::*active_callback)(),
-           ActiveClassType* active_class_object, void (*inactive_callback)()) {
-        init(label, node);
-        active_callback_func = std::bind(active_callback, active_class_object);
-        inactive_callback_func = inactive_callback;
-    }
-
-    static std::string get_published_topic_name(std::string label);
-    static std::string get_subscribed_topic_name(std::string label);
-
-    void set_success();
-    void set_running();
-    void set_failure();
-
-    bool is_active();
-    bool active_has_changed();
-
-    bool is_success();
-    bool is_running();
-    bool is_failure();
-
-    std::string get_label();
-
-    void publish();
-};
-
-};  // namespace bt 
diff --git a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp b/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp
deleted file mode 100644
index 8d142abe2..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp
+++ /dev/null
@@ -1,194 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#pragma once
-
-#include <stdint.h>
-
-#include <behavior_tree_msgs/msg/active.hpp>
-#include <behavior_tree_msgs/msg/status.hpp>
-#include <behavior_tree_msgs/msg/graph_viz_xdot.hpp>
-#include <behavior_tree_msgs/msg/graph_viz_xdot_compressed.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <string>
-#include <unordered_map>
-#include <vector>
-
-//===============================================================================================
-// ------------------------------------- Base Node Class ----------------------------------------
-//===============================================================================================
-
-class Node {
-   private:
-   public:
-    static double max_wait_time;
-
-    std::string label;
-    bool is_active;
-    uint8_t status;
-    std::vector<Node*> children;
-
-    virtual void add_child(Node*);
-    virtual bool tick(bool active, int traversal_count) = 0;
-};
-
-//===============================================================================================
-// ------------------------------------ Control Flow Nodes  -------------------------------------
-//===============================================================================================
-
-class ControlFlowNode : public Node {
-   private:
-   public:
-};
-
-class FallbackNode : public ControlFlowNode {
-   private:
-   public:
-    FallbackNode();
-    bool tick(bool active, int traversal_count) override;
-};
-
-class SequenceNode : public ControlFlowNode {
-   private:
-   public:
-    SequenceNode();
-    bool tick(bool active, int traversal_count) override;
-};
-
-class ParallelNode : public ControlFlowNode {
-   private:
-   public:
-    int child_success_threshold;
-    ParallelNode(int child_success_threshold);
-    bool tick(bool active, int traversal_count) override;
-};
-
-//===============================================================================================
-// -------------------------------------- Execution Nodes  --------------------------------------
-//===============================================================================================
-
-class ExecutionNode : public Node {
-   private:
-   public:
-    rclcpp::Time status_modification_time;
-    int current_traversal_count;
-    rclcpp::Node* node;
-    // rclcpp::Subscription subscriber;
-    // rclcpp::Publisher publisher;
-
-    ExecutionNode(rclcpp::Node* node);
-    void init_ros();
-    virtual std::string get_publisher_name() = 0;
-    virtual void init_publisher() = 0;
-    virtual std::string get_subscriber_name() = 0;
-    virtual void init_subscriber() = 0;
-    void set_status(uint8_t status);
-    double get_status_age();
-};
-
-class ConditionNode : public ExecutionNode {
-   private:
-    bool callback_status_updated;
-    // rclcpp::Node* node;
-
-   public:
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr subscriber;
-    // rclcpp::Publisher<>::SharedPtr publisher;
-
-    ConditionNode(std::string label, rclcpp::Node* node);
-    bool tick(bool active, int traversal_count) override;
-    std::string get_publisher_name() override;
-    void init_publisher() override;
-    std::string get_subscriber_name() override;
-    void init_subscriber() override;
-    void callback(const std_msgs::msg::Bool::SharedPtr msg);
-};
-
-class ActionNode : public ExecutionNode {
-   private:
-    int current_id;
-    bool publisher_initialized;
-    bool callback_status_updated;
-
-   public:
-    bool is_newly_active;
-
-    // rclcpp::Node* node;
-
-    rclcpp::Subscription<behavior_tree_msgs::msg::Status>::SharedPtr subscriber;
-    rclcpp::Publisher<behavior_tree_msgs::msg::Active>::SharedPtr publisher;
-
-    ActionNode(std::string label, rclcpp::Node* node);
-    bool tick(bool active, int traversal_count) override;
-    std::string get_publisher_name() override;
-    void init_publisher() override;
-    std::string get_subscriber_name() override;
-    void init_subscriber() override;
-    void callback(const behavior_tree_msgs::msg::Status::SharedPtr msg);
-    void publish_active_msg(int active_id);
-};
-
-//===============================================================================================
-// -------------------------------------- Decorator Nodes  --------------------------------------
-//===============================================================================================
-
-class DecoratorNode : public Node {
-   private:
-   public:
-    void add_child(Node* node) override;
-};
-
-class NotNode : public DecoratorNode {
-   private:
-   public:
-    NotNode();
-    void add_child(Node* node) override;
-    bool tick(bool active, int traversal_count) override;
-};
-
-//===============================================================================================
-// --------------------------------------- Behavior Tree  ---------------------------------------
-//===============================================================================================
-
-class BehaviorTree {
-   public:
-    rclcpp::Node* ros2_node;
-    std::string config_filename;
-    std::vector<Node*> nodes;
-    Node* root;
-    int traversal_count;
-    std::unordered_map<std::string, int> active_ids;
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr active_actions_pub;
-    rclcpp::Publisher<behavior_tree_msgs::msg::GraphVizXdot>::SharedPtr graphviz_pub;
-    rclcpp::Publisher<behavior_tree_msgs::msg::GraphVizXdotCompressed>::SharedPtr compressed_pub;
-    bool first_tick;
-
-    void parse_config();
-    int count_tabs(std::string str);
-    std::string strip_space(std::string str);
-    std::string strip_brackets(std::string str);
-    std::vector<int> get_arguments(std::string str);
-    std::string get_graphviz();
-    // public:
-    BehaviorTree(std::string config_filename, rclcpp::Node* node);
-    bool tick();
-}; 
diff --git a/robot/ros_ws/src/behavior/behavior_tree/package.xml b/robot/ros_ws/src/behavior/behavior_tree/package.xml
deleted file mode 100644
index a99a5382d..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/package.xml
+++ /dev/null
@@ -1,22 +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>behavior_tree</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="jkeller2@andrew.cmu.edu">john</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>behavior_tree_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>rclcpp</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree.cpp b/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree.cpp
deleted file mode 100644
index 0661b90ae..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <behavior_tree/behavior_tree.hpp>
-
-namespace bt {
-
-// ==============================================================================
-// -------------------------------- Condition -----------------------------------
-// ==============================================================================
-
-std::string Condition::get_published_topic_name(std::string label) {
-    std::string topic_name = label;
-    std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower);
-    std::replace(topic_name.begin(), topic_name.end(), ' ', '_');
-    topic_name += "_success";
-    return topic_name;
-}
-
-Condition::Condition(std::string label, rclcpp::Node* node) {
-    // ros::NodeHandle nh;
-    this->label = label;
-    // success_pub = nh.advertise<std_msgs::Bool>(Condition::get_published_topic_name(label), 10);
-    success_pub =
-        node->create_publisher<std_msgs::msg::Bool>(Condition::get_published_topic_name(label), 10);
-    success.data = false;
-}
-
-std::string Condition::get_label() { return label; }
-
-void Condition::set(bool b) { success.data = b; }
-
-bool Condition::get() { return success.data; }
-
-void Condition::publish() { success_pub->publish(success); }
-
-// ==============================================================================
-// --------------------------------- Action -------------------------------------
-// ==============================================================================
-
-std::string Action::get_published_topic_name(std::string label) {
-    std::string topic_name = label;
-    std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower);
-    std::replace(topic_name.begin(), topic_name.end(), ' ', '_');
-    topic_name += "_status";
-    return topic_name;
-}
-
-std::string Action::get_subscribed_topic_name(std::string label) {
-    std::string topic_name = label;
-    std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower);
-    std::replace(topic_name.begin(), topic_name.end(), ' ', '_');
-    topic_name += "_active";
-    return topic_name;
-}
-
-Action::Action(std::string label, rclcpp::Node* node) { init(label, node); }
-
-void Action::init(std::string label, rclcpp::Node* node) {
-    // ros::NodeHandle nh;
-    active_sub = node->create_subscription<behavior_tree_msgs::msg::Active>(
-        Action::get_subscribed_topic_name(label), 10,
-        std::bind(&Action::active_callback, this, std::placeholders::_1));
-    status_pub = node->create_publisher<behavior_tree_msgs::msg::Status>(
-        Action::get_published_topic_name(label), 1);
-
-    active = false;
-    prev_active = false;
-    active_changed = false;
-    status.status = behavior_tree_msgs::msg::Status::FAILURE;
-
-    active_callback_func = NULL;
-    inactive_callback_func = NULL;
-
-    this->label = label;
-}
-
-void Action::active_callback(const behavior_tree_msgs::msg::Active::SharedPtr msg) {
-    bool active_changed = active != msg->active;
-
-    active = msg->active;
-    current_id = msg->id;
-
-    if (active_changed && active_callback_func != NULL) {
-        if (active)
-            active_callback_func();
-        else
-            inactive_callback_func();
-    }
-}
-
-void Action::set_success() { status.status = behavior_tree_msgs::msg::Status::SUCCESS; }
-
-void Action::set_running() { status.status = behavior_tree_msgs::msg::Status::RUNNING; }
-
-void Action::set_failure() { status.status = behavior_tree_msgs::msg::Status::FAILURE; }
-
-bool Action::is_active() {
-    active_changed = prev_active != active;
-    prev_active = active;
-    return active;
-}
-
-bool Action::active_has_changed() { return active_changed; }
-
-bool Action::is_success() { return status.status == behavior_tree_msgs::msg::Status::SUCCESS; }
-
-bool Action::is_running() { return status.status == behavior_tree_msgs::msg::Status::RUNNING; }
-
-bool Action::is_failure() { return status.status == behavior_tree_msgs::msg::Status::FAILURE; }
-
-std::string Action::get_label() { return label; }
-
-void Action::publish() {
-    status.id = current_id;
-    status_pub->publish(status);
-}
-
-};  // namespace bt 
diff --git a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree_implementation.cpp b/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree_implementation.cpp
deleted file mode 100644
index 2d0f2c0ba..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree_implementation.cpp
+++ /dev/null
@@ -1,755 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <ctype.h>
-
-#include <algorithm>
-#include <behavior_tree/behavior_tree_implementation.hpp>
-#include <boost/iostreams/copy.hpp>
-#include <boost/iostreams/filter/zlib.hpp>
-#include <boost/iostreams/filtering_streambuf.hpp>
-#include <fstream>
-#include <sstream>
-#include <typeinfo>
-
-//===============================================================================================
-// ------------------------------------- Base Node Class ----------------------------------------
-//===============================================================================================
-
-double Node::max_wait_time = 1.0;
-
-void Node::add_child(Node* node) { children.push_back(node); }
-
-//===============================================================================================
-// ------------------------------------ Control Flow Nodes  -------------------------------------
-//===============================================================================================
-
-FallbackNode::FallbackNode() {
-    label = "?";
-    is_active = false;
-    status = behavior_tree_msgs::msg::Status::FAILURE;
-}
-
-bool FallbackNode::tick(bool active, int traversal_count) {
-    uint8_t prev_status = status;
-    bool child_changed = false;
-
-    if (!active) {
-        // TODO: is ticking children necessary if inactive?
-        for (int i = 0; i < children.size(); i++)
-            child_changed |= children[i]->tick(false, traversal_count);
-    } else {
-        status = behavior_tree_msgs::msg::Status::FAILURE;
-        for (int i = 0; i < children.size(); i++) {
-            Node* child = children[i];
-            if (status == behavior_tree_msgs::msg::Status::FAILURE) {
-                child_changed |= child->tick(true, traversal_count);
-                status = child->status;
-            } else
-                child_changed |= child->tick(false, traversal_count);
-        }
-    }
-
-    bool status_changed = (status != prev_status) || child_changed;
-    bool active_changed = (is_active != active);
-
-    is_active = active;
-
-    return status_changed || active_changed;
-}
-
-SequenceNode::SequenceNode() {
-    label = "->";  //"\u2192"; // unicode arrow character
-    is_active = false;
-    status = behavior_tree_msgs::msg::Status::FAILURE;
-}
-
-bool SequenceNode::tick(bool active, int traversal_count) {
-    uint8_t prev_status = status;
-    bool child_changed = false;
-
-    if (!active) {
-        for (int i = 0; i < children.size(); i++)
-            child_changed |= children[i]->tick(false, traversal_count);
-
-    } else {
-        status = behavior_tree_msgs::msg::Status::SUCCESS;
-        for (int i = 0; i < children.size(); i++) {
-            Node* child = children[i];
-            if (status == behavior_tree_msgs::msg::Status::SUCCESS) {
-                child_changed |= child->tick(true, traversal_count);
-                status = child->status;
-            } else
-                child_changed |= child->tick(false, traversal_count);
-        }
-    }
-
-    bool status_changed = (status != prev_status) || child_changed;
-    bool active_changed = (is_active != active);
-
-    is_active = active;
-
-    return status_changed || active_changed;
-}
-
-ParallelNode::ParallelNode(int child_success_threshold) {
-    label = "||";
-    is_active = false;
-    status = behavior_tree_msgs::msg::Status::FAILURE;
-    this->child_success_threshold = child_success_threshold;
-}
-
-bool ParallelNode::tick(bool active, int traversal_count) {
-    uint8_t prev_status = status;
-    bool child_changed = false;
-
-    if (!active) {
-        for (int i = 0; i < children.size(); i++)
-            child_changed |= children[i]->tick(false, traversal_count);
-    } else {
-        int child_success_count = 0;
-        int child_failure_count = 0;
-        for (int i = 0; i < children.size(); i++) {
-            Node* child = children[i];
-            child_changed |= child->tick(true, traversal_count);
-
-            if (child->status == behavior_tree_msgs::msg::Status::SUCCESS)
-                child_success_count++;
-            else if (child->status == behavior_tree_msgs::msg::Status::FAILURE)
-                child_failure_count++;
-        }
-
-        if (child_success_count >= child_success_threshold)
-            status = behavior_tree_msgs::msg::Status::SUCCESS;
-        else if (child_failure_count >= (children.size() - child_success_threshold + 1))
-            status = behavior_tree_msgs::msg::Status::FAILURE;
-        else
-            status = behavior_tree_msgs::msg::Status::RUNNING;
-    }
-
-    bool status_changed = (status != prev_status) || child_changed;
-    bool active_changed = (is_active != active);
-
-    is_active = active;
-
-    return status_changed || active_changed;
-}
-
-//===============================================================================================
-// -------------------------------------- Execution Nodes  --------------------------------------
-//===============================================================================================
-
-ExecutionNode::ExecutionNode(rclcpp::Node* node) : node(node) {}
-
-void ExecutionNode::init_ros() {
-    init_publisher();
-    init_subscriber();
-}
-
-void ExecutionNode::set_status(uint8_t status) {
-    this->status = status;
-    status_modification_time = node->get_clock()->now();  // rclcpp::Time::now();
-}
-
-double ExecutionNode::get_status_age() {
-    // return (rclcpp::Time::now() - status_modification_time).toSec();
-    return (node->get_clock()->now() - status_modification_time).seconds();
-}
-
-ConditionNode::ConditionNode(std::string label, rclcpp::Node* node) : ExecutionNode(node) {
-    this->label = label;
-    status_modification_time = node->get_clock()->now();  // rclcpp::Time::now();
-    is_active = false;
-    status = behavior_tree_msgs::msg::Status::FAILURE;
-    current_traversal_count = -1;
-    callback_status_updated = false;
-}
-
-bool ConditionNode::tick(bool active, int traversal_count) {
-    // if(active)
-    //   ROS_INFO_STREAM("Condition " << label << " active");
-
-    uint8_t prev_status = status;
-    bool child_changed = false;
-
-    if (current_traversal_count != traversal_count)
-        current_traversal_count = traversal_count;
-    else if (is_active)
-        return false;  // TODO: is this correct?
-
-    if (!is_active && active && status == behavior_tree_msgs::msg::Status::FAILURE)
-        set_status(behavior_tree_msgs::msg::Status::FAILURE);
-
-    if (get_status_age() > Node::max_wait_time)
-        set_status(behavior_tree_msgs::msg::Status::FAILURE);
-
-    bool status_changed = (status != prev_status) || callback_status_updated;
-    bool active_changed = (is_active != active);
-
-    is_active = active;
-    callback_status_updated = false;
-
-    return status_changed || active_changed;
-}
-
-std::string ConditionNode::get_publisher_name() { return ""; }
-
-void ConditionNode::init_publisher() {}
-
-std::string ConditionNode::get_subscriber_name() {
-    std::string name = label;
-    std::transform(name.begin(), name.end(), name.begin(),
-                   [](unsigned char c) { return std::tolower(c); });
-    std::replace(name.begin(), name.end(), ' ', '_');
-    name = name + "_success";
-    // ROS_INFO_STREAM("condition get subscriber name: " << name << " " << label);
-    return name;
-}
-
-void ConditionNode::init_subscriber() {
-    // ros::NodeHandle nh;
-    subscriber = node->create_subscription<std_msgs::msg::Bool>(
-        get_subscriber_name(), 1, std::bind(&ConditionNode::callback, this, std::placeholders::_1));
-}
-
-void ConditionNode::callback(const std_msgs::msg::Bool::SharedPtr msg) {
-    // ROS_INFO_STREAM(label << ": " << (int)msg.data);
-    //  TODO: bug where if status is set here the changed flag won't be set correctly in tick
-    uint8_t prev_status = status;
-
-    if (msg->data)
-        set_status(behavior_tree_msgs::msg::Status::SUCCESS);
-    else
-        set_status(behavior_tree_msgs::msg::Status::FAILURE);
-
-    if (status != prev_status) callback_status_updated = true;
-}
-
-ActionNode::ActionNode(std::string label, rclcpp::Node* node) : ExecutionNode(node) {
-    this->label = label;
-    status_modification_time = node->get_clock()->now();  // rclcpp::Time::now();
-    is_active = false;
-    status = behavior_tree_msgs::msg::Status::FAILURE;
-    current_traversal_count = -1;
-    is_newly_active = false;
-    current_id = 0;
-    publisher_initialized = false;
-    callback_status_updated = false;
-}
-
-bool ActionNode::tick(bool active, int traversal_count) {
-    // if(active)
-    //   ROS_INFO_STREAM("Action " << label << " active");
-
-    uint8_t prev_status = status;
-
-    if (current_traversal_count != traversal_count)
-        current_traversal_count = traversal_count;
-    else if (is_active)
-        return false;  // TOOD: is this correct?
-
-    if (!is_active and active) {
-        set_status(behavior_tree_msgs::msg::Status::RUNNING);
-        is_newly_active = true;
-    } else
-        is_newly_active = false;
-
-    if (get_status_age() > Node::max_wait_time)
-        set_status(behavior_tree_msgs::msg::Status::FAILURE);
-
-    bool status_changed = (status != prev_status) || callback_status_updated;
-    bool active_changed = (is_active != active);
-
-    is_active = active;
-    callback_status_updated = false;
-
-    return status_changed || active_changed;
-}
-
-std::string ActionNode::get_publisher_name() {
-    std::string name = label;
-    std::transform(name.begin(), name.end(), name.begin(),
-                   [](unsigned char c) { return std::tolower(c); });
-    std::replace(name.begin(), name.end(), ' ', '_');
-    name = name + "_active";
-    // ROS_INFO_STREAM("action get publisher name: " << name << " " << label);
-    return name;
-}
-
-void ActionNode::init_publisher() {
-    publisher = node->create_publisher<behavior_tree_msgs::msg::Active>(get_publisher_name(), 1);
-    publisher_initialized = true;
-}
-
-std::string ActionNode::get_subscriber_name() {
-    std::string name = label;
-    std::transform(name.begin(), name.end(), name.begin(),
-                   [](unsigned char c) { return std::tolower(c); });
-    std::replace(name.begin(), name.end(), ' ', '_');
-    name = name + "_status";
-    // ROS_INFO_STREAM("action get subscriber name: " << name << " " << label);
-    return name;
-}
-
-void ActionNode::init_subscriber() {
-    subscriber = node->create_subscription<behavior_tree_msgs::msg::Status>(
-        get_subscriber_name(), 1, std::bind(&ActionNode::callback, this, std::placeholders::_1));
-}
-
-void ActionNode::callback(const behavior_tree_msgs::msg::Status::SharedPtr msg) {
-    uint8_t prev_status = status;
-    if (is_active) {
-        if (msg->id != current_id) {
-            // ROS_ERROR_STREAM(label << " Incorrect ID " << msg->id << " " << current_id << " " <<
-            // is_active);
-            std::cout << label << " Incorrect ID " << msg->id << " " << current_id << " "
-                      << is_active << std::endl;
-            return;
-        }
-        set_status(msg->status);
-    }
-
-    if (status != prev_status) callback_status_updated = true;
-}
-
-void ActionNode::publish_active_msg(int active_id) {
-    if (publisher_initialized) {
-        behavior_tree_msgs::msg::Active active_msg;
-        active_msg.active = is_active;
-        active_msg.id = active_id;
-        current_id = active_id;
-        publisher->publish(active_msg);
-    }
-}
-
-//===============================================================================================
-// -------------------------------------- Decorator Nodes  --------------------------------------
-//===============================================================================================
-
-void DecoratorNode::add_child(Node* node) {
-    if (children.size() == 0)
-        children.push_back(node);
-    else {
-        // ROS_ERROR_STREAM("A decorator node can only have one child.");
-        std::cout << "A decorator node can only have one child." << std::endl;
-        exit(1);
-    }
-}
-
-NotNode::NotNode() {
-    label = "!";
-    is_active = false;
-    status = behavior_tree_msgs::msg::Status::FAILURE;
-}
-
-void NotNode::add_child(Node* node) {
-    ConditionNode* condition_node = dynamic_cast<ConditionNode*>(node);
-    if (condition_node != NULL)
-        DecoratorNode::add_child(condition_node);
-    else {
-        // ROS_ERROR_STREAM("A not decorator node can only have a condition node as a child.");
-        std::cout << "A not decorator node can only have a condition node as a child." << std::endl;
-        exit(1);
-    }
-}
-
-bool NotNode::tick(bool active, int traversal_count) {
-    uint8_t prev_status = status;
-    bool child_changed = false;
-
-    if (children.size() > 0) {
-        Node* child = children[0];
-        child_changed |= child->tick(active, traversal_count);
-
-        if (child->status == behavior_tree_msgs::msg::Status::SUCCESS)
-            status = behavior_tree_msgs::msg::Status::FAILURE;
-        else if (child->status == behavior_tree_msgs::msg::Status::FAILURE)
-            status = behavior_tree_msgs::msg::Status::SUCCESS;
-    }
-
-    bool status_changed = (status != prev_status) || child_changed;
-    bool active_changed = (is_active != active);
-
-    is_active = active;
-
-    return status_changed || active_changed;
-}
-
-//===============================================================================================
-// --------------------------------------- Behavior Tree  ---------------------------------------
-//===============================================================================================
-
-BehaviorTree::BehaviorTree(std::string config_filename, rclcpp::Node* node) : ros2_node(node) {
-    this->config_filename = config_filename;
-    root = NULL;
-    traversal_count = 0;
-    first_tick = true;
-
-    active_actions_pub = node->create_publisher<std_msgs::msg::String>("active_actions", 1);
-    graphviz_pub = node->create_publisher<behavior_tree_msgs::msg::GraphVizXdot>("behavior_tree_graphviz", 1);
-    compressed_pub =
-        node->create_publisher<behavior_tree_msgs::msg::GraphVizXdotCompressed>("behavior_tree_graphviz_compressed", 1);
-
-    parse_config();
-    for (int i = 0; i < nodes.size(); i++) {
-        ActionNode* action_node = dynamic_cast<ActionNode*>(nodes[i]);
-        if (action_node != NULL) action_node->init_ros();
-        ConditionNode* condition_node = dynamic_cast<ConditionNode*>(nodes[i]);
-        if (condition_node != NULL) condition_node->init_ros();
-    }
-}
-
-void BehaviorTree::parse_config() {
-    std::ifstream in(config_filename);
-
-    // ROS_INFO_STREAM("Config file: " << config_filename);
-
-    if (in.is_open()) {
-        std::vector<Node*> nodes_worklist;
-        int prev_tabs = 0;
-
-        std::string line;
-        while (getline(in, line)) {
-            // skip empty lines
-            if (line.size() == 0) continue;
-
-            // ROS_INFO_STREAM("Line: " << line);
-
-            // initialization
-            int curr_tabs = count_tabs(line);
-            std::string label = strip_space(line);
-            Node* node = NULL;
-
-            // ROS_INFO_STREAM("curr tabs: " << curr_tabs << " label: " << label);
-
-            // create a node of the right type
-            if (label.compare(0, 2, "->") == 0)
-                node = new SequenceNode();
-            else if (label.compare(0, 1, "?") == 0)
-                node = new FallbackNode();
-            else if (label.compare(0, 2, "||") == 0) {
-                std::vector<int> arguments = get_arguments(label);
-                int child_success_threshold = 0;
-                if (arguments.size() > 0)
-                    child_success_threshold = arguments[0];
-                else {
-                    // ROS_ERROR_STREAM("Arguments not provided to parallel node");
-                    std::cout << "Arguments not provided to parallel node" << std::endl;
-                    exit(1);
-                }
-                node = new ParallelNode(child_success_threshold);
-            } else if (label.compare(0, 1, "(") == 0)
-                node = new ConditionNode(strip_brackets(label), ros2_node);
-            else if (label.compare(0, 1, "[") == 0) {
-                node = new ActionNode(strip_brackets(label), ros2_node);
-                active_ids[node->label] = 0;
-            } else if (label.compare(0, 1, "<") == 0) {
-                std::string decorator_label = strip_brackets(label);
-                if (decorator_label.compare(0, 1, "!") == 0) node = new NotNode();
-            }
-
-            if (node != NULL) {
-                nodes.push_back(node);
-
-                if (root == NULL) {
-                    root = node;
-                    nodes_worklist.push_back(node);
-                    continue;
-                }
-
-                if (curr_tabs == prev_tabs + 1) {
-                    Node* parent = nodes_worklist[nodes_worklist.size() - 1];
-                    parent->add_child(node);
-                } else {
-                    for (int i = 0; i < prev_tabs - curr_tabs + 1; i++) nodes_worklist.pop_back();
-                    Node* parent = nodes_worklist[nodes_worklist.size() - 1];
-                    parent->add_child(node);
-                }
-
-                nodes_worklist.push_back(node);
-                prev_tabs = curr_tabs;
-            }
-        }
-        in.close();
-    } else
-        std::cout << "Failed to open behavior tree config file: " << config_filename << std::endl;
-    // ROS_ERROR_STREAM("Failed to open behavior tree config file: " << config_filename);
-}
-
-int BehaviorTree::count_tabs(std::string str) {
-    int count = 0;
-    for (int i = 0; i < str.size(); i++)
-        if (str[i] == '\t')
-            count++;
-        else
-            break;
-
-    return count;
-}
-
-std::string BehaviorTree::strip_space(std::string str) {
-    std::string result = str;
-    while (result.size() > 0)
-        if (std::isspace(result[0]))
-            result.erase(0, 1);
-        else
-            break;
-
-    while (result.size() > 0)
-        if (std::isspace(result[result.size() - 1]))
-            result.erase(result.size() - 1, 1);
-        else
-            break;
-
-    return result;
-}
-
-std::string BehaviorTree::strip_brackets(std::string str) {
-    std::string result = str;
-    while (result.size() > 0)
-        if (result[0] == '(' || result[0] == '[' || result[0] == '<')
-            result.erase(0, 1);
-        else
-            break;
-
-    while (result.size() > 0)
-        if (result[result.size() - 1] == ')' || result[result.size() - 1] == ']' ||
-            result[result.size() - 1] == '>')
-            result.erase(result.size() - 1, 1);
-        else
-            break;
-
-    return result;
-}
-
-std::vector<int> BehaviorTree::get_arguments(std::string str) {
-    std::vector<int> arguments;
-
-    std::istringstream ss(str);
-    std::string first;
-    ss >> first;
-
-    int i;
-    while (ss >> i) arguments.push_back(i);
-
-    return arguments;
-}
-
-bool BehaviorTree::tick() {
-    bool changed = false || first_tick;
-    first_tick = false;
-
-    if (root != NULL) {
-        changed = root->tick(true, traversal_count);
-        traversal_count++;
-
-        std_msgs::msg::String active_actions;
-
-        std::unordered_map<std::string, Node*> unique_action_nodes;
-        for (int i = 0; i < nodes.size(); i++) {
-            ActionNode* action_node = dynamic_cast<ActionNode*>(nodes[i]);
-            if (action_node != NULL) {
-                if (unique_action_nodes.count(action_node->label) == 0 || action_node->is_active) {
-                    unique_action_nodes[action_node->label] = action_node;
-                    if (action_node->is_newly_active) active_ids[action_node->label]++;
-                }
-            }
-        }
-
-        for (auto it = unique_action_nodes.begin(); it != unique_action_nodes.end(); it++) {
-            ActionNode* action_node = dynamic_cast<ActionNode*>(it->second);
-            if (action_node != NULL) {
-                action_node->publish_active_msg(active_ids[it->first]);
-
-                if (it->second->is_active) active_actions.data += action_node->label + ", ";
-            } else {
-                std::cout << "action node is NULL, something went wrong." << std::endl;
-                // ROS_ERROR_STREAM("action node is NULL, something went wrong.");
-            }
-        }
-        if (active_actions.data.length() >= 2) {
-            active_actions.data.pop_back();
-            active_actions.data.pop_back();
-        }
-        active_actions_pub->publish(active_actions);
-    }
-
-    return changed;
-}
-
-std::string BehaviorTree::get_graphviz() {
-    std::string gv = "digraph G {\n";
-
-    std::vector<Node*> nodes_worklist;
-    nodes_worklist.push_back(root);
-    int count = 0;
-    std::unordered_map<Node*, std::string> node_names;
-
-    while (!nodes_worklist.empty()) {
-        Node* node = nodes_worklist.back();
-        nodes_worklist.pop_back();
-        std::string name = "node_" + std::to_string(count);
-        count++;
-
-        std::string style = "";
-        if (node->is_active) {
-            style += "penwidth=2 color=black style=filled ";
-            if (node->status == behavior_tree_msgs::msg::Status::SUCCESS)
-                style += "fillcolor=chartreuse";  // dark green
-            else if (node->status == behavior_tree_msgs::msg::Status::RUNNING)
-                style += "fillcolor=skyblue";  // dark blue
-            else if (node->status == behavior_tree_msgs::msg::Status::FAILURE)
-                style += "fillcolor=tomato";  // dark red
-        } else {
-            style += "penwidth=2 ";
-            if (node->status == behavior_tree_msgs::msg::Status::SUCCESS)
-                style += "color=chartreuse";  // light green
-            else if (node->status == behavior_tree_msgs::msg::Status::RUNNING)
-                style += "color=skyblue";  // light blue
-            else if (node->status == behavior_tree_msgs::msg::Status::FAILURE)
-                style += "color=tomato";  // light red
-        }
-
-        if (dynamic_cast<ConditionNode*>(node) != NULL) {
-            gv += "\t" + name + " [label=\"" + node->label + "\" " + style + "]\n";
-        } else if (dynamic_cast<ActionNode*>(node) != NULL) {
-            gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n";
-        } else if (dynamic_cast<FallbackNode*>(node) != NULL) {
-            gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n";
-        } else if (dynamic_cast<SequenceNode*>(node) != NULL) {
-            gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n";
-        } else if (dynamic_cast<ParallelNode*>(node) != NULL) {
-            gv += "\t" + name + " [label=\"" + node->label + "\" shape=parallelogram " + style +
-                  "]\n";
-        } else if (dynamic_cast<DecoratorNode*>(node) != NULL) {
-            gv += "\t" + name + " [label=\"" + node->label + "\" shape=diamond " + style + "]\n";
-        }
-
-        node_names[node] = name;
-
-        for (int i = 0; i < node->children.size(); i++) nodes_worklist.push_back(node->children[i]);
-    }
-
-    gv += "\n\tordering=out;\n\n";
-
-    nodes_worklist.push_back(root);
-    while (!nodes_worklist.empty()) {
-        Node* node = nodes_worklist.back();
-        nodes_worklist.pop_back();
-
-        for (int i = 0; i < node->children.size(); i++) {
-            gv += "\t" + node_names[node] + " -> " + node_names[node->children[i]] + ";\n";
-            nodes_worklist.push_back(node->children[i]);
-        }
-    }
-
-    gv += "}\n";
-
-    return gv;
-}
-
-std::string string_compress_encode(const std::string& data) {
-    std::stringstream compressed;
-    std::stringstream original;
-    original << data;
-    boost::iostreams::filtering_streambuf<boost::iostreams::input> out;
-    out.push(boost::iostreams::zlib_compressor());
-    out.push(original);
-    boost::iostreams::copy(out, compressed);
-
-    /**need to encode here **/
-    // std::string compressed_encoded = base64_encode(reinterpret_cast<const unsigned
-    // char*>(compressed.c_str()), compressed.length());
-
-    return compressed.str();  // compressed_encoded;
-}
-
-BehaviorTree* bt;
-
-behavior_tree_msgs::msg::GraphVizXdot graphviz_msg;
-behavior_tree_msgs::msg::GraphVizXdotCompressed compressed_msg;
-bool only_publish_on_change;
-void timer_callback() {  // const rclcpp::TimerEvent& te){
-    bool changed = bt->tick();
-    // ROS_INFO_STREAM("Changed: " << changed);
-
-    if (changed) {
-        graphviz_msg.header.stamp = bt->ros2_node->get_clock()->now();  // rclcpp::Time::now();
-        graphviz_msg.xdot.data = bt->get_graphviz();
-        compressed_msg.header.stamp = graphviz_msg.header.stamp;
-        compressed_msg.xdot_compressed.data = string_compress_encode(graphviz_msg.xdot.data);
-
-        if (only_publish_on_change) {
-            bt->graphviz_pub->publish(graphviz_msg);
-            bt->compressed_pub->publish(compressed_msg);
-        }
-    }
-
-    if (!only_publish_on_change) {
-        bt->graphviz_pub->publish(graphviz_msg);
-        bt->compressed_pub->publish(compressed_msg);
-    }
-}
-
-class BehaviorTreeNode : public rclcpp::Node {
-   private:
-    rclcpp::TimerBase::SharedPtr timer;
-
-   public:
-    BehaviorTreeNode() : Node("behavior_tree_node") {
-        // timer = this->create_timer(std::chrono::milliseconds(50),
-        // std::bind(&MinimalPublisher::timer_callback, this));
-
-        this->declare_parameter("config", "");
-        std::string config_filename = this->get_parameter("config").as_string();
-
-        this->declare_parameter("timeout", 1.0);
-        ::Node::max_wait_time = this->get_parameter("timeout").as_double();
-
-        bt = new BehaviorTree(config_filename, this);
-
-        timer = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(50),
-                                     &timer_callback);
-    }
-};
-
-int main(int argc, char** argv) {
-    /*
-    ros::init(argc, argv, "behavior_tree");
-
-    ros::NodeHandle nh;
-    ros::NodeHandle pnh("~");
-
-    std::string config_filename = pnh.param("config", std::string(""));
-    only_publish_on_change = pnh.param("only_publish_on_change", false);
-    Node::max_wait_time = pnh.param("timeout", 1.0);
-
-    rclcpp::Timer timer = nh.createTimer(ros::Duration(0.05), timer_callback);
-
-    bt = new BehaviorTree(config_filename);
-
-    ros::spin();
-    */
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<BehaviorTreeNode>());
-    rclcpp::shutdown();
-
-    return 0;
-} 
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/.gitignore b/robot/ros_ws/src/behavior/behavior_tree_example/.gitignore
deleted file mode 100644
index e6458330d..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-*~
-*.pyc
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/CMakeLists.txt b/robot/ros_ws/src/behavior/behavior_tree_example/CMakeLists.txt
deleted file mode 100644
index 21d815efb..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/CMakeLists.txt
+++ /dev/null
@@ -1,69 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(behavior_tree_example)
-
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++14
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(behavior_tree_msgs REQUIRED)
-find_package(behavior_tree REQUIRED)
-find_package(tf2 REQUIRED)
-
-#add_executable(behavior_tree_example src/behavior_tree_example.cpp)
-#ament_target_dependencies(talker rclcpp std_msgs)
-
-add_executable(behavior_tree_example src/behavior_tree_example.cpp)
-ament_target_dependencies(behavior_tree_example rclcpp std_msgs geometry_msgs visualization_msgs nav_msgs behavior_tree_msgs behavior_tree tf2)
-
-add_executable(robot_node src/robot_node.cpp)
-ament_target_dependencies(robot_node rclcpp std_msgs geometry_msgs visualization_msgs nav_msgs behavior_tree_msgs tf2)
-
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME}/
-  )
-
-install(DIRECTORY
-  config
-  DESTINATION share/${PROJECT_NAME}/
-  )
-
-install(DIRECTORY
-  rviz
-  DESTINATION share/${PROJECT_NAME}/
-  )
-
-install(TARGETS
-  behavior_tree_example
-  robot_node
-  DESTINATION lib/${PROJECT_NAME})
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example.tree
deleted file mode 100644
index 4903ecf67..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example.tree
+++ /dev/null
@@ -1,22 +0,0 @@
-?
-	->
-		(Visited Destination)
-		(At Home)
-		?
-			(On Ground)
-			[Land]
-	->
-		?
-			(In No Fly Zone)
-			(At Flight Altitude)
-			[Takeoff]
-		?
-			(In Fly Zone)
-			(On Ground)
-			[Land]
-		?
-			(Visited Destination)
-			[Go To Destination]
-		?
-			(At Home)
-			[Go Home]
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example2.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example2.tree
deleted file mode 100644
index 535704900..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example2.tree
+++ /dev/null
@@ -1,27 +0,0 @@
-|| 2
-	?
-		(At Home)
-		[Spin]
-	?
-		->
-			(Visited Destination)
-			(At Home)
-			?
-				(On Ground)
-				[Land]
-		->
-			?
-				(In No Fly Zone)
-				(At Flight Altitude)
-				[Takeoff]
-			?
-				<!>
-					(In No Fly Zone)
-				(On Ground)
-				[Land]
-			?
-				(Visited Destination)
-				[Go To Destination]
-			?
-				(At Home)
-				[Go Home]
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example3.tree
deleted file mode 100644
index 19229d0a4..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3.tree
+++ /dev/null
@@ -1,12 +0,0 @@
-|| 2
-	?
-		(At Home)
-		[Spin]
-	?
-		->
-			(Visited Destination)
-			(At Home)
-			?
-				(On Ground)
-				[Land]
-		include $(find behavior_tree_example)/config/example3_subtree.tree
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_nested_subtree.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_nested_subtree.tree
deleted file mode 100644
index 923908277..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_nested_subtree.tree
+++ /dev/null
@@ -1,3 +0,0 @@
-?
-	(Visited Destination)
-	[Go To Destination]
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_subtree.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_subtree.tree
deleted file mode 100644
index a311d1d93..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_subtree.tree
+++ /dev/null
@@ -1,14 +0,0 @@
-->
-	?
-		(In No Fly Zone)
-		(At Flight Altitude)
-		[Takeoff]
-	?
-		<!>
-			(In No Fly Zone)
-		(On Ground)
-		[Land]
-	include $(find behavior_tree_example)/config/example3_nested_subtree.tree
-	?
-		(At Home)
-		[Go Home]
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree.tree
deleted file mode 100644
index 84e185046..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree.tree
+++ /dev/null
@@ -1,37 +0,0 @@
-?
-	->
-		(User Pressed Estop)
-		?
-			(Not Stopped)
-			[Stop]
-	->
-		(User Pressed Return Home Button)
-		?
-			(At Home)
-			[Go To Home]
-	->
-		(User Pressed Start Button)
-		?
-			(Not Stopped)
-			[Initiate]
-		?
-			(Not Stuck)
-			[Recovery 1]
-			[Recovery 2]
-			[Recovery 3]
-		?
-			(Low Battery)
-			->
-				?
-					(Good Comms)
-					[Explore]
-				?
-					(In Comms)
-					[Drop Comms]
-				[Return to Comms]
-		?
-			(At Home)
-			[Go To Home]
-		?
-			(Not Stopped)
-			[Stop]
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree_basic.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree_basic.tree
deleted file mode 100644
index 5d93cae6c..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree_basic.tree
+++ /dev/null
@@ -1,25 +0,0 @@
-?
-	->
-		(User Pressed Estop)
-		?
-			(Not Stopped)
-			[Stop]
-	->
-		(User Pressed Return Home Button)
-		?
-			(At Home)
-			[Go To Home]
-	->
-		(User Pressed Resume)
-		?
-			(Stopped)
-			[Initiate]
-		?
-			(Low Battery)
-			[Explore]
-		?
-			(At Home)
-			[Go To Home]
-		?
-			(Not Stopped)
-			[Stop]
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/launch/behavior_tree_example.xml b/robot/ros_ws/src/behavior/behavior_tree_example/launch/behavior_tree_example.xml
deleted file mode 100644
index fb5ba46ab..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/launch/behavior_tree_example.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-
-  <!-- Behavior Tree -->
-  <node pkg="behavior_tree" exec="behavior_tree_implementation">
-    <param name="config" value="$(find-pkg-share behavior_tree_example)/config/example.tree" />
-    <param name="timeout" value="1.0" />
-  </node>
-
-  <!-- Visualization -->
-  <node pkg="rviz2" exec="rviz2"
-    args="-d $(find-pkg-share behavior_tree_example)/rviz/behavior_tree_example.rviz" />
-  <node pkg="rqt_gui" exec="rqt_gui"
-    args="--perspective-file $(find-pkg-share behavior_tree_example)/rviz/behavior_tree.perspective" />
-
-  <!-- Robot Simluator -->
-  <node pkg="behavior_tree_example" exec="robot_node" output="screen">
-  </node>
-
-  <!-- Action and Condition Node -->
-  <node pkg="behavior_tree_example" exec="behavior_tree_example" output="screen">
-  </node>
-
-</launch>
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/package.xml b/robot/ros_ws/src/behavior/behavior_tree_example/package.xml
deleted file mode 100644
index 56aa8345d..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/package.xml
+++ /dev/null
@@ -1,27 +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>behavior_tree_example</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="jkeller2@andrew.cmu.edu">john</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>rclcpp</depend>
-  <depend>behavior_tree_msgs</depend>
-  <depend>behavior_tree</depend>
-  <depend>std_msgs</depend>
-  <depend>visualization_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>tf2</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree.perspective b/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree.perspective
deleted file mode 100644
index ea05a4467..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree.perspective
+++ /dev/null
@@ -1,62 +0,0 @@
-{
-  "keys": {},
-  "groups": {
-    "mainwindow": {
-      "keys": {
-        "geometry": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000006400000064000005600000038e0000006400000089000005600000038e00000000000000000a000000006400000089000005600000038e')",
-          "type": "repr(QByteArray.hex)",
-          "pretty-print": "     d d `   d   `        d   `  "
-        },
-        "state": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd0000000100000003000004fd000002dafc0100000001fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000000004fd0000017300ffffff000004fd0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
-          "type": "repr(QByteArray.hex)",
-          "pretty-print": "                 Brqt_behavior_tree__PyConsole__1__                            6MinimizedDockWidgetsToolbar        "
-        }
-      },
-      "groups": {
-        "toolbar_areas": {
-          "keys": {
-            "MinimizedDockWidgetsToolbar": {
-              "repr": "8",
-              "type": "repr"
-            }
-          },
-          "groups": {}
-        }
-      }
-    },
-    "pluginmanager": {
-      "keys": {
-        "running-plugins": {
-          "repr": "{'rqt_behavior_tree/PyConsole': [1]}",
-          "type": "repr"
-        }
-      },
-      "groups": {
-        "plugin__rqt_behavior_tree__PyConsole__1": {
-          "keys": {},
-          "groups": {
-            "dock_widget__": {
-              "keys": {
-                "dock_widget_title": {
-                  "repr": "''",
-                  "type": "repr"
-                },
-                "dockable": {
-                  "repr": "True",
-                  "type": "repr"
-                },
-                "parent": {
-                  "repr": "None",
-                  "type": "repr"
-                }
-              },
-              "groups": {}
-            }
-          }
-        }
-      }
-    }
-  }
-}
\ No newline at end of file
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree_example.rviz b/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree_example.rviz
deleted file mode 100644
index 1159cebd2..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree_example.rviz
+++ /dev/null
@@ -1,210 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-      Splitter Ratio: 0.5
-    Tree Height: 365
-  - 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
-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
-    - Alpha: 1
-      Class: rviz_default_plugins/PointStamped
-      Color: 0; 0; 255
-      Enabled: true
-      History Length: 1
-      Name: Destination
-      Radius: 0.4000000059604645
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /destination
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/PointStamped
-      Color: 0; 255; 0
-      Enabled: true
-      History Length: 1
-      Name: Home
-      Radius: 0.4000000059604645
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /home
-      Value: true
-    - 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: true
-      Keep: 1
-      Name: Robot 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: perception/state_estimation/odometry
-      Value: true
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: Markers
-      Namespaces:
-        destination_label: true
-        home_label: true
-        no_fly_zone: true
-        no_fly_zone_label: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /no_fly_zone_vis
-      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: 13.15915298461914
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 5.043847560882568
-        Y: -0.7309801578521729
-        Z: 1.8234843015670776
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.06039784476161003
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 1.57039475440979
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: true
-  Height: 846
-  Hide Left Dock: true
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002f4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b0000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1200
-  X: 1182
-  Y: 227
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/src/behavior_tree_example.cpp b/robot/ros_ws/src/behavior/behavior_tree_example/src/behavior_tree_example.cpp
deleted file mode 100644
index 9f7ff0607..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/src/behavior_tree_example.cpp
+++ /dev/null
@@ -1,312 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <behavior_tree/behavior_tree.hpp>  // Include the behavior tree library containing the Action and Condition classes.
-#include <geometry_msgs/msg/point_stamped.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <std_msgs/msg/float32.hpp>
-#include <std_msgs/msg/string.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-
-#include "tf2/LinearMath/Matrix3x3.h"
-#include "tf2/LinearMath/Quaternion.h"
-#include "tf2/LinearMath/Vector3.h"
-
-class BehaviorTreeExample : public rclcpp::Node {
-   private:
-    bool got_robot_odom, got_home, got_destination, got_no_fly_zone, got_no_fly_zone_radius;
-    nav_msgs::msg::Odometry robot_odom;
-    float flight_z, ground_z;
-    geometry_msgs::msg::PointStamped home, destination;
-    float acceptance_radius;
-    geometry_msgs::msg::PointStamped no_fly_zone;
-    float no_fly_zone_radius;
-
-    // Condition variables
-    bt::Condition* at_flight_altitude_condition;
-    bt::Condition* on_ground_condition;
-    bt::Condition* visited_destination_condition;
-    bt::Condition* at_home_condition;
-    bt::Condition* in_fly_zone_condition;
-    bt::Condition* in_no_fly_zone_condition;
-
-    // Action variables
-    bt::Action* land_action;
-    bt::Action* takeoff_action;
-    bt::Action* go_to_destination_action;
-    bt::Action* go_home_action;
-
-    // subscribers
-    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr robot_odom_sub;
-    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr home_sub;
-    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr destination_sub;
-    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr no_fly_zone_sub;
-    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr no_fly_zone_radius_sub;
-
-    // publishers
-    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr vel_cmd_pub;
-
-    // timers
-    rclcpp::TimerBase::SharedPtr timer;
-
-   public:
-    BehaviorTreeExample() : Node("behavior_tree_example") {
-        on_ground_condition = new bt::Condition("On Ground", this);
-        at_flight_altitude_condition = new bt::Condition("At Flight Altitude", this);
-        visited_destination_condition = new bt::Condition("Visited Destination", this);
-        at_home_condition = new bt::Condition("At Home", this);
-        in_fly_zone_condition = new bt::Condition("In Fly Zone", this);
-        in_no_fly_zone_condition = new bt::Condition("In No Fly Zone", this);
-
-        land_action = new bt::Action("Land", this);
-        // Do the same for the "Takeoff" action.
-        takeoff_action =
-            new bt::Action("Takeoff", this, &BehaviorTreeExample::takeoff_active_callback, this,
-                           &BehaviorTreeExample::takeoff_inactive_callback, this);
-        go_to_destination_action = new bt::Action("Go To Destination", this);
-        go_home_action = new bt::Action("Go Home", this);
-
-        // init subscribers
-        robot_odom_sub = this->create_subscription<nav_msgs::msg::Odometry>(
-            "odometry", 10,
-            std::bind(&BehaviorTreeExample::robot_odom_callback, this, std::placeholders::_1));
-        home_sub = this->create_subscription<geometry_msgs::msg::PointStamped>(
-            "home", 10,
-            std::bind(&BehaviorTreeExample::home_callback, this, std::placeholders::_1));
-        destination_sub = this->create_subscription<geometry_msgs::msg::PointStamped>(
-            "destination", 10,
-            std::bind(&BehaviorTreeExample::destination_callback, this, std::placeholders::_1));
-
-        no_fly_zone_sub = this->create_subscription<geometry_msgs::msg::PointStamped>(
-            "no_fly_zone", 10,
-            std::bind(&BehaviorTreeExample::no_fly_zone_callback, this, std::placeholders::_1));
-        no_fly_zone_radius_sub = this->create_subscription<std_msgs::msg::Float32>(
-            "no_fly_zone_radius", 10,
-            std::bind(&BehaviorTreeExample::no_fly_zone_radius_callback, this,
-                      std::placeholders::_1));
-
-        // init publishers
-        vel_cmd_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>("/vel_cmd", 10);
-
-        // initialization
-        got_robot_odom = false;
-        got_home = false;
-        got_destination = false;
-        got_no_fly_zone = false;
-        got_no_fly_zone_radius = false;
-        flight_z = 0.99;
-        ground_z = 0.01;
-        acceptance_radius = 0.01;
-
-        // init timer
-        timer = this->create_wall_timer(std::chrono::milliseconds(50),
-                                        std::bind(&BehaviorTreeExample::timer_callback, this));
-    }
-
-    void timer_callback() {
-        if (!got_robot_odom || !got_home || !got_destination) return;
-
-        // The condition should be success if the robot is above the flying height's z level,
-        // and failure otherwise.
-        at_flight_altitude_condition->set(robot_odom.pose.pose.position.z > flight_z);
-        at_flight_altitude_condition->publish();
-
-        // Set the condition to success if the robot is below the ground's z level,
-        // or to failure if the robot is above the ground's z level.
-        on_ground_condition->set(robot_odom.pose.pose.position.z < ground_z);
-        on_ground_condition->publish();
-
-        // The at home condition should be set to success if the robot is within
-        // a threshold distance of the home location and failure otherwise.
-        at_home_condition->set(distance(robot_odom, home) < acceptance_radius);
-        at_home_condition->publish();
-
-        if (got_no_fly_zone && got_no_fly_zone_radius) {
-            float distance = sqrt(pow(robot_odom.pose.pose.position.x - no_fly_zone.point.x, 2) +
-                                  pow(robot_odom.pose.pose.position.y - no_fly_zone.point.y, 2));
-
-            // Check if the robot is with the radius of the no fly zone point.
-            bool in_no_fly_zone = distance <= no_fly_zone_radius;
-
-            in_no_fly_zone_condition->set(in_no_fly_zone);  // Set the condition.
-            in_no_fly_zone_condition->publish();            // Publish.
-
-            in_fly_zone_condition->set(!in_no_fly_zone);  // Set the condition.
-            in_fly_zone_condition->publish();             // Publish.
-        }
-
-        // The visited destination condition should be set to success (true) when we first
-        // get close enough to the destination point and remain true forever.
-        float destination_distance = distance(robot_odom, destination);
-        if (!visited_destination_condition
-                 ->get())  // Only changed the condition value if it hasn't been set to true yet.
-            visited_destination_condition->set(destination_distance < acceptance_radius);
-        visited_destination_condition->publish();  // Publish the condition.
-
-        if (land_action
-                ->is_active()) {  // Make sure the robot only lands if the land action is active.
-            geometry_msgs::msg::TwistStamped vel_cmd;
-            vel_cmd.header.stamp = this->get_clock()->now();
-            vel_cmd.header.frame_id = "world";
-
-            if (on_ground_condition->get()) {
-                // Set the status to success if the robot has landed.
-                land_action->set_success();
-                // Command zero velocity when the robot is done landing.
-                vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0;
-            } else {
-                // Set the status to running while the robot is landing.
-                land_action->set_running();
-                // Command downward velocity to land.
-                vel_cmd.twist.linear.z = -0.3;
-            }
-
-            vel_cmd_pub->publish(vel_cmd);
-
-            // Publish the land action's status.
-            land_action->publish();
-        }
-
-        if (go_home_action->is_active()) {  // Make sure the robot tries to go home when the go home
-                                            // action is active.
-            geometry_msgs::msg::TwistStamped vel_cmd;
-            vel_cmd.header.stamp = this->get_clock()->now();
-            vel_cmd.header.frame_id = "world";
-
-            if (at_home_condition->get()) {
-                // Set the status to "SUCCESS" if the robot has reached the home location
-                go_home_action->set_success();
-                // Command zero velocity when the robot is done traveling home.
-                vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0;
-            } else {
-                // Set the status to "RUNNING" while the robot is moving toward the home location.
-                go_home_action->set_running();
-                // Command a velocity towards the home location.
-                tf2::Vector3 vel(home.point.x - robot_odom.pose.pose.position.x,
-                                 home.point.y - robot_odom.pose.pose.position.y, 0);
-                vel = 0.3 * vel.normalized();
-                vel_cmd.twist.linear.x = vel.x();
-                vel_cmd.twist.linear.y = vel.y();
-                vel_cmd.twist.linear.z = vel.z();
-            }
-
-            vel_cmd_pub->publish(vel_cmd);
-
-            // Publish the go home action's status
-            go_home_action->publish();
-        }
-
-        // Make sure the robot only tries to go to the destination while the go to destination
-        // action is active
-        if (go_to_destination_action->is_active()) {
-            geometry_msgs::msg::TwistStamped vel_cmd;
-            vel_cmd.header.stamp = this->get_clock()->now();
-            vel_cmd.header.frame_id = "world";
-
-            if (destination_distance < acceptance_radius) {
-                // Set the status to success if the robot has reached the destination location.
-                go_to_destination_action->set_success();
-                // Command zero velocity when the robot is done traveling to the destination.
-                vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0;
-            } else {
-                // Set the status to running while the robot is moving towards the destination
-                // location.
-                go_to_destination_action->set_running();
-                // Command a velocity towards the destination location.
-                tf2::Vector3 vel(destination.point.x - robot_odom.pose.pose.position.x,
-                                 destination.point.y - robot_odom.pose.pose.position.y, 0);
-                vel = 0.3 * vel.normalized();
-                vel_cmd.twist.linear.x = vel.x();
-                vel_cmd.twist.linear.y = vel.y();
-                vel_cmd.twist.linear.z = vel.z();
-            }
-
-            vel_cmd_pub->publish(vel_cmd);
-
-            // Publish the go to destination action's status.
-            go_to_destination_action->publish();
-        }
-    }
-
-    void takeoff_active_callback() {
-        geometry_msgs::msg::TwistStamped vel_cmd;
-        vel_cmd.header.stamp = this->get_clock()->now();
-        vel_cmd.header.frame_id = "world";
-        // Set the status to "RUNNING" while the take off is in progress.
-        takeoff_action->set_running();
-        // Command upward velocity to take off.
-        vel_cmd.twist.linear.z = 0.3;
-
-        vel_cmd_pub->publish(vel_cmd);
-    }
-
-    void takeoff_inactive_callback() {
-        geometry_msgs::msg::TwistStamped vel_cmd;
-        vel_cmd.header.stamp = this->get_clock()->now();
-        vel_cmd.header.frame_id = "world";
-        // Set the status to "RUNNING" while the take off is in progress.
-        takeoff_action->set_running();
-        // Command upward velocity to take off.
-        vel_cmd.twist.linear.z = 0.0;
-
-        vel_cmd_pub->publish(vel_cmd);
-    }
-
-    void robot_odom_callback(const nav_msgs::msg::Odometry::SharedPtr robot_odom) {
-        got_robot_odom = true;
-        this->robot_odom = *robot_odom;
-    }
-
-    void home_callback(const geometry_msgs::msg::PointStamped::SharedPtr home) {
-        got_home = true;
-        this->home = *home;
-    }
-
-    void destination_callback(const geometry_msgs::msg::PointStamped::SharedPtr destination) {
-        got_destination = true;
-        this->destination = *destination;
-    }
-
-    void no_fly_zone_callback(const geometry_msgs::msg::PointStamped::SharedPtr no_fly_zone) {
-        got_no_fly_zone = true;
-        this->no_fly_zone = *no_fly_zone;
-    }
-
-    void no_fly_zone_radius_callback(const std_msgs::msg::Float32::SharedPtr no_fly_zone_radius) {
-        got_no_fly_zone_radius = true;
-        this->no_fly_zone_radius = no_fly_zone_radius->data;
-    }
-
-    float distance(nav_msgs::msg::Odometry odom, geometry_msgs::msg::PointStamped point) {
-        float distance = sqrt(pow(odom.pose.pose.position.x - point.point.x, 2) +
-                              pow(odom.pose.pose.position.y - point.point.y, 2));
-        return distance;
-    }
-};
-
-int main(int argc, char* argv[]) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<BehaviorTreeExample>());
-    rclcpp::shutdown();
-    return 0;
-} 
diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/src/robot_node.cpp b/robot/ros_ws/src/behavior/behavior_tree_example/src/robot_node.cpp
deleted file mode 100644
index 4d711bc2a..000000000
--- a/robot/ros_ws/src/behavior/behavior_tree_example/src/robot_node.cpp
+++ /dev/null
@@ -1,229 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <rclcpp/rclcpp.hpp>
-
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <geometry_msgs/msg/point_stamped.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <std_msgs/msg/float32.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-#include "tf2/LinearMath/Quaternion.h"
-#include "tf2/LinearMath/Matrix3x3.h"
-
-class RobotNode : public rclcpp::Node
-{
-private:
-  nav_msgs::msg::Odometry odom;
-  geometry_msgs::msg::TwistStamped vel_cmd, ang_vel_cmd;
-  rclcpp::Time last_execute_time;
-  geometry_msgs::msg::PointStamped home, destination, no_fly_zone;
-  std_msgs::msg::Float32 no_fly_zone_radius;
-  visualization_msgs::msg::Marker no_fly_zone_marker, no_fly_zone_label, home_label, destination_label;
-  visualization_msgs::msg::MarkerArray marker_array;
-  
-  // subscribers
-  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr vel_cmd_sub;
-  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr ang_vel_cmd_sub;
-
-  // publishers
-  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr robot_odom_pub;
-  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr home_pub;
-  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr destination_pub;
-  rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr no_fly_zone_pub;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr no_fly_zone_radius_pub;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr no_fly_zone_vis_pub;
-
-  rclcpp::TimerBase::SharedPtr timer;
-  
-public:
-  RobotNode()
-    : Node("robot_node"){
-    // initialization
-    home.header.frame_id = "world";
-    home.point.x = 0;
-    home.point.y = 0;
-    home.point.z = 0;
-
-    destination.header.frame_id = "world";
-    destination.point.x = 10;
-    destination.point.y = 0;
-    destination.point.z = 0;
-
-    no_fly_zone.header.frame_id = "world";
-    no_fly_zone.point.x = 5;
-    no_fly_zone.point.y = 0;
-    no_fly_zone.point.z = 0;
-
-    no_fly_zone_radius.data = 2.f;
-
-    float marker_height = 10;
-    no_fly_zone_marker.header.frame_id = "world";
-    no_fly_zone_marker.ns = "no_fly_zone";
-    no_fly_zone_marker.id = 0;
-    no_fly_zone_marker.type = visualization_msgs::msg::Marker::CYLINDER;
-    no_fly_zone_marker.action = visualization_msgs::msg::Marker::ADD;
-    no_fly_zone_marker.pose.orientation.w = 1;
-    no_fly_zone_marker.pose.position.x = no_fly_zone.point.x;
-    no_fly_zone_marker.pose.position.y = no_fly_zone.point.y;
-    no_fly_zone_marker.pose.position.z = marker_height/2;
-    no_fly_zone_marker.scale.x = 2*no_fly_zone_radius.data;
-    no_fly_zone_marker.scale.y = 2*no_fly_zone_radius.data;
-    no_fly_zone_marker.scale.z = marker_height;
-    no_fly_zone_marker.color.r = 1;
-    no_fly_zone_marker.color.g = 0;
-    no_fly_zone_marker.color.b = 0;
-    no_fly_zone_marker.color.a = 0.3;
-  
-    no_fly_zone_label.header.frame_id = "world";
-    no_fly_zone_label.ns = "no_fly_zone_label";
-    no_fly_zone_label.id = 0;
-    no_fly_zone_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-    no_fly_zone_label.action = visualization_msgs::msg::Marker::ADD;
-    no_fly_zone_label.pose.orientation.w = 1;
-    no_fly_zone_label.pose.position.x = no_fly_zone.point.x;
-    no_fly_zone_label.pose.position.y = no_fly_zone.point.y;
-    no_fly_zone_label.pose.position.z = marker_height/2;
-    no_fly_zone_label.scale.x = 1;//2*no_fly_zone_radius.data;
-    no_fly_zone_label.scale.y = 1;//2*no_fly_zone_radius.data;
-    no_fly_zone_label.scale.z = 1;//marker_height;
-    no_fly_zone_label.color.r = 1;
-    no_fly_zone_label.color.g = 1;
-    no_fly_zone_label.color.b = 1;
-    no_fly_zone_label.color.a = 1;
-    no_fly_zone_label.text = "No Fly Zone";
-
-    home_label.header.frame_id = "world";
-    home_label.ns = "home_label";
-    home_label.id = 0;
-    home_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-    home_label.action = visualization_msgs::msg::Marker::ADD;
-    home_label.pose.orientation.w = 1;
-    home_label.pose.position.x = home.point.x;
-    home_label.pose.position.y = home.point.y;
-    home_label.pose.position.z = home.point.z;
-    home_label.scale.x = 0.5;
-    home_label.scale.y = 0.5;
-    home_label.scale.z = 0.5;
-    home_label.color.r = 1;
-    home_label.color.g = 1;
-    home_label.color.b = 1;
-    home_label.color.a = 1;
-    home_label.text = "Home";
-  
-    destination_label.header.frame_id = "world";
-    destination_label.ns = "destination_label";
-    destination_label.id = 0;
-    destination_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-    destination_label.action = visualization_msgs::msg::Marker::ADD;
-    destination_label.pose.orientation.w = 1;
-    destination_label.pose.position.x = destination.point.x;
-    destination_label.pose.position.y = destination.point.y;
-    destination_label.pose.position.z = destination.point.z;
-    destination_label.scale.x = 0.5;
-    destination_label.scale.y = 0.5;
-    destination_label.scale.z = 0.5;
-    destination_label.color.r = 1;
-    destination_label.color.g = 1;
-    destination_label.color.b = 1;
-    destination_label.color.a = 1;
-    destination_label.text = "Destination";
-
-    marker_array.markers.push_back(no_fly_zone_marker);
-    marker_array.markers.push_back(no_fly_zone_label);
-    marker_array.markers.push_back(home_label);
-    marker_array.markers.push_back(destination_label);
-  
-    odom.header.frame_id = "world";
-    odom.child_frame_id = "world";
-    odom.pose.pose.orientation.w = 1;
-  
-    last_execute_time = this->get_clock()->now();
-
-    
-    // init subscribers
-    vel_cmd_sub = this->create_subscription<geometry_msgs::msg::TwistStamped>("vel_cmd", 10, std::bind(&RobotNode::vel_cmd_callback, this, std::placeholders::_1));
-    ang_vel_cmd_sub = this->create_subscription<geometry_msgs::msg::TwistStamped>("ang_vel_cmd", 10, std::bind(&RobotNode::ang_vel_cmd_callback, this, std::placeholders::_1));
-
-    // init publishers
-    robot_odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("odometry", 10);
-    home_pub = this->create_publisher<geometry_msgs::msg::PointStamped>("home", 10);
-    destination_pub = this->create_publisher<geometry_msgs::msg::PointStamped>("destination", 10);
-    no_fly_zone_pub = this->create_publisher<geometry_msgs::msg::PointStamped>("no_fly_zone", 10);
-    no_fly_zone_radius_pub = this->create_publisher<std_msgs::msg::Float32>("no_fly_zone_radius", 10);
-    no_fly_zone_vis_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("no_fly_zone_vis", 10);
-
-    // init timer
-    timer = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&RobotNode::timer_callback, this));
-  }
-
-  void timer_callback(){
-    // keep track of time
-    rclcpp::Time now = this->get_clock()->now();
-    double dt = (now - last_execute_time).seconds();
-    last_execute_time = now;
-
-    // update the robot's position based on the velocity command
-    odom.pose.pose.position.x += vel_cmd.twist.linear.x*dt;
-    odom.pose.pose.position.y += vel_cmd.twist.linear.y*dt;
-    odom.pose.pose.position.z += vel_cmd.twist.linear.z*dt;
-
-  
-    // update the robot's orientation based on the velocity command
-    double roll, pitch, yaw;
-    tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
-    tf2::Matrix3x3 m(q);
-    m.getRPY(roll, pitch, yaw);
-    roll += ang_vel_cmd.twist.angular.x*dt;
-    pitch += ang_vel_cmd.twist.angular.y*dt;
-    yaw += ang_vel_cmd.twist.angular.z*dt;
-    q.setRPY(roll, pitch, yaw);
-    odom.pose.pose.orientation.x = q.x();
-    odom.pose.pose.orientation.y = q.y();
-    odom.pose.pose.orientation.z = q.z();
-    odom.pose.pose.orientation.w = q.w();
-
-    // publish
-    odom.header.stamp = home.header.stamp = destination.header.stamp = no_fly_zone.header.stamp = no_fly_zone_marker.header.stamp = this->get_clock()->now();
-    robot_odom_pub->publish(odom);
-    home_pub->publish(home);
-    destination_pub->publish(destination);
-    no_fly_zone_pub->publish(no_fly_zone);
-    no_fly_zone_radius_pub->publish(no_fly_zone_radius);
-    no_fly_zone_vis_pub->publish(marker_array);//no_fly_zone_marker);
-  }
-
-  void vel_cmd_callback(const geometry_msgs::msg::TwistStamped::SharedPtr twist){
-    vel_cmd = *twist;
-  }
-
-  void ang_vel_cmd_callback(const geometry_msgs::msg::TwistStamped::SharedPtr twist){
-    ang_vel_cmd = *twist;
-  }
-};
-
-int main(int argc, char * argv[])
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<RobotNode>());
-  rclcpp::shutdown();
-  return 0;
-} 
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/CMakeLists.txt b/robot/ros_ws/src/behavior/drone_safety_monitor/CMakeLists.txt
similarity index 60%
rename from robot/ros_ws/src/interface/drone_safety_monitor/CMakeLists.txt
rename to robot/ros_ws/src/behavior/drone_safety_monitor/CMakeLists.txt
index 503f0e46d..d9350e958 100644
--- a/robot/ros_ws/src/interface/drone_safety_monitor/CMakeLists.txt
+++ b/robot/ros_ws/src/behavior/drone_safety_monitor/CMakeLists.txt
@@ -5,51 +5,33 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(airstack_msgs REQUIRED)
 find_package(airstack_common REQUIRED)
-find_package(trajectory_library REQUIRED)
-find_package(mavros_msgs REQUIRED)
-find_package(std_srvs REQUIRED)
 
 add_executable(drone_safety_monitor src/drone_safety_monitor.cpp)
 target_include_directories(drone_safety_monitor PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-target_compile_features(drone_safety_monitor PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_compile_features(drone_safety_monitor PUBLIC c_std_99 cxx_std_17)
 ament_target_dependencies(
   drone_safety_monitor
+  rclcpp
   std_msgs
+  nav_msgs
   airstack_msgs
   airstack_common
-  trajectory_library
-  mavros_msgs
-  std_srvs
 )
 
 install(TARGETS drone_safety_monitor
   DESTINATION lib/${PROJECT_NAME})
 
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME}/
-  )
-
-install(DIRECTORY
-  config
-  DESTINATION share/${PROJECT_NAME})
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
   set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp b/robot/ros_ws/src/behavior/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp
similarity index 83%
rename from robot/ros_ws/src/interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp
rename to robot/ros_ws/src/behavior/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp
index 48e11fc1c..4b25a4abd 100644
--- a/robot/ros_ws/src/interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp
+++ b/robot/ros_ws/src/behavior/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp
@@ -20,17 +20,17 @@
 
 #pragma once
 
-#include <rclcpp/rclcpp.hpp>
-#include <nav_msgs/msg/odometry.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <airstack_common/ros2_helper.hpp>
 #include <limits>
 
-// =====================================================================================
-// ---------------------------------- TimeChecker --------------------------------------
-// =====================================================================================
+#include <airstack_common/ros2_helper.hpp>
+#include <airstack_msgs/srv/trajectory_mode.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
 
-class TimeChecker{
+class TimeChecker
+{
 private:
   rclcpp::Time time;
   bool time_initialized;
@@ -39,4 +39,4 @@ class TimeChecker{
   TimeChecker();
   void update(rclcpp::Time time);
   double elapsed_since_last_update(rclcpp::Time time);
-}; 
+};
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/package.xml b/robot/ros_ws/src/behavior/drone_safety_monitor/package.xml
similarity index 75%
rename from robot/ros_ws/src/interface/drone_safety_monitor/package.xml
rename to robot/ros_ws/src/behavior/drone_safety_monitor/package.xml
index 54c32c903..2ac348759 100644
--- a/robot/ros_ws/src/interface/drone_safety_monitor/package.xml
+++ b/robot/ros_ws/src/behavior/drone_safety_monitor/package.xml
@@ -3,20 +3,18 @@
 <package format="3">
   <name>drone_safety_monitor</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Safety monitor: state estimate watchdog and pause/resume/rewind command handling</description>
   <maintainer email="uav@todo.todo">uav</maintainer>
-  <license>TODO: License declaration</license>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>airstack_msgs</depend>
   <depend>airstack_common</depend>
-  <depend>trajectory_library</depend>
-  <depend>mavros_msgs</depend>
-  <depend>std_srvs</depend>
-  
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
diff --git a/robot/ros_ws/src/behavior/drone_safety_monitor/src/drone_safety_monitor.cpp b/robot/ros_ws/src/behavior/drone_safety_monitor/src/drone_safety_monitor.cpp
new file mode 100644
index 000000000..595724523
--- /dev/null
+++ b/robot/ros_ws/src/behavior/drone_safety_monitor/src/drone_safety_monitor.cpp
@@ -0,0 +1,147 @@
+// Copyright (c) 2024 Carnegie Mellon University
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <drone_safety_monitor/drone_safety_monitor.hpp>
+
+TimeChecker::TimeChecker()
+: time_initialized(false) {}
+
+void TimeChecker::update(rclcpp::Time time)
+{
+  this->time = time;
+  time_initialized = true;
+}
+
+double TimeChecker::elapsed_since_last_update(rclcpp::Time time)
+{
+  if (!time_initialized) {
+    return std::numeric_limits<double>::infinity();
+  }
+  return (time - this->time).seconds();
+}
+
+
+class DroneSafetyMonitorNode : public rclcpp::Node
+{
+public:
+  DroneSafetyMonitorNode()
+  : Node("drone_safety_monitor")
+  {
+    state_estimate_timeout_ = airstack::get_param(this, "state_estimate_timeout", 1.0);
+
+    // subscribers
+    state_estimate_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+      "state_estimate", 10,
+      std::bind(&DroneSafetyMonitorNode::state_estimate_callback, this, std::placeholders::_1));
+
+    command_sub_ = this->create_subscription<std_msgs::msg::String>(
+      "command", 10,
+      std::bind(&DroneSafetyMonitorNode::command_callback, this, std::placeholders::_1));
+
+    // publishers
+    state_estimate_timed_out_pub_ =
+      this->create_publisher<std_msgs::msg::Bool>("state_estimate_timed_out", 10);
+
+    // service client
+    traj_mode_client_ =
+      this->create_client<airstack_msgs::srv::TrajectoryMode>("set_trajectory_mode");
+
+    // timer at 1 Hz
+    timer_ = rclcpp::create_timer(
+      this, this->get_clock(), std::chrono::milliseconds(1000),
+      std::bind(&DroneSafetyMonitorNode::timer_callback, this));
+
+    RCLCPP_INFO(this->get_logger(), "DroneSafetyMonitorNode started. "
+      "Publish to 'command' topic with: pause | resume | rewind");
+  }
+
+private:
+  float state_estimate_timeout_;
+  bool was_timed_out_{false};
+
+  TimeChecker state_estimate_time_checker_;
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr state_estimate_sub_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_sub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr state_estimate_timed_out_pub_;
+  rclcpp::Client<airstack_msgs::srv::TrajectoryMode>::SharedPtr traj_mode_client_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  void set_trajectory_mode(int32_t mode)
+  {
+    if (!traj_mode_client_->wait_for_service(std::chrono::seconds(1))) {
+      RCLCPP_WARN(this->get_logger(), "set_trajectory_mode service not available");
+      return;
+    }
+    auto req = std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
+    req->mode = mode;
+    traj_mode_client_->async_send_request(req);
+  }
+
+  void state_estimate_callback(const nav_msgs::msg::Odometry::SharedPtr)
+  {
+    state_estimate_time_checker_.update(this->get_clock()->now());
+  }
+
+  void command_callback(const std_msgs::msg::String::SharedPtr msg)
+  {
+    const std::string & cmd = msg->data;
+    if (cmd == "pause") {
+      RCLCPP_INFO(this->get_logger(), "Safety command: PAUSE");
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::PAUSE);
+    } else if (cmd == "resume") {
+      RCLCPP_INFO(this->get_logger(), "Safety command: RESUME");
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+    } else if (cmd == "rewind") {
+      RCLCPP_INFO(this->get_logger(), "Safety command: REWIND");
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::REWIND);
+    } else {
+      RCLCPP_WARN(this->get_logger(),
+        "Unknown safety command: '%s'. Valid commands: pause | resume | rewind", cmd.c_str());
+    }
+  }
+
+  void timer_callback()
+  {
+    bool timed_out =
+      state_estimate_time_checker_.elapsed_since_last_update(this->get_clock()->now()) >
+      state_estimate_timeout_;
+
+    std_msgs::msg::Bool msg;
+    msg.data = timed_out;
+    state_estimate_timed_out_pub_->publish(msg);
+
+    // auto-pause if state estimate just timed out
+    if (timed_out && !was_timed_out_) {
+      RCLCPP_WARN(this->get_logger(),
+        "State estimate timed out! Auto-pausing trajectory controller.");
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::PAUSE);
+    }
+    was_timed_out_ = timed_out;
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<DroneSafetyMonitorNode>());
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst
deleted file mode 100644
index 0a789b786..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst
+++ /dev/null
@@ -1,149 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package rqt_py_console
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.0.2 (2021-08-31)
-------------------
-* Fix modern setuptools warning about dashes instead of underscores (`#11 <https://github.com/ros-visualization/rqt_py_console/issues/11>`_)
-* Contributors: Chris Lalancette
-
-1.0.1 (2021-04-27)
-------------------
-* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 <https://github.com/ros-visualization/rqt_py_console/issues/8>`_)
-* Contributors: Alejandro Hernández Cordero
-
-1.0.0 (2018-12-11)
-------------------
-* spyderlib -> spyder (`#5 <https://github.com/ros-visualization/rqt_py_console/issues/5>`_)
-* ros2 port (`#3 <https://github.com/ros-visualization/rqt_py_console/issues/3>`_)
-* autopep8 (`#2 <https://github.com/ros-visualization/rqt_py_console/issues/2>`_)
-* Contributors: Mike Lautman
-
-0.4.8 (2017-04-28)
-------------------
-
-0.4.7 (2017-03-02)
-------------------
-
-0.4.6 (2017-02-27)
-------------------
-
-0.4.5 (2017-02-03)
-------------------
-
-0.4.4 (2017-01-24)
-------------------
-* use Python 3 compatible syntax (`#421 <https://github.com/ros-visualization/rqt_common_plugins/pull/421>`_)
-
-0.4.3 (2016-11-02)
-------------------
-
-0.4.2 (2016-09-19)
-------------------
-
-0.4.1 (2016-05-16)
-------------------
-
-0.4.0 (2016-04-27)
-------------------
-* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 <https://github.com/ros-visualization/rqt_common_plugins/pull/359>`_)
-
-0.3.13 (2016-03-08)
--------------------
-
-0.3.12 (2015-07-24)
--------------------
-
-0.3.11 (2015-04-30)
--------------------
-
-0.3.10 (2014-10-01)
--------------------
-* update plugin scripts to use full name to avoid future naming collisions
-
-0.3.9 (2014-08-18)
-------------------
-
-0.3.8 (2014-07-15)
-------------------
-
-0.3.7 (2014-07-11)
-------------------
-* export architecture_independent flag in package.xml (`#254 <https://github.com/ros-visualization/rqt_common_plugins/issues/254>`_)
-
-0.3.6 (2014-06-02)
-------------------
-
-0.3.5 (2014-05-07)
-------------------
-
-0.3.4 (2014-01-28)
-------------------
-
-0.3.3 (2014-01-08)
-------------------
-* add groups for rqt plugins, renamed some plugins (`#167 <https://github.com/ros-visualization/rqt_common_plugins/issues/167>`_)
-
-0.3.2 (2013-10-14)
-------------------
-
-0.3.1 (2013-10-09)
-------------------
-
-0.3.0 (2013-08-28)
-------------------
-
-0.2.17 (2013-07-04)
--------------------
-
-0.2.16 (2013-04-09 13:33)
--------------------------
-
-0.2.15 (2013-04-09 00:02)
--------------------------
-
-0.2.14 (2013-03-14)
--------------------
-
-0.2.13 (2013-03-11 22:14)
--------------------------
-
-0.2.12 (2013-03-11 13:56)
--------------------------
-
-0.2.11 (2013-03-08)
--------------------
-
-0.2.10 (2013-01-22)
--------------------
-
-0.2.9 (2013-01-17)
-------------------
-
-0.2.8 (2013-01-11)
-------------------
-
-0.2.7 (2012-12-24)
-------------------
-
-0.2.6 (2012-12-23)
-------------------
-
-0.2.5 (2012-12-21 19:11)
-------------------------
-
-0.2.4 (2012-12-21 01:13)
-------------------------
-
-0.2.3 (2012-12-21 00:24)
-------------------------
-
-0.2.2 (2012-12-20 18:29)
-------------------------
-
-0.2.1 (2012-12-20 17:47)
-------------------------
-
-0.2.0 (2012-12-20 17:39)
-------------------------
-* first release of this package into groovy
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/README.md b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/README.md
deleted file mode 100644
index 82f4ee4c9..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/README.md
+++ /dev/null
@@ -1,19 +0,0 @@
-# RQT Python FixedTrajectoryGenerator
-
-If you `colcon build` this package in a workspace and then run "rqt --force-discover" after sourcing the workspace, the plugin should show up as "Fixed Trajectory Generator" in "Miscellaneous Tools" in the "Plugins" menu.
-
-You can use the `generate_rqt_py_package.sh` script to generate a new package by doing the following from the rqt_fixed_trajectory_generator directory
-
-```
-./generate_rqt_py_package.sh [package name] [class name] [plugin title]
-```
-
-[package name] will be the name of the package and a directory with this name will be created above `rqt_fixed_trajectory_generator/`. [class name] is the name of the class in `src/[package name]/template.py`. [plugin title] is what the plugin will be called in the "Miscellaneous Tools" menu.
-
-For example,
-
-```
-cd rqt_fixed_trajectory_generator/
-./generate_rqt_py_package.sh new_rqt_package ClassName "Plugin Title"
-```
-
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml
deleted file mode 100644
index 0fdeb0af2..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml
+++ /dev/null
@@ -1,47 +0,0 @@
-trajectories:
-  - Figure8:
-      attributes:
-        - frame_id
-        - velocity
-        - max_acceleration
-        - length
-        - width
-        - height
-  - Racetrack:
-      attributes:
-        - frame_id
-        - velocity
-        - turn_velocity
-        - max_acceleration
-        - length
-        - width
-        - height
-  - Circle:
-      attributes:
-        - frame_id
-        - velocity
-        - radius
-  - Line:
-      attributes:
-        - frame_id
-        - velocity
-        - max_acceleration
-        - length
-        - width
-        - height
-  - Point:
-      attributes:
-        - frame_id
-        - velocity
-        - max_acceleration
-        - x
-        - y
-        - height
-  - Lawnmower:
-      attributes:
-        - frame_id
-        - length
-        - width
-        - height
-        - velocity
-        - vertical
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/package.xml b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/package.xml
deleted file mode 100644
index 8f12f2f8d..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/package.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-<package format="2">
-  <name>rqt_fixed_trajectory_generator</name>
-  <version>1.0.2</version>
-  <description>rqt_fixed_trajectory_generator is a Python GUI template.</description>
-  <maintainer email="jkeller2@andrew.cmu.edu">John Keller</maintainer>
-
-  <license>BSD</license>
-
-  <url type="website"></url>
-  <url type="repository"></url>
-  <url type="bugtracker"></url>
-
-  <author>John Keller</author>
-
-  <exec_depend>ament_index_python</exec_depend>
-  <exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
-  <exec_depend>qt_gui</exec_depend>
-  <exec_depend>qt_gui_py_common</exec_depend>
-  <exec_depend>rclpy</exec_depend>
-  <exec_depend>rqt_gui</exec_depend>
-  <exec_depend>rqt_gui_py</exec_depend>
-
-  <export>
-    <architecture_independent/>
-    <rqt_gui plugin="${prefix}/plugin.xml"/>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/plugin.xml b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/plugin.xml
deleted file mode 100644
index ec1f2a2b4..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/plugin.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<library path="src">
-  <class name="FixedTrajectoryGenerator" type="rqt_fixed_trajectory_generator.template.FixedTrajectoryGenerator" base_class_type="rqt_gui_py::Plugin">
-    <description>
-      A Python GUI plugin providing an interactive Python console.
-    </description>
-    <qtgui>
-      <group>
-        <label>Miscellaneous Tools</label>
-        <icon type="theme">folder</icon>
-        <statustip>Plugins related to miscellaneous tools.</statustip>
-      </group>
-      <label>Fixed Trajectory Generator</label>
-      <icon type="theme">applications-python</icon>
-      <statustip>A Python RQT GUI template.</statustip>
-    </qtgui>
-  </class>
-</library>
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui
deleted file mode 100644
index 12810f1c5..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>PyConsole</class>
- <widget class="QWidget" name="PyConsole">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>276</width>
-    <height>212</height>
-   </rect>
-  </property>
-  <property name="windowTitle">
-   <string>PyConsole</string>
-  </property>
-  <layout class="QVBoxLayout" name="_2">
-   <property name="spacing">
-    <number>0</number>
-   </property>
-   <property name="leftMargin">
-    <number>0</number>
-   </property>
-   <property name="topMargin">
-    <number>0</number>
-   </property>
-   <property name="rightMargin">
-    <number>3</number>
-   </property>
-   <property name="bottomMargin">
-    <number>0</number>
-   </property>
-   <item>
-    <layout class="QHBoxLayout" name="header_layout">
-     <property name="spacing">
-      <number>0</number>
-     </property>
-     <item>
-      <widget class="PyConsoleTextEdit" name="py_console"/>
-     </item>
-    </layout>
-   </item>
-  </layout>
- </widget>
- <customwidgets>
-  <customwidget>
-   <class>PyConsoleTextEdit</class>
-   <extends>QTextEdit</extends>
-   <header>rqt_py_console.py_console_text_edit</header>
-  </customwidget>
- </customwidgets>
- <resources/>
- <connections/>
-</ui>
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/rqt_fixed_trajectory_generator b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/rqt_fixed_trajectory_generator
deleted file mode 100644
index e69de29bb..000000000
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.cfg b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.cfg
deleted file mode 100644
index 2394a82ac..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/rqt_fixed_trajectory_generator
-[install]
-install_scripts=$base/lib/rqt_fixed_trajectory_generator
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.py
deleted file mode 100644
index 12c8b1c23..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.py
+++ /dev/null
@@ -1,41 +0,0 @@
-from setuptools import setup
-from glob import glob
-
-package_name = 'rqt_fixed_trajectory_generator'
-
-setup(
-    name=package_name,
-    version='1.0.2',
-    packages=[package_name],
-    package_dir={'': 'src'},
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name + '/resource',
-            ['resource/py_console_widget.ui']),
-        ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name, ['plugin.xml']),
-        ('share/' + package_name + '/config/', glob('config/*')),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    author='',
-    maintainer='',
-    maintainer_email='',
-    keywords=['ROS'],
-    classifiers=[
-        '',
-        '',
-        '',
-        '',
-    ],
-    description=(
-        'rqt_fixed_trajectory_generator'
-    ),
-    license='BSD',
-    entry_points={
-        'console_scripts': [
-            'rqt_fixed_trajectory_generator = ' + package_name + '.main:main',
-        ],
-    },
-)
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py
deleted file mode 100755
index 9a5c9376e..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py
+++ /dev/null
@@ -1,12 +0,0 @@
-import sys
-
-from rqt_gui.main import Main
-
-
-def main():
-    main = Main()
-    sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole'))
-
-
-if __name__ == '__main__':
-    main()
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py
deleted file mode 100644
index dc9ce1a0a..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py
+++ /dev/null
@@ -1,69 +0,0 @@
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Dorian Scholz
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * Redistributions in binary form must reproduce the above
-#    copyright notice, this list of conditions and the following
-#    disclaimer in the documentation and/or other materials provided
-#    with the distribution.
-#  * Neither the name of Willow Garage, Inc. nor the names of its
-#    contributors may be used to endorse or promote products derived
-#    from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-
-import sys
-from code import InteractiveInterpreter
-
-from python_qt_binding import QT_BINDING, QT_BINDING_VERSION
-from python_qt_binding.QtCore import Qt, Signal
-
-from qt_gui_py_common.console_text_edit import ConsoleTextEdit
-
-
-class PyConsoleTextEdit(ConsoleTextEdit):
-    _color_stdin = Qt.darkGreen
-    _multi_line_char = ':'
-    _multi_line_indent = '    '
-    _prompt = ('>>> ', '... ')  # prompt for single and multi line
-    exit = Signal()
-
-    def __init__(self, parent=None):
-        super(PyConsoleTextEdit, self).__init__(parent)
-
-        self._interpreter_locals = {}
-        self._interpreter = InteractiveInterpreter(self._interpreter_locals)
-
-        self._comment_writer.write('Python %s on %s\n' %
-                                   (sys.version.replace('\n', ''), sys.platform))
-        self._comment_writer.write(
-            'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION))
-
-        self._add_prompt()
-
-    def update_interpreter_locals(self, newLocals):
-        self._interpreter_locals.update(newLocals)
-
-    def _exec_code(self, code):
-        try:
-            self._interpreter.runsource(code)
-        except SystemExit:  # catch sys.exit() calls, so they don't close the whole gui
-            self.exit.emit()
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py
deleted file mode 100644
index e69bde34c..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py
+++ /dev/null
@@ -1,59 +0,0 @@
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Dorian Scholz
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * Redistributions in binary form must reproduce the above
-#    copyright notice, this list of conditions and the following
-#    disclaimer in the documentation and/or other materials provided
-#    with the distribution.
-#  * Neither the name of Willow Garage, Inc. nor the names of its
-#    contributors may be used to endorse or promote products derived
-#    from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-
-import os
-from ament_index_python.resources import get_resource
-
-from python_qt_binding import loadUi
-from python_qt_binding.QtWidgets import QWidget
-from rqt_py_console.py_console_text_edit import PyConsoleTextEdit
-
-
-class PyConsoleWidget(QWidget):
-
-    def __init__(self, context=None):
-        super(PyConsoleWidget, self).__init__()
-
-        _, package_path = get_resource('packages', 'rqt_py_console')
-        ui_file = os.path.join(
-            package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui')
-
-        loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit})
-        self.setObjectName('PyConsoleWidget')
-
-        my_locals = {
-            'context': context
-        }
-        self.py_console.update_interpreter_locals(my_locals)
-        self.py_console.print_message(
-            'The variable "context" is set to the PluginContext of this plugin.')
-        self.py_console.exit.connect(context.close_plugin)
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py
deleted file mode 100644
index 374ef7a5d..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py
+++ /dev/null
@@ -1,60 +0,0 @@
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Dorian Scholz
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * Redistributions in binary form must reproduce the above
-#    copyright notice, this list of conditions and the following
-#    disclaimer in the documentation and/or other materials provided
-#    with the distribution.
-#  * Neither the name of Willow Garage, Inc. nor the names of its
-#    contributors may be used to endorse or promote products derived
-#    from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-
-from python_qt_binding.QtGui import QFont
-
-from spyder.widgets.internalshell import InternalShell
-from spyder.utils.module_completion import moduleCompletion
-
-class SpyderConsoleWidget(InternalShell):
-
-    def __init__(self, context=None):
-        my_locals = {
-            'context': context
-        }
-        super(SpyderConsoleWidget, self).__init__(namespace=my_locals)
-        self.setObjectName('SpyderConsoleWidget')
-        self.set_pythonshell_font(QFont('Mono'))
-        self.interpreter.restore_stds()
-
-    def get_module_completion(self, objtxt):
-        """Return module completion list associated to object name"""
-        return moduleCompletion(objtxt)
-
-    def run_command(self, *args):
-        self.interpreter.redirect_stds()
-        super(SpyderConsoleWidget, self).run_command(*args)
-        self.flush()
-        self.interpreter.restore_stds()
-
-    def shutdown(self):
-        self.exit_interpreter()
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py
deleted file mode 100644
index 76c60c770..000000000
--- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py
+++ /dev/null
@@ -1,269 +0,0 @@
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2012, Dorian Scholz
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-#  * Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-#  * Redistributions in binary form must reproduce the above
-#    copyright notice, this list of conditions and the following
-#    disclaimer in the documentation and/or other materials provided
-#    with the distribution.
-#  * Neither the name of Willow Garage, Inc. nor the names of its
-#    contributors may be used to endorse or promote products derived
-#    from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-
-from python_qt_binding.QtWidgets import QVBoxLayout, QWidget
-from rqt_gui_py.plugin import Plugin
-from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog
-from rqt_py_console.py_console_widget import PyConsoleWidget
-
-import python_qt_binding.QtWidgets as qt
-import python_qt_binding.QtWidgets as QtWidgets
-import python_qt_binding.QtGui as QtGui
-import python_qt_binding.QtCore as QtCore
-
-import os
-import time
-import numpy as np
-import yaml
-import collections
-import pickle
-from ament_index_python.packages import get_package_share_directory
-
-from std_msgs.msg import Bool
-from airstack_msgs.msg import FixedTrajectory
-from diagnostic_msgs.msg import KeyValue
-
-try:
-    from rqt_py_console.spyder_console_widget import SpyderConsoleWidget
-    _has_spyderlib = True
-except ImportError:
-    _has_spyderlib = False
-
-
-class FixedTrajectoryGenerator(Plugin):
-    """
-    Plugin providing an interactive Python console
-    """
-
-    def __init__(self, context):
-        super(FixedTrajectoryGenerator, self).__init__(context)
-        self.setObjectName('FixedTrajectoryGenerator')
-
-        self.context = context
-        # to access ros2 node use self.context.node
-
-        self.config_filename = ''
-
-        self.button_dct = {}
-        self.attribute_settings = collections.OrderedDict()
-
-        self.timer = self.context.node.create_timer(1./10., self.timer_callback)
-
-        self.fixed_trajectory_pub = self.context.node.create_publisher(FixedTrajectory, 'fixed_trajectory_command', 1)
-        self.global_plan_fixed_trajectory_pub = self.context.node.create_publisher(FixedTrajectory, 'global_plan_fixed_trajectory', 1)
-
-        # main layout
-        self.widget = QWidget()
-        self.vbox = qt.QVBoxLayout()
-        self.widget.setLayout(self.vbox)
-        context.add_widget(self.widget)
-
-        # config widget
-        self.config_widget = qt.QWidget()
-        self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}')
-        self.config_layout = qt.QHBoxLayout()
-        self.config_widget.setLayout(self.config_layout)
-        self.config_widget.setFixedHeight(50)
-
-        self.config_button = qt.QPushButton('Open Config...')
-        self.config_button.clicked.connect(self.select_config_file)
-        self.config_layout.addWidget(self.config_button)
-
-        self.config_label = qt.QLabel('config filename: ')
-        self.config_layout.addWidget(self.config_label)
-        self.vbox.addWidget(self.config_widget)
-
-        # trajectory widget
-        self.trajectory_widget = qt.QWidget()
-        self.trajectory_layout = qt.QVBoxLayout()
-        self.trajectory_widget.setLayout(self.trajectory_layout)
-        self.vbox.addWidget(self.trajectory_widget)
-        
-        self.tab_widget = qt.QTabWidget()
-        self.trajectory_layout.addWidget(self.tab_widget)
-
-        # button widget
-        self.button_widget = qt.QWidget()
-        self.button_layout = qt.QHBoxLayout()
-        self.button_widget.setLayout(self.button_layout)
-        self.vbox.addWidget(self.button_widget)
-        
-        self.publish_button = qt.QPushButton('Publish')
-        self.publish_button.clicked.connect(self.publish_trajectory)
-        self.button_layout.addWidget(self.publish_button)
-        
-        self.trajectory_type_label = qt.QLabel('Type: ')
-        self.button_layout.addWidget(self.trajectory_type_label)
-
-        self.trajectory_type_combo_box = qt.QComboBox()
-        self.trajectory_type_combo_box.addItem('Fixed Trajectory')
-        self.trajectory_type_combo_box.addItem('Global Plan')
-        self.button_layout.addWidget(self.trajectory_type_combo_box)
-
-    def publish_trajectory(self):
-        trajectory_type = self.trajectory_type_combo_box.currentText()
-        trajectory_name = self.tab_widget.tabText(self.tab_widget.currentIndex())
-        msg = FixedTrajectory()
-        msg.type = trajectory_name
-        for attribute, value in iter(self.attribute_settings[trajectory_name].items()):
-            key_value = KeyValue()
-            key_value.key = attribute
-            key_value.value = value
-            msg.attributes.append(key_value)
-        if trajectory_type == 'Fixed Trajectory':
-            self.fixed_trajectory_pub.publish(msg)
-        elif trajectory_type == 'Global Plan':
-            self.global_plan_fixed_trajectory_pub.publish(msg)
-
-    def select_config_file(self):
-        starting_path = get_package_share_directory('rqt_fixed_trajectory_generator') + '/config/'
-        print(starting_path)
-        filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0]
-        self.set_config(filename)
-
-    def set_config(self, filename):
-        if filename != '':
-            self.config_filename = filename
-            if self.config_filename != None:
-                self.config_label.setText('config filename: ' + os.path.basename(self.config_filename))
-                self.init_buttons(filename)
-
-    def init_buttons(self, filename):
-        y = yaml.load(open(filename, 'r').read(), Loader=yaml.Loader)
-        print(y)
-
-        def get_attribute_changed_function(trajectory_name, attribute_name):
-            def attribute_changed(text):
-                if trajectory_name not in self.attribute_settings:
-                    self.attribute_settings[trajectory_name] = {}
-                self.attribute_settings[trajectory_name][attribute_name] = text
-            return attribute_changed
-
-        def get_publish_function(trajectory_name):
-            def publish_function():
-                msg = FixedTrajectory()
-                msg.type = trajectory_name
-                for attribute, value in iter(self.attribute_settings[trajectory_name].items()):
-                    key_value = KeyValue()
-                    key_value.key = attribute
-                    key_value.value = value
-                    msg.attributes.append(key_value)
-                self.fixed_trajectory_pub.publish(msg)
-            return publish_function
-                
-
-        for trajectory in y['trajectories']:
-            trajectory_name = list(trajectory.keys())[0]
-            attributes = trajectory[trajectory_name]['attributes']
-            
-            trajectory_tab = qt.QWidget()
-            trajectory_layout = qt.QVBoxLayout()
-            trajectory_tab.setLayout(trajectory_layout)
-
-            for attribute in attributes:
-                attribute_widget = qt.QWidget()
-                attribute_layout = qt.QHBoxLayout()
-                attribute_widget.setLayout(attribute_layout)
-                
-                attribute_label = qt.QLabel()
-                attribute_label.setText(attribute)
-                attribute_layout.addWidget(attribute_label)
-
-                attribute_default = '0'
-                if attribute == 'frame_id':
-                    attribute_default = 'world'
-                if trajectory_name in self.attribute_settings.keys():
-                    if attribute in self.attribute_settings[trajectory_name].keys():
-                        attribute_default = self.attribute_settings[trajectory_name][attribute]
-                
-                attribute_edit = qt.QLineEdit()
-                attribute_edit.textChanged.connect(get_attribute_changed_function(trajectory_name,
-                                                                                  attribute))
-                attribute_edit.setText(attribute_default)
-                
-                attribute_layout.addWidget(attribute_edit)
-                
-                trajectory_layout.addWidget(attribute_widget)
-
-            #publish_button = qt.QPushButton('Publish')
-            #publish_button.clicked.connect(get_publish_function(trajectory_name))
-            #trajectory_layout.addWidget(publish_button)
-                
-            self.tab_widget.addTab(trajectory_tab, trajectory_name)
-            
-            
-        
-    def timer_callback(self):
-        bool_msg = Bool()
-        for key in self.button_dct.keys():
-            bool_msg.data = self.button_dct[key]['data']
-            self.button_dct[key]['publisher'].publish(bool_msg)
-
-    def save_settings(self, plugin_settings, instance_settings):
-        instance_settings.set_value('config_filename', self.config_filename)
-        instance_settings.set_value('attribute_settings', pickle.dumps(self.attribute_settings))
-
-    def restore_settings(self, plugin_settings, instance_settings):
-        attribute_settings = instance_settings.value('attribute_settings')
-        if attribute_settings != None:
-            self.attribute_settings = pickle.loads(attribute_settings)
-        self.set_config(instance_settings.value('config_filename'))
-
-        '''
-    def save_settings(self, plugin_settings, instance_settings):
-        pass
-        #instance_settings.set_value('some_variable', self.some_variable)
-
-    def restore_settings(self, plugin_settings, instance_settings):
-        pass
-        #self.some_variable = instance_settings.value('some_variable', default_value)
-        '''
-    def trigger_configuration(self):
-        options = [
-            {'title': 'Option 1',
-             'description': 'Description of option 1.',
-             'enabled': True},
-            {'title': 'Option 2',
-             'description': 'Description of option 2.'},
-        ]
-        dialog = SimpleSettingsDialog(title='Options')
-        dialog.add_exclusive_option_group(title='List of options:', options=options, selected_index=0)
-        selected_index = dialog.get_settings()[0]
-        if selected_index != None:
-            selected_index = selected_index['selected_index']
-            print('selected_index ', selected_index)
-
-    def shutdown_console_widget(self):
-        pass
-
-    def shutdown_plugin(self):
-        self.shutdown_console_widget()
diff --git a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml
index 549a166ca..cef093702 100644
--- a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml
+++ b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml
@@ -2,31 +2,14 @@
     <group>
         <push-ros-namespace namespace="behavior" />
 
-        <!-- Behavior Tree -->
-        <node pkg="behavior_tree" exec="behavior_tree_implementation">
-            <param name="config" value="$(find-pkg-share behavior_tree)/config/drone.tree" />
-            <param name="timeout" value="1.0" />
+        <!-- Safety Monitor: state estimate watchdog + pause/resume/rewind commands -->
+        <node pkg="drone_safety_monitor" exec="drone_safety_monitor"
+              namespace="drone_safety_monitor" output="screen">
+            <param name="state_estimate_timeout" value="1.0" />
+            <remap from="state_estimate"
+                   to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
+            <remap from="set_trajectory_mode"
+                   to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
         </node>
-
-        <!-- Behavior Executive -->
-        <node pkg="behavior_executive" exec="behavior_executive"
-            output="screen">
-            <!-- remap glue to AirStack -->
-            <remap from="is_armed" to="/$(env ROBOT_NAME)/interface/is_armed" />
-            <!-- topics -->
-            <remap from="has_control" to="/$(env ROBOT_NAME)/interface/has_control" />
-            <!-- services -->
-            <remap from="robot_command" to="/$(env ROBOT_NAME)/interface/robot_command" />
-            <remap from="set_trajectory_mode" to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
-            <remap from="set_takeoff_landing_command" to="/$(env ROBOT_NAME)/takeoff_landing_planner/set_takeoff_landing_command" />
-            <remap from="/global/trajectory" to="/$(env ROBOT_NAME)/global/trajectory" />
-            <!-- <remap from="get_random_walk_plan" to="/$(env ROBOT_NAME)/global/get_random_walk_plan" /> -->
-            <remap from="takeoff_state" to="/$(env ROBOT_NAME)/takeoff_landing_planner/takeoff_state" />
-            <remap from="landing_state" to="/$(env ROBOT_NAME)/takeoff_landing_planner/landing_state" />
-            <remap from="state_estimate_timed_out" to="/$(env ROBOT_NAME)/drone_safety_monitor/state_estimate_timed_out" />
-            <remap from="set_recording_status" to="/$(env ROBOT_NAME)/bag_record/set_recording_status" />
-
-        </node>
-
     </group>
 </launch>
diff --git a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml
index 0e0c9dd76..93217a883 100644
--- a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml
+++ b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml
@@ -8,29 +8,39 @@
     <log message="local_odometry_in_topic has $(var local_odometry_in_topic)"/>
 
 
-    <!-- Basic Planners -->
-    <node pkg="trajectory_library"
-        namespace="fixed_trajectory_generator" exec="fixed_trajectory_generator.py" output="screen">
+    <!-- Takeoff/Landing Task Action Servers -->
+    <node pkg="takeoff_landing_planner" exec="takeoff_landing_task"
+          namespace="takeoff_landing_planner" output="screen">
+        <param from="$(find-pkg-share takeoff_landing_planner)/config/takeoff_landing_planner.yaml"
+            allow_substs="true" />
         <remap from="trajectory_override"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_override" />
+        <remap from="set_trajectory_mode"
+            to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
+        <remap from="tracking_point"
+            to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
+        <remap from="odometry" to="$(var local_odometry_in_topic)" />
+        <remap from="is_armed" to="/$(env ROBOT_NAME)/interface/is_armed" />
+        <remap from="has_control" to="/$(env ROBOT_NAME)/interface/has_control" />
+        <remap from="state_estimate_timed_out"
+            to="/$(env ROBOT_NAME)/behavior/drone_safety_monitor/state_estimate_timed_out" />
+        <remap from="extended_state"
+            to="/$(env ROBOT_NAME)/interface/mavros/extended_state" />
+        <remap from="~/takeoff_task" to="/$(env ROBOT_NAME)/tasks/takeoff" />
+        <remap from="~/land_task" to="/$(env ROBOT_NAME)/tasks/land" />
     </node>
 
-    <!-- customize takeoff_landing_planner.launch.xml -->
-    <node pkg="takeoff_landing_planner"
-        namespace="takeoff_landing_planner"
-        exec="takeoff_landing_planner" output="screen">
-        <param from="$(find-pkg-share takeoff_landing_planner)/config/takeoff_landing_planner.yaml"
-            allow_substs="true" />
-
-        <!-- remap glue to AirStack -->
+    <!-- Fixed Trajectory Task Action Server -->
+    <node pkg="trajectory_controller" exec="fixed_trajectory_task"
+          namespace="trajectory_controller" output="screen">
         <remap from="trajectory_override"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_override" />
+        <remap from="set_trajectory_mode"
+            to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
         <remap from="trajectory_completion_percentage"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_completion_percentage" />
-        <remap from="tracking_point"
-            to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
         <remap from="odometry" to="$(var local_odometry_in_topic)" />
-        <remap from="mavros/cmd/takeoff" to="/$(env ROBOT_NAME)/interface/mavros/cmd/takeoff" />
+        <remap from="~/fixed_trajectory_task" to="/$(env ROBOT_NAME)/tasks/fixed_trajectory" />
     </node>
 
     <group>
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/README.md b/robot/ros_ws/src/interface/drone_safety_monitor/README.md
deleted file mode 100644
index efb46f285..000000000
--- a/robot/ros_ws/src/interface/drone_safety_monitor/README.md
+++ /dev/null
@@ -1,114 +0,0 @@
-# drone_safety_monitor
-
-A ROS 2 package that monitors critical drone safety conditions and publishes fault flags to the rest of the autonomy stack. Currently the package watches the state estimate stream and raises a timeout flag whenever odometry stops arriving, allowing downstream nodes (e.g. the behavior executive) to react safely.
-
-## Overview
-
-| Item | Value |
-|---|---|
-| Package name | `drone_safety_monitor` |
-| Node name | `odom_timeout_checker` |
-| Language | C++ 17 |
-| ROS distro | ROS 2 (Humble / Iron) |
-| License | MIT |
-
-## Nodes
-
-### `drone_safety_monitor` (executable → node name `odom_timeout_checker`)
-
-Monitors the incoming state estimate (odometry) and publishes a Boolean timeout flag once per second.
-
-**How it works**
-
-1. Every time an odometry message is received on `state_estimate`, the internal `TimeChecker` records the current ROS clock time.
-2. A 1 Hz timer fires every second and computes how long ago the last odometry message arrived.
-3. If that elapsed time exceeds `state_estimate_timeout`, the node publishes `true` on `state_estimate_timed_out`; otherwise it publishes `false`.
-4. If no odometry has ever been received (e.g. immediately after startup), the elapsed time is treated as infinite, so the timeout flag is raised immediately.
-
-#### Subscribed Topics
-
-| Topic | Type | Description |
-|---|---|---|
-| `state_estimate` | `nav_msgs/msg/Odometry` | Incoming odometry / state estimate from the localization system |
-
-#### Published Topics
-
-| Topic | Type | Description |
-|---|---|---|
-| `state_estimate_timed_out` | `std_msgs/msg/Bool` | `true` when the state estimate has not been received within `state_estimate_timeout` seconds; `false` otherwise. Published at 1 Hz. |
-
-#### Parameters
-
-| Parameter | Type | Default | Description |
-|---|---|---|---|
-| `state_estimate_timeout` | `double` | `1.0` | Maximum acceptable gap (in seconds) between consecutive state estimate messages before the timeout flag is raised |
-
-## Dependencies
-
-| Package | Purpose |
-|---|---|
-| `rclcpp` | ROS 2 C++ client library |
-| `std_msgs` | `Bool` message type for the timeout flag |
-| `nav_msgs` | `Odometry` message type for the state estimate |
-| `airstack_common` | AirStack helper utilities (`airstack::get_param`) |
-| `airstack_msgs` | AirStack custom message types |
-| `trajectory_library` | Trajectory utilities (linked at build time) |
-| `mavros_msgs` | MAVLink/PX4 bridge message types |
-| `std_srvs` | Standard service types |
-
-## Building
-
-```bash
-# From the workspace root
-colcon build --packages-select drone_safety_monitor
-source install/setup.bash
-```
-
-## Running
-
-```bash
-ros2 run drone_safety_monitor drone_safety_monitor
-```
-
-Override the timeout at the command line:
-
-```bash
-ros2 run drone_safety_monitor drone_safety_monitor \
-  --ros-args -p state_estimate_timeout:=0.5
-```
-
-## Configuration
-
-The `config/takeoff_landing_planner.yaml` file is included in the package install. Parameters can be loaded at launch time via `--params-file` or overridden inline with `--ros-args -p <name>:=<value>`.
-
-## Internal Classes
-
-### `TimeChecker`
-
-A lightweight helper that tracks the timestamp of the most recent event and computes the elapsed time since then.
-
-| Method | Description |
-|---|---|
-| `TimeChecker()` | Constructs the checker in an uninitialized state |
-| `update(rclcpp::Time)` | Records the provided timestamp as the most recent update time |
-| `elapsed_since_last_update(rclcpp::Time)` | Returns elapsed seconds since the last `update()` call, or `+∞` if `update()` has never been called |
-
-## Topic Graph
-
-```
-[localization / EKF]
-        │
-        │  nav_msgs/Odometry
-        ▼
-  state_estimate
-        │
-        ▼
- drone_safety_monitor
-        │
-        │  std_msgs/Bool
-        ▼
- state_estimate_timed_out
-        │
-        ▼
- [behavior executive / safety arbiter]
-```
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/interface/drone_safety_monitor/config/takeoff_landing_planner.yaml
deleted file mode 100644
index 23d11f151..000000000
--- a/robot/ros_ws/src/interface/drone_safety_monitor/config/takeoff_landing_planner.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-/**:
-  ros__parameters:
-    takeoff_height: 5.0
-    high_takeoff_height: 5.0
-    takeoff_landing_velocity: 0.3
-    takeoff_acceptance_distance: 0.3
-    takeoff_acceptance_time: 10.0
-    landing_stationary_distance: 0.02
-    landing_acceptance_time: 5.0
-    landing_tracking_point_ahead_time: 5.0
-
-    takeoff_path_roll: 0.  # in degrees
-    takeoff_path_pitch: 0.  # in degrees
-    takeoff_path_relative_to_orientation: false
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml b/robot/ros_ws/src/interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml
deleted file mode 100644
index 7b4a46a66..000000000
--- a/robot/ros_ws/src/interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-<launch>
-
-  <node pkg="takeoff_landing_planner" exec="takeoff_landing_planner" output="screen">
-    <param name="takeoff_height" value="0.5" />
-    <param name="high_takeoff_height" value="0.8" />
-    <param name="takeoff_landing_velocity" value="0.3" />
-    <param name="takeoff_acceptance_distance" value="0.3" />
-    <param name="takeoff_acceptance_time" value="10.0" />
-    <param name="landing_stationary_distance" value="0.02" />
-    <param name="landing_acceptance_time" value="5.0" />
-    <param name="landing_tracking_point_ahead_time" value="5.0" />
-
-    <param name="takeoff_path_roll" value="0." /> <!-- degrees -->
-    <param name="takeoff_path_pitch" value="20." /> <!-- degrees -->
-    <param name="takeoff_path_relative_to_orientation" value="false" />
-
-  </node>
-</launch>
\ No newline at end of file
diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/src/drone_safety_monitor.cpp b/robot/ros_ws/src/interface/drone_safety_monitor/src/drone_safety_monitor.cpp
deleted file mode 100644
index 86c21c94f..000000000
--- a/robot/ros_ws/src/interface/drone_safety_monitor/src/drone_safety_monitor.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-// Copyright (c) 2024 Carnegie Mellon University
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#include <drone_safety_monitor/drone_safety_monitor.hpp>
-
-TimeChecker::TimeChecker()
-  : time_initialized(false){
-}
-
-void TimeChecker::update(rclcpp::Time time){
-  this->time = time;
-  time_initialized = true;
-}
-
-double TimeChecker::elapsed_since_last_update(rclcpp::Time time){
-  if(!time_initialized)
-    return std::numeric_limits<float>::infinity();
-  return (time - this->time).seconds();
-}
-
-
-class DroneSafetyMonitorNode : public rclcpp::Node
-{
-public:
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr state_estimate_sub;
-  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr state_estimate_timeout_pub;
-  rclcpp::TimerBase::SharedPtr timer;
-  float state_estimate_timeout;
-
-  TimeChecker state_estimate_time_checker;
-  
-  DroneSafetyMonitorNode() : Node("odom_timeout_checker"){
-    state_estimate_timeout = airstack::get_param(this, "state_estimate_timeout", 1.);
-
-    // subscribers
-    state_estimate_sub = this->create_subscription<nav_msgs::msg::Odometry>("state_estimate", 10,
-									    std::bind(&DroneSafetyMonitorNode::state_estimate_callback,
-										      this, std::placeholders::_1));
-
-    // publishers
-    state_estimate_timeout_pub = this->create_publisher<std_msgs::msg::Bool>("state_estimate_timed_out", 10);
-
-    // timers
-    timer = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(1000),
-				 std::bind(&DroneSafetyMonitorNode::timer_callback, this));
-				    
-  }
-
-  void state_estimate_callback(const nav_msgs::msg::Odometry::SharedPtr msg){
-    state_estimate_time_checker.update(this->get_clock()->now());
-  }
-
-  void timer_callback(){
-    std_msgs::msg::Bool state_estimate_timeout_msg;
-    state_estimate_timeout_msg.data =
-      state_estimate_time_checker.elapsed_since_last_update(this->get_clock()->now()) > state_estimate_timeout;
-    state_estimate_timeout_pub->publish(state_estimate_timeout_msg);
-  }
-};
-
-int main(int argc, char **argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<DroneSafetyMonitorNode>());
-  rclcpp::shutdown();
-  return 0;
-} 
diff --git a/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py b/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py
index 9e0a1c494..1b1636424 100644
--- a/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py
+++ b/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py
@@ -122,21 +122,7 @@ def launch_setup(context, *args, **kwargs):
     )
     actions.append(odometry_conversion_node)
 
-    # --- Drone safety monitor -----------------------------------------------
-    drone_safety_monitor_node = Node(
-        package='drone_safety_monitor',
-        executable='drone_safety_monitor',
-        namespace='drone_safety_monitor',
-        output='screen',
-        parameters=[{
-            'state_estimate_timeout': 1.0,
-        }],
-        remappings=[
-            ('state_estimate',
-             f'/{robot_name}/odometry_conversion/odometry'),
-        ],
-    )
-    actions.append(drone_safety_monitor_node)
+    # NOTE: drone_safety_monitor is now launched from behavior_bringup
 
     return actions
 
diff --git a/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml b/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml
index 2e11a856c..a61891088 100644
--- a/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml
+++ b/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml
@@ -63,12 +63,6 @@
         <param name="restamp_now_post"                         value="false" />
     </node>
 
-    <!-- ── Safety monitor ──────────────────────────────────────────────── -->
-    <node pkg="drone_safety_monitor" exec="drone_safety_monitor"
-          namespace="drone_safety_monitor" output="screen">
-        <param name="state_estimate_timeout" value="1.0" />
-        <remap from="state_estimate"
-               to="$(var interface_odometry_out_topic)" />
-    </node>
+    <!-- NOTE: drone_safety_monitor is now launched from behavior_bringup -->
 
 </launch>
diff --git a/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt b/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt
index 880d79242..f19e0bd6d 100644
--- a/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt
+++ b/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt
@@ -8,6 +8,7 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
@@ -18,6 +19,7 @@ find_package(tf2_ros REQUIRED)
 find_package(airstack_msgs REQUIRED)
 find_package(airstack_common REQUIRED)
 find_package(trajectory_library REQUIRED)
+find_package(task_msgs REQUIRED)
 
 
 add_executable(trajectory_controller src/trajectory_controller.cpp)
@@ -35,7 +37,19 @@ ament_target_dependencies(trajectory_controller
   airstack_msgs
   airstack_common
   trajectory_library)
-install(TARGETS trajectory_controller
+
+add_executable(fixed_trajectory_task src/fixed_trajectory_task.cpp)
+target_include_directories(fixed_trajectory_task PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(fixed_trajectory_task
+  rclcpp
+  rclcpp_action
+  nav_msgs
+  airstack_msgs
+  task_msgs)
+
+install(TARGETS trajectory_controller fixed_trajectory_task
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY
diff --git a/robot/ros_ws/src/local/controls/trajectory_controller/package.xml b/robot/ros_ws/src/local/controls/trajectory_controller/package.xml
index 4c8876400..6d51bccf9 100644
--- a/robot/ros_ws/src/local/controls/trajectory_controller/package.xml
+++ b/robot/ros_ws/src/local/controls/trajectory_controller/package.xml
@@ -13,6 +13,7 @@
   <depend>tf2_ros</depend>
   <depend>tf2_tools</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>pluginlib</depend>
   <depend>geometry_msgs</depend>
   <depend>visualization_msgs</depend>
@@ -21,6 +22,7 @@
   <depend>airstack_msgs</depend>
   <depend>airstack_common</depend>
   <depend>trajectory_library</depend>
+  <depend>task_msgs</depend>
   
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/robot/ros_ws/src/local/controls/trajectory_controller/src/fixed_trajectory_task.cpp b/robot/ros_ws/src/local/controls/trajectory_controller/src/fixed_trajectory_task.cpp
new file mode 100644
index 000000000..e89445fe6
--- /dev/null
+++ b/robot/ros_ws/src/local/controls/trajectory_controller/src/fixed_trajectory_task.cpp
@@ -0,0 +1,595 @@
+// Copyright (c) 2024 Carnegie Mellon University
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+// Ports fixed_trajectory_generator.py to a ROS 2 action server.
+
+#include <atomic>
+#include <cmath>
+#include <map>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include <airstack_msgs/msg/fixed_trajectory.hpp>
+#include <airstack_msgs/msg/trajectory_xyzv_yaw.hpp>
+#include <airstack_msgs/msg/waypoint_xyzv_yaw.hpp>
+#include <airstack_msgs/srv/trajectory_mode.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <std_msgs/msg/float32.hpp>
+
+#include "task_msgs/action/fixed_trajectory_task.hpp"
+
+using WaypointXYZVYaw = airstack_msgs::msg::WaypointXYZVYaw;
+using TrajectoryXYZVYaw = airstack_msgs::msg::TrajectoryXYZVYaw;
+
+// ─────────────────────────── trajectory generation helpers ───────────────────
+
+static void get_velocities(TrajectoryXYZVYaw & traj, double velocity, double max_acc)
+{
+  auto & wps = traj.waypoints;
+  double v_prev = 0.0;
+  for (size_t i = 0; i < wps.size(); ++i) {
+    size_t j = (i + 1) % wps.size();
+    double dx = wps[j].position.x - wps[i].position.x;
+    double dy = wps[j].position.y - wps[i].position.y;
+    double dist = std::sqrt(dx * dx + dy * dy);
+    double v_limit = std::sqrt(v_prev * v_prev + 2.0 * max_acc * dist);
+    wps[i].velocity = std::min(velocity, v_limit);
+    v_prev = wps[i].velocity;
+  }
+}
+
+static void get_velocities_dual(TrajectoryXYZVYaw & traj, double velocity, double max_acc)
+{
+  auto & wps = traj.waypoints;
+  double v_prev = 0.0;
+  for (size_t i = 0; i < wps.size(); ++i) {
+    size_t j = (i + 1) % wps.size();
+    double dx = wps[j].position.x - wps[i].position.x;
+    double dy = wps[j].position.y - wps[i].position.y;
+    double dist = std::sqrt(dx * dx + dy * dy);
+    double v_limit = std::sqrt(v_prev * v_prev + 2.0 * max_acc * dist);
+    wps[i].velocity = std::min(velocity, v_limit);
+    v_prev = wps[i].velocity;
+  }
+
+  v_prev = 0.0;
+  size_t tail_start = static_cast<size_t>(wps.size() * 0.85);
+  for (size_t i = wps.size() - 1; i > tail_start; --i) {
+    size_t j = (i - 1 + wps.size()) % wps.size();
+    double dx = wps[j].position.x - wps[i].position.x;
+    double dy = wps[j].position.y - wps[i].position.y;
+    double dist = std::sqrt(dx * dx + dy * dy);
+    double v_limit = std::sqrt(v_prev * v_prev + 2.0 * max_acc * dist);
+    wps[i].velocity = std::min(velocity, v_limit);
+    v_prev = wps[i].velocity;
+  }
+  wps.back().velocity = 0.0;
+}
+
+static void get_accelerations(TrajectoryXYZVYaw & traj)
+{
+  auto & wps = traj.waypoints;
+  if (wps.size() < 3) {return;}
+
+  for (size_t i = 0; i + 2 < wps.size(); ++i) {
+    size_t j = i + 1;
+    size_t k = i + 2;
+
+    double dx1 = wps[j].position.x - wps[i].position.x;
+    double dy1 = wps[j].position.y - wps[i].position.y;
+    double dist1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
+    if (dist1 < 1e-9) {
+      wps[i].acceleration.x = 0.0;
+      wps[i].acceleration.y = 0.0;
+      wps[i].acceleration.z = 0.0;
+      continue;
+    }
+
+    double v_cx = wps[i].velocity * dx1 / dist1;
+    double v_cy = wps[i].velocity * dy1 / dist1;
+
+    double dx2 = wps[k].position.x - wps[j].position.x;
+    double dy2 = wps[k].position.y - wps[j].position.y;
+    double dist2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
+    if (dist2 < 1e-9) {
+      wps[i].acceleration.x = 0.0;
+      wps[i].acceleration.y = 0.0;
+      wps[i].acceleration.z = 0.0;
+      continue;
+    }
+
+    double v_nx = wps[j].velocity * dx2 / dist2;
+    double v_ny = wps[j].velocity * dy2 / dist2;
+
+    constexpr double acc_limit = 50.0;
+    wps[i].acceleration.x = (std::abs(dx1) < 1e-6)
+      ? 0.0 : std::min((v_nx * v_nx - v_cx * v_cx) / (2.0 * dx1), acc_limit);
+    wps[i].acceleration.y = (std::abs(dy1) < 1e-6)
+      ? 0.0 : std::min((v_ny * v_ny - v_cy * v_cy) / (2.0 * dy1), acc_limit);
+    wps[i].acceleration.z = 0.0;
+  }
+
+  // zero out last two
+  if (wps.size() >= 2) {
+    wps[wps.size() - 2].acceleration.x = 0.0;
+    wps[wps.size() - 2].acceleration.y = 0.0;
+    wps[wps.size() - 2].acceleration.z = 0.0;
+  }
+  wps.back().acceleration.x = 0.0;
+  wps.back().acceleration.y = 0.0;
+  wps.back().acceleration.z = 0.0;
+}
+
+static WaypointXYZVYaw make_wp(double x, double y, double z, double yaw, double v = 0.0)
+{
+  WaypointXYZVYaw wp;
+  wp.position.x = x;
+  wp.position.y = y;
+  wp.position.z = z;
+  wp.yaw = yaw;
+  wp.velocity = v;
+  return wp;
+}
+
+static TrajectoryXYZVYaw generate_circle(const std::map<std::string, std::string> & attrs)
+{
+  std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link";
+  double radius = std::stod(attrs.at("radius"));
+  double velocity = std::stod(attrs.at("velocity"));
+
+  TrajectoryXYZVYaw traj;
+  traj.header.frame_id = frame_id;
+
+  traj.waypoints.push_back(make_wp(0.0, 0.0, 0.0, 0.0, velocity));
+  traj.waypoints.push_back(make_wp(radius, 0.0, 0.0, 0.0, velocity));
+
+  for (double angle = 0.0; angle < 2.0 * M_PI; angle += 10.0 * M_PI / 180.0) {
+    traj.waypoints.push_back(
+      make_wp(radius * std::cos(angle), radius * std::sin(angle), 0.0, 0.0, velocity));
+  }
+
+  traj.waypoints.push_back(make_wp(radius, 0.0, 0.0, 0.0, velocity));
+  traj.waypoints.push_back(make_wp(0.0, 0.0, 0.0, 0.0, velocity));
+  return traj;
+}
+
+static TrajectoryXYZVYaw generate_figure8(const std::map<std::string, std::string> & attrs)
+{
+  std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link";
+  double length = std::stod(attrs.at("length"));
+  double width = std::stod(attrs.at("width"));
+  double height = std::stod(attrs.at("height"));
+  double velocity = std::stod(attrs.at("velocity"));
+  double max_acc = std::stod(attrs.at("max_acceleration"));
+
+  TrajectoryXYZVYaw traj;
+  traj.header.frame_id = frame_id;
+
+  int n = 600;
+  for (int i = 0; i < n - 1; ++i) {
+    double t = 2.0 * M_PI * i / n;
+    double x = std::cos(t) * length - length;
+    double y = std::cos(t) * std::sin(t) * 2.0 * width;
+    double xd = -std::sin(t) * length;
+    double yd = (std::cos(t) * std::cos(t) - std::sin(t) * std::sin(t)) * 2.0 * width;
+    double yaw = std::atan2(yd, xd);
+    traj.waypoints.push_back(make_wp(x, y, height, yaw));
+  }
+
+  get_velocities_dual(traj, velocity, max_acc);
+  get_accelerations(traj);
+  return traj;
+}
+
+static TrajectoryXYZVYaw generate_racetrack(const std::map<std::string, std::string> & attrs)
+{
+  std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link";
+  double length = std::stod(attrs.at("length"));
+  double width = std::stod(attrs.at("width"));
+  double height = std::stod(attrs.at("height"));
+  double velocity = std::stod(attrs.at("velocity"));
+  double turn_velocity = std::stod(attrs.at("turn_velocity"));
+  double max_acc = std::stod(attrs.at("max_acceleration"));
+
+  TrajectoryXYZVYaw traj;
+  traj.header.frame_id = frame_id;
+
+  double sl = length - width;  // straightaway length
+
+  // first straightaway
+  for (int i = 0; i <= 79; ++i) {
+    double x = sl * i / 79.0;
+    traj.waypoints.push_back(make_wp(x, 0.0, height, 0.0, velocity));
+  }
+
+  // first turn: semi-circle at (sl, width/2), from -pi/2 to pi/2
+  int turn_n = 48;  // 50 - 2 endpoints already covered
+  for (int i = 1; i < turn_n + 1; ++i) {
+    double t = -M_PI / 2.0 + M_PI * i / (turn_n + 1);
+    double x = width / 2.0 * std::cos(t) + sl;
+    double y = width / 2.0 * std::sin(t) + width / 2.0;
+    double xd = -width / 2.0 * std::sin(t);
+    double yd = width / 2.0 * std::cos(t);
+    double yaw = std::atan2(yd, xd);
+    traj.waypoints.push_back(make_wp(x, y, height, yaw, turn_velocity));
+  }
+
+  // second straightaway
+  for (int i = 0; i <= 79; ++i) {
+    double x = sl * (1.0 - i / 79.0);
+    traj.waypoints.push_back(make_wp(x, width, height, M_PI, velocity));
+  }
+
+  // second turn: semi-circle at (0, width/2), from pi/2 to 3pi/2
+  for (int i = 1; i < turn_n + 1; ++i) {
+    double t = M_PI / 2.0 + M_PI * i / (turn_n + 1);
+    double x = width / 2.0 * std::cos(t);
+    double y = width / 2.0 * std::sin(t) + width / 2.0;
+    double xd = -width / 2.0 * std::sin(t);
+    double yd = width / 2.0 * std::cos(t);
+    double yaw = std::atan2(yd, xd) + M_PI;
+    traj.waypoints.push_back(make_wp(x, y, height, yaw, turn_velocity));
+  }
+
+  if (!traj.waypoints.empty()) {
+    traj.waypoints.front().velocity = 0.0;
+    traj.waypoints.back().velocity = 0.0;
+  }
+
+  get_accelerations(traj);
+  (void)max_acc;
+  return traj;
+}
+
+static TrajectoryXYZVYaw generate_line(const std::map<std::string, std::string> & attrs)
+{
+  std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link";
+  double length = std::stod(attrs.at("length"));
+  double height = std::stod(attrs.at("height"));
+  double velocity = std::stod(attrs.at("velocity"));
+  double max_acc = std::stod(attrs.at("max_acceleration"));
+
+  TrajectoryXYZVYaw traj;
+  traj.header.frame_id = frame_id;
+
+  for (double y = 0.0; y > -length; y -= 0.5) {
+    traj.waypoints.push_back(make_wp(-y, 0.0, height, 0.0));
+  }
+
+  get_velocities(traj, velocity, max_acc);
+  return traj;
+}
+
+static TrajectoryXYZVYaw generate_point(const std::map<std::string, std::string> & attrs)
+{
+  std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link";
+  double x = std::stod(attrs.at("x"));
+  double y = std::stod(attrs.at("y"));
+  double height = std::stod(attrs.at("height"));
+  double velocity = std::stod(attrs.at("velocity"));
+  double max_acc = std::stod(attrs.at("max_acceleration"));
+
+  TrajectoryXYZVYaw traj;
+  traj.header.frame_id = frame_id;
+  traj.waypoints.push_back(make_wp(x, y, height, 0.0));
+
+  get_velocities(traj, velocity, max_acc);
+  return traj;
+}
+
+static TrajectoryXYZVYaw generate_lawnmower(const std::map<std::string, std::string> & attrs)
+{
+  std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link";
+  double length = std::stod(attrs.at("length"));
+  double width = std::stod(attrs.at("width"));
+  double height = std::stod(attrs.at("height"));
+  double velocity = std::stod(attrs.at("velocity"));
+  bool vertical = (attrs.count("vertical") && std::stoi(attrs.at("vertical")) != 0);
+
+  TrajectoryXYZVYaw traj;
+  traj.header.frame_id = frame_id;
+
+  int strips = static_cast<int>(std::abs(height / width));
+  double h_sign = (height >= 0) ? 1.0 : -1.0;
+
+  for (int i = 0; i < strips; ++i) {
+    double z = h_sign * (i + 1) * width;
+    double yaw = std::atan2(length, 0.0);  // pointing along y
+
+    if (i % 2 == 0) {
+      // y goes 0 → length
+      traj.waypoints.push_back(make_wp(0.0, 0.0, z, vertical ? 0.0 : yaw, 0.1));
+      traj.waypoints.push_back(make_wp(0.0, 0.5, z, vertical ? 0.0 : yaw, 0.1));
+      traj.waypoints.push_back(make_wp(0.0, 0.5, z, vertical ? 0.0 : yaw, velocity));
+      traj.waypoints.push_back(make_wp(0.0, length - 0.5, z, vertical ? 0.0 : yaw, velocity));
+      traj.waypoints.push_back(make_wp(0.0, length, z, vertical ? 0.0 : yaw, 0.1));
+    } else {
+      // y goes length → 0
+      traj.waypoints.push_back(make_wp(0.0, length, z, vertical ? 0.0 : -yaw, 0.1));
+      traj.waypoints.push_back(make_wp(0.0, length - 0.5, z, vertical ? 0.0 : -yaw, 0.1));
+      traj.waypoints.push_back(make_wp(0.0, length - 0.5, z, vertical ? 0.0 : -yaw, velocity));
+      traj.waypoints.push_back(make_wp(0.0, 0.5, z, vertical ? 0.0 : -yaw, velocity));
+      traj.waypoints.push_back(make_wp(0.0, 0.0, z, vertical ? 0.0 : -yaw, 0.1));
+    }
+  }
+
+  // return waypoint
+  traj.waypoints.push_back(make_wp(0.0, 0.0, 0.0, 0.0, 0.5));
+
+  // if not vertical, swap x and z
+  if (!vertical) {
+    for (auto & wp : traj.waypoints) {
+      std::swap(wp.position.x, wp.position.z);
+    }
+  }
+
+  return traj;
+}
+
+// ─────────────────────────── action server node ───────────────────────────────
+
+class FixedTrajectoryTaskNode : public rclcpp::Node
+{
+public:
+  using FixedTrajectoryTask = task_msgs::action::FixedTrajectoryTask;
+  using GoalHandle = rclcpp_action::ServerGoalHandle<FixedTrajectoryTask>;
+
+  FixedTrajectoryTaskNode()
+  : rclcpp::Node("fixed_trajectory_task")
+  {
+    completion_sub_ = this->create_subscription<std_msgs::msg::Float32>(
+      "trajectory_completion_percentage", 1,
+      [this](std_msgs::msg::Float32::SharedPtr msg) {
+        std::lock_guard<std::mutex> lock(completion_mutex_);
+        completion_percentage_ = msg->data;
+      });
+
+    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+      "odometry", rclcpp::QoS(1).best_effort(),
+      [this](nav_msgs::msg::Odometry::SharedPtr msg) {
+        std::lock_guard<std::mutex> lock(odom_mutex_);
+        current_odom_ = *msg;
+      });
+
+    traj_override_pub_ =
+      this->create_publisher<TrajectoryXYZVYaw>("trajectory_override", 1);
+
+    traj_mode_client_ =
+      this->create_client<airstack_msgs::srv::TrajectoryMode>("set_trajectory_mode");
+
+    action_server_ = rclcpp_action::create_server<FixedTrajectoryTask>(
+      this, "~/fixed_trajectory_task",
+      std::bind(&FixedTrajectoryTaskNode::handle_goal, this,
+        std::placeholders::_1, std::placeholders::_2),
+      std::bind(&FixedTrajectoryTaskNode::handle_cancel, this, std::placeholders::_1),
+      std::bind(&FixedTrajectoryTaskNode::handle_accepted, this, std::placeholders::_1));
+
+    RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTaskNode started");
+  }
+
+private:
+  std::atomic<bool> task_active_{false};
+  std::atomic<bool> cancel_requested_{false};
+
+  std::mutex completion_mutex_;
+  float completion_percentage_{0.0f};
+
+  std::mutex odom_mutex_;
+  nav_msgs::msg::Odometry current_odom_;
+
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr completion_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<TrajectoryXYZVYaw>::SharedPtr traj_override_pub_;
+  rclcpp::Client<airstack_msgs::srv::TrajectoryMode>::SharedPtr traj_mode_client_;
+  rclcpp_action::Server<FixedTrajectoryTask>::SharedPtr action_server_;
+
+  bool set_trajectory_mode(int32_t mode)
+  {
+    if (!traj_mode_client_->wait_for_service(std::chrono::seconds(2))) {
+      RCLCPP_ERROR(this->get_logger(), "set_trajectory_mode service not available");
+      return false;
+    }
+    auto req = std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
+    req->mode = mode;
+    auto future = traj_mode_client_->async_send_request(req);
+    future.wait();
+    return future.get()->success;
+  }
+
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID &,
+    std::shared_ptr<const FixedTrajectoryTask::Goal> goal)
+  {
+    if (task_active_) {
+      RCLCPP_WARN(this->get_logger(), "FixedTrajectoryTask rejected: task already active");
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    // validate trajectory type
+    const std::string & type = goal->trajectory_spec.type;
+    if (type != "Figure8" && type != "Circle" && type != "Racetrack" &&
+      type != "Line" && type != "Point" && type != "Lawnmower")
+    {
+      RCLCPP_WARN(this->get_logger(),
+        "FixedTrajectoryTask rejected: unknown trajectory type '%s'", type.c_str());
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    task_active_ = true;
+    cancel_requested_ = false;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<GoalHandle>)
+  {
+    cancel_requested_ = true;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handle_accepted(std::shared_ptr<GoalHandle> goal_handle)
+  {
+    std::thread{
+      std::bind(&FixedTrajectoryTaskNode::execute, this, std::placeholders::_1),
+      goal_handle}.detach();
+  }
+
+  void execute(std::shared_ptr<GoalHandle> goal_handle)
+  {
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<FixedTrajectoryTask::Result>();
+    auto feedback = std::make_shared<FixedTrajectoryTask::Feedback>();
+
+    // parse attributes into a map
+    std::map<std::string, std::string> attrs;
+    for (const auto & kv : goal->trajectory_spec.attributes) {
+      attrs[kv.key] = kv.value;
+    }
+
+    // generate trajectory
+    TrajectoryXYZVYaw traj;
+    const std::string & type = goal->trajectory_spec.type;
+    try {
+      if (type == "Circle") {
+        traj = generate_circle(attrs);
+      } else if (type == "Figure8") {
+        traj = generate_figure8(attrs);
+      } else if (type == "Racetrack") {
+        traj = generate_racetrack(attrs);
+      } else if (type == "Line") {
+        traj = generate_line(attrs);
+      } else if (type == "Point") {
+        traj = generate_point(attrs);
+      } else if (type == "Lawnmower") {
+        traj = generate_lawnmower(attrs);
+      }
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(this->get_logger(), "FixedTrajectoryTask: trajectory generation failed: %s",
+        e.what());
+      result->success = false;
+      result->message = std::string("trajectory generation failed: ") + e.what();
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+
+    if (traj.waypoints.empty()) {
+      result->success = false;
+      result->message = "trajectory generation produced no waypoints";
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+
+    traj.header.stamp = this->now();
+
+    // set trajectory mode to TRACK and publish
+    if (!set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::TRACK)) {
+      result->success = false;
+      result->message = "failed to set trajectory mode";
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+
+    traj_override_pub_->publish(traj);
+    RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTask: executing %s (%zu waypoints)",
+      type.c_str(), traj.waypoints.size());
+
+    // reset completion percentage after publishing
+    {
+      std::lock_guard<std::mutex> lock(completion_mutex_);
+      completion_percentage_ = 0.0f;
+    }
+
+    // monitor completion
+    rclcpp::Rate rate(5);
+    while (rclcpp::ok()) {
+      if (cancel_requested_) {
+        set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+        result->success = false;
+        result->message = "cancelled";
+        goal_handle->canceled(result);
+        task_active_ = false;
+        return;
+      }
+
+      float completion;
+      {
+        std::lock_guard<std::mutex> lock(completion_mutex_);
+        completion = completion_percentage_;
+      }
+
+      nav_msgs::msg::Odometry odom;
+      {
+        std::lock_guard<std::mutex> lock(odom_mutex_);
+        odom = current_odom_;
+      }
+
+      feedback->progress = completion / 100.0f;
+      feedback->status = type + " trajectory running";
+      feedback->current_position.x = odom.pose.pose.position.x;
+      feedback->current_position.y = odom.pose.pose.position.y;
+      feedback->current_position.z = odom.pose.pose.position.z;
+      goal_handle->publish_feedback(feedback);
+
+      if (completion >= 100.0f) {
+        if (goal->loop) {
+          // republish and reset
+          traj.header.stamp = this->now();
+          traj_override_pub_->publish(traj);
+          {
+            std::lock_guard<std::mutex> lock(completion_mutex_);
+            completion_percentage_ = 0.0f;
+          }
+          RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTask: looping %s", type.c_str());
+        } else {
+          RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTask: %s complete", type.c_str());
+          set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+          result->success = true;
+          result->message = type + " trajectory complete";
+          goal_handle->succeed(result);
+          task_active_ = false;
+          return;
+        }
+      }
+
+      rate.sleep();
+    }
+
+    set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+    result->success = false;
+    result->message = "node shutting down";
+    goal_handle->abort(result);
+    task_active_ = false;
+  }
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<FixedTrajectoryTaskNode>();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml
index 9c7283035..fd46b9282 100644
--- a/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml
+++ b/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml
@@ -15,29 +15,41 @@
     <log message="local_odometry_in_topic has $(var local_odometry_in_topic)" />
 
 
-    <!-- Basic Planners -->
-    <node pkg="trajectory_library"
-        namespace="fixed_trajectory_generator" exec="fixed_trajectory_generator.py" output="screen">
+    <!-- Takeoff/Landing Task Action Servers -->
+    <node pkg="takeoff_landing_planner" exec="takeoff_landing_task"
+          namespace="takeoff_landing_planner" output="screen">
+        <param from="$(find-pkg-share takeoff_landing_planner)/config/takeoff_landing_planner.yaml"
+            allow_substs="true" />
         <remap from="trajectory_override"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_override" />
+        <remap from="set_trajectory_mode"
+            to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
+        <remap from="tracking_point"
+            to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
+        <remap from="odometry" to="$(var local_odometry_in_topic)" />
+        <remap from="is_armed" to="/$(env ROBOT_NAME)/interface/is_armed" />
+        <remap from="has_control" to="/$(env ROBOT_NAME)/interface/has_control" />
+        <remap from="state_estimate_timed_out"
+            to="/$(env ROBOT_NAME)/behavior/drone_safety_monitor/state_estimate_timed_out" />
+        <remap from="extended_state"
+            to="/$(env ROBOT_NAME)/interface/mavros/extended_state" />
+        <remap from="robot_command"
+            to="/$(env ROBOT_NAME)/interface/robot_command" />
+        <remap from="~/takeoff_task" to="/$(env ROBOT_NAME)/tasks/takeoff" />
+        <remap from="~/land_task" to="/$(env ROBOT_NAME)/tasks/land" />
     </node>
 
-    <!-- customize takeoff_landing_planner.launch.xml -->
-    <node pkg="takeoff_landing_planner"
-        namespace="takeoff_landing_planner"
-        exec="takeoff_landing_planner" output="screen">
-        <param from="$(find-pkg-share takeoff_landing_planner)/config/takeoff_landing_planner.yaml"
-            allow_substs="true" />
-
-        <!-- remap glue to AirStack -->
+    <!-- Fixed Trajectory Task Action Server -->
+    <node pkg="trajectory_controller" exec="fixed_trajectory_task"
+          namespace="trajectory_controller" output="screen">
         <remap from="trajectory_override"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_override" />
+        <remap from="set_trajectory_mode"
+            to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
         <remap from="trajectory_completion_percentage"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_completion_percentage" />
-        <remap from="tracking_point"
-            to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
         <remap from="odometry" to="$(var local_odometry_in_topic)" />
-        <remap from="mavros/cmd/takeoff" to="/$(env ROBOT_NAME)/interface/mavros/cmd/takeoff" />
+        <remap from="~/fixed_trajectory_task" to="/$(env ROBOT_NAME)/tasks/fixed_trajectory" />
     </node>
 
     <group>
diff --git a/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml b/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml
index a65d20b96..453674779 100644
--- a/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml
+++ b/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml
@@ -15,29 +15,41 @@
     <log message="local_odometry_in_topic has $(var local_odometry_in_topic)" />
 
 
-    <!-- Basic Planners -->
-    <node pkg="trajectory_library"
-        namespace="fixed_trajectory_generator" exec="fixed_trajectory_generator.py" output="screen">
+    <!-- Takeoff/Landing Task Action Servers -->
+    <node pkg="takeoff_landing_planner" exec="takeoff_landing_task"
+          namespace="takeoff_landing_planner" output="screen">
+        <param from="$(find-pkg-share takeoff_landing_planner)/config/takeoff_landing_planner.yaml"
+            allow_substs="true" />
         <remap from="trajectory_override"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_override" />
+        <remap from="set_trajectory_mode"
+            to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
+        <remap from="tracking_point"
+            to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
+        <remap from="odometry" to="$(var local_odometry_in_topic)" />
+        <remap from="is_armed" to="/$(env ROBOT_NAME)/interface/is_armed" />
+        <remap from="has_control" to="/$(env ROBOT_NAME)/interface/has_control" />
+        <remap from="state_estimate_timed_out"
+            to="/$(env ROBOT_NAME)/behavior/drone_safety_monitor/state_estimate_timed_out" />
+        <remap from="extended_state"
+            to="/$(env ROBOT_NAME)/interface/mavros/extended_state" />
+        <remap from="robot_command"
+            to="/$(env ROBOT_NAME)/interface/robot_command" />
+        <remap from="~/takeoff_task" to="/$(env ROBOT_NAME)/tasks/takeoff" />
+        <remap from="~/land_task" to="/$(env ROBOT_NAME)/tasks/land" />
     </node>
 
-    <!-- customize takeoff_landing_planner.launch.xml -->
-    <node pkg="takeoff_landing_planner"
-        namespace="takeoff_landing_planner"
-        exec="takeoff_landing_planner" output="screen">
-        <param from="$(find-pkg-share takeoff_landing_planner)/config/takeoff_landing_planner.yaml"
-            allow_substs="true" />
-
-        <!-- remap glue to AirStack -->
+    <!-- Fixed Trajectory Task Action Server -->
+    <node pkg="trajectory_controller" exec="fixed_trajectory_task"
+          namespace="trajectory_controller" output="screen">
         <remap from="trajectory_override"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_override" />
+        <remap from="set_trajectory_mode"
+            to="/$(env ROBOT_NAME)/trajectory_controller/set_trajectory_mode" />
         <remap from="trajectory_completion_percentage"
             to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_completion_percentage" />
-        <remap from="tracking_point"
-            to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
         <remap from="odometry" to="$(var local_odometry_in_topic)" />
-        <remap from="mavros/cmd/takeoff" to="/$(env ROBOT_NAME)/interface/mavros/cmd/takeoff" />
+        <remap from="~/fixed_trajectory_task" to="/$(env ROBOT_NAME)/tasks/fixed_trajectory" />
     </node>
 
     <group>
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt b/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt
index 1adb3320b..2022599a1 100644
--- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt
@@ -8,18 +8,21 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(airstack_msgs REQUIRED)
 find_package(airstack_common REQUIRED)
 find_package(trajectory_library REQUIRED)
 find_package(mavros_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(task_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 
 add_executable(takeoff_landing_planner src/takeoff_landing_planner.cpp)
 target_include_directories(takeoff_landing_planner PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-target_compile_features(takeoff_landing_planner PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_compile_features(takeoff_landing_planner PUBLIC c_std_99 cxx_std_17)
 ament_target_dependencies(
   takeoff_landing_planner
   std_msgs
@@ -30,7 +33,25 @@ ament_target_dependencies(
   std_srvs
 )
 
-install(TARGETS takeoff_landing_planner
+add_executable(takeoff_landing_task src/takeoff_landing_task.cpp)
+target_include_directories(takeoff_landing_task PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(takeoff_landing_task PUBLIC c_std_99 cxx_std_17)
+ament_target_dependencies(
+  takeoff_landing_task
+  rclcpp
+  rclcpp_action
+  std_msgs
+  nav_msgs
+  airstack_msgs
+  airstack_common
+  trajectory_library
+  mavros_msgs
+  task_msgs
+)
+
+install(TARGETS takeoff_landing_planner takeoff_landing_task
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md b/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md
index 3cb5ca33e..d1505bfe2 100644
--- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md
@@ -2,116 +2,119 @@
 
 ## Overview
 
-The TakeoffLandingPlanner provides a rich interface for handling takeoff/landing commands, with support for both trajectory-based and ArduPilot command-based modes. It monitors the robot's position, generates appropriate trajectories, and tracks the completion status of takeoff and landing maneuvers.
+The TakeoffLandingPlanner provides ROS 2 action servers for takeoff and landing maneuvers. It monitors the robot's state, generates trajectory overrides to reach the target altitude (takeoff) or ground (landing), and tracks completion via position and time thresholds.
 
-## Highlights
+## Features
 
-- Two takeoff modes: standard and high altitude
-- Configurable takeoff and landing velocities
-- Configurable takeoff trajectory direction
-- Trajectory-based takeoff and landing with position tracking
-- Direct ArduPilot takeoff command support
-- Completion monitoring with distance and time thresholds
-- State publishing for integration with higher-level systems
+- Trajectory-based takeoff to a configurable target altitude and velocity
+- Trajectory-based landing with on-ground detection, followed by automatic disarm
+- Configurable takeoff trajectory direction (roll/pitch, absolute or body-relative)
+- Precondition checking: rejects goals when state estimate is timed out or a task is already running
+- Publishes `is_airborne` for downstream consumers (e.g. the RViz Tasks Panel)
 
 ## Topics
 
 ### Subscriptions
 
-| Topic                              | Type                     | Description                                                                                                                                      |
-| ---------------------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `trajectory_completion_percentage` | `std_msgs/Float32`       | Current completion percentage of trajectory (from [trajectory_controller](../../c_controls/trajectory_controller/src/trajectory_controller.cpp)) |
-| `tracking_point`                   | `airstack_msgs/Odometry` | Current tracking point for trajectory (from [trajectory_controller](../../c_controls/trajectory_controller/src/trajectory_controller.cpp))       |
-| `odometry`                         | `nav_msgs/Odometry`      | Robot odometry data                                                                                                                              |
-| `ekf_active`                       | `std_msgs/Bool`          | EKF status flag                                                                                                                                  |
-| `high_takeoff`                     | `std_msgs/Bool`          | Flag to trigger high altitude takeoff                                                                                                            |
+| Topic                              | Type                              | Description                                          |
+|------------------------------------|-----------------------------------|------------------------------------------------------|
+| `odometry`                         | `nav_msgs/Odometry`               | Robot pose and velocity                              |
+| `tracking_point`                   | `airstack_msgs/Odometry`          | Current trajectory tracking point                   |
+| `trajectory_completion_percentage` | `std_msgs/Float32`                | Trajectory completion (0–100) from trajectory_controller |
+| `is_armed`                         | `std_msgs/Bool`                   | Vehicle arm status                                   |
+| `has_control`                      | `std_msgs/Bool`                   | Offboard control acquired                            |
+| `state_estimate_timed_out`         | `std_msgs/Bool`                   | Whether the state estimate has timed out             |
+| `extended_state`                   | `mavros_msgs/ExtendedState`       | MAVROS landed state (used to publish `is_airborne`)  |
 
 ### Publications
 
-| Topic                 | Type                              | Description                                              |
-| --------------------- | --------------------------------- | -------------------------------------------------------- |
-| `trajectory_override` | `airstack_msgs/TrajectoryXYZVYaw` | Generated takeoff/landing trajectory                     |
-| `takeoff_state`       | `std_msgs/String`                 | Current takeoff state ("NONE", "TAKING_OFF", "COMPLETE") |
-| `landing_state`       | `std_msgs/String`                 | Current landing state ("NONE", "LANDING", "COMPLETE")    |
+| Topic               | Type                              | Description                                             |
+|---------------------|-----------------------------------|---------------------------------------------------------|
+| `trajectory_override` | `airstack_msgs/TrajectoryXYZVYaw` | Takeoff/landing trajectory sent to trajectory_controller |
+| `is_airborne`       | `std_msgs/Bool`                   | True when `landed_state == LANDED_STATE_IN_AIR`         |
 
-## Services
+## Service Clients
 
-| Service                       | Type                                  | Description                               |
-| ----------------------------- | ------------------------------------- | ----------------------------------------- |
-| `set_takeoff_landing_command` | `airstack_msgs/TakeoffLandingCommand` | Sets command mode (NONE, TAKEOFF, LAND)   |
-| `ardupilot_takeoff`           | `std_srvs/Trigger`                    | Triggers direct ArduPilot takeoff command |
+| Service               | Type                              | Description                            |
+|-----------------------|-----------------------------------|----------------------------------------|
+| `set_trajectory_mode` | `airstack_msgs/TrajectoryMode`    | Switch trajectory controller mode      |
+| `robot_command`       | `airstack_msgs/RobotCommand`      | Arm, request offboard control, disarm  |
 
-## Parameters
+## Action Servers
 
-| Parameter                              | Type  | Default | Description                                             |
-| -------------------------------------- | ----- | ------- | ------------------------------------------------------- |
-| `takeoff_height`                       | float | 0.5     | Standard takeoff height in meters                       |
-| `high_takeoff_height`                  | float | 1.2     | High altitude takeoff height in meters                  |
-| `takeoff_velocity`                     | float | 0.3     | Velocity during takeoff in m/s                          |
-| `landing_velocity`                     | float | 0.3     | Velocity during landing in m/s                          |
-| `takeoff_acceptance_distance`          | float | 0.1     | Maximum distance to consider takeoff complete in meters |
-| `takeoff_acceptance_time`              | float | 2.0     | Required time at acceptance distance in seconds         |
-| `landing_stationary_distance`          | float | 0.02    | Maximum movement distance to consider landed in meters  |
-| `landing_acceptance_time`              | float | 5.0     | Required time at stationary distance in seconds         |
-| `landing_tracking_point_ahead_time`    | float | 5.0     | How far ahead the tracking point should be              |
-| `takeoff_path_roll`                    | float | 0.0     | Roll angle for takeoff path in degrees                  |
-| `takeoff_path_pitch`                   | float | 0.0     | Pitch angle for takeoff path in degrees                 |
-| `takeoff_path_relative_to_orientation` | bool  | false   | Whether to use robot's current orientation for takeoff  |
+| Action          | Type                      | Description                  |
+|-----------------|---------------------------|------------------------------|
+| `~/takeoff_task` | `task_msgs/TakeoffTask`  | Execute a takeoff maneuver   |
+| `~/land_task`    | `task_msgs/LandTask`     | Execute a landing maneuver   |
 
-## Usage
+### TakeoffTask
 
-### Basic Usage
+**Goal**
 
-To launch the TakeoffLandingPlanner node:
+| Field               | Type    | Description                    |
+|---------------------|---------|--------------------------------|
+| `target_altitude_m` | float32 | Target altitude in meters      |
+| `velocity_m_s`      | float32 | Ascent velocity in m/s         |
 
-```bash
-ros2 run takeoff_landing_planner takeoff_landing_planner
-```
+**Feedback**
 
-### Initiating Takeoff
+| Field                | Type    | Description                    |
+|----------------------|---------|--------------------------------|
+| `status`             | string  | Human-readable status message  |
+| `current_altitude_m` | float32 | Current altitude in meters     |
+| `target_altitude_m`  | float32 | Target altitude in meters      |
 
-To command a takeoff:
+**Result**
 
-```bash
-ros2 service call /set_takeoff_landing_command airstack_msgs/srv/TakeoffLandingCommand "{command: 1}"
-```
+| Field     | Type    | Description              |
+|-----------|---------|--------------------------|
+| `success` | bool    | Whether takeoff succeeded |
+| `message` | string  | Outcome description      |
 
-### Initiating Landing
+### LandTask
 
-To command a landing:
+**Goal**
 
-```bash
-ros2 service call /set_takeoff_landing_command airstack_msgs/srv/TakeoffLandingCommand "{command: 2}"
-```
+| Field          | Type    | Description              |
+|----------------|---------|--------------------------|
+| `velocity_m_s` | float32 | Descent velocity in m/s  |
 
-### ArduPilot Takeoff
+**Feedback**
 
-To use direct ArduPilot takeoff commands:
+| Field                | Type    | Description                   |
+|----------------------|---------|-------------------------------|
+| `status`             | string  | Human-readable status message |
+| `current_altitude_m` | float32 | Current altitude in meters    |
 
-```bash
-ros2 service call /ardupilot_takeoff std_srvs/srv/Trigger "{}"
-```
+**Result**
 
-## State Monitoring
+| Field     | Type   | Description               |
+|-----------|--------|---------------------------|
+| `success` | bool   | Whether landing succeeded |
+| `message` | string | Outcome description       |
 
-The node publishes the current state of takeoff and landing operations through the `takeoff_state` and `landing_state` topics. Monitor these topics to track the progress and completion of operations:
+## Parameters
 
-```bash
-ros2 topic echo /takeoff_state
-ros2 topic echo /landing_state
-```
+| Parameter                              | Type  | Default | Description                                                   |
+|----------------------------------------|-------|---------|---------------------------------------------------------------|
+| `takeoff_velocity`                     | float | 1.0     | Default ascent velocity in m/s                                |
+| `landing_velocity`                     | float | 0.3     | Default descent velocity in m/s                               |
+| `takeoff_acceptance_distance`          | float | 0.3     | Distance threshold to consider target altitude reached (m)    |
+| `takeoff_acceptance_time`              | float | 1.0     | Time that must be spent within acceptance distance (s)        |
+| `landing_stationary_distance`          | float | 0.02    | Max movement to consider the drone stationary on landing (m)  |
+| `landing_acceptance_time`              | float | 5.0     | Time the drone must remain stationary to confirm landing (s)  |
+| `landing_tracking_point_ahead_time`    | float | 5.0     | Lookahead time for the landing tracking point                 |
+| `takeoff_path_roll`                    | float | 0.0     | Roll offset for the takeoff trajectory (degrees)              |
+| `takeoff_path_pitch`                   | float | 0.0     | Pitch offset for the takeoff trajectory (degrees)             |
+| `takeoff_path_relative_to_orientation` | bool  | false   | Apply roll/pitch relative to the robot's current orientation  |
 
 ## Dependencies
 
-- airstack_msgs
-- nav_msgs
-- mavros_msgs
-- std_msgs
-- std_srvs
-- tf2_ros
-
-## Notes
-
-- The node uses TF2 to track relative positions between the robot and tracking points
-- Landing completion detection relies on both position stability and tracking point metrics
-- The default landing target is hardcoded (-10000.0) and should be parameterized in future versions
+- `rclcpp` / `rclcpp_action`
+- `airstack_msgs`
+- `airstack_common`
+- `nav_msgs`
+- `mavros_msgs`
+- `std_msgs`
+- `task_msgs`
+- `trajectory_library`
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_task.hpp b/robot/ros_ws/src/local/planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_task.hpp
new file mode 100644
index 000000000..7873484ce
--- /dev/null
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_task.hpp
@@ -0,0 +1,143 @@
+// Copyright (c) 2024 Carnegie Mellon University
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include <atomic>
+#include <deque>
+#include <mutex>
+#include <string>
+
+#include <airstack_common/tflib.hpp>
+#include <airstack_msgs/msg/odometry.hpp>
+#include <airstack_msgs/msg/trajectory_xyzv_yaw.hpp>
+#include <airstack_msgs/srv/robot_command.hpp>
+#include <airstack_msgs/srv/trajectory_mode.hpp>
+#include <mavros_msgs/msg/extended_state.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <trajectory_library/trajectory_library.hpp>
+
+#include "task_msgs/action/land_task.hpp"
+#include "task_msgs/action/takeoff_task.hpp"
+
+class TakeoffLandingTaskNode : public rclcpp::Node
+{
+public:
+  using TakeoffTask = task_msgs::action::TakeoffTask;
+  using TakeoffGoalHandle = rclcpp_action::ServerGoalHandle<TakeoffTask>;
+  using LandTask = task_msgs::action::LandTask;
+  using LandGoalHandle = rclcpp_action::ServerGoalHandle<LandTask>;
+
+  TakeoffLandingTaskNode();
+
+private:
+  // parameters
+  double default_takeoff_velocity_;
+  double default_landing_velocity_;
+  double takeoff_acceptance_distance_;
+  double takeoff_acceptance_time_;
+  double landing_stationary_distance_;
+  double landing_acceptance_time_;
+  double landing_tracking_point_ahead_time_;
+  double takeoff_path_roll_;
+  double takeoff_path_pitch_;
+  bool takeoff_path_relative_to_orientation_;
+
+  // shared state (protected by mutex)
+  std::mutex odom_mutex_;
+  nav_msgs::msg::Odometry robot_odom_;
+  bool got_robot_odom_{false};
+
+  std::mutex tracking_point_mutex_;
+  airstack_msgs::msg::Odometry tracking_point_odom_;
+  bool got_tracking_point_{false};
+
+  std::mutex completion_mutex_;
+  float completion_percentage_{0.0f};
+  bool got_completion_percentage_{false};
+
+  // precondition state (atomic for lock-free reads in handle_goal)
+  std::atomic<bool> is_armed_{false};
+  std::atomic<bool> has_control_{false};
+  std::atomic<bool> state_estimate_timed_out_{false};
+
+  // landed state from mavros
+  std::atomic<uint8_t> landed_state_{0};  // mavros_msgs::msg::ExtendedState::LANDED_STATE_UNDEFINED
+
+  // task exclusion
+  std::atomic<bool> task_active_{false};
+  std::atomic<bool> cancel_requested_{false};
+
+  // set by land_execute on success; cleared by takeoff_execute on start.
+  // prevents extended_state callback from re-publishing is_airborne=true
+  // after MAVROS transiently reverts to IN_AIR post-landing.
+  std::atomic<bool> landed_{false};
+
+  // subscribers
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr robot_odom_sub_;
+  rclcpp::Subscription<airstack_msgs::msg::Odometry>::SharedPtr tracking_point_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr is_armed_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr has_control_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr state_estimate_timed_out_sub_;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr completion_percentage_sub_;
+  rclcpp::Subscription<mavros_msgs::msg::ExtendedState>::SharedPtr extended_state_sub_;
+
+  // publishers
+  rclcpp::Publisher<airstack_msgs::msg::TrajectoryXYZVYaw>::SharedPtr traj_override_pub_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr is_airborne_pub_;
+
+  // service clients
+  rclcpp::Client<airstack_msgs::srv::TrajectoryMode>::SharedPtr traj_mode_client_;
+  rclcpp::Client<airstack_msgs::srv::RobotCommand>::SharedPtr robot_command_client_;
+
+  // action servers
+  rclcpp_action::Server<TakeoffTask>::SharedPtr takeoff_server_;
+  rclcpp_action::Server<LandTask>::SharedPtr land_server_;
+
+  // subscription callbacks
+  void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void tracking_point_callback(const airstack_msgs::msg::Odometry::SharedPtr msg);
+  void completion_percentage_callback(const std_msgs::msg::Float32::SharedPtr msg);
+
+  // helpers
+  bool set_trajectory_mode(int32_t mode);
+  bool send_robot_command(uint8_t command);
+
+  // TakeoffTask action server callbacks
+  rclcpp_action::GoalResponse takeoff_handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const TakeoffTask::Goal> goal);
+  rclcpp_action::CancelResponse takeoff_handle_cancel(
+    std::shared_ptr<TakeoffGoalHandle> goal_handle);
+  void takeoff_handle_accepted(std::shared_ptr<TakeoffGoalHandle> goal_handle);
+  void takeoff_execute(std::shared_ptr<TakeoffGoalHandle> goal_handle);
+
+  // LandTask action server callbacks
+  rclcpp_action::GoalResponse land_handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const LandTask::Goal> goal);
+  rclcpp_action::CancelResponse land_handle_cancel(
+    std::shared_ptr<LandGoalHandle> goal_handle);
+  void land_handle_accepted(std::shared_ptr<LandGoalHandle> goal_handle);
+  void land_execute(std::shared_ptr<LandGoalHandle> goal_handle);
+};
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml b/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml
index 410fb2b74..a718f1309 100644
--- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml
@@ -10,12 +10,15 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
   <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>airstack_msgs</depend>
   <depend>airstack_common</depend>
   <depend>trajectory_library</depend>
   <depend>mavros_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>task_msgs</depend>
   
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/src/takeoff_landing_task.cpp b/robot/ros_ws/src/local/planners/takeoff_landing_planner/src/takeoff_landing_task.cpp
new file mode 100644
index 000000000..3e736d05a
--- /dev/null
+++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/src/takeoff_landing_task.cpp
@@ -0,0 +1,509 @@
+// Copyright (c) 2024 Carnegie Mellon University
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <takeoff_landing_planner/takeoff_landing_task.hpp>
+
+#include <airstack_common/ros2_helper.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <thread>
+
+TakeoffLandingTaskNode::TakeoffLandingTaskNode()
+: rclcpp::Node("takeoff_landing_task")
+{
+  // parameters
+  default_takeoff_velocity_ = airstack::get_param(this, "takeoff_velocity", 1.0);
+  default_landing_velocity_ = airstack::get_param(this, "landing_velocity", 0.3);
+  takeoff_acceptance_distance_ = airstack::get_param(this, "takeoff_acceptance_distance", 0.3);
+  takeoff_acceptance_time_ = airstack::get_param(this, "takeoff_acceptance_time", 1.0);
+  landing_stationary_distance_ = airstack::get_param(this, "landing_stationary_distance", 0.02);
+  landing_acceptance_time_ = airstack::get_param(this, "landing_acceptance_time", 5.0);
+  landing_tracking_point_ahead_time_ =
+    airstack::get_param(this, "landing_tracking_point_ahead_time", 5.0);
+  takeoff_path_roll_ = airstack::get_param(this, "takeoff_path_roll", 0.0) * M_PI / 180.0;
+  takeoff_path_pitch_ = airstack::get_param(this, "takeoff_path_pitch", 0.0) * M_PI / 180.0;
+  takeoff_path_relative_to_orientation_ =
+    airstack::get_param(this, "takeoff_path_relative_to_orientation", false);
+
+  // subscribers
+  auto sensor_qos = rclcpp::QoS(1);
+  sensor_qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
+
+  robot_odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "odometry", sensor_qos,
+    std::bind(&TakeoffLandingTaskNode::odom_callback, this, std::placeholders::_1));
+
+  tracking_point_sub_ = this->create_subscription<airstack_msgs::msg::Odometry>(
+    "tracking_point", 1,
+    std::bind(&TakeoffLandingTaskNode::tracking_point_callback, this, std::placeholders::_1));
+
+  completion_percentage_sub_ = this->create_subscription<std_msgs::msg::Float32>(
+    "trajectory_completion_percentage", 1,
+    std::bind(
+      &TakeoffLandingTaskNode::completion_percentage_callback, this, std::placeholders::_1));
+
+  is_armed_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+    "is_armed", 1,
+    [this](std_msgs::msg::Bool::SharedPtr msg) { is_armed_ = msg->data; });
+
+  has_control_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+    "has_control", 1,
+    [this](std_msgs::msg::Bool::SharedPtr msg) { has_control_ = msg->data; });
+
+  state_estimate_timed_out_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+    "state_estimate_timed_out", 1,
+    [this](std_msgs::msg::Bool::SharedPtr msg) { state_estimate_timed_out_ = msg->data; });
+
+  extended_state_sub_ = this->create_subscription<mavros_msgs::msg::ExtendedState>(
+    "extended_state", 1,
+    [this](mavros_msgs::msg::ExtendedState::SharedPtr msg) {
+      landed_state_ = msg->landed_state;
+      bool in_air = (msg->landed_state == mavros_msgs::msg::ExtendedState::LANDED_STATE_IN_AIR);
+      if (!in_air) {
+        // Definitive non-airborne state from MAVROS — always publish false.
+        std_msgs::msg::Bool airborne_msg;
+        airborne_msg.data = false;
+        is_airborne_pub_->publish(airborne_msg);
+      } else if (!landed_) {
+        // IN_AIR and no confirmed landing — publish true.
+        std_msgs::msg::Bool airborne_msg;
+        airborne_msg.data = true;
+        is_airborne_pub_->publish(airborne_msg);
+      }
+      // If landed_ is set and MAVROS reports IN_AIR, publish nothing:
+      // land_execute already published false; MAVROS is transiently wrong.
+    });
+
+  // publishers
+  traj_override_pub_ =
+    this->create_publisher<airstack_msgs::msg::TrajectoryXYZVYaw>("trajectory_override", 1);
+  is_airborne_pub_ = this->create_publisher<std_msgs::msg::Bool>("is_airborne", 1);
+
+  // service clients
+  traj_mode_client_ =
+    this->create_client<airstack_msgs::srv::TrajectoryMode>("set_trajectory_mode");
+  robot_command_client_ =
+    this->create_client<airstack_msgs::srv::RobotCommand>("robot_command");
+
+  // action servers
+  takeoff_server_ = rclcpp_action::create_server<TakeoffTask>(
+    this, "~/takeoff_task",
+    std::bind(&TakeoffLandingTaskNode::takeoff_handle_goal, this,
+      std::placeholders::_1, std::placeholders::_2),
+    std::bind(&TakeoffLandingTaskNode::takeoff_handle_cancel, this, std::placeholders::_1),
+    std::bind(&TakeoffLandingTaskNode::takeoff_handle_accepted, this, std::placeholders::_1));
+
+  land_server_ = rclcpp_action::create_server<LandTask>(
+    this, "~/land_task",
+    std::bind(&TakeoffLandingTaskNode::land_handle_goal, this,
+      std::placeholders::_1, std::placeholders::_2),
+    std::bind(&TakeoffLandingTaskNode::land_handle_cancel, this, std::placeholders::_1),
+    std::bind(&TakeoffLandingTaskNode::land_handle_accepted, this, std::placeholders::_1));
+
+  RCLCPP_INFO(this->get_logger(), "TakeoffLandingTaskNode started");
+}
+
+// ─────────────────────────── subscription callbacks ───────────────────────────
+
+void TakeoffLandingTaskNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(odom_mutex_);
+  robot_odom_ = *msg;
+  got_robot_odom_ = true;
+}
+
+void TakeoffLandingTaskNode::tracking_point_callback(
+  const airstack_msgs::msg::Odometry::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(tracking_point_mutex_);
+  tracking_point_odom_ = *msg;
+  got_tracking_point_ = true;
+}
+
+void TakeoffLandingTaskNode::completion_percentage_callback(
+  const std_msgs::msg::Float32::SharedPtr msg)
+{
+  std::lock_guard<std::mutex> lock(completion_mutex_);
+  completion_percentage_ = msg->data;
+  got_completion_percentage_ = true;
+}
+
+// ─────────────────────────── helper ───────────────────────────────────────────
+
+bool TakeoffLandingTaskNode::set_trajectory_mode(int32_t mode)
+{
+  if (!traj_mode_client_->wait_for_service(std::chrono::seconds(2))) {
+    RCLCPP_ERROR(this->get_logger(), "set_trajectory_mode service not available");
+    return false;
+  }
+  auto request = std::make_shared<airstack_msgs::srv::TrajectoryMode::Request>();
+  request->mode = mode;
+  auto future = traj_mode_client_->async_send_request(request);
+  future.wait();
+  return future.get()->success;
+}
+
+bool TakeoffLandingTaskNode::send_robot_command(uint8_t command)
+{
+  if (!robot_command_client_->wait_for_service(std::chrono::seconds(2))) {
+    RCLCPP_ERROR(this->get_logger(), "robot_command service not available");
+    return false;
+  }
+  auto request = std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
+  request->command = command;
+  auto future = robot_command_client_->async_send_request(request);
+  future.wait();
+  return future.get()->success;
+}
+
+// ─────────────────────────── TakeoffTask ──────────────────────────────────────
+
+rclcpp_action::GoalResponse TakeoffLandingTaskNode::takeoff_handle_goal(
+  const rclcpp_action::GoalUUID &,
+  std::shared_ptr<const TakeoffTask::Goal> goal)
+{
+  if (task_active_) {
+    RCLCPP_WARN(this->get_logger(), "TakeoffTask rejected: another task is already active");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  if (state_estimate_timed_out_) {
+    RCLCPP_WARN(this->get_logger(), "TakeoffTask rejected: state estimate timed out");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  if (goal->target_altitude_m <= 0.0f) {
+    RCLCPP_WARN(this->get_logger(), "TakeoffTask rejected: target_altitude_m must be positive");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  task_active_ = true;
+  cancel_requested_ = false;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse TakeoffLandingTaskNode::takeoff_handle_cancel(
+  std::shared_ptr<TakeoffGoalHandle>)
+{
+  cancel_requested_ = true;
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void TakeoffLandingTaskNode::takeoff_handle_accepted(std::shared_ptr<TakeoffGoalHandle> goal_handle)
+{
+  std::thread{
+    std::bind(&TakeoffLandingTaskNode::takeoff_execute, this, std::placeholders::_1),
+    goal_handle}.detach();
+}
+
+void TakeoffLandingTaskNode::takeoff_execute(std::shared_ptr<TakeoffGoalHandle> goal_handle)
+{
+  const auto goal = goal_handle->get_goal();
+  float target_altitude = goal->target_altitude_m;
+  float velocity = (goal->velocity_m_s > 0.0f) ? goal->velocity_m_s : default_takeoff_velocity_;
+
+  auto result = std::make_shared<TakeoffTask::Result>();
+  auto feedback = std::make_shared<TakeoffTask::Feedback>();
+  feedback->target_altitude_m = target_altitude;
+
+  landed_ = false;  // clear latch — a new takeoff is starting
+
+  // wait for odometry
+  rclcpp::Rate wait_rate(10);
+  int wait_count = 0;
+  while (!got_robot_odom_ && rclcpp::ok()) {
+    if (wait_count++ > 50) {
+      RCLCPP_ERROR(this->get_logger(), "TakeoffTask aborted: no odometry received");
+      result->success = false;
+      result->message = "no odometry received";
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+    wait_rate.sleep();
+  }
+
+  // arm the robot
+  if (!is_armed_) {
+    RCLCPP_INFO(this->get_logger(), "TakeoffTask: arming robot");
+    if (!send_robot_command(airstack_msgs::srv::RobotCommand::Request::ARM)) {
+      RCLCPP_ERROR(this->get_logger(), "TakeoffTask aborted: failed to arm");
+      result->success = false;
+      result->message = "failed to arm";
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+  }
+
+  // request offboard control
+  if (!has_control_) {
+    RCLCPP_INFO(this->get_logger(), "TakeoffTask: requesting offboard control");
+    if (!send_robot_command(airstack_msgs::srv::RobotCommand::Request::REQUEST_CONTROL)) {
+      RCLCPP_ERROR(this->get_logger(), "TakeoffTask aborted: failed to request offboard control");
+      result->success = false;
+      result->message = "failed to request offboard control";
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+  }
+
+  // set trajectory mode to TRACK
+  if (!set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::TRACK)) {
+    result->success = false;
+    result->message = "failed to set trajectory mode";
+    goal_handle->abort(result);
+    task_active_ = false;
+    return;
+  }
+
+  // generate and publish takeoff trajectory
+  {
+    std::lock_guard<std::mutex> odom_lock(odom_mutex_);
+    std::lock_guard<std::mutex> tp_lock(tracking_point_mutex_);
+
+    airstack_msgs::msg::Odometry start_point;
+    if (got_tracking_point_) {
+      start_point = tracking_point_odom_;
+    } else {
+      // fall back to current robot odom as start point
+      start_point.header = robot_odom_.header;
+      start_point.pose.position.x = robot_odom_.pose.pose.position.x;
+      start_point.pose.position.y = robot_odom_.pose.pose.position.y;
+      start_point.pose.position.z = robot_odom_.pose.pose.position.z;
+      start_point.pose.orientation = robot_odom_.pose.pose.orientation;
+    }
+
+    // height is relative offset; target_altitude_m is absolute
+    float current_z = robot_odom_.pose.pose.position.z;
+    float relative_height = target_altitude - current_z;
+    if (relative_height <= 0.0f) {
+      RCLCPP_WARN(this->get_logger(),
+        "TakeoffTask: target_altitude_m (%.2f) is not above current altitude (%.2f)",
+        target_altitude, current_z);
+      relative_height = 0.5f;  // minimum ascent
+    }
+
+    if (takeoff_path_relative_to_orientation_) {
+      start_point.pose.orientation = robot_odom_.pose.pose.orientation;
+    }
+
+    TakeoffTrajectory traj_gen(
+      relative_height, velocity,
+      takeoff_path_roll_, takeoff_path_pitch_,
+      takeoff_path_relative_to_orientation_);
+    traj_override_pub_->publish(traj_gen.get_trajectory(start_point));
+  }
+
+  RCLCPP_INFO(this->get_logger(), "TakeoffTask: ascending to %.2fm at %.2f m/s",
+    target_altitude, velocity);
+
+  // monitor completion
+  rclcpp::Rate rate(10);  // 10 Hz feedback
+  rclcpp::Time acceptance_start;
+  bool in_acceptance_window = false;
+
+  while (rclcpp::ok()) {
+    if (cancel_requested_) {
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+      result->success = false;
+      result->message = "cancelled";
+      goal_handle->canceled(result);
+      task_active_ = false;
+      return;
+    }
+
+    float current_z;
+    {
+      std::lock_guard<std::mutex> lock(odom_mutex_);
+      current_z = robot_odom_.pose.pose.position.z;
+    }
+
+    feedback->current_altitude_m = current_z;
+    goal_handle->publish_feedback(feedback);
+
+    // check completion: within acceptance distance of target for acceptance_time
+    float dist = std::abs(current_z - target_altitude);
+    if (dist <= takeoff_acceptance_distance_) {
+      if (!in_acceptance_window) {
+        in_acceptance_window = true;
+        acceptance_start = this->now();
+      }
+      if ((this->now() - acceptance_start).seconds() >= takeoff_acceptance_time_) {
+        RCLCPP_INFO(this->get_logger(), "TakeoffTask: complete at %.2fm", current_z);
+        set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+        result->success = true;
+        result->message = "takeoff complete";
+        goal_handle->succeed(result);
+        task_active_ = false;
+        return;
+      }
+    } else {
+      in_acceptance_window = false;
+    }
+
+    rate.sleep();
+  }
+
+  set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+  result->success = false;
+  result->message = "node shutting down";
+  goal_handle->abort(result);
+  task_active_ = false;
+}
+
+// ─────────────────────────── LandTask ─────────────────────────────────────────
+
+rclcpp_action::GoalResponse TakeoffLandingTaskNode::land_handle_goal(
+  const rclcpp_action::GoalUUID &,
+  std::shared_ptr<const LandTask::Goal>)
+{
+  if (task_active_) {
+    RCLCPP_WARN(this->get_logger(), "LandTask rejected: another task is already active");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  task_active_ = true;
+  cancel_requested_ = false;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse TakeoffLandingTaskNode::land_handle_cancel(
+  std::shared_ptr<LandGoalHandle>)
+{
+  cancel_requested_ = true;
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void TakeoffLandingTaskNode::land_handle_accepted(std::shared_ptr<LandGoalHandle> goal_handle)
+{
+  std::thread{
+    std::bind(&TakeoffLandingTaskNode::land_execute, this, std::placeholders::_1),
+    goal_handle}.detach();
+}
+
+void TakeoffLandingTaskNode::land_execute(std::shared_ptr<LandGoalHandle> goal_handle)
+{
+  const auto goal = goal_handle->get_goal();
+  float velocity = (goal->velocity_m_s > 0.0f) ? goal->velocity_m_s : default_landing_velocity_;
+
+  auto result = std::make_shared<LandTask::Result>();
+  auto feedback = std::make_shared<LandTask::Feedback>();
+
+  // wait for odometry
+  rclcpp::Rate wait_rate(10);
+  int wait_count = 0;
+  while (!got_robot_odom_ && rclcpp::ok()) {
+    if (wait_count++ > 50) {
+      RCLCPP_ERROR(this->get_logger(), "LandTask aborted: no odometry received");
+      result->success = false;
+      result->message = "no odometry received";
+      goal_handle->abort(result);
+      task_active_ = false;
+      return;
+    }
+    wait_rate.sleep();
+  }
+
+  // set trajectory mode to TRACK
+  if (!set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::TRACK)) {
+    result->success = false;
+    result->message = "failed to set trajectory mode";
+    goal_handle->abort(result);
+    task_active_ = false;
+    return;
+  }
+
+  // generate and publish landing trajectory
+  {
+    std::lock_guard<std::mutex> tp_lock(tracking_point_mutex_);
+    airstack_msgs::msg::Odometry start_point;
+    if (got_tracking_point_) {
+      start_point = tracking_point_odom_;
+    } else {
+      std::lock_guard<std::mutex> odom_lock(odom_mutex_);
+      start_point.header = robot_odom_.header;
+      start_point.pose.position.x = robot_odom_.pose.pose.position.x;
+      start_point.pose.position.y = robot_odom_.pose.pose.position.y;
+      start_point.pose.position.z = robot_odom_.pose.pose.position.z;
+      start_point.pose.orientation = robot_odom_.pose.pose.orientation;
+    }
+    TakeoffTrajectory land_traj(-10000.0, velocity);
+    traj_override_pub_->publish(land_traj.get_trajectory(start_point));
+  }
+
+  RCLCPP_INFO(this->get_logger(), "LandTask: descending at %.2f m/s", velocity);
+
+  rclcpp::Rate rate(10);
+  while (rclcpp::ok()) {
+    if (cancel_requested_) {
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+      result->success = false;
+      result->message = "cancelled";
+      goal_handle->canceled(result);
+      task_active_ = false;
+      return;
+    }
+
+    float current_z;
+    {
+      std::lock_guard<std::mutex> lock(odom_mutex_);
+      current_z = robot_odom_.pose.pose.position.z;
+    }
+
+    feedback->current_altitude_m = current_z;
+    feedback->status = "landing";
+    goal_handle->publish_feedback(feedback);
+
+    // check if mavros reports on-ground
+    if (landed_state_ == mavros_msgs::msg::ExtendedState::LANDED_STATE_ON_GROUND) {
+      RCLCPP_INFO(this->get_logger(), "LandTask: landed (mavros landed_state = ON_GROUND) at %.2fm",
+        current_z);
+      set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+      send_robot_command(airstack_msgs::srv::RobotCommand::Request::DISARM);
+      landed_ = true;
+      std_msgs::msg::Bool airborne_msg;
+      airborne_msg.data = false;
+      is_airborne_pub_->publish(airborne_msg);
+      result->success = true;
+      result->message = "landing complete";
+      goal_handle->succeed(result);
+      task_active_ = false;
+      return;
+    }
+
+    rate.sleep();
+  }
+
+  set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE);
+  result->success = false;
+  result->message = "node shutting down";
+  goal_handle->abort(result);
+  task_active_ = false;
+}
+
+// ─────────────────────────── main ─────────────────────────────────────────────
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<TakeoffLandingTaskNode>();
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt b/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt
index b582b711f..cc014b395 100644
--- a/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt
+++ b/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt
@@ -62,7 +62,6 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME})
 
 
-install(PROGRAMS scripts/fixed_trajectory_generator.py DESTINATION lib/${PROJECT_NAME})
 
 ament_export_include_directories(
   include
diff --git a/robot/ros_ws/src/local/planners/trajectory_library/scripts/fixed_trajectory_generator.py b/robot/ros_ws/src/local/planners/trajectory_library/scripts/fixed_trajectory_generator.py
deleted file mode 100755
index 958a405a1..000000000
--- a/robot/ros_ws/src/local/planners/trajectory_library/scripts/fixed_trajectory_generator.py
+++ /dev/null
@@ -1,551 +0,0 @@
-#!/usr/bin/python3
-import numpy as np
-from nav_msgs.msg import Odometry
-from geometry_msgs.msg import Point
-from airstack_msgs.msg import WaypointXYZVYaw
-from airstack_msgs.msg import TrajectoryXYZVYaw
-from airstack_msgs.msg import FixedTrajectory
-import time
-import copy
-import rclpy
-from rclpy.node import Node
-
-logger = None
-
-def get_velocities(traj, velocity, max_acc):
-    v_prev = 0.0
-
-    for i in range(len(traj.waypoints)):
-        j = (i + 1) % len(traj.waypoints)
-        dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-
-        dist = np.sqrt(dx**2 + dy**2)
-        v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-        traj.waypoints[i].velocity = min(velocity, v_limit)
-        v_prev = traj.waypoints[i].velocity
-
-
-def get_velocities_dual(traj, velocity, max_acc):
-    v_prev = 0.0
-
-    for i in range(len(traj.waypoints)):
-        j = (i + 1) % len(traj.waypoints)
-        dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-
-        dist = np.sqrt(dx**2 + dy**2)
-        v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-        traj.waypoints[i].velocity = min(velocity, v_limit)
-        v_prev = traj.waypoints[i].velocity
-
-    v_prev = 0.0
-    for i in range(len(traj.waypoints) - 1, int(len(traj.waypoints) * 0.85), -1):
-        j = (i - 1) % len(traj.waypoints)
-        dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-
-        dist = np.sqrt(dx**2 + dy**2)
-        v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-        traj.waypoints[i].velocity = min(velocity, v_limit)
-        v_prev = traj.waypoints[i].velocity
-
-    traj.waypoints[len(traj.waypoints) - 1].velocity = 0.0
-
-def get_velocities_adjust(traj, max_acc):
-    slow_down_adjust = []
-    speed_up_adjust = []
-    
-    for i in range(len(traj.waypoints)):
-        if i != len(traj.waypoints)-1:
-            if traj.waypoints[i].velocity > traj.waypoints[i+1].velocity:
-                v_start = traj.waypoints[i+1].velocity
-                v_target = traj.waypoints[i].velocity
-                slow_down_adjust.append((i+1, v_start, v_target))
-            elif traj.waypoints[i].velocity < traj.waypoints[i+1].velocity:
-                v_start = traj.waypoints[i].velocity
-                v_target = traj.waypoints[i+1].velocity
-                speed_up_adjust.append((i, v_start, v_target))
-
-    for start_index, v_start, v_target in slow_down_adjust:
-        prev_i = start_index
-        v_prev = v_start
-        for i in range(start_index-1, -1, -1):
-            dx = traj.waypoints[prev_i].position.x - traj.waypoints[i].position.x
-            dy = traj.waypoints[prev_i].position.y - traj.waypoints[i].position.y
-            dist = np.sqrt(dx**2 + dy**2)
-            v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-            traj.waypoints[i].velocity = min(v_target, v_limit)
-            v_prev = traj.waypoints[i].velocity
-            if v_prev >= v_target:
-                break
-
-    for start_index, v_start, v_target in speed_up_adjust:
-        prev_i = start_index
-        v_prev = v_start
-        for i in range(start_index+1, len(traj.waypoints)):
-            dx = traj.waypoints[prev_i].position.x - traj.waypoints[i].position.x
-            dy = traj.waypoints[prev_i].position.y - traj.waypoints[i].position.y
-            dist = np.sqrt(dx**2 + dy**2)
-            v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-            if v_limit > traj.waypoints[i].velocity:
-                break
-            traj.waypoints[i].velocity = min(v_target, v_limit)
-            v_prev = traj.waypoints[i].velocity
-            if v_prev >= v_target:
-                break
-
-
-def get_accelerations(traj):
-    a_prev = 0.0
-
-    for i in range(len(traj.waypoints) - 2):
-        j = (i + 1) % len(traj.waypoints)
-        k = (i + 2) % len(traj.waypoints)
-
-        dx1 = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy1 = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-        dist1 = np.sqrt(dx1**2 + dy1**2)
-
-        v_currx = traj.waypoints[i].velocity * (dx1) / dist1
-        v_curry = traj.waypoints[i].velocity * (dy1) / dist1
-
-        dx2 = traj.waypoints[k].position.x - traj.waypoints[j].position.x
-        dy2 = traj.waypoints[k].position.y - traj.waypoints[j].position.y
-        dist2 = np.sqrt(dx2**2 + dy2**2)
-
-        v_nextx = traj.waypoints[j].velocity * (dx2) / dist2
-        v_nexty = traj.waypoints[j].velocity * (dy2) / dist2
-
-        acc_limit = 50.
-        if abs(dx1 - 0) < 1e-6:
-            traj.waypoints[i].acceleration.x = 0.0
-        else:
-            traj.waypoints[i].acceleration.x = min(
-                (v_nextx**2 - v_currx**2) / (2 * dx1), acc_limit
-            )
-
-        if abs(dy1 - 0) < 1e-6:
-            traj.waypoints[i].acceleration.y = 0.0
-        else:
-            traj.waypoints[i].acceleration.y = min(
-                (v_nexty**2 - v_curry**2) / (2 * dy1), acc_limit
-            )
-
-        traj.waypoints[i].acceleration.z = 0.0
-
-    traj.waypoints[len(traj.waypoints) - 2].acceleration.x = 0.0
-    traj.waypoints[len(traj.waypoints) - 2].acceleration.y = 0.0
-    traj.waypoints[len(traj.waypoints) - 2].acceleration.z = 0.0
-    traj.waypoints[len(traj.waypoints) - 1].acceleration.x = 0.0
-    traj.waypoints[len(traj.waypoints) - 1].acceleration.y = 0.0
-    traj.waypoints[len(traj.waypoints) - 1].acceleration.z = 0.0
-
-
-def get_racetrack_waypoints(attributes):  # length, width, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    width = float(attributes["width"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    turn_velocity = float(attributes["turn_velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-    straightaway_length = length - width
-
-    # first straightaway
-    xs1 = np.linspace(0, straightaway_length, 80)
-    ys1 = np.zeros(xs1.shape)
-    vs1 = np.ones(xs1.shape)*velocity
-    yaws1 = np.zeros(xs1.shape)
-
-    # first turn
-    t = np.linspace(-np.pi / 2, np.pi / 2, 50)[1:-1]
-    xs2 = width / 2.0 * np.cos(t) + straightaway_length
-    ys2 = width / 2.0 * np.sin(t) + width / 2.0
-    xs2d = -width / 2.0 * np.sin(t)  # derivative of xs
-    ys2d = width / 2.0 * np.cos(t)  # derivative of ys
-    vs2 = np.ones(xs2.shape)*turn_velocity
-    yaws2 = np.arctan2(ys2d, xs2d)
-
-    # second straightaway
-    xs3 = np.linspace(straightaway_length, 0, 80)
-    ys3 = width * np.ones(xs3.shape)
-    vs3 = np.ones(xs3.shape)*velocity
-    yaws3 = np.pi * np.ones(xs3.shape)
-
-    # second turn
-    t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1]
-    xs4 = width / 2.0 * np.cos(t)
-    ys4 = width / 2.0 * np.sin(t) + width / 2.0
-    vs4 = np.ones(xs4.shape)*turn_velocity
-    yaws4 = yaws2 + np.pi
-
-    xs = np.hstack((xs1, xs2, xs3, xs4))
-    ys = np.hstack((ys1, ys2, ys3, ys4))
-    vs = np.hstack((vs1, vs2, vs3, vs4))
-    vs[0] = 0.
-    vs[-1] = 0.
-    yaws = np.hstack((yaws1, yaws2, yaws3, yaws4))
-
-    # now = rospy.Time.now()
-    for i in range(xs.shape[0]):
-        wp = WaypointXYZVYaw()
-        wp.position.x = xs[i]
-        wp.position.y = ys[i]
-        wp.position.z = height
-        wp.velocity = vs[i]
-
-        #logger.info(str(i) + ' ' + str(wp.velocity))
-        
-        wp.yaw = yaws[i]
-
-        traj.waypoints.append(wp)
-
-    #get_velocities_dual(traj, velocity, max_acceleration)
-    get_velocities_adjust(traj, max_acceleration)
-    get_accelerations(traj)
-
-    return traj
-
-
-def get_figure8_waypoints(attributes):  # length, width, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    width = float(attributes["width"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-    # now = rospy.Time.now()
-
-    # figure 8 points
-    t = np.linspace(0, 2 * np.pi, 600)
-    x = np.cos(t) * length - length
-    y = np.cos(t) * np.sin(t) * 2 * width
-
-    # derivative of figure 8 curve, used to find yaw
-    xd = -np.sin(t) * length
-    yd = (np.cos(t) ** 2 - np.sin(t) ** 2) * 2 * width
-
-    for i in range(t.shape[0] - 1):
-        x1 = x[i]
-        y1 = y[i]
-        x2 = x1 + xd[i]
-        y2 = y1 + yd[i]
-
-        yaw = np.arctan2(y2 - y1, x2 - x1)
-
-        wp = WaypointXYZVYaw()
-        wp.position.x = x1
-        wp.position.y = y1
-        wp.position.z = height
-        wp.yaw = yaw
-
-        traj.waypoints.append(wp)
-
-    get_velocities_dual(traj, velocity, max_acceleration)
-    get_accelerations(traj)
-
-    return traj
-
-
-def get_line_waypoints(attributes):  # length, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    for y in np.arange(0, -length, -0.5):
-        wp = WaypointXYZVYaw()
-        wp.position.x = -y
-        wp.position.y = 0.
-        wp.position.z = height
-        wp.yaw = 0.
-
-        traj.waypoints.append(wp)
-
-    get_velocities(traj, velocity, max_acceleration)
-
-    return traj
-
-
-def get_point_waypoints(attributes):  # length, height):
-    frame_id = str(attributes["frame_id"])
-    x = float(attributes["x"])
-    y = float(attributes["y"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    # add first point
-    wp = WaypointXYZVYaw()
-    wp.position.x = x
-    wp.position.y = y
-    wp.position.z = height
-    wp.yaw = 0.
-
-    traj.waypoints.append(wp)
-
-    get_velocities(traj, velocity, max_acceleration)
-
-    return traj
-
-
-def get_box_waypoints(attributes):  # length, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    wp1 = WaypointXYZVYaw()
-    wp1.position.x = 0.0
-    wp1.position.y = 0.0
-    wp1.position.z = height
-    wp1.yaw = 0
-    traj.waypoints.append(wp1)
-
-    wp2 = WaypointXYZVYaw()
-    wp2.position.x = length
-    wp2.position.y = 0.0
-    wp2.position.z = height
-    wp2.yaw = 0.0
-    traj.waypoints.append(wp2)
-
-    wp3 = WaypointXYZVYaw()
-    wp3.position.x = length
-    wp3.position.y = 0.0
-    wp3.position.z = height + height
-    wp3.yaw = 0.0
-    traj.waypoints.append(wp3)
-
-    wp4 = WaypointXYZVYaw()
-    wp4.position.x = 0.0
-    wp4.position.y = 0.0
-    wp4.position.z = height + height
-    wp4.yaw = 0.0
-    traj.waypoints.append(wp4)
-
-    return traj
-
-
-def get_lawnmower_waypoints(attributes):  # length, width, height, velocity):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    width = float(attributes["width"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    vertical = bool(int(attributes["vertical"]))
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    for i in range(abs(int(height / width))):
-        wp1 = WaypointXYZVYaw()
-        wp1.position.x = 0.0
-        wp1.position.y = 0.0
-        wp1.position.z = np.sign(height) * (i + 1) * width
-        wp1.yaw = 0.0
-        wp1.velocity = 0.1
-
-        wp1_ = WaypointXYZVYaw()
-        wp1_.position.x = 0.0
-        wp1_.position.y = 0.5
-        wp1_.position.z = np.sign(height) * (i + 1) * width
-        wp1_.yaw = 0.0
-        wp1_.velocity = velocity
-
-        wp2 = WaypointXYZVYaw()
-        wp2.position.x = 0.0
-        wp2.position.y = length
-        wp2.position.z = np.sign(height) * (i + 1) * width
-        wp2.yaw = 0.0
-        wp2.velocity = 0.1
-
-        wp2_ = WaypointXYZVYaw()
-        wp2_.position.x = 0.0
-        wp2_.position.y = length - 0.5
-        wp2_.position.z = np.sign(height) * (i + 1) * width
-        wp2_.yaw = 0.0
-        wp2_.velocity = velocity
-
-        yaw = np.arctan2(wp2.position.y - wp1.position.y, wp2.position.z - wp1.position.z)
-
-        if i % 2 == 0:
-            if not vertical:
-                wp1.yaw = yaw
-                wp1_.yaw = yaw
-                wp2_.yaw = yaw
-                wp2.yaw = yaw
-            
-            traj.waypoints.append(wp1)
-            wp1_slow = copy.deepcopy(wp1_)
-            wp1_slow.velocity = 0.1
-            traj.waypoints.append(wp1_slow)
-            traj.waypoints.append(wp1_)
-            traj.waypoints.append(wp2_)
-            traj.waypoints.append(wp2)
-        else:
-            if not vertical:
-                wp2.yaw = -yaw
-                wp2_.yaw = -yaw
-                wp1_.yaw = -yaw
-                wp1.yaw = -yaw
-            
-            traj.waypoints.append(wp2)
-            wp2_slow = copy.deepcopy(wp2_)
-            wp2_slow.velocity = 0.1
-            traj.waypoints.append(wp2_slow)
-            traj.waypoints.append(wp2_)
-            traj.waypoints.append(wp1_)
-            traj.waypoints.append(wp1)
-
-    wp = WaypointXYZVYaw()
-    wp.position.x = 0.0
-    wp.position.y = 0.0
-    wp.position.z = 0.0
-    wp.yaw = 0.0
-    wp.velocity = 0.5
-    traj.waypoints.append(wp)
-    
-    if not vertical:
-        for i, wp in enumerate(traj.waypoints):
-            wp.position.x, wp.position.y, wp.position.z = wp.position.z, wp.position.y, wp.position.x
-
-    return traj
-
-
-def get_circle_waypoints(attributes):  # radius, velocity, frame_id):
-    frame_id = str(attributes["frame_id"])
-    radius = float(attributes["radius"])
-    velocity = float(attributes["velocity"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-    # traj.header.stamp = rospy.Time.now()
-
-    wp0 = WaypointXYZVYaw()
-    wp0.position.x = 0.0
-    wp0.position.y = 0.0
-    wp0.position.z = 0.0
-    wp0.yaw = 0.0
-    wp0.velocity = velocity
-    traj.waypoints.append(wp0)
-
-    wp1 = WaypointXYZVYaw()
-    wp1.position.x = radius
-    wp1.position.y = 0.0
-    wp1.position.z = 0.0
-    wp1.yaw = 0.0
-    wp1.velocity = velocity
-    traj.waypoints.append(wp1)
-
-    for angle in np.arange(0, 2 * np.pi, 10.0 * np.pi / 180.0):
-        wp = WaypointXYZVYaw()
-        wp.position.x = radius * np.cos(angle)
-        wp.position.y = radius * np.sin(angle)
-        wp.position.z = 0.0
-        wp.yaw = 0.0
-        wp.velocity = velocity
-        traj.waypoints.append(wp)
-
-    wp_end0 = WaypointXYZVYaw()
-    wp_end0.position.x = radius
-    wp_end0.position.y = 0.0
-    wp_end0.position.z = 0.0
-    wp_end0.yaw = 0.0
-    wp_end0.velocity = velocity
-    traj.waypoints.append(wp_end0)
-
-    wp_end1 = WaypointXYZVYaw()
-    wp_end1.position.x = 0.0
-    wp_end1.position.y = 0.0
-    wp_end1.position.z = 0.0
-    wp_end1.yaw = 0.0
-    wp_end1.velocity = velocity
-    traj.waypoints.append(wp_end1)
-
-    return traj
-
-
-class FixedTrajectoryGenerator(Node):
-    def __init__(self):
-        super().__init__("fixed_trajectory_generator")
-        self.fixed_trajectory_sub = self.create_subscription(
-            FixedTrajectory,
-            "fixed_trajectory_command",
-            self.fixed_trajectory_callback,
-            1,
-        )
-        self.trajectory_override_pub = self.create_publisher(
-            TrajectoryXYZVYaw, "trajectory_override", 1
-        )
-
-    def fixed_trajectory_callback(self, msg):
-        print("generating")
-        attributes = {}
-
-        for key_value in msg.attributes:
-            attributes[key_value.key] = key_value.value
-
-        trajectory_msg = None
-
-        if msg.type == "Figure8":
-            trajectory_msg = get_figure8_waypoints(attributes)
-        elif msg.type == "Circle":
-            trajectory_msg = get_circle_waypoints(attributes)
-        elif msg.type == "Racetrack":
-            trajectory_msg = get_racetrack_waypoints(attributes)
-        elif msg.type == "Line":
-            trajectory_msg = get_line_waypoints(attributes)
-        elif msg.type == "Point":
-            trajectory_msg = get_point_waypoints(attributes)
-        elif msg.type == "Lawnmower":
-            trajectory_msg = get_lawnmower_waypoints(attributes)
-
-        if trajectory_msg != None:
-            self.trajectory_override_pub.publish(trajectory_msg)
-        else:
-            print("No trajectory sent.")
-
-#import matplotlib.pyplot as plt
-
-if __name__ == "__main__":
-    '''
-    dct = {'frame_id': 'base_link', 'length': 2, 'width': 1, 'height': 0, 'velocity': 2, 'turn_velocity': 0.5, 'max_acceleration': 0.1}
-    traj1 = get_racetrack_waypoints(dct)
-    dct['max_acceleration'] = 100000000.
-    traj2 = get_racetrack_waypoints(dct)
-
-    v1 = [wp.velocity for wp in traj1.waypoints]
-    v2 = [wp.velocity for wp in traj2.waypoints]
-
-    plt.plot(v1, '.-')
-    plt.plot(v2, '.-')
-    plt.show()
-    exit()
-
-    '''
-    rclpy.init(args=None)
-
-    node = FixedTrajectoryGenerator()
-    logger = node.get_logger()
-    
-    rclpy.spin(node)
-    node.destroy_node()
-    rclpy.shutdown()
diff --git a/robot/ros_ws/src/local/planners/trajectory_library/src/fixed_trajectory_generator.py b/robot/ros_ws/src/local/planners/trajectory_library/src/fixed_trajectory_generator.py
deleted file mode 100755
index b13f7e7c8..000000000
--- a/robot/ros_ws/src/local/planners/trajectory_library/src/fixed_trajectory_generator.py
+++ /dev/null
@@ -1,453 +0,0 @@
-#!/usr/bin/python
-import numpy as np
-from nav_msgs.msg import Odometry
-from geometry_msgs.msg import Point
-import rospy
-import tf.transformations as trans
-import argparse
-
-# from at_fcs_mavros.msg import Trajectory
-# from at_fcs_mavros.msg import Waypoint
-# from ca_nav_msgs.msg import XYZVPsi
-# from ca_nav_msgs.msg import PathXYZVPsi
-from airstack_msgs.msg import WaypointXYZVYaw
-from airstack_msgs.msg import TrajectoryXYZVYaw
-from trajectory_controller.msg import Trajectory
-from airstack_msgs.msg import FixedTrajectory
-import time
-import copy
-
-
-def get_velocities(traj, velocity, max_acc):
-    v_prev = 0.0
-
-    for i in range(len(traj.waypoints)):
-        j = (i + 1) % len(traj.waypoints)
-        dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-
-        dist = np.sqrt(dx**2 + dy**2)
-        v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-        traj.waypoints[i].velocity = min(velocity, v_limit)
-        v_prev = traj.waypoints[i].velocity
-
-
-def get_velocities_dual(traj, velocity, max_acc):
-    v_prev = 0.0
-
-    for i in range(len(traj.waypoints)):
-        j = (i + 1) % len(traj.waypoints)
-        dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-
-        dist = np.sqrt(dx**2 + dy**2)
-        v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-        traj.waypoints[i].velocity = min(velocity, v_limit)
-        v_prev = traj.waypoints[i].velocity
-
-    v_prev = 0.0
-    for i in range(len(traj.waypoints) - 1, int(len(traj.waypoints) * 0.85), -1):
-        j = (i - 1) % len(traj.waypoints)
-        dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-
-        dist = np.sqrt(dx**2 + dy**2)
-        v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
-        traj.waypoints[i].velocity = min(velocity, v_limit)
-        v_prev = traj.waypoints[i].velocity
-
-    traj.waypoints[len(traj.waypoints) - 1].velocity = 0
-
-
-def get_accelerations(traj):
-    a_prev = 0.0
-
-    for i in range(len(traj.waypoints) - 2):
-        j = (i + 1) % len(traj.waypoints)
-        k = (i + 2) % len(traj.waypoints)
-
-        dx1 = traj.waypoints[j].position.x - traj.waypoints[i].position.x
-        dy1 = traj.waypoints[j].position.y - traj.waypoints[i].position.y
-        dist1 = np.sqrt(dx1**2 + dy1**2)
-
-        v_currx = traj.waypoints[i].velocity * (dx1) / dist1
-        v_curry = traj.waypoints[i].velocity * (dy1) / dist1
-
-        dx2 = traj.waypoints[k].position.x - traj.waypoints[j].position.x
-        dy2 = traj.waypoints[k].position.y - traj.waypoints[j].position.y
-        dist2 = np.sqrt(dx2**2 + dy2**2)
-
-        v_nextx = traj.waypoints[j].velocity * (dx2) / dist2
-        v_nexty = traj.waypoints[j].velocity * (dy2) / dist2
-
-        acc_limit = 50
-        if abs(dx1 - 0) < 1e-6:
-            traj.waypoints[i].acceleration.x = 0
-        else:
-            traj.waypoints[i].acceleration.x = min(
-                (v_nextx**2 - v_currx**2) / (2 * dx1), acc_limit
-            )
-
-        if abs(dy1 - 0) < 1e-6:
-            traj.waypoints[i].acceleration.y = 0
-        else:
-            traj.waypoints[i].acceleration.y = min(
-                (v_nexty**2 - v_curry**2) / (2 * dy1), acc_limit
-            )
-
-        traj.waypoints[i].acceleration.z = 0
-
-    traj.waypoints[len(traj.waypoints) - 2].acceleration.x = 0
-    traj.waypoints[len(traj.waypoints) - 2].acceleration.y = 0
-    traj.waypoints[len(traj.waypoints) - 2].acceleration.z = 0
-    traj.waypoints[len(traj.waypoints) - 1].acceleration.x = 0
-    traj.waypoints[len(traj.waypoints) - 1].acceleration.y = 0
-    traj.waypoints[len(traj.waypoints) - 1].acceleration.z = 0
-
-
-def get_racetrack_waypoints(attributes):  # length, width, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    width = float(attributes["width"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-    straightaway_length = length - width
-
-    # first straightaway
-    xs1 = np.linspace(0, straightaway_length, 80)
-    ys1 = np.zeros(xs1.shape)
-    yaws1 = np.zeros(xs1.shape)
-
-    # first turn
-    t = np.linspace(-np.pi / 2, np.pi / 2, 50)[1:-1]
-    xs2 = width / 2.0 * np.cos(t) + straightaway_length
-    ys2 = width / 2.0 * np.sin(t) + width / 2.0
-    xs2d = -width / 2.0 * np.sin(t)  # derivative of xs
-    ys2d = width / 2.0 * np.cos(t)  # derivative of ys
-    yaws2 = np.arctan2(ys2d, xs2d)
-
-    # second straightaway
-    xs3 = np.linspace(straightaway_length, 0, 80)
-    ys3 = width * np.ones(xs3.shape)
-    yaws3 = np.pi * np.ones(xs3.shape)
-
-    # second turn
-    t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1]
-    xs4 = width / 2.0 * np.cos(t)
-    ys4 = width / 2.0 * np.sin(t) + width / 2.0
-    yaws4 = yaws2 + np.pi
-
-    xs = np.hstack((xs1, xs2, xs3, xs4))
-    ys = np.hstack((ys1, ys2, ys3, ys4))
-    yaws = np.hstack((yaws1, yaws2, yaws3, yaws4))
-
-    now = rospy.Time.now()
-    for i in range(xs.shape[0]):
-        wp = WaypointXYZVYaw()
-        wp.position.x = xs[i]
-        wp.position.y = ys[i]
-        wp.position.z = height
-        wp.yaw = yaws[i]
-
-        traj.waypoints.append(wp)
-
-    get_velocities_dual(traj, velocity, max_acceleration)
-    get_accelerations(traj)
-
-    return traj
-
-
-def get_figure8_waypoints(attributes):  # length, width, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    width = float(attributes["width"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-    now = rospy.Time.now()
-
-    # figure 8 points
-    t = np.linspace(0, 2 * np.pi, 600)
-    x = np.cos(t) * length - length
-    y = np.cos(t) * np.sin(t) * 2 * width
-
-    # derivative of figure 8 curve, used to find yaw
-    xd = -np.sin(t) * length
-    yd = (np.cos(t) ** 2 - np.sin(t) ** 2) * 2 * width
-
-    for i in range(t.shape[0] - 1):
-        x1 = x[i]
-        y1 = y[i]
-        x2 = x1 + xd[i]
-        y2 = y1 + yd[i]
-
-        yaw = np.arctan2(y2 - y1, x2 - x1)
-
-        wp = WaypointXYZVYaw()
-        wp.position.x = x1
-        wp.position.y = y1
-        wp.position.z = height
-        wp.yaw = yaw
-
-        traj.waypoints.append(wp)
-
-    get_velocities_dual(traj, velocity, max_acceleration)
-    get_accelerations(traj)
-
-    return traj
-
-
-def get_line_waypoints(attributes):  # length, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    for y in np.arange(0, -length, -0.5):
-        wp = WaypointXYZVYaw()
-        wp.position.x = -y
-        wp.position.y = 0
-        wp.position.z = height
-        wp.yaw = 0
-
-        traj.waypoints.append(wp)
-
-    get_velocities(traj, velocity, max_acceleration)
-
-    return traj
-
-
-def get_point_waypoints(attributes):  # length, height):
-    frame_id = str(attributes["frame_id"])
-    x = float(attributes["x"])
-    y = float(attributes["y"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    # add first point
-    wp = WaypointXYZVYaw()
-    wp.position.x = x
-    wp.position.y = y
-    wp.position.z = height
-    wp.yaw = 0
-
-    traj.waypoints.append(wp)
-
-    get_velocities(traj, velocity, max_acceleration)
-
-    return traj
-
-
-def get_box_waypoints(attributes):  # length, height):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-    max_acceleration = float(attributes["max_acceleration"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    wp1 = WaypointXYZVYaw()
-    wp1.position.x = 0.0
-    wp1.position.y = 0.0
-    wp1.position.z = height
-    wp1.yaw = 0
-    traj.waypoints.append(wp1)
-
-    wp2 = WaypointXYZVYaw()
-    wp2.position.x = length
-    wp2.position.y = 0.0
-    wp2.position.z = height
-    wp2.yaw = 0
-    traj.waypoints.append(wp2)
-
-    wp3 = WaypointXYZVYaw()
-    wp3.position.x = length
-    wp3.position.y = 0.0
-    wp3.position.z = height + height
-    wp3.yaw = 0
-    traj.waypoints.append(wp3)
-
-    wp4 = WaypointXYZVYaw()
-    wp4.position.x = 0.0
-    wp4.position.y = 0.0
-    wp4.position.z = height + height
-    wp4.yaw = 0
-    traj.waypoints.append(wp4)
-
-    return traj
-
-
-def get_vertical_lawnmower_waypoints(attributes):  # length, width, height, velocity):
-    frame_id = str(attributes["frame_id"])
-    length = float(attributes["length"])
-    width = float(attributes["width"])
-    height = float(attributes["height"])
-    velocity = float(attributes["velocity"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-
-    for i in range(abs(int(height / width))):
-        wp1 = WaypointXYZVYaw()
-        wp1.position.x = 0
-        wp1.position.y = 0
-        wp1.position.z = np.sign(height) * (i + 1) * width
-        wp1.yaw = 0
-        wp1.velocity = 0.1
-
-        wp1_ = WaypointXYZVYaw()
-        wp1_.position.x = 0
-        wp1_.position.y = 0.5
-        wp1_.position.z = np.sign(height) * (i + 1) * width
-        wp1_.yaw = 0
-        wp1_.velocity = velocity
-
-        wp2 = WaypointXYZVYaw()
-        wp2.position.x = 0
-        wp2.position.y = length
-        wp2.position.z = np.sign(height) * (i + 1) * width
-        wp2.yaw = 0
-        wp2.velocity = 0.1
-
-        wp2_ = WaypointXYZVYaw()
-        wp2_.position.x = 0
-        wp2_.position.y = length - 0.5
-        wp2_.position.z = np.sign(height) * (i + 1) * width
-        wp2_.yaw = 0
-        wp2_.velocity = velocity
-
-        if i % 2 == 0:
-            traj.waypoints.append(wp1)
-            wp1_slow = copy.deepcopy(wp1_)
-            wp1_slow.velocity = 0.1
-            traj.waypoints.append(wp1_slow)
-            traj.waypoints.append(wp1_)
-            traj.waypoints.append(wp2_)
-            traj.waypoints.append(wp2)
-        else:
-            traj.waypoints.append(wp2)
-            wp2_slow = copy.deepcopy(wp2_)
-            wp2_slow.velocity = 0.1
-            traj.waypoints.append(wp2_slow)
-            traj.waypoints.append(wp2_)
-            traj.waypoints.append(wp1_)
-            traj.waypoints.append(wp1)
-
-    wp = WaypointXYZVYaw()
-    wp.position.x = 0
-    wp.position.y = 0
-    wp.position.z = 0
-    wp.yaw = 0
-    wp.velocity = 0.5
-    traj.waypoints.append(wp)
-
-    return traj
-
-
-def get_circle_waypoints(attributes):  # radius, velocity, frame_id):
-    frame_id = str(attributes["frame_id"])
-    radius = float(attributes["radius"])
-    velocity = float(attributes["velocity"])
-
-    traj = TrajectoryXYZVYaw()
-    traj.header.frame_id = frame_id
-    traj.header.stamp = rospy.Time.now()
-
-    wp0 = WaypointXYZVYaw()
-    wp0.position.x = 0
-    wp0.position.y = 0
-    wp0.position.z = 0
-    wp0.yaw = 0
-    wp0.velocity = velocity
-    traj.waypoints.append(wp0)
-
-    wp1 = WaypointXYZVYaw()
-    wp1.position.x = radius
-    wp1.position.y = 0
-    wp1.position.z = 0
-    wp1.yaw = 0
-    wp1.velocity = velocity
-    traj.waypoints.append(wp1)
-
-    for angle in np.arange(0, 2 * np.pi, 10.0 * np.pi / 180.0):
-        wp = WaypointXYZVYaw()
-        wp.position.x = radius * np.cos(angle)
-        wp.position.y = radius * np.sin(angle)
-        wp.position.z = 0
-        wp.yaw = 0
-        wp.velocity = velocity
-        traj.waypoints.append(wp)
-
-    wp_end0 = WaypointXYZVYaw()
-    wp_end0.position.x = radius
-    wp_end0.position.y = 0
-    wp_end0.position.z = 0
-    wp_end0.yaw = 0
-    wp_end0.velocity = velocity
-    traj.waypoints.append(wp_end0)
-
-    wp_end1 = WaypointXYZVYaw()
-    wp_end1.position.x = 0
-    wp_end1.position.y = 0
-    wp_end1.position.z = 0
-    wp_end1.yaw = 0
-    wp_end1.velocity = velocity
-    traj.waypoints.append(wp_end1)
-
-    return traj
-
-
-def fixed_trajectory_callback(msg):
-    attributes = {}
-
-    for key_value in msg.attributes:
-        attributes[key_value.key] = key_value.value
-
-    trajectory_msg = None
-
-    if msg.type == "Figure8":
-        trajectory_msg = get_figure8_waypoints(attributes)
-    elif msg.type == "Circle":
-        trajectory_msg = get_circle_waypoints(attributes)
-    elif msg.type == "Racetrack":
-        trajectory_msg = get_racetrack_waypoints(attributes)
-    elif msg.type == "Line":
-        trajectory_msg = get_line_waypoints(attributes)
-    elif msg.type == "Point":
-        trajectory_msg = get_point_waypoints(attributes)
-
-    if trajectory_msg != None:
-        trajectory_override_pub.publish(trajectory_msg)
-    else:
-        print("No trajectory sent.")
-
-
-if __name__ == "__main__":
-    rospy.init_node("fixed_trajectory_generator")
-
-    fixed_trajectory_sub = rospy.Subscriber(
-        "fixed_trajectory", FixedTrajectory, fixed_trajectory_callback
-    )
-
-    trajectory_override_pub = rospy.Publisher(
-        "trajectory_override", TrajectoryXYZVYaw, queue_size=1
-    )
-
-    rospy.spin()
diff --git a/scenes/two_drone_RetroNeighborhood.usd b/scenes/two_drone_RetroNeighborhood.usd
deleted file mode 100644
index 18bfb3358..000000000
Binary files a/scenes/two_drone_RetroNeighborhood.usd and /dev/null differ
diff --git a/scenes/two_drone_fire_new.usd b/scenes/two_drone_fire_new.usd
deleted file mode 100644
index dd57a8d79..000000000
Binary files a/scenes/two_drone_fire_new.usd and /dev/null differ