diff --git a/.gitignore b/.gitignore
index a0302b69..00d07c85 100644
--- a/.gitignore
+++ b/.gitignore
@@ -183,6 +183,10 @@ dmypy.json
 # Cython debug symbols
 cython_debug/
 
+# debug stuff (Joe-Up GlamSon)
+videos/
+bag_files/
+
 # PyCharm
 #  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
 #  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
diff --git a/scripts/setup.bash b/scripts/setup.bash
index 015b1c79..9780865b 100644
--- a/scripts/setup.bash
+++ b/scripts/setup.bash
@@ -246,6 +246,25 @@ alias start-controller='ros2 service call /pid_controller/enable std_srvs/srv/Se
 alias stop-controller='ros2 service call /pid_controller/enable std_srvs/srv/SetBool "{data: false}"'
 alias reset-controller="ros2 service call /pid_controller/reset std_srvs/srv/Empty"
 
+# aliases for waypoints
+wp-set() {
+	if [ $# -lt 1 ]; then
+		echo "missing waypoint name! should be: wp-set <waypoint_name>"
+		return 1
+	fi
+
+	ros2 service call /wp/set subjugator_msgs/srv/StringTrigger "{wp_name: '$1'}"
+}
+
+wp-goto() {
+	if [ $# -lt 1 ]; then
+		echo "missing waypoint name! should be: wp-goto <waypoint_name>"
+		return 1
+	fi
+
+	ros2 service call /wp/goto subjugator_msgs/srv/StringTrigger "{wp_name: '$1'}"
+}
+
 #explain
 dropper() {
 	if [ $# -lt 1 ]; then
diff --git a/src/subjugator/gnc/subjugator_localization/scripts/reset_srv_node.py b/src/subjugator/gnc/subjugator_localization/scripts/reset_srv_node.py
index 224c1097..9da5936e 100755
--- a/src/subjugator/gnc/subjugator_localization/scripts/reset_srv_node.py
+++ b/src/subjugator/gnc/subjugator_localization/scripts/reset_srv_node.py
@@ -5,12 +5,17 @@
 from rclpy.node import Node
 from robot_localization.srv import SetPose
 from std_srvs.srv import Empty
+from std_msgs.msg import Empty as TopicEmpty
 
 
 class ResetLocalizationService(Node):
 
     def __init__(self):
         super().__init__("reset_localization_service")
+
+        # this topic is Joe Handsome origonal :)
+        self.reset_pub_ = self.create_publisher(TopicEmpty, "localization/was_reset", 10)
+
         self.srv = self.create_service(
             Empty,
             "subjugator_localization/reset",
@@ -24,6 +29,10 @@ def __init__(self):
             self.get_logger().info("service not available, waiting again...")
 
     def reset_localization_callback(self, request, response):
+        # inform the world of the reset
+        msg = TopicEmpty()
+        self.reset_pub_.publish(msg)
+
         self.get_logger().info("Incoming request to reset localization")
         set_pose_request = SetPose.Request()
         set_pose_request.pose = PoseWithCovarianceStamped()
@@ -38,6 +47,7 @@ def reset_localization_callback(self, request, response):
         set_pose_request.pose.pose.covariance = [0.0] * 36
         self.set_pose_client.call_async(set_pose_request)
         response = Empty.Response()
+
         return response
 
 
diff --git a/src/subjugator/gnc/subjugator_waypoints/package.xml b/src/subjugator/gnc/subjugator_waypoints/package.xml
new file mode 100644
index 00000000..9b01765e
--- /dev/null
+++ b/src/subjugator/gnc/subjugator_waypoints/package.xml
@@ -0,0 +1,20 @@
+<?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>subjugator_waypoints</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nottellingu@gmail.com">jh</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <depend>subjugator_msgs</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/src/subjugator/gnc/subjugator_waypoints/resource/subjugator_waypoints b/src/subjugator/gnc/subjugator_waypoints/resource/subjugator_waypoints
new file mode 100644
index 00000000..e69de29b
diff --git a/src/subjugator/gnc/subjugator_waypoints/setup.cfg b/src/subjugator/gnc/subjugator_waypoints/setup.cfg
new file mode 100644
index 00000000..b99a4ccb
--- /dev/null
+++ b/src/subjugator/gnc/subjugator_waypoints/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/subjugator_waypoints
+[install]
+install_scripts=$base/lib/subjugator_waypoints
diff --git a/src/subjugator/gnc/subjugator_waypoints/setup.py b/src/subjugator/gnc/subjugator_waypoints/setup.py
new file mode 100644
index 00000000..9128194d
--- /dev/null
+++ b/src/subjugator/gnc/subjugator_waypoints/setup.py
@@ -0,0 +1,26 @@
+from setuptools import find_packages, setup
+
+package_name = 'subjugator_waypoints'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='jh',
+    maintainer_email='nottellingu@gmail.com',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'subjugator_waypoints = subjugator_waypoints.wp_node:main'
+        ],
+    },
+)
diff --git a/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/__init__.py b/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/wp_manager.py b/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/wp_manager.py
new file mode 100644
index 00000000..62d60efb
--- /dev/null
+++ b/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/wp_manager.py
@@ -0,0 +1,158 @@
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Pose
+
+class MyPose:
+    def __init__(self, x: float, y: float, z: float, i: float, j: float, k: float, w: float):
+        self.x: float = x
+        self.y: float = y
+        self.z: float = z
+        self.i: float = i
+        self.j: float = j
+        self.k: float = k
+        self.w: float = w
+
+    def as_geo_msg_pose(self) -> Pose:
+        p = Pose()
+        p.position.x = float(self.x)
+        p.position.y = float(self.y)
+        p.position.z = float(self.z)
+        p.orientation.x = float(self.i)
+        p.orientation.y = float(self.j)
+        p.orientation.z = float(self.k)
+        p.orientation.w = float(self.w)
+
+        return p
+
+def pose_from_odom(odom: Odometry) -> MyPose:
+    return MyPose(
+        odom.pose.pose.position.x,
+        odom.pose.pose.position.y,
+        odom.pose.pose.position.z,
+        odom.pose.pose.orientation.x,
+        odom.pose.pose.orientation.y,
+        odom.pose.pose.orientation.z,
+        odom.pose.pose.orientation.w
+    )
+
+def quaternion_conjugate(q: MyPose) -> MyPose:
+    return MyPose(0, 0, 0, -q.i, -q.j, -q.k, q.w)
+
+def quaternion_multiply(q1: MyPose, q2: MyPose) -> MyPose:
+    i = q1.w * q2.i + q1.i * q2.w + q1.j * q2.k - q1.k * q2.j
+    j = q1.w * q2.j - q1.i * q2.k + q1.j * q2.w + q1.k * q2.i
+    k = q1.w * q2.k + q1.i * q2.j - q1.j * q2.i + q1.k * q2.w
+    w = q1.w * q2.w - q1.i * q2.i - q1.j * q2.j - q1.k * q2.k
+    return MyPose(0, 0, 0, i, j, k, w)
+
+def rotate_point_by_quaternion(point: MyPose, q: MyPose) -> MyPose:
+    # Create a quaternion representing just the position (with w=0 for pure vector part)
+    p = MyPose(point.x, point.y, point.z, 0, 0, 0, 0)
+    
+    # Perform rotation: q * p * q^-1
+    rotated = quaternion_multiply(quaternion_multiply(q, p), quaternion_conjugate(q))
+    
+    # Return the rotated position
+    return MyPose(rotated.x, rotated.y, rotated.z, 0, 0, 0, 0)
+
+def transform_pose(pose: MyPose, origin: MyPose) -> MyPose:
+    rel_x = pose.x - origin.x
+    rel_y = pose.y - origin.y
+    rel_z = pose.z - origin.z
+    rel_position = MyPose(rel_x, rel_y, rel_z, 0, 0, 0, 0)
+
+    origin_inv = quaternion_conjugate(origin)
+    rotated_pos = rotate_point_by_quaternion(rel_position, origin_inv)
+    new_rotation = quaternion_multiply(origin_inv, pose)
+
+    return MyPose(rotated_pos.x, rotated_pos.y, rotated_pos.z,
+                new_rotation.i, new_rotation.j, new_rotation.k, new_rotation.w)
+
+def inverse_transform_pose(local_pose: MyPose, origin: MyPose) -> MyPose:
+    # First apply rotation to the position part
+    local_position = MyPose(local_pose.x, local_pose.y, local_pose.z, 0, 0, 0, 0)
+    rotated_pos = rotate_point_by_quaternion(local_position, origin)
+    
+    # Then add the origin position
+    world_x = rotated_pos.x + origin.x
+    world_y = rotated_pos.y + origin.y
+    world_z = rotated_pos.z + origin.z
+    
+    # Calculate world orientation by multiplying quaternions
+    world_orientation = quaternion_multiply(origin, local_pose)
+    
+    return MyPose(world_x, world_y, world_z,
+                world_orientation.i, world_orientation.j, world_orientation.k, world_orientation.w)
+
+def transform_poses(poses, origin):
+    return [transform_pose(p, origin) for p in poses]
+
+class WaypointManager:
+    def __init__(self):
+        self.waypoints: dict[str, MyPose] = {}
+
+    # if the waypoint already exists, it is reset
+    def add_waypoint(self, wp_name: str, wp: MyPose):
+        self.waypoints[wp_name] = wp
+
+    def print(self):
+        for (key,val) in self.waypoints.items():
+            print(key + " ", val.x, " ",val.y, " ",val.z, " ",val.i, " ",val.j, " ",val.k, " ",val.w, " ")
+
+    # if the waypoint doesn't exist, does nothing
+    def remove_waypoint(self, wp_name: str):
+        del self.waypoints[wp_name]
+
+    def reset_localization(self, current_pose: MyPose):
+        for key, wp in self.waypoints.items():
+            # Calculate vector from current position to waypoint
+            dx = wp.x - current_pose.x
+            dy = wp.y - current_pose.y
+            dz = wp.z - current_pose.z
+            
+            # For rotation, we need the conjugate of the quaternion
+            # The conjugate is (−i, −j, −k, w)
+            q_i = -current_pose.i
+            q_j = -current_pose.j
+            q_k = -current_pose.k
+            q_w = current_pose.w
+            
+            # Rotate the vector using our new function
+            new_x, new_y, new_z = rotate_vector(dx, dy, dz, q_i, q_j, q_k, q_w)
+            
+            # For orientation, multiply quaternions (using your existing function)
+            # Create orientation quaternion for waypoint
+            wp_quat = MyPose(0, 0, 0, wp.i, wp.j, wp.k, wp.w)
+            
+            # Create conjugate quaternion for current pose
+            conj_quat = MyPose(0, 0, 0, q_i, q_j, q_k, q_w)
+            
+            # Calculate new orientation
+            new_orientation = quaternion_multiply(conj_quat, wp_quat)
+            
+            # Update the waypoint
+            self.waypoints[key] = MyPose(
+                new_x, new_y, new_z,
+                new_orientation.i, new_orientation.j, new_orientation.k, new_orientation.w
+            )
+
+def rotate_vector(v_x, v_y, v_z, q_i, q_j, q_k, q_w):
+    """Rotate a vector [x,y,z] by a quaternion [i,j,k,w]"""
+    # Formula from: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
+    
+    # Pre-calculate some common terms
+    ii = q_i * q_i
+    jj = q_j * q_j
+    kk = q_k * q_k
+    ij = q_i * q_j
+    ik = q_i * q_k
+    jk = q_j * q_k
+    iw = q_i * q_w
+    jw = q_j * q_w
+    kw = q_k * q_w
+    
+    # Calculate rotated vector
+    x = v_x * (1 - 2*(jj + kk)) + v_y * 2*(ij - kw) + v_z * 2*(ik + jw)
+    y = v_x * 2*(ij + kw) + v_y * (1 - 2*(ii + kk)) + v_z * 2*(jk - iw)
+    z = v_x * 2*(ik - jw) + v_y * 2*(jk + iw) + v_z * (1 - 2*(ii + jj))
+    
+    return x, y, z
diff --git a/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/wp_node.py b/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/wp_node.py
new file mode 100644
index 00000000..90753314
--- /dev/null
+++ b/src/subjugator/gnc/subjugator_waypoints/subjugator_waypoints/wp_node.py
@@ -0,0 +1,75 @@
+import rclpy
+from geometry_msgs.msg import Pose
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from std_msgs.msg import Empty
+from subjugator_msgs.srv import StringTrigger
+
+from subjugator_waypoints import wp_manager
+
+
+class WpNode(Node):
+    def __init__(self):
+        super().__init__("wp_node")
+        self.wps = wp_manager.WaypointManager()
+
+        self.reset_sub_ = self.create_subscription(
+            Empty,
+            "localization/was_reset",
+            self.reset_cb,
+            10,
+        )
+
+        self.odom_sub_ = self.create_subscription(
+            Odometry,
+            "odometry/filtered",
+            self.odom_cb,
+            10,
+        )
+        self.current_pose: Odometry
+
+        self.goal_pose_pub_ = self.create_publisher(Pose, "/goal_pose", 10)
+
+        self.set_srv_ = self.create_service(StringTrigger, "wp/set", self.set_wp)
+        self.goto_srv_ = self.create_service(StringTrigger, "wp/goto", self.goto_wp)
+
+    def set_wp(self, req, res):
+        self.wps.add_waypoint(req.wp_name, wp_manager.pose_from_odom(self.current_pose))
+        self.get_logger().info(f"adding new waypoint: {req.wp_name}")
+        return res
+
+    def goto_wp(self, req, res):
+        wp_name = req.wp_name
+
+        wp_exists = wp_name in self.wps.waypoints
+        if not wp_exists:
+            self.get_logger().warn(f"waypoint: {wp_name} does not exist...")
+            return res
+
+        goal: Pose = self.wps.waypoints[wp_name].as_geo_msg_pose()
+        self.get_logger().info(f"going to waypoint: {req.wp_name}")
+
+        self.goal_pose_pub_.publish(goal)
+
+        return res
+
+    def reset_cb(self, _: Empty):
+        current_pose: wp_manager.MyPose = wp_manager.pose_from_odom(self.current_pose)
+
+        self.wps.reset_localization(current_pose)
+        self.get_logger().info("localization was reset!")
+
+    def odom_cb(self, msg: Odometry):
+        self.current_pose = msg
+
+
+def main():
+    rclpy.init()
+    node = WpNode()
+    node.current_pose = Odometry()
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py b/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py
index 2ad62178..3e8f65e0 100644
--- a/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py
+++ b/src/subjugator/subjugator_bringup/launch/subjugator_setup.launch.py
@@ -117,6 +117,13 @@ def generate_launch_description():
         output="both",
     )
 
+    waypoints = Node(
+        package="subjugator_waypoints",
+        executable="subjugator_waypoints",
+        name="subjugator_waypoints",
+        output="both",
+    )
+
     return LaunchDescription(
         [
             gui_cmd,
@@ -128,5 +135,6 @@ def generate_launch_description():
             controller,
             path_planner,
             trajectory_planner,
+            waypoints,
         ],
     )
diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py
index fc9a7e23..7e9621b4 100644
--- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py
+++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/launch/task_server_launch.py
@@ -29,5 +29,11 @@ def generate_launch_description():
                 name="start_gate_server",
                 parameters=[],
             ),
+            Node(
+                package="subjugator_mission_planner",
+                executable="waypoint_server",
+                name="waypoint_server",
+                parameters=[],
+            ),
         ],
     )
diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wp_test.yaml b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wp_test.yaml
new file mode 100644
index 00000000..0f55b5e4
--- /dev/null
+++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/missions/wp_test.yaml
@@ -0,0 +1,23 @@
+mission:
+
+  - task: waypoint
+    parameters:
+      wp_name: sample_wp
+      goto_wp: false
+      set_wp: true
+  - task: move
+    parameters:
+      type: "Relative"
+      x: 1.0
+      y: 0.0
+      z: 0.0
+      i: 0.0
+      j: 0.0
+      k: 0.0
+      w: 1.0
+      timeout: 120
+  - task: waypoint
+    parameters:
+      wp_name: sample_wp
+      goto_wp: true
+      set_wp: false
diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py
index 5810db5c..4f62c34f 100644
--- a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py
+++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/setup.py
@@ -39,6 +39,7 @@
             "movement_server = subjugator_mission_planner.movement_server:main",
             "wait_server = subjugator_mission_planner.wait_server:main",
             "start_gate_server = subjugator_mission_planner.start_gate_server:main",
+            "waypoint_server = subjugator_mission_planner.waypoint_server:main",
         ],
     },
 )
diff --git a/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/waypoint_server.py b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/waypoint_server.py
new file mode 100644
index 00000000..bcdbe62a
--- /dev/null
+++ b/src/subjugator/subjugator_missions/mission_planner/subjugator_mission_planner/subjugator_mission_planner/waypoint_server.py
@@ -0,0 +1,86 @@
+import rclpy
+from rclpy.action.server import ActionServer, CancelResponse, GoalResponse
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+from subjugator_msgs.action import Waypoint
+from subjugator_msgs.srv import StringTrigger
+
+
+class WpServer(Node):
+    def __init__(self):
+        super().__init__("waypoint")
+
+        # Action server
+        self._action_server = ActionServer(
+            self,
+            Waypoint,
+            "waypoint",
+            execute_callback=self.execute_callback,
+            goal_callback=self.goal_callback,
+            cancel_callback=self.cancel_callback,
+        )
+
+        self.set_client = self.create_client(StringTrigger, "wp/set")
+        self.goto_client = self.create_client(StringTrigger, "wp/goto")
+
+    def goal_callback(self, goal_request: Waypoint.Goal):
+        self.wp_name = goal_request.wp_name
+        self.wp_goto = goal_request.goto_wp
+        self.wp_set = goal_request.set_wp
+        self.get_logger().info(
+            f"name: {self.wp_name}. goto: {self.wp_goto}. set: {self.wp_set}",
+        )
+        return GoalResponse.ACCEPT
+
+    def cancel_callback(self, _):
+        self.get_logger().info("Received cancel request")
+        return CancelResponse.ACCEPT
+
+    def execute_callback(self, goal_handle):
+        if self.wp_set and self.wp_goto:
+            # can't do both so will crash
+            goal_handle.self.fail("both set and goto were true!")
+            result = Waypoint.Result()
+            result.success = False
+            result.message = "both set and goto were true!"
+            return result
+
+        if not (self.wp_set or self.wp_goto):
+            # do nothing? screw you!
+            goal_handle.self.fail("neither set what the heck")
+            result = Waypoint.Result()
+            result.success = False
+            result.message = "neither set what the heck"
+            return result
+
+        # now just set or goto the waypoint
+        if self.wp_set:
+            req = StringTrigger.Request()
+            req.wp_name = self.wp_name
+            self.set_client.call(req)
+        elif self.wp_goto:
+            req = StringTrigger.Request()
+            req.wp_name = self.wp_name
+            self.goto_client.call(req)
+        else:
+            # unreachable
+            pass
+
+        goal_handle.succeed()
+        result = Waypoint.Result()
+        result.success = True
+        result.message = "wp set" if self.wp_set else "wp reached"
+        return result
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = WpServer()
+    executor = MultiThreadedExecutor()
+    executor.add_node(node)
+    executor.spin()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/src/subjugator/subjugator_msgs/CMakeLists.txt b/src/subjugator/subjugator_msgs/CMakeLists.txt
index a564e173..c88a6ae1 100644
--- a/src/subjugator/subjugator_msgs/CMakeLists.txt
+++ b/src/subjugator/subjugator_msgs/CMakeLists.txt
@@ -37,7 +37,9 @@ rosidl_generate_interfaces(
   "action/NavigateAround.action"
   "action/Move.action"
   "action/Wait.action"
+  "srv/StringTrigger.srv"
   "action/StartGate.action"
+  "action/Waypoint.action"
   DEPENDENCIES
   geometry_msgs
   std_msgs
diff --git a/src/subjugator/subjugator_msgs/action/Waypoint.action b/src/subjugator/subjugator_msgs/action/Waypoint.action
new file mode 100644
index 00000000..f347735c
--- /dev/null
+++ b/src/subjugator/subjugator_msgs/action/Waypoint.action
@@ -0,0 +1,13 @@
+# Goal
+string wp_name
+bool goto_wp
+bool set_wp
+
+---
+# Result
+bool success
+string message
+
+---
+# Feedback
+string status_message
diff --git a/src/subjugator/subjugator_msgs/srv/StringTrigger.srv b/src/subjugator/subjugator_msgs/srv/StringTrigger.srv
new file mode 100644
index 00000000..3d2774be
--- /dev/null
+++ b/src/subjugator/subjugator_msgs/srv/StringTrigger.srv
@@ -0,0 +1,3 @@
+string wp_name
+---
+# Empty response