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