Ejemplo n.º 1
0
 def __find_temporary_target(scan, drive_distance, drive_angle, location,
                             current_target):
     best_current_temporary_distance = None
     best_temporary_x, best_temporary_y = None, None
     for angle, distance in sorted(scan.points, key=lambda (a, _): a):
         if distance > drive_distance:
             for _d in logic.drange(ROBO_WIDTH, distance, 100.0):
                 temporary_x, temporary_y = logic.convert_polar_to_grid(
                     _d - 1.5 * ROBO_WIDTH, angle + location.angle)
                 if DriveToPoint.__check_temporary_target(
                         scan, (temporary_x, temporary_y)):
                     temporary_x, temporary_y = temporary_x / 1000.0 + location.x, temporary_y / 1000.0 + location.y
                     current_temporary_distance = math.sqrt(
                         math.pow(current_target[0] - temporary_x, 2.0) +
                         math.pow(current_target[1] - temporary_y, 2.0))
                     condition = abs(temporary_x - location.x) > abs(current_target[0] - location.x) or \
                                 abs(temporary_y - location.y) > abs(current_target[1] - location.y)
                     if (best_current_temporary_distance is None or
                                 current_temporary_distance < best_current_temporary_distance) and \
                             condition:
                         best_temporary_x, best_temporary_y = temporary_x, temporary_y
                         best_current_temporary_distance = current_temporary_distance
         if best_temporary_x is not None and best_temporary_y is not None:
             return best_temporary_x, best_temporary_y, ROBO_WIDTH * 1.3 / 1000.0
         return None
 def __check_temporary_target(scan, point):
     for alpha in logic.drange(0, 1.5707963267948966, 0.17453292519943295):
         x, y = logic.convert_polar_to_grid(ROBO_WIDTH * 0.35, alpha)
         _a, _d = logic.convert_grid_to_polar(x + point[0], y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
         _a, _d = logic.convert_grid_to_polar(-x + point[0], y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
         _a, _d = logic.convert_grid_to_polar(x + point[0], -y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
         _a, _d = logic.convert_grid_to_polar(-x + point[0], -y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
     return True
Ejemplo n.º 3
0
 def __check_temporary_target(scan, point):
     for alpha in logic.drange(0, 1.5707963267948966, 0.17453292519943295):
         x, y = logic.convert_polar_to_grid(ROBO_WIDTH * 0.35, alpha)
         _a, _d = logic.convert_grid_to_polar(x + point[0], y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
         _a, _d = logic.convert_grid_to_polar(-x + point[0], y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
         _a, _d = logic.convert_grid_to_polar(x + point[0], -y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
         _a, _d = logic.convert_grid_to_polar(-x + point[0], -y + point[1])
         s_d = drive_support_logic.get_distance(scan, _a)
         if 100.0 < s_d < _d:
             return False
     return True
 def __find_temporary_target(scan, drive_distance, drive_angle, location, current_target):
     best_current_temporary_distance = None
     best_temporary_x, best_temporary_y = None, None
     for angle, distance in sorted(scan.points, key=lambda (a, _): a):
         if distance > drive_distance:
             for _d in logic.drange(ROBO_WIDTH, distance, 100.0):
                 temporary_x, temporary_y = logic.convert_polar_to_grid(_d - 1.5 * ROBO_WIDTH,
                                                                        angle + location.angle)
                 if DriveToPoint.__check_temporary_target(scan, (temporary_x, temporary_y)):
                     temporary_x, temporary_y = temporary_x / 1000.0 + location.x, temporary_y / 1000.0 + location.y
                     current_temporary_distance = math.sqrt(math.pow(current_target[0] - temporary_x, 2.0) +
                                                            math.pow(current_target[1] - temporary_y, 2.0))
                     condition = abs(temporary_x - location.x) > abs(current_target[0] - location.x) or \
                                 abs(temporary_y - location.y) > abs(current_target[1] - location.y)
                     if (best_current_temporary_distance is None or
                                 current_temporary_distance < best_current_temporary_distance) and \
                             condition:
                         best_temporary_x, best_temporary_y = temporary_x, temporary_y
                         best_current_temporary_distance = current_temporary_distance
         if best_temporary_x is not None and best_temporary_y is not None:
             return best_temporary_x, best_temporary_y, ROBO_WIDTH * 1.3 / 1000.0
         return None