1import socket
2import time
3import shlex
4
5# Connect the socket
6s = socket.create_connection(("localhost", 29202))
7s.settimeout(5)
8
9# Create the file-like object
10f = s.makefile("rw")
11
12# Read the state
13f.writelines(["STATE\n"])
14f.flush()
15state_str = f.readline().strip()
16state_str_p = shlex.split(state_str)
17assert state_str_p[0] != "ERROR"
18print(f"Robot position: {float(state_str_p[1])}, {float(state_str_p[2])}")
19print(f"Arm position: {float(state_str_p[3])}, {float(state_str_p[4])}, {float(state_str_p[5])}")
20
21# Teleport the robot
22f.writelines(["TELEPORT 100 -200\n"])
23f.flush()
24assert f.readline().strip() == "OK"
25
26# Drive the robot
27f.writelines(["DRIVE 500 -200\n"])
28f.flush()
29assert f.readline().strip() == "OK"
30
31time.sleep(1)
32
33# Stop the robot
34f.writelines(["DRIVE 0 0\n"])
35f.flush()
36assert f.readline().strip() == "OK"
37
38# Set the arm position
39f.writelines(["SETARM 100 -30 -70\n"])
40f.flush()
41assert f.readline().strip() == "OK"
42
43# Drive the arm
44f.writelines(["DRIVEARM 10 -30 -15\n"])
45f.flush()
46assert f.readline().strip() == "OK"
47
48time.sleep(1)
49
50# Stop the arm
51f.writelines(["DRIVEARM 0 0 0\n"])
52f.flush()
53assert f.readline().strip() == "OK"
54
55# Read the color
56
57f.writelines(["COLORGET\n"])
58f.flush()
59color_str = f.readline().strip()
60color_str_p = shlex.split(color_str)
61assert color_str_p[0] != "ERROR"
62print(f"Color: {float(color_str_p[1])}, {float(color_str_p[2])}, {float(color_str_p[3])}")
63
64# Set the color
65f.writelines(["COLORSET 1 0 0\n"])
66f.flush()
67assert f.readline().strip() == "OK"
68
69time.sleep(1)
70
71# Reset the color
72f.writelines(["COLORSET 0.929 0.49 0.192\n"])
73f.flush()
74assert f.readline().strip() == "OK"
75
76# Say hello
77f.writelines(["SAY \"Hello World From Socket!\"\n"])
78f.flush()
79assert f.readline().strip() == "OK"
80
81# Read any messages
82while True:
83 f.writelines(["MESSAGE\n"])
84 f.flush()
85 msg_res_str = f.readline().strip()
86 msg_res_str_p = shlex.split(msg_res_str)
87 assert msg_res_str_p[0] != "ERROR"
88 if msg_res_str_p[0] == "NOMESSAGE":
89 break
90 print(f"New message: {msg_res_str_p[1]}")