그래프 기반으로 작성한 상태천이 프로그램

1. 개요

문제 해결 방법 소개

 해결책으로 BFS 기반의 프로그램을 개발하여, 각 노드에서 적절한 행동을 수행하도록 구현했습니다. 이는 에상치 못한 상황에서도 일관성 있는 동작을 수행하도록 했습니다.

문제 해결의 필요성

 이전에는 시나리오 기반의 상태 전이 프로그램을 사용했었는데, 이는 외부 간섭이 발생하면 로봇이 미션을 수행하지 못하는 경우가 발생하였습니다. 이러한 문제를 해결하기 위해 문의 상태에 따라 다른 행동을 취하는 프로그램을 개발하게 되었습니다. 이로써 외부 간섭에도 더욱 견고한 시스템을 구현할 수 있게 되었습니다.

 

2. 문제 정의

문제의 내용 및 조건 설명

로봇이 ROLL-UP DOOR을 통과하기 위해서는 정해진 waypoint에 도착하면 ROLL-UP DOOR의 상태를 알아내기 위해 아두이노와 통신합니다. 예를 들어, 로봇이 waypoint에 도착하면 ROLL-UP DOOR의 아두이노와 연결되고, ROLL-UP DOOR의 상태가 열림인지, 닫힘인지, 비상정지 등의 상태를 전달받습니다.

 

ROLL-UP DOOR의 초기 상태를 전달 받으면, 4가지 CASE 중 한 가지 CASE로 특정할 수 있습니다. 예를 들어, 초기 상태가 CLOSE 상태라면 CASE 1로 특정할 수 있습니다.

 

이후에는 특정된 CASE의 플로우를 실행하여 ROLL-UP DOOR의 문을 제어하여 로봇이 통과할 수 있습니다. CASE 1을 예시로 들면, "문을 열어"라는 명령을 보낸 후, 잠시 대기하고 상승 limit 스위치를 이용해 문이 완전히 열렸다는 신호를 받으면 로봇이 ROLL-UP DOOR을 통과합니다.

 

문제의 제약 조건

• 운영 제약: 로봇은 문을 제어하여 통과해야만 하며, 그렇지 않을 경우 미션 실패로 이어지므로 모든 문을 반드시 통과해야 합니다.
• 하드웨어 제약: 일부 문은 예상한 대로 동작하지 않을 수 있으며, 예를 들어 비상 스위치를 해제 후 문 닫침 버튼을 눌러야 닫히는 문이나, 자동으로 닫히는 문이 있었습니다
• 외부 간섭 허용: 문을 열고 닫는 중, 사람이 지나가서 문의 상태가 변경되더라도 로봇은 문을 열거나/닫는 액션은 성공해야합니다.

3. 알고리즘 소개

알고리즘 개요

 이 알고리즘은 로봇이 ROLL-UP DOOR을 통과하기 위해 문의 상태를 관리하고, 현재 상태에서 목표 상태로 이동하기 위해 필요한 최소 비용 경로를 찾는 것을 해결하기 위해 작성된 코드입니다. 이를 위해 데익스트라 알고리즘을 사용하여 각 상황을 노드로 나누고, OPEN 상태와 CLOSE 상태일 때 수행하는 동작도 노드로 구성하여 현재 문의 상태에서 목표 문의 상태까지의 도달 가능성을 확인합니다. 이 코드는 보다 범용적으로 사용 가능한 코드입니다.

 

구현 방법

class DoorStateMachine():
    """DoorStateMachine 클래스는 문의 상태를 관리하고, 현재 상태에서 목표 상태로 이동하기 위해 필요한 최소 비용 경로를 찾습니다
    """

    def __init__(self, door) -> None:
        self.door = door
        self.door_map = {
            Status.EMERGENCY_OPENED: [Status.EMERGENCY_OPENED, Status.OPENED],
            Status.OPENED: [Status.EMERGENCY_OPENED, Status.OPENED, Status.IDLE],
            Status.IDLE: [Status.OPENED, Status.IDLE, Status.CLOSED],
            Status.CLOSED: [Status.EMERGENCY_CLOSED, Status.CLOSED, Status.IDLE],
            Status.EMERGENCY: [Status.EMERGENCY],
            Status.EMERGENCY_CLOSED: [Status.EMERGENCY_CLOSED]
        }
        self.route = []
        self.start_time = -1
        self.retry_count = 0

    def update(self):
        """statemachine 실행"""
        pass

    def find_route_to(self, curret, final):
        cost = self.get_cost_route(current_state=curret, target_state=final)
        return self.change_cost_to_route(cost, target_state=final)

    def get_cost_route(self, current_state, target_state):
        cost_arr = [987654321] * 10
        heqp = []

        heapq.heappush(heqp, (0, current_state))
        cost_arr[current_state] = 0

        #다익스트라
        while heqp:
            cost, status = heapq.heappop(heqp)

            if cost_arr[status] < cost:
                continue

            cost_arr[status] = cost
            if status == target_state:
                break

            candinate = self.door_map[status]

            for cand_status in candinate:
                if cost + 1 < cost_arr[cand_status]:
                    cost_arr[cand_status] = cost + 1
                    heapq.heappush(heqp, (cost_arr[cand_status], cand_status))

        return cost_arr

    def change_cost_to_route(self, dist, target_state):
        cost = dist[target_state]
        dist_arr = [int(target_state)]

        if cost == 987654321:
            return []

        #디버깅 용도
        door_status = [(int(v), str(v)) for v in Status]
        door_status = dict(door_status)

        while cost > 0:
            for idx, cand_dist in enumerate(dist):
                if cand_dist == cost - 1:
                    dist_arr.append(idx)
                    cost -= 1
                    break

        return list(reversed(dist_arr))

class DoorOpenCase(DoorStateMachine):

    def __init__(self, door) -> None:
        DoorStateMachine.__init__(self, door)
        self.door = door

    def update(self):
        """update 메소드는 현재 시간과 최대 재시도 횟수를 체크합니다. 만약 재시도 횟수가 최대 재시도 횟수보다 크거나,
           경과 시간이 ACTION_TIMER를 초과하면 실패 상태를 반환합니다.
           이후, 문의 현재 상태를 갱신하고, 상태 갱신이 실패하면 실패 상태를 반환합니다.
        Return:
            bool: Action result
        """
        if self.start_time == -1:
            self.start_time = time.time()

        if time.time() - self.start_time >= ACTION_TIMER or MAXIMUM_RETRY_COUNT < self.retry_count:
            return CaseStatus.FAILED

        #도어의 상태 정보를 갱신함
        result = self.door.request_status()

        if result == False:
            return CaseStatus.FAILED

        self.route = self.find_route_to(self.door.current_door_state, Status.OPENED)
        if not self.route:
            return CaseStatus.FAILED

        #타겟 스테이트
        if self.door.is_emergency_opened():
            return CaseStatus.SUCCESS

        if self.door.is_opened():
            if not self.wait_command_response(command="REQ_ESTOP_ON",
                                              target_state=Status.EMERGENCY_OPENED, wait_time=1):
                return CaseStatus.FAILED

        if self.door.is_closed():
            if not self.wait_command_response(command="REQ_COMMAND", target_state=Status.IDLE):
                return CaseStatus.FAILED

            #what is menaing
            self.start_time = -1
            self.retry_count += 1

        return self.door.convert_enum_to_string(self.door.current_door_state)

 

코드 설명 및 주요 기능 소개

이 코드는 문(Door)의 상태 변화를 다루는 상태 기계(State Machine)를 구현한 것입니다. 문은 다양한 상태(열림, 닫힘, 비상 상황 등)를 가지고 있으며, 이러한 상태 변화를 효율적으로 다루기 위해 상태 기계(State Machine)를 사용합니다.

 

상태 기계는 상태와 상태 변화를 정의하고, 각 상태에 따른 동작을 정의하여 시스템의 동작을 제어하는 프로그래밍 방식입니다. 이 코드는 문의 상태를 나타내는 열거형(Enum)을 사용하고, 다익스트라 알고리즘(Dijkstra Algorithm)을 이용하여 문의 상태를 변경할 수 있는 최소 비용 경로(Cost)를 찾아내는 기능을 구현합니다.

 

이 코드에서는 DoorStateMachine 클래스를 상속하여 DoorOpenCase 클래스와 DoorCloseCase 클래스를 구현하였습니다. DoorOpenCase 클래스는 문이 열릴 때 실행되며, DoorCloseCase 클래스는 문이 닫힐 때 실행됩니다. 각 클래스는 update() 메서드를 구현하여 문의 상태 변화를 처리하고, CaseStatus 클래스를 사용하여 처리 결과를 반환합니다. 이렇게 함으로써, 여러 가지 문제 상황에 대처할 수 있는 유연성을 가진 코드를 만들 수 있습니다.

 

 

3. 결론

알고리즘의 장점

• 다른 형태의 문에 이식하기가 굉장히 쉽고, 문의 상태가 늘어난다고 하더라도 유지보수 하기 쉬운 구조입니다.
• 여러 가지 문제 상황에 대처할 수 있는 유연성을 가질 수 있습니다

알고리즘의 단점

• 블루투스 신호를 주고 받는 과정에서는 응답 결과를 기다리는 지연 시간이 필요합니다. 이러한 대기 시간은 상수로 정의되어 있기 때문에, 프로그램의 강건성을 저해할 수 있습니다. 따라서 상황에 맞는 적절한 능동적 대기 시간을 사용해야 합니다.