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
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
