modularization/src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.cpp

#include "vehicle_patrol.h"

#include <QFile>
#include <math.h>
#include <thread>

#include "modulecomm.h"
#include "gpsimu.pb.h"
#include "licenseplate.pb.h"
#include "startturnstile.pb.h"
#include "turnstile.pb.h"

extern std::string gstrserverip;
extern std::string gstrpatrolPort;
extern std::string gstrpatrolInterval;
extern std::string gstrid;
extern std::string gstrplateNumber;

extern char stryamlpath[256];

using org::jeecg::defsPatrol::grpc::Empty;
using org::jeecg::defsPatrol::grpc::GPSPoint;

VehiclePatrolExceptionClient::VehiclePatrolExceptionClient(std::shared_ptr<Channel> channel)
{
    stub_ = VehiclePatrolException::NewStub(channel);

    dec_yaml(stryamlpath);

    ModuleFun funupdate = std::bind(&VehiclePatrolExceptionClient::ListenGPSIMUMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmGPSIMU.mpa = iv::modulecomm::RegisterRecvPlus(shmGPSIMU.mstrmsgname,funupdate);
    funupdate = std::bind(&VehiclePatrolExceptionClient::ListenTurnstileMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmTurnstile.mpa = iv::modulecomm::RegisterRecvPlus(shmTurnstile.mstrmsgname,funupdate);

    shmStartTurnstile.mpa = iv::modulecomm::RegisterSend(shmStartTurnstile.mstrmsgname,shmStartTurnstile.mnBufferSize,shmStartTurnstile.mnBufferCount);
}

VehiclePatrolExceptionClient::~VehiclePatrolExceptionClient(void)
{
    if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
    requestInterruption();
    while(this->isFinished() == false);
}

void VehiclePatrolExceptionClient::dec_yaml(const char *stryamlpath)
{
    YAML::Node config;
    try
    {
        config = YAML::LoadFile(stryamlpath);
    }
    catch(YAML::BadFile &e)
    {
        std::cout<<e.what()<<std::endl;
        std::cout<<"yaml file load fail."<<std::endl;
        return;
    }
    catch(YAML::ParserException &e)
    {
        std::cout<<e.what()<<std::endl;
        std::cout<<"yaml file is malformed."<<std::endl;
        return;
    }

    std::string strmsgname;

    if(config["GPS_IMU"])
    {
        if(config["GPS_IMU"]["msgname"]&&config["GPS_IMU"]["buffersize"]&&config["GPS_IMU"]["buffercount"])
        {
            strmsgname = config["GPS_IMU"]["msgname"].as<std::string>();
            strncpy(shmGPSIMU.mstrmsgname,strmsgname.data(),255);
            shmGPSIMU.mnBufferSize = config["GPS_IMU"]["buffersize"].as<int>();
            shmGPSIMU.mnBufferCount = config["GPS_IMU"]["buffercount"].as<int>();
            std::cout << "GPS_IMU:" << shmGPSIMU.mstrmsgname << "," << shmGPSIMU.mnBufferSize << "," << shmGPSIMU.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "hcp2_gpsimu";
        strncpy(shmGPSIMU.mstrmsgname,strmsgname.data(),255);
        shmGPSIMU.mnBufferSize = 10000;
        shmGPSIMU.mnBufferCount = 1;
    }

    if(config["pic_front"])
    {
        if(config["pic_front"]["msgname"]&&config["pic_front"]["buffersize"]&&config["pic_front"]["buffercount"])
        {
            strmsgname = config["pic_front"]["msgname"].as<std::string>();
            strncpy(shmPicFront.mstrmsgname,strmsgname.data(),255);
            shmPicFront.mnBufferSize = config["pic_front"]["buffersize"].as<int>();
            shmPicFront.mnBufferCount = config["pic_front"]["buffercount"].as<int>();
            std::cout << "pic_front:" << shmPicFront.mstrmsgname << "," << shmPicFront.mnBufferSize << "," << shmPicFront.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "picfront";
        strncpy(shmPicFront.mstrmsgname,strmsgname.data(),255);
        shmPicFront.mnBufferSize = 10000000;
        shmPicFront.mnBufferCount = 1;
    }

    if(config["pic_rear"])
    {
        if(config["pic_rear"]["msgname"]&&config["pic_rear"]["buffersize"]&&config["pic_rear"]["buffercount"])
        {
            strmsgname = config["pic_rear"]["msgname"].as<std::string>();
            strncpy(shmPicRear.mstrmsgname,strmsgname.data(),255);
            shmPicRear.mnBufferSize = config["pic_rear"]["buffersize"].as<int>();
            shmPicRear.mnBufferCount = config["pic_rear"]["buffercount"].as<int>();
            std::cout << "pic_rear:" << shmPicRear.mstrmsgname << "," << shmPicRear.mnBufferSize << "," << shmPicRear.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "picrear";
        strncpy(shmPicRear.mstrmsgname,strmsgname.data(),255);
        shmPicRear.mnBufferSize = 10000000;
        shmPicRear.mnBufferCount = 1;
    }

    if(config["pic_left"])
    {
        if(config["pic_left"]["msgname"]&&config["pic_left"]["buffersize"]&&config["pic_left"]["buffercount"])
        {
            strmsgname = config["pic_left"]["msgname"].as<std::string>();
            strncpy(shmPicLeft.mstrmsgname,strmsgname.data(),255);
            shmPicLeft.mnBufferSize = config["pic_left"]["buffersize"].as<int>();
            shmPicLeft.mnBufferCount = config["pic_left"]["buffercount"].as<int>();
            std::cout << "pic_left:" << shmPicLeft.mstrmsgname << "," << shmPicLeft.mnBufferSize << "," << shmPicLeft.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "picleft";
        strncpy(shmPicLeft.mstrmsgname,strmsgname.data(),255);
        shmPicLeft.mnBufferSize = 10000000;
        shmPicLeft.mnBufferCount = 1;
    }

    if(config["pic_right"])
    {
        if(config["pic_right"]["msgname"]&&config["pic_right"]["buffersize"]&&config["pic_right"]["buffercount"])
        {
            strmsgname = config["pic_right"]["msgname"].as<std::string>();
            strncpy(shmPicRight.mstrmsgname,strmsgname.data(),255);
            shmPicRight.mnBufferSize = config["pic_right"]["buffersize"].as<int>();
            shmPicRight.mnBufferCount = config["pic_right"]["buffercount"].as<int>();
            std::cout << "pic_right:" << shmPicRight.mstrmsgname << "," << shmPicRight.mnBufferSize << "," << shmPicRight.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "picright";
        strncpy(shmPicRight.mstrmsgname,strmsgname.data(),255);
        shmPicRight.mnBufferSize = 10000000;
        shmPicRight.mnBufferCount = 1;
    }

    if(config["start_turnstile"])
    {
        if(config["start_turnstile"]["msgname"]&&config["start_turnstile"]["buffersize"]&&config["start_turnstile"]["buffercount"])
        {
            strmsgname = config["start_turnstile"]["msgname"].as<std::string>();
            strncpy(shmStartTurnstile.mstrmsgname,strmsgname.data(),255);
            shmStartTurnstile.mnBufferSize = config["start_turnstile"]["buffersize"].as<int>();
            shmStartTurnstile.mnBufferCount = config["start_turnstile"]["buffercount"].as<int>();
            std::cout << "start_turnstile:" << shmStartTurnstile.mstrmsgname << "," << shmStartTurnstile.mnBufferSize << "," << shmStartTurnstile.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "startturnstile";
        strncpy(shmStartTurnstile.mstrmsgname,strmsgname.data(),255);
        shmStartTurnstile.mnBufferSize = 10000;
        shmStartTurnstile.mnBufferCount = 1;
    }

    if(config["turnstile"])
    {
        if(config["turnstile"]["msgname"]&&config["turnstile"]["buffersize"]&&config["turnstile"]["buffercount"])
        {
            strmsgname = config["turnstile"]["msgname"].as<std::string>();
            strncpy(shmTurnstile.mstrmsgname,strmsgname.data(),255);
            shmTurnstile.mnBufferSize = config["turnstile"]["buffersize"].as<int>();
            shmTurnstile.mnBufferCount = config["turnstile"]["buffercount"].as<int>();
            std::cout << "turnstile:" << shmTurnstile.mstrmsgname << "," << shmTurnstile.mnBufferSize << "," << shmTurnstile.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "turnstile";
        strncpy(shmTurnstile.mstrmsgname,strmsgname.data(),255);
        shmTurnstile.mnBufferSize = 10000000;
        shmTurnstile.mnBufferCount = 1;
    }

    return;
}

void VehiclePatrolExceptionClient::ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
{
    iv::gps::gpsimu xdata;
    if(!xdata.ParseFromArray(strdata,nSize))
    {
        std::cout<<" ListenGPSIMUMsg parese error."<<std::endl;
        return;
    }

    double speed = sqrt(xdata.ve()*xdata.ve() + xdata.vn()*xdata.vn() + xdata.vd()*xdata.vd());

    mutex_GPSIMU.lock();
    currentPosition.set_latitude(xdata.lat());
    currentPosition.set_longitude(xdata.lon());
    currentPosition.set_height(xdata.height());
    currentSpeed = speed;
    mutex_GPSIMU.unlock();

}

void VehiclePatrolExceptionClient::ListenTurnstileMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
{
    iv::vision::turnstile xdata;
    if(!xdata.ParseFromArray(strdata,nSize))
    {
        std::cout<<" ListenTurnstileMsg parese error."<<std::endl;
        return;
    }

    if(statusTSGM == 1)
    {
        std::cout<<"Get Turnstile result"<<std::endl;
        //set TSGM resul
        isTSGM = true;
        if(xdata.state() == false)
            gateStatus = 1; //0 no gate 1 gate close 2 gate open
        else
            gateStatus = 2;
        std::cout<<"gateStatus : "<<(int)gateStatus<<std::endl;
        gateImage.clear();
        gateImage.append(xdata.pic().picdata().data(),xdata.pic().picdata().size());
        gateTime = QDateTime::currentMSecsSinceEpoch(); //time when get gateImage
        mutex_GPSIMU.lock();
        gatePosition.CopyFrom(currentPosition); //positon when get gateImage
        mutex_GPSIMU.unlock();

        timerTSGM.restart();
        statusTSGM = 2;
    }
}

std::string VehiclePatrolExceptionClient::uploadVehiclePatrolInfo(void)
{
    // Data we are sending to the server.
    PatrolRequest request;
    request.set_id(id);
    request.set_istvr(isTVR);
    request.set_violationstatus(violationStatus);
    request.set_vehiclelicensenumber(vehicleLicenseNumber);
    request.set_violationimage(violationImage.data(),violationImage.size());
    request.set_violationtime(violationTime);
    request.mutable_violationposition()->CopyFrom(violationPosition);

    request.set_isfsm(isFSM);
    request.set_firestatus(fireStatus);
    request.set_fireimage(fireImage.data(),fireImage.size());
    request.set_firetime(fireTime);
    request.mutable_fireposition()->CopyFrom(firePosition);

    request.set_istsgm(isTSGM);
    request.set_gatestatus(gateStatus);
    request.set_gateimage(gateImage.data(),gateImage.size());
    request.set_gatetime(gateTime);
    request.mutable_gateposition()->CopyFrom(gatePosition);

    request.set_platenumber(plateNumber);

    // Container for the data we expect from the server.
    Empty reply;

    // Context for the client. It could be used to convey extra information to
    // the server and/or tweak certain RPC behaviors.
    ClientContext context;
    gpr_timespec timespec;
    timespec.tv_sec = 5;
    timespec.tv_nsec = 0;
    timespec.clock_type = GPR_TIMESPAN;
    context.set_deadline(timespec);

    // The actual RPC.
    Status status = stub_ -> uploadVehiclePatrolInfo(&context,request,&reply);

    // Act upon its status.
    if (status.ok()) {

        return "uploadVehiclePatrolInfo RPC successed";
    } else {
        std::cout << status.error_code() << ": " << status.error_message()
                  << std::endl;
        if(status.error_code() == 4)
        {
            std::cout << "vehicleControl RPC connect timeout" << std::endl;
        }
        return "uploadVehiclePatrolInfo RPC failed";
    }
}

void VehiclePatrolExceptionClient::updatePatrolData(void)
{
    id = gstrid;

//    isTVR = true;
//    violationStatus = 2;
//    vehicleLicenseNumber = "津B654321";

//    QFile xFile;
//    xFile.setFileName("/home/samuel/Pictures/123.jpg");
//    if(xFile.open(QIODevice::ReadOnly))
//    {
//        violationImage = xFile.readAll();
//    }
//    xFile.close();

//    violationTime = QDateTime::currentMSecsSinceEpoch();
//    violationPosition.set_height(0.1);
//    violationPosition.set_latitude(39.0666552);
//    violationPosition.set_longitude(117.3542963);

//    isFSM = true;
//    fireStatus = 1;

//    xFile.setFileName("/home/samuel/Pictures/123.jpg");
//    if(xFile.open(QIODevice::ReadOnly))
//    {
//        fireImage = xFile.readAll();
//    }
//    xFile.close();

//    fireTime = QDateTime::currentMSecsSinceEpoch();
//    firePosition.set_height(0.1);
//    firePosition.set_latitude(39.0667552);
//    firePosition.set_longitude(117.3542963);

//    isTSGM = true;
//    gateStatus = 2;

//    xFile.setFileName("/home/samuel/Pictures/123.jpg");
//    if(xFile.open(QIODevice::ReadOnly))
//    {
//        gateImage = xFile.readAll();
//    }
//    xFile.close();

//    gateTime = QDateTime::currentMSecsSinceEpoch();
//    gatePosition.set_height(0.1);
//    gatePosition.set_latitude(39.0665552);
//    gatePosition.set_longitude(117.3542963);

    plateNumber = gstrplateNumber;
}

void VehiclePatrolExceptionClient::run()
{
    QTime xTime;
    xTime.start();
    int lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstrpatrolInterval.c_str());
    org::jeecg::defsPatrol::grpc::GPSPoint tempPosition;
    //set isNeedTSGM
    isNeedTSGM = true;
    //set gateDestination
    gateDestination.set_latitude(39.0665855);
    gateDestination.set_longitude(117.3554362);
    timerTSGM.start();
    while (!QThread::isInterruptionRequested())
    {
        if(abs(xTime.elapsed() - lastTime)>=interval)
        {
            // do something
            mutex_GPSIMU.lock();
            tempPosition.CopyFrom(currentPosition);
            double tempSpeed = currentSpeed;
            mutex_GPSIMU.unlock();

            if(fabs(tempPosition.latitude()-gateDestination.latitude()) < 0.0001 && fabs(tempPosition.longitude()-gateDestination.longitude()) < 0.0001)
            {
                if(tempSpeed < 0.1 && statusTSGM == 0 && isNeedTSGM == true)
                {
                    std::cout<<"Send startTurnstile"<<std::endl;
                    iv::vision::startturnstile xmsg;

                    xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
                    xmsg.set_cameraname(shmPicRight.mstrmsgname);
                    xmsg.set_start(true);

                    int ndatasize = xmsg.ByteSize();
                    char * str = new char[ndatasize];
                    std::shared_ptr<char> pstr;pstr.reset(str);
                    if(!xmsg.SerializeToArray(str,ndatasize))
                    {
                        std::cout<<"StartTurnstile serialize error."<<std::endl;
                        return;
                    }
                    iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
                    statusTSGM = 1;
                    timerTSGM.restart();
                }
            }

            if(statusTSGM == 1 && timerTSGM.elapsed() > 80000)
            {
                //timeout
                std::cout<<"Turnstile timeout"<<std::endl;
                iv::vision::startturnstile xmsg;

                xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
                xmsg.set_cameraname(shmPicLeft.mstrmsgname);
                xmsg.set_start(false);

                int ndatasize = xmsg.ByteSize();
                char * str = new char[ndatasize];
                std::shared_ptr<char> pstr;pstr.reset(str);
                if(!xmsg.SerializeToArray(str,ndatasize))
                {
                    std::cout<<"StartTurnstile serialize error."<<std::endl;
                    return;
                }
                iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);

                isTSGM = false;
                gateStatus = 0;
                gateImage.clear();
                gateTime = QDateTime::currentMSecsSinceEpoch();
                mutex_GPSIMU.lock();
                gatePosition.CopyFrom(currentPosition);
                mutex_GPSIMU.unlock();

                timerTSGM.restart();
                statusTSGM = 3;
            }

            if(statusTSGM == 2 || statusTSGM == 3)
            {
                if(fabs(tempPosition.latitude()-gateDestination.latitude()) > 0.0001 || fabs(tempPosition.longitude()-gateDestination.longitude()) > 0.0001)
                {
                    std::cout<<"Turnstile detect finish"<<std::endl;
                    iv::vision::startturnstile xmsg;

                    xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
                    xmsg.set_cameraname(shmPicLeft.mstrmsgname);
                    xmsg.set_start(false);

                    int ndatasize = xmsg.ByteSize();
                    char * str = new char[ndatasize];
                    std::shared_ptr<char> pstr;pstr.reset(str);
                    if(!xmsg.SerializeToArray(str,ndatasize))
                    {
                        std::cout<<"StartTurnstile serialize error."<<std::endl;
                        return;
                    }
                    iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);

                    timerTSGM.restart();
                    statusTSGM = 0;
                }
            }


//            updatePatrolData();
//            std::string reply = uploadVehiclePatrolInfo();
//            std::cout<< reply <<std::endl;
            lastTime = xTime.elapsed();
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::microseconds(1000));//sleep 1ms
        }
    }
}