tools: Replace animator with Python.

We used to have:

pybricks-micropython --> [pipe] --> python --> [webserver] --> react app

This simplifies it to

pybricks-micropython --> [socket] --> python

The animator can stay open at all times for easy pybricks-micropython debugging.

Author: laurensvalk

.vscode/launch.json

@@ -65,8 +65,8 @@
             "cwd": "${workspaceFolder}",
             "environment": [
                 {
-                    "name": "PBIO_TEST_DATA_PARSER",
-                    "value": "./tools/virtual-hub-animator/data_server.py"
+                    "name": "PBIO_TEST_CONNECT_SOCKET",
+                    "value": "true"
                 },
             ],
             "externalConsole": false,
@@ -85,6 +85,13 @@
             ],
             "preLaunchTask": "build virtualhub"
         },
+        {
+            "name":"Virtual Hub Animation",
+            "type":"debugpy",
+            "request":"launch",
+            "program":"${workspaceFolder}/lib/pbio/platform/virtual_hub/animation.py",
+            "console":"integratedTerminal"
+        },
         {
             "name": "test-pbio",
             "type": "cppdbg",

lib/pbio/drv/motor_driver/motor_driver_virtual_simulation.c

@@ -5,11 +5,14 @@
 
 #if PBDRV_CONFIG_MOTOR_DRIVER_VIRTUAL_SIMULATION
 
+#include <arpa/inet.h>
 #include <stdint.h>
-#include <stdlib.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
 #include <sys/wait.h>
 #include <unistd.h>
+#include <unistd.h>
 
 #include <contiki.h>
 
@@ -145,8 +148,35 @@ pbio_error_t pbdrv_motor_driver_set_duty_cycle(pbdrv_motor_driver_dev_t *driver,
     return PBIO_SUCCESS;
 }
 
-static pid_t data_parser_pid;
-static FILE *data_parser_in;
+static int data_socket = -1;
+
+static void animation_socket_connect(void) {
+
+    struct sockaddr_in serv_addr;
+    data_socket = socket(AF_INET, SOCK_STREAM, 0);
+    if (data_socket < 0) {
+        perror("socket() failed");
+        return;
+    }
+
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_port = htons(5002);
+    if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) {
+        perror("inet_pton() failed");
+        close(data_socket);
+        data_socket = -1;
+        return;
+    }
+
+    if (connect(data_socket, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
+        perror("connect() failed. Animation not running?");
+        close(data_socket);
+        data_socket = -1;
+        return;
+    }
+
+    printf("Connected to animation socket.\n");
+}
 
 PROCESS(pbdrv_motor_driver_virtual_simulation_process, "pbdrv_motor_driver_virtual_simulation");
 
@@ -166,27 +196,29 @@ PROCESS_THREAD(pbdrv_motor_driver_virtual_simulation_process, ev, data) {
         PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&tick_timer));
 
         // If data parser pipe is connected, output the motor angles.
-        if (data_parser_in && timer_expired(&frame_timer)) {
+        if (data_socket >= 0 && timer_expired(&frame_timer)) {
             timer_reset(&frame_timer);
-
+            char buf[PBDRV_CONFIG_MOTOR_DRIVER_NUM_DEV * 20];
+            size_t len = 0;
             // Output motor angles on one line.
             for (dev_index = 0; dev_index < PBDRV_CONFIG_MOTOR_DRIVER_NUM_DEV; dev_index++) {
                 driver = &motor_driver_devs[dev_index];
-                fprintf(data_parser_in, "%d ", ((int32_t)(driver->angle / 1000)));
-            }
-            fprintf(data_parser_in, "\r\n");
-
-            // Check that process is still running.
-            pid_t p = waitpid(data_parser_pid, NULL, WNOHANG);
-            if (p == -1 || p == data_parser_pid) {
-                fclose(data_parser_in);
-                printf("Process failed or ended.");
-                exit(1);
+
+                // Append each motor angle to the buffer
+                len += snprintf(buf + len, sizeof(buf) - len, "%d ", (int)(driver->angle / 1000));
             }
-            if (fflush(data_parser_in) == -1) {
-                printf("Flush failed.");
-                fclose(data_parser_in);
-                exit(1);
+
+            // Replace last space with newline
+            buf[len++] = '\r';
+            buf[len++] = '\n';
+            buf[len] = '\0';
+
+            // Send the constructed message
+            ssize_t sent = send(data_socket, buf, len, MSG_NOSIGNAL);
+            if (sent == -1) {
+                perror("send() failed");
+                close(data_socket);
+                data_socket = -1;
             }
         }
 
@@ -252,51 +284,6 @@ PROCESS_THREAD(pbdrv_motor_driver_virtual_simulation_process, ev, data) {
     PROCESS_END();
 }
 
-// Optionally starts script that receives motor angles through a pipe
-// for visualization and debugging purposes.
-static void pbdrv_motor_driver_virtual_simulation_prepare_parser(void) {
-
-    const char *data_parser_cmd = getenv("PBIO_TEST_DATA_PARSER");
-
-    // Skip if no data parser is given.
-    if (!data_parser_cmd) {
-        return;
-    }
-
-    // Create the pipe to parser script.
-    int data_parser_stdin[2];
-    if (pipe(data_parser_stdin) == -1) {
-        printf("pipe(data_parser_in) failed\n");
-        return;
-    }
-
-    // For the process to run the parser in parallel.
-    data_parser_pid = fork();
-    if (data_parser_pid == -1) {
-        printf("fork() failed");
-        return;
-    }
-
-    // The child process executes the data parser.
-    if (data_parser_pid == 0) {
-        dup2(data_parser_stdin[0], STDIN_FILENO);
-        close(data_parser_stdin[1]);
-        char *args[] = {NULL, NULL};
-        if (execvp(data_parser_cmd, args) == -1) {
-            printf("Failed to start data parser.");
-        }
-        exit(EXIT_FAILURE);
-    }
-
-    // Get file streams for pipes to data parser.
-    close(data_parser_stdin[0]);
-    data_parser_in = fdopen(data_parser_stdin[1], "w");
-    if (data_parser_in == NULL) {
-        printf("fdopen(data_parser_stdin[1], \"w\") failed");
-        exit(1);
-    }
-}
-
 static bool simulation_enabled = true;
 
 void pbdrv_motor_driver_disable_process(void) {
@@ -334,8 +321,11 @@ void pbdrv_motor_driver_init(void) {
         }
     }
 
+    // Skip if no data parser is given.
+    if (getenv("PBIO_TEST_CONNECT_SOCKET")) {
+        animation_socket_connect();
+    }
 
-    pbdrv_motor_driver_virtual_simulation_prepare_parser();
     if (simulation_enabled) {
         process_start(&pbdrv_motor_driver_virtual_simulation_process);
     }

lib/pbio/platform/virtual_hub/animation.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python3
+import pygame
+import socket
+import threading
+from pathlib import Path
+
+
+HOST = "127.0.0.1"
+PORT = 5002
+SCREEN_WIDTH = 1400
+SCREEN_HEIGHT = 1000
+FPS = 25
+
+# Initialize shared state
+angles = [0, 0, 0, 0, 0, 0]
+
+
+# Handles socket communication. Accepts new connection when client disconnects.
+# This means it can stay open at all times even when restarting pybricks-micropython
+# for fast debugging.
+def socket_listener_thread():
+    global angles
+    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+        s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        s.bind((HOST, PORT))
+        s.listen(1)
+        print(f"Listening on {HOST}:{PORT}")
+        while True:
+            conn, addr = s.accept()
+            print(f"Client connected: {addr}")
+            with conn:
+                buffer = b""
+                while True:
+                    data = conn.recv(1024)
+                    if not data:
+                        print("Client disconnected.")
+                        break
+                    buffer += data
+                    while b"\n" in buffer:
+                        line, buffer = buffer.split(b"\n", 1)
+                        try:
+                            angles = list(map(int, line.decode().strip().split()))
+                        except ValueError:
+                            print("Invalid data received:", line.decode().strip())
+
+
+def blit_rotate_at_center(screen, image, position, angle):
+    """Rotate image around a specific center point"""
+    rotated_image = pygame.transform.rotate(image, angle)
+    rotated_rect = rotated_image.get_rect(center=position)
+    screen.blit(rotated_image, rotated_rect)
+
+
+# Define the base path for images
+img_path = Path("lib/pbio/test/animator/img")
+
+
+def load_and_scale_image(filename, scale=0.25):
+    """Load an image and scale it by the given scale factor."""
+    image = pygame.image.load(img_path / filename)
+    original_size = image.get_size()
+    scaled_size = (int(original_size[0] * scale), int(original_size[1] * scale))
+    return pygame.transform.smoothscale(image, scaled_size)
+
+
+# Main Pygame function
+def main():
+    global angles
+
+    # Initialize Pygame
+    pygame.init()
+    screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
+    pygame.display.set_caption("Virtual Hub Animator")
+    clock = pygame.time.Clock()
+
+    # Load images
+    hub = load_and_scale_image("main-model.png")
+    wheel_left = load_and_scale_image("wheel-left.png")
+    wheel_right = load_and_scale_image("wheel-right.png")
+    shaft = load_and_scale_image("shaft.png")
+    stall = load_and_scale_image("stall.png")
+    gear_drive = load_and_scale_image("gear-drive.png")
+    gear_follow = load_and_scale_image("gear-follow.png")
+
+    # Start the socket listener thread
+    threading.Thread(target=socket_listener_thread, daemon=True).start()
+
+    running = True
+    while running:
+        for event in pygame.event.get():
+            if event.type == pygame.QUIT:
+                running = False
+
+        # Clear screen.
+        screen.fill((161, 168, 175))
+
+        # Draw and rotate components, with left wheel behind hub.
+        blit_rotate_at_center(screen, wheel_left, (1242, 156), -angles[0])
+        screen.blit(hub, (0, 0))
+        blit_rotate_at_center(screen, wheel_right, (1067, 331), angles[1])
+        blit_rotate_at_center(screen, shaft, (247, 712), angles[4])
+        blit_rotate_at_center(screen, stall, (245, 189), angles[2])
+        blit_rotate_at_center(screen, gear_drive, (943, 694), -angles[5])
+        blit_rotate_at_center(screen, gear_follow, (1037, 694), angles[5] / 3)
+
+        # Update display
+        pygame.display.flip()
+        clock.tick(FPS)
+
+    pygame.quit()
+
+
+if __name__ == "__main__":
+    main()

lib/pbio/test/animator/data_parser.py

@@ -4,19 +4,38 @@
 # driver outputs it at intervals of 40 ms (25 fps).
 
 from collections import namedtuple
+import socket
 
-# Get live output from process.
-with open("output.txt", "w") as f:
-    while True:
-        try:
-            data = input()
-        except EOFError:
-            break
-        f.write(data)
+HOST = "127.0.0.1"
+PORT = 5002
+
+angles = []
+
+print(f"Listening on {HOST}:{PORT}")
+
+
+# Wait for a connection once and writes results when simulator disconnects.
+with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+    s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    s.bind((HOST, PORT))
+    s.listen(1)
+    conn, addr = s.accept()
+    print(f"Client connected: {addr}")
+    with conn:
+        # receive until client disconnects
+        buffer = b""
+        while True:
+            data = conn.recv(1024)
+            if not data:
+                print("Client disconnected.")
+                break
+            buffer += data
+            while b"\n" in buffer:
+                line, buffer = buffer.split(b"\n", 1)
+                values = list(map(int, line.decode().strip().split()))
+                print("Angles:", values)
+                angles.append(values)
 
-# Load angle data from file
-with open("output.txt", "r") as f:
-    angles = [line.split(" ") for line in f]
 
 if len(angles) < 1:
     exit()
@@ -36,7 +55,7 @@
 )
 
 # Write the CSS component with frames.
-with open("../results/frames.css", "w") as frame_file:
+with open("lib/pbio/test/results/frames.css", "w") as frame_file:
     for info in FRAME_INFO:
         # CSS rows for each frame.
         frames = "".join(

pyproject.toml

@@ -10,7 +10,6 @@ package-mode = false
 python = ">=3.10,<4.0"
 freetype-py = "^2.5.1"
 
-
 [tool.poetry.group.dev.dependencies]
 eventlet = "^0.40.3"
 flake8 = "^3.8.3"