from pyaubo_agvc_sdk import RpcClient, StationMark, PathStation, PathShape


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 editMap(agv):
    # 抢夺控制权
    agv.releasePriority()
    agv.setPriority("aa", "bb")

    # 添加站点
    stations = []
    station1 = StationMark()
    station2 = StationMark()

    station1.header.id = "add_station_1"  # 站点1的id
    station1.header.map_id = "current_map"  # 站点1所在地图id，默认使用当前地图
    station1.header.name = "ST1"  # 站点1的名称
    station1.pose.x = 0.0  # 站点1的x坐标
    station1.pose.y = 0.0  # 站点1的y坐标
    station1.pose.yaw = 0.0  # 站点1的yaw角
    station1.type = "ST"  # 站点1的类型 "ST"为普通站点，“CP”为充电站点
    stations.append(station1)

    station2.header.id = "add_station_2"
    station2.header.map_id = "current_map"
    station2.header.name = "ST2"
    station2.pose.x = 1.0
    station2.pose.y = 0.0
    station2.pose.yaw = 0.0
    station2.type = "ST"
    stations.append(station2)

    # ===================== 关键修改：接收并打印返回值 =====================
    ret_add = agv.addStations(stations)
    print(f"addStations result: {ret_add}")

    # 添加路径
    path = PathStation()

    path.header.id = "new_path_1-2"  # 新路径id
    path.header.map_id = "current_map"  # 路径所在地图
    path.header.name = "path1-2"  # 路径名称
    path.start_station_id = "add_station_1"  # 路径起始站点id
    path.end_station_id = "add_station_2"  # 路径终止站点id
    path.shape = PathShape.LINE  # 路径类型 - 直线
    path.use_direction = False  # 是否考虑站点方向生成路径
    path.max_speed = 1.0  # 路径的最大允许运行速度

    # ===================== 关键修改：接收并打印返回值 =====================
    ret_gen = agv.generatePath(path)
    print(f"generatePath result: {ret_gen}")


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