diff --git a/ros2/src/robobin/launch/robobin_launch.py b/ros2/src/robobin/launch/robobin_launch.py
index 53efd3a4b835767406daa482be80cea39858c823..50e80250ab233bc37ef956ae088274e6912ecc68 100755
--- a/ros2/src/robobin/launch/robobin_launch.py
+++ b/ros2/src/robobin/launch/robobin_launch.py
@@ -13,14 +13,6 @@ def generate_launch_description():
             emulate_tty=True
 
         ),
-       
-        Node(
-            package='robobin',
-            executable='uwb_navigation_node',    
-            name='uwb_navigation_node',
-            output='screen',
-            emulate_tty=True
-        ),
     
         Node(
             package='robobin',
@@ -43,13 +35,7 @@ def generate_launch_description():
             output='screen',
             emulate_tty=True
         ),
-        Node(
-            package='robobin',
-            executable='uwb_pathing_node',    
-            name='uwb_pathing_node',
-            output='screen',
-            emulate_tty=True
-        ),
+
 
         # Add additional nodes
         # Example:
diff --git a/ros2/src/robobin/robobin/api_node.py b/ros2/src/robobin/robobin/api_node.py
index 14063fc3b94f477445f6a081459d257a54f3bb56..2f05d1383f0a274db9a4acb2b07ff8bc21362cf9 100644
--- a/ros2/src/robobin/robobin/api_node.py
+++ b/ros2/src/robobin/robobin/api_node.py
@@ -107,6 +107,7 @@ class ApiNode(Node):
                 if result is not None:
                     topic, message = result  # Safely unpack after checking
                     if topic is not None:
+                        self.get_logger().info(f"Received message: {message}")
                         self.get_logger().info(f"Publishing to topic: {topic}")
                         self.publish_to_topic(topic, message)
         except Exception as e:
diff --git a/ros2/src/robobin/robobin/helpers/message_handler.py b/ros2/src/robobin/robobin/helpers/message_handler.py
index 3182da3bd7e274b158b5f3003f77798ee748a734..0b61cab31a77cc5240dd366ffe8bf72746e8b728 100644
--- a/ros2/src/robobin/robobin/helpers/message_handler.py
+++ b/ros2/src/robobin/robobin/helpers/message_handler.py
@@ -16,8 +16,19 @@ class MessageHandler:
             "CALLME": self.handle_call_me,
             "BPM": self.handle_call_bpm,
             "REQMAP": self.handle_request_map,
-            "LOCATION": self.handle_request_location
+            "LOCATION": self.handle_request_location,
+            "CALLMELIDAR": self.handle_call_me_lidar
         }
+    def handle_call_me_lidar(self, client_socket, message):
+        #CALLMELIDAR 1 (for example)
+        response = b"Requested Number is " + message.encode()
+        if self.testing:
+            print(response.decode())
+        else:
+            client_socket.sendall(response)
+        return "nav_command", "CALLMELIDAR " + message
+
+
     def handle_request_location(self, client_socket, message):
         """Handles the LOC command."""
         response = f"location is {self.api_node.connection_manager.location}".encode()
diff --git a/ros2/src/robobin/robobin/uwb_navigation_node.py b/ros2/src/robobin/robobin/uwb_navigation_node.py
deleted file mode 100644
index 21767f41fc4ca24e576017fa7aaf1132242df8ef..0000000000000000000000000000000000000000
--- a/ros2/src/robobin/robobin/uwb_navigation_node.py
+++ /dev/null
@@ -1,677 +0,0 @@
-import json
-import os
-import numpy as np
-from scipy.optimize import least_squares
-from geometry_msgs.msg import Point, Twist
-import rclpy
-from rclpy.node import Node
-from std_msgs.msg import String
-from sensor_msgs.msg import Imu
-from tf.transformations import euler_from_quaternion
-
-import math
-
-class SerialController:
-    def __init__(self, port, windowSize = 10, minSuccessfulValues = 5, maxRetries = 20):
-        self.ser = serial.Serial(port, 115200, timeout = 1)
-        self.windowSize = windowSize
-        self.minSuccessfulValues = minSuccessfulValues
-        self.maxRetries = maxRetries
-        self.calibration_active = False
-        self.calibration_timer = None
-        self.calib_start_position = None
-        self.calib_start_yaw = None
-
-
-
-        self.tag1Window = [[] for _ in range(4)]
-        self.tag2Window = [[] for _ in range(4)]
-
-    def readFromDevice(self, expectedLines = 1):
-        lines = []
-        while self.ser.in_waiting or len(lines) < expectedLines:
-            lines.append(self.ser.readline().decode())
-        return list(map(lambda line: line.strip(), lines))
-    
-    def getBPMOnce(self):
-        self.writeToDevice("bpm")
-        buffer = self.readFromDevice(1)[0].split(" ")
-        print(buffer)
-        if len(buffer) == 6: #received all bpm distances
-            return list(map(float, buffer))
-        elif len(buffer) == 4: #live check saying which anchors are unreachable
-            return list(map(lambda x: bool(int(x)), buffer))
-        else:
-            return []
-            # return int(buffer[0]) if len(buffer) else None
-        
-    def getBeaconPositioningDistances(self):
-        bpmWindow = []
-        successfulValues = 0
-        currentTry = 0
-        anchorsOnline = []
-        while (successfulValues < self.minSuccessfulValues) and (currentTry < self.maxRetries):
-            values = self.getBPMOnce()
-            if len(values) == 4: #bpm returned live check, some anchors are offline
-                anchorsOnline = values
-            if len(values) == 6: #bpm distances retrieved
-                if all(map(lambda x: x >= 0.0, values)): #ensure all values are non-negative
-                    bpmWindow.append(values)
-                    successfulValues += 1
-            currentTry += 1
-        
-        if currentTry >= self.maxRetries:
-            return anchorsOnline
-        else:
-            return [sum(map(lambda bpmEntry: bpmEntry[i], bpmWindow)) / len(bpmWindow) for i in range(6)]
-    
-    def getRNGOnce(self):
-        self.writeToDevice("rng")
-        lines = self.readFromDevice(2)
-        distances = []
-        distances.append(list(map(float, lines[0].split(" "))))
-        if len(lines[1]) > 1:
-            distances.append(list(map(float, lines[1].split(" "))))
-        else:
-            distances.append(None)
-        return distances
-    
-    def getRangingDistances(self):
-        tag1Distances, tag2Distances = self.getRNGOnce()
-        if (all(map(lambda x: x >= 0.0, tag1Distances))): #all values are non-negative
-            if len(self.tag1Window[0]) < self.windowSize:
-                for i, dist in enumerate(tag1Distances):
-                    (self.tag1Window[i]).append(dist)
-            else:
-                for i, dist in enumerate(tag1Distances):
-                    self.tag1Window[i] = self.tag1Window[i][1:] + [dist]
-
-        if tag2Distances == None:
-            self.tag2Window = [[] for _ in range(4)]
-        else:
-            if (all(map(lambda x: x >= 0.0, tag2Distances))): #all values are non-negative
-                if len(self.tag2Window[0]) < self.windowSize:
-                    for i, dist in enumerate(tag2Distances):
-                        self.tag2Window[i].append(dist)
-                else:
-                    for i, dist in enumerate(tag2Distances):
-                        self.tag2Window[i] = self.tag2Window[i][1:] + [dist]
-        
-        return [[(sum(distArray) / (len(distArray) - distArray.count(0.0)) if (len(distArray) - distArray.count(0.0)) else 0.0) for distArray in self.tag1Window], [(sum(distArray) / (len(distArray) - distArray.count(0.0)) if (len(distArray) - distArray.count(0.0)) else 0.0) for distArray in self.tag2Window] if len(self.tag2Window[0]) else None]
-    
-    def writeToDevice(self, value):
-        self.ser.write((value + "\n").encode())
-    
-    def __del__(self):
-        print("Closing port")
-        self.ser.close()
-
-class UWBNode(Node):
-    def __init__(self):
-        super().__init__('uwb_navigation_node')
-
-        self.publisher = self.create_publisher(Point, '/tag1_location', 10)
-        self.start_publisher = self.create_publisher(Point, '/start_location', 10)
-        self.target_location_pub = self.create_publisher(Point, '/target_location', 10)
-        self.api_pub = self.create_publisher(String, '/api_command', 10)
-      
-        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
-
-        self.subscription = self.create_subscription(String, '/nav_command', self.handle_nav_command, 10)
-        self.imu_subscription = self.create_subscription(Imu, '/imu', self.handle_imu, 10)
-
-        self.current_yaw = 0.0 
-        self.uwb_to_imu_offset = 0.0 
-        self.mode = "Manual"
-        self.serial_controller = SerialController("/dev/ttyACM0")
-        self.anchors = {}
-        self.anchorHeight = 250
-        self.anchors_coords_known = False
-        self.previous_tag1_position = None
-
-        self.windowSize = 10
-        self.tag1Window = []
-        self.tag2Window = []
-
-        self.current_tag1_position = None
-        self.current_tag2_position = None
-
-        # Graph data for call mode navigation
-        self.graph_data = None
-
-        # For Follow mode:
-        self.follow_offset_computed = False  # True once we have computed initial heading offset
-        self.follow_target_reached = False
-
-        # For Call mode:
-        self.path = []
-        self.current_target_index = 0  # Index in the path
-        self.call_mode_active = False
-        workspace_root = os.getcwd()  # Current working directory
-        anchors_file_path = os.path.abspath(os.path.join(workspace_root, "src",  "robobin", "robobin", "graphs", "anchors.json"))
-        if os.path.exists(anchors_file_path):
-            try:
-                with open(anchors_file_path, 'r') as f:
-                    anchors_data = json.load(f)
-                    self.anchors = anchors_data
-                    self.anchors_coords_known = True
-                    self.get_logger().info("Anchor coordinates loaded from anchors.json.")
-            except Exception as e:
-                self.get_logger().error(f"Failed to load anchors.json: {e}")
-                self.anchors_coords_known = False
-        else:
-            self.get_logger().warning("anchors.json not found, anchor coordinates not known.")
-            self.anchors_coords_known = False
-
-        # Timer to continuously fetch and publish positions
-        self.timer = self.create_timer(1.0, self.update_positions)
-
-    def call_bpm(self):
-        try:
-            # Retrieve beacon distances and determine anchor coordinates
-            beacon_distances = self.serial_controller.getBeaconPositioningDistances()
-            self.get_logger().info(f"Retrieved values {beacon_distances}")
-            self.determine_anchor_coords(beacon_distances)
-            self.anchors_coords_known = True
-            self.get_logger().info("Anchor coordinates determined.")
-
-            # Define workspace path once
-            workspace_root = os.getcwd()
-            anchors_file_path = os.path.abspath(os.path.join(workspace_root, "src", "robobin", "robobin", "graphs", "anchors.json"))
-            graph_file_path = os.path.abspath(os.path.join(workspace_root, "src", "robobin", "robobin", "graphs", "graph.json"))
-
-            try:
-                # Save anchors to anchors.json
-                with open(anchors_file_path, 'w') as f:
-                    json.dump(self.anchors, f)
-                self.get_logger().info("Anchor coordinates saved to anchors.json.")
-
-                # Construct graph structure
-                graph = {
-                    "name": "Lab 1 Extended",
-                    "nodes": [],
-                    "connections": []
-                }
-
-                # Populate nodes with anchor data
-                anchor_names = ["A1", "A2", "A3", "A4"]
-                for i, (key, coords) in enumerate(self.anchors.items()):
-                    graph["nodes"].append({"name": anchor_names[i], "x": coords[0], "y": coords[1]})
-
-                # Create a fully connected adjacency matrix (all nodes connected)
-                num_nodes = len(graph["nodes"])
-                graph["connections"] = [[True if i != j else False for j in range(num_nodes)] for i in range(num_nodes)]
-
-                # Save graph to graph.json
-                with open(graph_file_path, 'w') as f:
-                    json.dump(graph, f, indent=2)
-                self.get_logger().info("Graph saved to graph.json.")
-
-            except Exception as e:
-                self.get_logger().error(f"Failed to save graph or anchors: {e}")
-
-        except Exception as e:
-            self.get_logger().error(f"Failed to determine anchors: {e}")
-
-    def handle_imu(self, msg):
-        # Extract the quaternion from the IMU message
-        qx = msg.orientation.x
-        qy = msg.orientation.y
-        qz = msg.orientation.z
-        qw = msg.orientation.w
-
-        # Convert quaternion to Euler angles (roll, pitch, yaw)
-        # euler_from_quaternion expects [x, y, z, w]
-        (roll, pitch, yaw) = euler_from_quaternion([qx, qy, qz, qw])
-
-        # Store yaw (in radians) for usage in your node
-        self.current_yaw = yaw
-
-        # Optional: Print or log if you want to see the values
-        self.get_logger().info(f"IMU yaw (rad): {yaw:.3f}")
-
-
-    def determine_anchor_coords(self, measured_distances):
-        y_A = measured_distances[2]  # Distance from A to D
-
-        # Guess for B based on distance A-B and B-D
-        x_B = measured_distances[0] / 2
-        y_B = measured_distances[4] / 2 + 200  # Start y_B above y_C
-
-        # Guess for C with symmetrical logic
-        x_C = -measured_distances[5] / 2  # Allow for negative x_C
-        y_C = -measured_distances[1] / 2  # Allow for negative y_C
-
-        initial_guess = [x_B, y_B, x_C, y_C, y_A]
-
-        min_dist = min(measured_distances)
-        max_dist = max(measured_distances)
-        lower_bounds = [-max_dist, -max_dist, -max_dist, -max_dist, min_dist / 2]
-        upper_bounds = [max_dist * 1.5 for i in range(5)]
-
-        def error_function(variables, measured):
-            x_B, y_B, x_C, y_C, y_A = variables
-
-            # Map measured distances to a, e, d, b, f, c
-            a_measured, e_measured, d_measured, b_measured, f_measured, c_measured = measured
-
-            # Compute each distance
-            a_calc = np.sqrt((x_B - 0)**2 + (y_B - y_A)**2)  # A-B
-            b_calc = np.sqrt((x_C - x_B)**2 + (y_C - y_B)**2)  # B-C
-            c_calc = np.sqrt(x_C**2 + y_C**2)                 # C-D
-            d_calc = y_A                                      # A-D
-            e_calc = np.sqrt(x_C**2 + (y_C - y_A)**2)         # A-C
-            f_calc = np.sqrt(x_B**2 + y_B**2)                 # B-D
-
-            # Residuals
-            r_a = a_calc - a_measured
-            r_b = b_calc - b_measured
-            r_c = c_calc - c_measured
-            r_d = d_calc - d_measured
-            r_e = e_calc - e_measured
-            r_f = f_calc - f_measured
-
-            # Add a smoother penalty if y_B <= y_C
-            penalty = 1e3 * max(0, y_C - y_B + 10)  # Soft penalty to enforce constraint
-
-            return [r_a, r_b, r_c, r_d, r_e, r_f, penalty]
-
-        result = least_squares(error_function, initial_guess, args=(measured_distances,), bounds=(lower_bounds, upper_bounds), loss='soft_l1')
-
-        x_B, y_B, x_C, y_C, y_A = result.x
-        self.anchors = {
-            "A1": (0, y_A, self.anchorHeight),
-            "A2": (x_B, y_B, self.anchorHeight),
-            "A3": (x_C, y_C, self.anchorHeight),
-            "A4": (0, 0, self.anchorHeight)
-        }
-
-    def calculate_tag_coordinates(self, tag_distances):
-        if not self.anchors_coords_known or len(self.anchors) < 3:
-            self.get_logger().warning("Anchor coordinates not known.")
-            return None
-        
-        available_beacons = []
-        available_distances = []
-
-        for key, dist in tag_distances.items():
-            if dist > 0:
-                available_distances.append(dist)
-                available_beacons.append(self.anchors[key])
-
-        if len(available_beacons) < 3:
-            return None
-        
-        heights = []
-        for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
-            horizontal_distance = np.sqrt((bx - 0)**2 + (by - 0)**2)  # Assume tag starts at (0, 0)
-            estimated_z = np.sqrt(max(0, d_measured**2 - horizontal_distance**2))  # Ensure non-negative
-            heights.append(bz - estimated_z)  # Estimate tag height relative to beacon
-
-        initial_z = max(0, min(self.anchorHeight, np.mean(heights)))  # Constrain height to [0, 200]
-        
-        beacon_xs = [b[0] for b in available_beacons]
-        beacon_ys = [b[1] for b in available_beacons]
-        initial_guess = [np.mean(beacon_xs), np.mean(beacon_ys), initial_z]
-
-        # Define bounds for the tag position
-        x_min = min(beacon_xs) - 1000  # Add a small margin around the beacons
-        x_max = max(beacon_xs) + 1000
-        y_min = min(beacon_ys) - 1000
-        y_max = max(beacon_ys) + 1000
-        z_min = 0.0  # Tag's height cannot be below the ground
-        z_max = self.anchorHeight  # Tag's height cannot exceed the beacon height
-        bounds = ([x_min, y_min, z_min], [x_max, y_max, z_max])
-
-        def error_function(vars):
-            x, y, z = vars
-            residuals = []
-            for (bx, by, bz), d_measured in zip(available_beacons, available_distances):
-                d_computed = np.sqrt((x - bx)**2 + (y - by)**2 + (z - bz)**2)
-                residuals.append(d_computed - d_measured)
-            return residuals
-
-        result = least_squares(error_function, initial_guess, bounds=bounds, loss='soft_l1')
-        # self.get_logger().info(f"Optimization result: {result.x}")
-        return tuple(result.x)
-
-    def update_positions(self):
-        self.get_logger().info("Updating positions...")
-        if self.anchors_coords_known:
-            try:
-                ranging_distances = self.serial_controller.getRangingDistances()
-                # self.get_logger().info(f"Ranging distances: {ranging_distances}")
-                self.get_logger().warning("Ranging distances: " + str(ranging_distances))
-                tag1_distances = ranging_distances[0]
-                tag_distances_dict = {
-                    "A1": tag1_distances[0],
-                    "A2": tag1_distances[1],
-                    "A3": tag1_distances[2],
-                    "A4": tag1_distances[3]
-                }
-
-                tag1_position = self.calculate_tag_coordinates(tag_distances_dict)
-
-                if tag1_position:
-                    if len(self.tag1Window < self.windowSize):
-                        self.tag1Window.append(tag1_position)
-                    else:
-                        self.tag1Window = self.tag1Window[1:] + [tag1_position]
-                    
-                    self.current_tag1_position = tuple(map(lambda li: sum(li) / len(li), zip(self.tag1Window)))
-
-                    self.get_logger().info(f"Tag 1 position: {tag1_position}")
-                    position_msg = Point(x=self.current_tag1_position[0], y=self.current_tag1_position[1], z=self.current_tag1_position[2])
-                    self.publisher.publish(position_msg)
-
-                if self.mode == "Follow":
-                    self.get_logger().info("Follow mode active.")
-                    tag2_distances = ranging_distances[1]
-                    if tag2_distances:
-                        tag2_distances_dict = {
-                            "A1": tag2_distances[0],
-                            "A2": tag2_distances[1],
-                            "A3": tag2_distances[2],
-                            "A4": tag2_distances[3]
-                        }
-                        tag2_position = self.calculate_tag_coordinates(tag2_distances_dict)
-                        if tag2_position:
-                            if len(self.tag2Window < self.windowSize):
-                                self.tag2Window.append(tag2_position)
-                            else:
-                                self.tag2Window = self.tag2Window[1:] + [tag2_position]
-                            
-                            self.current_tag2_position = tuple(map(lambda li: sum(li) / len(li), zip(self.tag2Window)))
-
-                            self.get_logger().info(f"Tag 1 position: {self.current_tag1_position}")
-                            self.get_logger().info(f"Tag 2 position: {self.current_tag2_position}")
-                            target_msg = Point(x=self.current_tag2_position[0], y=self.current_tag2_position[1], z=self.current_tag2_position[2])
-                            self.target_location_pub.publish(target_msg)
-
-                            # Compute offset if not yet computed
-                            if not self.follow_offset_computed and self.current_yaw is not None and self.current_tag1_position is not None:
-                                self.uwb_to_imu_offset = self.compute_heading_offset(self.current_tag1_position, self.current_tag2_position, self.current_yaw)
-                                self.follow_offset_computed = True
-                        else:
-                            self.tag2Window = []
-                    else:
-                        self.get_logger().warning("Tag 2 not known")
-                        self.tag2Window = []
-                
-                if self.mode == "Call" and self.call_mode_active:
-                    self.get_logger().info("Call mode active.")
-                    if self.current_tag1_position is not None and self.current_target_index < len(self.path):
-                        self.get_logger().info(f"Current target index: {self.current_target_index}")
-                        target_node_name = self.path[self.current_target_index]
-                        target_pos = self.get_node_position(target_node_name)
-                        self.get_logger().info(f"Target position: {target_pos}")
-                        # Publish current position
-                        position_msg = Point(x=self.current_tag1_position[0], y=self.current_tag1_position[1], z=0.0)
-                        self.start_publisher.publish(position_msg)
-                        self.get_logger().info(f"Current position: {self.current_tag1_position} Success!")
-
-                        # Publish target position
-                        target_msg = Point(x=target_pos[0], y=target_pos[1], z=0.0)
-                        self.target_location_pub.publish(target_msg)
-                        self.get_logger().info(f"Target position: {target_pos} Success!")
-                        # Check if close to the node
-                        dist = math.sqrt((self.current_tag1_position[0] - target_pos[0])**2 + 
-                                        (self.current_tag1_position[1] - target_pos[1])**2)
-                        if dist < 10:  # Threshold of 5 cm
-                            self.current_target_index += 1
-                            if self.current_target_index >= len(self.path):
-                                self.get_logger().info("Call mode: Reached final destination.")
-                                self.api_pub.publish(String(data="CALLEND"))
-                            else:
-                                self.get_logger().info(f"Moving to next node: {self.path[self.current_target_index]}")
-
-            except Exception as e:
-                self.get_logger().error(f"Error updating positions: {e}")
-#ros2\src\robobin\robobin\graphs\graph.json
-    def clear_graph(self):
-        self.anchors_coords_known = False
-        self.anchors = {}
-        self.previous_tag1_position = None
-        workspace_root = os.getcwd()  # Current working directory
-        graph_file_path =  os.path.abspath(os.path.join(workspace_root, "src", "robobin", "robobin", "graphs", "graph.json"))
-        default_file_path = os.path.abspath(os.path.join(workspace_root, "src","robobin", "robobin", "graphs", "default_graph.json"))
-        try:
-            with open(default_file_path, 'r') as f:
-                graph_data = json.load(f)
-                with open(graph_file_path, 'w') as f:
-                    json.dump(graph_data, f)
-        except FileNotFoundError:
-            self.get_logger().error(f"Error: File not found at {default_file_path}")
-        except json.JSONDecodeError as e:
-            self.get_logger().error(f"Error: Failed to decode JSON - {str(e)}")
-        except Exception as e:
-            self.get_logger().error(f"Error: {str(e)}")
-
-        self.get_logger().info("Graph cleared.")
-
-    def handle_nav_command(self, msg):
-        self.get_logger().info(f"Raw nav command: {msg.data}")
-        data = msg.data.strip()
-        parts = data.split(" ")
-        self.get_logger().info(f"Received nav command: {data}")
-        self.get_logger().info(f"Parts: {parts}, length of first part: {len(parts[0])}")
-        if data == "BPM":
-            self.clear_graph()
-            self.call_bpm()
-            self.anchors_coords_known = True
-        elif data.startswith("SETMODE"):
-                mode = parts[1]
-                stop_cmd = Twist()
-                self.cmd_pub.publish(stop_cmd)  # Publish zero velocity
-                self.get_logger().info("Motors stopped.")
-                    
-                if mode in ["Manual", "Follow", "Call","Calibration"]:
-                    self.mode = mode
-                    self.get_logger().info(f"Mode set to: {mode}")
-
-                    
-                    if mode == "Manual":
-                        self.call_mode_active = False 
-                        self.follow_offset_computed = False
-                        self.follow_target_reached = False
-                        self.get_logger().info("Publishing to pathing node stopped.")
-                        
-                       
-                        stop_cmd = Twist()
-                        self.cmd_pub.publish(stop_cmd)  
-                        self.get_logger().info("Motors stopped.")
-                        
-                    elif mode == "Follow":
-                        self.follow_offset_computed = False
-                        self.follow_target_reached = False
-                        self.get_logger().info("Follow mode initialized.")
-                    
-                    elif mode == "Call":
-                        self.call_mode_active = False
-                        self.get_logger().info("Call mode initialized but not active yet.")
-                    elif mode == "Calibration":
-                        self.get_logger().info("Calibration mode initialized.")
-                        self.start_calibration()
-                else:
-                    self.get_logger().error("Invalid mode for UWB Navigation")
-
-        elif self.mode == "Call" and parts and parts[0] == "CALL":
-            self.get_logger().info(f"Received CALL command: {data}")
-            # Example: CALL A1 Node6
-            parts = data.split(" ")
-            if len(parts) < 3:
-                self.get_logger().error("CALL command requires start and end nodes.")
-                return
-            start_location = parts[1]
-            end_location = parts[2]
-            self.get_logger().info(f"Call received: Start={start_location}, End={end_location}")
-            # Load graph
-            workspace_root = os.getcwd()
-            graph_file_path = os.path.abspath(os.path.join(workspace_root, "src", "robobin", "robobin", "graphs", "graph.json"))
-            try:
-                with open(graph_file_path, 'r') as f:
-                    self.graph_data = json.load(f)
-                    self.get_logger().info("Graph loaded.")
-            except FileNotFoundError:
-                self.get_logger().error(f"Error: File not found at {graph_file_path}")
-                return
-            except json.JSONDecodeError as e:
-                self.get_logger().error(f"Error: Failed to decode JSON - {str(e)}")
-                return
-            except Exception as e:
-                self.get_logger().error(f"Error: {str(e)}")
-                return
-
-            
-            self.path = self.a_star_search(self.graph_data, start_location, end_location)
-            if self.path:
-                self.get_logger().info(f"Path found: {self.path}")
-                self.current_target_index = 0
-                self.call_mode_active = True
-            else:
-                self.get_logger().error("No path found.")
-
-    def compute_heading_offset(self, robot_pos, target_pos, imu_yaw):
-        # Compute heading from UWB positions
-        dx = target_pos[0] - robot_pos[0]
-        dy = target_pos[1] - robot_pos[1]
-        uwb_heading = math.atan2(dy, dx)
-        offset = imu_yaw - uwb_heading
-        return offset
-
-    def get_node_position(self, node_name):
-        if self.graph_data is None:
-            return None
-        for node in self.graph_data["nodes"]:
-            if node["name"] == node_name:
-                return (node["x"], node["y"])
-        return None
-
-    def a_star_search(self, graph_data, start_name, goal_name):
-        # Parse nodes and connections
-        nodes = graph_data["nodes"]
-        connections = graph_data["connections"]
-
-        # Create a mapping from name to index
-        name_to_index = {node["name"]: i for i, node in enumerate(nodes)}
-        if start_name not in name_to_index or goal_name not in name_to_index:
-            self.get_logger().error("Start or Goal node not found in the graph.")
-            return None
-
-        start_index = name_to_index[start_name]
-        goal_index = name_to_index[goal_name]
-
-      
-        def heuristic(n1, n2):
-            x1, y1 = nodes[n1]["x"], nodes[n1]["y"]
-            x2, y2 = nodes[n2]["x"], nodes[n2]["y"]
-            return math.sqrt((x1 - x2)**2 + (y1 - y2)**2)
-
-        open_set = {start_index}
-        came_from = {}
-        g_score = {i: float('inf') for i in range(len(nodes))}
-        g_score[start_index] = 0
-        f_score = {i: float('inf') for i in range(len(nodes))}
-        f_score[start_index] = heuristic(start_index, goal_index)
-
-        while open_set:
-            current = min(open_set, key=lambda idx: f_score[idx])
-            if current == goal_index:
-                
-                return self.reconstruct_path(came_from, current, nodes)
-
-            open_set.remove(current)
-            
-            for neighbor, connected in enumerate(connections[current]):
-                if not connected:
-                    continue
-                tentative_g_score = g_score[current] + heuristic(current, neighbor)
-                if tentative_g_score < g_score[neighbor]:
-                    came_from[neighbor] = current
-                    g_score[neighbor] = tentative_g_score
-                    f_score[neighbor] = tentative_g_score + heuristic(neighbor, goal_index)
-                    if neighbor not in open_set:
-                        open_set.add(neighbor)
-
-        return None
-
-    def reconstruct_path(self, came_from, current, nodes):
-        total_path = [nodes[current]["name"]]
-        while current in came_from:
-            current = came_from[current]
-            total_path.insert(0, nodes[current]["name"])
-        return total_path
-
-    def start_calibration(self):
-        """
-        When doing calibration, the robobin will drive forward for 2 seconds at 0.25 m/s (WARNING).
-        """
-        if not self.current_tag1_position:
-            self.get_logger().error("No current UWB position available - cannot calibrate.")
-            return
-
-        self.calibration_active = True
-
-      
-        self.calib_start_position = self.current_tag1_position
-        self.calib_start_yaw = self.current_yaw
-  
-        forward_cmd = Twist()
-        forward_cmd.linear.x = 0.25
-        self.cmd_pub.publish(forward_cmd)
-        self.get_logger().info("Driving forward at 0.25 m/s for 2 seconds...")
-
-    
-        self.calibration_timer = self.create_timer(2.0, self.finish_calibration)
-
-    def finish_calibration(self):
-    
-     
-        if self.calibration_timer:
-            self.destroy_timer(self.calibration_timer)
-            self.calibration_timer = None
-
-        stop_cmd = Twist()
-        self.cmd_pub.publish(stop_cmd)
-
-        if not self.current_tag1_position:
-            self.get_logger().error("No final UWB position available - calibration failed.")
-            self.calibration_active = False
-            return
-
-        calib_end_position = self.current_tag1_position
-        calib_end_yaw = self.current_yaw  
-
-   
-        dx = calib_end_position[0] - self.calib_start_position[0]
-        dy = calib_end_position[1] - self.calib_start_position[1]
-        uwb_heading = math.atan2(dy, dx)
-
-        offset = calib_end_yaw - uwb_heading
-        self.uwb_to_imu_offset = offset
-
-
-        self.get_logger().info("CALIBRATION COMPLETE!")
-        self.get_logger().info(f" Start Pos (UWB):   {self.calib_start_position}")
-        self.get_logger().info(f" End Pos (UWB):     {calib_end_position}")
-        self.get_logger().info(f" UWB heading:       {uwb_heading:.3f} rad")
-        self.get_logger().info(f" IMU yaw:           {calib_end_yaw:.3f} rad")
-        self.get_logger().info(f" --> OFFSET:        {offset:.3f} rad")
-
-        
-        self.calibration_active = False
-        self.mode = "Manual" 
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = UWBNode()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == "__main__":
-    main()
diff --git a/ros2/src/robobin/robobin/uwb_pathing_node.py b/ros2/src/robobin/robobin/uwb_pathing_node.py
deleted file mode 100644
index 00fc1b498f4e752d4c447257a718e56f52858d9c..0000000000000000000000000000000000000000
--- a/ros2/src/robobin/robobin/uwb_pathing_node.py
+++ /dev/null
@@ -1,122 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import Twist, Point
-from math import sqrt, pow, atan2, radians, sin, cos
-import time
-
-
-class UWBPathingNode(Node):
-    def __init__(self):
-        super().__init__('uwb_pathing_node')
-
-        # Publisher for cmd_vel
-        self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
-
-        # Subscriptions
-        self.create_subscription(Point, '/start_location', self.current_location_callback, 10)
-        self.create_subscription(Point, '/target_location', self.target_location_callback, 10)
-
-        # Current and target positions
-        self.current_x = None
-        self.current_y = None
-        self.target_x = None
-        self.target_y = None
-
-        # Navigation thresholds
-        self.distance_threshold = 0.25  # meters
-        self.angle_threshold = radians(0.5)  # radians
-
-        self.kp_linear = 0.5  # Proportional gain for linear movement
-        self.kp_angular = 1.0  # Proportional gain for angular movement
-
-        self.get_logger().info('UWB Pathing Node started and waiting for positions.')
-
-    def current_location_callback(self, msg: Point):
-        """Callback to update the robot's current position."""
-        self.current_x = msg.x
-        self.current_y = msg.y
-        self.get_logger().info(f"Current Location Updated: x={self.current_x:.2f}, y={self.current_y:.2f}")
-        self.check_and_navigate()
-
-    def target_location_callback(self, msg: Point):
-        """Callback to update the target position."""
-        self.target_x = msg.x
-        self.target_y = msg.y
-        self.get_logger().info(f"Target Location Updated: x={self.target_x:.2f}, y={self.target_y:.2f}")
-        self.check_and_navigate()
-
-    def check_and_navigate(self):
-        """Check if both positions are available and navigate to the target."""
-        if self.current_x is not None and self.current_y is not None and self.target_x is not None and self.target_y is not None:
-            self.get_logger().info("Navigating to target...")
-            self.navigate_to_target()
-        else:
-            self.get_logger().warning("Waiting for both current and target positions to be available...")
-
-    def navigate_to_target(self):
-        # Ensure positions are known
-        if self.current_x is None or self.current_y is None or self.target_x is None or self.target_y is None:
-            self.get_logger().warning("Positions not fully known yet.")
-            return
-
-        # Calculate distance and angle to the target
-        displacement_x = self.target_x - self.current_x
-        displacement_y = self.target_y - self.current_y
-        distance_to_target = sqrt(pow(displacement_x, 2) + pow(displacement_y, 2))
-        angle_to_target = atan2(displacement_y, displacement_x)
-
-
-        # Check if we are close enough to the target
-        if distance_to_target <= self.distance_threshold:
-            self.stop_robot()
-            self.get_logger().info("Target reached successfully.")
-            return
-
-        # Calculate yaw error (assuming robot orientation 0 = facing x-axis)
-
-        yaw_error = angle_to_target
-        yaw_error = atan2(sin(yaw_error), cos(yaw_error))  # Normalize angle
-        self.get_logger().info(f"Current Position: x={self.current_x:.2f}, y={self.current_y:.2f}")
-        self.get_logger().info(f"Target Position: x={self.target_x:.2f}, y={self.target_y:.2f}")
-        self.get_logger().info(f"Distance to Target: distance_to_target={distance_to_target:.2f} meters")
-        self.get_logger().info(f"Angle to Target: angle_to_target={angle_to_target:.2f} radians")
-        twist = Twist()
-
-        # Decide on angular velocity first
-        if abs(yaw_error) > self.angle_threshold:
-            angular_speed = self.kp_angular * abs(yaw_error)
-            twist.angular.z = angular_speed if yaw_error > 0 else -angular_speed
-            self.get_logger().info(f"Correcting Heading: yaw_error={yaw_error:.2f} radians")
-        else:
-            # Move forward when aligned with the target
-            linear_speed = self.kp_linear * distance_to_target
-            twist.linear.x = min(0.2, linear_speed)  # Limit max speed
-            self.get_logger().info(f"Moving to Target: distance={distance_to_target:.2f} meters")
-
-        # # Publish movement command
-        # self.cmd_vel_pub.publish(twist)
-
-
-    def stop_robot(self):
-        """Stops the robot by publishing zero velocities."""
-        twist = Twist()
-        self.cmd_vel_pub.publish(twist)
-        time.sleep(0.5)
-        self.get_logger().info("Robot stopped.")
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = UWBPathingNode()
-
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()
diff --git a/ros2/src/robobin/setup.py b/ros2/src/robobin/setup.py
index b32395395586e8c9b6388eb6d76f649819af4727..489980a1de2829a169ae52292f59fee539b65ffa 100644
--- a/ros2/src/robobin/setup.py
+++ b/ros2/src/robobin/setup.py
@@ -26,12 +26,10 @@ setup(
         'console_scripts': [
             'api_node = robobin.api_node:main',
             'motor_control_node = robobin.motor_control_node:main',
-            'uwb_navigation_node = robobin.uwb_navigation_node:main',
             'imu_node = robobin.imu_node:main',
             'motor_node = robobin.motor_control_node:main',
             'encoder_node = robobin.encoder:main',
             'control_feedback = robobin.control_feedback:main',
-            'uwb_pathing_node = robobin.uwb_pathing_node:main',
         ],
     },
 )