def centerHeadToPoint(self, pos):
cur_pan = dm.headGetPan()
cur_tilt = dm.headGetTilt()
dist_x = self.IMG_WIDTH / 2. - pos[0]
dist_y = self.IMG_HEIGHT / 2. - pos[1]
if self.debug: print(dist_x, dist_y)
new_pan = cur_pan + (dist_x * self.STEP_SIZE)
new_tilt = cur_tilt + (dist_y * self.STEP_SIZE)
return new_pan, new_tilt
def centerHeadToPoint(pos, debug=False):
cur_pan = dm.headGetPan()
cur_tilt = dm.headGetTilt()
print("POSS:", pos)
dist_x = 320. - pos[0]
dist_y = 240. - pos[1]
if debug:
print(dist_x, dist_y)
print("Pan: {} Tilt: {}".format(cur_pan, cur_tilt))
new_pan = cur_pan + (dist_x * STEP_SIZE)
new_tilt = cur_tilt + (dist_y * STEP_SIZE)
return new_pan, new_tilt
# 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
if look_dir == 1:
dm.headMoveByAngle(cur_pan + LOOK_STEP, cur_tilt)
else:
dm.headMoveByAngle(cur_pan - LOOK_STEP, cur_tilt)
# Observe target motion to predict its position later
elif state == States.OBSERVE_TARGET:
print("Observe Target.")
pygame.init()
screen = pygame.display.set_mode((300, 300))
CONFIG_INI = "/home/juarez/Darwin-tools/Data/config.ini"
# Sets up all the Action, Walking, Head and Motion Manager
dm.initMotionManager(CONFIG_INI)
# Stand up in a nice motion
dm.playMotion(16)
dm.headMoveToHome()
while True:
# Get current values of pan and tilt
cur_pan = dm.headGetPan()
cur_tilt = dm.headGetTilt()
for event in pygame.event.get():
if event.type == QUIT:
sys.exit(1)
keys = pygame.key.get_pressed()
# Amount to increment in each angle
tilt_i = 0
pan_i = 0
mod = 1
if keys[K_LSHIFT]:
mod = 0.25
if keys[K_s]:
