def handle_look(self, pitch, yaw):
""" Publishes a series of movement_msg packets (1 every 0.5 seconds for
a maximum of 3 seconds) which transitions the current look direction to
the desired pitch and yaw as passed in the arguments. If the movement
was successful within 3 seconds the function returns True, otherwise it
returns False.
"""
timer = 0.
while timer < 3:
if self.in_range(self.yaw, yaw) and self.in_range(
self.pitch, pitch):
return True
frames = phy.get_look_frames(self.pos_to_dict(), pitch, yaw)
for frame in frames:
msg = movement_msg()
msg.x = frame['x']
msg.y = frame['y']
msg.z = frame['z']
msg.pitch = frame['pitch']
msg.yaw = frame['yaw']
self.pub_move.publish(msg)
timer += 0.5
rospy.sleep(0.5)
# if we have not reached correct position in 3 seconds
return False
def handle_relative_look(self, pitch, yaw):
"""Same as handle look but adds the values to the current look position.
"""
pos = self.pos_to_dict()
# TODO: Maybe we should add the pitch for consistency, even though -90
# is up and 90 is down.
desired_pitch = pos['pitch'] - pitch
desired_yaw = pos['yaw'] + yaw
timer = 0.
while timer < 3:
if self.in_range(self.yaw, desired_yaw) and self.in_range(
self.pitch, desired_pitch):
return True
if desired_pitch < -90:
desired_pitch = -90
elif desired_pitch > 90:
desired_pitch = 90
if desired_yaw > 180:
while desired_yaw > 180:
desired_yaw -= 360
elif desired_yaw < -180:
while desired_yaw < -180:
desired_yaw += 360
frames = phy.get_look_frames(pos, desired_pitch, desired_yaw)
for frame in frames:
msg = movement_msg()
msg.x = frame['x']
msg.y = frame['y']
msg.z = frame['z']
msg.pitch = frame['pitch']
msg.yaw = frame['yaw']
# print msg
self.pub_move.publish(msg)
timer += 0.5
rospy.sleep(0.5)
# if we have not reached correct position in 3 seconds
return False
def handle_relative_look(self, pitch, yaw):
pos = self.pos_to_dict()
desired_pitch = pos['pitch'] - pitch
desired_yaw = pos['yaw'] + yaw
timer = 0.
while timer < 3:
if self.inRange(self.yaw, desired_yaw) and self.inRange(
self.pitch, desired_pitch):
return True
if desired_pitch < -90:
desired_pitch = -90
elif desired_pitch > 90:
desired_pitch = 90
if desired_yaw > 180:
while desired_yaw > 180:
desired_yaw -= 360
elif desired_yaw < -180:
while desired_yaw < -180:
desired_yaw += 360
frames = phy.get_look_frames(pos, desired_pitch, desired_yaw)
for frame in frames:
msg = movement_msg()
msg.x = frame['x']
msg.y = frame['y']
msg.z = frame['z']
msg.pitch = frame['pitch']
msg.yaw = frame['yaw']
print msg
self.pub_move.publish(msg)
timer += 0.5
rospy.sleep(0.5)
# if we have not reached correct position in 3 seconds
return False
def handle_relative_look(self, pitch, yaw):
pos = self.pos_to_dict()
desired_pitch = pos['pitch'] - pitch
desired_yaw = pos['yaw'] + yaw
timer = 0.
while timer < 3:
if self.inRange(self.yaw, desired_yaw) and self.inRange(self.pitch, desired_pitch):
return True
if desired_pitch < -90:
desired_pitch = -90
elif desired_pitch > 90:
desired_pitch = 90
if desired_yaw > 180:
while desired_yaw > 180:
desired_yaw -= 360
elif desired_yaw < -180:
while desired_yaw < -180:
desired_yaw += 360
frames = phy.get_look_frames(pos, desired_pitch, desired_yaw)
for frame in frames:
msg = movement_msg()
msg.x = frame['x']
msg.y = frame['y']
msg.z = frame['z']
msg.pitch = frame['pitch']
msg.yaw = frame['yaw']
print msg
self.pub_move.publish(msg)
timer += 0.5
rospy.sleep(0.5)
# if we have not reached correct position in 3 seconds
return False
def handle_look(self, pitch, yaw):
timer = 0.
while timer < 3:
if self.inRange(self.yaw, yaw) and self.inRange(self.pitch, pitch):
return True
frames = phy.get_look_frames(self.pos_to_dict(), pitch, yaw)
for frame in frames:
msg = movement_msg()
msg.x = frame['x']
msg.y = frame['y']
msg.z = frame['z']
msg.pitch = frame['pitch']
msg.yaw = frame['yaw']
self.pub_move.publish(msg)
timer += 0.5
rospy.sleep(0.5)
# if we have not reached correct position in 3 seconds
return False
def handle_look(self, pitch, yaw):
timer = 0.
while timer < 3:
if self.inRange(self.yaw, yaw) and self.inRange(self.pitch, pitch):
return True
frames = phy.get_look_frames(self.pos_to_dict(), pitch, yaw)
for frame in frames:
msg = movement_msg()
msg.x = frame['x']
msg.y = frame['y']
msg.z = frame['z']
msg.pitch = frame['pitch']
msg.yaw = frame['yaw']
self.pub_move.publish(msg)
timer += 0.5
rospy.sleep(0.5)
# if we have not reached correct position in 3 seconds
return False
