Skip to content

Commit c089e5b

Browse files
stedwickstedwick
committed
format black
1 parent af4eec1 commit c089e5b

File tree

2 files changed

+152
-118
lines changed

2 files changed

+152
-118
lines changed

‎client_win-mac-nix/main.py

Lines changed: 83 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -2,48 +2,47 @@
22
import logging
33
from time import time, ctime
44
from dataclasses import dataclass
5-
import ctypes # for windows mouse
6-
import socket # udp networking
7-
import struct # binary unpacking
5+
import ctypes # for windows mouse
6+
import socket # udp networking
7+
import struct # binary unpacking
88

99
print("\n\nCLIENT: Starting PhilNav\n")
1010

1111
# parse command line arguments
1212
parser = argparse.ArgumentParser()
13-
parser.add_argument( '-v',
14-
'--verbose',
15-
action='store_true',
16-
help='provide verbose logging' )
17-
parser.add_argument( '-H',
18-
'--host',
19-
type=str,
20-
default="0.0.0.0",
21-
help='bind to ip address, default 0.0.0.0' )
22-
parser.add_argument( '-p',
23-
'--port',
24-
type=int,
25-
default=4245,
26-
help='bind to port, default 4245' )
27-
parser.add_argument( '-s',
28-
'--speed',
29-
type=int,
30-
default=30,
31-
help='mouse speed, default 30' )
13+
parser.add_argument(
14+
"-v", "--verbose", action="store_true", help="provide verbose logging"
15+
)
16+
parser.add_argument(
17+
"-H",
18+
"--host",
19+
type=str,
20+
default="0.0.0.0",
21+
help="bind to ip address, default 0.0.0.0",
22+
)
23+
parser.add_argument(
24+
"-p", "--port", type=int, default=4245, help="bind to port, default 4245"
25+
)
26+
parser.add_argument(
27+
"-s", "--speed", type=int, default=30, help="mouse speed, default 30"
28+
)
3229
args = parser.parse_args()
3330

3431
if args.verbose:
35-
logging.getLogger().setLevel(logging.DEBUG)
36-
logging.info(" Logging verbosely\n")
32+
logging.getLogger().setLevel(logging.DEBUG)
33+
logging.info(" Logging verbosely\n")
34+
3735

3836
# returned from ctypes.windll.user32.GetCursorPos
3937
# simple point.x, point.y
4038
class POINT(ctypes.Structure):
41-
_fields_ = [("x", ctypes.c_long), ("y", ctypes.c_long)]
39+
_fields_ = [("x", ctypes.c_long), ("y", ctypes.c_long)]
40+
4241

4342
# initialize networking
44-
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Read datagrams over UDP
45-
sock.settimeout(1) # Without a timeout, this script will "hang" if nothing is received
46-
sock.bind((args.host, args.port)) # Register our socket
43+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Read datagrams over UDP
44+
sock.settimeout(1) # Without a timeout, this script will "hang" if nothing is received
45+
sock.bind((args.host, args.port)) # Register our socket
4746

4847
# How to get local IP address?
4948
# Doesn't work for me: socket.gethostbyname(socket.gethostname())
@@ -52,64 +51,74 @@ class POINT(ctypes.Structure):
5251
# This is not your public Internet address. This is your local area network address.
5352
# On Windows, make sure it's set to a private network to allow discovery.
5453
# You'd think public would allow discovery, but when you are *in public* - like at a coffee shop - you don't want strangers to access your PC.
55-
text_listening = f"Listening on {sock.getsockname()} for mouse data from Raspberry Pi server..."
54+
text_listening = (
55+
f"Listening on {sock.getsockname()} for mouse data from Raspberry Pi server..."
56+
)
5657
print(ctime() + " - " + text_listening)
5758
print("\nPress Ctrl-C to exit\n")
5859

60+
5961
# Stats for debugging & performance.
6062
# The goal is 60 frames per second, or 16.67ms per frame.
6163
# That leads to a very smooth mouse cursor. (SmartNav was 100 fps)
6264
# A standard non-gaming monitor is also 60Hz. (TV is 30 fps)
6365
@dataclass
6466
class PhilNavDebug:
65-
time_start = time()
66-
time_debug = time()
67-
debug_num = 0
68-
msg_time_start = time()
69-
msg_time_total = 0
70-
msg_num = 0
67+
time_start = time()
68+
time_debug = time()
69+
debug_num = 0
70+
msg_time_start = time()
71+
msg_time_total = 0
72+
msg_num = 0
73+
7174

7275
# Main event loop:
7376
# 1. Receive mouse delta over UDP
7477
# 2. Update mouse cursor position
7578
# 3. Repeat forever until Ctrl-C
7679
while True:
77-
try:
78-
# 48 bytes of 6 doubles in binary C format. Why? Because it's OpenTrack's protocol.
79-
# x, y, z, pitch, yaw, roll = struct.unpack('dddddd', data)
80-
# PhilNav uses x, y as x_diff, y_diff and moves the mouse relative to its current position.
81-
# https://github.com/opentrack/opentrack/issues/747
82-
data, addr = sock.recvfrom(48)
83-
except TimeoutError:
84-
if int(time() - PhilNavDebug.time_start) % 5 == 0:
85-
logging.info(f" {ctime()} - {text_listening}")
86-
continue
87-
else:
88-
# measure time
89-
PhilNavDebug.msg_time_start = time()
90-
PhilNavDebug.msg_num += 1
91-
92-
# Using OpenTrack protocol, but PhilNav uses:
93-
# x_diff, y_diff, n/a, n/a, n/a, camera capture time
94-
x, y, z, pitch, yaw, roll = struct.unpack('dddddd', data)
95-
96-
# The Magic Happens Now! eg. move mouse cursor =P
97-
pt = POINT()
98-
ctypes.windll.user32.GetCursorPos(ctypes.byref(pt)) # get current mouse position by reference (C++ thing)
99-
# I'm moving the Y axis slightly faster because looking left and right is easier than nodding up and down.
100-
# Also, monitors are wider than they are tall.
101-
x_new = round(pt.x + x*args.speed)
102-
y_new = round(pt.y + y*args.speed*1.33)
103-
ctypes.windll.user32.SetCursorPos(x_new, y_new) # move mouse cursor
104-
105-
# I'm trying to measure the total time from capturing the frame on the camera to moving the mouse cursor on my PC. This isn't super accurate. It's sometimes negative (TIME TRAVEL!!!). The clock difference between the Raspberry Pi and my PC seems to be around 10-20ms?
106-
time_diff_ms = int((time() - roll)*1000)
107-
108-
# it's 60 FPS, so only debug once per second
109-
if time() - PhilNavDebug.time_debug > 1:
110-
PhilNavDebug.time_debug = time()
111-
PhilNavDebug.debug_num += 1
112-
# display legend every 5 seconds
113-
if PhilNavDebug.debug_num % 5 == 1:
114-
logging.info(f" {ctime()} - Received: ({'x_diff':>8},{'y_diff':>8},{'n/a':>8},{'n/a':>8},{'loc ns':>8},{'net ms':>8} )")
115-
logging.info(f" {ctime()} - Received: ({x:> 8.2f},{y:> 8.2f},{z:> 8.2f},{pitch:> 8.2f},{(time() - PhilNavDebug.msg_time_start)*1000:> 8.2f},{time_diff_ms:> 8} )")
80+
try:
81+
# 48 bytes of 6 doubles in binary C format. Why? Because it's OpenTrack's protocol.
82+
# x, y, z, pitch, yaw, roll = struct.unpack('dddddd', data)
83+
# PhilNav uses x, y as x_diff, y_diff and moves the mouse relative to its current position.
84+
# https://github.com/opentrack/opentrack/issues/747
85+
data, addr = sock.recvfrom(48)
86+
except TimeoutError:
87+
if int(time() - PhilNavDebug.time_start) % 5 == 0:
88+
logging.info(f" {ctime()} - {text_listening}")
89+
continue
90+
else:
91+
# measure time
92+
PhilNavDebug.msg_time_start = time()
93+
PhilNavDebug.msg_num += 1
94+
95+
# Using OpenTrack protocol, but PhilNav uses:
96+
# x_diff, y_diff, n/a, n/a, n/a, camera capture time
97+
x, y, z, pitch, yaw, roll = struct.unpack("dddddd", data)
98+
99+
# The Magic Happens Now! eg. move mouse cursor =P
100+
pt = POINT()
101+
ctypes.windll.user32.GetCursorPos(
102+
ctypes.byref(pt)
103+
) # get current mouse position by reference (C++ thing)
104+
# I'm moving the Y axis slightly faster because looking left and right is easier than nodding up and down.
105+
# Also, monitors are wider than they are tall.
106+
x_new = round(pt.x + x * args.speed)
107+
y_new = round(pt.y + y * args.speed * 1.33)
108+
ctypes.windll.user32.SetCursorPos(x_new, y_new) # move mouse cursor
109+
110+
# I'm trying to measure the total time from capturing the frame on the camera to moving the mouse cursor on my PC. This isn't super accurate. It's sometimes negative (TIME TRAVEL!!!). The clock difference between the Raspberry Pi and my PC seems to be around 10-20ms?
111+
time_diff_ms = int((time() - roll) * 1000)
112+
113+
# it's 60 FPS, so only debug once per second
114+
if time() - PhilNavDebug.time_debug > 1:
115+
PhilNavDebug.time_debug = time()
116+
PhilNavDebug.debug_num += 1
117+
# display legend every 5 seconds
118+
if PhilNavDebug.debug_num % 5 == 1:
119+
logging.info(
120+
f" {ctime()} - Received: ({'x_diff':>8},{'y_diff':>8},{'n/a':>8},{'n/a':>8},{'loc ns':>8},{'net ms':>8} )"
121+
)
122+
logging.info(
123+
f" {ctime()} - Received: ({x:> 8.2f},{y:> 8.2f},{z:> 8.2f},{pitch:> 8.2f},{(time() - PhilNavDebug.msg_time_start)*1000:> 8.2f},{time_diff_ms:> 8} )"
124+
)

‎server_raspberrypi/main.py

Lines changed: 69 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,24 @@
77
import inspect
88
import socket
99
import struct
10+
1011
# from keyboard import is_pressed
1112

1213
picam2 = Picamera2()
13-
config = picam2.create_preview_configuration(main={"size": (320, 240)},transform=Transform(hflip=1))
14+
config = picam2.create_preview_configuration(
15+
main={"size": (320, 240)}, transform=Transform(hflip=1)
16+
)
1417
picam2.configure(config)
15-
picam2.set_controls({"AnalogueGain": 2.0, "Brightness": -0.4, "Contrast": 5, "ExposureValue": 1, "Saturation": 0, "FrameRate": 85})
18+
picam2.set_controls(
19+
{
20+
"AnalogueGain": 2.0,
21+
"Brightness": -0.4,
22+
"Contrast": 5,
23+
"ExposureValue": 1,
24+
"Saturation": 0,
25+
"FrameRate": 85,
26+
}
27+
)
1628
picam2.start_preview(Preview.QT)
1729
# picam2.start_preview(Preview.NULL)
1830
picam2.start()
@@ -28,46 +40,59 @@
2840
MESSAGE = None
2941
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
3042

31-
frame=0
32-
x=0
33-
y=0
34-
keypoints=None
35-
started_at_ms = time.time()*1000
36-
frame_start_ms = time.time()*1000
43+
frame = 0
44+
x = 0
45+
y = 0
46+
keypoints = None
47+
started_at_ms = time.time() * 1000
48+
frame_start_ms = time.time() * 1000
49+
3750

3851
def blobby(request):
39-
with MappedArray(request, "main") as m:
40-
global frame
41-
global x
42-
global y
43-
global keypoints
44-
global frame_start_ms
45-
x_diff = 0
46-
y_diff = 0
47-
48-
frame = frame + 1
49-
50-
keypoints = detector.detect(m.array)
51-
cv2.drawKeypoints(m.array, keypoints, m.array, (255,0,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
52-
53-
if len(keypoints) > 0:
54-
kp = keypoints[0]
55-
x_new, y_new = kp.pt
56-
x_diff = x_new - x
57-
y_diff = y_new - y
58-
x = x_new
59-
y = y_new
60-
if (x_diff**2 > 0 or y_diff**2 > 0) and x_diff**2 < 10 and y_diff**2 < 10:
61-
MESSAGE = struct.pack("dddddd", x_diff, y_diff, 0, 0, 0, frame_start_ms)
62-
sock.sendto(MESSAGE, (UDP_IP, UDP_PORT))
63-
64-
65-
if frame % 1 == 0:
66-
fps = frame/((time.time()*1000-started_at_ms)/1000)
67-
ms = time.time()*1000 - frame_start_ms
68-
logging.warning(f"Frame: {frame}, Diff: ({int(x_diff)}, {int(y_diff)}), FPS: {int(fps)}, local-MS: {int(ms)}")
69-
70-
frame_start_ms = time.time()*1000
52+
with MappedArray(request, "main") as m:
53+
global frame
54+
global x
55+
global y
56+
global keypoints
57+
global frame_start_ms
58+
x_diff = 0
59+
y_diff = 0
60+
61+
frame = frame + 1
62+
63+
keypoints = detector.detect(m.array)
64+
cv2.drawKeypoints(
65+
m.array,
66+
keypoints,
67+
m.array,
68+
(255, 0, 0),
69+
cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS,
70+
)
71+
72+
if len(keypoints) > 0:
73+
kp = keypoints[0]
74+
x_new, y_new = kp.pt
75+
x_diff = x_new - x
76+
y_diff = y_new - y
77+
x = x_new
78+
y = y_new
79+
if (
80+
(x_diff**2 > 0 or y_diff**2 > 0)
81+
and x_diff**2 < 10
82+
and y_diff**2 < 10
83+
):
84+
MESSAGE = struct.pack("dddddd", x_diff, y_diff, 0, 0, 0, frame_start_ms)
85+
sock.sendto(MESSAGE, (UDP_IP, UDP_PORT))
86+
87+
if frame % 1 == 0:
88+
fps = frame / ((time.time() * 1000 - started_at_ms) / 1000)
89+
ms = time.time() * 1000 - frame_start_ms
90+
logging.warning(
91+
f"Frame: {frame}, Diff: ({int(x_diff)}, {int(y_diff)}), FPS: {int(fps)}, local-MS: {int(ms)}"
92+
)
93+
94+
frame_start_ms = time.time() * 1000
95+
7196

7297
picam2.pre_callback = blobby
7398
time.sleep(75)
@@ -84,7 +109,7 @@ def blobby(request):
84109
# keypoints = detector.detect(frame)
85110
# mat_with_keypoints = cv2.drawKeypoints(frame, keypoints, numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
86111
# cv2.imshow("img", mat_with_keypoints)
87-
#
112+
#
88113
# if time.time() - started_at > 5:
89114
# break
90115
# finally:
@@ -94,13 +119,13 @@ def blobby(request):
94119

95120
# array = picam2.capture_array("main")
96121
# cv2.imshow("img", array); cv2.waitKey(0)
97-
#
122+
#
98123
# src = cv2.imread("/home/philip/test.jpg", cv2.IMREAD_GRAYSCALE);
99-
#
124+
#
100125
# params = cv2.SimpleBlobDetector_Params()
101126
# params.blobColor = 255
102127
# detector = cv2.SimpleBlobDetector_create(params)
103-
#
128+
#
104129
# keypoints = detector.detect(src); keypoints
105130
# im_with_keypoints = cv2.drawKeypoints(src, keypoints, numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
106131
# cv2.imshow("img", im_with_keypoints); cv2.waitKey(0)

0 commit comments

Comments
 (0)