def run(self):
self._init_motion()
while True:
ret, frame = self.cap.read()
r = self.detector.detectFromRGB(frame, return_area=True)
if r is not None:
obj_pos, obj_area = r
if self.debug:
cv2.circle(frame, (obj_pos[0], obj_pos[1]), 2, (0, 0, 255),
-1)
print(obj_pos, obj_area)
pan, tilt = self.centerHeadToPoint(obj_pos)
dm.headMoveByAngle(pan, tilt)
if self.debug:
cv2.imshow("Vision", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
def run(self):
self.init_motion()
while True:
ret, frame = self.cap.read()
pos = self._getObjPosFromBGR(frame)
if pos is not None:
cx, cy = pos
cv2.circle(frame, (cx, cy), 3, (0, 0, 255), 3)
pan, tilt = self.centerHeadToPoint((cx, cy))
dm.headMoveByAngle(pan, tilt)
if self.debug:
cv2.imshow("Vision", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
print("Ready.")
# Wait for the start button press
if btn == Button.START: state = States.FIND_TARGET
# Look for target
elif state == States.FIND_TARGET:
print("Find Target.")
# Get obj info from tracker
obj_info = tracker.getObj()
if obj_info is not None:
pos, _ = obj_info
p, t = tracker.centerHeadToPoint(pos)
dm.headMoveByAngle(p, t)
### STATE CHANGE STUFF ###
target_info = TargetInfo()
state = States.OBSERVE_TARGET
##########################
else:
cur_pan = dm.headGetPan()
cur_tilt = dm.headGetTilt()
if cur_pan > 120:
look_dir = 0
elif cur_pan < 0:
look_dir = 1
keys = pygame.key.get_pressed()
cur_pan = dm.headGetPan()
cur_tilt = dm.headGetTilt()
# Amount to increment in each angle
tilt_i = 0
pan_i = 0
mod = 1
if keys[K_LSHIFT]:
mod = 0.25
if keys[K_j]:
tilt_i += -1 * mod
if keys[K_u]:
tilt_i += +1 * mod
if keys[K_h]:
pan_i += 1 * mod
if keys[K_k]:
pan_i += -1 * mod
if keys[K_o]:
dm.headMoveToHome()
else:
dm.headMoveByAngle(cur_pan + pan_i, cur_tilt + tilt_i)
dm.walkSetVelocities(X_AMPLITUDE, Y_AMPLITUDE, A_AMPLITUDE)
print("X: {}, Y: {}, A: {}".format(X_AMPLITUDE, Y_AMPLITUDE, A_AMPLITUDE))
sleep(0.01)
from head_tracker import JuarezHeadTracker as JHT
MAX_P = 15
MAX_X_SPEED = 15 # Max linear velocity
UPDATE_RATE = 3 # Number of ticks
UPDATE_STEP = 0.5 # Increment in speed each update
tracker = JHT()
dm.initMotionManager()
# Walk ready pose
dm.playMotion(51)
dm.headMoveToHome()
dm.headMoveByAngle(0.0, 105.0)
dm.walkSetPeriodTime(580.0)
initial_xOffset = -8.0
dm.walkSetXOffset(initial_xOffset)
sleep(5)
dm.walkPrintParams()
def updateLinearSpeed(cur_speed):
if cur_speed >= MAX_X_SPEED:
return MAX_X_SPEED
else:
return cur_speed + UPDATE_STEP