コード例 #1
0
    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)
コード例 #2
0
ファイル: fastestpath.py プロジェクト: MDP-Group3/mdp-rpi
    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)