From d1c5fafe937043397151579fd38796bb3b4cab7d Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Thu, 14 Nov 2024 15:49:19 +0000
Subject: [PATCH] Added the base functinality of receieve tcp messages and
process them
---
ros2/src/robobin/robobin/api_node.py | 30 ++++++++-
.../src/robobin/robobin/connection_manager.py | 65 +++++++++++++++++++
ros2/src/robobin/robobin/message_handler.py | 40 ++++++++++++
3 files changed, 132 insertions(+), 3 deletions(-)
create mode 100644 ros2/src/robobin/robobin/connection_manager.py
create mode 100644 ros2/src/robobin/robobin/message_handler.py
diff --git a/ros2/src/robobin/robobin/api_node.py b/ros2/src/robobin/robobin/api_node.py
index 2a718548..df71bb2f 100644
--- a/ros2/src/robobin/robobin/api_node.py
+++ b/ros2/src/robobin/robobin/api_node.py
@@ -1,24 +1,48 @@
# robobin/api_node.py
-
import rclpy
from rclpy.node import Node
+from .message_handler import MessageHandler
+from .connection_manager import ConnectionManager
class ApiNode(Node):
def __init__(self):
super().__init__('api_node')
self.get_logger().info("ApiNode has been started.")
+ # Initialize handlers
+ self.message_handler = MessageHandler(self)
+ self.connection_manager = ConnectionManager(self)
+
+ # Start connection manager
+ self.connection_manager.start()
+
+ def handle_client_connection(self, client_socket):
+ """Handles incoming TCP client connections."""
+ try:
+ while True:
+ data = client_socket.recv(1024).decode()
+ if not data:
+ break
+ command, *args = data.split(" ", 1)
+ self.message_handler.handle_message(client_socket, command, args[0] if args else None)
+ finally:
+ client_socket.close()
+ self.get_logger().info("Client disconnected.")
+
+ def shutdown(self):
+ """Stops the connection manager."""
+ self.connection_manager.stop()
+
def main(args=None):
rclpy.init(args=args)
node = ApiNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
- pass
+ node.shutdown()
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
-
diff --git a/ros2/src/robobin/robobin/connection_manager.py b/ros2/src/robobin/robobin/connection_manager.py
new file mode 100644
index 00000000..64485cbf
--- /dev/null
+++ b/ros2/src/robobin/robobin/connection_manager.py
@@ -0,0 +1,65 @@
+# robobin/connection_manager.py
+import socket
+import threading
+import time
+
+class ConnectionManager:
+ def __init__(self, api_node, udp_ip="255.255.255.255", udp_port=5005, listen_port=5006):
+ self.api_node = api_node
+ self.UDP_IP = udp_ip
+ self.UDP_PORT = udp_port
+ self.LISTEN_PORT = listen_port
+ self.stop_event = threading.Event()
+
+ def start(self):
+ """Starts listening for connections and broadcasting presence."""
+ self.listen_thread = threading.Thread(target=self.listen_for_connections)
+ self.broadcast_thread = threading.Thread(target=self.broadcast_presence)
+ self.listen_thread.start()
+ self.broadcast_thread.start()
+
+ def listen_for_connections(self):
+ """Listens for UDP 'CONNECT' messages and sets up TCP connections."""
+ udp_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ udp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ udp_sock.bind(('', self.LISTEN_PORT))
+
+ while not self.stop_event.is_set():
+ try:
+ data, addr = udp_sock.recvfrom(1024)
+ if data.decode() == "CONNECT":
+ print(f"Connection request from {addr}")
+ tcp_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ tcp_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ tcp_sock.bind(('', self.LISTEN_PORT))
+ tcp_sock.listen(1)
+ client_socket, client_addr = tcp_sock.accept()
+ print(f"Client connected from {client_addr}")
+ threading.Thread(target=self.api_node.handle_client_connection, args=(client_socket,)).start()
+ except socket.timeout:
+ continue
+ except OSError:
+ break
+
+ udp_sock.close()
+ print("Stopped listening for connections.")
+
+ def broadcast_presence(self):
+ """Broadcasts 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():
+ try:
+ sock.sendto(b"ROBOBIN_PRESENT", (self.UDP_IP, self.UDP_PORT))
+ print("Broadcasting presence.")
+ time.sleep(5)
+ except OSError:
+ break
+
+ sock.close()
+ print("Stopped broadcasting presence.")
+
+ def stop(self):
+ """Stops the connection manager."""
+ self.stop_event.set()
diff --git a/ros2/src/robobin/robobin/message_handler.py b/ros2/src/robobin/robobin/message_handler.py
new file mode 100644
index 00000000..46f3ffa3
--- /dev/null
+++ b/ros2/src/robobin/robobin/message_handler.py
@@ -0,0 +1,40 @@
+# robobin/message_handler.py
+
+class MessageHandler:
+ def __init__(self, api_node):
+ self.api_node = api_node
+ self.handlers = {
+ "PING": self.handle_ping,
+ "TIME": self.handle_time_request,
+ "MANUALCTRL": self.handle_manual_control
+ }
+
+ def handle_message(self, client_socket, command, data):
+ """Routes the command to the appropriate handler."""
+ handler = self.handlers.get(command, self.handle_unknown_message)
+ handler(client_socket, data)
+
+ def handle_ping(self, client_socket, _):
+ """Responds with a PONG message."""
+ client_socket.sendall(b"PONG")
+
+ def handle_time_request(self, client_socket, _):
+ """Sends the current server time."""
+ client_socket.sendall(time.ctime().encode())
+
+ def handle_manual_control(self, client_socket, message):
+ """Handles manual control commands: W, A, S, D."""
+ directions = {
+ "W": (1, 0, 0), # Move forward
+ "A": (0, 0, 1), # Turn left
+ "S": (-1, 0, 0), # Move backward
+ "D": (0, 0, -1) # Turn right
+ }
+ response_data = directions.get(message.strip().upper(), (0, 0, 0))
+ response = f"Manual control command received: {response_data}".encode()
+ client_socket.sendall(response)
+ print("Processed manual control command:", response_data)
+
+ def handle_unknown_message(self, client_socket, _):
+ """Handles unknown commands."""
+ client_socket.sendall(b"Unknown command")
--
GitLab