modularization/src/test/testipgroad/main.cpp

#include <QCoreApplication>

#include <QProcess>

#include "stdarg.h"

#include "road.h"
#include "roadbuilder.h"

#include <iostream>

#include <getopt.h>

#include <iostream>

#include "rd5route.h"


static char gstr_inputpath[256];
static char gstr_outputpath[256];

static char gstr_fromroad[256];
static char gstr_fromlane[256];

static char gstr_toroad[256];
static char gstr_tolane[256];

/**
 * @brief print_useage
 */
void print_useage()
{
    std::cout<<" -i --input $xodrfile : set  input file path. eq.  -i d:/lk.xodr"<<std::endl;
    std::cout<<" -o --output $outputfile : set output file. eq.  -o d:/test.rd5"<<std::endl;
    std::cout<<" -h --help print help"<<std::endl;
}

int  GetOptLong(int argc, char *argv[]) {
    int nRtn = 0;
    int opt; // getopt_long() 的返回值
    int digit_optind = 0; // 设置短参数类型及是否需要参数
    (void)digit_optind;

    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
    // 第几个元素的描述，即是long_opts的下标值
    int option_index = 0;
    // 设置短参数类型及是否需要参数
    const char *optstring = "i:o:a:b:c:d:h";

    // 设置长参数类型及其简写，比如 --reqarg <==>-r
    /*
    struct option {
             const char * name;  // 参数的名称
             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
        };
    其中：
        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
     */
    static struct option long_options[] = {
        {"input", required_argument, NULL, 'v'},
        {"output", required_argument, NULL, 'o'},
        {"fromroad", required_argument, NULL, 'a'},
        {"fromlane", required_argument, NULL, 'b'},
        {"toroad", required_argument, NULL, 'c'},
        {"tolane", required_argument, NULL, 'd'},
        {"help",  no_argument,       NULL, 'h'},
 //       {"optarg", optional_argument, NULL, 'o'},
        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
    };

    while ( (opt = getopt_long(argc,
                               argv,
                               optstring,
                               long_options,
                               &option_index)) != -1) {
//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
//        printf("optarg = %s\n", optarg); // 参数内容
//        printf("optind = %d\n", optind); // 下一个被处理的下标值
//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
//        printf("\n");
        switch(opt)
        {
        case 'i':
            strncpy(gstr_inputpath,optarg,255);
            break;
        case 'o':
            strncpy(gstr_outputpath,optarg,255);
            break;
        case 'a':
            strncpy(gstr_fromroad,optarg,255);
            break;
        case 'b':
            strncpy(gstr_fromlane,optarg,255);
            break;
        case 'c':
            strncpy(gstr_toroad,optarg,255);
            break;
        case 'd':
            strncpy(gstr_tolane,optarg,255);
            break;
        case 'h':
            print_useage();
            nRtn = 1; //because use -h
            break;
        default:
            break;
        }

    }

    return nRtn;
}




int
ipgErrorExit(tRoad *road)
{
    int nErrMsg;
    tRoadMessageList msgList;

    nErrMsg = RoadGetMessageList(road, RMT_Error, &msgList);
    while (--nErrMsg>=0)  fprintf(stderr, "%s\n", msgList.msg[nErrMsg].msg);
    RoadDeleteMessageList(road,&msgList);
    RoadClearMessageBuffer(road, RMT_Error);

    return ROAD_Error;
}

void
ipgWarnMsg(tRoad *road)
{
    int nWarn;
    tRoadMessageList msgList;

    nWarn = RoadGetMessageList(road, RMT_Warn, &msgList);
    while (--nWarn>=0)  fprintf(stderr, "%s\n", msgList.msg[nWarn].msg);
    RoadDeleteMessageList(road,&msgList);
    RoadClearMessageBuffer(road, RMT_Warn);
}

int
createRoadFile (tRoad *road, char* fname)
{
    tObjId lObjId0,lObjId1,lObjId2, lsObjId, jObjId, segObjId, jArmObjId0,jArmObjId1,jArmObjId2;
    tRoadLink rLink;
    tRoadJunction rJunc;
    tRoadJunctionArm rJArm;
    tRoadSegment rSeg;
    tRoadLaneSection rLS;
    tRoadLane rLane,rBorderLane;
    tRoadZP rZP;
    double angle,length;


    // Creating the first Link
    memset(&rLink,0,sizeof(rLink));

    // The start (position and angle) is defined explicitly by rLink.xyz0={0,0,0} and rLink.angle0=0
    rLink.xyz0[0]  = 0;
    rLink.xyz0[1]  = 0;
    rLink.xyz0[2]  = 0;
    rLink.angle0   = 0;
    rLink.rst      = RST_Countryroad;
    rLink.material = NULL;

    if ((lObjId0=RoadAddLink(road,&rLink))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Creating the Reference line of the Link by adding a list of Segments
    memset(&rSeg,0,sizeof(rSeg));

    rSeg.type     = RST_Straight;
    rSeg.param[0] = 50;

    if ((segObjId=RoadAddSegment(road,lObjId0,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    rSeg.type     = RST_ClothRight;
    rSeg.param[0] = 1000;
    rSeg.param[1] = 100;
    rSeg.param[2] = 20;

    if ((segObjId=RoadAddSegment(road,lObjId0,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    rSeg.type     = RST_TurnRight;
    rSeg.param[0] = 100;
    rSeg.param[1] = 15;
    rSeg.param[2] = 0;

    if ((segObjId=RoadAddSegment(road,lObjId0,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    rSeg.type     = RST_ClothRight;
    rSeg.param[0] = 100;
    rSeg.param[1] = 1000;
    rSeg.param[2] = 20;

    if ((segObjId=RoadAddSegment(road,lObjId0,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    rSeg.type     = RST_Straight;
    rSeg.param[0] = 15;
    rSeg.param[1] = 0;
    rSeg.param[2] = 0;

    if ((segObjId=RoadAddSegment(road,lObjId0,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Creating an Elevation profile
    memset(&rZP,0,sizeof(rZP));

    rZP.isAbsolute = 0;
    rZP.n = 6;
    rZP.node = (tRoadNode1D*) calloc(rZP.n,sizeof(tRoadNode1D));
    rZP.node[0].type = RTT_Cubic;
    rZP.node[0].s    = 0;
    rZP.node[0].c[0] = 0;
    rZP.node[0].c[1] = 0;
    rZP.node[1].s    = 15;
    rZP.node[1].c[0] = -0.03;
    rZP.node[1].c[1] = ROAD_UNDEFINED;
    rZP.node[2].s    = 50;
    rZP.node[2].c[0] = 0.5;
    rZP.node[2].c[1] = ROAD_UNDEFINED;
    rZP.node[3].s    = 80;
    rZP.node[3].c[0] = 0.0;
    rZP.node[3].c[1] = ROAD_UNDEFINED;
    rZP.node[4].lonR = RLR_Node1; //1;
    rZP.node[4].s    = 50;
    rZP.node[4].c[0] = -0.5;
    rZP.node[4].c[1] = ROAD_UNDEFINED;
    rZP.node[5].lonR =RLR_Node1;//  1;
    rZP.node[5].s    = 5;
    rZP.node[5].c[0] = -1.5;
    rZP.node[5].c[1] = ROAD_UNDEFINED;
    rZP.smooth = 0.0;

    if (RoadAddZProfile(road,lObjId0, ROT_LonZPElevation,&rZP)==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    free(rZP.node);

    // Adding a Lane section to the Link
    memset(&rLS,0,sizeof(rLS));

    rLS.s = 0;

    if ((lsObjId=RoadAddLaneSection(road,lObjId0,&rLS))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Update of the internal road structure
    if (RoadBuilderUpdate(road)<0)
        return ipgErrorExit(road);

    // Adding a border lane on both sides
    memset(&rBorderLane,0,sizeof(rBorderLane));

    rBorderLane.type      = RTT_Cubic;
    rBorderLane.w0        = 0.5;
    rBorderLane.w1        = 0.5;
    rBorderLane.attr.type = RLT_Border;

    if (RoadAddLane(road,lsObjId,R_RIGHT,&rBorderLane,ROAD_InvalidObjId)==ROAD_InvalidObjId)
        return ipgErrorExit(road);
    if (RoadAddLane(road,lsObjId,R_LEFT, &rBorderLane,ROAD_InvalidObjId)==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Getting the length of the Link
    length = RoadGetLinkLength(road,lObjId0);

    // Adding a new Lane section at the end of the Link
    rLS.s = length-50;

    if ((lsObjId=RoadAddLaneSection(road,lObjId0,&rLS))==ROAD_InvalidObjId)
         return ipgErrorExit(road);

    // Adding Lanes to the new Lane section
    // First delete all default Lanes of this Lane section
    RoadDeleteAllLSLanes(road,lsObjId);

    memset(&rLane,0,sizeof(rLane));

    rLane.type      = RTT_Cubic;
    rLane.w0        = 0;
    rLane.w1        = 4;
    rLane.attr.type = RLT_Road;
    rLane.n         = 1;
    rLane.width = (tRoadNode1D*) calloc(rLane.n,sizeof(tRoadNode1D));
    rLane.width[0].lonR = RLR_Node0;
    rLane.width[0].s    = 25;
    rLane.width[0].type = RTT_Cubic;
    rLane.width[0].c[0] = 4;

    if (RoadAddLane(road,lsObjId,R_RIGHT,&rLane,ROT_Invalid)==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    free(rLane.width);

    rLane.n     = 0;
    rLane.width = NULL;
    rLane.w0    = 3.5;
    rLane.w1    = 3.5;

    if (RoadAddLane(road,lsObjId,R_RIGHT,&rLane,ROT_Invalid)==ROAD_InvalidObjId)
        return ipgErrorExit(road);
    if (RoadAddLane(road,lsObjId,R_LEFT, &rLane,ROT_Invalid)==ROAD_InvalidObjId)
        return ipgErrorExit(road);
    if (RoadAddLane(road,lsObjId,R_RIGHT,&rBorderLane,ROT_Invalid)==ROAD_InvalidObjId)
        return ipgErrorExit(road);
    if (RoadAddLane(road,lsObjId,R_LEFT, &rBorderLane,ROT_Invalid)==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Update of the internal road structure
    if (RoadBuilderUpdate(road)<0)
        return ipgErrorExit(road);


    RoadGetLink(road,lObjId0,&rLink);
    angle = RoadGetLinkHeading(road,lObjId0,length);
    angle = angle>180 ? angle-180 : angle+180;

    // Preparing to add a Junction to the end of the first Link
    memset(&rJunc,0,sizeof(rJunc));

    // The knot is defined explicitly using the position of the first Link's end
    rJunc.rjt      = RJT_Simple;
    rJunc.knot[0]  = rLink.xyz1[0];
    rJunc.knot[1]  = rLink.xyz1[1];
    rJunc.knot[2]  = rLink.xyz1[2];
    rJunc.rst      = RST_Urban;


    // Creating the second Link
    memset(&rLink,0,sizeof(rLink));

    // The start (position and angle) is defined explicitly using the position of the Junction's knot
    rLink.xyz0[0]  = rJunc.knot[0];
    rLink.xyz0[1]  = rJunc.knot[1];
    rLink.xyz0[2]  = rJunc.knot[2];
    rLink.angle0   = angle<200 ? angle+160 : angle+160-360;
    rLink.rst      = RST_Countryroad;
    rLink.material = NULL;

    if ((lObjId1=RoadAddLink(road,&rLink))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Creating the Reference line of the Link by adding a list of Segments
    memset(&rSeg,0,sizeof(rSeg));

    rSeg.type     = RST_Straight;
    rSeg.param[0] = 50;

    if ((segObjId=RoadAddSegment(road,lObjId1,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Adding a Lane section to the Link
    memset(&rLS,0,sizeof(rLS));

    rLS.s = 0;

    if ((lsObjId=RoadAddLaneSection(road,lObjId1,&rLS))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Update of the internal road structure
    if (RoadBuilderUpdate(road)<0)
        return ipgErrorExit(road);

    // Adding a border lane on both sides
    if (RoadAddLane(road,lsObjId,R_RIGHT,&rBorderLane,ROAD_InvalidObjId)==ROAD_InvalidObjId)
        return ipgErrorExit(road);
    if (RoadAddLane(road,lsObjId,R_LEFT,&rBorderLane,ROAD_InvalidObjId)==ROAD_InvalidObjId)
        return ipgErrorExit(road);


    // Creating the third Link
    memset(&rLink,0,sizeof(rLink));

    // The start (position and angle) is defined explicitly using the position of the Junction's knot
    rLink.xyz0[0]  = rJunc.knot[0];
    rLink.xyz0[1]  = rJunc.knot[1];
    rLink.xyz0[2]  = rJunc.knot[2];
    rLink.angle0   = angle>70  ? angle-70 : angle-70+360;
    rLink.rst      = RST_Countryroad;
    rLink.material = NULL;

    if ((lObjId2=RoadAddLink(road,&rLink))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Creating the Reference line of the Link by adding a list of Segments
    memset(&rSeg,0,sizeof(rSeg));

    rSeg.type     = RST_Straight;
    rSeg.param[0] = 50;

    if ((segObjId=RoadAddSegment(road,lObjId2,&rSeg,ROAD_InvalidObjId))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Adding a Lane section to the Link
    memset(&rLS,0,sizeof(rLS));

    rLS.s = 0;

    if ((lsObjId=RoadAddLaneSection(road,lObjId2,&rLS))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Update of the internal road structure
    if (RoadBuilderUpdate(road)<0)
        return ipgErrorExit(road);

    // Adding a border lane on both sides
    if (RoadAddLane(road,lsObjId,R_RIGHT,&rBorderLane,ROAD_InvalidObjId)==ROAD_InvalidObjId)
        return ipgErrorExit(road);
    if (RoadAddLane(road,lsObjId,R_LEFT,&rBorderLane,ROAD_InvalidObjId)==ROAD_InvalidObjId)
        return ipgErrorExit(road);


    // Adding a Junction to connect the three Links
    if ((jObjId=RoadAddJunction(road,&rJunc))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    // Adding Junction arms to the Junction
    memset(&rJArm,0,sizeof(rJArm));

    rJArm.lengthOnLink      = 10;   // Defines the junction entry distance

    if ((jArmObjId0=RoadAddJunctionArm(road,jObjId,&rJArm))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    if ((jArmObjId1=RoadAddJunctionArm(road,jObjId,&rJArm))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    if ((jArmObjId2=RoadAddJunctionArm(road,jObjId,&rJArm))==ROAD_InvalidObjId)
        return ipgErrorExit(road);

    RoadJunctionSetMainArms(road,jObjId,jArmObjId0,jArmObjId1);

    // Connecting the Links to the Junction arms
    if (RoadJunctionArmConnectLink(road,jArmObjId0,lObjId0,1,ROAD_JUNC_FORCE_CONNECT)<0)
        return ipgErrorExit(road);

    if (RoadJunctionArmConnectLink(road,jArmObjId1,lObjId1,0,ROAD_JUNC_FORCE_CONNECT)<0)
        return ipgErrorExit(road);

    if (RoadJunctionArmConnectLink(road,jArmObjId2,lObjId2,0,ROAD_JUNC_FORCE_CONNECT)<0)
        return ipgErrorExit(road);


    // Creating Road markings automatically using the built-in function
    RoadBuilderAutoCompletion(road, AC_RoadMarkings);


    // Finishing update of the internal road structure
    RoadSetAllUpdateFlags(road);
    if (RoadBuilderFinish(road)<0)
        return ipgErrorExit(road);

    RoadWriteFile(road,fname,NULL);


    // Writing the road InfoFile
//    if (RoadWriteFile(road, fname ? fname:"Example.rd5", NULL)<0)
//        return ipgErrorExit(road);


    // Print all messages
    ipgWarnMsg(road);

    return ROAD_Ok;
}

#include <QFileInfo>

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);


    snprintf(gstr_fromroad,256,"1");
    snprintf(gstr_toroad,256,"3");
    snprintf(gstr_fromlane,256,"-1");
    snprintf(gstr_tolane,256,"-1");

    int nRtn = GetOptLong(argc,argv);
    if(nRtn == 1)  //show help,so exit.
    {
        return 0;
    }


#ifdef TESTC
    snprintf(gstr_inputpath,255,"D:/lk.xlsx");
    snprintf(gstr_outputpath,255,"D:/lk.rd5");
#endif

    if(strncmp(gstr_inputpath , "",255) == 0)
    {
        std::cout<<"Please use -i set input file path."<<std::endl;
        print_useage();
        return 0;
    }

    char strout[1000];
    snprintf(strout,1000,"Input File Path: %s",gstr_inputpath);
    std::cout<<strout<<std::endl;

    if(strncmp(gstr_outputpath , "",255) == 0)
    {
        std::cout<<"Please use -o set output file path."<<std::endl;
        print_useage();
        return 0;
    }

    snprintf(strout,1000,"Output File Path: %s",gstr_outputpath);
    std::cout<<strout<<std::endl;




    char stroutxodrname[256];
    snprintf(stroutxodrname,255,"%s.xodr",gstr_outputpath);

    char strproccmd[600];
    snprintf(strproccmd,600,"cdaproc.exe -i %s -o %s",gstr_inputpath,stroutxodrname);
    std::cout<<" cmd : "<<strproccmd<<std::endl;

    QProcess * pProc = new QProcess();

    pProc->start(strproccmd);
    pProc->waitForFinished();

    QFileInfo fileinfo(stroutxodrname);
    if(!fileinfo.isFile())
    {
        std::cout<<" convert to cda FAIL."<<std::endl;
        return -1;
    }

    tRoad * pRoad;

    pRoad = RoadNew();

    if(pRoad == NULL)
    {
        std::cout<<" Road New Fail."<<std::endl;
        return -1;
    }
    else
    {
        std::cout<<" Road New Successfully."<<std::endl;
    }


//    createRoadFile(pRoad,"D:/z.rd5");

    int nrtn = RoadReadOpenDRIVE(pRoad,stroutxodrname);

    if(nrtn<0)
    {
        ipgErrorExit(pRoad);
        return -1;
    }


    rd5route * proute = new rd5route(pRoad);
//    proute->route();

    int nfromroad = atoi(gstr_fromroad);
    int ntoroad = atoi(gstr_toroad);
    int nfromlane = atoi(gstr_fromlane);
    int ntolane = atoi(gstr_tolane);
    proute->route(nfromroad,nfromlane,ntoroad,ntolane);
 //   proute->route(1,-3,2,3);
 //   proute->route(1,-1,1,-1);
    delete proute;

    RoadWriteFile(pRoad,gstr_outputpath,NULL);

    return 0;
    return a.exec();
}