Api node now has a few publisher nodes being created, need to actaully

test on a machine

.vs/robobin/v17/DocumentLayout.json

+{
+  "Version": 1,
+  "WorkspaceRootPath": "C:\\Users\\plw1g21\\source\\repos\\robobin\\",
+  "Documents": [],
+  "DocumentGroupContainers": [
+    {
+      "Orientation": 0,
+      "VerticalTabListWidth": 256,
+      "DocumentGroups": [
+        {
+          "DockedWidth": 200,
+          "SelectedChildIndex": -1,
+          "Children": [
+            {
+              "$type": "Bookmark",
+              "Name": "ST:0:0:{cce594b6-0c39-4442-ba28-10c64ac7e89f}"
+            }
+          ]
+        }
+      ]
+    }
+  ]
+}
\ No newline at end of file

App/RobobinApp/RobobinApp.csproj.user

@@ -2,9 +2,9 @@
 <Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
   <PropertyGroup>
     <IsFirstTimeProjectOpen>False</IsFirstTimeProjectOpen>
-    <ActiveDebugFramework>net8.0-windows10.0.19041.0</ActiveDebugFramework>
-    <ActiveDebugProfile>Windows Machine</ActiveDebugProfile>
-    <SelectedPlatformGroup>PhysicalDevice</SelectedPlatformGroup>
+    <ActiveDebugFramework>net8.0-android</ActiveDebugFramework>
+    <ActiveDebugProfile>Android Emulator</ActiveDebugProfile>
+    <SelectedPlatformGroup>Emulator</SelectedPlatformGroup>
     <DefaultDevice>pixel_5_-_api_34</DefaultDevice>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(TargetFramework)|$(Platform)'=='Debug|net8.0-android|AnyCPU'">

ros2/src/robobin/robobin/message_handler.py → ros2/src/robobin/robobin/api_helpers/message_handler.py

 # robobin/message_handler.py
-
+import time
 class MessageHandler:
-    def __init__(self, api_node):
+    def __init__(self, api_node, testing=False):
         self.api_node = api_node
+        self.testing = testing
         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."""
+    def handle_message(self, client_socket, raw_message):
+        """Parses the incoming message and routes the command to the appropriate handler."""
+        command, *args = raw_message.split(" ", 1)
+        data = args[0] if args else None
         handler = self.handlers.get(command, self.handle_unknown_message)
-        handler(client_socket, data)
+        return handler(client_socket, data)
 
     def handle_ping(self, client_socket, _):
         """Responds with a PONG message."""
-        client_socket.sendall(b"PONG")
+        response = b"PONG"
+        if self.testing:
+            print(response.decode())
+        else:
+            client_socket.sendall(response)
+
+        return None
 
     def handle_time_request(self, client_socket, _):
         """Sends the current server time."""
-        client_socket.sendall(time.ctime().encode())
+        response = time.ctime().encode()
+        if self.testing:
+            print(response.decode())
+        else:
+            client_socket.sendall(response)
 
+        return None
+    
     def handle_manual_control(self, client_socket, message):
         """Handles manual control commands: W, A, S, D."""
+        # W: linear.x = 0.5, angular.z = 0
+        # A: linear.x = 0, angular.z = 0.5
+        # S: linear.x = -0.5, angular.z = 0
+        # D: linear.x = 0, angular.z = -0.5
+        
         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
+            "W": (0.5, 0),    # Move forward
+            "A": (0, 0.5),    # Turn left
+            "S": (-0.5, 0),   # Move backward
+            "D": (0, -0.5)    # Turn right
         }
-        response_data = directions.get(message.strip().upper(), (0, 0, 0))
+        response_data = directions.get(message.strip().upper(), (0, 0))
         response = f"Manual control command received: {response_data}".encode()
-        client_socket.sendall(response)
+        if self.testing:
+            print(response.decode())
+        else:
+            client_socket.sendall(response)
         print("Processed manual control command:", response_data)
-
+        return "cmd_vel", response_data
+    
     def handle_unknown_message(self, client_socket, _):
         """Handles unknown commands."""
-        client_socket.sendall(b"Unknown command")
+        response = b"Unknown command"
+        if self.testing:
+            print(response.decode())
+        else:
+            client_socket.sendall(response)
+        return None
+    
+# Test class without api_node and with testing enabled
+if __name__ == "__main__":
+    message_handler = MessageHandler(None, testing=True)
+    assert message_handler.handle_message(None, "PING") is None
+    assert message_handler.handle_message(None, "TIME") is None
+    assert message_handler.handle_message(None, "MANUALCTRL W") == ("cmd_vel", (0.5, 0))
+    assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.5))
+    assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.5, 0))
+    assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.5))
+    assert message_handler.handle_message(None, "UNKNOWN") is None
\ No newline at end of file

ros2/src/robobin/robobin/api_node.py

+
+from api_helpers.message_handler import MessageHandler
+from api_helpers.connection_manager import ConnectionManager
+
+from geometry_msgs.msg import Twist
 # 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.publisher_topics = {
+            "cmd_vel": self.create_publisher(Twist, '/cmd_vel', 10),
+            "nav_send": self.create_publisher(Twist, '/nav_send', 10), 
+            "map": self.create_publisher(Twist, '/map', 10) 
+        }
+        subscriber_topics = {
+            "location": "/location"
+        }
         self.message_handler = MessageHandler(self)
         self.connection_manager = ConnectionManager(self)
 
-        # Start connection manager
         self.connection_manager.start()
 
     def handle_client_connection(self, client_socket):
@@ -23,11 +32,34 @@ class ApiNode(Node):
                 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)
+                topic,message = self.message_handler.handle_message(client_socket, data)
+                if topic is not None:
+                    self.publish_to_topic(topic, message)
         finally:
             client_socket.close()
             self.get_logger().info("Client disconnected.")
+    def publish_to_topic(self, topic, message):
+        """Publishes the message to the specified topic."""
+        if topic in self.publisher_topics:
+            publisher = self.publisher_topics[topic]
+
+            # Check if the topic is 'cmd_vel' and format the message as a Twist message
+            if topic == "cmd_vel" and isinstance(message, tuple):
+                linear_x, angular_z = message
+                twist_msg = Twist()
+                twist_msg.linear.x = linear_x
+                twist_msg.linear.y = 0.0
+                twist_msg.linear.z = 0.0
+                twist_msg.angular.x = 0.0
+                twist_msg.angular.y = 0.0
+                twist_msg.angular.z = angular_z
+
+                publisher.publish(twist_msg)
+                self.get_logger().info(f"Published to {topic}: linear_x={linear_x}, angular_z={angular_z}")
+            else:
+                self.get_logger().warning(f"Unhandled message type for topic: {topic}")
+        else:
+            self.get_logger().warning(f"Unknown topic: {topic}")
 
     def shutdown(self):
         """Stops the connection manager."""