diff --git a/App/RobobinApp/Networking/WifiManager.cs b/App/RobobinApp/Networking/WifiManager.cs
index 1ba99cc03b7edc3659ac4991482a22295259a77c..df8f21e970cd3e6c17239a588790a2646ac12285 100644
--- a/App/RobobinApp/Networking/WifiManager.cs
+++ b/App/RobobinApp/Networking/WifiManager.cs
@@ -210,7 +210,7 @@ namespace RobobinApp.Networking
         public async Task SendCallMeRequest(string endPoint)
         {
             Debug.WriteLine("Call me request");
-            await SendMessageAsync("CALLME " + endPoint);
+            await SendMessageAsync("CALLMELIDAR " + endPoint);
         }
 
         public async Task SendPingMessage()
@@ -228,7 +228,7 @@ namespace RobobinApp.Networking
                 Debug.WriteLine("Connected to Robobin via TCP.");
                 _isConnected = true;
 
-                await SendMessageAsync("REQMAP");
+                //await SendMessageAsync("REQMAP");
                 Task.Run(() => ReceiveMessages());
 
   
diff --git a/App/RobobinApp/Resources/Images/lab_map.jpg b/App/RobobinApp/Resources/Images/lab_map.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..1d7ac60e7dc9e87a9b961d14535710a373025f43
Binary files /dev/null and b/App/RobobinApp/Resources/Images/lab_map.jpg differ
diff --git a/App/RobobinApp/RobobinApp.csproj b/App/RobobinApp/RobobinApp.csproj
index a8e008bc3cd5f030d851ba9a7f601d16378528d6..ecc4a079aa94816911a4b1dd42710bd42213e475 100644
--- a/App/RobobinApp/RobobinApp.csproj
+++ b/App/RobobinApp/RobobinApp.csproj
@@ -48,11 +48,21 @@
 		<AndroidResource Remove="Platforms\Android\Resources\mipmap\robobinlogo1.png" />
 	</ItemGroup>
 
+	<ItemGroup>
+	  <MauiImage Remove="Resources\Images\lab_map.jpg" />
+	</ItemGroup>
+
 	<ItemGroup>
 		<None Remove="Platforms\Android\Resources\drawable\bin.svg" />
 		<None Remove="Platforms\Android\Resources\mipmap\robobinlogo1.png" />
 	</ItemGroup>
 
+	<ItemGroup>
+	  <EmbeddedResource Include="Resources\Images\lab_map.jpg">
+	    <CopyToOutputDirectory>Always</CopyToOutputDirectory>
+	  </EmbeddedResource>
+	</ItemGroup>
+
 	<ItemGroup>
 		<PackageReference Include="Microsoft.Extensions.Logging.Console" Version="8.0.1" />
 		<PackageReference Include="Microsoft.Maui.Controls" Version="8.0.91" />
diff --git a/App/RobobinApp/ViewModels/MainPageViewModel.cs b/App/RobobinApp/ViewModels/MainPageViewModel.cs
index 1eb56a51bc0920d75029071f84cd909967fe75d5..e6236440d9ac13e9024c6d690de1d7c83f07577d 100644
--- a/App/RobobinApp/ViewModels/MainPageViewModel.cs
+++ b/App/RobobinApp/ViewModels/MainPageViewModel.cs
@@ -1,6 +1,7 @@
 using System.Collections.ObjectModel;
 using System.ComponentModel;
 using System.Diagnostics;
+using System.Linq;
 using System.Runtime.CompilerServices;
 using System.Windows.Input;
 using Microsoft.Extensions.Logging;
@@ -58,15 +59,26 @@ namespace RobobinApp.ViewModels
             ToggleModeCommand = new Command(async () => await OnToggleMode());
             GetNewGraphCommand = new Command(async () => await GetNewGraph());
             var graphNodes1 = new List<Node>
-
-{
-    new Node("No graph loaded", 50, 50),
-   
-};
-
-            var graphMatrix1 = CalculatePolarConnections(graphNodes1, new bool[,]
             {
-    { false } });
+                new Node("1", 347, 276),
+                new Node("2", 321, 280),
+                new Node("3", 350, 318),
+                new Node("4", 326, 317),
+                new Node("5", 357, 360),
+                new Node("6", 328, 355),
+                new Node("7", 355, 404),
+                new Node("8", 332, 390),
+                new Node("9", 359, 455),
+                new Node("10", 334, 425),
+                new Node("11", 423, 443),
+                new Node("12", 417, 400),
+                new Node("13", 417, 355),
+                new Node("14", 416, 313),
+                new Node("15", 417, 269)
+            };
+
+
+            var graphMatrix1 = CalculatePolarConnections(graphNodes1, new bool[graphNodes1.Count, graphNodes1.Count]);
             var mapdata = App.WifiManager.MapData;
             Debug.WriteLine($"Mapdata : { mapdata}");
             var graphNodes = graphNodes1;
@@ -86,7 +98,7 @@ namespace RobobinApp.ViewModels
 
         private async Task GetNewGraph()
         {
-            await App.WifiManager.SendMessageAsync("REQMAP");
+            return;
         }
 
         public static (float magnitude, float angle)[,] CalculatePolarConnections(List<Node> nodes, bool[,] connections)
diff --git a/App/RobobinApp/Views/CentreGraph.xaml.cs b/App/RobobinApp/Views/CentreGraph.xaml.cs
index 41c3ff8b771c9ce8dc879c9c1e843243150c8ac8..43cf7946362f3b3aad18e02f3d7ee389ab668b43 100644
--- a/App/RobobinApp/Views/CentreGraph.xaml.cs
+++ b/App/RobobinApp/Views/CentreGraph.xaml.cs
@@ -1,42 +1,30 @@
 using Microsoft.Maui.Graphics;
 using Robobin.Models;
-using RobobinApp.Models;
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
-using System.Linq;
 
 namespace RobobinApp.Views
 {
     public class GraphicsDrawable : IDrawable
     {
         private Graph _graph;
-        private (float, float) _location;
-        public const float BaseNodeRadius = 5f; // Make this public if needed elsewhere
+        public const float NodeRadius = 5f; // Fixed radius for nodes
         private static readonly Color DefaultNodeColor = Colors.Red;
-        private static readonly Color HomeNodeColor = Colors.Green;
-        private static readonly Color BackgroundColor = Color.FromHex("#2C2F33");
-        private static readonly Color ConnectionColor = Colors.Grey;
-        private static readonly Color HighlightedNodeColor = Colors.Yellow; // For the highlighted node
-        private static readonly Color PathColor = Colors.Blue; // For the path
+        private static readonly Color HighlightedNodeColor = Colors.Yellow; // Highlighted node color
         private RectF storedDirtyRect;
 
-        public float CurrentScale { get; private set; } = 1.0f;
-        public (float X, float Y) CurrentOffset { get; private set; } = (0, 0);
-
         private Node _highlightedNode;
-        private List<Node> _path;
 
         public void SetGraph(Graph graph)
         {
             _graph = graph ?? throw new ArgumentNullException(nameof(graph));
             _highlightedNode = null;
-            _path = null;
         }
 
-        public void SetLocation((float, float) location)
+        public void HighlightNode(Node node)
         {
-            _location = location;
+            _highlightedNode = node;
         }
 
         public void Draw(ICanvas canvas, RectF dirtyRect)
@@ -44,243 +32,112 @@ namespace RobobinApp.Views
             storedDirtyRect = dirtyRect;
             if (_graph == null) return;
 
+            // Draw the background image
             DrawBackground(canvas, dirtyRect);
 
-            (float minX, float minY, float maxX, float maxY) = CalculateBounds();
-
-            CurrentScale = CalculateScale(dirtyRect, minX, minY, maxX, maxY);
-            (float offsetX, float offsetY) = CalculateOffsets(dirtyRect, minX, minY, maxX, maxY, CurrentScale);
-            CurrentOffset = (offsetX, offsetY);
-            float nodeRadius = BaseNodeRadius * CurrentScale;
-
-            DrawConnections(canvas, CurrentScale, offsetX, offsetY);
-            DrawPath(canvas, CurrentScale, offsetX, offsetY);
-            DrawNodes(canvas, CurrentScale, offsetX, offsetY, nodeRadius, _highlightedNode);
-            DrawLocation(canvas, CurrentScale, offsetX, offsetY);
-        }
-
-        private void DrawLocation(ICanvas canvas, float scale, float offsetX, float offsetY)
-        {
-            if (_location.Item1 == 0 && _location.Item2 == 0)
-            {
-                return;
-            }
-            Debug.WriteLine($"Drawing Location {_location.Item1} {_location.Item2}");
-            float scaledX = _location.Item1 * scale + offsetX;
-            float flippedY = storedDirtyRect.Height - (_location.Item2 * scale + offsetY);
-
-            canvas.FillColor = Colors.Yellow;
-            canvas.FillCircle(scaledX, flippedY, 5);
-            canvas.FontColor = Colors.White;
-            canvas.DrawString($"Robobin ({_location.Item1} {_location.Item2})", scaledX + 7, flippedY - 5, HorizontalAlignment.Left);
+            // Draw the nodes on top of the image
+            DrawNodes(canvas, NodeRadius, _highlightedNode);
         }
 
         private void DrawBackground(ICanvas canvas, RectF dirtyRect)
         {
-            canvas.FillColor = BackgroundColor;
-            canvas.FillRectangle(dirtyRect);
-        }
-
-        private (float minX, float minY, float maxX, float maxY) CalculateBounds()
-        {
-            float minX = _graph.Nodes.Min(node => node.X);
-            float minY = _graph.Nodes.Min(node => node.Y);
-            float maxX = _graph.Nodes.Max(node => node.X);
-            float maxY = _graph.Nodes.Max(node => node.Y);
-            return (minX, minY, maxX, maxY);
-        }
-
-        private float CalculateScale(RectF dirtyRect, float minX, float minY, float maxX, float maxY)
-        {
-            float graphWidth = maxX - minX;
-            float graphHeight = maxY - minY;
-            float scaleX = dirtyRect.Width / graphWidth;
-            float scaleY = dirtyRect.Height / graphHeight;
-            return Math.Min(scaleX, scaleY) * 0.8f;
-        }
-
-        private (float offsetX, float offsetY) CalculateOffsets(RectF dirtyRect, float minX, float minY, float maxX, float maxY, float scale)
-        {
-            float offsetX = dirtyRect.Center.X - ((minX + maxX) * 0.5f) * scale;
-            float offsetY = dirtyRect.Center.Y - ((minY + maxY) * 0.5f) * scale;
-            return (offsetX, offsetY);
-        }
-
-        private void DrawConnections(ICanvas canvas, float scale, float offsetX, float offsetY)
-        {
-            canvas.StrokeColor = ConnectionColor;
-            canvas.StrokeSize = 2;
-
-            foreach (var node in _graph.Nodes)
+            try
             {
-                int nodeIndex = _graph.Nodes.IndexOf(node);
-                float startX = node.X * scale + offsetX;
-                float startY = storedDirtyRect.Height - (node.Y * scale + offsetY); // flip Y
+                // Load the embedded image resource
+                var assembly = typeof(GraphicsDrawable).Assembly;
+                var resourceStream = assembly.GetManifestResourceStream("Robobin.Resources.Images.lab_map.jpg");
 
-                for (int j = 0; j < _graph.Nodes.Count; j++)
+                if (resourceStream != null)
                 {
-                    var (magnitude, angle) = _graph.AdjacencyMatrix[nodeIndex, j];
-                    if (magnitude > 0)
-                    {
-                        float targetX = (node.X + magnitude * MathF.Cos(MathF.PI * angle / 180)) * scale + offsetX;
-                        float targetRawY = (node.Y + magnitude * MathF.Sin(MathF.PI * angle / 180)) * scale + offsetY;
-                        float targetY = storedDirtyRect.Height - targetRawY; // flip Y
-
-                        canvas.DrawLine(startX, startY, targetX, targetY);
-                    }
+                    var image = Microsoft.Maui.Graphics.Platform.PlatformImage.FromStream(resourceStream);
+                    canvas.DrawImage(image, dirtyRect.Left, dirtyRect.Top, dirtyRect.Width, dirtyRect.Height);
+                }
+                else
+                {
+                    throw new Exception("Embedded resource not found.");
                 }
             }
-        }
-
-        private void DrawNodes(ICanvas canvas, float scale, float offsetX, float offsetY, float nodeRadius, Node highlightedNode)
-        {
-            foreach (var node in _graph.Nodes)
+            catch (Exception ex)
             {
-                float scaledX = node.X * scale + offsetX;
-                float flippedY = storedDirtyRect.Height - (node.Y * scale + offsetY); // flip Y
-
-                canvas.FillColor = (highlightedNode == node)
-                                   ? HighlightedNodeColor
-                                   : (node.Name == "Home" ? HomeNodeColor : DefaultNodeColor);
+                Debug.WriteLine($"Error loading image: {ex.Message}");
 
-                canvas.FillCircle(scaledX, flippedY, nodeRadius);
-                canvas.FontColor = Colors.White;
-                var nodeDetails = $"{node.Name}";
-                canvas.DrawString(nodeDetails, scaledX + nodeRadius + 2, flippedY - nodeRadius, HorizontalAlignment.Left);
+                // Fallback: fill background with a color if the image is unavailable
+                canvas.FillColor = Colors.Black;
+                canvas.FillRectangle(dirtyRect);
             }
         }
 
-        public Node FindNearestNode(float clickX, float clickY, float scale, float offsetX, float offsetY)
+        private void DrawNodes(ICanvas canvas, float nodeRadius, Node highlightedNode)
         {
-            // The coordinates given (clickX, clickY) are in the original coordinate system 
-            // where the y-axis goes down. We must transform them similarly to how we did 
-            // when drawing: flip the Y using storedDirtyRect.
-            float flippedClickY = clickY;
-
-            Node nearestNode = null;
-            float minDistance = float.MaxValue;
-
-            foreach (var node in _graph.Nodes)
-            {
-                float scaledX = node.X * scale + offsetX;
-                // Flip Y when comparing distances
-                float flippedY = storedDirtyRect.Height - (node.Y * scale + offsetY);
-
-                float dx = clickX - scaledX;
-                float dy = flippedClickY - flippedY;
-                float distance = MathF.Sqrt(dx * dx + dy * dy);
-                if (distance < minDistance)
-                {
-                    minDistance = distance;
-                    nearestNode = node;
-                }
-            }
+            // Calculate offsets to center the nodes
+            float canvasCenterX = storedDirtyRect.Width * 4.5f / 7;
+            float canvasCenterY = storedDirtyRect.Height / 2;
 
-            return nearestNode;
-        }
+            // Calculate the center of the graph based on node coordinates
+            float graphCenterX = (_graph.Nodes.Min(node => node.X) + _graph.Nodes.Max(node => node.X)) / 2;
+            float graphCenterY = (_graph.Nodes.Min(node => node.Y) + _graph.Nodes.Max(node => node.Y)) / 2;
 
-        public void HighlightNode(Node node)
-        {
-            _highlightedNode = node;
-        }
-
-        public List<Node> AStarPathfinding(Node startNode, Node targetNode)
-        {
-            var openSet = new List<Node> { startNode };
-            var cameFrom = new Dictionary<Node, Node>();
-            var gScore = new Dictionary<Node, float>();
-            var fScore = new Dictionary<Node, float>();
+            // Offset to center the graph
+            float offsetX = canvasCenterX - graphCenterX;
+            float offsetY = canvasCenterY - graphCenterY;
 
             foreach (var node in _graph.Nodes)
             {
-                gScore[node] = float.MaxValue;
-                fScore[node] = float.MaxValue;
-            }
-            gScore[startNode] = 0;
-            fScore[startNode] = Heuristic(startNode, targetNode);
-
-            while (openSet.Count > 0)
-            {
-                Node currentNode = openSet.OrderBy(node => fScore[node]).First();
+                // Apply centering offsets
+                float centeredX = node.X + offsetX;
+                float centeredY = node.Y + offsetY;
 
-                if (currentNode == targetNode)
-                {
-                    return ReconstructPath(cameFrom, currentNode);
-                }
+                // Set fill color based on whether the node is highlighted
+                canvas.FillColor = (highlightedNode == node) ? HighlightedNodeColor : DefaultNodeColor;
 
-                openSet.Remove(currentNode);
+                // Draw the node as a circle
+                canvas.FillCircle(centeredX, centeredY, nodeRadius);
 
-                foreach (var edge in currentNode.Edges)
-                {
-                    Node neighbor = edge.TargetNode;
-                    float tentativeGScore = gScore[currentNode] + edge.Cost;
-
-                    if (tentativeGScore < gScore[neighbor])
-                    {
-                        cameFrom[neighbor] = currentNode;
-                        gScore[neighbor] = tentativeGScore;
-                        fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor, targetNode);
-
-                        if (!openSet.Contains(neighbor))
-                        {
-                            openSet.Add(neighbor);
-                        }
-                    }
-                }
+                // Optionally, draw node name or other details
+                canvas.FontColor = Colors.Black;
+                canvas.DrawString(node.Name, centeredX + nodeRadius + 2, centeredY - nodeRadius, HorizontalAlignment.Left);
             }
-
-            return new List<Node>();
         }
 
-        private float Heuristic(Node a, Node b)
-        {
-            return MathF.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y));
-        }
 
-        private List<Node> ReconstructPath(Dictionary<Node, Node> cameFrom, Node currentNode)
+        public Node FindNearestNode(float tapPointX, float tapPointY)
         {
-            var totalPath = new List<Node> { currentNode };
+            // Calculate offsets to center the nodes
+            float canvasCenterX = storedDirtyRect.Width * 4.5f / 7;
+            float canvasCenterY = storedDirtyRect.Height / 2;
 
-            while (cameFrom.ContainsKey(currentNode))
-            {
-                currentNode = cameFrom[currentNode];
-                totalPath.Add(currentNode);
-            }
+            // Calculate the center of the graph based on node coordinates
+            float graphCenterX = (_graph.Nodes.Min(node => node.X) + _graph.Nodes.Max(node => node.X)) / 2;
+            float graphCenterY = (_graph.Nodes.Min(node => node.Y) + _graph.Nodes.Max(node => node.Y)) / 2;
 
-            totalPath.Reverse();
-            return totalPath;
-        }
+            // Offset to center the graph
+            float offsetX = canvasCenterX - graphCenterX;
+            float offsetY = canvasCenterY - graphCenterY;
 
-        public Node GetHomeNode()
-        {
-            return _graph.Nodes.FirstOrDefault(n => n.Name == "Home");
-        }
-
-        public List<Node> Path
-        {
-            get => _path;
-            set => _path = value;
-        }
+            float minDistance = float.MaxValue;
+            Node nearestNode = null;
 
-        private void DrawPath(ICanvas canvas, float scale, float offsetX, float offsetY)
-        {
-            if (_path != null && _path.Count > 0)
+            foreach (var node in _graph.Nodes)
             {
-                canvas.StrokeColor = PathColor;
-                canvas.StrokeSize = 3;
+                // Apply centering offsets to node positions
+                float centeredX = node.X + offsetX;
+                float centeredY = node.Y + offsetY;
 
-                for (int i = 0; i < _path.Count - 1; i++)
-                {
-                    float startX = _path[i].X * scale + offsetX;
-                    float startRawY = _path[i].Y * scale + offsetY;
-                    float startY = storedDirtyRect.Height - startRawY; // flip Y
-                    float endX = _path[i + 1].X * scale + offsetX;
-                    float endRawY = _path[i + 1].Y * scale + offsetY;
-                    float endY = storedDirtyRect.Height - endRawY; // flip Y
+                // Calculate the distance between the tapped point and the centered node position
+                float distance = (float)Math.Sqrt(
+                    Math.Pow(centeredX - tapPointX, 2) +
+                    Math.Pow(centeredY - tapPointY, 2)
+                );
 
-                    canvas.DrawLine(startX, startY, endX, endY);
+                if (distance < minDistance)
+                {
+                    minDistance = distance;
+                    nearestNode = node;
                 }
             }
+
+            return nearestNode;
         }
+
     }
 }
diff --git a/App/RobobinApp/Views/MainPage.xaml.cs b/App/RobobinApp/Views/MainPage.xaml.cs
index 533e36ad3deccfbb9d762b15e0ab72f50a9c730a..3022db93b3b46d8817e10f8d0278ded97ec8c5e4 100644
--- a/App/RobobinApp/Views/MainPage.xaml.cs
+++ b/App/RobobinApp/Views/MainPage.xaml.cs
@@ -52,40 +52,32 @@ namespace RobobinApp.Views
 
         private void OnGraphicsViewTapped(object sender, TappedEventArgs e)
         {
-            if (App.WifiManager.Mode != "Call")
-            {
-                DisplayAlert("NO CALL","Bin must be in call mode to use this feature!","Sorry!");
-                return;
-            }
+            //if (App.WifiManager.Mode != "Call")
+            //{
+            //    DisplayAlert("NO CALL","Bin must be in call mode to use this feature!","Sorry!");
+            //    return;
+            //}
             var tapPoint = e.GetPosition(GraphicsView);
             var drawable = (GraphicsDrawable)Resources["drawable"];
 
-            float scale = drawable.CurrentScale;
-            var offset = drawable.CurrentOffset;
+    
 
             var tapPointX = (float)tapPoint.Value.X;
             var tapPointY = (float)tapPoint.Value.Y;
+            Debug.WriteLine($"position: {tapPointX}, {tapPointY}");
             (float,float) endPoint = (tapPointX, tapPointY);
             
 
-            var nearestNode = drawable.FindNearestNode(tapPointX, tapPointY, scale, offset.X, offset.Y);
-            Task task = App.WifiManager.SendCallMeRequest(nearestNode.Name);
-            // Highlight the nearest node
-            drawable.HighlightNode(nearestNode);
+            var nearestNode = drawable.FindNearestNode(tapPointX, tapPointY);
+           
+
+        
 
             if (nearestNode != null)
             {
-                // Find the home node
-                Node homeNode = drawable.GetHomeNode(); 
-
-                if (homeNode != null)
-                {
-         
-                    List<Node> path = drawable.AStarPathfinding(nearestNode, homeNode);
-                    drawable.Path = path;
-                    GraphicsView.Invalidate();
-                }
-
+                Task task = App.WifiManager.SendCallMeRequest(nearestNode.Name);
+                // Highlight the nearest node
+                drawable.HighlightNode(nearestNode);
                 DisplayAlert("Node Clicked", $"You clicked on node: {nearestNode.Name}", "OK");
             }
             else
@@ -113,7 +105,7 @@ namespace RobobinApp.Views
                 {
                     var drawable = (GraphicsDrawable)Resources["drawable"];
                     Debug.WriteLine($"VM Location  { viewModel.Location }");
-                    drawable.SetLocation(viewModel.Location);
+                    //drawable.SetLocation(viewModel.Location);
                     drawable.SetGraph(viewModel.Graph);
                     GraphicsView.Invalidate(); // Redraw the GraphicsView
                     Debug.WriteLine("Graph updated and redrawn.");
diff --git a/App/RobobinApp/Views/MainPage_Android.xaml.cs b/App/RobobinApp/Views/MainPage_Android.xaml.cs
index cedfdc3c0458358b39e6cfb4407c12a9ba87749b..d7f68d373ae281dd7ac797dc9329c431f0166ed1 100644
--- a/App/RobobinApp/Views/MainPage_Android.xaml.cs
+++ b/App/RobobinApp/Views/MainPage_Android.xaml.cs
@@ -34,33 +34,21 @@ namespace RobobinApp.Views
             var tapPoint = e.GetPosition(GraphicsView);
             var drawable = (GraphicsDrawable)Resources["drawable"];
 
-            float scale = drawable.CurrentScale;
-            var offset = drawable.CurrentOffset;
-
+        
             var tapPointX = (float)tapPoint.Value.X;
             var tapPointY = (float)tapPoint.Value.Y;
 
             (float, float) endPoint = (tapPointX, tapPointY);
            
 
-           var nearestNode = drawable.FindNearestNode(tapPointX, tapPointY, scale, offset.X, offset.Y);
+           var nearestNode = drawable.FindNearestNode(tapPointX, tapPointY);
             Task task = App.WifiManager.SendCallMeRequest(nearestNode.Name);
             // Highlight the nearest node
             drawable.HighlightNode(nearestNode);
 
             if (nearestNode != null)
             {
-                // Find the home node
-                Node homeNode = drawable.GetHomeNode();
-
-                if (homeNode != null)
-                {
-
-                    List<Node> path = drawable.AStarPathfinding(nearestNode, homeNode);
-                    drawable.Path = path;
-                    GraphicsView.Invalidate();
-                }
-
+             
                 DisplayAlert("Node Clicked", $"You clicked on node: {nearestNode.Name}", "OK");
             }
             else
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/launch/robobin_no_components_launch.py b/ros2/src/robobin/launch/robobin_no_components_launch.py
index fb6fabd42f37330c2392b52a3f97195b5f08da27..725aab91b4fd571cb3f61b992b5b6be1706aef6d 100644
--- a/ros2/src/robobin/launch/robobin_no_components_launch.py
+++ b/ros2/src/robobin/launch/robobin_no_components_launch.py
@@ -13,18 +13,11 @@ 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',
-            executable='motor_control_node',    
-            name='motor_control_node',
+            executable='control_feedback',    
+            name='control_feedback',
             output='screen',
             emulate_tty=True
         ),
diff --git a/ros2/src/robobin/robobin/api_node.py b/ros2/src/robobin/robobin/api_node.py
index 8a506d758554204ebb454ffe497e95ebe7401202..17bc92818f72d1c1f537c55087f12e922e892ec3 100644
--- a/ros2/src/robobin/robobin/api_node.py
+++ b/ros2/src/robobin/robobin/api_node.py
@@ -10,6 +10,7 @@ from geometry_msgs.msg import Point
 from std_msgs.msg import String
 import rclpy
 from rclpy.node import Node
+from std_msgs.msg import Int32
 
 class ApiNode(Node):
     def __init__(self):
@@ -20,6 +21,7 @@ class ApiNode(Node):
             "cmd_vel": self.create_publisher(Twist, '/cmd_vel', 10),
             "nav_send": self.create_publisher(Twist, '/nav_send', 10), 
             "nav_command": self.create_publisher(String, '/nav_command', 10),
+            "nav_command_lidar": self.create_publisher(Int32, '/nav_command_lidar', 10)
         }
         self.location_subscriber = self.create_subscription(
             Point,  # Message type
@@ -51,50 +53,17 @@ class ApiNode(Node):
         if self.mode == "Call": 
             if len(self.called_locations) > 0:
                 self.get_logger().info("Handling queue operations...")
-                start_node = self.getNearestNode(self.connection_manager.location)
                 client_socket, end_location = self.called_locations[0]
-                if start_node != self.current_nearest_node:
-                    self.current_nearest_node = start_node
-                    self.get_logger().info(f"New nearest node: {start_node}")
-                    self.get_logger().info(f"(Start Node: {start_node}, End Location: {end_location})")
-                    # self.get_logger().info(f"Sending location: {end_location}")
+                self.get_logger().info(f" End Location: {end_location})")
+                # self.get_logger().info(f"Sending location: {end_location}")
 
-                    self.publisher_topics["nav_command"].publish(String(data="CALL " + start_node + " " + end_location))
-                else:
-                    self.get_logger().info("Already at nearest node.")
-                
-    def getNearestNode(self, location): 
-        """
-        Return the name of the node nearest to the given (x, y) location.
-        """
-        workspace_root = os.getcwd()  # Current working directory
-        self.get_logger().info(f"Workspace root: {workspace_root}")
-        graph_file_path = os.path.abspath(os.path.join(workspace_root, "src", "robobin", "robobin", "graphs", "graph.json"))
-        self.get_logger().info(f"Graph file path: {graph_file_path}")
-        if not os.path.exists(graph_file_path):
-            self.get_logger().error(f"Graph file not found at: {graph_file_path}")
-            return None
-
-        x_loc, y_loc = location
-        try:
-            with open(graph_file_path, 'r') as f:
-                graph_data = json.load(f)
-
-            closest_node_name = None
-            closest_distance = float('inf')
-
-            for node in graph_data["nodes"]:
-                x_node = node["x"]
-                y_node = node["y"]
-                dist = math.sqrt((x_node - x_loc)**2 + (y_node - y_loc)**2)
-                if dist < closest_distance:
-                    closest_distance = dist
-                    closest_node_name = node["name"]
+               
 
-            return closest_node_name
-        except Exception as e:
-            self.get_logger().error(f"Failed to determine nearest node: {str(e)}")
-            return None
+                self.publisher_topics["nav_command"].publish(String(data="CALL " + start_node + " " + end_location))
+                
+                self.get_logger().info("Already at nearest node.")
+                
+    
     def handle_client_connection(self, client_socket):
         """Handles incoming TCP client connections."""
         try:
@@ -111,6 +80,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:
@@ -165,6 +135,9 @@ class ApiNode(Node):
             elif topic == "nav_command" and isinstance(message, str):
                 self.get_logger().info(f"Published to {topic}: {message}")
                 publisher.publish(String(data=message))
+            elif topic == "nav_command_lidar" and isinstance(message, int):
+                self.get_logger().info(f"Published to {topic}: {message}")
+                publisher.publish(Int32(data=message))
             else:
                 self.get_logger().warning(f"Unhandled message type for topic: {topic}")
         else:
diff --git a/ros2/src/robobin/robobin/helpers/connection_manager.py b/ros2/src/robobin/robobin/helpers/connection_manager.py
index a2498443aada764e8130e516640e070c90e93bc1..769a59694a6810ddbef17a495d803733c9653fe6 100644
--- a/ros2/src/robobin/robobin/helpers/connection_manager.py
+++ b/ros2/src/robobin/robobin/helpers/connection_manager.py
@@ -69,8 +69,8 @@ class ConnectionManager:
 
                 # Send the JSON message over UDP
                 sock.sendto(json_message, (self.UDP_IP, self.UDP_PORT))
-                self.api_node.get_logger().info("Broadcasting JSON presence.")
-                self.api_node.get_logger().info(f"Location: {self.location}, Mode: {mode}")
+                # self.api_node.get_logger().info("Broadcasting JSON presence.")
+                # self.api_node.get_logger().info(f"Location: {self.location}, Mode: {mode}")
                 time.sleep(0.5)
             except OSError:
                 break
diff --git a/ros2/src/robobin/robobin/helpers/message_handler.py b/ros2/src/robobin/robobin/helpers/message_handler.py
index 6c5701bf300aeb34dec02939e32f1f091de3feed..a3a380d0fc3002134507dfe6015804e8d17e78aa 100644
--- a/ros2/src/robobin/robobin/helpers/message_handler.py
+++ b/ros2/src/robobin/robobin/helpers/message_handler.py
@@ -16,12 +16,24 @@ class MessageHandler:
             "CALLME": self.handle_call_me,
             "CALLBPM": 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_lidar", int(message)
+
+
     def handle_request_location(self, client_socket, message):
         """Handles the LOC command."""
         if self.testing:
-            print("Requesting location")
+            print("Received LOC command")
             return "nav_command", "LOC"
         response = f"location is {self.api_node.connection_manager.location}".encode()
         if self.testing:
@@ -47,14 +59,16 @@ class MessageHandler:
         print(message)
         location = message.strip() # Remove parentheses
         location = f"{location}"  # Add parentheses back for proper formatting
-        if not self.testing:
-            for existing_socket, existing_location in self.api_node.called_locations:
-                    if existing_socket == client_socket:
-                        client_socket.sendall(b"User already requested location")
-                        return None
-
-            # Append the client socket and location to the list
-            self.api_node.called_locations.append((client_socket, location))
+        if self.testing:
+            print(f"Received CALLME command from {ip} with location {location}")
+            return None
+        for existing_socket, existing_location in self.api_node.called_locations:
+                if existing_socket == client_socket:
+                    client_socket.sendall(b"User already requested location")
+                    return None
+
+        # Append the client socket and location to the list
+        self.api_node.called_locations.append((client_socket, location))
         print(f"Added {ip} with location {location} to the queue.")
         if self.testing:
             return None
@@ -70,7 +84,6 @@ class MessageHandler:
             print(response.decode())
         else:
             client_socket.sendall(response)
-    
             self.api_node.called_locations = [] # Clear the queue
         return "cmd_vel", (0.0, 0.0)
     
@@ -88,12 +101,13 @@ class MessageHandler:
         response = f"Setting mode to {message}".encode()
         if self.testing:
             print(response.decode())
+            return "nav_command", "SETMODE " + message
         else:
             client_socket.sendall(response)
-        if self.api_node is not None:
+        
             self.api_node.mode = message
             self.api_node.called_locations = [] # Clear the queue
-        return "nav_command", "SETMODE " + message
+            return "nav_command", "SETMODE " + message
     def handle_time_request(self, client_socket, _):
         """Sends the current server time."""
         response = time.ctime().encode()
@@ -137,14 +151,14 @@ class MessageHandler:
         else:
             client_socket.sendall(response)
         
-        print("Processed manual control command:", response_data)
+        # print("Processed manual control command:", response_data)
         return "cmd_vel", response_data
 
     def handle_request_map(self, client_socket, _):
             """Handles the REQMAP command."""
             # Relative path to the JSON file
             working_dir = os.getcwd()
-            graph_file_path =  os.path.abspath(os.path.join(working_dir,"src", "robobin", "robobin", "graphs", "graph_pt2.json"))
+            graph_file_path =  os.path.abspath(os.path.join(working_dir,"src", "robobin", "robobin", "graphs", "anchors.json"))
 
             try:
                 # Load the graph JSON file
@@ -205,15 +219,12 @@ if __name__ == "__main__":
     assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.25))
     assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.25, 0))
     assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.25))
-
+    assert message_handler.handle_message(None, "MANUALCTRL E") == ("cmd_vel", (0.0, 0.0))
     assert message_handler.handle_message(None, "STOP") == ("cmd_vel", (0.0, 0.0))
     assert message_handler.handle_message(None, "CALLME (212.9638, 283.98108)") == None
     assert message_handler.handle_message(None, "UNKNOWN") is None
     assert message_handler.handle_message(None, "REQMAP") is None
     assert message_handler.handle_message(None, "SETMODE MANUAL") == ("nav_command", "SETMODE MANUAL")
- 
-    
- 
-  
     assert message_handler.handle_message(None, "CALLBPM")== ("nav_command", "BPM")
-    assert message_handler.handle_message(None, "LOCATION") == ("nav_command", "LOC")
\ No newline at end of file
+    assert message_handler.handle_message(None, "LOCATION") is ("nav_command", "LOC")
+    assert message_handler.handle_message(None, "CALLMELIDAR 1") == ("nav_command_lidar", 1)
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',
         ],
     },
 )