def execute_path_exploration(self, path, goal_pos):
_output_string = []
_temp_block = path.pop()
_ori_target = None
_temp_robot = Robot(self._robot.get_pos(), False)
_temp_robot.set_ori(self._robot.get_ori())
_temp_robot.set_speed(0)
while _temp_robot.get_pos()[0] != goal_pos[0] or _temp_robot.get_pos(
)[1] != goal_pos[1]:
if _temp_robot.get_pos()[0] == _temp_block.get_pos(
)[0] and _temp_robot.get_pos()[1] == _temp_block.get_pos()[1]:
_temp_block = path.pop()
_ori_target = self.get_target_ori(_temp_robot.get_pos(),
_temp_robot.get_ori(),
_temp_block)
_action = None
if _temp_robot.get_ori() != _ori_target:
_action = self.get_target_move(_temp_robot.get_ori(),
_ori_target)
else:
_action = Action.FORWARD
print('Action {} from {} to {}'.format(_action.value,
_temp_robot.get_pos(),
_temp_block.get_pos()))
_temp_robot.move(_action)
self._actions.append(_action)
_output_string.append(_action.value)
for _act in self._actions:
if _act == Action.FORWARD:
if not self.can_move_forward():
print("Fastest Path Execution terminated!")
return "T"
self._robot.move(_act)
self._robot.notify(_act.value, self._map, True)
self._robot.set_sensor()
self._robot.sense(self._map) #, self._real_map)
if self._surface is not None:
self._map.draw(self._surface)
return ''.join(_output_string)
def execute_path(self, path, goal_pos):
_output_string = []
_temp_block = path.pop()
_ori_target = None
_actions = []
_temp_robot = Robot(self._robot.get_pos(), False)
_temp_robot.set_ori(self._robot.get_ori())
_temp_robot.set_speed(0)
while _temp_robot.get_pos()[0] != goal_pos[0] or _temp_robot.get_pos(
)[1] != goal_pos[1]:
if _temp_robot.get_pos()[0] == _temp_block.get_pos(
)[0] and _temp_robot.get_pos()[1] == _temp_block.get_pos()[1]:
_temp_block = path.pop()
_ori_target = self.get_target_ori(_temp_robot.get_pos(),
_temp_robot.get_ori(),
_temp_block)
_action = None
if _temp_robot.get_ori() != _ori_target:
_action = self.get_target_move(_temp_robot.get_ori(),
_ori_target)
else:
_action = Action.FORWARD
print('Action {} from {} to {}'.format(_action.value,
_temp_robot.get_pos(),
_temp_block.get_pos()))
_temp_robot.move(_action)
_actions.append(_action)
_output_string.append(_action.value)
if not self._robot.is_actual_robot():
for _act in _actions:
if _act == Action.FORWARD:
if not self.can_move_forward():
print("Fastest Path Execution terminated!")
return "T"
self._robot.move(_act)
self._robot.notify(_act, self._map, True)
if self._exploration_mode:
self._robot.set_sensor()
self._robot.sense(self._map) #, self._real_map)
if self._surface is not None:
self._map.draw(self._surface)
else:
_fCount = 0
for _act in _actions:
if _act == Action.FORWARD:
_fCount += 1
if _fCount == 9:
self._robot.move_multiple(_fCount)
_fCount == 0
_command = 'F' + str(_fCount)
self._robot.notify(_command, self._map, True)
elif _act == Action.RIGHT or _act == Action.LEFT:
if _fCount > 0:
self._robot.move_multiple(_fCount)
_fCount == 0
_command = 'F' + str(_fCount)
self._robot.notify(_command, self._map, True)
self._robot.move(_act)
self._robot.notify(_act, self._map, True)
if _fCount > 0:
self._robot.move_multiple(_fCount)
_fCount == 0
_command = 'F' + str(_fCount)
self._robot.notify(_command, self._map, True)
print('Actions: {}'.format(''.join(_output_string)))
return ''.join(_output_string)