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
""" 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.)