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_move(self, direction, dist, jump):
speed = 1
pos = self.pos_to_dict()
desired_yaw = direction
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_movement_frames(pos, direction, dist, speed, jump)
# print "dx,dy,dz", dx, dy, dz
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']
msg.jump = jump
# print "rel_move_msg", msg
self.pub_move.publish(msg)
return True
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_move(self, x, z, jump):
speed = 1
pos = self.pos_to_dict()
direction = self.get_desired_yaw(pos['x'], pos['z'], x, z)
dist = sqrt((x - pos['x'])**2 + (z - pos['z'])**2)
frames = phy.get_movement_frames(pos, direction, dist, speed, jump)
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']
msg.jump = jump
# print "abs_move_msg", msg
self.pub_move.publish(msg)
return True
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
"""
move around in a triangle
<-25, 13, -40>
<-25, 13, -60>
<-10 , 13, -50>
"""
positions = [
(-25., 13., -40.),
(-25., 13., -60.),
(-10., 13., -50.)]
while not rospy.is_shutdown():
message = movement_msg()
for coords in positions:
message.jump = False
message.speed = 1
message.x = coords[0]
message.y = coords[1]
message.z = coords[2]
mv_pub.publish(message)
print ("sent command: %2f, %2f, %2f") %(message.x, message.y, message.z)
rospy.sleep(8.)
