import time
from pyaubo_agvc_sdk import RpcClient, NavGoalType, NavType, NavInfo, NavStatus


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 nav(agv):
    # 抢夺控制权  
    agv.releasePriority()
    agv.setPriority("aa", "bb")
    
    nav_goal = NavGoalType()
    nav_goal.header.id = "test_nav_goal_1"  # 设置导航任务的id
    nav_goal.header.map_id = "current_map"   # 设置导航任务的地图id
    nav_goal.type = NavType.PATH_TO_STATION  # 设置导航任务类型 - 寻迹
    nav_goal.forward_flag = False            # 行驶方向
    nav_goal.goal_end_rotate = False         # 到终点是否旋转
    nav_goal.max_vel.v = 0.5                 # 最大线速度
    nav_goal.stations_id.append("add_station_2")
    nav_goal.stations_id.append("add_station_1")

    # 下发导航任务 
    agv.setNavGoal(nav_goal)
    time.sleep(1)

    # 判断任务状态 
    while True:
        nav_info = agv.getNavInfo()
        # 判断导航任务id，确定是否为本次任务
        if nav_info.header.id == "test_nav_goal_1":
            if nav_info.status == NavStatus.FINISHED:
                print("nav back finish!")
                break
            elif nav_info.status == NavStatus.FAILED:
                print("nav back failed!")
                break
            elif nav_info.status == NavStatus.RUNNING:
                print("nav back running!")
        time.sleep(1)

    # ================== 第二段导航 ==================
    nav_goal.header.id = "test_nav_goal_2"
    nav_goal.header.map_id = "current_map"
    nav_goal.type = NavType.PATH_TO_STATION
    nav_goal.forward_flag = True
    nav_goal.goal_end_rotate = False
    nav_goal.max_vel.v = 0.5
    nav_goal.stations_id.clear()
    nav_goal.stations_id.append("add_station_1")
    nav_goal.stations_id.append("add_station_2")

    # 下发导航任务
    agv.setNavGoal(nav_goal)
    time.sleep(1)

    # 判断任务状态
    while True:
        nav_info = agv.getNavInfo()
        if nav_info.header.id == "test_nav_goal_2":
            if nav_info.status == NavStatus.FINISHED:
                print("nav back finish!")
                break
            elif nav_info.status == NavStatus.FAILED:
                print("nav back failed!")
                break
            elif nav_info.status == NavStatus.RUNNING:
                print("nav back running!")
        time.sleep(1)


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