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

ip = "192.168.192.100"
agv_port = 30104
robot_port = 30004
rpc_name = "aa"
rpc_password = "bb"

def login():
    agv = RpcClient()
    agv.setRequestTimeout(10000)
    ret = agv.connect(ip, agv_port)
    print(f"connect result: {ret}")
    ret = agv.login(rpc_name, rpc_password)
    print(f"login result: {ret}")
    return agv


def nav(agv):
    # 抢夺控制权
    agv.release_priority()
    agv.set_priority(rpc_name, rpc_password)
    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  # 设置行驶方向 - 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.set_nav_goal(nav_goal)
    time.sleep(1)

    # 判断任务状态
    while True:
        nav_info = agv.get_nav_info()
        # 判断导航任务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!")
            break
        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.set_nav_goal(nav_goal)
    time.sleep(1)

    # 判断任务状态
    while True:
        nav_info = agv.get_nav_info()
        # 判断导航任务id，确定是否为本次任务
        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!")
            break
        time.sleep(1)


# 阻塞
def wait_arrival(robot_interface):
    max_retry_count = 5
    cnt = 0

    # 接口调用: 获取当前的运动指令 ID
    exec_id = robot_interface.getMotionControl().getExecId()

    # 等待机械臂开始运动
    while exec_id == -1:
        if cnt > max_retry_count:
            return -1
        time.sleep(0.05)
        cnt += 1
        exec_id = robot_interface.getMotionControl().getExecId()

    # 等待机械臂运动完成
    while robot_interface.getMotionControl().getExecId() != -1:
        time.sleep(0.05)

    return 0


# 先关节运动起始位置，然后循环依次以直线运动的方式经过3个路点
def example_movel(rpc_client):
    # 路点，用关节角来表示，单位: 弧度
    q = [0.0 * (math.pi / 180), -15.0 * (math.pi / 180), 100.0 * (math.pi / 180), 25.0 * (math.pi / 180),
         90.0 * (math.pi / 180), 0.0 * (math.pi / 180)]
    # 路点，用位姿来表示，形式[x,y,z,rx,ry,rz], 位置单位：米, 姿态单位：弧度
    pose1 = [0.551, -0.124, 0.419, -3.134, 0.004, 1.567]
    pose2 = [0.552, 0.235, 0.419, -3.138, 0.007, 1.567]
    pose3 = [0.551, -0.124, 0.261, -3.134, 0.0042, 1.569]

    # 接口调用：获取机器人的名字
    robot_name = rpc_client.getRobotNames()[0]

    robot_interface = rpc_client.getRobotInterface(robot_name)

    # 接口调用: 设置机械臂的速度比率
    robot_interface.getMotionControl().setSpeedFraction(0.75)

    # 接口调用: 设置工具中心点（TCP相对于法兰盘中心的偏移）
    tcp_offset = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    robot_interface.getRobotConfig().setTcpOffset(tcp_offset)

    # 关节运动到初始位置
    robot_interface.getMotionControl() \
        .moveJoint(q, 80 * (math.pi / 180), 60 * (math.pi / 180), 0, 0)

    # 阻塞
    ret = wait_arrival(robot_interface)
    if ret == 0:
        print("关节运动到初始位置成功")
    else:
        print("关节运动到初始位置失败")

    # 以直线运动的方式依次走3个路点
    # 直线运动
    robot_interface.getMotionControl() \
        .moveLine(pose1, 1.2, 0.25, 0, 0)

    # 阻塞
    ret = wait_arrival(robot_interface)
    if ret == 0:
        print("直线运动到路点1成功")
    else:
        print("直线运动到路点1失败")

    # 直线运动
    robot_interface.getMotionControl() \
        .moveLine(pose2, 1.2, 0.25, 0, 0)

    # 阻塞
    ret = wait_arrival(robot_interface)
    if ret == 0:
        print("直线运动到路点2成功")
    else:
        print("直线运动到路点2失败")

    # 直线运动
    robot_interface.getMotionControl() \
        .moveLine(pose3, 1.2, 0.25, 0, 0)

    # 阻塞
    ret = wait_arrival(robot_interface)
    if ret == 0:
        print("直线运动到路点3成功")
    else:
        print("直线运动到路点3失败")


if __name__ == "__main__":
    agv_client = login()
    robot_rpc_client = pyaubo_sdk.RpcClient()
    robot_rpc_client.setRequestTimeout(1000)  # 接口调用: 设置 RPC 超时
    robot_rpc_client.connect(ip, robot_port)  # 接口调用: 连接 RPC 服务
    if robot_rpc_client.hasConnected():
        print("RPC客户端连接成功!")
        robot_rpc_client.login("aubo", "123456")  # 接口调用: 登录
        nav(agv_client)
        example_movel(robot_rpc_client)  # 先关节运动起始位置，然后以直线运动的方式经过3个路点
        robot_rpc_client.logout()  # 退出登录
    robot_rpc_client.disconnect()  # 断开连接
    agv_client.disconnect()
