import time
from pyaubo_agvc_sdk import RpcClient, Pose2d, Header, ChangeRunningModeStatus, RunningInfo, RelocationStatus


def login():
    ip = "192.168.192.100"
    port = 30104
    rpc_name = "aa"
    rpc_password = "bb"
    agv = RpcClient()
    agv.setRequestTimeout(10000)

    ret = agv.connect(ip, port)
    print(f"connect result: {ret}")
    ret = agv.login(rpc_name, rpc_password)
    print(f"login result: {ret}")
    return agv


def relocate(agv):
    # 抢夺控制权 
    agv.releasePriority()
    agv.setPriority("aa", "bb")
    
    # 查看定位是否丢失 
    running_info = agv.getRunningInfo()
    if running_info.localization_loss:
        # 设置重定位位置参数
        init_pose = Pose2d()
        command_header = Header()
        init_pose.x = 0.0
        init_pose.y = 0.0
        init_pose.yaw = 0.0
        command_header.id = "test_relocation"
        command_header.map_id = "current_map"
        command_header.name = "test_map"

        # 调用重定位接口 
        ret = agv.relocation(init_pose, command_header)
        print(f"relocation 调用返回值(int): {ret}")

        # 判断是否重定位成功
        while True:
            # 获取异步状态
            mode_status = agv.getAsyncInterfaceResultStatus()
            print(f"mode status id: {mode_status.relocation_status.header.id}")
            
            if mode_status.relocation_status.header.id == command_header.id:
                if mode_status.relocation_status.status == RelocationStatus.FINISHED:
                    ret = agv.relocation(init_pose, command_header)
                    print(f"relocation final result: {ret}")
                    break
            time.sleep(1)


if __name__ == "__main__":
    agv = login()
    relocate(agv)
    agv.disconnect()
