Skip to content
Snippets Groups Projects
Commit a42c1b8e authored by Paul-Winpenny's avatar Paul-Winpenny
Browse files

Merge branch 'app_using_lidar'

parents cadd9c35 f7a5a3ca
No related branches found
No related tags found
No related merge requests found
Showing
with 174 additions and 476 deletions
......@@ -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());
......
App/RobobinApp/Resources/Images/lab_map.jpg

44.7 KiB

......@@ -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" />
......
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),
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[,]
{
{ false } });
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)
......
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()
try
{
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);
}
// Load the embedded image resource
var assembly = typeof(GraphicsDrawable).Assembly;
var resourceStream = assembly.GetManifestResourceStream("Robobin.Resources.Images.lab_map.jpg");
private float CalculateScale(RectF dirtyRect, float minX, float minY, float maxX, float maxY)
if (resourceStream != null)
{
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;
var image = Microsoft.Maui.Graphics.Platform.PlatformImage.FromStream(resourceStream);
canvas.DrawImage(image, dirtyRect.Left, dirtyRect.Top, dirtyRect.Width, dirtyRect.Height);
}
private (float offsetX, float offsetY) CalculateOffsets(RectF dirtyRect, float minX, float minY, float maxX, float maxY, float scale)
else
{
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)
{
int nodeIndex = _graph.Nodes.IndexOf(node);
float startX = node.X * scale + offsetX;
float startY = storedDirtyRect.Height - (node.Y * scale + offsetY); // flip Y
for (int j = 0; j < _graph.Nodes.Count; j++)
{
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);
}
}
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
Debug.WriteLine($"Error loading image: {ex.Message}");
canvas.FillColor = (highlightedNode == node)
? HighlightedNodeColor
: (node.Name == "Home" ? HomeNodeColor : DefaultNodeColor);
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;
// Calculate offsets to center the nodes
float canvasCenterX = storedDirtyRect.Width * 4.5f / 7;
float canvasCenterY = storedDirtyRect.Height / 2;
Node nearestNode = null;
float minDistance = float.MaxValue;
// 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;
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);
// Offset to center the graph
float offsetX = canvasCenterX - graphCenterX;
float offsetY = canvasCenterY - graphCenterY;
float dx = clickX - scaledX;
float dy = flippedClickY - flippedY;
float distance = MathF.Sqrt(dx * dx + dy * dy);
if (distance < minDistance)
foreach (var node in _graph.Nodes)
{
minDistance = distance;
nearestNode = node;
}
}
// Apply centering offsets
float centeredX = node.X + offsetX;
float centeredY = node.Y + offsetY;
return nearestNode;
}
// Set fill color based on whether the node is highlighted
canvas.FillColor = (highlightedNode == node) ? HighlightedNodeColor : DefaultNodeColor;
public void HighlightNode(Node node)
{
_highlightedNode = node;
}
// Draw the node as a circle
canvas.FillCircle(centeredX, centeredY, nodeRadius);
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>();
foreach (var node in _graph.Nodes)
{
gScore[node] = float.MaxValue;
fScore[node] = float.MaxValue;
// Optionally, draw node name or other details
canvas.FontColor = Colors.Black;
canvas.DrawString(node.Name, centeredX + nodeRadius + 2, centeredY - nodeRadius, HorizontalAlignment.Left);
}
gScore[startNode] = 0;
fScore[startNode] = Heuristic(startNode, targetNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.OrderBy(node => fScore[node]).First();
if (currentNode == targetNode)
{
return ReconstructPath(cameFrom, currentNode);
}
openSet.Remove(currentNode);
foreach (var edge in currentNode.Edges)
public Node FindNearestNode(float tapPointX, float tapPointY)
{
Node neighbor = edge.TargetNode;
float tentativeGScore = gScore[currentNode] + edge.Cost;
// Calculate offsets to center the nodes
float canvasCenterX = storedDirtyRect.Width * 4.5f / 7;
float canvasCenterY = storedDirtyRect.Height / 2;
if (tentativeGScore < gScore[neighbor])
{
cameFrom[neighbor] = currentNode;
gScore[neighbor] = tentativeGScore;
fScore[neighbor] = gScore[neighbor] + Heuristic(neighbor, targetNode);
// 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;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
// Offset to center the graph
float offsetX = canvasCenterX - graphCenterX;
float offsetY = canvasCenterY - graphCenterY;
return new List<Node>();
}
float minDistance = float.MaxValue;
Node nearestNode = null;
private float Heuristic(Node a, Node b)
foreach (var node in _graph.Nodes)
{
return MathF.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y));
}
// Apply centering offsets to node positions
float centeredX = node.X + offsetX;
float centeredY = node.Y + offsetY;
private List<Node> ReconstructPath(Dictionary<Node, Node> cameFrom, Node currentNode)
{
var totalPath = new List<Node> { currentNode };
// 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)
);
while (cameFrom.ContainsKey(currentNode))
if (distance < minDistance)
{
currentNode = cameFrom[currentNode];
totalPath.Add(currentNode);
}
totalPath.Reverse();
return totalPath;
minDistance = distance;
nearestNode = node;
}
public Node GetHomeNode()
{
return _graph.Nodes.FirstOrDefault(n => n.Name == "Home");
}
public List<Node> Path
{
get => _path;
set => _path = value;
return nearestNode;
}
private void DrawPath(ICanvas canvas, float scale, float offsetX, float offsetY)
{
if (_path != null && _path.Count > 0)
{
canvas.StrokeColor = PathColor;
canvas.StrokeSize = 3;
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
canvas.DrawLine(startX, startY, endX, endY);
}
}
}
}
}
......@@ -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();
}
if (nearestNode != null)
{
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.");
......
......@@ -34,8 +34,6 @@ 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;
......@@ -43,23 +41,13 @@ namespace RobobinApp.Views
(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");
}
......
......@@ -14,14 +14,6 @@ def generate_launch_description():
),
Node(
package='robobin',
executable='uwb_navigation_node',
name='uwb_navigation_node',
output='screen',
emulate_tty=True
),
Node(
package='robobin',
executable='imu_node',
......@@ -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:
......
......@@ -16,15 +16,8 @@ def generate_launch_description():
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
),
......
......@@ -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" 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')
self.publisher_topics["nav_command"].publish(String(data="CALL " + start_node + " " + end_location))
self.get_logger().info("Already at nearest node.")
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
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:
......
......@@ -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
......
......@@ -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,7 +59,9 @@ class MessageHandler:
print(message)
location = message.strip() # Remove parentheses
location = f"{location}" # Add parentheses back for proper formatting
if not self.testing:
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")
......@@ -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,9 +101,10 @@ 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
......@@ -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)
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()
......@@ -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',
],
},
)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment