From fce8b2296defd22bb812c6c4bab3b64bc27d60d8 Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Fri, 8 Nov 2024 16:50:09 +0000
Subject: [PATCH] Beginning to handle the loading map data from the robobins
side.
---
App/RobobinApp/Networking/WifiManager.cs | 29 ++-
.../RPi_Wifi/Layouts/Lab1.json | 37 +++
.../RPi_Wifi/WifiControllerRPi.py | 232 +++++++++++++-----
Wireless_Communication/RPi_Wifi/config.json | 3 +
4 files changed, 223 insertions(+), 78 deletions(-)
create mode 100644 Wireless_Communication/RPi_Wifi/Layouts/Lab1.json
create mode 100644 Wireless_Communication/RPi_Wifi/config.json
diff --git a/App/RobobinApp/Networking/WifiManager.cs b/App/RobobinApp/Networking/WifiManager.cs
index b7a43813..fd820a13 100644
--- a/App/RobobinApp/Networking/WifiManager.cs
+++ b/App/RobobinApp/Networking/WifiManager.cs
@@ -15,6 +15,7 @@ namespace RobobinApp.Networking
private bool _isConnected = false;
private CancellationTokenSource _cancellationTokenSource;
private TcpClient _tcpClient;
+ public string Location { get; private set; } = "Unknown";
// Event to notify the UI or other parts of the app of specific messages
public event Action<string> OnMessageReceived;
@@ -29,11 +30,7 @@ namespace RobobinApp.Networking
{
while (true) // Continuous listening loop with reconnection attempts
{
- if (_isConnected)
- {
- Debug.WriteLine("Already connected. Stopping further listening.");
- break;
- }
+
try
{
@@ -41,10 +38,19 @@ namespace RobobinApp.Networking
var result = await _udpClient.ReceiveAsync();
string message = Encoding.ASCII.GetString(result.Buffer);
- if (message == "ROBOBIN_PRESENT")
+ if (message.StartsWith("ROBOBIN_PRESENT"))
{
- Debug.WriteLine("Detected Robobin presence from: " + result.RemoteEndPoint);
- SendConnectMessage(result.RemoteEndPoint.Address.ToString());
+ var parts = message.Split(new[] { '(', ',', ')' }, StringSplitOptions.RemoveEmptyEntries);
+ if (parts.Length == 3)
+ {
+ Location = $"({parts[1].Trim()}, {parts[2].Trim()})";
+ Debug.WriteLine($"Detected Robobin presence at location: {Location} from: {result.RemoteEndPoint}");
+ SendConnectMessage(result.RemoteEndPoint.Address.ToString());
+ }
+ else
+ {
+ Debug.WriteLine("Invalid message format received.");
+ }
}
}
catch (ObjectDisposedException)
@@ -127,12 +133,11 @@ namespace RobobinApp.Networking
catch (Exception ex)
{
Debug.WriteLine($"TCP connection failed: {ex.Message}");
- _isConnected = false; // Reset connection status
- await StartListening(); // Retry listening for presence broadcast
+ _isConnected = false;
+ await StartListening();
}
- // If TCP connection is lost, attempt to reconnect
- _cancellationTokenSource.Cancel(); // Stop the current listening task
+ _cancellationTokenSource.Cancel();
}
private async Task ReceiveMessages()
diff --git a/Wireless_Communication/RPi_Wifi/Layouts/Lab1.json b/Wireless_Communication/RPi_Wifi/Layouts/Lab1.json
new file mode 100644
index 00000000..6442655a
--- /dev/null
+++ b/Wireless_Communication/RPi_Wifi/Layouts/Lab1.json
@@ -0,0 +1,37 @@
+{
+ "name": "Lab 1",
+ "nodes": [
+ {"label": "A", "x": 50, "y": 50},
+ {"label": "B", "x": 150, "y": 50},
+ {"label": "C", "x": 250, "y": 50},
+ {"label": "Home", "x": 350, "y": 50},
+ {"label": "E", "x": 50, "y": 150},
+ {"label": "F", "x": 150, "y": 150},
+ {"label": "G", "x": 250, "y": 150},
+ {"label": "H", "x": 350, "y": 150},
+ {"label": "I", "x": 50, "y": 250},
+ {"label": "J", "x": 150, "y": 250},
+ {"label": "K", "x": 250, "y": 250},
+ {"label": "L", "x": 350, "y": 250},
+ {"label": "M", "x": 50, "y": 350},
+ {"label": "N", "x": 150, "y": 350},
+ {"label": "O", "x": 350, "y": 350}
+ ],
+ "adjacency_matrix": [
+ [false, true, false, false, true, false, false, false, false, false, false, false, false, false, false],
+ [true, false, true, false, false, true, false, false, false, false, false, false, false, false, false],
+ [false, true, false, true, false, false, true, false, false, false, false, false, false, false, false],
+ [false, false, true, false, false, false, false, true, false, false, false, false, false, false, true],
+ [true, false, false, false, false, true, false, false, true, false, false, false, false, false, false],
+ [false, true, false, false, true, false, true, false, false, true, false, false, false, false, false],
+ [false, false, true, false, false, true, false, true, false, false, true, false, false, false, false],
+ [false, false, false, true, false, false, true, false, false, false, false, true, false, false, false],
+ [false, false, false, false, true, false, false, false, false, true, false, false, true, false, false],
+ [false, false, false, false, false, true, false, false, true, false, true, false, false, true, false],
+ [false, false, false, false, false, false, true, false, false, true, false, true, false, false, false],
+ [false, false, false, false, false, false, false, true, false, false, true, false, false, false, true],
+ [false, false, false, false, false, false, false, false, true, false, false, false, false, true, false],
+ [false, false, false, false, false, false, false, false, false, true, false, false, true, false, true],
+ [false, false, false, true, false, false, false, false, false, false, false, true, false, true, false]
+ ]
+}
diff --git a/Wireless_Communication/RPi_Wifi/WifiControllerRPi.py b/Wireless_Communication/RPi_Wifi/WifiControllerRPi.py
index 40bc7121..0fd7b315 100644
--- a/Wireless_Communication/RPi_Wifi/WifiControllerRPi.py
+++ b/Wireless_Communication/RPi_Wifi/WifiControllerRPi.py
@@ -1,72 +1,168 @@
-import queue
+import os
+import json
import socket
import threading
+import queue
import time
class RoboBinConnectionHandler:
- def __init__(self, udp_ip="255.255.255.255", udp_port=5005, listen_port=5006):
- self.queue = queue.Queue(10)
+ def __init__(self, udp_ip="255.255.255.255", udp_port=5005, listen_port=5006, layouts_folder="Layouts", config_file="config.json"):
+ self.command_queue = queue.Queue(10)
self.UDP_IP = udp_ip
self.UDP_PORT = udp_port
self.LISTEN_PORT = listen_port
- self.stop_event = threading.Event()
+ self.stop_event = threading.Event()
+ self.layouts_folder = layouts_folder
+ self.config_file = config_file
+ self.default_layout = "lab1"
+ self.current_location = (0,0)
+ # Load the configuration on startup
+ config = self.load_config()
+ self.current_layout = config.get("current_layout", self.default_layout) #NAME OF LAYOUT FILE
+
+ # Load and print nodes from the current layout file
+ self.current_nodes = self.load_layout(self.current_layout)
+
+
+
+
+ # Command handlers
self.message_handlers = {
"PING": self.handle_ping,
"TIME": self.handle_time_request,
- "CALLOVER" : self.handle_call_over,
- "CUSTOM": self.handle_custom_message,
+ "CALLOVER": self.handle_call_over,
+ "CUSTOM": self.handle_custom_message,
+ "REQUEST_MAP": self.handle_request_map,
+ "SET_LAYOUT": self.handle_set_layout
}
- self.udp_sock = None
- self.tcp_socket = None #
+
+ self.udp_sock = None
+ self.tcp_socket = None
+ print("RoboBinConnectionHandler initialized.")
+ print("Current layout: {}".format(self.current_layout))
- def broadcast_presence(self):
- sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
- sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+ def load_config(self):
+ """
+ Loads the configuration from the config file.
+ If the file does not exist, it creates a default configuration.
+ """
+ if os.path.exists(self.config_file):
+ with open(self.config_file, "r") as file:
+ config = json.load(file)
+ print(f"Loaded configuration: {config}")
+
+ config_nodes = config.get("nodes", [])
+ for node in config_nodes:
+ print(f"Node: {node}")
+ return config
+ else:
+ config = {"current_layout": self.default_layout}
+ self.save_config(config)
+ print("No config file found. Created default configuration.")
+ return config
- while not self.stop_event.is_set():
- message = b"ROBOBIN_PRESENT"
- try:
- sock.sendto(message, (self.UDP_IP, self.UDP_PORT))
- print("Broadcasting: {}".format(message.decode()))
- except OSError as e:
- print(f"Broadcast error: {e}")
- break
- time.sleep(5)
+ def save_config(self, config):
+ """
+ Saves the configuration to the config file.
+ """
+ with open(self.config_file, "w") as file:
+ json.dump(config, file, indent=4)
+ print(f"Configuration saved: {config}")
- sock.close()
- print("Broadcasting stopped.")
+ def handle_set_layout(self, client_socket, new_layout):
+ """
+ Updates the current layout in the configuration and persists it.
+ """
+ self.current_layout = new_layout
+ # Update config file with the new current_layout
+ config = {"current_layout": self.current_layout}
+ self.save_config(config)
+ response = f"Current layout set to '{self.current_layout}'.".encode()
+ client_socket.sendall(response)
+ print(f"Current layout updated to '{self.current_layout}' and saved to config.")
+
+ def load_layout(self, layout_name):
+ """
+ Loads the specified layout file and prints all nodes in it.
+ """
+ layout_file = os.path.join(self.layouts_folder, f"{layout_name}.json")
+ if os.path.exists(layout_file):
+ with open(layout_file, "r") as file:
+ layout_data = json.load(file)
+ print(f"Loaded layout '{layout_name}':")
+
+ nodes = layout_data.get("nodes", [])
+ for node in nodes:
+ print(f"Node: {node['label']}, Position: ({node['x']}, {node['y']})")
+
+ return layout_data
+ else:
+ print(f"Layout file '{layout_file}' not found.")
+ return None
+
+ def load_current_map(self):
+ """
+ Loads the current map JSON data based on the current_layout attribute.
+ If the specified layout is not found, falls back to the default layout.
+ """
+ # Attempt to load the specified layout
+ map_data = self.load_layout(self.current_layout)
+ if map_data:
+ print(f"Loaded map '{self.current_layout}' successfully.")
+ return map_data
- def handle_ping(self, client_socket):
- print("Received PING from client.")
+ # Attempt to load the default layout if the specified layout is missing
+ map_data = self.load_layout(self.default_layout)
+ if map_data:
+ print(f"Specified layout not found. Loaded default layout '{self.default_layout}'.")
+ return map_data
+
+ # Both the specified and default layouts are missing
+ print("Error: Neither specified nor default layout found.")
+ return None
+
+ def handle_request_map(self, client_socket, _):
+ """
+ Sends the current map layout (nodes and adjacency matrix) to the client.
+ """
+ map_data = self.load_current_map()
+ if map_data:
+ map_json = json.dumps(map_data).encode()
+ client_socket.sendall(map_json)
+ print(f"Sent map data for layout '{self.current_layout}' to client.")
+ else:
+ error_message = b"Map not found."
+ client_socket.sendall(error_message)
+ print("Error: Map data not found, sent error message to client.")
+
+ # Other command handlers
+ def handle_ping(self, client_socket, _):
response = b"PONG"
- print("Sending PONG to client.")
client_socket.sendall(response)
- def handle_time_request(self, client_socket):
+ def handle_time_request(self, client_socket, _):
current_time = time.ctime().encode()
- print(f"Sending current time: {current_time.decode()}")
client_socket.sendall(current_time)
def handle_custom_message(self, client_socket, message):
response = f"Received custom message: {message}".encode()
- print(f"Custom handler response: {response.decode()}")
client_socket.sendall(response)
def handle_call_over(self, client_socket, message):
- response = f"User has requested node: {message}".encode()
- print(f"Call over handler response: {response.decode()}")
+ node = message
+ response = f"User has requested node: {node}".encode()
+ #TODO: Node should be added to queue, respond to user with queue position
client_socket.sendall(response)
- def handle_unknown_message(self, client_socket):
+ def handle_unknown_message(self, client_socket, _):
response = b"I don't know this message."
- print("Sending response to unknown message.")
client_socket.sendall(response)
def handle_client_connection(self, client_socket):
try:
while not self.stop_event.is_set():
- client_socket.settimeout(1)
+ client_socket.settimeout(1)
try:
request = client_socket.recv(1024)
except socket.timeout:
@@ -82,16 +178,10 @@ class RoboBinConnectionHandler:
print("Received from client: {}".format(message))
parts = message.split(" ", 1)
- message_type = parts[0]
- message_data = parts[1] if len(parts) > 1 else None
-
- if message_type in self.message_handlers:
- if message_data:
- self.message_handlers[message_type](client_socket, message_data)
- else:
- self.message_handlers[message_type](client_socket)
- else:
- self.handle_unknown_message(client_socket)
+ command = parts[0]
+ data = parts[1] if len(parts) > 1 else None
+ handler = self.message_handlers.get(command, self.handle_unknown_message)
+ handler(client_socket, data)
except ConnectionResetError:
print("Client connection was forcibly closed.")
@@ -108,14 +198,13 @@ class RoboBinConnectionHandler:
while not self.stop_event.is_set():
try:
- self.udp_sock.settimeout(1) # Timeout for blocking recvfrom call
+ self.udp_sock.settimeout(1)
data, addr = self.udp_sock.recvfrom(1024)
if data.decode() == "CONNECT":
print("Received connection request from {}".format(addr))
-
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.tcp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
- self.tcp_socket.bind(('', self.LISTEN_PORT))
+ self.tcp_socket.bind(('', self.LISTEN_PORT))
self.tcp_socket.listen(1)
print("Listening for TCP connection...")
@@ -125,18 +214,37 @@ class RoboBinConnectionHandler:
threading.Thread(target=self.handle_client_connection, args=(client_socket,)).start()
except socket.timeout:
continue
- except OSError as e:
- print(f"UDP socket no longer open (likely due to stop event):" )
+ except OSError:
+ print("UDP socket no longer open (likely due to stop event).")
break
except Exception as e:
print(f"An error occurred while listening for connections: {e}")
- # Close sockets if stop event is set
if self.udp_sock:
self.udp_sock.close()
if self.tcp_socket:
self.tcp_socket.close()
print("Listening stopped.")
+ def broadcast_presence(self):
+ """
+ Broadcasts RoboBin's presence periodically over UDP.
+ """
+ sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+
+ while not self.stop_event.is_set():
+ #add location to back of message
+ message = b"ROBOBIN_PRESENT " + str(self.current_location).encode()
+ try:
+ sock.sendto(message, (self.UDP_IP, self.UDP_PORT))
+ print("Broadcasting: {}".format(message.decode()))
+ except OSError as e:
+ print(f"Broadcast error: {e}")
+ break
+ time.sleep(5)
+
+ sock.close()
+ print("Broadcasting stopped.")
def start(self):
self.broadcast_thread = threading.Thread(target=self.broadcast_presence)
@@ -146,29 +254,21 @@ class RoboBinConnectionHandler:
self.listen_thread.start()
def stop(self):
- print("Stopping server...")
self.stop_event.set()
- # Safely close the sockets to prevent further operations on closed sockets
if self.udp_sock:
self.udp_sock.close()
if self.tcp_socket:
self.tcp_socket.close()
self.broadcast_thread.join()
self.listen_thread.join()
- print("Server stopped.")
-# Instantiate and start the handler
if __name__ == "__main__":
- robobin_handler = RoboBinConnectionHandler()
- robobin_handler.start()
- print("Server started. Type 'stop' to shut down.")
-
- # Main loop to accept CLI input
- while True:
- command = input("Enter command: ")
- if command.strip().lower() == "stop":
- robobin_handler.stop()
- elif command.strip().lower() == "status":
- print("Server is running.")
- #printstatus() # Funciton will be added later
- break
+ handler = RoboBinConnectionHandler()
+ handler.start()
+ try:
+ while True:
+ time.sleep(1)
+ except KeyboardInterrupt:
+ print("Stopping...")
+ handler.stop()
+ print("Stopped.")
\ No newline at end of file
diff --git a/Wireless_Communication/RPi_Wifi/config.json b/Wireless_Communication/RPi_Wifi/config.json
new file mode 100644
index 00000000..f397d68b
--- /dev/null
+++ b/Wireless_Communication/RPi_Wifi/config.json
@@ -0,0 +1,3 @@
+{
+ "current_layout": "lab1"
+}
--
GitLab