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', ], }, )