mount versions of pose-detectors combined
gestures4
5 days ago
parent
8a43b58710
commit
08c5b66ace
@ -1,262 +1,324 @@
|
||||
# gesture_control/arm_control.py
|
||||
import numpy as np
|
||||
import cv2
|
||||
|
||||
class ArmController:
|
||||
|
||||
def _clip_unit(v):
|
||||
return float(np.clip(v, -1.0, 1.0))
|
||||
|
||||
|
||||
def _apply_dead_zone(v, dz):
|
||||
return 0.0 if abs(v) < dz else v
|
||||
|
||||
|
||||
def _robust_metrics(landmarks, min_conf=0.5):
|
||||
"""
|
||||
Compute shoulder_center, shoulder_width, torso_height robustly.
|
||||
Uses hips if available; otherwise falls back to nose/shoulder geometry.
|
||||
|
||||
Returns:
|
||||
shoulder_center (np.array shape (2,))
|
||||
shoulder_width (float)
|
||||
torso_height (float)
|
||||
ok (bool)
|
||||
"""
|
||||
def pt(i):
|
||||
return np.array(landmarks[i][:2], dtype=float), float(landmarks[i][3])
|
||||
|
||||
l_sh, c_lsh = pt(11)
|
||||
r_sh, c_rsh = pt(12)
|
||||
if c_lsh < min_conf or c_rsh < min_conf:
|
||||
return None, 0.0, 0.0, False
|
||||
|
||||
shoulder_center = (l_sh + r_sh) / 2.0
|
||||
shoulder_width = float(np.linalg.norm(r_sh - l_sh))
|
||||
if shoulder_width < 1e-3:
|
||||
return shoulder_center, 0.0, 0.0, False
|
||||
|
||||
# Try hips
|
||||
l_hip, c_lhip = pt(23)
|
||||
r_hip, c_rhip = pt(24)
|
||||
|
||||
if c_lhip >= min_conf and c_rhip >= min_conf:
|
||||
hip_center = (l_hip + r_hip) / 2.0
|
||||
torso_height = float(np.linalg.norm(hip_center - shoulder_center))
|
||||
if torso_height >= 1e-3:
|
||||
return shoulder_center, shoulder_width, torso_height, True
|
||||
|
||||
# Fallbacks (upper-body only)
|
||||
nose, c_nose = pt(0)
|
||||
if c_nose >= min_conf:
|
||||
nose_to_shoulder = abs(nose[1] - shoulder_center[1])
|
||||
torso_height = max(1.6 * nose_to_shoulder, 0.9 * shoulder_width)
|
||||
else:
|
||||
torso_height = max(1.2 * shoulder_width, 1.0)
|
||||
|
||||
return shoulder_center, shoulder_width, float(torso_height), True
|
||||
|
||||
|
||||
class ArmControllerMethod1:
|
||||
"""
|
||||
Method 1: Single-hand driving with right wrist.
|
||||
- Linear: vertical offset of right wrist from shoulder center (normalized by torso height)
|
||||
- Angular: horizontal offset of right wrist from shoulder center (normalized by shoulder width)
|
||||
"""
|
||||
def __init__(self, config, mirror=False):
|
||||
"""
|
||||
Wrist-only controller using pose landmarks (indices 15 and 16).
|
||||
Behavior:
|
||||
- RIGHT wrist controls motion.
|
||||
- EXTINGUISHING only when BOTH wrists are visible AND BOTH are inside the TOP-CENTER rectangle:
|
||||
y < H/3 AND w/3 <= x <= 2w/3
|
||||
- Otherwise:
|
||||
* If RIGHT wrist visible -> 'move'
|
||||
* If no RIGHT wrist -> 'stop'
|
||||
"""
|
||||
self.config = config
|
||||
self.mirror = mirror
|
||||
self.dead_zone = config.get('dead_zone', 0.1)
|
||||
self.debug = config.get('debug', False)
|
||||
self.min_conf = config.get('min_conf', 0.5)
|
||||
|
||||
def compute_speeds(self, landmarks, frame_shape=None):
|
||||
if landmarks is None:
|
||||
return 0.0, 0.0
|
||||
|
||||
# Require: shoulders + right wrist
|
||||
need = [11, 12, 16]
|
||||
if any(landmarks[i][3] < self.min_conf for i in need):
|
||||
return 0.0, 0.0
|
||||
|
||||
shoulder_center, shoulder_width, torso_height, ok = _robust_metrics(landmarks, self.min_conf)
|
||||
if not ok or shoulder_width < 1e-3 or torso_height < 1e-3:
|
||||
return 0.0, 0.0
|
||||
|
||||
r_wr = landmarks[16][:2]
|
||||
|
||||
# Positive linear when wrist above shoulder center (forward)
|
||||
linear = (shoulder_center[1] - r_wr[1]) / torso_height
|
||||
# Positive angular when wrist to the right of shoulder center
|
||||
angular = (r_wr[0] - shoulder_center[0]) / shoulder_width
|
||||
|
||||
self.dead_zone = config.get('dead_zone', 0.15)
|
||||
if self.mirror:
|
||||
angular = -angular
|
||||
|
||||
linear = _clip_unit(_apply_dead_zone(linear, self.dead_zone))
|
||||
angular = _clip_unit(_apply_dead_zone(angular, self.dead_zone))
|
||||
|
||||
if self.debug:
|
||||
print(f"[M1] L:{linear:.2f} A:{angular:.2f}")
|
||||
return linear, angular
|
||||
|
||||
def draw_overlay(self, frame, landmarks=None):
|
||||
if frame is None:
|
||||
return frame
|
||||
h, w = frame.shape[:2]
|
||||
# Draw center cross (screen center approximation)
|
||||
cv2.line(frame, (w // 2, 0), (w // 2, h), (0, 0, 0), 1)
|
||||
cv2.line(frame, (0, h // 2), (w, h // 2), (0, 0, 0), 1)
|
||||
# Draw right wrist
|
||||
if landmarks is not None and landmarks[16][3] > 0.5:
|
||||
x, y = int(landmarks[16][0]), int(landmarks[16][1])
|
||||
cv2.circle(frame, (x, y), 8, (0, 255, 255), -1)
|
||||
return frame
|
||||
|
||||
|
||||
class ArmControllerMethod2:
|
||||
"""
|
||||
Method 2: Two-hand blended control.
|
||||
- Linear: average vertical offset of both wrists from shoulder center (normalized by torso height)
|
||||
- Angular: horizontal balance of wrists around shoulder center (normalized by shoulder width)
|
||||
"""
|
||||
def __init__(self, config, mirror=False):
|
||||
self.config = config
|
||||
self.mirror = mirror
|
||||
self.dead_zone = config.get('dead_zone', 0.1)
|
||||
self.debug = config.get('debug', False)
|
||||
self.min_conf = config.get('min_conf', 0.5)
|
||||
|
||||
# UI speeds in 0..100; used to map from wrist offset to final robot speeds
|
||||
self.max_speed = config.get('max_speed', 100)
|
||||
def compute_speeds(self, landmarks, frame_shape=None):
|
||||
if landmarks is None:
|
||||
return 0.0, 0.0
|
||||
|
||||
# Require: shoulders + both wrists
|
||||
need = [11, 12, 15, 16]
|
||||
if any(landmarks[i][3] < self.min_conf for i in need):
|
||||
return 0.0, 0.0
|
||||
|
||||
# Robot kinematics limits
|
||||
self.max_speed_linear = config.get('max_speed_linear', 0.8)
|
||||
self.max_speed_angular = config.get('max_speed_angular', 1.5)
|
||||
shoulder_center, shoulder_width, torso_height, ok = _robust_metrics(landmarks, self.min_conf)
|
||||
if not ok or shoulder_width < 1e-3 or torso_height < 1e-3:
|
||||
return 0.0, 0.0
|
||||
|
||||
l_wr = landmarks[15][:2]
|
||||
r_wr = landmarks[16][:2]
|
||||
|
||||
self.screen_width = config.get('screen_width', 640)
|
||||
self.screen_height = config.get('screen_height', 480)
|
||||
# Linear: average elevation of both wrists
|
||||
lin_l = (shoulder_center[1] - l_wr[1]) / torso_height
|
||||
lin_r = (shoulder_center[1] - r_wr[1]) / torso_height
|
||||
linear = 0.5 * (lin_l + lin_r)
|
||||
|
||||
def update_frame_size(self, w, h):
|
||||
self.screen_width = int(w)
|
||||
self.screen_height = int(h)
|
||||
# Angular: horizontal balance
|
||||
ang = ((r_wr[0] - shoulder_center[0]) - (shoulder_center[0] - l_wr[0])) / shoulder_width
|
||||
angular = ang
|
||||
|
||||
def _get_wrist_index(self, side):
|
||||
"""
|
||||
Wrist indices (MediaPipe Pose):
|
||||
- left wrist: 15
|
||||
- right wrist: 16
|
||||
With mirror=True, interpret 'left'/'right' as in mirrored preview.
|
||||
"""
|
||||
if self.mirror:
|
||||
return 16 if side == 'left' else 15
|
||||
else:
|
||||
return 15 if side == 'left' else 16
|
||||
|
||||
def _get_wrist_position(self, landmarks, side):
|
||||
"""
|
||||
Return (x, y) of the requested wrist, or None if not visible.
|
||||
Only wrist landmarks are used; all other landmarks are ignored.
|
||||
"""
|
||||
idx = self._get_wrist_index(side)
|
||||
if idx >= len(landmarks):
|
||||
return None
|
||||
# If visibility exists (4th value), require >= 0.5
|
||||
if len(landmarks[idx]) >= 4 and landmarks[idx][3] < 0.5:
|
||||
return None
|
||||
x = float(landmarks[idx][0])
|
||||
y = float(landmarks[idx][1])
|
||||
return x, y
|
||||
|
||||
def _in_top_center_section(self, x, y):
|
||||
"""
|
||||
Top-center rectangle:
|
||||
- y in [0, H/3)
|
||||
- x in [W/3, 2W/3)
|
||||
"""
|
||||
w_third = self.screen_width / 3.0
|
||||
h_third = self.screen_height / 3.0
|
||||
return (y < h_third) and (x >= w_third) and (x < 2.0 * w_third)
|
||||
|
||||
def _both_wrists_in_top_center(self, landmarks):
|
||||
"""
|
||||
Returns True only if BOTH wrists are visible AND both are inside the top-center rectangle.
|
||||
"""
|
||||
lw = self._get_wrist_position(landmarks, 'left')
|
||||
rw = self._get_wrist_position(landmarks, 'right')
|
||||
if lw is None or rw is None:
|
||||
return False
|
||||
return self._in_top_center_section(lw[0], lw[1]) and self._in_top_center_section(rw[0], rw[1])
|
||||
|
||||
def _scale_speed(self, norm_offset):
|
||||
"""
|
||||
norm_offset is in [0..1].
|
||||
Convert from [dead_zone..1] to [0..1], then scale to 0..max_speed.
|
||||
"""
|
||||
eff = (norm_offset - self.dead_zone) / (1.0 - self.dead_zone)
|
||||
eff = np.clip(eff, 0.0, 1.0)
|
||||
return round(eff * self.max_speed)
|
||||
|
||||
def _compute_speeds(self, x, y):
|
||||
"""
|
||||
Compute directions and 0..100 speeds from a wrist position relative to screen center.
|
||||
Returns (h_dir, h_speed, v_dir, v_speed)
|
||||
- h_dir: 'left' | 'right' | 'center'
|
||||
- v_dir: 'up' | 'down' | 'center'
|
||||
"""
|
||||
cx = self.screen_width / 2.0
|
||||
cy = self.screen_height / 2.0
|
||||
|
||||
# Normalize offset: -1..1
|
||||
dx = (x - cx) / (self.screen_width / 2.0)
|
||||
dy = (y - cy) / (self.screen_height / 2.0)
|
||||
dx = float(np.clip(dx, -1.0, 1.0))
|
||||
dy = float(np.clip(dy, -1.0, 1.0))
|
||||
|
||||
# Horizontal
|
||||
if abs(dx) < self.dead_zone:
|
||||
h_dir, h_speed = 'center', 0
|
||||
elif dx > 0:
|
||||
h_dir = 'right'
|
||||
h_speed = self._scale_speed(abs(dx))
|
||||
else:
|
||||
h_dir = 'left'
|
||||
h_speed = self._scale_speed(abs(dx))
|
||||
|
||||
# Vertical (image y grows down; up is dy < 0)
|
||||
if abs(dy) < self.dead_zone:
|
||||
v_dir, v_speed = 'center', 0
|
||||
elif dy < 0:
|
||||
v_dir = 'up'
|
||||
v_speed = self._scale_speed(abs(dy))
|
||||
else:
|
||||
v_dir = 'down'
|
||||
v_speed = self._scale_speed(abs(dy))
|
||||
|
||||
return h_dir, h_speed, v_dir, v_speed
|
||||
|
||||
def get_command(self, landmarks):
|
||||
"""
|
||||
Logic:
|
||||
- If BOTH wrists are visible AND BOTH are inside the top-center rectangle (the 'UP' section)
|
||||
-> {'command': 'extinguishing fire'}
|
||||
- Else if RIGHT wrist is visible -> {'command': 'move', ...} using RIGHT wrist position
|
||||
- Else -> {'command': 'stop'}
|
||||
"""
|
||||
if self._both_wrists_in_top_center(landmarks):
|
||||
if self.debug:
|
||||
print("Two wrists in TOP-CENTER ('UP' section) -> EXTINGUISHING FIRE")
|
||||
return {'command': 'extinguishing fire'}
|
||||
|
||||
# Otherwise, control with RIGHT wrist only
|
||||
right_wrist = self._get_wrist_position(landmarks, 'right')
|
||||
if right_wrist is not None:
|
||||
x, y = right_wrist
|
||||
h_dir, h_speed, v_dir, v_speed = self._compute_speeds(x, y)
|
||||
return {
|
||||
'command': 'move',
|
||||
'x': x, 'y': y,
|
||||
'h_dir': h_dir, 'h_speed': h_speed,
|
||||
'v_dir': v_dir, 'v_speed': v_speed,
|
||||
}
|
||||
|
||||
return {'command': 'stop'}
|
||||
|
||||
def to_linear_angular(self, cmd):
|
||||
"""
|
||||
Convert a 'move' command (0..100 UI speeds) to robot linear/angular velocities.
|
||||
|
||||
Mapping:
|
||||
- Vertical axis controls linear velocity: up -> +linear (forward), down -> -linear (backward)
|
||||
- Horizontal axis controls angular velocity: left -> +angular (CCW), right -> -angular (CW)
|
||||
"""
|
||||
if cmd.get('command') != 'move':
|
||||
angular = -angular
|
||||
|
||||
linear = _clip_unit(_apply_dead_zone(linear, self.dead_zone))
|
||||
angular = _clip_unit(_apply_dead_zone(angular, self.dead_zone))
|
||||
|
||||
if self.debug:
|
||||
print(f"[M2] L:{linear:.2f} A:{angular:.2f}")
|
||||
return linear, angular
|
||||
|
||||
def draw_overlay(self, frame, landmarks=None):
|
||||
if frame is None:
|
||||
return frame
|
||||
if landmarks is not None:
|
||||
for idx, color in [(15, (255, 0, 255)), (16, (0, 255, 255))]:
|
||||
if landmarks[idx][3] > 0.5:
|
||||
x, y = int(landmarks[idx][0]), int(landmarks[idx][1])
|
||||
cv2.circle(frame, (x, y), 8, color, -1)
|
||||
return frame
|
||||
|
||||
|
||||
class ArmControllerMethod3:
|
||||
"""
|
||||
Method 3: Elbow-augmented control.
|
||||
- Linear: average vertical offset of elbows (normalized by torso height)
|
||||
- Angular: wrist horizontal balance (normalized by shoulder width)
|
||||
"""
|
||||
def __init__(self, config, mirror=False):
|
||||
self.config = config
|
||||
self.mirror = mirror
|
||||
self.dead_zone = config.get('dead_zone', 0.1)
|
||||
self.debug = config.get('debug', False)
|
||||
self.min_conf = config.get('min_conf', 0.5)
|
||||
|
||||
def compute_speeds(self, landmarks, frame_shape=None):
|
||||
if landmarks is None:
|
||||
return 0.0, 0.0
|
||||
|
||||
v_dir, v_speed = cmd['v_dir'], float(cmd['v_speed'])
|
||||
h_dir, h_speed = cmd['h_dir'], float(cmd['h_speed'])
|
||||
# Require shoulders; prefer elbows for linear; wrists for angular.
|
||||
need_base = [11, 12]
|
||||
if any(landmarks[i][3] < self.min_conf for i in need_base):
|
||||
return 0.0, 0.0
|
||||
|
||||
# Normalize speeds to 0..1
|
||||
v_norm = v_speed / float(self.max_speed) if self.max_speed > 0 else 0.0
|
||||
h_norm = h_speed / float(self.max_speed) if self.max_speed > 0 else 0.0
|
||||
elbows_ok = (landmarks[13][3] >= self.min_conf and landmarks[14][3] >= self.min_conf)
|
||||
wrists_ok = (landmarks[15][3] >= self.min_conf and landmarks[16][3] >= self.min_conf)
|
||||
|
||||
# Linear: forward/backward
|
||||
if v_dir == 'up':
|
||||
linear = +v_norm * self.max_speed_linear
|
||||
elif v_dir == 'down':
|
||||
linear = -v_norm * self.max_speed_linear
|
||||
else:
|
||||
linear = 0.0
|
||||
if not elbows_ok and not wrists_ok:
|
||||
return 0.0, 0.0
|
||||
|
||||
shoulder_center, shoulder_width, torso_height, ok = _robust_metrics(landmarks, self.min_conf)
|
||||
if not ok or shoulder_width < 1e-3 or torso_height < 1e-3:
|
||||
return 0.0, 0.0
|
||||
|
||||
# Angular: left/right
|
||||
# Convention: left turn -> +angular, right turn -> -angular
|
||||
if h_dir == 'left':
|
||||
angular = +h_norm * self.max_speed_angular
|
||||
elif h_dir == 'right':
|
||||
angular = -h_norm * self.max_speed_angular
|
||||
# Linear: prefer elbows, fallback to wrists average if elbows missing
|
||||
if elbows_ok:
|
||||
l_el = landmarks[13][:2]
|
||||
r_el = landmarks[14][:2]
|
||||
lin_l = (shoulder_center[1] - l_el[1]) / torso_height
|
||||
lin_r = (shoulder_center[1] - r_el[1]) / torso_height
|
||||
linear = 0.5 * (lin_l + lin_r)
|
||||
else:
|
||||
l_wr = landmarks[15][:2]
|
||||
r_wr = landmarks[16][:2]
|
||||
lin_l = (shoulder_center[1] - l_wr[1]) / torso_height
|
||||
lin_r = (shoulder_center[1] - r_wr[1]) / torso_height
|
||||
linear = 0.5 * (lin_l + lin_r)
|
||||
|
||||
# Angular: use wrists if available, else 0
|
||||
if wrists_ok:
|
||||
l_wr = landmarks[15][:2]
|
||||
r_wr = landmarks[16][:2]
|
||||
angular = ((r_wr[0] + l_wr[0]) - 2 * shoulder_center[0]) / shoulder_width
|
||||
else:
|
||||
angular = 0.0
|
||||
|
||||
return float(linear), float(angular)
|
||||
|
||||
def draw_hands(self, frame, landmarks):
|
||||
"""
|
||||
Overlay control UI on the frame.
|
||||
- Shows 3x3 grid.
|
||||
- Extinguish only when both wrists are in the TOP-CENTER rectangle (the 'UP' section).
|
||||
- Otherwise, RIGHT wrist controls motion.
|
||||
"""
|
||||
cmd = self.get_command(landmarks)
|
||||
w, h = self.screen_width, self.screen_height
|
||||
|
||||
# 3x3 grid
|
||||
grid_color = (200, 200, 200)
|
||||
cv2.line(frame, (w // 3, 0), (w // 3, h), grid_color, 1)
|
||||
cv2.line(frame, (2 * w // 3, 0), (2 * w // 3, h), grid_color, 1)
|
||||
cv2.line(frame, (0, h // 3), (w, h // 3), grid_color, 1)
|
||||
cv2.line(frame, (0, 2 * h // 3), (w, 2 * h // 3), grid_color, 1)
|
||||
|
||||
# Highlight the top-center rectangle (UP section)
|
||||
x1, x2 = int(w / 3.0), int(2 * w / 3.0)
|
||||
y2 = int(h / 3.0)
|
||||
overlay = frame.copy()
|
||||
cv2.rectangle(overlay, (x1, 0), (x2, y2), (0, 165, 255), -1) # orange overlay
|
||||
cv2.addWeighted(overlay, 0.10, frame, 0.90, 0, frame)
|
||||
|
||||
# Center dot
|
||||
cv2.circle(frame, (w // 2, h // 2), 6, (255, 255, 255), -1)
|
||||
|
||||
# Draw detected wrist points for feedback (if available)
|
||||
lw = self._get_wrist_position(landmarks, 'left')
|
||||
rw = self._get_wrist_position(landmarks, 'right')
|
||||
if lw is not None:
|
||||
cv2.circle(frame, (int(lw[0]), int(lw[1])), 10, (255, 0, 0), -1) # left: blue
|
||||
if rw is not None:
|
||||
cv2.circle(frame, (int(rw[0]), int(rw[1])), 10, (0, 255, 0), -1) # right: green
|
||||
|
||||
if cmd['command'] == 'stop':
|
||||
cv2.putText(frame, "COMMAND: STOP", (20, 40),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 0, 255), 3)
|
||||
if self.debug:
|
||||
print("COMMAND: stop")
|
||||
return frame
|
||||
if self.mirror:
|
||||
angular = -angular
|
||||
|
||||
linear = _clip_unit(_apply_dead_zone(linear, self.dead_zone))
|
||||
angular = _clip_unit(_apply_dead_zone(angular, self.dead_zone))
|
||||
|
||||
if cmd['command'] == 'extinguishing fire':
|
||||
cv2.putText(frame, "EXTINGUISHING FIRE (2 WRISTS IN TOP-CENTER 'UP' SECTION)", (20, 40),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 165, 255), 2)
|
||||
if self.debug:
|
||||
print("COMMAND: extinguishing fire (two wrists in top-center section)")
|
||||
if self.debug:
|
||||
print(f"[M3] L:{linear:.2f} A:{angular:.2f}")
|
||||
return linear, angular
|
||||
|
||||
def draw_overlay(self, frame, landmarks=None):
|
||||
if frame is None:
|
||||
return frame
|
||||
if landmarks is not None:
|
||||
for idx, color in [(13, (0, 200, 0)), (14, (0, 200, 0)), (15, (0, 255, 255)), (16, (255, 0, 255))]:
|
||||
if landmarks[idx][3] > 0.5:
|
||||
x, y = int(landmarks[idx][0]), int(landmarks[idx][1])
|
||||
cv2.circle(frame, (x, y), 6, color, -1)
|
||||
return frame
|
||||
|
||||
# command == move (right wrist visible)
|
||||
x, y = int(cmd['x']), int(cmd['y'])
|
||||
cv2.circle(frame, (x, y), 12, (0, 255, 0), -1)
|
||||
|
||||
# Vector from center to wrist
|
||||
cv2.line(frame, (w // 2, h // 2), (x, y), (0, 255, 0), 2)
|
||||
class ArmControllerMethod4:
|
||||
"""
|
||||
Method 4: 3x3 grid based on landmark 19 (right index finger tip).
|
||||
Screen split at 2/5 and 3/5 (both axes). Center band = 0.
|
||||
Proportional speed away from the center bands.
|
||||
"""
|
||||
def __init__(self, config, mirror=False):
|
||||
self.config = config
|
||||
self.mirror = mirror
|
||||
self.finger_idx = 19 # right index finger tip
|
||||
self.debug = config.get('debug', False)
|
||||
|
||||
def compute_speeds(self, landmarks, frame_shape=None):
|
||||
linear = 0.0
|
||||
angular = 0.0
|
||||
|
||||
if frame_shape is None or landmarks is None:
|
||||
return 0.0, 0.0
|
||||
if landmarks[self.finger_idx][3] < 0.5:
|
||||
return 0.0, 0.0
|
||||
|
||||
h, w = int(frame_shape[0]), int(frame_shape[1])
|
||||
x = landmarks[self.finger_idx][0]
|
||||
y = landmarks[self.finger_idx][1]
|
||||
|
||||
# Angular (horizontal): center band 2/5..3/5 = 0
|
||||
if 2 * w / 5 <= x <= 3 * w / 5:
|
||||
angular = 0.0
|
||||
elif x > 3 * w / 5:
|
||||
angular = (x * 5) / (2 * w) - 1
|
||||
else:
|
||||
angular = (x - 3 * w / 5) / (2 * w / 5)
|
||||
|
||||
v_dir, v_speed = cmd['v_dir'], cmd['v_speed']
|
||||
h_dir, h_speed = cmd['h_dir'], cmd['h_speed']
|
||||
# Linear (vertical): center band 2/5..3/5 = 0
|
||||
if 2 * h / 5 <= y <= 3 * h / 5:
|
||||
linear = 0.0
|
||||
elif y > 3 * h / 5:
|
||||
linear = -((y * 5) / (2 * h) - 1)
|
||||
else:
|
||||
linear = -(y - 3 * h / 5) / (2 * h / 5)
|
||||
|
||||
cv2.putText(frame, f"V: {v_dir.upper()} {v_speed}", (20, 40),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
|
||||
cv2.putText(frame, f"H: {h_dir.upper()} {h_speed}", (20, 75),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
|
||||
if self.mirror:
|
||||
angular = -angular
|
||||
|
||||
if self.debug:
|
||||
print(f"V: {v_dir} {v_speed} | H: {h_dir} {h_speed}")
|
||||
print(f"[M4] L:{linear:.2f} A:{angular:.2f}")
|
||||
return _clip_unit(linear), _clip_unit(angular)
|
||||
|
||||
def draw_overlay(self, frame, landmarks=None):
|
||||
if frame is None:
|
||||
return frame
|
||||
|
||||
h, w = frame.shape[:2]
|
||||
x1, x2 = int(w * 2 / 5), int(w * 3 / 5)
|
||||
y1, y2 = int(h * 2 / 5), int(h * 3 / 5)
|
||||
|
||||
# Grid lines
|
||||
cv2.line(frame, (x1, 0), (x1, h), (0, 0, 0), 2)
|
||||
cv2.line(frame, (x2, 0), (x2, h), (0, 0, 0), 2)
|
||||
cv2.line(frame, (0, y1), (w, y1), (0, 0, 0), 2)
|
||||
cv2.line(frame, (0, y2), (w, y2), (0, 0, 0), 2)
|
||||
|
||||
# Highlight active cell + finger
|
||||
if landmarks is not None and landmarks[self.finger_idx][3] > 0.5:
|
||||
fx, fy = int(landmarks[self.finger_idx][0]), int(landmarks[self.finger_idx][1])
|
||||
cx0, cx1 = (0, x1) if fx < x1 else ((x2, w) if fx > x2 else (x1, x2))
|
||||
cy0, cy1 = (0, y1) if fy < y1 else ((y2, h) if fy > y2 else (y1, y2))
|
||||
|
||||
overlay = frame.copy()
|
||||
cv2.rectangle(overlay, (cx0, cy0), (cx1, cy1), (0, 255, 255), -1)
|
||||
frame = cv2.addWeighted(overlay, 0.2, frame, 0.8, 0)
|
||||
|
||||
cv2.circle(frame, (fx, fy), 8, (0, 255, 255), -1)
|
||||
cv2.circle(frame, (fx, fy), 12, (0, 120, 120), 2)
|
||||
|
||||
return frame
|
||||
|
||||
Loading…
Reference in New Issue