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 __drive_to(self, target, next_targets_timestamp):
self.__logger.info('Drive to %s', str(target))
location = self.__locator.get_absolute_location()
start_time = time.time()
while not DriveToPoint.target_reached(location, target) and \
self.__driving_allowed and self.__is_active and \
not self.__next_targets_timestamp > next_targets_timestamp and \
start_time + MAXIMUM_TIME_DRIVE_TO > time.time():
drive_angle, drive_distance = DriveToPoint.__compute_drive_angle_distance(
location, target)
if drive_angle is not None and drive_distance is not None:
scan = self.__scan
scan_distance = drive_support_logic.get_distance(
scan, drive_angle)
if drive_distance > scan_distance:
temporary_target = DriveToPoint.__find_temporary_target(
scan, drive_distance, drive_angle, location, target)
self.__logger.warn('Try to drive to temporary target: %s',
str(temporary_target))
drive_angle, drive_distance = DriveToPoint.__compute_drive_angle_distance(
location, temporary_target)
if drive_angle is not None and drive_distance is not None:
self.__logger.info('Drive angle: %f, distance: %f',
drive_angle, drive_distance)
self.__send_commands(drive_angle, drive_distance)
else:
self.__send_commands(drive_angle, drive_distance)
self.__logger.info('Drive angle: %f, distance: %f',
drive_angle, drive_distance)
time.sleep(0.07)
location = self.__locator.get_absolute_location()
if start_time + MAXIMUM_TIME_DRIVE_TO < time.time():
self.__logger.warn('Target %s not reachable', str(target))
else:
self.__logger.info('Target %s reached', str(target))
def __drive_to(self, target, next_targets_timestamp):
self.__logger.info('Drive to %s', str(target))
location = self.__locator.get_absolute_location()
start_time = time.time()
while not DriveToPoint.target_reached(location, target) and \
self.__driving_allowed and self.__is_active and \
not self.__next_targets_timestamp > next_targets_timestamp and \
start_time + MAXIMUM_TIME_DRIVE_TO > time.time():
drive_angle, drive_distance = DriveToPoint.__compute_drive_angle_distance(location, target)
if drive_angle is not None and drive_distance is not None:
scan = self.__scan
scan_distance = drive_support_logic.get_distance(scan, drive_angle)
if drive_distance > scan_distance:
temporary_target = DriveToPoint.__find_temporary_target(scan, drive_distance, drive_angle,
location, target)
self.__logger.warn('Try to drive to temporary target: %s', str(temporary_target))
drive_angle, drive_distance = DriveToPoint.__compute_drive_angle_distance(location,
temporary_target)
if drive_angle is not None and drive_distance is not None:
self.__logger.info('Drive angle: %f, distance: %f', drive_angle, drive_distance)
self.__send_commands(drive_angle, drive_distance)
else:
self.__send_commands(drive_angle, drive_distance)
self.__logger.info('Drive angle: %f, distance: %f', drive_angle, drive_distance)
time.sleep(0.07)
location = self.__locator.get_absolute_location()
if start_time + MAXIMUM_TIME_DRIVE_TO < time.time():
self.__logger.warn('Target %s not reachable', str(target))
else:
self.__logger.info('Target %s reached', str(target))
