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 numpy as np
|
||||||
import cv2
|
import cv2
|
||||||
|
|
||||||
class ArmController:
|
|
||||||
def __init__(self, config, mirror=False):
|
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):
|
||||||
"""
|
"""
|
||||||
Wrist-only controller using pose landmarks (indices 15 and 16).
|
Compute shoulder_center, shoulder_width, torso_height robustly.
|
||||||
Behavior:
|
Uses hips if available; otherwise falls back to nose/shoulder geometry.
|
||||||
- RIGHT wrist controls motion.
|
|
||||||
- EXTINGUISHING only when BOTH wrists are visible AND BOTH are inside the TOP-CENTER rectangle:
|
Returns:
|
||||||
y < H/3 AND w/3 <= x <= 2w/3
|
shoulder_center (np.array shape (2,))
|
||||||
- Otherwise:
|
shoulder_width (float)
|
||||||
* If RIGHT wrist visible -> 'move'
|
torso_height (float)
|
||||||
* If no RIGHT wrist -> 'stop'
|
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):
|
||||||
self.config = config
|
self.config = config
|
||||||
self.mirror = mirror
|
self.mirror = mirror
|
||||||
|
self.dead_zone = config.get('dead_zone', 0.1)
|
||||||
self.dead_zone = config.get('dead_zone', 0.15)
|
|
||||||
self.debug = config.get('debug', False)
|
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
|
def compute_speeds(self, landmarks, frame_shape=None):
|
||||||
self.max_speed = config.get('max_speed', 100)
|
if landmarks is None:
|
||||||
|
return 0.0, 0.0
|
||||||
|
|
||||||
# Robot kinematics limits
|
# Require: shoulders + right wrist
|
||||||
self.max_speed_linear = config.get('max_speed_linear', 0.8)
|
need = [11, 12, 16]
|
||||||
self.max_speed_angular = config.get('max_speed_angular', 1.5)
|
if any(landmarks[i][3] < self.min_conf for i in need):
|
||||||
|
return 0.0, 0.0
|
||||||
|
|
||||||
self.screen_width = config.get('screen_width', 640)
|
shoulder_center, shoulder_width, torso_height, ok = _robust_metrics(landmarks, self.min_conf)
|
||||||
self.screen_height = config.get('screen_height', 480)
|
if not ok or shoulder_width < 1e-3 or torso_height < 1e-3:
|
||||||
|
return 0.0, 0.0
|
||||||
|
|
||||||
def update_frame_size(self, w, h):
|
r_wr = landmarks[16][:2]
|
||||||
self.screen_width = int(w)
|
|
||||||
self.screen_height = int(h)
|
# 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
|
||||||
|
|
||||||
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:
|
if self.mirror:
|
||||||
return 16 if side == 'left' else 15
|
angular = -angular
|
||||||
else:
|
|
||||||
return 15 if side == 'left' else 16
|
|
||||||
|
|
||||||
def _get_wrist_position(self, landmarks, side):
|
linear = _clip_unit(_apply_dead_zone(linear, self.dead_zone))
|
||||||
"""
|
angular = _clip_unit(_apply_dead_zone(angular, self.dead_zone))
|
||||||
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):
|
if self.debug:
|
||||||
"""
|
print(f"[M1] L:{linear:.2f} A:{angular:.2f}")
|
||||||
Returns True only if BOTH wrists are visible AND both are inside the top-center rectangle.
|
return linear, angular
|
||||||
"""
|
|
||||||
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):
|
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:
|
||||||
"""
|
"""
|
||||||
norm_offset is in [0..1].
|
Method 2: Two-hand blended control.
|
||||||
Convert from [dead_zone..1] to [0..1], then scale to 0..max_speed.
|
- 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)
|
||||||
"""
|
"""
|
||||||
eff = (norm_offset - self.dead_zone) / (1.0 - self.dead_zone)
|
def __init__(self, config, mirror=False):
|
||||||
eff = np.clip(eff, 0.0, 1.0)
|
self.config = config
|
||||||
return round(eff * self.max_speed)
|
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, x, y):
|
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
|
||||||
|
|
||||||
|
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]
|
||||||
|
|
||||||
|
# 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)
|
||||||
|
|
||||||
|
# Angular: horizontal balance
|
||||||
|
ang = ((r_wr[0] - shoulder_center[0]) - (shoulder_center[0] - l_wr[0])) / shoulder_width
|
||||||
|
angular = ang
|
||||||
|
|
||||||
|
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"[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:
|
||||||
"""
|
"""
|
||||||
Compute directions and 0..100 speeds from a wrist position relative to screen center.
|
Method 3: Elbow-augmented control.
|
||||||
Returns (h_dir, h_speed, v_dir, v_speed)
|
- Linear: average vertical offset of elbows (normalized by torso height)
|
||||||
- h_dir: 'left' | 'right' | 'center'
|
- Angular: wrist horizontal balance (normalized by shoulder width)
|
||||||
- v_dir: 'up' | 'down' | 'center'
|
|
||||||
"""
|
"""
|
||||||
cx = self.screen_width / 2.0
|
def __init__(self, config, mirror=False):
|
||||||
cy = self.screen_height / 2.0
|
self.config = config
|
||||||
|
self.mirror = mirror
|
||||||
# Normalize offset: -1..1
|
self.dead_zone = config.get('dead_zone', 0.1)
|
||||||
dx = (x - cx) / (self.screen_width / 2.0)
|
self.debug = config.get('debug', False)
|
||||||
dy = (y - cy) / (self.screen_height / 2.0)
|
self.min_conf = config.get('min_conf', 0.5)
|
||||||
dx = float(np.clip(dx, -1.0, 1.0))
|
|
||||||
dy = float(np.clip(dy, -1.0, 1.0))
|
def compute_speeds(self, landmarks, frame_shape=None):
|
||||||
|
if landmarks is None:
|
||||||
# Horizontal
|
return 0.0, 0.0
|
||||||
if abs(dx) < self.dead_zone:
|
|
||||||
h_dir, h_speed = 'center', 0
|
# Require shoulders; prefer elbows for linear; wrists for angular.
|
||||||
elif dx > 0:
|
need_base = [11, 12]
|
||||||
h_dir = 'right'
|
if any(landmarks[i][3] < self.min_conf for i in need_base):
|
||||||
h_speed = self._scale_speed(abs(dx))
|
return 0.0, 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)
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
# 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:
|
else:
|
||||||
h_dir = 'left'
|
l_wr = landmarks[15][:2]
|
||||||
h_speed = self._scale_speed(abs(dx))
|
r_wr = landmarks[16][:2]
|
||||||
|
lin_l = (shoulder_center[1] - l_wr[1]) / torso_height
|
||||||
# Vertical (image y grows down; up is dy < 0)
|
lin_r = (shoulder_center[1] - r_wr[1]) / torso_height
|
||||||
if abs(dy) < self.dead_zone:
|
linear = 0.5 * (lin_l + lin_r)
|
||||||
v_dir, v_speed = 'center', 0
|
|
||||||
elif dy < 0:
|
# Angular: use wrists if available, else 0
|
||||||
v_dir = 'up'
|
if wrists_ok:
|
||||||
v_speed = self._scale_speed(abs(dy))
|
l_wr = landmarks[15][:2]
|
||||||
|
r_wr = landmarks[16][:2]
|
||||||
|
angular = ((r_wr[0] + l_wr[0]) - 2 * shoulder_center[0]) / shoulder_width
|
||||||
else:
|
else:
|
||||||
v_dir = 'down'
|
angular = 0.0
|
||||||
v_speed = self._scale_speed(abs(dy))
|
|
||||||
|
if self.mirror:
|
||||||
|
angular = -angular
|
||||||
|
|
||||||
return h_dir, h_speed, v_dir, v_speed
|
linear = _clip_unit(_apply_dead_zone(linear, self.dead_zone))
|
||||||
|
angular = _clip_unit(_apply_dead_zone(angular, self.dead_zone))
|
||||||
|
|
||||||
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:
|
if self.debug:
|
||||||
print("Two wrists in TOP-CENTER ('UP' section) -> EXTINGUISHING FIRE")
|
print(f"[M3] L:{linear:.2f} A:{angular:.2f}")
|
||||||
return {'command': 'extinguishing fire'}
|
return linear, angular
|
||||||
|
|
||||||
# Otherwise, control with RIGHT wrist only
|
def draw_overlay(self, frame, landmarks=None):
|
||||||
right_wrist = self._get_wrist_position(landmarks, 'right')
|
if frame is None:
|
||||||
if right_wrist is not None:
|
return frame
|
||||||
x, y = right_wrist
|
if landmarks is not None:
|
||||||
h_dir, h_speed, v_dir, v_speed = self._compute_speeds(x, y)
|
for idx, color in [(13, (0, 200, 0)), (14, (0, 200, 0)), (15, (0, 255, 255)), (16, (255, 0, 255))]:
|
||||||
return {
|
if landmarks[idx][3] > 0.5:
|
||||||
'command': 'move',
|
x, y = int(landmarks[idx][0]), int(landmarks[idx][1])
|
||||||
'x': x, 'y': y,
|
cv2.circle(frame, (x, y), 6, color, -1)
|
||||||
'h_dir': h_dir, 'h_speed': h_speed,
|
return frame
|
||||||
'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:
|
class ArmControllerMethod4:
|
||||||
- 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':
|
Method 4: 3x3 grid based on landmark 19 (right index finger tip).
|
||||||
return 0.0, 0.0
|
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
|
||||||
|
|
||||||
v_dir, v_speed = cmd['v_dir'], float(cmd['v_speed'])
|
if frame_shape is None or landmarks is None:
|
||||||
h_dir, h_speed = cmd['h_dir'], float(cmd['h_speed'])
|
return 0.0, 0.0
|
||||||
|
if landmarks[self.finger_idx][3] < 0.5:
|
||||||
|
return 0.0, 0.0
|
||||||
|
|
||||||
# Normalize speeds to 0..1
|
h, w = int(frame_shape[0]), int(frame_shape[1])
|
||||||
v_norm = v_speed / float(self.max_speed) if self.max_speed > 0 else 0.0
|
x = landmarks[self.finger_idx][0]
|
||||||
h_norm = h_speed / float(self.max_speed) if self.max_speed > 0 else 0.0
|
y = landmarks[self.finger_idx][1]
|
||||||
|
|
||||||
# Linear: forward/backward
|
# Angular (horizontal): center band 2/5..3/5 = 0
|
||||||
if v_dir == 'up':
|
if 2 * w / 5 <= x <= 3 * w / 5:
|
||||||
linear = +v_norm * self.max_speed_linear
|
angular = 0.0
|
||||||
elif v_dir == 'down':
|
elif x > 3 * w / 5:
|
||||||
linear = -v_norm * self.max_speed_linear
|
angular = (x * 5) / (2 * w) - 1
|
||||||
else:
|
else:
|
||||||
linear = 0.0
|
angular = (x - 3 * w / 5) / (2 * w / 5)
|
||||||
|
|
||||||
# Angular: left/right
|
# Linear (vertical): center band 2/5..3/5 = 0
|
||||||
# Convention: left turn -> +angular, right turn -> -angular
|
if 2 * h / 5 <= y <= 3 * h / 5:
|
||||||
if h_dir == 'left':
|
linear = 0.0
|
||||||
angular = +h_norm * self.max_speed_angular
|
elif y > 3 * h / 5:
|
||||||
elif h_dir == 'right':
|
linear = -((y * 5) / (2 * h) - 1)
|
||||||
angular = -h_norm * self.max_speed_angular
|
|
||||||
else:
|
else:
|
||||||
angular = 0.0
|
linear = -(y - 3 * h / 5) / (2 * h / 5)
|
||||||
|
|
||||||
return float(linear), float(angular)
|
if self.mirror:
|
||||||
|
angular = -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:
|
if self.debug:
|
||||||
print("COMMAND: stop")
|
print(f"[M4] L:{linear:.2f} A:{angular:.2f}")
|
||||||
return frame
|
return _clip_unit(linear), _clip_unit(angular)
|
||||||
|
|
||||||
if cmd['command'] == 'extinguishing fire':
|
def draw_overlay(self, frame, landmarks=None):
|
||||||
cv2.putText(frame, "EXTINGUISHING FIRE (2 WRISTS IN TOP-CENTER 'UP' SECTION)", (20, 40),
|
if frame is None:
|
||||||
cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 165, 255), 2)
|
|
||||||
if self.debug:
|
|
||||||
print("COMMAND: extinguishing fire (two wrists in top-center section)")
|
|
||||||
return frame
|
return frame
|
||||||
|
|
||||||
# command == move (right wrist visible)
|
h, w = frame.shape[:2]
|
||||||
x, y = int(cmd['x']), int(cmd['y'])
|
x1, x2 = int(w * 2 / 5), int(w * 3 / 5)
|
||||||
cv2.circle(frame, (x, y), 12, (0, 255, 0), -1)
|
y1, y2 = int(h * 2 / 5), int(h * 3 / 5)
|
||||||
|
|
||||||
# Vector from center to wrist
|
# Grid lines
|
||||||
cv2.line(frame, (w // 2, h // 2), (x, y), (0, 255, 0), 2)
|
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)
|
||||||
|
|
||||||
v_dir, v_speed = cmd['v_dir'], cmd['v_speed']
|
# Highlight active cell + finger
|
||||||
h_dir, h_speed = cmd['h_dir'], cmd['h_speed']
|
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))
|
||||||
|
|
||||||
cv2.putText(frame, f"V: {v_dir.upper()} {v_speed}", (20, 40),
|
overlay = frame.copy()
|
||||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
|
cv2.rectangle(overlay, (cx0, cy0), (cx1, cy1), (0, 255, 255), -1)
|
||||||
cv2.putText(frame, f"H: {h_dir.upper()} {h_speed}", (20, 75),
|
frame = cv2.addWeighted(overlay, 0.2, frame, 0.8, 0)
|
||||||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
|
|
||||||
|
|
||||||
if self.debug:
|
cv2.circle(frame, (fx, fy), 8, (0, 255, 255), -1)
|
||||||
print(f"V: {v_dir} {v_speed} | H: {h_dir} {h_speed}")
|
cv2.circle(frame, (fx, fy), 12, (0, 120, 120), 2)
|
||||||
|
|
||||||
return frame
|
return frame
|
||||||
|
|||||||
Loading…
Reference in New Issue