diff --git a/CMakeLists.txt b/CMakeLists.txt
index b2a3d00a..a4776866 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -277,6 +277,7 @@ install(PROGRAMS
         localization/basestation_gps_driver.py
         superstructure/superstructure.py
         scripts/debug_course_publisher.py
+		scripts/visualizer.py
 
         # starter project sources
         starter_project/autonomy/src/localization.py
diff --git a/config/simulator.yaml b/config/simulator.yaml
index 5ffa981b..46934c08 100644
--- a/config/simulator.yaml
+++ b/config/simulator.yaml
@@ -23,7 +23,7 @@ simulator:
       tag_0:
         type: urdf
         uri: package://mrover/urdf/world/tag_0.urdf.xacro
-        position: [ -2.0, -2.0, 0.7 ]
+        position: [ 11.0, 3.0, 0.7 ]
       tag_1:
         type: urdf
         uri: package://mrover/urdf/world/tag_1.urdf.xacro
@@ -31,7 +31,7 @@ simulator:
       hammer:
         type: urdf
         uri: package://mrover/urdf/world/hammer.urdf.xacro
-        position: [ 5.0, 2.0, 0.7 ]
+        position: [ -2.0, -2.0, 0.7 ]
       bottle:
         type: urdf
         uri: package://mrover/urdf/world/bottle.urdf.xacro
@@ -58,4 +58,4 @@ simulator:
     ref_lon: -110.7844653
     ref_alt: 0.0
     world_frame: "map"
-    rover_frame: "base_link"
+    rover_frame: "sim_base_link"
diff --git a/scripts/visualizer.py b/scripts/visualizer.py
index fe787f9c..298f023f 100755
--- a/scripts/visualizer.py
+++ b/scripts/visualizer.py
@@ -98,11 +98,17 @@ def __init__(self, state_machine_instance, *args, **kwargs):
         self.viz = Node("Visualizer")
 
         self.viz.create_subscription(
-            StateMachineStructure, STRUCTURE_TOPIC, self.state_machine.container_structure_callback, 1
+            StateMachineStructure,
+            STRUCTURE_TOPIC,
+            self.state_machine.container_structure_callback,
+            1
         )
 
         self.viz.create_subscription(
-            StateMachineStateUpdate, STATUS_TOPIC, self.state_machine.container_status_callback, 1
+            StateMachineStateUpdate,
+            STATUS_TOPIC,
+            self.state_machine.container_status_callback,
+            1
         )
 
     def paintEvent(self, event):
@@ -138,10 +144,20 @@ def main():
         signal.signal(signal.SIGINT, signal.SIG_DFL)
         state_machine = StateMachine()
 
+<<<<<<< HEAD
         print("Node Created...")
 
         print("Subscriptions Created...")
 
+=======
+
+        print("Node Created...")
+
+
+        print("Subscriptions Created...")
+
+
+>>>>>>> nav_fixes
         app = QApplication([])  # type: ignore
         g = GUI(state_machine)
         g.show()
diff --git a/starter_project/autonomy/launch/starter_project.launch.py b/starter_project/autonomy/launch/starter_project.launch.py
index 7c3c4897..4b383395 100644
--- a/starter_project/autonomy/launch/starter_project.launch.py
+++ b/starter_project/autonomy/launch/starter_project.launch.py
@@ -31,21 +31,22 @@ def generate_launch_description():
         condition=LaunchConfigurationEquals("rviz", "true"),
     )
 
-    return LaunchDescription(
-        [
-            launch_include_sim,
-            rviz_node,
-            # ==========
-            # Perception
-            # ==========
-            perception_node,
-            # ===========
-            # Navigation
-            # ===========
-            # navigation_node,
-            # ============
-            # Localization
-            # ============
-            # localization_node
-        ]
-    )
+    return LaunchDescription([
+        launch_include_sim,
+        rviz_node,
+
+        # ==========
+        # Perception
+        # ==========
+        perception_node,
+
+        # ===========
+        # Navigation
+        # ===========
+        navigation_node,
+
+        # ============
+        # Localization
+        # ============
+        localization_node
+    ])
diff --git a/starter_project/autonomy/src/localization.py b/starter_project/autonomy/src/localization.py
index 55b991d4..d46d695d 100755
--- a/starter_project/autonomy/src/localization.py
+++ b/starter_project/autonomy/src/localization.py
@@ -6,6 +6,7 @@
 # library for interacting with ROS and TF tree
 import rclpy
 from rclpy.node import Node
+import rclpy.time
 import tf2_ros
 
 # ROS message types we need to use
@@ -60,7 +61,6 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar
         """
         # TODO
 
-
 def main():
     # initialize the node
     rclpy.init()
@@ -71,8 +71,7 @@ def main():
     # let the callback functions run asynchronously in the background
     rclpy.spin(localization)
 
-    # TODO (ali): shutdown maybe?
-    # rclpy.shutdown()
+    rclpy.shutdown()
 
 
 if __name__ == "__main__":
diff --git a/starter_project/autonomy/src/navigation/context.py b/starter_project/autonomy/src/navigation/context.py
index e32179bd..d4c48242 100644
--- a/starter_project/autonomy/src/navigation/context.py
+++ b/starter_project/autonomy/src/navigation/context.py
@@ -5,15 +5,16 @@
 
 import numpy as np
 import rclpy
+from rclpy.publisher import Publisher
+from rclpy.subscription import Subscription
 from rclpy.node import Node
 import tf2_ros
 from geometry_msgs.msg import Twist
 from mrover.msg import StarterProjectTag
 
-# import sys
-# TODO (ali): python relative imports are the bane of my existence
-# sys.path.append('..')
-# print(sys.path)
+import sys
+import os
+sys.path.append(os.getcwd() + '/starter_project/autonomy/src')
 from util.SE3 import SE3
 from visualization_msgs.msg import Marker
 
@@ -34,7 +35,6 @@ def send_drive_stop(self):
         # TODO: tell the rover to stop
         pass
 
-
 @dataclass
 class Environment:
     """
@@ -55,25 +55,30 @@ def get_fid_data(self) -> Optional[StarterProjectTag]:
         if it exists, otherwise returns None
         """
         # TODO: return either None or your position message
-        pass
 
 
-class Context(Node):
+class Context:
+    node: Node
     tf_buffer: tf2_ros.Buffer
     tf_listener: tf2_ros.TransformListener
-    vel_cmd_publisher: rclpy.Publisher
-    vis_publisher: rclpy.Publisher
-    fid_listener: rclpy.Subscriber
+    vel_cmd_publisher: Publisher
+    vis_publisher: Publisher
+    fid_listener: Subscription
 
     # Use these as the primary interfaces in states
     rover: Rover
     env: Environment
+    disable_requested: bool
 
-    def __init__(self):
+    def setup(self, node: Node):
+        self.node = node
         self.tf_buffer = tf2_ros.Buffer()
-        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
-        self.vel_cmd_publisher = self.create_publisher(Twist, "cmd_vel", queue_size=1)
-        self.vis_publisher = self.create_publisher("nav_vis", Marker)
+
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node)
+        self.vel_cmd_publisher = node.create_publisher(Twist, "cmd_vel", 1)
+        self.vis_publisher = node.create_publisher(Marker, "nav_vis", 1)
         self.rover = Rover(self)
         self.env = Environment(self, None)
-        self.fid_listener = self.create_subscription(StarterProjectTag, "/tag", self.env.receive_fid_data)
+        self.disable_requested = False
+
+        self.fid_listener = node.create_subscription(StarterProjectTag, "/tag", self.env.receive_fid_data, 1)
diff --git a/starter_project/autonomy/src/navigation/drive_state.py b/starter_project/autonomy/src/navigation/drive_state.py
index a0e7ea4d..8ea8434e 100644
--- a/starter_project/autonomy/src/navigation/drive_state.py
+++ b/starter_project/autonomy/src/navigation/drive_state.py
@@ -2,29 +2,26 @@
 
 from context import Context
 from drive import get_drive_command
-from state import BaseState
+from state_machine.state import State
+from tag_seek import TagSeekState
 
+class DriveState(State):
+    def on_enter(self, context) -> None:
+        pass
 
-class DriveState(BaseState):
-    def __init__(self, context: Context):
-        super().__init__(
-            context,
-            # TODO:
-            add_outcomes=["TODO: add outcomes here"],
-        )
+    def on_exit(self, context) -> None:
+        pass
 
-    def evaluate(self, ud):
+    def on_loop(self, context) -> State:
         target = np.array([8.0, 2.0, 0.0])
 
-        # TODO: get the rover's pose, if it doesn't exist stay in DriveState (with outcome "driving_to_point")
+        # TODO: get the rover's pose, if it doesn't exist stay in DriveState (return self)
 
         # TODO: get the drive command and completion status based on target and pose
         # (HINT: use get_drive_command(), with completion_thresh set to 0.7 and turn_in_place_thresh set to 0.2)
 
-        # TODO: if we are finished getting to the target, go to TagSeekState (with outcome "reached_point")
-
+        # TODO: if we are finished getting to the target, go to TagSeekState
+        
         # TODO: send the drive command to the rover
 
-        # TODO: tell smach to stay in the DriveState by returning with outcome "driving_to_point"
-
-        pass
+        # TODO: tell state machine to stay in the DriveState by returning self
diff --git a/starter_project/autonomy/src/navigation/navigation_starter_project.py b/starter_project/autonomy/src/navigation/navigation_starter_project.py
index ca0c1ee4..1882e0b2 100755
--- a/starter_project/autonomy/src/navigation/navigation_starter_project.py
+++ b/starter_project/autonomy/src/navigation/navigation_starter_project.py
@@ -1,35 +1,34 @@
 #!/usr/bin/env python3
 
-import signal
 import sys
-import threading
 
 # ros and state machine imports
 import rclpy
 from rclpy.executors import ExternalShutdownException
-
+from rclpy.node import Node
 from state_machine.state_machine import StateMachine
 from state_machine.state_publisher_server import StatePublisher
 
 # navigation specific imports
 from context import Context
 from drive_state import DriveState
-from state import DoneState, Off
+from state import DoneState, FailState
 from tag_seek import TagSeekState
 
 
-class Navigation(threading.Thread):
+class Navigation(Node):
     state_machine: StateMachine
-    context: Context
+    ctx: Context
     state_machine_server: StatePublisher
 
-    def __init__(self, context: Context):
-        super().__init__()
-        self.name = "NavigationThread"
-        self.state_machine = StateMachine(outcomes=["terminated"])
-        self.context = context
-        self.state_machine_server = StatePublisher(self, self.state_machine, "/SM_ROOT")
-        self.state_machine = StateMachine[Context]
+    def __init__(self, ctx: Context):
+        super().__init__("navigation")
+
+        self.get_logger().info("Starting...")
+
+        self.ctx = ctx
+
+        self.state_machine = StateMachine[Context](DriveState(), "NavigationStateMachine", self.ctx, self.get_logger())
 
         # TODO: add DriveState and its transitions here
 
@@ -38,19 +37,18 @@ def __init__(self, context: Context):
             DoneState(),
             [DoneState()],
         )
-        # TODO: add TagSeekState and its transitions here
 
-    def run(self):
-        self.state_machine.execute()
+        # FailState and its transitions
+        self.state_machine.add_transitions(
+            FailState(),
+            [FailState()],
+        )
+
+        # TODO: add TagSeekState and its transitions here
 
-    def stop(self):
-        self.sis.stop()
-        # Requests current state to go into 'terminated' to cleanly exit state machine
-        self.state_machine.request_preempt()
-        # Wait for smach thread to terminate
-        self.join()
-        self.context.rover.send_drive_stop()
+        self.state_machine_server = StatePublisher(self, self.state_machine, "nav_structure", 1, "nav_state", 10)
 
+        self.create_timer(1 / 60, self.state_machine.update)
 
 def main():
     try:
@@ -59,6 +57,7 @@ def main():
         # context and navigation objects
         context = Context()
         navigation = Navigation(context)
+        context.setup(navigation)
 
         rclpy.spin(navigation)
         rclpy.shutdown()
diff --git a/starter_project/autonomy/src/navigation/state.py b/starter_project/autonomy/src/navigation/state.py
index c1a8e623..0c29028e 100644
--- a/starter_project/autonomy/src/navigation/state.py
+++ b/starter_project/autonomy/src/navigation/state.py
@@ -1,63 +1,33 @@
 from abc import ABC, abstractmethod
 from typing import List, Optional
 
-import smach
+from state_machine.state import State
 from geometry_msgs.msg import Twist
 
 from context import Context
 
+class DoneState(State):
+    def on_enter(self, context) -> None:
+        pass
 
-class BaseState(smach.State, ABC):
-    """
-    Custom base state which handles termination cleanly via smach preemption.
-    """
-
-    context: Context
-
-    def __init__(
-        self,
-        context: Context,
-        add_outcomes: Optional[List[str]] = None,
-        add_input_keys: Optional[List[str]] = None,
-        add_output_keys: Optional[List[str]] = None,
-    ):
-        add_outcomes = add_outcomes or []
-        add_input_keys = add_input_keys or []
-        add_output_keys = add_output_keys or []
-        super().__init__(
-            add_outcomes + ["terminated"],
-            add_input_keys,
-            add_output_keys,
-        )
-        self.context = context
-
-    def execute(self, ud):
-        """
-        Override execute method to add logic for early termination.
-        Base classes should override evaluate instead of this!
-        :param ud:  State machine user data
-        :return:    Next state, 'terminated' if we want to quit early
-        """
-        if self.preempt_requested():
-            self.service_preempt()
-            return "terminated"
-        return self.evaluate(ud)
-
-    @abstractmethod
-    def evaluate(self, ud: smach.UserData) -> str:
-        """Override me instead of execute!"""
-        ...
+    def on_exit(self, context) -> None:
+        pass
 
+    def on_loop(self, context) -> State:
+        # Stop rover
+        cmd_vel = Twist()
+        context.rover.send_drive_command(cmd_vel)
+        return self
+    
+class FailState(State):
+    def on_enter(self, context) -> None:
+        pass
 
-class DoneState(BaseState):
-    def __init__(self, context: Context):
-        super().__init__(
-            context,
-            add_outcomes=["done"],
-        )
+    def on_exit(self, context) -> None:
+        pass
 
-    def evaluate(self, ud):
+    def on_loop(self, context) -> State:
         # Stop rover
         cmd_vel = Twist()
-        self.context.rover.send_drive_command(cmd_vel)
-        return "done"
+        context.rover.send_drive_command(cmd_vel)
+        return self
\ No newline at end of file
diff --git a/starter_project/autonomy/src/navigation/tag_seek.py b/starter_project/autonomy/src/navigation/tag_seek.py
index 57773151..f6fd707f 100644
--- a/starter_project/autonomy/src/navigation/tag_seek.py
+++ b/starter_project/autonomy/src/navigation/tag_seek.py
@@ -1,30 +1,29 @@
 from geometry_msgs.msg import Twist
 
 from context import Context
-from state import BaseState
+from state_machine.state import State
+from state import DoneState, FailState
 
 
-class TagSeekState(BaseState):
-    def __init__(self, context: Context):
-        super().__init__(
-            context,
-            # TODO: add outcomes
-            add_outcomes=["TODO: add outcomes here"],
-        )
+class TagSeekState(State):
+    def on_enter(self, context) -> None:
+        pass
+
+    def on_exit(self, context) -> None:
+        pass
 
-    def evaluate(self, ud):
-        DISTANCE_TOLERANCE = 0.99
+    def on_loop(self, context) -> State:
+        DISTANCE_TOLERANCE = 0.995
         ANUGLAR_TOLERANCE = 0.3
         # TODO: get the tag's location and properties (HINT: use get_fid_data() from context.env)
+        tag = context.env.get_fid_data()
 
-        # TODO: if we don't have a tag: go to the DoneState (with outcome "failure")
+        # TODO: if we don't have a tag: go to the FailState
 
-        # TODO: if we are within angular and distance tolerances: go to DoneState (with outcome "success")
+        # TODO: if we are within angular and distance tolerances: go to DoneState
 
         # TODO: figure out the Twist command to be applied to move the rover closer to the tag
 
         # TODO: send Twist command to rover
 
         # TODO: stay in the TagSeekState (with outcome "working")
-
-        pass
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
index bc558726..20a5d7e0 100644
--- a/starter_project/autonomy/src/perception.cpp
+++ b/starter_project/autonomy/src/perception.cpp
@@ -80,7 +80,6 @@ namespace mrover {
         // TODO: implement me!
         (void)tag;
 
-        mTagPublisher->publish(tag);
     }
 
     auto Perception::getClosenessMetricFromTagCorners(cv::Mat const& image, std::vector<cv::Point2f> const& tagCorners) -> float { // NOLINT(*-convert-member-functions-to-static)
diff --git a/starter_project/autonomy/src/util/SE3.py b/starter_project/autonomy/src/util/SE3.py
index 0c9cc04e..15e118db 100644
--- a/starter_project/autonomy/src/util/SE3.py
+++ b/starter_project/autonomy/src/util/SE3.py
@@ -4,6 +4,7 @@
 
 import numpy as np
 import rclpy
+from rclpy.duration import Duration
 import tf2_ros
 from geometry_msgs.msg import TransformStamped, Vector3, Quaternion
 
@@ -58,7 +59,7 @@ def from_tf_tree(cls, tf_buffer: tf2_ros.Buffer, parent_frame: str, child_frame:
 
         :returns: an SE3 containing the transform from parent_frame to child_frame
         """
-        tf_msg = tf_buffer.lookup_transform(parent_frame, child_frame, rospy.Time()).transform
+        tf_msg = tf_buffer.lookup_transform(parent_frame, child_frame, rclpy.time.Time()).transform
         result = SE3(position=numpify(tf_msg.translation), rotation=SO3(numpify(tf_msg.rotation)))
         return result
 
diff --git a/state_machine/state_machine.py b/state_machine/state_machine.py
index c3b623eb..9adeaf28 100644
--- a/state_machine/state_machine.py
+++ b/state_machine/state_machine.py
@@ -42,7 +42,7 @@ def __init__(
         self.transition_log: list[TransitionRecord] = []
         self.context = context
         self.name = name
-        self.off_lamdba = None
+        self.off_lambda = None
         self.off_state = None
         self.logger = logger