#include <iostream>
#include <chrono>
#include <functional>
#include <memory>
#include <thread>
#include <unistd.h>
#include <vector>
#include "agvc_interface/rpc.h"
#include "agvc_interface/rtde.h"
#include "agvc_interface/agvc_interface.h"

// 添加站点
void addStationsTest(agvc_interface::RpcClientPtr agv)
{

    // 1. 释放控制权
    agv->releasePriority();

    // 2. 获取控制权
    agv->setPriority("user", "");

    // 3. 构造站点数据
    std::vector<agvc_interface::StationMark> stations;
    agvc_interface::StationMark station;

    station.header.id     = "add_station_1";
    station.header.map_id = "current_map";
    station.header.name   = "ST1";
    station.pose.x        = 0.0;
    station.pose.y        = 0.0;
    station.pose.yaw      = 0.0;
    station.type          = "ST";

    stations.push_back(station);

    // 4. 调用添加站点接口
    int ret = agv->addStations(stations);
    std::cout << "添加站点接口返回值: " << ret << std::endl;
}

int main()
{
    std::cout << "AGVC Add Station Test..." << std::endl;

    // 初始化
    auto agv = std::make_shared<agvc_interface::RpcClient>();
    agv->setRequestTimeout(100);
    agv->connect("192.168.192.100", 30104);
    agv->login("aa", "bb");

    // 添加站点
    addStationsTest(agv);

    return 0;
}
